先來看arduino 部分
| //For ease of programming | |
| #define THR 0 | |
| #define YAW 1 | |
| #define PITCH 2 | |
| #define ROLL 3 | |
| // Assign your channel in pins | |
| #define THROTTLE_IN_PIN 8 | |
| #define YAW_IN_PIN 11 | |
| #define PITCH_IN_PIN 10 | |
| #define ROLL_IN_PIN 9 |
| // Assign your channel out pins | |
| #define FL_MOTOR_OUT_PIN 4 | |
| #define FR_MOTOR_OUT_PIN 5 | |
| #define BL_MOTOR_OUT_PIN 6 | |
| #define BR_MOTOR_OUT_PIN 7 |
ESC 接線
FL -> PIN 4
FR -> PIN 5
BL -> PIN 6
BR -> PIN 7
RC 接收器接線
THROTTLE -> PIN 8
YAW -> PIN 11
PITCH -> PIN 10
ROLL -> PIN 9
// 宣告五個 interrupt...... 四個rc input 和 一個 spi...
PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE); PCintPort::attachInterrupt(YAW_IN_PIN, calcYaw,CHANGE); PCintPort::attachInterrupt(PITCH_IN_PIN, calcPitch,CHANGE); PCintPort::attachInterrupt(ROLL_IN_PIN, calcRoll,CHANGE); 後面的CHANGE 是代表rising edge 的時候才會觸發..... // if the pin is high, its a rising edge of the signal pulse, so lets record its value if(digitalRead(THROTTLE_IN_PIN) == HIGH) { ulThrottleStart = micros(); } else { // else it must be a falling edge, so lets get the time and subtract the time of the rising edge // this gives use the time between the rising and falling edges i.e. the pulse duration. unThrottleInShared = (uint16_t)(micros() - ulThrottleStart); } unThrottleInShared 就是代表為level 1 的時間....單位為us(10^-6) |
| void setup() | |
| { | |
| Serial.begin(9600); //for debugging... | |
| pinMode(LED_PIN, OUTPUT); | |
| /* | |
| PWM measurement settings | |
| */ | |
| // using the PinChangeInt library, attach the interrupts | |
| // used to read the channels // 宣告成 interrupt | |
| PCintPort::attachInterrupt(THROTTLE_IN_PIN, calcThrottle,CHANGE); | |
| PCintPort::attachInterrupt(YAW_IN_PIN, calcYaw,CHANGE); | |
| PCintPort::attachInterrupt(PITCH_IN_PIN, calcPitch,CHANGE); | |
| PCintPort::attachInterrupt(ROLL_IN_PIN, calcRoll,CHANGE); | |
| // attach servo objects, these will generate the correct | |
| // pulses for driving Electronic speed controllers, servos or other devices | |
| // designed to interface directly with RC Receivers | |
| MOTOR[0].attach(FL_MOTOR_OUT_PIN); | |
| MOTOR[1].attach(FR_MOTOR_OUT_PIN); | |
| MOTOR[2].attach(BL_MOTOR_OUT_PIN); | |
| MOTOR[3].attach(BR_MOTOR_OUT_PIN); | |
| //Set servo values to min | |
| for (int i=0;i<SERVO_NUM;i++) | |
| { | |
| MOTOR[i].writeMicroseconds(RC_MIN); | |
| } | |
| /* | |
| SPI settings | |
| */ | |
| // Declare MISO as output : have to send on | |
| //master in, *slave out* | |
| pinMode(MISO, OUTPUT); | |
| // turn on SPI in slave mode | |
| SPCR |= _BV(SPE); | |
| // now turn on interrupts | |
| SPI.attachInterrupt(); | |
| } |
| void loop() | |
| { | |
| //Constantly update rc_data | |
| rc_data[THR].u16 = unThrottleInShared; | |
| rc_data[YAW].u16 = unYawInShared; | |
| rc_data[PITCH].u16 = unPitchInShared; | |
| rc_data[ROLL].u16 = unRollInShared; | |
| if (unThrottleInShared < 1000 & | |
| unYawInShared < 1200 & | |
| unPitchInShared < 1200 & | |
| unRollInShared > 1200 ) { | |
| uint32_t t_old = millis(); | |
| while (millis()-t_old < 500 ){ | |
| //wait to be sure that sticks are still in position | |
| } | |
| // if so change current status | |
| if (unThrottleInShared < 1000 & | |
| unYawInShared < 1200 & | |
| unPitchInShared < 1200 & | |
| unRollInShared > 1200 ) { | |
| start = !start; | |
| //change LED status | |
| digitalWrite(13, start); | |
| } | |
| } | |
| //Update servo (ESC) if necessary and started | |
| if (update_servo & start){ | |
| uint8_t ipos=0; | |
| byte checksum2=0; | |
| for (int i=0;i<SERVO_NUM;i++) | |
| { | |
| //process buffer values | |
| for (int ibyte = 0;ibyte<2;ibyte++){ | |
| esc_data[i].u8[ibyte] = rx_buf[ipos]; | |
| checksum2+=rx_buf[ipos]; | |
| ipos++; | |
| } | |
| } | |
| if (rx_buf[ipos] == checksum2) { | |
| //write ESC output | |
| for (int i=0;i<SERVO_NUM;i++) | |
| MOTOR[i].writeMicroseconds(esc_data[i].u16); | |
| } | |
| update_servo = false; | |
| } else if (!start) { | |
| for (int i=0;i<SERVO_NUM;i++) | |
| { | |
| MOTOR[i].writeMicroseconds(RC_MIN); | |
| } | |
| } | |
| } |
| void calcThrottle() | |
| { | |
| // if the pin is high, its a rising edge of the signal pulse, so lets record its value | |
| if(digitalRead(THROTTLE_IN_PIN) == HIGH) | |
| { | |
| ulThrottleStart = micros(); | |
| } | |
| else | |
| { | |
| // else it must be a falling edge, so lets get the time and subtract the time of the rising edge | |
| // this gives use the time between the rising and falling edges i.e. the pulse duration. | |
| unThrottleInShared = (uint16_t)(micros() - ulThrottleStart); | |
| } | |
| } |
| /*----------------------- | |
| SPI interrupt routine | |
| ------------------------*/ | |
| ISR (SPI_STC_vect) | |
| { | |
| //grab a new command and process it | |
| byte cmd = SPDR; | |
| if (cmd == 'S') { | |
| //STOP do nothing : end of sending RC data | |
| pos=0; | |
| return; | |
| } | |
| if (cmd == 'P') { | |
| //process it ! | |
| update_servo = true; | |
| pos=0; | |
| return; | |
| } | |
| if (cmd == 'C') { | |
| //push Cheksum into the register | |
| SPDR = checksum; | |
| checksum = 0; | |
| pos=0; | |
| return; | |
| } | |
| //push data into a byte buffer | |
| rx_buf[pos++] = cmd; | |
| // 10-11 -> send 2bytes channel 1 value | |
| // ... | |
| // 40-41 -> send 2bytes channel 4 value | |
| // ... | |
| // 60-61 -> send 2bytes channel 6 value | |
| switch (cmd){ | |
| case 10: | |
| SPDR = rc_data[THR].u8[0]; | |
| checksum += rc_data[THR].u8[0]; | |
| break; | |
| case 11: | |
| SPDR = rc_data[THR].u8[1]; | |
| checksum += rc_data[THR].u8[1]; | |
| break; | |
| case 20: | |
| SPDR = rc_data[YAW].u8[0]; | |
| checksum += rc_data[YAW].u8[0]; | |
| break; | |
| case 21: | |
| SPDR = rc_data[YAW].u8[1]; | |
| checksum += rc_data[YAW].u8[1]; | |
| break; | |
| case 30: | |
| SPDR = rc_data[PITCH].u8[0]; | |
| checksum += rc_data[PITCH].u8[0]; | |
| break; | |
| case 31: | |
| SPDR = rc_data[PITCH].u8[1]; | |
| checksum += rc_data[PITCH].u8[1]; | |
| break; | |
| case 40: | |
| SPDR = rc_data[ROLL].u8[0]; | |
| checksum += rc_data[ROLL].u8[0]; | |
| break; | |
| case 41: | |
| SPDR = rc_data[ROLL].u8[1]; | |
| checksum += rc_data[ROLL].u8[1]; | |
| break; | |
| } | |
| } |
再來看 RPI 的部分
| void TimerClass::sig_handler_(int signum) | |
| { | |
| pthread_mutex_lock(&TimerMutex_); | |
| //output to a log file | |
| //open log file | |
| fstream logfile; | |
| logfile.open("quadpilot.log", ios::out|ios::app); | |
| if (logfile.fail()) // Check for file creation and return error. | |
| { | |
| cout << "Error opening output.\n"; | |
| } | |
| float RCinput[N_RC_CHAN],PIDout[3]; | |
| uint16_t ESC[N_SERVO]; | |
| //------------------------------------------------------ | |
| //1-Get Remote values using SPI | |
| union bytes{ | |
| uint8_t u8[2]; | |
| uint16_t u16; | |
| } rc_union; | |
| uint8_t checksum=0,recv_checksum=1; | |
| while (checksum != recv_checksum) { | |
| checksum=0; | |
| for (int i=0;i<4;i++){ | |
| ArduSPI.writeByte((uint8_t) (i+1)*10); | |
| rc_union.u8[0] = ArduSPI.rwByte((uint8_t) (i+1)*10+1); | |
| rc_union.u8[1] = ArduSPI.rwByte('S'); | |
| //transaction ended | |
| RCinput[i] = (float) rc_union.u16; | |
| checksum+=rc_union.u8[0]; | |
| checksum+=rc_union.u8[1]; | |
| } | |
| //Control checksum | |
| ArduSPI.writeByte('C'); | |
| recv_checksum = ArduSPI.rwByte('S'); | |
| } | |
| //Bounding RC values to avoid division by zeros fex. | |
| for (int i=1;i<4;i++){ | |
| if ( RCinput[i] > RC_MAX) RCinput[i] = RC_MAX; | |
| if ( RCinput[i] < RC_MIN) RCinput[i] = RC_MIN; | |
| } | |
| //outputing values to logfile | |
| logfile << RCinput[0] << " " << RCinput[1] << " " | |
| << RCinput[2] << " " << RCinput[3] << " "; | |
| // //convert into PID usable values | |
| RCinput[0] = (RCinput[0] - THR_MIN)/(THR_MAX-THR_MIN) * 100.0; | |
| RCinput[1] = -(RCinput[1] -(RC_MAX+RC_MIN)/2.) / | |
| (RC_MAX-RC_MIN) * K_YAW; | |
| RCinput[2] = (RCinput[2] -(RC_MAX+RC_MIN)/2.)/ | |
| (RC_MAX-RC_MIN) * K_PITCH; | |
| RCinput[3] = (RCinput[3] -(RC_MAX+RC_MIN)/2.)/ | |
| (RC_MAX-RC_MIN) * K_ROLL; | |
| //outputing values to logfile | |
| logfile << RCinput[0] << " " << RCinput[1] << " " | |
| << RCinput[2] << " " << RCinput[3] << " "; | |
| #ifdef XMODE | |
| //Switch to Xmode instead of +mode | |
| //orders are given in a ref frame rotated by 90deg. | |
| float cs45 = sqrt(2.)/2.; | |
| float Px = RCinput[2]*cs45 + RCinput[3]*cs45; | |
| float Rx = - RCinput[2]*cs45 + RCinput[3]*cs45; | |
| RCinput[2] = Px; | |
| RCinput[3] = Rx; | |
| #endif | |
| // printf("Received : %6.3f %6.3f %6.3f %6.3f\n", RCinput[0], | |
| // RCinput[1], RCinput[2], RCinput[3]); | |
| //------------------------------------------------------ | |
| //2- Get attitude of the drone | |
| while (imu.getAttitude() < 0){ | |
| }; | |
| //Compensate lost of Thrust due to angle of drone | |
| RCinput[0] = RCinput[0]/cos(imu.ypr[ROLL]/180*M_PI) | |
| /cos(imu.ypr[PITCH]/180*M_PI); | |
| //output to logfile | |
| // logfile << imu.ypr[YAW] << " " << imu.ypr[PITCH] << " " | |
| // << imu.ypr[ROLL] << " " | |
| // << imu.gyro[YAW] << " " << imu.gyro[PITCH] << " " | |
| // << imu.gyro[ROLL] << " "; | |
| // printf("ATTITUDE: %7.2f %7.2f %7.2f\n",imu.ypr[YAW], | |
| // imu.ypr[PITCH], | |
| // imu.ypr[ROLL]); | |
| // printf(" %7.2f %7.2f %7.2f\n",imu.gyro[YAW], | |
| // imu.gyro[PITCH], | |
| // imu.gyro[ROLL]); | |
| //------------------------------------------------------ | |
| //3- Timer dt | |
| Timer.calcdt_(); | |
| //printf("dt : %f \n",Timer.dt); | |
| //------------------------------------------------------ | |
| //4-1 Calculate PID on attitude | |
| #ifdef PID_STAB | |
| for (int i=1;i<DIM;i++){ | |
| //yprSTAB[i].updateKpKi(RCinput[i+1],imu.ypr[i]); | |
| PIDout[i] = | |
| yprSTAB[i].update_pid_std(RCinput[i+1], | |
| imu.ypr[i], | |
| Timer.dt); | |
| } | |
| //yaw is rate PID only | |
| PIDout[YAW] = RCinput[YAW+1]; | |
| // printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1], | |
| // imu.ypr[PITCH], | |
| // PIDout[PITCH]); | |
| // printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1], | |
| // imu.ypr[ROLL], | |
| // PIDout[ROLL]); | |
| for (int i=0;i<DIM;i++){ | |
| PIDout[i] = | |
| yprRATE[i].update_pid_std(PIDout[i], | |
| imu.gyro[i], | |
| Timer.dt); | |
| } | |
| // printf("YAW: %7.2f %7.2f %7.2f\n",RCinput[YAW+1], | |
| // imu.gyro[YAW], | |
| // PIDout[YAW]); | |
| // printf("PITCH: %7.2f %7.2f %7.2f\n",RCinput[PITCH+1], | |
| // imu.gyro[PITCH], | |
| // PIDout[PITCH]); | |
| // printf("ROLL: %7.2f %7.2f %7.2f\n",RCinput[ROLL+1], | |
| // imu.gyro[ROLL], | |
| // PIDout[ROLL]); | |
| #endif | |
| //4-2 Calculate PID on rotational rate | |
| #ifdef PID_RATE | |
| for (int i=0;i<DIM;i++){ | |
| PIDout[i] = | |
| yprRATE[i].update_pid_std(RCinput[i+1], | |
| imu.gyro[i], | |
| Timer.dt); | |
| } | |
| //printf("%7.2f %7.2f\n",imu.gyro[PITCH],Timer.PIDout[PITCH]); | |
| #endif | |
| // logfile << PIDout[YAW] << " " << PIDout[PITCH] << " " | |
| // << PIDout[ROLL] << " "; | |
| //------------------------------------------------------ | |
| //5- Send ESC update via SPI | |
| //compute each new ESC value | |
| //if THR is low disable PID and be sure that ESC receive Zero | |
| //printf("%f \n",RCinput[0]); | |
| if (RCinput[0] < 7.0) { | |
| for (int i=0;i<4;i++){ | |
| ESC[i] = (uint16_t)0; | |
| } | |
| } else { | |
| ESC[1] = (uint16_t)(RCinput[0]*10+1000 | |
| + PIDout[ROLL] + PIDout[YAW]); | |
| ESC[3] = (uint16_t)(RCinput[0]*10+1000 | |
| - PIDout[ROLL] + PIDout[YAW]); | |
| ESC[0] = (uint16_t)(RCinput[0]*10+1000 | |
| + PIDout[PITCH] - PIDout[YAW]); | |
| ESC[2] = (uint16_t)(RCinput[0]*10+1000 | |
| - PIDout[PITCH] - PIDout[YAW]); | |
| } | |
| checksum = 0; | |
| for (int iesc=0;iesc < N_SERVO; iesc++) { | |
| ArduSPI.writeByte(ESC[iesc] & 0xff); | |
| checksum+=ESC[iesc] & 0xff; | |
| ArduSPI.writeByte((ESC[iesc] >> 8) & 0xff); | |
| checksum+=(ESC[iesc] >> 8) & 0xff; | |
| } | |
| ArduSPI.writeByte(checksum); | |
| //sending Proccess it | |
| ArduSPI.writeByte('P'); | |
| // printf(" Sent : %4d %4d %4d %4d\n", ESC[0], | |
| // ESC[1], ESC[2], ESC[3]); | |
| //------------------------------------------------------ | |
| //6-compensate dt | |
| Timer.compensate_(); | |
| //ouputting ESC values to logfile | |
| logfile << ESC[0] << " " << ESC[1] << " " | |
| << ESC[2] << " " << ESC[3] << " " << endl; | |
| //closing logfile | |
| logfile.close(); | |
| pthread_mutex_unlock(&TimerMutex_); | |
| //end of interrupt | |
| } |
Ref : https://github.com/vjaunet/QUADCOPTER_V2
沒有留言:
張貼留言