From 9b48d45bca0d482ffaf3a7a6ab3450f3aad3f1c2 Mon Sep 17 00:00:00 2001 From: timecop Date: Mon, 5 Mar 2012 02:01:26 +0000 Subject: [PATCH] trashed old eeprom config struct and retarded eeprom code. replaced with config_t stuff to allow more dynamic changes implemented cli (press # in serial to enter it). no commands except exit/version yet. added uartPrint fixed bug in task switching with missing breaks - was failing baro and perhaps mag readings dynamic yaw direction, camtilt feature, camtrig feature. ported some of 2.0-pre1 features: * gyro smoothing * baro/althold cleanup git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@102 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61 --- baseflight.uvgui.timecop | 128 ++++++++++++++------------- baseflight.uvopt | 130 ++++++++++++--------------- baseflight.uvproj | 10 +++ board.h | 6 ++ cli.c | 117 ++++++++++++++++++++++++ config.c | 186 ++++++++++++++++----------------------- drv_uart.c | 6 ++ drv_uart.h | 1 + imu.c | 79 +++++++---------- mixer.c | 125 +++++++++++++------------- mw.c | 141 ++++++++++++++--------------- mw.h | 135 ++++++++++++++++------------ sensors.c | 62 ++++++++----- serial.c | 64 ++++++++------ 14 files changed, 667 insertions(+), 523 deletions(-) create mode 100644 cli.c diff --git a/baseflight.uvgui.timecop b/baseflight.uvgui.timecop index 7fb8b5ed6..a17cb5f24 100755 --- a/baseflight.uvgui.timecop +++ b/baseflight.uvgui.timecop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fly_122\projects\baseflight\drv_system.h + D:\fly_122\projects\baseflight\drv_uart.h 0 1 - 15 + 9 + + + C:\Keil\ARM\RV31\Inc\stdlib.h + 0 + 477 + 477 @@ -8052,102 +8058,102 @@ 0 100 - 13 - - .\drv_pwm.c - 8 - 258 - 258 - + 11 - .\config.c - 27 - 103 - 115 - - - .\drv_system.c + .\cli.c 0 1 1 - mw.h + .\serial.c 0 - 225 - 253 + 220 + 233 - .\main.c + .\drv_uart.c 0 - 10 - 24 + 110 + 144 - .\mixer.c + drv_uart.h 0 - 92 - 108 + 1 + 9 - .\serial.c - 12 - 190 - 208 + .\startup_stm32f10x_md.s + 0 + 133 + 133 - drv_system.h + .\mw.c 0 - 1 - 15 + 660 + 679 - .\drv_bmp085.c + .\imu.c 0 - 246 - 264 + 67 + 89 .\drv_i2c.c - 1 + 0 122 - 143 + 131 - .\imu.c - 4 - 55 - 73 + board.h + 26 + 14 + 38 - .\startup_stm32f10x_md.s + .\drv_system.c 0 - 147 - 180 + 34 + 43 - .\drv_mpu3050.c - 39 - 1 - 1 + C:\Keil\ARM\RV31\Inc\stdlib.h + 0 + 477 + 477 .\sensors.c - 21 - 183 - 214 + 0 + 168 + 203 - .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_flash.c - 34 - 852 - 858 + mw.h + 0 + 187 + 200 - .\drv_hmc5883l.c - 22 - 13 - 27 + .\config.c + 0 + 92 + 103 + + + .\mixer.c + 0 + 235 + 248 + + + .\main.c + 8 + 16 + 25 diff --git a/baseflight.uvopt b/baseflight.uvopt index 0dd072494..61d95ae7f 100755 --- a/baseflight.uvopt +++ b/baseflight.uvopt @@ -157,38 +157,6 @@ -UV0168AVR -O238 -S8 -C0 -N00("ARM CoreSight SW-DP") -D00(1BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000 - - - 0 - 0 - 114 - 1 -
134231132
- 0 - 0 - 0 - 0 - 1 - - - \\baseflight\config.c\114 - - - 1 - 0 - 77 - 1 -
134230968
- 0 - 0 - 0 - 0 - 1 - - - \\baseflight\config.c\77 - - 0 @@ -510,10 +478,10 @@ 1 0 0 - 0 + 8 0 - 10 - 24 + 16 + 25 0 .\main.c main.c @@ -524,10 +492,10 @@ 5 0 0 - 25 + 26 0 - 0 - 0 + 14 + 38 0 .\board.h board.h @@ -538,10 +506,10 @@ 1 0 0 - 4 + 0 0 - 55 - 73 + 67 + 89 0 .\imu.c imu.c @@ -554,8 +522,8 @@ 0 0 0 - 0 - 0 + 602 + 634 0 .\mw.c mw.c @@ -568,8 +536,8 @@ 0 0 0 - 225 - 253 + 187 + 200 0 .\mw.h mw.h @@ -580,10 +548,10 @@ 1 0 0 - 21 + 18 0 - 183 - 214 + 124 + 148 0 .\sensors.c sensors.c @@ -596,8 +564,8 @@ 0 0 0 - 92 - 108 + 235 + 248 0 .\mixer.c mixer.c @@ -608,10 +576,10 @@ 1 0 0 - 12 + 0 0 - 190 - 208 + 220 + 233 0 .\serial.c serial.c @@ -622,14 +590,28 @@ 1 0 0 - 27 + 0 0 - 103 - 115 + 92 + 103 0 .\config.c config.c + + 1 + 0 + 1 + 0 + 0 + 0 + 0 + 1 + 1 + 0 + .\cli.c + cli.c + @@ -643,10 +625,10 @@ 1 0 0 - 1 + 0 0 122 - 143 + 131 0 .\drv_i2c.c drv_i2c.c @@ -659,8 +641,8 @@ 0 8 0 - 258 - 258 + 0 + 0 0 .\drv_pwm.c drv_pwm.c @@ -687,8 +669,8 @@ 0 0 0 - 246 - 264 + 0 + 0 0 .\drv_bmp085.c drv_bmp085.c @@ -715,8 +697,8 @@ 0 39 0 - 1 - 1 + 0 + 0 0 .\drv_mpu3050.c drv_mpu3050.c @@ -729,8 +711,8 @@ 0 0 0 - 0 - 0 + 110 + 144 0 .\drv_uart.c drv_uart.c @@ -743,8 +725,8 @@ 0 0 0 - 1 - 1 + 34 + 43 0 .\drv_system.c drv_system.c @@ -757,8 +739,8 @@ 0 22 0 - 13 - 27 + 0 + 0 0 .\drv_hmc5883l.c drv_hmc5883l.c @@ -932,8 +914,8 @@ 0 34 0 - 852 - 858 + 0 + 0 0 .\lib\STM32F10x_StdPeriph_Driver\src\stm32f10x_flash.c stm32f10x_flash.c @@ -946,8 +928,8 @@ 0 0 0 - 147 - 180 + 133 + 133 0 .\startup_stm32f10x_md.s startup_stm32f10x_md.s diff --git a/baseflight.uvproj b/baseflight.uvproj index b0f24fc45..345e36f0e 100755 --- a/baseflight.uvproj +++ b/baseflight.uvproj @@ -431,6 +431,11 @@ 1 .\config.c + + cli.c + 1 + .\cli.c + @@ -980,6 +985,11 @@ 1 .\config.c + + cli.c + 1 + .\cli.c + diff --git a/board.h b/board.h index fcbd6fa68..daedee56d 100755 --- a/board.h +++ b/board.h @@ -1,8 +1,10 @@ #pragma once #include +#include #include #include +#include #include "stm32f10x_conf.h" #include "core_cm3.h" @@ -30,6 +32,10 @@ typedef enum { FEATURE_VBAT = 1 << 1, FEATURE_SERVO = 1 << 2, FEATURE_DIGITAL_SERVO = 1 << 3, + FEATURE_MOTOR_STOP = 1 << 4, + FEATURE_SERVO_TILT = 1 << 5, + FEATURE_CAMTRIG = 1 << 6, + FEATURE_GYRO_SMOOTHING = 1 << 7, } AvailableFeatures; #define digitalHi(p, i) { p->BSRR = i; } diff --git a/cli.c b/cli.c new file mode 100644 index 000000000..471d441b9 --- /dev/null +++ b/cli.c @@ -0,0 +1,117 @@ +#include "board.h" +#include "mw.h" + +// we unset this on 'exit' +extern uint8_t cliMode; +static void cliExit(char *cmdline); +static void cliHelp(char *cmdline); +static void cliVersion(char *cmdline); + +// buffer +char cliBuffer[32]; +uint8_t bufferIndex = 0; + +typedef struct { + char *name; + char *param; + void (*func)(char *cmdline); +} cliCmd; + +// should be sorted a..z for bsearch() +const cliCmd cmdTable[] = { + { "exit", "", cliExit }, + { "help", "", cliHelp }, + { "version", "", cliVersion }, +}; +#define CMD_COUNT (sizeof cmdTable / sizeof cmdTable[0]) + +static void cliPrompt(void) +{ + uartPrint("\r\n# "); + memset(cliBuffer, 0, sizeof(cliBuffer)); + bufferIndex = 0; +} + +static int cliCompare(const void *a, const void *b) +{ + const cliCmd *ca = a, *cb = b; + return strncasecmp(ca->name, cb->name, strlen(cb->name)); +} + +static void cliExit(char *cmdline) +{ + uartPrint("Leaving CLI mode...\r\n"); + memset(cliBuffer, 0, sizeof(cliBuffer)); + bufferIndex = 0; + cliMode = 0; +} + +static void cliHelp(char *cmdline) +{ + uint8_t i = 0; + + uartPrint("Available commands:\r\n"); + + for (i = 0; i < CMD_COUNT; i++) { + uartPrint((uint8_t *)cmdTable[i].name); + uartWrite(' '); + uartPrint((uint8_t *)cmdTable[i].param); + uartPrint("\r\n"); + } +} + +static void cliVersion(char *cmdline) +{ + uartPrint("Afro32 CLI version 2.0-pre1"); +} + +void cliProcess(void) +{ + while (uartAvailable()) { + uint8_t c = uartRead(); + + cliBuffer[bufferIndex++] = c; + if (bufferIndex == sizeof(cliBuffer)) { + bufferIndex--; + c = '\n'; + } + + if (bufferIndex && (c == '\n' || c == '\r')) { + // enter pressed + cliCmd *cmd = NULL; + cliCmd target; + uartPrint("\r\n"); + cliBuffer[bufferIndex] = 0; // null terminate + + target.name = cliBuffer; + target.param = NULL; + + cmd = bsearch(&target, cmdTable, CMD_COUNT, sizeof cmdTable[0], cliCompare); + if (cmd) + cmd->func(cliBuffer + strlen(cmd->name)); + else + uartPrint("ERR: Unknown command, try 'HELP'"); + + // 'exit' will reset this flag, so we don't need to print prompt again + if (cliMode) + cliPrompt(); + + } else if (c == 127) { + // backspace + if (bufferIndex > 1) { + cliBuffer[bufferIndex - 2] = 0; + uartPrint("\r# "); + uartPrint((uint8_t *)cliBuffer); + uartWrite(' '); + uartPrint("\r# "); + uartPrint((uint8_t *)cliBuffer); + bufferIndex -= 2; + } + } else if (c < 32 || c > 126) { + // non-printable ascii + bufferIndex--; + } else { + uartWrite(c); + } + } +} diff --git a/config.c b/config.c index f7cb0b5e3..a6abec52e 100755 --- a/config.c +++ b/config.c @@ -5,93 +5,45 @@ #define FLASH_PAGE_SIZE ((uint16_t)0x400) #define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * 63) // use the last KB for storage -uint32_t enabledSensors = 0; -uint32_t enabledFeatures = 0; - -static uint8_t checkNewConf = 152; - -typedef struct eep_entry_t { - void *var; - uint8_t size; -} eep_entry_t; - -// ************************************************************************************************************ -// EEPROM Layout definition -// ************************************************************************************************************ -volatile eep_entry_t eep_entry[] = { - {&checkNewConf, sizeof(checkNewConf)} - , {&enabledFeatures, sizeof(enabledFeatures)} - , {&mixerConfiguration, sizeof(mixerConfiguration)} - , {&P8, sizeof(P8)} - , {&I8, sizeof(I8)} - , {&D8, sizeof(D8)} - , {&rcRate8, sizeof(rcRate8)} - , {&rcExpo8, sizeof(rcExpo8)} - , {&rollPitchRate, sizeof(rollPitchRate)} - , {&yawRate, sizeof(yawRate)} - , {&dynThrPID, sizeof(dynThrPID)} - , {&accZero, sizeof(accZero)} - , {&magZero, sizeof(magZero)} - , {&accTrim, sizeof(accTrim)} - , {&activate1, sizeof(activate1)} - , {&activate2, sizeof(activate2)} - , {&powerTrigger1, sizeof(powerTrigger1)} - , {&wing_left_mid, sizeof(wing_left_mid)} - , {&wing_right_mid, sizeof(wing_right_mid)} - , {&tri_yaw_middle, sizeof(tri_yaw_middle)} -}; - -#define EEBLOCK_SIZE sizeof(eep_entry) / sizeof(eep_entry_t) +config_t cfg; + +static uint32_t enabledSensors = 0; +static uint8_t checkNewConf = 2; void readEEPROM(void) { - uint8_t i, _address = eep_entry[0].size; + uint8_t i; // Read flash - for (i = 1; i < EEBLOCK_SIZE; i++) { - memcpy(eep_entry[i].var, (char *)FLASH_WRITE_ADDR + _address, eep_entry[i].size); - _address += eep_entry[i].size; - } + memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t)); #if defined(POWERMETER) - pAlarm = (uint32_t) powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying + pAlarm = (uint32_t) cfg.powerTrigger1 *(uint32_t) PLEVELSCALE *(uint32_t) PLEVELDIV; // need to cast before multiplying #endif for (i = 0; i < 7; i++) - lookupRX[i] = (2500 + rcExpo8 * (i * i - 25)) * i * (int32_t) rcRate8 / 1250; + lookupRX[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 1250; - wing_left_mid = constrain(wing_left_mid, WING_LEFT_MIN, WING_LEFT_MAX); //LEFT - wing_right_mid = constrain(wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); //RIGHT - tri_yaw_middle = constrain(tri_yaw_middle, TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR + 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) { - FLASH_Status FLASHStatus; - uint32_t address; - uint8_t conf[256]; - uint8_t *p = conf; - uint8_t i; + FLASH_Status status; + uint32_t i; - // TODO this is garbage. do it properly later using FLASH_ProgramHalfWord without caching shit. - for (i = 0; i < EEBLOCK_SIZE; i++) { - memcpy(p, eep_entry[i].var, eep_entry[i].size); - p += eep_entry[i].size; - } - - p = conf; - FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); - - if ((FLASHStatus = FLASH_ErasePage(FLASH_WRITE_ADDR)) == FLASH_COMPLETE) { - address = 0; - while (FLASHStatus == FLASH_COMPLETE && address < sizeof(eep_entry)) { - if ((FLASHStatus = FLASH_ProgramWord(FLASH_WRITE_ADDR + address, *(uint32_t *)((char *)p + address))) != FLASH_COMPLETE) - break; - address += 4; - } + + if (FLASH_ErasePage(FLASH_WRITE_ADDR) == FLASH_COMPLETE) { + for (i = 0; i < sizeof(config_t); i += 4) { + status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *)((char *)&cfg + i)); + if (status != FLASH_COMPLETE) + break; // TODO: fail + } } FLASH_Lock(); @@ -103,55 +55,69 @@ void writeParams(void) void checkFirstTime(void) { uint8_t test_val, i; - + test_val = *(uint8_t *)FLASH_WRITE_ADDR; if (test_val == checkNewConf) return; // Default settings - mixerConfiguration = MULTITYPE_QUADX; + cfg.version = checkNewConf; + cfg.mixerConfiguration = MULTITYPE_QUADX; featureClearAll(); featureSet(FEATURE_VBAT | FEATURE_PPM); - P8[ROLL] = 40; - I8[ROLL] = 30; - D8[ROLL] = 23; - P8[PITCH] = 40; - I8[PITCH] = 30; - D8[PITCH] = 23; - P8[YAW] = 85; - I8[YAW] = 0; - D8[YAW] = 0; - P8[PIDALT] = 16; - I8[PIDALT] = 15; - D8[PIDALT] = 7; - P8[PIDGPS] = 10; - I8[PIDGPS] = 0; - D8[PIDGPS] = 0; - P8[PIDVEL] = 0; - I8[PIDVEL] = 0; - D8[PIDVEL] = 0; - P8[PIDLEVEL] = 90; - I8[PIDLEVEL] = 45; - D8[PIDLEVEL] = 100; - P8[PIDMAG] = 40; - rcRate8 = 45; // = 0.9 in GUI - rcExpo8 = 65; - rollPitchRate = 0; - yawRate = 0; - dynThrPID = 0; + cfg.P8[ROLL] = 40; + cfg.I8[ROLL] = 30; + cfg.D8[ROLL] = 23; + cfg.P8[PITCH] = 40; + cfg.I8[PITCH] = 30; + cfg.D8[PITCH] = 23; + cfg.P8[YAW] = 85; + cfg.I8[YAW] = 0; + cfg.D8[YAW] = 0; + cfg.P8[PIDALT] = 16; + cfg.I8[PIDALT] = 15; + cfg.D8[PIDALT] = 7; + cfg.P8[PIDGPS] = 50; + cfg.I8[PIDGPS] = 0; + cfg.D8[PIDGPS] = 15; + cfg.P8[PIDVEL] = 0; + cfg.I8[PIDVEL] = 0; + cfg.D8[PIDVEL] = 0; + cfg.P8[PIDLEVEL] = 90; + cfg.I8[PIDLEVEL] = 45; + cfg.D8[PIDLEVEL] = 100; + cfg.P8[PIDMAG] = 40; + cfg.rcRate8 = 45; // = 0.9 in GUI + cfg.rcExpo8 = 65; + cfg.rollPitchRate = 0; + cfg.yawRate = 0; + cfg.dynThrPID = 0; for (i = 0; i < CHECKBOXITEMS; i++) { - activate1[i] = 0; - activate2[i] = 0; + cfg.activate1[i] = 0; + cfg.activate2[i] = 0; } - accTrim[0] = 0; - accTrim[1] = 0; - powerTrigger1 = 0; - - wing_left_mid = WING_LEFT_MID; - wing_right_mid = WING_RIGHT_MID; - tri_yaw_middle = TRI_YAW_MIDDLE; + cfg.accTrim[0] = 0; + cfg.accTrim[1] = 0; + cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y + cfg.powerTrigger1 = 0; + + // Radio/ESC + cfg.midrc = 1500; + cfg.minthrottle = 1150; + cfg.maxthrottle = 1850; + cfg.mincommand = 1000; + + // servos + cfg.yaw_direction = 1; + cfg.wing_left_mid = 1500; + cfg.wing_right_mid = 1500; + cfg.tri_yaw_middle = 1500; + + // gimbal + cfg.tilt_pitch_prop = 10; + cfg.tilt_roll_prop = 10; writeParams(); } @@ -173,20 +139,20 @@ void sensorsClear(uint32_t mask) bool feature(uint32_t mask) { - return enabledFeatures & mask; + return cfg.enabledFeatures & mask; } void featureSet(uint32_t mask) { - enabledFeatures |= mask; + cfg.enabledFeatures |= mask; } void featureClear(uint32_t mask) { - enabledFeatures &= ~(mask); + cfg.enabledFeatures &= ~(mask); } void featureClearAll() { - enabledFeatures = 0; + cfg.enabledFeatures = 0; } diff --git a/drv_uart.c b/drv_uart.c index dcdca5bd8..d0a095813 100755 --- a/drv_uart.c +++ b/drv_uart.c @@ -139,3 +139,9 @@ void uartWrite(uint8_t ch) if (!(DMA1_Channel4->CCR & 1)) uartTxDMA(); } + +void uartPrint(uint8_t *str) +{ + while (*str) + uartWrite(*(str++)); +} diff --git a/drv_uart.h b/drv_uart.h index a6f02fc7a..a404ce06b 100755 --- a/drv_uart.h +++ b/drv_uart.h @@ -5,3 +5,4 @@ uint16_t uartAvailable(void); uint8_t uartRead(void); uint8_t uartReadPoll(void); void uartWrite(uint8_t ch); +void uartPrint(uint8_t *str); diff --git a/imu.c b/imu.c index 6d688f70a..0d13c5549 100755 --- a/imu.c +++ b/imu.c @@ -5,21 +5,17 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; int16_t acc_25deg = 0; -int32_t pressure = 0; int32_t BaroAlt; int32_t EstAlt; // in cm int16_t BaroPID = 0; int32_t AltHold; int16_t errorAltitudeI = 0; -int32_t EstVelocity; // ************** // gyro+acc IMU // ************** int16_t gyroData[3] = { 0, 0, 0 }; int16_t gyroZero[3] = { 0, 0, 0 }; -int16_t accZero[3] = { 0, 0, 0 }; -int16_t magZero[3] = { 0, 0, 0 }; int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 int8_t smallAngle25 = 1; @@ -70,8 +66,22 @@ void computeIMU(void) if (!sensors(SENSOR_ACC)) accADC[axis] = 0; } - - if (mixerConfiguration == MULTITYPE_TRI) { + + if (feature(FEATURE_GYRO_SMOOTHING)) { + static uint8_t Smoothing[3] = { 0, 0, 0 }; + static int16_t gyroSmooth[3] = { 0, 0, 0 }; + if (Smoothing[0] == 0) { + // initialize + Smoothing[ROLL] = (cfg.gyro_smoothing_factor >> 16) & 0xff; + Smoothing[PITCH] = (cfg.gyro_smoothing_factor >> 8) & 0xff; + Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff; + } + for (axis = 0; axis < 3; axis++) { + Smoothing[axis] = constrain(Smoothing[axis], 1, 100); // Avoid to divide with Zero + gyroData[axis] = (gyroSmooth[axis] * (Smoothing[axis] - 1) + gyroData[axis] + 1) / Smoothing[axis]; + gyroSmooth[axis] = gyroData[axis]; + } + } else if (cfg.mixerConfiguration == MULTITYPE_TRI) { gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3; gyroYawSmooth = gyroData[YAW]; } @@ -287,76 +297,53 @@ static void getEstimatedAttitude(void) } } -float InvSqrt(float x) -{ - union { - int32_t i; - float f; - } conv; - conv.f = x; - conv.i = 0x5f3759df - (conv.i >> 1); - return 0.5f * conv.f * (3.0f - x * conv.f * conv.f); -} - -int32_t isq(int32_t x) -{ - return x * x; -} - #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) #define INIT_DELAY 4000000 // 4 sec initialization delay #define BARO_TAB_SIZE 40 -#define Kp1 5.5f // PI observer velocity gain -#define Kp2 10.0f // PI observer position gain -#define Ki 0.01f // PI observer integral gain (bias cancellation) -#define dt (UPDATE_INTERVAL / 1000000.0f) void getEstimatedAltitude(void) { - static uint8_t inited = 0; + uint8_t index; static uint32_t deadLine = INIT_DELAY; static int16_t BaroHistTab[BARO_TAB_SIZE]; - static int8_t BaroHistIdx = 0; + static int8_t BaroHistIdx; int32_t BaroHigh, BaroLow; int32_t temp32; + int16_t last; if (currentTime < deadLine) return; deadLine = currentTime + UPDATE_INTERVAL; - if (!inited) { - inited = 1; - EstAlt = BaroAlt; - } - //**** Alt. Set Point stabilization PID **** //calculate speed for D calculation - BaroHistTab[BaroHistIdx] = BaroAlt; - BaroHigh = 0; - BaroLow = 0; - BaroPID = 0; - for (temp32 = 0; temp32 < BARO_TAB_SIZE / 2; temp32++) { - BaroHigh += BaroHistTab[(BaroHistIdx - temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum last half samples - BaroLow += BaroHistTab[(BaroHistIdx + temp32 + BARO_TAB_SIZE) % BARO_TAB_SIZE]; // sum older samples - } + last = BaroHistTab[BaroHistIdx]; + BaroHistTab[BaroHistIdx] = BaroAlt / 10; + BaroHigh += BaroHistTab[BaroHistIdx]; + index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE; + BaroHigh -= BaroHistTab[index]; + BaroLow += BaroHistTab[index]; + BaroLow -= last; BaroHistIdx++; if (BaroHistIdx >= BARO_TAB_SIZE) BaroHistIdx = 0; - temp32 = D8[PIDALT] * (BaroHigh - BaroLow) / 400; + BaroPID = 0; + //D + temp32 = cfg.D8[PIDALT] * (BaroHigh - BaroLow) / 40; BaroPID -= temp32; - EstAlt = BaroHigh / (BARO_TAB_SIZE / 2); + EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2); - temp32 = constrain(AltHold - EstAlt, -1000, 1000); + temp32 = AltHold - EstAlt; if (abs(temp32) < 10 && BaroPID < 10) BaroPID = 0; // remove small D parametr to reduce noise near zoro position // P - BaroPID += P8[PIDALT] * constrain(temp32, (-2) * P8[PIDALT], 2 * P8[PIDALT]) / 100; + BaroPID += cfg.P8[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100; BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150 // I - errorAltitudeI += temp32 * I8[PIDALT] / 50; + errorAltitudeI += temp32 * cfg.I8[PIDALT] / 50; errorAltitudeI = constrain(errorAltitudeI, -30000, 30000); temp32 = errorAltitudeI / 500; // I in range +/-60 BaroPID += temp32; diff --git a/mixer.c b/mixer.c index 35e117c57..b622c6962 100755 --- a/mixer.c +++ b/mixer.c @@ -4,17 +4,17 @@ static uint8_t numberMotor = 4; int16_t motor[8]; int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; -uint8_t mixerConfiguration = MULTITYPE_TRI; -uint16_t wing_left_mid = WING_LEFT_MID; -uint16_t wing_right_mid = WING_RIGHT_MID; -uint16_t tri_yaw_middle = TRI_YAW_MIDDLE; void mixerInit(void) { - if (mixerConfiguration == MULTITYPE_BI || mixerConfiguration == MULTITYPE_TRI || mixerConfiguration == MULTITYPE_GIMBAL || mixerConfiguration == MULTITYPE_FLYING_WING) + // enable servos for mixes that require them. note, this shifts motor counts. + if (cfg.mixerConfiguration == MULTITYPE_BI || cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_GIMBAL || cfg.mixerConfiguration == MULTITYPE_FLYING_WING) + featureSet(FEATURE_SERVO); + // if we want camstab/trig, that also enabled servos. this is kinda lame. maybe rework feature bits later. + if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_CAMTRIG)) featureSet(FEATURE_SERVO); - switch (mixerConfiguration) { + switch (cfg.mixerConfiguration) { case MULTITYPE_GIMBAL: numberMotor = 0; break; @@ -54,10 +54,10 @@ void writeServos(void) if (!feature(FEATURE_SERVO)) return; - if (mixerConfiguration == MULTITYPE_TRI || mixerConfiguration == MULTITYPE_BI) { + if (cfg.mixerConfiguration == MULTITYPE_TRI || cfg.mixerConfiguration == MULTITYPE_BI) { /* One servo on Motor #4 */ pwmWrite(0, servo[4]); - if (mixerConfiguration == MULTITYPE_BI) + if (cfg.mixerConfiguration == MULTITYPE_BI) pwmWrite(1, servo[5]); } else { /* Two servos for camstab or FLYING_WING */ @@ -89,7 +89,7 @@ void writeAllMotors(int16_t mc) writeMotors(); } -#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + YAW_DIRECTION * axisPID[YAW] * Z +#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL] * X + axisPID[PITCH] * Y + cfg.yaw_direction * axisPID[YAW] * Z void mixTable(void) { @@ -104,20 +104,20 @@ void mixTable(void) axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); } - switch (mixerConfiguration) { + switch (cfg.mixerConfiguration) { case MULTITYPE_BI: motor[0] = PIDMIX(+1, 0, 0); //LEFT motor[1] = PIDMIX(-1, 0, 0); //RIGHT - servo[4] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT - servo[5] = constrain(1500 + YAW_DIRECTION * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT + servo[4] = constrain(1500 + cfg.yaw_direction * (axisPID[YAW] + axisPID[PITCH]), 1020, 2000); //LEFT + servo[5] = constrain(1500 + cfg.yaw_direction * (axisPID[YAW] - axisPID[PITCH]), 1020, 2000); //RIGHT break; case MULTITYPE_TRI: motor[0] = PIDMIX(0, +4 / 3, 0); //REAR motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT - servo[4] = constrain(tri_yaw_middle + YAW_DIRECTION * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR + servo[4] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR break; case MULTITYPE_QUADP: @@ -207,77 +207,80 @@ void mixTable(void) motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L break; - + case MULTITYPE_GIMBAL: - servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX); - servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX); + servo[0] = constrain(TILT_PITCH_MIDDLE + cfg.tilt_pitch_prop * angle[PITCH] / 16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX); + servo[1] = constrain(TILT_ROLL_MIDDLE + cfg.tilt_roll_prop * angle[ROLL] / 16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX); break; - + case MULTITYPE_FLYING_WING: motor[0] = rcCommand[THROTTLE]; if (passThruMode) { // do not use sensors for correction, simple 2 channel mixing - servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_L * (rcData[ROLL] - MIDRC); - servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - MIDRC) + ROLL_DIRECTION_R * (rcData[ROLL] - MIDRC); + servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_L * (rcData[ROLL] - cfg.midrc); + servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_R * (rcData[ROLL] - cfg.midrc); } else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL]; servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL]; } - servo[0] = constrain(servo[0] + wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX); - servo[1] = constrain(servo[1] + wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); + servo[0] = constrain(servo[0] + cfg.wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX); + servo[1] = constrain(servo[1] + cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX); break; + } + + // do camstab + if (feature(FEATURE_SERVO_TILT)) { + servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500; + servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500; + + if (rcOptions[BOXCAMSTAB]) { + servo[0] += cfg.tilt_pitch_prop * angle[PITCH] / 16; + servo[1] += cfg.tilt_roll_prop * angle[ROLL] / 16; } -#ifdef SERVO_TILT - servo[0] = TILT_PITCH_MIDDLE + rcData[AUX3] - 1500; - servo[1] = TILT_ROLL_MIDDLE + rcData[AUX4] - 1500; - - if (rcOptions[BOXCAMSTAB]) { - servo[0] += TILT_PITCH_PROP * angle[PITCH] / 16; - servo[1] += TILT_ROLL_PROP * angle[ROLL] / 16; + servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX); + servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX); } - - servo[0] = constrain(servo[0], TILT_PITCH_MIN, TILT_PITCH_MAX); - servo[1] = constrain(servo[1], TILT_ROLL_MIN, TILT_ROLL_MAX); -#endif -#if defined(CAMTRIG) - if (camCycle == 1) { - if (camState == 0) { - servo[2] = CAM_SERVO_HIGH; - camState = 1; - camTime = millis(); - } else if (camState == 1) { - if ((millis() - camTime) > CAM_TIME_HIGH) { - servo[2] = CAM_SERVO_LOW; - camState = 2; + + // do camtrig (this doesn't actually work) + if (feature(FEATURE_CAMTRIG)) { + if (camCycle == 1) { + if (camState == 0) { + servo[2] = CAM_SERVO_HIGH; + camState = 1; camTime = millis(); - } - } else { //camState ==2 - if ((millis() - camTime) > CAM_TIME_LOW) { - camState = 0; - camCycle = 0; + } else if (camState == 1) { + if ((millis() - camTime) > CAM_TIME_HIGH) { + servo[2] = CAM_SERVO_LOW; + camState = 2; + camTime = millis(); + } + } else { //camState ==2 + if ((millis() - camTime) > CAM_TIME_LOW) { + camState = 0; + camCycle = 0; + } } } - } - if (rcOptions[BOXCAMTRIG]) - camCycle = 1; -#endif + if (rcOptions[BOXCAMTRIG]) + camCycle = 1; + } maxMotor = motor[0]; for (i = 1; i < numberMotor; i++) if (motor[i] > maxMotor) maxMotor = motor[i]; for (i = 0; i < numberMotor; i++) { - if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max. - motor[i] -= maxMotor - MAXTHROTTLE; - motor[i] = constrain(motor[i], MINTHROTTLE, MAXTHROTTLE); - if ((rcData[THROTTLE]) < MINCHECK) -#ifndef MOTOR_STOP - motor[i] = MINTHROTTLE; -#else - motor[i] = MINCOMMAND; -#endif + if (maxMotor > cfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. + motor[i] -= maxMotor - cfg.maxthrottle; + motor[i] = constrain(motor[i], cfg.minthrottle, cfg.maxthrottle); + if ((rcData[THROTTLE]) < MINCHECK) { + if (!feature(FEATURE_MOTOR_STOP)) + motor[i] = cfg.minthrottle; + else + motor[i] = cfg.mincommand; + } if (armed == 0) - motor[i] = MINCOMMAND; + motor[i] = cfg.mincommand; } #if (LOG_VALUES == 2) || defined(POWERMETER_SOFT) diff --git a/mw.c b/mw.c index ca4e06739..ca98a690a 100755 --- a/mw.c +++ b/mw.c @@ -1,7 +1,7 @@ #include "board.h" #include "mw.h" -// February 2012 V1.dev +// March 2012 V2.0_pre_version_1 #define CHECKBOXITEMS 11 #define PIDITEMS 8 @@ -24,17 +24,17 @@ volatile int16_t failsafeCnt = 0; int16_t failsafeEvents = 0; int16_t rcData[8]; // interval [1000;2000] int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -uint8_t rcRate8; -uint8_t rcExpo8; +//uint8_t rcRate8; +//uint8_t rcExpo8; int16_t lookupRX[7]; // lookup table for expo & RC rate -uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter +// uint8_t P8[8], I8[8], D8[8]; //8 bits is much faster and the code is much shorter uint8_t dynP8[3], dynI8[3], dynD8[3]; -uint8_t rollPitchRate; -uint8_t yawRate; -uint8_t dynThrPID; -uint8_t activate1[CHECKBOXITEMS]; -uint8_t activate2[CHECKBOXITEMS]; +// uint8_t rollPitchRate; +// uint8_t yawRate; +// uint8_t dynThrPID; +// uint8_t activate1[CHECKBOXITEMS]; +// uint8_t activate2[CHECKBOXITEMS]; uint8_t rcOptions[CHECKBOXITEMS]; uint8_t okToArm = 0; uint8_t accMode = 0; // if level mode is a activated @@ -54,6 +54,8 @@ uint8_t GPS_fix, GPS_fix_home = 0; uint8_t GPS_numSat; uint16_t GPS_distanceToHome; // in meters int16_t GPS_directionToHome = 0; // in degrees +uint16_t GPS_distanceToHome, GPS_distanceToHold; // distance to home or hold point in meters +int16_t GPS_directionToHome, GPS_directionToHold; // direction to home or hol point in degrees 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 @@ -73,7 +75,7 @@ uint16_t AccInflightCalibrationActive = 0; uint32_t pMeter[PMOTOR_SUM + 1]; //we use [0:7] for eight motors,one extra for sum uint8_t pMeterV; // dummy to satisfy the paramStruct logic in ConfigurationLoop() uint32_t pAlarm; // we scale the eeprom value from [0:255] to this value we can directly compare to the sum in pMeter[6] -uint8_t powerTrigger1 = 0; // trigger for alarm based on power consumption +// uint8_t powerTrigger1 = 0; uint16_t powerValue = 0; // last known current uint16_t intPowerMeterSum, intPowerTrigger1; @@ -117,13 +119,13 @@ void annexCode(void) if (rcData[THROTTLE] < 1500) { prop2 = 100; } else if (rcData[THROTTLE] < 2000) { - prop2 = 100 - (uint16_t) dynThrPID *(rcData[THROTTLE] - 1500) / 500; + prop2 = 100 - (uint16_t) cfg.dynThrPID *(rcData[THROTTLE] - 1500) / 500; } else { - prop2 = 100 - dynThrPID; + prop2 = 100 - cfg.dynThrPID; } for (axis = 0; axis < 3; axis++) { - uint16_t tmp = min(abs(rcData[axis] - MIDRC), 500); + uint16_t tmp = min(abs(rcData[axis] - cfg.midrc), 500); #if defined(DEADBAND) if (tmp > DEADBAND) { tmp -= DEADBAND; @@ -134,18 +136,18 @@ void annexCode(void) if (axis != 2) { //ROLL & PITCH uint16_t tmp2 = tmp / 100; rcCommand[axis] = lookupRX[tmp2] + (tmp - tmp2 * 100) * (lookupRX[tmp2 + 1] - lookupRX[tmp2]) / 100; - prop1 = 100 - (uint16_t) rollPitchRate *tmp / 500; + prop1 = 100 - (uint16_t) cfg.rollPitchRate *tmp / 500; prop1 = (uint16_t) prop1 *prop2 / 100; } else { //YAW rcCommand[axis] = tmp; - prop1 = 100 - (uint16_t) yawRate * tmp / 500; + prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500; } - dynP8[axis] = (uint16_t) P8[axis] * prop1 / 100; - dynD8[axis] = (uint16_t) D8[axis] * prop1 / 100; - if (rcData[axis] < MIDRC) + dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100; + dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100; + if (rcData[axis] < cfg.midrc) rcCommand[axis] = -rcCommand[axis]; } - rcCommand[THROTTLE] = MINTHROTTLE + (int32_t) (MAXTHROTTLE - MINTHROTTLE) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK); + rcCommand[THROTTLE] = cfg.minthrottle + (int32_t) (cfg.maxthrottle - cfg.minthrottle) * (rcData[THROTTLE] - MINCHECK) / (2000 - MINCHECK); if (headFreeMode) { float radDiff = (heading - headFreeModeHold) * 0.0174533f; // where PI/180 ~= 0.0174533 @@ -251,7 +253,7 @@ void annexCode(void) #if defined(POWERMETER) intPowerMeterSum = (pMeter[PMOTOR_SUM] / PLEVELDIV); - intPowerTrigger1 = powerTrigger1 * PLEVELSCALE; + intPowerTrigger1 = cfg.powerTrigger1 * PLEVELSCALE; #endif #ifdef LCD_TELEMETRY_AUTO @@ -329,7 +331,7 @@ void loop(void) static int16_t errorAngleI[2] = { 0, 0 }; static uint32_t rcTime = 0; static int16_t initialThrottleHold; - + #if defined(SPEKTRUM) if (rcFrameComplete) computeRC(); @@ -392,7 +394,7 @@ void loop(void) } } #endif - else if ((activate1[BOXARM] > 0) || (activate2[BOXARM] > 0)) { + else if ((cfg.activate1[BOXARM] > 0) || (cfg.activate2[BOXARM] > 0)) { if (rcOptions[BOXARM] && okToArm) { armed = 1; headFreeModeHold = heading; @@ -431,25 +433,25 @@ void loop(void) calibratingM = 1; // MAG calibration request rcDelayCommand++; } else if (rcData[PITCH] > MAXCHECK) { - accTrim[PITCH] += 2; + cfg.accTrim[PITCH] += 2; writeParams(); #if defined(LED_RING) blinkLedRing(); #endif } else if (rcData[PITCH] < MINCHECK) { - accTrim[PITCH] -= 2; + cfg.accTrim[PITCH] -= 2; writeParams(); #if defined(LED_RING) blinkLedRing(); #endif } else if (rcData[ROLL] > MAXCHECK) { - accTrim[ROLL] += 2; + cfg.accTrim[ROLL] += 2; writeParams(); #if defined(LED_RING) blinkLedRing(); #endif } else if (rcData[ROLL] < MINCHECK) { - accTrim[ROLL] -= 2; + cfg.accTrim[ROLL] -= 2; writeParams(); #if defined(LED_RING) blinkLedRing(); @@ -483,7 +485,7 @@ void loop(void) #endif 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) & 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) & 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) & 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]); } //note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false @@ -512,7 +514,6 @@ void loop(void) AltHold = EstAlt; initialThrottleHold = rcCommand[THROTTLE]; errorAltitudeI = 0; - EstVelocity = 0; BaroPID = 0; } } else @@ -539,16 +540,21 @@ void loop(void) } else GPSModeHome = 0; if (rcOptions[BOXGPSHOLD]) { - GPSModeHold = 1; - } else + if (GPSModeHold == 0) { + GPSModeHold = 1; + GPS_latitude_hold = GPS_latitude; + GPS_longitude_hold = GPS_longitude; + } + } else { GPSModeHold = 0; + } #endif if (rcOptions[BOXPASSTHRU]) { passThruMode = 1; } else passThruMode = 0; } else { // not in rc loop - static int8_t taskOrder = 0; //never call all function in the same loop + static int8_t taskOrder = 0; //never call all function in the same loop, to avoid high delay spikes switch (taskOrder) { case 0: taskOrder++; @@ -559,10 +565,19 @@ void loop(void) taskOrder++; if (sensors(SENSOR_BARO)) Baro_update(); + break; case 2: taskOrder++; if (sensors(SENSOR_BARO)) getEstimatedAltitude(); + break; + case 3: + taskOrder++; +#if GPS + GPS_NewData(); +#endif + break; + default: taskOrder = 0; break; @@ -584,7 +599,7 @@ void loop(void) if (dif >= +180) dif -= 360; if (smallAngle25) - rcCommand[YAW] -= dif * P8[PIDMAG] / 30; //18 deg + rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; //18 deg } else magHold = heading; } @@ -592,23 +607,30 @@ void loop(void) if (sensors(SENSOR_BARO)) { if (baroMode) { if (abs(rcCommand[THROTTLE] - initialThrottleHold) > 20) { - AltHold = EstAlt; - initialThrottleHold = rcCommand[THROTTLE]; - errorAltitudeI = 0; - EstVelocity = 0; - BaroPID = 0; + baroMode = 0; // so that a new althold reference is defined } rcCommand[THROTTLE] = initialThrottleHold + BaroPID; } } #if GPS - if ((GPSModeHome == 1)) { - float radDiff = (GPS_directionToHome - heading) * 0.0174533f; - GPS_angle[ROLL] = constrain(P8[PIDGPS] * sinf(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // with P=5, 1 meter = 0.5deg inclination - GPS_angle[PITCH] = constrain(P8[PIDGPS] * cosf(radDiff) * GPS_distanceToHome / 10, -D8[PIDGPS] * 10, +D8[PIDGPS] * 10); // max inclination = D deg - } else { - GPS_angle[ROLL] = 0; + uint16_t GPS_dist; + int16_t GPS_dir; + + if ((GPSModeHome == 0 && GPSModeHold == 0) || (GPS_fix_home == 0)) { + GPS_angle[ROLL] = 0; GPS_angle[PITCH] = 0; + } else { + if (GPSModeHome == 1) { + GPS_dist = GPS_distanceToHome; + GPS_dir = GPS_directionToHome; + } + if (GPSModeHold == 1) { + GPS_dist = GPS_distanceToHold; + GPS_dir = GPS_directionToHold; + } + float radDiff = (GPS_dir - heading) * 0.0174533f; + GPS_angle[ROLL] = constrain(cfg.P8[PIDGPS] * sin(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // with P=5.0, a distance of 1 meter = 0.5deg inclination + GPS_angle[PITCH] = constrain(cfg.P8[PIDGPS] * cos(radDiff) * GPS_dist / 10, -cfg.D8[PIDGPS] * 10, +cfg.D8[PIDGPS] * 10); // max inclination = D deg } #endif @@ -616,18 +638,18 @@ void loop(void) for (axis = 0; axis < 3; axis++) { if (accMode == 1 && axis < 2) { //LEVEL MODE // 50 degrees max inclination - errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + accTrim[axis]; //16 bits is ok here + errorAngle = constrain(2 * rcCommand[axis] - GPS_angle[axis], -500, +500) - angle[axis] + cfg.accTrim[axis]; //16 bits is ok here #ifdef LEVEL_PDF - PTerm = -(int32_t) angle[axis] * P8[PIDLEVEL] / 100; + PTerm = -(int32_t) angle[axis] * cfg.P8[PIDLEVEL] / 100; #else - PTerm = (int32_t) errorAngle * P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result + PTerm = (int32_t) errorAngle * cfg.P8[PIDLEVEL] / 100; //32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result #endif - PTerm = constrain(PTerm, -D8[PIDLEVEL] * 5, +D8[PIDLEVEL] * 5); + PTerm = constrain(PTerm, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5); errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); //WindUp //16 bits is ok here - ITerm = ((int32_t) errorAngleI[axis] * I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result + ITerm = ((int32_t) errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; //32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result } else { //ACRO MODE or YAW axis - error = (int32_t) rcCommand[axis] * 10 * 8 / P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2) + error = (int32_t) rcCommand[axis] * 10 * 8 / cfg.P8[axis]; //32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2) error -= gyroData[axis]; PTerm = rcCommand[axis]; @@ -635,7 +657,7 @@ void loop(void) errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); //WindUp //16 bits is ok here if (abs(gyroData[axis]) > 640) errorGyroI[axis] = 0; - ITerm = (errorGyroI[axis] / 125 * I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 + ITerm = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 } PTerm -= (int32_t) gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation @@ -653,23 +675,4 @@ void loop(void) mixTable(); writeServos(); writeMotors(); - -#if GPS - while (SerialAvailable(GPS_SERIAL)) { - if (GPS_newFrame(SerialRead(GPS_SERIAL))) { - if (GPS_update == 1) - GPS_update = 0; - else - GPS_update = 1; - if (GPS_fix == 1 && GPS_numSat == 4) { - if (GPS_fix_home == 0) { - GPS_fix_home = 1; - GPS_latitude_home = GPS_latitude; - GPS_longitude_home = GPS_longitude; - } - GPS_distance(GPS_latitude_home, GPS_longitude_home, GPS_latitude, GPS_longitude, &GPS_distanceToHome, &GPS_directionToHome); - } - } - } -#endif } diff --git a/mw.h b/mw.h index 4815eac06..9bc58eb66 100755 --- a/mw.h +++ b/mw.h @@ -3,23 +3,9 @@ #define MINCHECK 1100 #define MAXCHECK 1900 -#define YAW_DIRECTION 1 // if you want to reverse the yaw correction direction -//#define YAW_DIRECTION -1 - /* this is the value for the ESCs when they are not armed in some cases, this value must be lowered down to 900 for some specific ESCs */ -#define MINCOMMAND 1000 - -/* Set the minimum throttle command sent to the ESC (Electronic Speed Controller) - This is the minimum value that allow motors to run at a idle speed */ -//#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A -//#define MINTHROTTLE 1120 // for Super Simple ESCs 10A -//#define MINTHROTTLE 1220 -#define MINTHROTTLE 1150 -/* this is the maximum value for the ESCs at full power this value can be increased up to 2000 */ -#define MAXTHROTTLE 1850 -/* some radios have not a neutral point centered on 1500. can be changed here */ -#define MIDRC 1500 +// #define MINCOMMAND 1000 /* This option should be uncommented if ACC Z is accurate enough when motors are running*/ /* should now be ok with BMA020 and BMA180 ACC */ @@ -41,7 +27,6 @@ /* you can change the tricopter servo travel here */ #define TRI_YAW_CONSTRAINT_MIN 1020 #define TRI_YAW_CONSTRAINT_MAX 2000 -#define TRI_YAW_MIDDLE 1500 // tail servo center pos. - use this for initial trim; later trim midpoint via LCD /* Flying Wing: you can change change servo orientation and servo min/max values here */ /* valid for all flight modes, even passThrough mode */ @@ -50,8 +35,6 @@ #define PITCH_DIRECTION_R -1 // right servo - pitch orientation (opposite sign to PITCH_DIRECTION_L, if servos are mounted in mirrored orientation) #define ROLL_DIRECTION_L 1 // left servo - roll orientation #define ROLL_DIRECTION_R 1 // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation) -#define WING_LEFT_MID 1500 // left servo center pos. - use this for initial trim; later trim midpoint via LCD -#define WING_RIGHT_MID 1500 // right servo center pos. - use this for initial trim; later trim midpoint via LCD #define WING_LEFT_MIN 1020 // limit servo travel range must be inside [1020;2000] #define WING_LEFT_MAX 2000 // limit servo travel range must be inside [1020;2000] #define WING_RIGHT_MIN 1020 // limit servo travel range must be inside [1020;2000] @@ -60,28 +43,39 @@ /* The following lines apply only for a pitch/roll tilt stabilization system On promini board, it is not compatible with config with 6 motors or more Uncomment the first line to activate it */ -//#define SERVO_TILT #define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020 #define TILT_PITCH_MAX 2000 //servo travel max, max value=2000 #define TILT_PITCH_MIDDLE 1500 //servo neutral value -#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement #define TILT_ROLL_MIN 1020 #define TILT_ROLL_MAX 2000 #define TILT_ROLL_MIDDLE 1500 -#define TILT_ROLL_PROP 10 + +/* experimental + camera trigger function : activated via Rc Options in the GUI, servo output=A2 on promini */ +#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo +#define CAM_SERVO_LOW 1020 // the position of LOW state servo +#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms +#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms /* for V BAT monitoring after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN with R1=33k and R2=51k vbat = [0;1023]*16/VBATSCALE */ #define VBATFREQ 6 // to read battery voltage - keep equal to PSENSORFREQ (6) unless you know what you are doing -#define VBATSCALE 131 // change this value if readed Battery voltage is different than real voltage +#define VBATSCALE 151 // change this value if readed Battery voltage is different than real voltage #define VBATLEVEL1_3S 107 // 10,7V #define VBATLEVEL2_3S 103 // 10,3V #define VBATLEVEL3_3S 99 // 9.9V #define NO_VBAT 16 // Avoid beeping without any battery -#define VERSION 19 +// 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 201 // Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization. typedef enum MultiType @@ -99,9 +93,11 @@ typedef enum MultiType MULTITYPE_OCTOX8 = 11, // XK MULTITYPE_OCTOFLATP = 12, // XL the GUI is the same for all 8 motor configs MULTITYPE_OCTOFLATX = 13, // XM the GUI is the same for all 8 motor configs - // XN missing for some reason?? - MULTITYPE_VTAIL4 = 15, // XO - MULTITYPE_LAST = 16 + MULTITYPE_AIRPLANE = 14, // XN + MULTITYPE_HELI_120_CCPM = 15, // XO simple model + MULTITYPE_HELI_90_DEG = 16, // XP simple model + MULTITYPE_VTAIL4 = 17, // XO + MULTITYPE_LAST = 18 } MultiType; /*********** RC alias *****************/ @@ -120,16 +116,16 @@ typedef enum MultiType #define PIDLEVEL 6 #define PIDMAG 7 -#define BOXACC 0 -#define BOXBARO 1 -#define BOXMAG 2 -#define BOXCAMSTAB 3 -#define BOXCAMTRIG 4 -#define BOXARM 5 -#define BOXGPSHOME 6 -#define BOXGPSHOLD 7 -#define BOXPASSTHRU 8 -#define BOXHEADFREE 9 +#define BOXACC 0 +#define BOXBARO 1 +#define BOXMAG 2 +#define BOXCAMSTAB 3 +#define BOXCAMTRIG 4 +#define BOXARM 5 +#define BOXGPSHOME 6 +#define BOXGPSHOLD 7 +#define BOXPASSTHRU 8 +#define BOXHEADFREE 9 #define BOXBEEPERON 10 #define CHECKBOXITEMS 11 @@ -137,21 +133,60 @@ typedef enum MultiType #define ACC_ORIENTATION(X, Y, Z) { accADC[ROLL] = Y; accADC[PITCH] = -X; accADC[YAW] = Z; } #define GYRO_ORIENTATION(X, Y, Z) { gyroADC[ROLL] = -Y; gyroADC[PITCH] = X; gyroADC[YAW] = Z; } -#define MAG_ORIENTATION(X, Y, Z) { magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = Z; } #define min(a,b) ((a)<(b)?(a):(b)) #define max(a,b) ((a)>(b)?(a):(b)) #define abs(x) ((x)>0?(x):-(x)) #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) -extern int16_t accZero[3]; -extern int16_t accTrim[2]; +typedef struct config_t { + uint8_t version; + uint8_t mixerConfiguration; + uint32_t enabledFeatures; + + uint8_t P8[8]; + uint8_t I8[8]; + uint8_t D8[8]; + + uint8_t rcRate8; + uint8_t rcExpo8; + uint8_t rollPitchRate; + uint8_t yawRate; + + uint8_t dynThrPID; + int16_t accZero[3]; + int16_t magZero[3]; + int16_t accTrim[2]; + 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]; + uint8_t powerTrigger1; // trigger for alarm based on power consumption + + // Radio/ESC-related configuration + uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here + uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. + uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 + uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs + + // mixer-related configuration + int8_t yaw_direction; + uint16_t wing_left_mid; // left servo center pos. - use this for initial trim + uint16_t wing_right_mid; // right servo center pos. - use this for initial trim + uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim + + // gimbal-related configuration + int8_t tilt_pitch_prop; // servo proportional (tied to angle) ; can be negative to invert movement + int8_t tilt_roll_prop; // servo proportional (tied to angle) ; can be negative to invert movement + +} config_t; + extern int16_t gyroZero[3]; -extern int16_t magZero[3]; extern int16_t gyroData[3]; extern int16_t angle[2]; extern int16_t axisPID[3]; extern int16_t rcCommand[4]; +extern uint8_t rcOptions[CHECKBOXITEMS]; extern int16_t debug1, debug2, debug3, debug4; extern uint8_t armed; @@ -171,7 +206,6 @@ extern int32_t BaroAlt; extern int32_t EstAlt; extern int32_t AltHold; extern int16_t errorAltitudeI; -extern int32_t EstVelocity; extern int16_t BaroPID; extern uint8_t headFreeMode; extern int16_t headFreeModeHold; @@ -185,16 +219,7 @@ extern int16_t rcData[8]; extern uint8_t accMode; extern uint8_t magMode; extern uint8_t baroMode; -extern uint8_t P8[8], I8[8], D8[8]; -extern uint8_t dynThrPID; -extern uint8_t activate1[CHECKBOXITEMS]; -extern uint8_t activate2[CHECKBOXITEMS]; extern uint16_t intPowerMeterSum, intPowerTrigger1; -extern uint8_t rollPitchRate; -extern uint8_t yawRate; -extern uint8_t dynThrPID; -extern uint8_t rcRate8; -extern uint8_t rcExpo8; extern int32_t GPS_latitude, GPS_longitude; extern int32_t GPS_latitude_home, GPS_longitude_home; extern uint8_t GPS_fix, GPS_fix_home; @@ -205,12 +230,9 @@ extern uint8_t GPS_update; extern uint8_t GPSModeHome; extern uint8_t GPSModeHold; extern uint8_t vbat; -extern uint8_t powerTrigger1; extern int16_t lookupRX[7]; // lookup table for expo & RC rate -extern uint8_t mixerConfiguration; -extern uint16_t wing_left_mid; -extern uint16_t wing_right_mid; -extern uint16_t tri_yaw_middle; + +extern config_t cfg; // main void loop(void); @@ -250,3 +272,6 @@ bool feature(uint32_t mask); void featureSet(uint32_t mask); void featureClear(uint32_t mask); void featureClearAll(void); + +// cli +void cliProcess(void); diff --git a/sensors.c b/sensors.c index 307e13de0..37f7941fa 100755 --- a/sensors.c +++ b/sensors.c @@ -6,7 +6,6 @@ uint16_t calibratingA = 0; // the calibration is done is the main loop. Ca uint16_t calibratingG = 0; uint8_t calibratingM = 0; uint16_t acc_1G = 256; // this is the 1G measured acceleration -int16_t accTrim[2] = { 0, 0 }; int16_t heading, magHold; void sensorsAutodetect(void) @@ -33,15 +32,15 @@ static void ACC_Common(void) a[axis] += accADC[axis]; // Clear global variables for next reading accADC[axis] = 0; - accZero[axis] = 0; + cfg.accZero[axis] = 0; } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (calibratingA == 1) { - accZero[ROLL] = a[ROLL] / 400; - accZero[PITCH] = a[PITCH] / 400; - accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G - accTrim[ROLL] = 0; - accTrim[PITCH] = 0; + cfg.accZero[ROLL] = a[ROLL] / 400; + cfg.accZero[PITCH] = a[PITCH] / 400; + 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 } calibratingA--; @@ -94,9 +93,9 @@ static void ACC_Common(void) writeParams(); // write accZero in EEPROM } #endif - accADC[ROLL] -= accZero[ROLL]; - accADC[PITCH] -= accZero[PITCH]; - accADC[YAW] -= accZero[YAW]; + accADC[ROLL] -= cfg.accZero[ROLL]; + accADC[PITCH] -= cfg.accZero[PITCH]; + accADC[YAW] -= cfg.accZero[YAW]; } @@ -118,6 +117,8 @@ static int16_t baroTemp = 0; void Baro_update(void) { + int32_t pressure; + if (currentTime < baroDeadline) return; @@ -155,6 +156,15 @@ static void GYRO_Common(void) static int16_t previousGyroADC[3] = { 0, 0, 0 }; static int32_t g[3]; uint8_t axis; + +#if defined MMGYRO + // Moving Average Gyros by Magnetron1 + //--------------------------------------------------- + static int16_t mediaMobileGyroADC[3][MMGYROVECTORLENGTH]; + static int32_t mediaMobileGyroADCSum[3]; + static uint8_t mediaMobileGyroIDX; + //--------------------------------------------------- +#endif if (calibratingG > 0) { for (axis = 0; axis < 3; axis++) { @@ -174,12 +184,25 @@ static void GYRO_Common(void) calibratingG--; } +#ifdef MMGYRO + mediaMobileGyroIDX = ++mediaMobileGyroIDX % MMGYROVECTORLENGTH; + for (axis = 0; axis < 3; axis++) { + gyroADC[axis] -= gyroZero[axis]; + mediaMobileGyroADCSum[axis] -= mediaMobileGyroADC[axis][mediaMobileGyroIDX]; + //anti gyro glitch, limit the variation between two consecutive readings + mediaMobileGyroADC[axis][mediaMobileGyroIDX] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800); + mediaMobileGyroADCSum[axis] += mediaMobileGyroADC[axis][mediaMobileGyroIDX]; + gyroADC[axis] = mediaMobileGyroADCSum[axis] / MMGYROVECTORLENGTH; + previousGyroADC[axis] = gyroADC[axis]; + } +#else for (axis = 0; axis < 3; axis++) { gyroADC[axis] -= gyroZero[axis]; //anti gyro glitch, limit the variation between two consecutive readings gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 800, previousGyroADC[axis] + 800); previousGyroADC[axis] = gyroADC[axis]; } +#endif } void Gyro_getADC(void) @@ -204,14 +227,9 @@ static void Mag_getRawADC(void) { static int16_t rawADC[3]; hmc5883lRead(rawADC); - - // Hearty FUCK-YOU goes to all teh breakout sensor faggots who make a new orientation for each shitty board they make - // sensor order: X Z Y -// magADC[ROLL] = rawADC[0]; // X or negative? who knows mag stuff in multiwii is broken hardcore -// magADC[PITCH] = rawADC[2]; // Y // no way? is this finally the proper orientation?? - magADC[ROLL] = -rawADC[2]; // X or negative? who knows mag stuff in multiwii is broken hardcore + magADC[ROLL] = -rawADC[2]; // X magADC[PITCH] = rawADC[0]; // Y magADC[YAW] = rawADC[1]; // Z } @@ -227,7 +245,7 @@ void Mag_init(void) magCal[ROLL] = 1000.0 / abs(magADC[ROLL]); magCal[PITCH] = 1000.0 / abs(magADC[PITCH]); magCal[YAW] = 1000.0 / abs(magADC[YAW]); - + hmc5883lFinishCal(); magInit = 1; } @@ -249,7 +267,7 @@ void Mag_getADC(void) if (calibratingM == 1) { tCal = t; for (axis = 0; axis < 3; axis++) { - magZero[axis] = 0; + cfg.magZero[axis] = 0; magZeroTempMin[axis] = 0; magZeroTempMax[axis] = 0; } @@ -259,9 +277,9 @@ void Mag_getADC(void) magADC[PITCH] = magADC[PITCH] * magCal[PITCH]; magADC[YAW] = magADC[YAW] * magCal[YAW]; if (magInit) { // we apply offset only once mag calibration is done - magADC[ROLL] -= magZero[ROLL]; - magADC[PITCH] -= magZero[PITCH]; - magADC[YAW] -= magZero[YAW]; + magADC[ROLL] -= cfg.magZero[ROLL]; + magADC[PITCH] -= cfg.magZero[PITCH]; + magADC[YAW] -= cfg.magZero[YAW]; } if (tCal != 0) { @@ -276,7 +294,7 @@ void Mag_getADC(void) } else { tCal = 0; for (axis = 0; axis < 3; axis++) - magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; + cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; writeParams(); } } diff --git a/serial.c b/serial.c index 46a9d5d5c..088f42be5 100755 --- a/serial.c +++ b/serial.c @@ -1,6 +1,9 @@ #include "board.h" #include "mw.h" +// signal that we're in cli mode +uint8_t cliMode = 0; + void serialize16(int16_t a) { uartWrite(a); @@ -19,11 +22,21 @@ void UartSendData() void serialCom(void) { - int16_t a; uint8_t i; + + // in cli mode, all uart stuff goes to here. enter cli mode by sending # + if (cliMode) { + cliProcess(); + return; + } if (uartAvailable()) { switch (uartRead()) { + case '#': + uartPrint("\r\nEntering CLI Mode, type 'exit' to return\r\n"); + cliMode = 1; + break; + #ifdef BTSERIAL case 'K': //receive RC data from Bluetooth Serial adapter as a remote rcData[THROTTLE] = (SerialRead(0) * 4) + 1000; @@ -155,20 +168,21 @@ void serialCom(void) serialize16(i2cGetErrorCounter()); for (i = 0; i < 2; i++) serialize16(angle[i]); - serialize8(mixerConfiguration); + serialize8(cfg.mixerConfiguration); for (i = 0; i < PIDITEMS; i++) { - serialize8(P8[i]); - serialize8(I8[i]); - serialize8(D8[i]); + serialize8(cfg.P8[i]); + serialize8(cfg.I8[i]); + serialize8(cfg.D8[i]); } - serialize8(rcRate8); - serialize8(rcExpo8); - serialize8(rollPitchRate); - serialize8(yawRate); - serialize8(dynThrPID); + serialize8(cfg.rcRate8); + serialize8(cfg.rcExpo8); + serialize8(cfg.rollPitchRate); + serialize8(cfg.yawRate); + serialize8(cfg.dynThrPID); for (i = 0; i < CHECKBOXITEMS; i++) { - serialize8(activate1[i]); - serialize8(activate2[i]); + serialize8(cfg.activate1[i]); + serialize8(cfg.activate2[i] | (rcOptions[i] << 7)); // use highest bit to transport state in mwc + } serialize16(GPS_distanceToHome); serialize16(GPS_directionToHome); @@ -179,7 +193,7 @@ void serialCom(void) serialize16(intPowerTrigger1); serialize8(vbat); serialize16(BaroAlt / 10); // 4 variables are here for general monitoring purpose - serialize16(0); // debug2 + serialize16(debug2); // debug2 serialize16(debug3); // debug3 serialize16(debug4); // debug4 serialize8('M'); @@ -216,7 +230,7 @@ void serialCom(void) if (i > 64 && i < 64 + MULTITYPE_LAST) { serialize8('O'); serialize8('K'); - mixerConfiguration = i - '@'; // A..B..C.. index + cfg.mixerConfiguration = i - '@'; // A..B..C.. index writeParams(); systemReset(false); break; @@ -228,21 +242,21 @@ void serialCom(void) case 'W': //GUI write params to eeprom @ arduino // while (uartAvailable() < (7 + 3 * PIDITEMS + 2 * CHECKBOXITEMS)) { } for (i = 0; i < PIDITEMS; i++) { - P8[i] = uartReadPoll(); - I8[i] = uartReadPoll(); - D8[i] = uartReadPoll(); + cfg.P8[i] = uartReadPoll(); + cfg.I8[i] = uartReadPoll(); + cfg.D8[i] = uartReadPoll(); } - rcRate8 = uartReadPoll(); - rcExpo8 = uartReadPoll(); //2 - rollPitchRate = uartReadPoll(); - yawRate = uartReadPoll(); //4 - dynThrPID = uartReadPoll(); //5 + cfg.rcRate8 = uartReadPoll(); + cfg.rcExpo8 = uartReadPoll(); //2 + cfg.rollPitchRate = uartReadPoll(); + cfg.yawRate = uartReadPoll(); //4 + cfg.dynThrPID = uartReadPoll(); //5 for (i = 0; i < CHECKBOXITEMS; i++) { - activate1[i] = uartReadPoll(); - activate2[i] = uartReadPoll(); + cfg.activate1[i] = uartReadPoll(); + cfg.activate2[i] = uartReadPoll(); } #if defined(POWERMETER) - powerTrigger1 = (uartReadPoll() + 256 * uartReadPoll()) / PLEVELSCALE; // we rely on writeParams() to compute corresponding pAlarm value + cfg.powerTrigger1 = (uartReadPoll() + 256 * uartReadPoll()) / PLEVELSCALE; // we rely on writeParams() to compute corresponding pAlarm value #else uartReadPoll(); uartReadPoll(); //7 so we unload the two bytes