diff --git a/baseflight.uvopt b/baseflight.uvopt index 86e2437e6..ba1c30d5e 100755 --- a/baseflight.uvopt +++ b/baseflight.uvopt @@ -134,7 +134,7 @@ 0 DLGTARM - (1010=333,106,699,649,0)(1007=-1,-1,-1,-1,0)(1008=90,117,456,339,0)(1009=-1,-1,-1,-1,0)(100=-1,-1,-1,-1,0)(110=-1,-1,-1,-1,0)(111=-1,-1,-1,-1,0)(1011=-1,-1,-1,-1,0)(180=-1,-1,-1,-1,0)(120=-1,-1,-1,-1,0)(121=-1,-1,-1,-1,0)(122=-1,-1,-1,-1,0)(123=-1,-1,-1,-1,0)(140=-1,-1,-1,-1,0)(240=-1,-1,-1,-1,0)(190=-1,-1,-1,-1,0)(200=-1,-1,-1,-1,0)(170=-1,-1,-1,-1,0)(130=582,118,1166,798,0)(131=549,135,1133,815,0)(132=-1,-1,-1,-1,0)(133=-1,-1,-1,-1,0)(160=-1,-1,-1,-1,0)(161=-1,-1,-1,-1,0)(162=-1,-1,-1,-1,0)(210=-1,-1,-1,-1,0)(211=-1,-1,-1,-1,0)(220=-1,-1,-1,-1,0)(221=791,309,1258,816,0)(230=-1,-1,-1,-1,0)(231=-1,-1,-1,-1,0)(232=-1,-1,-1,-1,0)(233=-1,-1,-1,-1,0)(150=570,175,1163,795,0)(151=-1,-1,-1,-1,0) + (1010=333,106,699,649,0)(1007=-1,-1,-1,-1,0)(1008=90,117,456,339,0)(1009=-1,-1,-1,-1,0)(100=-1,-1,-1,-1,0)(110=-1,-1,-1,-1,0)(111=-1,-1,-1,-1,0)(1011=-1,-1,-1,-1,0)(180=-1,-1,-1,-1,0)(120=-1,-1,-1,-1,0)(121=-1,-1,-1,-1,0)(122=-1,-1,-1,-1,0)(123=-1,-1,-1,-1,0)(140=-1,-1,-1,-1,0)(240=-1,-1,-1,-1,0)(190=-1,-1,-1,-1,0)(200=-1,-1,-1,-1,0)(170=-1,-1,-1,-1,0)(130=1289,156,1873,836,1)(131=549,135,1133,815,0)(132=-1,-1,-1,-1,0)(133=-1,-1,-1,-1,0)(160=-1,-1,-1,-1,0)(161=-1,-1,-1,-1,0)(162=-1,-1,-1,-1,0)(210=-1,-1,-1,-1,0)(211=-1,-1,-1,-1,0)(220=-1,-1,-1,-1,0)(221=791,309,1258,816,0)(230=-1,-1,-1,-1,0)(231=-1,-1,-1,-1,0)(232=-1,-1,-1,-1,0)(233=-1,-1,-1,-1,0)(150=570,175,1163,795,0)(151=-1,-1,-1,-1,0) 0 @@ -157,6 +157,38 @@ -UV0168AVR -O206 -S8 -C0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC72000000 -TP21 -TDS803D -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO11 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000 + + + 0 + 0 + 361 + 1 +
134245422
+ 0 + 0 + 0 + 0 + 1 + F:\rc\freeflight\projects\baseflight\src\drv_pwm.c + + + + + 1 + 0 + 406 + 1 +
134245620
+ 0 + 0 + 0 + 0 + 1 + + + \\baseflight\src/drv_pwm.c\406 + + 0 @@ -403,6 +435,21 @@ 0 0 + 361 + 1 +
0
+ 0 + 0 + 0 + 0 + 0 + F:\rc\freeflight\projects\baseflight\src\drv_pwm.c + + + + + 1 + 0 71 1
134232596
@@ -753,10 +800,10 @@ 1 0 0 - 0 + 17 0 - 1 - 1 + 336 + 349 0 .\src\cli.c cli.c @@ -767,10 +814,10 @@ 1 0 0 - 0 + 19 0 - 125 - 125 + 79 + 79 0 .\src\config.c config.c @@ -781,10 +828,10 @@ 1 0 0 - 104 + 1 0 - 114 - 135 + 1 + 24 0 .\src\imu.c imu.c @@ -797,8 +844,8 @@ 0 0 0 - 24 - 37 + 0 + 0 0 .\src\main.c main.c @@ -809,10 +856,10 @@ 1 0 0 - 61 + 0 0 - 194 - 194 + 187 + 211 0 .\src\mixer.c mixer.c @@ -823,10 +870,10 @@ 1 0 0 - 19 + 0 0 - 199 - 219 + 1 + 12 0 .\src\mw.c mw.c @@ -837,10 +884,10 @@ 1 0 0 - 17 + 26 0 - 23 - 26 + 139 + 152 0 .\src\sensors.c sensors.c @@ -851,10 +898,10 @@ 1 0 0 - 31 + 25 0 - 0 - 0 + 460 + 476 0 .\src\serial.c serial.c @@ -865,10 +912,10 @@ 5 0 0 - 0 + 13 0 - 55 - 66 + 1 + 1 0 .\src\board.h board.h @@ -879,10 +926,10 @@ 5 0 0 - 19 + 0 0 - 59 - 80 + 215 + 237 0 .\src\mw.h mw.h @@ -895,8 +942,8 @@ 0 0 0 - 0 - 0 + 169 + 209 0 .\src\gps.c gps.c @@ -907,10 +954,10 @@ 1 0 0 - 8 + 0 0 - 60 - 81 + 0 + 0 0 .\src\spektrum.c spektrum.c @@ -944,8 +991,8 @@ 0 0 0 - 0 - 0 + 43 + 73 0 .\src\drv_adxl345.c drv_adxl345.c @@ -958,8 +1005,8 @@ 0 15 0 - 1 - 19 + 0 + 0 0 .\src\drv_bmp085.c drv_bmp085.c @@ -986,8 +1033,8 @@ 0 0 0 - 132 - 150 + 0 + 0 0 .\src\drv_i2c.c drv_i2c.c @@ -1012,10 +1059,10 @@ 1 0 0 - 36 + 18 0 - 362 - 362 + 0 + 0 0 .\src\drv_pwm.c drv_pwm.c @@ -1026,10 +1073,10 @@ 1 0 0 - 0 + 15 0 - 38 - 50 + 0 + 0 0 .\src\drv_system.c drv_system.c @@ -1040,7 +1087,7 @@ 1 0 0 - 0 + 24 0 0 0 @@ -1068,10 +1115,10 @@ 1 0 0 - 0 + 26 0 147 - 168 + 167 0 .\src\drv_mpu6050.c drv_mpu6050.c @@ -1084,8 +1131,8 @@ 0 0 0 - 124 - 145 + 0 + 0 0 .\src\drv_adc_fy90q.c drv_adc_fy90q.c @@ -1098,8 +1145,8 @@ 0 0 0 - 176 - 176 + 0 + 0 0 .\src\drv_pwm_fy90q.c drv_pwm_fy90q.c @@ -1131,7 +1178,7 @@ 1 0 0 - 0 + 35 0 0 0 @@ -1161,8 +1208,8 @@ 0 19 0 - 656 - 656 + 0 + 0 0 .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_adc.c stm32f10x_adc.c @@ -1287,8 +1334,8 @@ 0 0 0 - 146 - 166 + 0 + 0 0 .\src\baseflight_startups\startup_stm32f10x_md.s startup_stm32f10x_md.s @@ -1301,8 +1348,8 @@ 0 0 0 - 125 - 133 + 0 + 0 0 .\src\baseflight_startups\startup_stm32f10x_md_fy90q.s startup_stm32f10x_md_fy90q.s diff --git a/src/buzzer.c b/src/buzzer.c new file mode 100644 index 000000000..5e884d972 --- /dev/null +++ b/src/buzzer.c @@ -0,0 +1,115 @@ +#include "board.h" +#include "mw.h" + +void buzzer(uint8_t warn_vbat) +{ + static uint16_t ontime, offtime, beepcount, repeat, repeatcounter; + static uint32_t buzzerLastToggleTime; + static uint8_t buzzerIsOn = 0, activateBuzzer, beeperOnBox, i, last_rcOptions[CHECKBOXITEMS], warn_noGPSfix = 0, warn_failsafe = 0; + + //===================== Beeps for changing rcOptions ===================== +#if defined(RCOPTIONSBEEP) + if (last_rcOptions[i] != rcOptions[i]) { + toggleBeep = 1; + } + last_rcOptions[i] = rcOptions[i]; + i++; + if (i >= CHECKBOXITEMS) + i = 0; +#endif + //===================== BeeperOn via rcOptions ===================== + if (rcOptions[BOXBEEPERON]) { // unconditional beeper on via AUXn switch + beeperOnBox = 1; + } else { + beeperOnBox = 0; + } + //===================== Beeps for failsafe ===================== +#if defined(FAILSAFE) + if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 1) { + warn_failsafe = 1; //set failsafe warning level to 1 while landing + if (failsafeCnt > 5 * (FAILSAVE_DELAY + FAILSAVE_OFF_DELAY)) + warn_failsafe = 2; //start "find me" signal after landing + } + if (failsafeCnt > (5 * FAILSAVE_DELAY) && armed == 0) + warn_failsafe = 2; // tx turned off while motors are off: start "find me" signal + if (failsafeCnt == 0) + warn_failsafe = 0; // turn off alarm if TX is okay +#endif + //===================== GPS fix notification handling ===================== + if (sensors(SENSOR_GPS)) { + if ((GPSModeHome || GPSModeHold) && !GPS_fix) { //if no fix and gps funtion is activated: do warning beeps. + warn_noGPSfix = 1; + } else { + warn_noGPSfix = 0; + } + } + //===================== Main Handling Block ===================== + repeat = 1; // set repeat to default + ontime = 100; // set offtime to default + + //the order of the below is the priority from high to low, the last entry has the lowest priority, only one option can be active at the same time + if (warn_failsafe == 2) { + activateBuzzer = 1; + offtime = 2000; + ontime = 300; + repeat = 1; + } //failsafe "find me" signal + else if (warn_failsafe == 1) { + activateBuzzer = 1; + offtime = 50; + } //failsafe landing active + else if (warn_noGPSfix == 1) { + activateBuzzer = 1; + offtime = 10; + } else if (beeperOnBox == 1) { + activateBuzzer = 1; + offtime = 50; + } //beeperon + else if (warn_vbat == 4) { + activateBuzzer = 1; + offtime = 500; + repeat = 3; + } else if (warn_vbat == 2) { + activateBuzzer = 1; + offtime = 1000; + repeat = 2; + } else if (warn_vbat == 1) { + activateBuzzer = 1; + offtime = 2000; + } else if (toggleBeep > 0) { + activateBuzzer = 1; + ontime = 50; + offtime = 50; + } //fast confirmation beep + else { + activateBuzzer = 0; + } + + if (activateBuzzer) { + if (repeatcounter > 1 && !buzzerIsOn && (millis() >= (buzzerLastToggleTime + 80))) { // if the buzzer is off and there is a short pause neccessary (multipe buzzes) + buzzerIsOn = 1; + BEEP_ON; + buzzerLastToggleTime = millis(); // save the time the buzer turned on + repeatcounter--; + } else if (!buzzerIsOn && (millis() >= (buzzerLastToggleTime + offtime))) { // Buzzer is off and long pause time is up -> turn it on + buzzerIsOn = 1; + BEEP_ON; + buzzerLastToggleTime = millis(); // save the time the buzer turned on + repeatcounter = repeat; //set the amount of repeats + } else if (buzzerIsOn && (millis() >= buzzerLastToggleTime + ontime)) { //Buzzer is on and time is up -> turn it off + buzzerIsOn = 0; + BEEP_OFF; + buzzerLastToggleTime = millis(); // save the time the buzer turned on + if (toggleBeep > 0) + beepcount++; // only increment if confirmation beep, the rest is endless while the condition is given + } + if (beepcount >= toggleBeep) { //confirmation flag is 0,1 or 2 + beepcount = 0; //reset the counter for the next time + toggleBeep = 0; //reset the flag after all beeping is done + } + } else { //no beeping neccessary:reset everything (just in case) + beepcount = 0; //reset the counter for the next time + BEEP_OFF; + buzzerIsOn = 0; + } +} diff --git a/src/cli.c b/src/cli.c index 2b669ba5c..55e8f9715 100644 --- a/src/cli.c +++ b/src/cli.c @@ -346,7 +346,7 @@ static void cliMixer(char *cmdline) static void cliSave(char *cmdline) { uartPrint("Saving..."); - writeParams(); + writeParams(0); uartPrint("\r\nRebooting..."); delay(10); systemReset(false); diff --git a/src/config.c b/src/config.c index 11c312df7..6a29e3536 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ config_t cfg; const char rcChannelLetters[] = "AERT1234"; static uint32_t enabledSensors = 0; -static uint8_t checkNewConf = 13; +uint8_t checkNewConf = 14; void parseRcChannels(const char *input) { @@ -33,15 +33,26 @@ void readEEPROM(void) // Read flash memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t)); - for (i = 0; i < 7; i++) - lookupRX[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 1250; + for(i = 0; i < 6; i++) + lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t)cfg.rcRate8 / 2500; + + for(i = 0; i < 11; i++) { + int16_t tmp = 10 * i - cfg.thrMid8; + uint8_t y = 1; + if (tmp > 0) + y = 100 - cfg.thrMid8; + if (tmp < 0) + y = cfg.thrMid8; + lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t)cfg.thrExpo8 * (tmp * tmp) / (y * y) ) / 10; // [0;1000] + lookupThrottleRC[i] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle)* lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE] + } cfg.wing_left_mid = constrain(cfg.wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT cfg.wing_right_mid = constrain(cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR } -void writeParams(void) +void writeParams(uint8_t b) { FLASH_Status status; uint32_t i; @@ -61,7 +72,8 @@ void writeParams(void) FLASH_Lock(); readEEPROM(); - blinkLED(15, 20, 1); + if (b) + blinkLED(15, 20, 1); } void checkFirstTime(bool reset) @@ -86,7 +98,7 @@ void checkFirstTime(bool reset) cfg.I8[PITCH] = 30; cfg.D8[PITCH] = 23; cfg.P8[YAW] = 85; - cfg.I8[YAW] = 0; + cfg.I8[YAW] = 45; cfg.D8[YAW] = 0; cfg.P8[PIDALT] = 16; cfg.I8[PIDALT] = 15; @@ -97,25 +109,25 @@ void checkFirstTime(bool reset) cfg.P8[PIDVEL] = 0; cfg.I8[PIDVEL] = 0; cfg.D8[PIDVEL] = 0; - cfg.P8[PIDLEVEL] = 90; - cfg.I8[PIDLEVEL] = 45; + cfg.P8[PIDLEVEL] = 70; + cfg.I8[PIDLEVEL] = 10; cfg.D8[PIDLEVEL] = 100; cfg.P8[PIDMAG] = 40; - cfg.rcRate8 = 45; // = 0.9 in GUI + cfg.rcRate8 = 90; cfg.rcExpo8 = 65; cfg.rollPitchRate = 0; cfg.yawRate = 0; cfg.dynThrPID = 0; - for (i = 0; i < CHECKBOXITEMS; i++) { - cfg.activate1[i] = 0; - cfg.activate2[i] = 0; - } + cfg.thrMid8 = 50; + cfg.thrExpo8 = 0; + for (i = 0; i < CHECKBOXITEMS; i++) + cfg.activate[i] = 0; cfg.accTrim[0] = 0; cfg.accTrim[1] = 0; cfg.accZero[0] = 0; cfg.accZero[1] = 0; cfg.accZero[2] = 0; - cfg.acc_lpf_factor = 4; + cfg.acc_lpf_factor = 100; cfg.gyro_lpf = 42; cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y cfg.vbatscale = 110; @@ -163,7 +175,7 @@ void checkFirstTime(bool reset) // serial(uart1) baudrate cfg.serial_baudrate = 115200; - writeParams(); + writeParams(0); } bool sensors(uint32_t mask) diff --git a/src/drv_adxl345.c b/src/drv_adxl345.c index 8ccfa6158..7d6245d04 100755 --- a/src/drv_adxl345.c +++ b/src/drv_adxl345.c @@ -29,6 +29,7 @@ #define ADXL345_RANGE_16G 0x03 #define ADXL345_FIFO_STREAM 0x80 +extern uint16_t acc_1G; static void adxl345Init(void); static void adxl345Read(int16_t *accelData); @@ -67,6 +68,7 @@ static void adxl345Init(void) i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); } + acc_1G = 265; // 3.3V operation } uint8_t acc_samples = 0; diff --git a/src/drv_ledring.c b/src/drv_ledring.c index 6d9f56e4c..e7e1e552e 100644 --- a/src/drv_ledring.c +++ b/src/drv_ledring.c @@ -21,25 +21,25 @@ void ledringState(void) static uint8_t state; if (state == 0) { - b[0] = 'z'; - b[1] = (180 - heading) / 2; // 1 unit = 2 degrees; + b[0] = 'z'; + b[1] = (180 - heading) / 2; // 1 unit = 2 degrees; i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 2, b); - state = 1; + state = 1; } else if (state == 1) { - b[0] = 'y'; - b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180); - b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180); + b[0] = 'y'; + b[1] = constrain(angle[ROLL] / 10 + 90, 0, 180); + b[2] = constrain(angle[PITCH] / 10 + 90, 0, 180); i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); - state = 2; + state = 2; } else if (state == 2) { - b[0] = 'd'; // all unicolor GREEN - b[1] = 1; - if (armed) - b[2] = 1; - else - b[2] = 0; + b[0] = 'd'; // all unicolor GREEN + b[1] = 1; + if (armed) + b[2] = 1; + else + b[2] = 0; i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); - state = 0; + state = 0; } } diff --git a/src/gps.c b/src/gps.c index 7d78cc920..50599fb34 100644 --- a/src/gps.c +++ b/src/gps.c @@ -42,6 +42,12 @@ static void GPS_NewData(uint16_t c) } } +void GPS_reset_home_position(void) +{ + GPS_latitude_home = GPS_latitude; + GPS_longitude_home = GPS_longitude; +} + /* this is an equirectangular approximation to calculate distance and bearing between 2 GPS points (lat/long) it's much more faster than an exact calculation the error is neglectible for few kilometers assuming a constant R for earth diff --git a/src/imu.c b/src/imu.c index f8e1a1ab0..e2ad6dbde 100755 --- a/src/imu.c +++ b/src/imu.c @@ -4,6 +4,7 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; int16_t acc_25deg = 0; int32_t BaroAlt; +int16_t sonarAlt; //to think about the unit int32_t EstAlt; // in cm int16_t BaroPID = 0; int32_t AltHold; @@ -175,7 +176,7 @@ static void getEstimatedAttitude(void) #if defined(MG_LPF_FACTOR) static int16_t mgSmooth[3]; #endif - static float accTemp[3]; // projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer + static float accLPF[3]; static uint32_t previousT; uint32_t currentT = micros(); float scale, deltaGyroAngle[3]; @@ -187,10 +188,8 @@ static void getEstimatedAttitude(void) for (axis = 0; axis < 3; axis++) { deltaGyroAngle[axis] = gyroADC[axis] * scale; if (cfg.acc_lpf_factor > 0) { - accTemp[axis] = accTemp[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor); - accSmooth[axis] = roundf(accTemp[axis]); - // accTemp[axis] = (accTemp[axis] - (accTemp[axis] >> cfg.acc_lpf_factor)) + accADC[axis]; - // accSmooth[axis] = accTemp[axis] >> cfg.acc_lpf_factor; + accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / cfg.acc_lpf_factor)) + accADC[axis] * (1.0f / cfg.acc_lpf_factor); + accSmooth[axis] = accLPF[axis]; } else { accSmooth[axis] = accADC[axis]; } @@ -221,10 +220,8 @@ static void getEstimatedAttitude(void) // If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation. // To do that, we just skip filter, as EstV already rotated by Gyro if ((36 < accMag && accMag < 196) || smallAngle25) { - for (axis = 0; axis < 3; axis++) { - // int16_t acc = accSmooth[axis]; - EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accTemp[axis]) * INV_GYR_CMPF_FACTOR; - } + for (axis = 0; axis < 3; axis++) + EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; } if (sensors(SENSOR_MAG)) { diff --git a/src/mixer.c b/src/mixer.c index 36263e718..016384ccb 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -205,10 +205,10 @@ void mixTable(void) break; case MULTITYPE_VTAIL4: - motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R - motor[1] = PIDMIX(-1, -1, +2 / 10); //FRONT_R - motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L - motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L + motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R + motor[1] = PIDMIX(-1, -1, +0); //FRONT_R + motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L + motor[3] = PIDMIX(+1, -1, -0); //FRONT_L break; case MULTITYPE_GIMBAL: diff --git a/src/mw.c b/src/mw.c index 4d1762a74..d80e35650 100755 --- a/src/mw.c +++ b/src/mw.c @@ -8,6 +8,7 @@ int16_t debug1, debug2, debug3, debug4; uint8_t buzzerState = 0; +uint8_t toggleBeep = 0; uint32_t currentTime = 0; uint32_t previousTime = 0; uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop @@ -16,6 +17,7 @@ uint8_t GPSModeHold = 0; // if GPS PH is activated uint8_t headFreeMode = 0; // if head free mode is a activated uint8_t passThruMode = 0; // if passthrough mode is activated int16_t headFreeModeHold; + int16_t annex650_overrun_count = 0; uint8_t armed = 0; uint8_t vbat; // battery voltage in 0.1V steps @@ -24,7 +26,8 @@ volatile int16_t failsafeCnt = 0; int16_t failsafeEvents = 0; int16_t rcData[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; // interval [1000;2000] int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -int16_t lookupRX[7]; // lookup table for expo & RC rate +int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL +int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) uint8_t dynP8[3], dynI8[3], dynD8[3]; @@ -50,7 +53,8 @@ uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or h int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis uint8_t GPS_update = 0; // it's a binary toogle to distinct a GPS position update -int16_t GPS_angle[2]; // it's the angles that must be applied for GPS correction +int16_t GPS_angle[2] = { 0, 0 }; // it's the angles that must be applied for GPS correction +uint16_t GPS_ground_course = 0; // degrees*10 //Automatic ACC Offset Calibration // ********************** @@ -79,10 +83,13 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat) } } +#define BREAKPOINT 1500 + // this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds void annexCode(void) { static uint32_t buzzerTime, calibratedAccTime; + uint16_t tmp,tmp2; static uint8_t vbatTimer = 0; static uint8_t buzzerFreq; //delay between buzzer ring uint8_t axis, prop1, prop2; @@ -92,18 +99,19 @@ void annexCode(void) uint8_t i; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value - if (rcData[THROTTLE] < 1500) { + if (rcData[THROTTLE] < BREAKPOINT) { prop2 = 100; - } else if (rcData[THROTTLE] < 2000) { - prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - 1500) / 500; } else { - prop2 = 100 - cfg.dynThrPID; + if (rcData[THROTTLE] < 2000) { + prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT); + } else { + prop2 = 100 - cfg.dynThrPID; + } } for (axis = 0; axis < 3; axis++) { - uint16_t tmp = min(abs(rcData[axis] - cfg.midrc), 500); + tmp = min(abs(rcData[axis] - cfg.midrc), 500); if (axis != 2) { // ROLL & PITCH - uint16_t tmp2; if (cfg.deadband) { if (tmp > cfg.deadband) { tmp -= cfg.deadband; @@ -111,8 +119,9 @@ void annexCode(void) tmp = 0; } } + tmp2 = tmp / 100; - rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100; + rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else { // YAW @@ -131,7 +140,12 @@ void annexCode(void) if (rcData[axis] < cfg.midrc) rcCommand[axis] = -rcCommand[axis]; } - rcCommand[THROTTLE] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - cfg.mincheck) / (2000 - cfg.mincheck); + + tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000); + tmp = (uint32_t)(tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000] + tmp2 = tmp / 100; + rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] + // rcCommand[THROTTLE] = cfg.minthrottle + (int32_t)(cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - cfg.mincheck) / (2000 - cfg.mincheck); if (headFreeMode) { float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f; @@ -157,6 +171,8 @@ void annexCode(void) BEEP_OFF; } else buzzerFreq = 4; // low battery + +#if 0 if (buzzerFreq) { if (buzzerState && (currentTime > buzzerTime + 250000)) { buzzerState = 0; @@ -168,8 +184,11 @@ void annexCode(void) buzzerTime = currentTime; } } +#endif } + buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now + if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis LED0_TOGGLE; } else { @@ -290,8 +309,11 @@ void loop(void) errorAngleI[PITCH] = 0; rcDelayCommand++; if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && armed == 0) { - if (rcDelayCommand == 20) + if (rcDelayCommand == 20) { calibratingG = 400; + if (feature(FEATURE_GPS)) + GPS_reset_home_position(); + } } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (armed == 0 && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) { if (rcDelayCommand == 20) { if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing @@ -300,13 +322,13 @@ void loop(void) } else { AccInflightCalibrationArmed = !AccInflightCalibrationArmed; if (AccInflightCalibrationArmed) { - blinkLED(10, 1, 2); + toggleBeep = 2; } else { - blinkLED(10, 10, 3); + toggleBeep = 3; } } } - } else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) { + } else if (cfg.activate[BOXARM] > 0) { if (rcOptions[BOXARM] && okToArm) { armed = 1; headFreeModeHold = heading; @@ -334,28 +356,28 @@ void loop(void) rcDelayCommand++; } else if (rcData[PITCH] > cfg.maxcheck) { cfg.accTrim[PITCH] += 2; - writeParams(); + writeParams(1); #ifdef LEDRING if (feature(FEATURE_LED_RING)) ledringBlink(); #endif } else if (rcData[PITCH] < cfg.mincheck) { cfg.accTrim[PITCH] -= 2; - writeParams(); + writeParams(1); #ifdef LEDRING if (feature(FEATURE_LED_RING)) ledringBlink(); #endif } else if (rcData[ROLL] > cfg.maxcheck) { cfg.accTrim[ROLL] += 2; - writeParams(); + writeParams(1); #ifdef LEDRING if (feature(FEATURE_LED_RING)) ledringBlink(); #endif } else if (rcData[ROLL] < cfg.mincheck) { cfg.accTrim[ROLL] -= 2; - writeParams(); + writeParams(1); #ifdef LEDRING if (feature(FEATURE_LED_RING)) ledringBlink(); @@ -364,12 +386,6 @@ void loop(void) rcDelayCommand = 0; } } -#ifdef LOG_VALUES - if (cycleTime > cycleTimeMax) - cycleTimeMax = cycleTime; // remember highscore - if (cycleTime < cycleTimeMin) - cycleTimeMin = cycleTime; // remember lowscore -#endif if (feature(FEATURE_INFLIGHT_ACC_CAL)) { if (AccInflightCalibrationArmed && armed == 1 && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement @@ -389,8 +405,11 @@ void loop(void) } for (i = 0; i < CHECKBOXITEMS; i++) { - rcOptions[i] = (((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) << 1 | (rcData[AUX1] > 1700) << 2 | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5) & cfg.activate1[i]) - || (((rcData[AUX3] < 1300) | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 1 | (rcData[AUX3] > 1700) << 2 | (rcData[AUX4] < 1300) << 3 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 4 | (rcData[AUX4] > 1700) << 5) & cfg.activate2[i]); + rcOptions[i] = ( + ((rcData[AUX1] < 1300) | (1300 < rcData[AUX1] && rcData[AUX1] < 1700) <<1 | (rcData[AUX1] > 1700) << 2 + | (rcData[AUX2] < 1300) << 3 | (1300 < rcData[AUX2] && rcData[AUX2] < 1700) << 4 | (rcData[AUX2] > 1700) << 5 + | (rcData[AUX3] < 1300) << 6 | (1300 < rcData[AUX3] && rcData[AUX3] < 1700) << 7 | (rcData[AUX3] > 1700) << 8 + | (rcData[AUX4] < 1300) << 9 | (1300 < rcData[AUX4] && rcData[AUX4] < 1700) << 10| (rcData[AUX4] > 1700) << 11) & cfg.activate[i]) > 0; } // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false @@ -402,7 +421,7 @@ void loop(void) accMode = 1; } } else - accMode = 0; // modified by MIS for failsave support + accMode = 0; // failsave support if ((rcOptions[BOXARM]) == 0) okToArm = 1; @@ -467,32 +486,29 @@ void loop(void) passThruMode = 0; } else { // not in rc loop static int8_t taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes - switch (taskOrder) { + switch (taskOrder++ % 4) { case 0: - taskOrder++; #ifdef MAG if (sensors(SENSOR_MAG)) Mag_getADC(); #endif break; case 1: - taskOrder++; #ifdef BARO if (sensors(SENSOR_BARO)) Baro_update(); #endif break; case 2: - taskOrder++; #ifdef BARO if (sensors(SENSOR_BARO)) getEstimatedAltitude(); #endif break; case 3: - taskOrder++; -#if 0 // GPS - not used as we read gps data in interrupt mode - GPS_NewData(); +#ifdef SONAR + Sonar_update(); + debug3 = sonarAlt; #endif break; default: diff --git a/src/mw.h b/src/mw.h index ee5b0e298..a725a2768 100755 --- a/src/mw.h +++ b/src/mw.h @@ -1,9 +1,5 @@ #pragma once -/* This option should be uncommented if ACC Z is accurate enough when motors are running*/ -/* should now be ok with BMA020 and BMA180 ACC */ -#define TRUSTED_ACCZ - /* Failsave settings - added by MIS Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated. After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered @@ -43,14 +39,7 @@ /* for VBAT monitoring frequency */ #define VBATFREQ 6 // to read battery voltage - keep equal to PSENSORFREQ (6) unless you know what you are doing -// Moving Average Gyros by Magnetron1 (Michele Ardito) ########## beta -//#define MMGYRO // Active Moving Average Function for Gyros -//#define MMGYROVECTORLENGHT 10 // Lenght of Moving Average Vector -// Moving Average ServoGimbal Signal Output -//#define MMSERVOGIMBAL // Active Output Moving Average Function for Servos Gimbal -//#define MMSERVOGIMBALVECTORLENGHT 32 // Lenght of Moving Average Vector - -#define VERSION 20 +#define VERSION 200 // Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization. typedef enum MultiType @@ -128,6 +117,9 @@ typedef struct config_t { uint8_t rcRate8; uint8_t rcExpo8; + uint8_t thrMid8; + uint8_t thrExpo8; + uint8_t rollPitchRate; uint8_t yawRate; @@ -141,8 +133,7 @@ typedef struct config_t { uint16_t gyro_lpf; // mpuX050 LPF setting uint32_t gyro_smoothing_factor; // How much to smoothen with per axis (32bit value with Roll, Pitch, Yaw in bits 24, 16, 8 respectively - uint8_t activate1[CHECKBOXITEMS]; - uint8_t activate2[CHECKBOXITEMS]; + uint16_t activate[CHECKBOXITEMS]; // activate switches uint8_t vbatscale; // adjust this to match battery voltage to reported value uint8_t vbatmaxcellvoltage; // maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery out alarms, in 0.1V units, default is 33 (3.3V) @@ -211,6 +202,7 @@ extern int16_t heading; extern int16_t annex650_overrun_count; extern int32_t pressure; extern int32_t BaroAlt; +extern int16_t sonarAlt; extern int32_t EstAlt; extern int32_t AltHold; extern int16_t errorAltitudeI; @@ -240,7 +232,9 @@ extern uint8_t GPSModeHold; extern uint16_t GPS_altitude; extern uint16_t GPS_speed; // altitude in 0.1m and speed in 0.1m/s - Added by Mis extern uint8_t vbat; -extern int16_t lookupRX[7]; // lookup table for expo & RC rate +extern int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL +extern int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE +extern uint8_t toggleBeep; extern config_t cfg; extern sensor_t acc; @@ -280,7 +274,7 @@ void serialCom(void); // Config void parseRcChannels(const char *input); void readEEPROM(void); -void writeParams(void); +void writeParams(uint8_t b); void checkFirstTime(bool reset); bool sensors(uint32_t mask); void sensorsSet(uint32_t mask); @@ -301,3 +295,4 @@ void cliProcess(void); // gps void gpsInit(uint32_t baudrate); +void GPS_reset_home_position(void); diff --git a/src/sensors.c b/src/sensors.c index 8a8368677..6c781ab8c 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -5,7 +5,7 @@ uint8_t calibratedACC = 0; uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. uint16_t calibratingG = 0; uint8_t calibratingM = 0; -uint16_t acc_1G = 256; // this is the 1G measured acceleration +uint16_t acc_1G = 256; // this is the 1G measured acceleration. int16_t heading, magHold; extern uint16_t InflightcalibratingA; @@ -116,7 +116,7 @@ static void ACC_Common(void) cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G cfg.accTrim[ROLL] = 0; cfg.accTrim[PITCH] = 0; - writeParams(); // write accZero in EEPROM + writeParams(1); // write accZero in EEPROM } calibratingA--; } @@ -149,7 +149,7 @@ static void ACC_Common(void) if (InflightcalibratingA == 1) { AccInflightCalibrationActive = 0; AccInflightCalibrationMeasurementDone = 1; - blinkLED(10, 10, 2); //buzzer for indicatiing the start inflight + toggleBeep = 2; //buzzer for indicatiing the end of calibration // recover saved values to maintain current flight behavior until new values are transferred cfg.accZero[ROLL] = accZero_saved[ROLL]; cfg.accZero[PITCH] = accZero_saved[PITCH]; @@ -167,7 +167,7 @@ static void ACC_Common(void) cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G cfg.accTrim[ROLL] = 0; cfg.accTrim[PITCH] = 0; - writeParams(); // write accZero in EEPROM + writeParams(1); // write accZero in EEPROM } } @@ -367,7 +367,7 @@ void Mag_getADC(void) tCal = 0; for (axis = 0; axis < 3; axis++) cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; - writeParams(); + writeParams(1); } } } diff --git a/src/serial.c b/src/serial.c index 64f90fa1c..38eb0d6fc 100755 --- a/src/serial.c +++ b/src/serial.c @@ -1,26 +1,361 @@ #include "board.h" #include "mw.h" -// signal that we're in cli mode -uint8_t cliMode = 0; +#define MSP_IDENT 100 //out message multitype + version +#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation +#define MSP_RAW_IMU 102 //out message 9 DOF +#define MSP_SERVO 103 //out message 8 servos +#define MSP_MOTOR 104 //out message 8 motors +#define MSP_RC 105 //out message 8 rc chan +#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed +#define MSP_COMP_GPS 107 //out message distance home, direction home +#define MSP_ATTITUDE 108 //out message 2 angles 1 heading +#define MSP_ALTITUDE 109 //out message 1 altitude +#define MSP_BAT 110 //out message vbat, powermetersum +#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_PID 112 //out message up to 16 P I D (8 are used) +#define MSP_BOX 113 //out message up to 16 checkbox (11 are used) +#define MSP_MISC 114 //out message powermeter trig + 8 free for future use + +#define MSP_SET_RAW_RC 200 //in message 8 rc chan +#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed +#define MSP_SET_PID 202 //in message up to 16 P I D (8 are used) +#define MSP_SET_BOX 203 //in message up to 16 checkbox (11 are used) +#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID +#define MSP_ACC_CALIBRATION 205 //in message no param +#define MSP_MAG_CALIBRATION 206 //in message no param +#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use +#define MSP_RESET_CONF 208 //in message no param + +#define MSP_EEPROM_WRITE 250 //in message no param + +#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 + +static uint8_t checksum, stateMSP, indRX, inBuf[64]; + +void serialize32(uint32_t a) +{ + static uint8_t t; + t = a; + uartWrite(t); + checksum ^= t; + t = a >> 8; + uartWrite(t); + checksum ^= t; + t = a >> 16; + uartWrite(t); + checksum ^= t; + t = a >> 24; + uartWrite(t); + checksum ^= t; +} void serialize16(int16_t a) { - uartWrite(a); - uartWrite(a >> 8 & 0xff); + static uint8_t t; + t = a; + uartWrite(t); + checksum ^= t; + t = a >> 8 & 0xff; + uartWrite(t); + checksum ^= t; } void serialize8(uint8_t a) { uartWrite(a); + checksum ^= a; } +uint32_t read32(void) +{ + uint32_t t = inBuf[indRX++]; + t+= inBuf[indRX++] << 8; + t+= (uint32_t)inBuf[indRX++] << 16; + t+= (uint32_t)inBuf[indRX++] << 24; + return t; +} + +uint16_t read16(void) +{ + uint16_t t = inBuf[indRX++]; + t+= inBuf[indRX++] << 8; + return t; +} + +uint8_t read8(void) +{ + return inBuf[indRX++] & 0xff; +} + +void headSerialReply(uint8_t c, uint8_t s) +{ + serialize8('$'); + serialize8('M'); + serialize8('>'); + serialize8(s); + serialize8(c); + checksum = 0; +} + +void tailSerialReply(void) +{ + serialize8(checksum); + // no need to send + // UartSendData(); +} + +// signal that we're in cli mode +uint8_t cliMode = 0; + void serialInit(uint32_t baudrate) { uartInit(baudrate); } void serialCom(void) +{ + uint8_t i, c; + static uint8_t offset, dataSize; + + // in cli mode, all uart stuff goes to here. enter cli mode by sending # + if (cliMode) { + cliProcess(); + return; + } + + while (uartAvailable()) { + c = uartRead(); + + if (stateMSP > 99) { // a message with a length indication, indicating a non null payload + if (offset <= dataSize) { // there are still some octets to read (including checksum) to complete a full message + if (offset < dataSize) + checksum ^= c; // the checksum is computed, except for the last octet + inBuf[offset++] = c; + } else { // we have read all the payload + if (checksum == inBuf[dataSize]) { // we check is the computed checksum is ok + switch (stateMSP) { // if yes, then we execute different code depending on the message code. read8/16/32 will look into the inBuf buffer + case MSP_SET_RAW_RC: + for (i = 0; i < 8; i++) { + rcData[i] = read16(); + } + break; + case MSP_SET_RAW_GPS: + GPS_fix = read8(); + GPS_numSat = read8(); + GPS_latitude = read32(); + GPS_longitude = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); + GPS_update = 1; + break; + case MSP_SET_PID: + for (i = 0; i < PIDITEMS; i++) { + cfg.P8[i] = read8(); + cfg.I8[i] = read8(); + cfg.D8[i] = read8(); + } + break; + case MSP_SET_BOX: + for (i = 0; i < CHECKBOXITEMS; i++) { + cfg.activate[i] = read16(); + } + break; + case MSP_SET_RC_TUNING: + cfg.rcRate8 = read8(); + cfg.rcExpo8 = read8(); + cfg.rollPitchRate = read8(); + cfg.yawRate = read8(); + cfg.dynThrPID = read8(); + cfg.thrMid8 = read8(); + cfg.thrExpo8 = read8(); + break; + case MSP_SET_MISC: + break; + } + } + stateMSP = 0; // in any case we reset the MSP state + } + } + + if (stateMSP < 5) { + if (stateMSP == 4) { // this protocol state indicates we have a message with a lenght indication, and we read here the message code (fifth octet) + if (c > 99) { // we check if it's a valid code (should be >99) + stateMSP = c; // the message code is then reuse to feed the protocol state + offset = 0; + checksum = 0; + indRX = 0; // and we init some values which will be used in the next loops to grasp the payload + } else { + stateMSP = 0; // the message code seems to be invalid. this should not happen => we reset the protocol state + } + } + if (stateMSP == 3) { // here, we need to check if the fourth octet indicates a code indication (>99) or a payload lenght indication (<100) + if (c < 100) { // a message with a length indication, indicating a non null payload + stateMSP++; // we update the protocol state to read the next octet + dataSize = c; // we store the payload lenght + if (dataSize > 63) + dataSize = 63; // in order to avoid overflow, we limit the size. this should not happen + } else { + switch (c) { // if we are here, the fourth octet indicates a code message + case MSP_IDENT: // and we check message code to execute the relative code + headSerialReply(c, 2); // we reply with an header indicating a payload lenght of 2 octets + serialize8(VERSION); // the first octet. serialize8/16/32 is used also to compute a checksum + serialize8(cfg.mixerConfiguration); // the second one + tailSerialReply(); + break; // mainly to send the last octet which is the checksum + case MSP_STATUS: + headSerialReply(c, 8); + serialize16(cycleTime); + serialize16(i2cGetErrorCounter()); + serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); + serialize16(accMode << BOXACC | baroMode << BOXBARO | magMode << BOXMAG | armed << BOXARM | GPSModeHome << BOXGPSHOME | GPSModeHold << BOXGPSHOLD | headFreeMode << BOXHEADFREE); + tailSerialReply(); + break; + case MSP_RAW_IMU: + headSerialReply(c, 18); + for (i = 0; i < 3; i++) + serialize16(accSmooth[i]); + for (i = 0; i < 3; i++) + serialize16(gyroData[i]); + for (i = 0; i < 3; i++) + serialize16(magADC[i]); + tailSerialReply(); + break; + case MSP_SERVO: + headSerialReply(c, 16); + for (i = 0; i < 8; i++) + serialize16(servo[i]); + tailSerialReply(); + break; + case MSP_MOTOR: + headSerialReply(c, 16); + for (i = 0; i < 8; i++) + serialize16(motor[i]); + tailSerialReply(); + break; + case MSP_RC: + headSerialReply(c, 16); + for (i = 0; i < 8; i++) + serialize16(rcData[i]); + tailSerialReply(); + break; + case MSP_RAW_GPS: + headSerialReply(c, 14); + serialize8(GPS_fix); + serialize8(GPS_numSat); + serialize32(GPS_latitude); + serialize32(GPS_longitude); + serialize16(GPS_altitude); + serialize16(GPS_speed); + tailSerialReply(); + break; + case MSP_COMP_GPS: + headSerialReply(c, 5); + serialize16(GPS_distanceToHome); + serialize16(GPS_directionToHome + 180); + serialize8(GPS_update); + tailSerialReply(); + break; + case MSP_ATTITUDE: + headSerialReply(c, 6); + for (i = 0; i < 2; i++) + serialize16(angle[i]); + serialize16(heading); + tailSerialReply(); + break; + case MSP_ALTITUDE: + headSerialReply(c, 4); + serialize32(EstAlt); + tailSerialReply(); + break; + case MSP_BAT: + headSerialReply(c, 3); + serialize8(vbat); + serialize16(0); + tailSerialReply(); + break; + case MSP_RC_TUNING: + headSerialReply(c, 7); + serialize8(cfg.rcRate8); + serialize8(cfg.rcExpo8); + serialize8(cfg.rollPitchRate); + serialize8(cfg.yawRate); + serialize8(cfg.dynThrPID); + serialize8(cfg.thrMid8); + serialize8(cfg.thrExpo8); + tailSerialReply(); + break; + case MSP_PID: + headSerialReply(c, 3 * PIDITEMS); + for (i = 0; i < PIDITEMS; i++) { + serialize8(cfg.P8[i]); + serialize8(cfg.I8[i]); + serialize8(cfg.D8[i]); + } + tailSerialReply(); + break; + case MSP_BOX: + headSerialReply(c, 2 * CHECKBOXITEMS); + for (i = 0; i < CHECKBOXITEMS; i++) { + serialize16(cfg.activate[i]); + } + tailSerialReply(); + break; + case MSP_MISC: + headSerialReply(c, 2); + serialize16(0); + tailSerialReply(); + break; + case MSP_RESET_CONF: + checkFirstTime(true); + break; + case MSP_ACC_CALIBRATION: + calibratingA = 400; + break; + case MSP_MAG_CALIBRATION: + calibratingM = 1; + break; + case MSP_EEPROM_WRITE: + writeParams(0); + break; + case MSP_DEBUG: + headSerialReply(c, 8); + serialize16(debug1); // 4 variables are here for general monitoring purpose + serialize16(debug2); + serialize16(debug3); + serialize16(debug4); + tailSerialReply(); + break; + } + stateMSP = 0; // we reset the protocol state for the next loop + } + } else { + switch (c) { // header detection $MW< + case '$': + if (stateMSP == 0) + stateMSP++; + break; // first octet ok, no need to go further + case 'M': + if (stateMSP == 1) + stateMSP++; + break; // second octet ok, no need to go further + case '<': + if (stateMSP == 2) + stateMSP++; + break; // third octet ok, no need to go further + } + } + } + if (stateMSP == 0) { // still compliant with older single octet command + // enable CLI + if (c == '#') + cliProcess(); + } + } +} + +#if 0 +void oldSserialCom(void) { uint8_t i; @@ -83,10 +418,8 @@ void serialCom(void) serialize8(cfg.rollPitchRate); serialize8(cfg.yawRate); serialize8(cfg.dynThrPID); - for (i = 0; i < CHECKBOXITEMS; i++) { - serialize8(cfg.activate1[i]); - serialize8(cfg.activate2[i] | (rcOptions[i] << 7)); // use highest bit to transport state in mwc - } + for (i = 0; i < CHECKBOXITEMS; i++) + serialize16(cfg.activate[i]); serialize16(GPS_distanceToHome); serialize16(GPS_directionToHome + 180); serialize8(GPS_numSat); @@ -145,13 +478,11 @@ void serialCom(void) cfg.rollPitchRate = uartReadPoll(); cfg.yawRate = uartReadPoll(); //4 cfg.dynThrPID = uartReadPoll(); //5 - for (i = 0; i < CHECKBOXITEMS; i++) { - cfg.activate1[i] = uartReadPoll(); - cfg.activate2[i] = uartReadPoll(); - } + for (i = 0; i < CHECKBOXITEMS; i++) + cfg.activate[i] = uartReadPoll(); uartReadPoll(); // power meter crap, removed uartReadPoll(); // power meter crap, removed - writeParams(); + writeParams(0); break; case 'S': // GUI to arduino ACC calibration request calibratingA = 400; @@ -162,3 +493,4 @@ void serialCom(void) } } } +#endif