diff --git a/src/board.h b/src/board.h index 22e5f9313..30bd3a7f3 100755 --- a/src/board.h +++ b/src/board.h @@ -56,6 +56,7 @@ typedef enum { FEATURE_SONAR = 1 << 10, FEATURE_TELEMETRY = 1 << 11, FEATURE_POWERMETER = 1 << 12, + FEATURE_VARIO = 1 << 13, } AvailableFeatures; typedef enum { @@ -72,10 +73,11 @@ typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver typedef struct sensor_t { - sensorInitFuncPtr init; - sensorReadFuncPtr read; - sensorReadFuncPtr align; - sensorReadFuncPtr temperature; + sensorInitFuncPtr init; // initialize function + sensorReadFuncPtr read; // read 3 axis data function + sensorReadFuncPtr align; // sensor align + sensorReadFuncPtr temperature; // read temperature if available + float scale; // scalefactor (currently used for gyro only, todo for accel) } sensor_t; typedef struct baro_t diff --git a/src/cli.c b/src/cli.c index 8ebb92ec2..d8aa3f0b5 100644 --- a/src/cli.c +++ b/src/cli.c @@ -12,6 +12,7 @@ static void cliFeature(char *cmdline); static void cliHelp(char *cmdline); static void cliMap(char *cmdline); static void cliMixer(char *cmdline); +static void cliProfile(char *cmdline); static void cliSave(char *cmdline); static void cliSet(char *cmdline); static void cliStatus(char *cmdline); @@ -43,7 +44,7 @@ const char * const mixerNames[] = { const char * const featureNames[] = { "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SPEKTRUM", "MOTOR_STOP", "SERVO_TILT", "GYRO_SMOOTHING", "LED_RING", "GPS", - "FAILSAFE", "SONAR", "TELEMETRY", + "FAILSAFE", "SONAR", "TELEMETRY", "VARIO", NULL }; @@ -74,6 +75,7 @@ const clicmd_t cmdTable[] = { { "help", "", cliHelp }, { "map", "mapping of rc channel order", cliMap }, { "mixer", "mixer name or list", cliMixer }, + { "profile", "index (0 to 2)", cliProfile }, { "save", "save and reboot", cliSave }, { "set", "name=value or blank or * for list", cliSet }, { "status", "show system status", cliStatus }, @@ -99,34 +101,50 @@ typedef struct { } clivalue_t; const clivalue_t valueTable[] = { + { "looptime", VAR_UINT16, &mcfg.looptime, 0, 9000 }, + { "midrc", VAR_UINT16, &mcfg.midrc, 1200, 1700 }, + { "minthrottle", VAR_UINT16, &mcfg.minthrottle, 0, 2000 }, + { "maxthrottle", VAR_UINT16, &mcfg.maxthrottle, 0, 2000 }, + { "mincommand", VAR_UINT16, &mcfg.mincommand, 0, 2000 }, + { "mincheck", VAR_UINT16, &mcfg.mincheck, 0, 2000 }, + { "maxcheck", VAR_UINT16, &mcfg.maxcheck, 0, 2000 }, + { "retarded_arm", VAR_UINT8, &mcfg.retarded_arm, 0, 1 }, + { "motor_pwm_rate", VAR_UINT16, &mcfg.motor_pwm_rate, 50, 498 }, + { "servo_pwm_rate", VAR_UINT16, &mcfg.servo_pwm_rate, 50, 498 }, + { "serial_baudrate", VAR_UINT32, &mcfg.serial_baudrate, 1200, 115200 }, + { "gps_baudrate", VAR_UINT32, &mcfg.gps_baudrate, 1200, 115200 }, + { "spektrum_hires", VAR_UINT8, &mcfg.spektrum_hires, 0, 1 }, + { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 }, + { "vbatmaxcellvoltage", VAR_UINT8, &mcfg.vbatmaxcellvoltage, 10, 50 }, + { "vbatmincellvoltage", VAR_UINT8, &mcfg.vbatmincellvoltage, 10, 50 }, + { "power_adc_channel", VAR_UINT8, &mcfg.power_adc_channel, 0, 9 }, + { "align_gyro_x", VAR_INT8, &mcfg.align[ALIGN_GYRO][0], -3, 3 }, + { "align_gyro_y", VAR_INT8, &mcfg.align[ALIGN_GYRO][1], -3, 3 }, + { "align_gyro_z", VAR_INT8, &mcfg.align[ALIGN_GYRO][2], -3, 3 }, + { "align_acc_x", VAR_INT8, &mcfg.align[ALIGN_ACCEL][0], -3, 3 }, + { "align_acc_y", VAR_INT8, &mcfg.align[ALIGN_ACCEL][1], -3, 3 }, + { "align_acc_z", VAR_INT8, &mcfg.align[ALIGN_ACCEL][2], -3, 3 }, + { "align_mag_x", VAR_INT8, &mcfg.align[ALIGN_MAG][0], -3, 3 }, + { "align_mag_y", VAR_INT8, &mcfg.align[ALIGN_MAG][1], -3, 3 }, + { "align_mag_z", VAR_INT8, &mcfg.align[ALIGN_MAG][2], -3, 3 }, + { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 3 }, + { "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 }, + { "gyro_lpf", VAR_UINT16, &mcfg.gyro_lpf, 0, 256 }, + { "gyro_cmpf_factor", VAR_UINT16, &mcfg.gyro_cmpf_factor, 100, 1000 }, + { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 }, + { "deadband", VAR_UINT8, &cfg.deadband, 0, 32 }, { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 }, - { "midrc", VAR_UINT16, &cfg.midrc, 1200, 1700 }, { "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 }, { "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 }, { "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 }, { "thr_expo", VAR_UINT8, &cfg.thrExpo8, 0, 250 }, { "roll_pitch_rate", VAR_UINT8, &cfg.rollPitchRate, 0, 100 }, { "yawrate", VAR_UINT8, &cfg.yawRate, 0, 100 }, - { "minthrottle", VAR_UINT16, &cfg.minthrottle, 0, 2000 }, - { "maxthrottle", VAR_UINT16, &cfg.maxthrottle, 0, 2000 }, - { "mincommand", VAR_UINT16, &cfg.mincommand, 0, 2000 }, - { "mincheck", VAR_UINT16, &cfg.mincheck, 0, 2000 }, - { "maxcheck", VAR_UINT16, &cfg.maxcheck, 0, 2000 }, - { "retarded_arm", VAR_UINT8, &cfg.retarded_arm, 0, 1 }, { "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 }, { "failsafe_off_delay", VAR_UINT8, &cfg.failsafe_off_delay, 0, 200 }, { "failsafe_throttle", VAR_UINT16, &cfg.failsafe_throttle, 1000, 2000 }, - { "motor_pwm_rate", VAR_UINT16, &cfg.motor_pwm_rate, 50, 498 }, - { "servo_pwm_rate", VAR_UINT16, &cfg.servo_pwm_rate, 50, 498 }, - { "serial_baudrate", VAR_UINT32, &cfg.serial_baudrate, 1200, 115200 }, - { "gps_baudrate", VAR_UINT32, &cfg.gps_baudrate, 1200, 115200 }, - { "spektrum_hires", VAR_UINT8, &cfg.spektrum_hires, 0, 1 }, - { "vbatscale", VAR_UINT8, &cfg.vbatscale, 10, 200 }, - { "vbatmaxcellvoltage", VAR_UINT8, &cfg.vbatmaxcellvoltage, 10, 50 }, - { "vbatmincellvoltage", VAR_UINT8, &cfg.vbatmincellvoltage, 10, 50 }, - { "power_adc_channel", VAR_UINT8, &cfg.power_adc_channel, 0, 9 }, { "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 }, { "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 }, { "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 }, @@ -150,28 +168,14 @@ const clivalue_t valueTable[] = { { "gimbal_roll_min", VAR_UINT16, &cfg.gimbal_roll_min, 100, 3000 }, { "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 }, { "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 }, - { "align_gyro_x", VAR_INT8, &cfg.align[ALIGN_GYRO][0], -3, 3 }, - { "align_gyro_y", VAR_INT8, &cfg.align[ALIGN_GYRO][1], -3, 3 }, - { "align_gyro_z", VAR_INT8, &cfg.align[ALIGN_GYRO][2], -3, 3 }, - { "align_acc_x", VAR_INT8, &cfg.align[ALIGN_ACCEL][0], -3, 3 }, - { "align_acc_y", VAR_INT8, &cfg.align[ALIGN_ACCEL][1], -3, 3 }, - { "align_acc_z", VAR_INT8, &cfg.align[ALIGN_ACCEL][2], -3, 3 }, - { "align_mag_x", VAR_INT8, &cfg.align[ALIGN_MAG][0], -3, 3 }, - { "align_mag_y", VAR_INT8, &cfg.align[ALIGN_MAG][1], -3, 3 }, - { "align_mag_z", VAR_INT8, &cfg.align[ALIGN_MAG][2], -3, 3 }, - { "acc_hardware", VAR_UINT8, &cfg.acc_hardware, 0, 3 }, { "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 }, { "acc_lpf_for_velocity", VAR_UINT8, &cfg.acc_lpf_for_velocity, 1, 250 }, { "acc_trim_pitch", VAR_INT16, &cfg.angleTrim[PITCH], -300, 300 }, { "acc_trim_roll", VAR_INT16, &cfg.angleTrim[ROLL], -300, 300 }, - { "gyro_lpf", VAR_UINT16, &cfg.gyro_lpf, 0, 256 }, - { "gyro_cmpf_factor", VAR_UINT16, &cfg.gyro_cmpf_factor, 100, 1000 }, { "baro_tab_size", VAR_UINT8, &cfg.baro_tab_size, 0, BARO_TAB_SIZE_MAX }, { "baro_noise_lpf", VAR_FLOAT, &cfg.baro_noise_lpf, 0, 1 }, { "baro_cf", VAR_FLOAT, &cfg.baro_cf, 0, 1 }, - { "moron_threshold", VAR_UINT8, &cfg.moron_threshold, 0, 128 }, { "mag_declination", VAR_INT16, &cfg.mag_declination, -18000, 18000 }, - { "gps_type", VAR_UINT8, &cfg.gps_type, 0, 3 }, { "gps_pos_p", VAR_UINT8, &cfg.P8[PIDPOS], 0, 200 }, { "gps_pos_i", VAR_UINT8, &cfg.I8[PIDPOS], 0, 200 }, { "gps_pos_d", VAR_UINT8, &cfg.D8[PIDPOS], 0, 200 }, @@ -186,7 +190,6 @@ const clivalue_t valueTable[] = { { "nav_speed_min", VAR_UINT16, &cfg.nav_speed_min, 10, 2000 }, { "nav_speed_max", VAR_UINT16, &cfg.nav_speed_max, 10, 2000 }, { "nav_slew_rate", VAR_UINT8, &cfg.nav_slew_rate, 0, 100 }, - { "looptime", VAR_UINT16, &cfg.looptime, 0, 9000 }, { "p_pitch", VAR_UINT8, &cfg.P8[PITCH], 0, 200 }, { "i_pitch", VAR_UINT8, &cfg.I8[PITCH], 0, 200 }, { "d_pitch", VAR_UINT8, &cfg.D8[PITCH], 0, 200 }, @@ -435,26 +438,30 @@ static void cliCMix(char *cmdline) if (len == 0) { uartPrint("Custom mixer: \r\nMotor\tThr\tRoll\tPitch\tYaw\r\n"); for (i = 0; i < MAX_MOTORS; i++) { - if (cfg.customMixer[i].throttle == 0.0f) + if (mcfg.customMixer[i].throttle == 0.0f) break; - mixsum[i] = 0.0f; num_motors++; printf("#%d:\t", i + 1); - printf("%s\t", ftoa(cfg.customMixer[i].throttle, buf)); - printf("%s\t", ftoa(cfg.customMixer[i].roll, buf)); - printf("%s\t", ftoa(cfg.customMixer[i].pitch, buf)); - printf("%s\r\n", ftoa(cfg.customMixer[i].yaw, buf)); + printf("%s\t", ftoa(mcfg.customMixer[i].throttle, buf)); + printf("%s\t", ftoa(mcfg.customMixer[i].roll, buf)); + printf("%s\t", ftoa(mcfg.customMixer[i].pitch, buf)); + printf("%s\r\n", ftoa(mcfg.customMixer[i].yaw, buf)); } + mixsum[0] = mixsum[1] = mixsum[2] = 0.0f; for (i = 0; i < num_motors; i++) { - mixsum[0] += cfg.customMixer[i].roll; - mixsum[1] += cfg.customMixer[i].pitch; - mixsum[2] += cfg.customMixer[i].yaw; + mixsum[0] += mcfg.customMixer[i].roll; + mixsum[1] += mcfg.customMixer[i].pitch; + mixsum[2] += mcfg.customMixer[i].yaw; } uartPrint("Sanity check:\t"); for (i = 0; i < 3; i++) uartPrint(fabs(mixsum[i]) > 0.01f ? "NG\t" : "OK\t"); uartPrint("\r\n"); return; + } else if (strncasecmp(cmdline, "reset", 5) == 0) { + // erase custom mixer + for (i = 0; i < MAX_MOTORS; i++) + mcfg.customMixer[i].throttle = 0.0f; } else if (strncasecmp(cmdline, "load", 4) == 0) { ptr = strchr(cmdline, ' '); if (ptr) { @@ -478,22 +485,22 @@ static void cliCMix(char *cmdline) if (--i < MAX_MOTORS) { ptr = strchr(ptr, ' '); if (ptr) { - cfg.customMixer[i].throttle = _atof(++ptr); + mcfg.customMixer[i].throttle = _atof(++ptr); check++; } ptr = strchr(ptr, ' '); if (ptr) { - cfg.customMixer[i].roll = _atof(++ptr); + mcfg.customMixer[i].roll = _atof(++ptr); check++; } ptr = strchr(ptr, ' '); if (ptr) { - cfg.customMixer[i].pitch = _atof(++ptr); + mcfg.customMixer[i].pitch = _atof(++ptr); check++; } ptr = strchr(ptr, ' '); if (ptr) { - cfg.customMixer[i].yaw = _atof(++ptr); + mcfg.customMixer[i].yaw = _atof(++ptr); check++; } if (check != 4) { @@ -519,7 +526,7 @@ static void cliDefaults(char *cmdline) static void cliDump(char *cmdline) { - int i, val = 0; + int i; char buf[16]; float thr, roll, pitch, yaw; uint32_t mask; @@ -531,17 +538,17 @@ static void cliDump(char *cmdline) cliAux(""); // print out current motor mix - printf("mixer %s\r\n", mixerNames[cfg.mixerConfiguration - 1]); + printf("mixer %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]); // print custom mix if exists - if (cfg.customMixer[0].throttle != 0.0f) { + if (mcfg.customMixer[0].throttle != 0.0f) { for (i = 0; i < MAX_MOTORS; i++) { - if (cfg.customMixer[i].throttle == 0.0f) + if (mcfg.customMixer[i].throttle == 0.0f) break; - thr = cfg.customMixer[i].throttle; - roll = cfg.customMixer[i].roll; - pitch = cfg.customMixer[i].pitch; - yaw = cfg.customMixer[i].yaw; + thr = mcfg.customMixer[i].throttle; + roll = mcfg.customMixer[i].roll; + pitch = mcfg.customMixer[i].pitch; + yaw = mcfg.customMixer[i].yaw; printf("cmix %d", i + 1); if (thr < 0) printf(" "); @@ -575,7 +582,7 @@ static void cliDump(char *cmdline) // print RC MAPPING for (i = 0; i < 8; i++) - buf[cfg.rcmap[i]] = rcChannelLetters[i]; + buf[mcfg.rcmap[i]] = rcChannelLetters[i]; buf[i] = '\0'; printf("map %s\r\n", buf); @@ -685,20 +692,20 @@ static void cliMap(char *cmdline) } uartPrint("Current assignment: "); for (i = 0; i < 8; i++) - out[cfg.rcmap[i]] = rcChannelLetters[i]; + out[mcfg.rcmap[i]] = rcChannelLetters[i]; out[i] = '\0'; printf("%s\r\n", out); } static void cliMixer(char *cmdline) { - uint8_t i; - uint8_t len; - + int i; + int len; + len = strlen(cmdline); if (len == 0) { - printf("Current mixer: %s\r\n", mixerNames[cfg.mixerConfiguration - 1]); + printf("Current mixer: %s\r\n", mixerNames[mcfg.mixerConfiguration - 1]); return; } else if (strncasecmp(cmdline, "list", len) == 0) { uartPrint("Available mixers: "); @@ -717,17 +724,36 @@ static void cliMixer(char *cmdline) break; } if (strncasecmp(cmdline, mixerNames[i], len) == 0) { - cfg.mixerConfiguration = i + 1; + mcfg.mixerConfiguration = i + 1; printf("Mixer set to %s\r\n", mixerNames[i]); break; } } } +static void cliProfile(char *cmdline) +{ + uint8_t len; + int i; + + len = strlen(cmdline); + if (len == 0) { + printf("Current profile: %d\r\n", mcfg.current_profile); + return; + } else { + i = atoi(cmdline); + if (i >= 0 && i <= 2) { + mcfg.current_profile = i; + writeEEPROM(0, false); + cliProfile(""); + } + } +} + static void cliSave(char *cmdline) { uartPrint("Saving..."); - writeParams(0); + writeEEPROM(0, true); uartPrint("\r\nRebooting..."); delay(10); systemReset(false); @@ -856,11 +882,11 @@ static void cliStatus(char *cmdline) if (sensors(SENSOR_ACC)) { printf("ACCHW: %s", accNames[accHardware]); if (accHardware == ACC_MPU6050) - printf(".%c", cfg.mpu6050_scale ? 'o' : 'n'); + printf(".%c", mcfg.mpu6050_scale ? 'o' : 'n'); } uartPrint("\r\n"); - printf("Cycle Time: %d, I2C Errors: %d\r\n", cycleTime, i2cGetErrorCounter()); + printf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cGetErrorCounter(), sizeof(master_t)); } static void cliVersion(char *cmdline) diff --git a/src/config.c b/src/config.c index fd25704fe..fdb7b10a3 100755 --- a/src/config.c +++ b/src/config.c @@ -9,10 +9,11 @@ #define FLASH_PAGE_SIZE ((uint16_t)0x400) #define FLASH_WRITE_ADDR (0x08000000 + (uint32_t)FLASH_PAGE_SIZE * (FLASH_PAGE_COUNT - 1)) // use the last KB for storage -config_t cfg; +master_t mcfg; // master config struct with data independent from profiles +config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static uint8_t EEPROM_CONF_VERSION = 34; +static uint8_t EEPROM_CONF_VERSION = 47; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -23,13 +24,13 @@ void parseRcChannels(const char *input) for (c = input; *c; c++) { s = strchr(rcChannelLetters, *c); if (s) - cfg.rcmap[s - rcChannelLetters] = c - input; + mcfg.rcmap[s - rcChannelLetters] = c - input; } } static uint8_t validEEPROM(void) { - const config_t *temp = (const config_t *)FLASH_WRITE_ADDR; + const master_t *temp = (const master_t *)FLASH_WRITE_ADDR; const uint8_t *p; uint8_t chk = 0; @@ -38,11 +39,11 @@ static uint8_t validEEPROM(void) return 0; // check size and magic numbers - if (temp->size != sizeof(config_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) + if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) return 0; // verify integrity of temporary copy - for (p = (const uint8_t *)temp; p < ((const uint8_t *)temp + sizeof(config_t)); p++) + for (p = (const uint8_t *)temp; p < ((const uint8_t *)temp + sizeof(master_t)); p++) chk ^= *p; // checksum failed @@ -57,8 +58,16 @@ void readEEPROM(void) { uint8_t i; + // Sanity check + if (!validEEPROM()) + failureMode(10); + // Read flash - memcpy(&cfg, (char *)FLASH_WRITE_ADDR, sizeof(config_t)); + memcpy(&mcfg, (char *)FLASH_WRITE_ADDR, sizeof(master_t)); + // Copy current profile + if (mcfg.current_profile > 2) // sanity check + mcfg.current_profile = 0; + memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); for (i = 0; i < 6; i++) lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500; @@ -71,42 +80,64 @@ void readEEPROM(void) 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] + lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE] } cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR } -void writeParams(uint8_t b) +void writeEEPROM(uint8_t b, uint8_t updateProfile) { FLASH_Status status; uint32_t i; uint8_t chk = 0; const uint8_t *p; + int tries = 0; + + // prepare checksum/version constants + mcfg.version = EEPROM_CONF_VERSION; + mcfg.size = sizeof(master_t); + mcfg.magic_be = 0xBE; + mcfg.magic_ef = 0xEF; + mcfg.chk = 0; + + // when updateProfile = true, we copy contents of cfg to global configuration. when false, only profile number is updated, and then that profile is loaded on readEEPROM() + if (updateProfile) { + // copy current in-memory profile to stored configuration + memcpy(&mcfg.profile[mcfg.current_profile], &cfg, sizeof(config_t)); + } - cfg.version = EEPROM_CONF_VERSION; - cfg.size = sizeof(config_t); - cfg.magic_be = 0xBE; - cfg.magic_ef = 0xEF; - cfg.chk = 0; // recalculate checksum before writing - for (p = (const uint8_t *)&cfg; p < ((const uint8_t *)&cfg + sizeof(config_t)); p++) + for (p = (const uint8_t *)&mcfg; p < ((const uint8_t *)&mcfg + sizeof(master_t)); p++) chk ^= *p; - cfg.chk = chk; + mcfg.chk = chk; // write it +retry: FLASH_Unlock(); FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); 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 + for (i = 0; i < sizeof(master_t); i += 4) { + status = FLASH_ProgramWord(FLASH_WRITE_ADDR + i, *(uint32_t *) ((char *)&mcfg + i)); + if (status != FLASH_COMPLETE) { + FLASH_Lock(); + tries++; + if (tries < 3) + goto retry; + else + break; + } } } FLASH_Lock(); + + // Flash write failed - just die now + if (tries == 3 || !validEEPROM()) { + failureMode(10); + } + // re-read written data readEEPROM(); if (b) blinkLED(15, 20, 1); @@ -115,8 +146,11 @@ void writeParams(uint8_t b) void checkFirstTime(bool reset) { // check the EEPROM integrity before resetting values - if (!validEEPROM() || reset) + if (!validEEPROM() || reset) { resetConf(); + // no need to memcpy profile again, we just did it in resetConf() above + writeEEPROM(0, false); + } } // Default settings @@ -125,13 +159,47 @@ static void resetConf(void) int i; const int8_t default_align[3][3] = { /* GYRO */ { 0, 0, 0 }, /* ACC */ { 0, 0, 0 }, /* MAG */ { -2, -3, 1 } }; + // Clear all configuration + memset(&mcfg, 0, sizeof(master_t)); memset(&cfg, 0, sizeof(config_t)); - cfg.version = EEPROM_CONF_VERSION; - cfg.mixerConfiguration = MULTITYPE_QUADX; + mcfg.version = EEPROM_CONF_VERSION; + mcfg.mixerConfiguration = MULTITYPE_QUADX; featureClearAll(); featureSet(FEATURE_VBAT); + // global settings + mcfg.current_profile = 0; // default profile + mcfg.gyro_cmpf_factor = 400; // default MWC + mcfg.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead + mcfg.accZero[0] = 0; + mcfg.accZero[1] = 0; + mcfg.accZero[2] = 0; + memcpy(&mcfg.align, default_align, sizeof(mcfg.align)); + mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect + mcfg.moron_threshold = 32; + mcfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y + mcfg.vbatscale = 110; + mcfg.vbatmaxcellvoltage = 43; + mcfg.vbatmincellvoltage = 33; + mcfg.power_adc_channel = 0; + mcfg.spektrum_hires = 0; + mcfg.midrc = 1500; + mcfg.mincheck = 1100; + mcfg.maxcheck = 1900; + mcfg.retarded_arm = 0; // disable arm/disarm on roll left/right + // Motor/ESC/Servo + mcfg.minthrottle = 1150; + mcfg.maxthrottle = 1850; + mcfg.mincommand = 1000; + mcfg.motor_pwm_rate = 400; + mcfg.servo_pwm_rate = 50; + // gps/nav stuff + mcfg.gps_type = GPS_NMEA; + mcfg.gps_baudrate = 115200; + // serial (USART1) baudrate + mcfg.serial_baudrate = 115200; + // cfg.looptime = 0; cfg.P8[ROLL] = 40; cfg.I8[ROLL] = 30; @@ -141,82 +209,57 @@ static void resetConf(void) cfg.D8[PITCH] = 23; cfg.P8[YAW] = 85; cfg.I8[YAW] = 45; - // cfg.D8[YAW] = 0; - cfg.P8[PIDALT] = 16; - cfg.I8[PIDALT] = 15; - cfg.D8[PIDALT] = 7; + cfg.D8[YAW] = 0; + cfg.P8[PIDALT] = 64; + cfg.I8[PIDALT] = 25; + cfg.D8[PIDALT] = 24; cfg.P8[PIDPOS] = 11; // POSHOLD_P * 100; - // cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; - // cfg.D8[PIDPOS] = 0; + cfg.I8[PIDPOS] = 0; // POSHOLD_I * 100; + cfg.D8[PIDPOS] = 0; cfg.P8[PIDPOSR] = 20; // POSHOLD_RATE_P * 10; cfg.I8[PIDPOSR] = 8; // POSHOLD_RATE_I * 100; cfg.D8[PIDPOSR] = 45; // POSHOLD_RATE_D * 1000; cfg.P8[PIDNAVR] = 14; // NAV_P * 10; cfg.I8[PIDNAVR] = 20; // NAV_I * 100; cfg.D8[PIDNAVR] = 80; // NAV_D * 1000; - cfg.P8[PIDLEVEL] = 70; + cfg.P8[PIDLEVEL] = 90; cfg.I8[PIDLEVEL] = 10; - cfg.D8[PIDLEVEL] = 20; + cfg.D8[PIDLEVEL] = 100; cfg.P8[PIDMAG] = 40; - // cfg.P8[PIDVEL] = 0; - // cfg.I8[PIDVEL] = 0; - // cfg.D8[PIDVEL] = 0; + cfg.P8[PIDVEL] = 0; + cfg.I8[PIDVEL] = 0; + cfg.D8[PIDVEL] = 0; cfg.rcRate8 = 90; cfg.rcExpo8 = 65; - // cfg.rollPitchRate = 0; - // cfg.yawRate = 0; - // cfg.dynThrPID = 0; + cfg.rollPitchRate = 0; + cfg.yawRate = 0; + cfg.dynThrPID = 0; cfg.thrMid8 = 50; - // cfg.thrExpo8 = 0; + cfg.thrExpo8 = 0; // for (i = 0; i < CHECKBOXITEMS; i++) // cfg.activate[i] = 0; - // cfg.angleTrim[0] = 0; - // cfg.angleTrim[1] = 0; - // cfg.accZero[0] = 0; - // cfg.accZero[1] = 0; - // cfg.accZero[2] = 0; - // cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. - memcpy(&cfg.align, default_align, sizeof(cfg.align)); - cfg.acc_hardware = ACC_DEFAULT; // default/autodetect + cfg.angleTrim[0] = 0; + cfg.angleTrim[1] = 0; + cfg.mag_declination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. cfg.acc_lpf_factor = 4; cfg.acc_lpf_for_velocity = 10; cfg.accz_deadband = 50; - cfg.gyro_cmpf_factor = 400; // default MWC - cfg.gyro_lpf = 42; - cfg.mpu6050_scale = 1; // fuck invensense cfg.baro_tab_size = 21; cfg.baro_noise_lpf = 0.6f; cfg.baro_cf = 0.985f; - cfg.moron_threshold = 32; - cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y - cfg.vbatscale = 110; - cfg.vbatmaxcellvoltage = 43; - cfg.vbatmincellvoltage = 33; - // cfg.power_adc_channel = 0; // Radio parseRcChannels("AETR1234"); - // cfg.deadband = 0; - // cfg.yawdeadband = 0; - cfg.alt_hold_throttle_neutral = 20; - // cfg.spektrum_hires = 0; - cfg.midrc = 1500; - cfg.mincheck = 1100; - cfg.maxcheck = 1900; - // cfg.retarded_arm = 0; // disable arm/disarm on roll left/right + cfg.deadband = 0; + cfg.yawdeadband = 0; + cfg.alt_hold_throttle_neutral = 40; + cfg.alt_hold_fast_change = 1; // Failsafe Variables cfg.failsafe_delay = 10; // 1sec cfg.failsafe_off_delay = 200; // 20sec cfg.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people. - // Motor/ESC/Servo - cfg.minthrottle = 1150; - cfg.maxthrottle = 1850; - cfg.mincommand = 1000; - cfg.motor_pwm_rate = 400; - cfg.servo_pwm_rate = 50; - // servos cfg.yaw_direction = 1; cfg.tri_yaw_middle = 1500; @@ -247,23 +290,21 @@ static void resetConf(void) cfg.gimbal_roll_mid = 1500; // gps/nav stuff - cfg.gps_type = GPS_NMEA; - cfg.gps_baudrate = 115200; cfg.gps_wp_radius = 200; cfg.gps_lpf = 20; cfg.nav_slew_rate = 30; cfg.nav_controls_heading = 1; cfg.nav_speed_min = 100; cfg.nav_speed_max = 300; - - // serial (USART1) baudrate - cfg.serial_baudrate = 115200; + cfg.ap_mode = 40; // custom mixer. clear by defaults. for (i = 0; i < MAX_MOTORS; i++) - cfg.customMixer[i].throttle = 0.0f; + mcfg.customMixer[i].throttle = 0.0f; - writeParams(0); + // copy default config into all 3 profiles + for (i = 0; i < 3; i++) + memcpy(&mcfg.profile[i], &cfg, sizeof(config_t)); } bool sensors(uint32_t mask) @@ -288,25 +329,25 @@ uint32_t sensorsMask(void) bool feature(uint32_t mask) { - return cfg.enabledFeatures & mask; + return mcfg.enabledFeatures & mask; } void featureSet(uint32_t mask) { - cfg.enabledFeatures |= mask; + mcfg.enabledFeatures |= mask; } void featureClear(uint32_t mask) { - cfg.enabledFeatures &= ~(mask); + mcfg.enabledFeatures &= ~(mask); } void featureClearAll() { - cfg.enabledFeatures = 0; + mcfg.enabledFeatures = 0; } uint32_t featureMask(void) { - return cfg.enabledFeatures; + return mcfg.enabledFeatures; } diff --git a/src/drv_adc_fy90q.c b/src/drv_adc_fy90q.c index a57b75422..07689ce0e 100644 --- a/src/drv_adc_fy90q.c +++ b/src/drv_adc_fy90q.c @@ -21,6 +21,7 @@ void adcSensorInit(sensor_t *acc, sensor_t *gyro) gyro->init = adcDummyInit; gyro->read = adcGyroRead; gyro->align = adcGyroAlign; + gyro->scale = 1.0f; acc_1G = 376; } diff --git a/src/drv_l3g4200d.c b/src/drv_l3g4200d.c index 6cd09d01d..943958109 100644 --- a/src/drv_l3g4200d.c +++ b/src/drv_l3g4200d.c @@ -29,7 +29,7 @@ static void l3g4200dInit(void); static void l3g4200dRead(int16_t *gyroData); static void l3g4200dAlign(int16_t *gyroData); -bool l3g4200dDetect(sensor_t *gyro) +bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf) { uint8_t deviceid; @@ -42,28 +42,27 @@ bool l3g4200dDetect(sensor_t *gyro) gyro->init = l3g4200dInit; gyro->read = l3g4200dRead; gyro->align = l3g4200dAlign; + // 14.2857dps/lsb scalefactor + gyro->scale = (((32767.0f / 14.2857f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f)); - return true; -} - -void l3g4200dConfig(uint16_t lpf) -{ + // default LPF is set to 32Hz switch (lpf) { - case 32: - mpuLowPassFilter = L3G4200D_DLPF_32HZ; - break; - case 54: - mpuLowPassFilter = L3G4200D_DLPF_54HZ; - break; - case 78: - mpuLowPassFilter = L3G4200D_DLPF_78HZ; - break; - case 93: - mpuLowPassFilter = L3G4200D_DLPF_93HZ; - break; + default: + case 32: + mpuLowPassFilter = L3G4200D_DLPF_32HZ; + break; + case 54: + mpuLowPassFilter = L3G4200D_DLPF_54HZ; + break; + case 78: + mpuLowPassFilter = L3G4200D_DLPF_78HZ; + break; + case 93: + mpuLowPassFilter = L3G4200D_DLPF_93HZ; + break; } - i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter); + return true; } static void l3g4200dInit(void) diff --git a/src/drv_l3g4200d.h b/src/drv_l3g4200d.h index 7c8d0a338..f7b532e90 100644 --- a/src/drv_l3g4200d.h +++ b/src/drv_l3g4200d.h @@ -1,4 +1,3 @@ #pragma once -bool l3g4200dDetect(sensor_t * gyro); -void l3g4200dConfig(uint16_t lpf); +bool l3g4200dDetect(sensor_t *gyro, uint16_t lpf); diff --git a/src/drv_mpu3050.c b/src/drv_mpu3050.c index 8aca692c9..643d4d9f5 100755 --- a/src/drv_mpu3050.c +++ b/src/drv_mpu3050.c @@ -31,7 +31,7 @@ static void mpu3050Read(int16_t *gyroData); static void mpu3050Align(int16_t *gyroData); static void mpu3050ReadTemp(int16_t *tempData); -bool mpu3050Detect(sensor_t *gyro) +bool mpu3050Detect(sensor_t *gyro, uint16_t lpf) { bool ack; @@ -45,12 +45,10 @@ bool mpu3050Detect(sensor_t *gyro) gyro->read = mpu3050Read; gyro->align = mpu3050Align; gyro->temperature = mpu3050ReadTemp; + // 16.4 dps/lsb scalefactor + gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f)); - return true; -} - -void mpu3050Config(uint16_t lpf) -{ + // default lpf is 42Hz switch (lpf) { case 256: mpuLowPassFilter = MPU3050_DLPF_256HZ; @@ -61,6 +59,7 @@ void mpu3050Config(uint16_t lpf) case 98: mpuLowPassFilter = MPU3050_DLPF_98HZ; break; + default: case 42: mpuLowPassFilter = MPU3050_DLPF_42HZ; break; @@ -72,7 +71,7 @@ void mpu3050Config(uint16_t lpf) break; } - i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter); + return true; } static void mpu3050Init(void) diff --git a/src/drv_mpu3050.h b/src/drv_mpu3050.h index 1ae321e23..228c79185 100755 --- a/src/drv_mpu3050.h +++ b/src/drv_mpu3050.h @@ -1,4 +1,3 @@ #pragma once -bool mpu3050Detect(sensor_t *gyro); -void mpu3050Config(uint16_t lpf); +bool mpu3050Detect(sensor_t *gyro, uint16_t lpf); diff --git a/src/drv_mpu6050.c b/src/drv_mpu6050.c index a1d6f5c7d..6c3ed1475 100644 --- a/src/drv_mpu6050.c +++ b/src/drv_mpu6050.c @@ -114,27 +114,23 @@ #define MPU_RA_WHO_AM_I 0x75 #define MPU6050_SMPLRT_DIV 0 //8000Hz -// #define MPU6050_DLPF_CFG 0 // 256Hz -#define MPU6050_DLPF_CFG 3 // 42Hz - -#define MPU6000ES_REV_C4 0x14 -#define MPU6000ES_REV_C5 0x15 -#define MPU6000ES_REV_D6 0x16 -#define MPU6000ES_REV_D7 0x17 -#define MPU6000ES_REV_D8 0x18 -#define MPU6000_REV_C4 0x54 -#define MPU6000_REV_C5 0x55 -#define MPU6000_REV_D6 0x56 -#define MPU6000_REV_D7 0x57 -#define MPU6000_REV_D8 0x58 -#define MPU6000_REV_D9 0x59 + +#define MPU6050_LPF_256HZ 0 +#define MPU6050_LPF_188HZ 1 +#define MPU6050_LPF_98HZ 2 +#define MPU6050_LPF_42HZ 3 +#define MPU6050_LPF_20HZ 4 +#define MPU6050_LPF_10HZ 5 +#define MPU6050_LPF_5HZ 6 + +static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ; static void mpu6050AccInit(void); -static void mpu6050AccRead(int16_t * accData); -static void mpu6050AccAlign(int16_t * accData); +static void mpu6050AccRead(int16_t *accData); +static void mpu6050AccAlign(int16_t *accData); static void mpu6050GyroInit(void); -static void mpu6050GyroRead(int16_t * gyroData); -static void mpu6050GyroAlign(int16_t * gyroData); +static void mpu6050GyroRead(int16_t *gyroData); +static void mpu6050GyroAlign(int16_t *gyroData); #ifdef MPU6050_DMP static void mpu6050DmpInit(void); @@ -145,7 +141,7 @@ int16_t dmpGyroData[3]; extern uint16_t acc_1G; static uint8_t mpuAccelHalf = 0; -bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale) +bool mpu6050Detect(sensor_t *acc, sensor_t *gyro, uint16_t lpf, uint8_t *scale) { bool ack; uint8_t sig, rev; @@ -194,11 +190,39 @@ bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale) gyro->init = mpu6050GyroInit; gyro->read = mpu6050GyroRead; gyro->align = mpu6050GyroAlign; + // 16.4 dps/lsb scalefactor + gyro->scale = (((32767.0f / 16.4f) * M_PI) / ((32767.0f / 4.0f) * 180.0f * 1000000.0f)); // give halfacc (old revision) back to system if (scale) *scale = mpuAccelHalf; + // default lpf is 42Hz + switch (lpf) { + case 256: + mpuLowPassFilter = MPU6050_LPF_256HZ; + break; + case 188: + mpuLowPassFilter = MPU6050_LPF_188HZ; + break; + case 98: + mpuLowPassFilter = MPU6050_LPF_98HZ; + break; + default: + case 42: + mpuLowPassFilter = MPU6050_LPF_42HZ; + break; + case 20: + mpuLowPassFilter = MPU6050_LPF_20HZ; + break; + case 10: + mpuLowPassFilter = MPU6050_LPF_10HZ; + break; + case 5: + mpuLowPassFilter = MPU6050_LPF_5HZ; + break; + } + #ifdef MPU6050_DMP mpu6050DmpInit(); #endif @@ -256,7 +280,7 @@ static void mpu6050GyroInit(void) i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS - i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, MPU6050_DLPF_CFG); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) + i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec // ACC Init stuff. Moved into gyro init because the reset above would screw up accel config. Oops. diff --git a/src/drv_mpu6050.h b/src/drv_mpu6050.h index cdb50d28a..c9501f271 100644 --- a/src/drv_mpu6050.h +++ b/src/drv_mpu6050.h @@ -1,5 +1,5 @@ #pragma once -bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint8_t *scale); +bool mpu6050Detect(sensor_t * acc, sensor_t * gyro, uint16_t lpf, uint8_t *scale); void mpu6050DmpLoop(void); void mpu6050DmpResetFifo(void); diff --git a/src/gps.c b/src/gps.c index c6aaa320a..77bfd4212 100644 --- a/src/gps.c +++ b/src/gps.c @@ -45,12 +45,12 @@ void gpsInit(uint32_t baudrate) GPS_set_pids(); uart2Init(baudrate, GPS_NewData, false); - if (cfg.gps_type == GPS_UBLOX) + if (mcfg.gps_type == GPS_UBLOX) offset = 0; - else if (cfg.gps_type == GPS_MTK) + else if (mcfg.gps_type == GPS_MTK) offset = 4; - if (cfg.gps_type != GPS_NMEA) { + if (mcfg.gps_type != GPS_NMEA) { for (i = 0; i < 5; i++) { uart2ChangeBaud(init_speed[i]); switch (baudrate) { @@ -72,12 +72,12 @@ void gpsInit(uint32_t baudrate) } uart2ChangeBaud(baudrate); - if (cfg.gps_type == GPS_UBLOX) { + if (mcfg.gps_type == GPS_UBLOX) { for (i = 0; i < sizeof(ubloxInit); i++) { uart2Write(ubloxInit[i]); // send ubx init binary delay(4); } - } else if (cfg.gps_type == GPS_MTK) { + } else if (mcfg.gps_type == GPS_MTK) { gpsPrint("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // only GGA and RMC sentence gpsPrint("$PMTK220,200*2C\r\n"); // 5 Hz update rate } @@ -92,7 +92,7 @@ static void gpsPrint(const char *str) { while (*str) { uart2Write(*str); - if (cfg.gps_type == GPS_UBLOX) + if (mcfg.gps_type == GPS_UBLOX) delay(4); str++; } @@ -134,6 +134,29 @@ static int16_t GPS_calc_desired_speed(int16_t max_speed, bool _slow); int32_t wrap_18000(int32_t error); static int32_t wrap_36000(int32_t angle); +typedef struct { + int16_t last_velocity; +} LeadFilter_PARAM; + +void leadFilter_clear(LeadFilter_PARAM *param) +{ + param->last_velocity = 0; +} + +int32_t leadFilter_getPosition(LeadFilter_PARAM *param, int32_t pos, int16_t vel, float lag_in_seconds) +{ + int16_t accel_contribution = (vel - param->last_velocity) * lag_in_seconds * lag_in_seconds; + int16_t vel_contribution = vel * lag_in_seconds; + + // store velocity for next iteration + param->last_velocity = vel; + + return pos + vel_contribution + accel_contribution; +} + +LeadFilter_PARAM xLeadFilter; +LeadFilter_PARAM yLeadFilter; + typedef struct { float kP; float kI; @@ -752,7 +775,7 @@ static uint8_t hex_c(uint8_t n) static bool GPS_newFrame(char c) { - switch (cfg.gps_type) { + switch (mcfg.gps_type) { case GPS_NMEA: // NMEA case GPS_MTK: // MTK outputs NMEA too return GPS_NMEA_newFrame(c); diff --git a/src/imu.c b/src/imu.c index 78605c9a4..64da1462f 100755 --- a/src/imu.c +++ b/src/imu.c @@ -4,12 +4,13 @@ int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; float accLPFVel[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; -int16_t errorAltitudeI = 0; +int32_t BaroAlt; +int16_t sonarAlt; // to think about the unit +int32_t EstAlt; // in cm +int16_t BaroPID = 0; +int32_t AltHold; +int16_t errorAltitudeI = 0; +int16_t vario = 0; // variometer in cm/s float magneticDeclination = 0.0f; // calculated at startup from config float accVelScale; @@ -87,15 +88,15 @@ void computeIMU(void) 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; + Smoothing[ROLL] = (mcfg.gyro_smoothing_factor >> 16) & 0xff; + Smoothing[PITCH] = (mcfg.gyro_smoothing_factor >> 8) & 0xff; + Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff; } for (axis = 0; axis < 3; axis++) { gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]); gyroSmooth[axis] = gyroData[axis]; } - } else if (cfg.mixerConfiguration == MULTITYPE_TRI) { + } else if (mcfg.mixerConfiguration == MULTITYPE_TRI) { gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3; gyroYawSmooth = gyroData[YAW]; } @@ -137,17 +138,10 @@ void computeIMU(void) //****** end of advanced users settings ************* -#define INV_GYR_CMPF_FACTOR (1.0f / ((float)cfg.gyro_cmpf_factor + 1.0f)) +#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f)) #define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f)) -#define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000 - -// #define GYRO_SCALE ((2380 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result (ITG-3200) - // +-2000/sec deg scale - //#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f) - // +- 200/sec deg scale - // 1.5 is emperical, not sure what it means - // should be in rad/sec +// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000 typedef struct fp_vector { float X; @@ -225,7 +219,7 @@ static void getEstimatedAttitude(void) uint32_t currentT = micros(); float scale, deltaGyroAngle[3]; - scale = (currentT - previousT) * GYRO_SCALE; + scale = (currentT - previousT) * gyro.scale; previousT = currentT; // Initialization @@ -265,7 +259,7 @@ static void getEstimatedAttitude(void) // To do that, we just skip filter, as EstV already rotated by Gyro if ((36 < accMag && accMag < 196) || f.SMALL_ANGLES_25) { for (axis = 0; axis < 3; axis++) - EstG.A[axis] = (EstG.A[axis] * (float)cfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; + EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR; } if (sensors(SENSOR_MAG)) { @@ -282,7 +276,7 @@ static void getEstimatedAttitude(void) angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z); angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f); #endif - + #ifdef MAG if (sensors(SENSOR_MAG)) { #if INACCURATE @@ -356,7 +350,7 @@ int32_t isq(int32_t x) return x * x; } -void getEstimatedAltitude(void) +int getEstimatedAltitude(void) { static uint32_t deadLine = INIT_DELAY; static int16_t baroHistTab[BARO_TAB_SIZE_MAX]; @@ -371,7 +365,7 @@ void getEstimatedAltitude(void) float baroVel; if ((int32_t)(currentTime - deadLine) < UPDATE_INTERVAL) - return; + return 0; dTime = currentTime - deadLine; deadLine = currentTime; @@ -421,5 +415,7 @@ void getEstimatedAltitude(void) // D BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150); debug[3] = BaroPID; + + return 1; } #endif /* BARO */ diff --git a/src/main.c b/src/main.c index 8d4972e4a..cb07356db 100755 --- a/src/main.c +++ b/src/main.c @@ -48,17 +48,15 @@ int main(void) readEEPROM(); // configure power ADC - if (cfg.power_adc_channel > 0 && (cfg.power_adc_channel == 1 || cfg.power_adc_channel == 9)) - adc_params.powerAdcChannel = cfg.power_adc_channel; + if (mcfg.power_adc_channel > 0 && (mcfg.power_adc_channel == 1 || mcfg.power_adc_channel == 9)) + adc_params.powerAdcChannel = mcfg.power_adc_channel; else { adc_params.powerAdcChannel = 0; - cfg.power_adc_channel = 0; + mcfg.power_adc_channel = 0; } adcInit(&adc_params); - serialInit(cfg.serial_baudrate); - // We have these sensors #ifndef FY90Q // AfroFlight32 @@ -70,7 +68,7 @@ int main(void) mixerInit(); // this will set useServo var depending on mixer type // when using airplane/wing mixer, servo/motor outputs are remapped - if (cfg.mixerConfiguration == MULTITYPE_AIRPLANE || cfg.mixerConfiguration == MULTITYPE_FLYING_WING) + if (mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_FLYING_WING) pwm_params.airplane = true; else pwm_params.airplane = false; @@ -79,9 +77,9 @@ int main(void) pwm_params.enableInput = !feature(FEATURE_SPEKTRUM); // disable inputs if using spektrum pwm_params.useServos = useServo; pwm_params.extraServos = cfg.gimbal_flags & GIMBAL_FORWARDAUX; - pwm_params.motorPwmRate = cfg.motor_pwm_rate; - pwm_params.servoPwmRate = cfg.servo_pwm_rate; - switch (cfg.power_adc_channel) { + pwm_params.motorPwmRate = mcfg.motor_pwm_rate; + pwm_params.servoPwmRate = mcfg.servo_pwm_rate; + switch (mcfg.power_adc_channel) { case 1: pwm_params.adcChannel = PWM2; break; @@ -105,7 +103,7 @@ int main(void) // spektrum and GPS are mutually exclusive // Optional GPS - available in both PPM and PWM input mode, in PWM input, reduces number of available channels by 2. if (feature(FEATURE_GPS)) - gpsInit(cfg.gps_baudrate); + gpsInit(mcfg.gps_baudrate); } #ifdef SONAR // sonar stuff only works with PPM @@ -135,11 +133,14 @@ int main(void) // Check battery type/voltage if (feature(FEATURE_VBAT)) batteryInit(); - + + serialInit(mcfg.serial_baudrate); + previousTime = micros(); - if (cfg.mixerConfiguration == MULTITYPE_GIMBAL) + if (mcfg.mixerConfiguration == MULTITYPE_GIMBAL) calibratingA = 400; - calibratingG = 1000; + calibratingG = 1000; + calibratingB = 200; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles f.SMALL_ANGLES_25 = 1; // loopy @@ -151,6 +152,6 @@ int main(void) void HardFault_Handler(void) { // fall out of the sky - writeAllMotors(cfg.mincommand); + writeAllMotors(mcfg.mincommand); while (1); } diff --git a/src/mixer.c b/src/mixer.c index 0edb0f9f8..dd9c5699a 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -7,6 +7,7 @@ int16_t motor[MAX_MOTORS]; int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; static motorMixer_t currentMixer[MAX_MOTORS]; +static servoParam_t currentServo[MAX_SERVOS]; static const motorMixer_t mixerTri[] = { { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR @@ -131,31 +132,38 @@ const mixer_t mixers[] = { { 0, 0, NULL }, // MULTITYPE_CUSTOM }; +static const servoParam_t servoInit = { + SERVO_NORMAL, // direction + 1500, // middle + 1020, // min + 2000, // max +}; + void mixerInit(void) { int i; // enable servos for mixes that require them. note, this shifts motor counts. - useServo = mixers[cfg.mixerConfiguration].useServo; + useServo = mixers[mcfg.mixerConfiguration].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't if (feature(FEATURE_SERVO_TILT)) useServo = 1; - if (cfg.mixerConfiguration == MULTITYPE_CUSTOM) { + if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) { // load custom mixer into currentMixer for (i = 0; i < MAX_MOTORS; i++) { // check if done - if (cfg.customMixer[i].throttle == 0.0f) + if (mcfg.customMixer[i].throttle == 0.0f) break; - currentMixer[i] = cfg.customMixer[i]; + currentMixer[i] = mcfg.customMixer[i]; numberMotor++; } } else { - numberMotor = mixers[cfg.mixerConfiguration].numberMotor; + numberMotor = mixers[mcfg.mixerConfiguration].numberMotor; // copy motor-based mixers - if (mixers[cfg.mixerConfiguration].motor) { + if (mixers[mcfg.mixerConfiguration].motor) { for (i = 0; i < numberMotor; i++) - currentMixer[i] = mixers[cfg.mixerConfiguration].motor[i]; + currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; } } } @@ -168,12 +176,12 @@ void mixerLoadMix(int index) index++; // clear existing for (i = 0; i < MAX_MOTORS; i++) - cfg.customMixer[i].throttle = 0.0f; + mcfg.customMixer[i].throttle = 0.0f; // do we have anything here to begin with? if (mixers[index].motor != NULL) { for (i = 0; i < mixers[index].numberMotor; i++) - cfg.customMixer[i] = mixers[index].motor[i]; + mcfg.customMixer[i] = mixers[index].motor[i]; } } @@ -182,7 +190,7 @@ void writeServos(void) if (!useServo) return; - switch (cfg.mixerConfiguration) { + switch (mcfg.mixerConfiguration) { case MULTITYPE_BI: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); @@ -298,7 +306,7 @@ void mixTable(void) motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw; // airplane / servo mixes - switch (cfg.mixerConfiguration) { + switch (mcfg.mixerConfiguration) { case MULTITYPE_BI: 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 @@ -321,8 +329,8 @@ void mixTable(void) motor[0] = rcCommand[THROTTLE]; if (f.PASSTHRU_MODE) { // do not use sensors for correction, simple 2 channel mixing - servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - cfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - cfg.midrc); - servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - cfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - cfg.midrc); + servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - mcfg.midrc); + servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - mcfg.midrc); } else { // use sensors to correct (gyro only or gyro + acc) servo[0] = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL]; @@ -338,9 +346,9 @@ void mixTable(void) uint16_t aux[2] = { 0, 0 }; if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY)) - aux[0] = rcData[AUX3] - cfg.midrc; + aux[0] = rcData[AUX3] - mcfg.midrc; if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34)) - aux[1] = rcData[AUX4] - cfg.midrc; + aux[1] = rcData[AUX4] - mcfg.midrc; servo[0] = cfg.gimbal_pitch_mid + aux[0]; servo[1] = cfg.gimbal_roll_mid + aux[1]; @@ -372,16 +380,16 @@ void mixTable(void) if (motor[i] > maxMotor) maxMotor = motor[i]; for (i = 0; i < numberMotor; i++) { - 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]) < cfg.mincheck) { + if (maxMotor > mcfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. + motor[i] -= maxMotor - mcfg.maxthrottle; + motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); + if ((rcData[THROTTLE]) < mcfg.mincheck) { if (!feature(FEATURE_MOTOR_STOP)) - motor[i] = cfg.minthrottle; + motor[i] = mcfg.minthrottle; else - motor[i] = cfg.mincommand; + motor[i] = mcfg.mincommand; } if (!f.ARMED) - motor[i] = cfg.mincommand; + motor[i] = mcfg.mincommand; } } diff --git a/src/mw.c b/src/mw.c index 7330e2490..9156154a3 100755 --- a/src/mw.c +++ b/src/mw.c @@ -21,6 +21,7 @@ int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // inter int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE +uint16_t rssi; // range: [0;1023] rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) uint8_t dynP8[3], dynI8[3], dynD8[3]; @@ -100,7 +101,7 @@ void annexCode(void) } for (axis = 0; axis < 3; axis++) { - tmp = min(abs(rcData[axis] - cfg.midrc), 500); + tmp = min(abs(rcData[axis] - mcfg.midrc), 500); if (axis != 2) { // ROLL & PITCH if (cfg.deadband) { if (tmp > cfg.deadband) { @@ -127,12 +128,12 @@ void annexCode(void) } dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100; dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100; - if (rcData[axis] < cfg.midrc) + if (rcData[axis] < mcfg.midrc) rcCommand[axis] = -rcCommand[axis]; } - tmp = constrain(rcData[THROTTLE], cfg.mincheck, 2000); - tmp = (uint32_t) (tmp - cfg.mincheck) * 1000 / (2000 - cfg.mincheck); // [MINCHECK;2000] -> [0;1000] + tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000); + tmp = (uint32_t) (tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.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] @@ -152,7 +153,7 @@ void annexCode(void) vbatRaw += vbatRawArray[i]; vbat = batteryAdcToVoltage(vbatRaw / 8); } - if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off + if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off buzzerFreq = 0; } else buzzerFreq = 4; // low battery @@ -214,9 +215,9 @@ uint16_t pwmReadRawRC(uint8_t chan) { uint16_t data; - data = pwmRead(cfg.rcmap[chan]); + data = pwmRead(mcfg.rcmap[chan]); if (data < 750 || data > 2250) - data = cfg.midrc; + data = mcfg.midrc; return data; } @@ -242,9 +243,36 @@ void computeRC(void) } } +static void mwArm(void) +{ + if (calibratingG == 0 && f.ACC_CALIBRATED) { + // TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2 + // TODO: && ( !feature || ( feature && ( failsafecnt > 2) ) + if (!f.ARMED) { // arm now! + f.ARMED = 1; + headFreeModeHold = heading; + } + } else if (!f.ARMED) { + blinkLED(2, 255, 1); + } +} + +static void mwDisarm(void) +{ + if (f.ARMED) + f.ARMED = 0; +} + +static void mwVario(void) +{ + +} + void loop(void) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors + static uint8_t rcSticks; // this hold sticks position for command combos + uint8_t stTmp = 0; uint8_t axis, i; int16_t error, errorAngle; int16_t delta, deltaSum; @@ -258,6 +286,7 @@ void loop(void) static uint32_t loopTime; uint16_t auxState = 0; int16_t prop; + static uint8_t GPSNavReset = 1; // this will return false if spektrum is disabled. shrug. if (spektrumFrameComplete()) @@ -273,36 +302,72 @@ void loop(void) if (feature(FEATURE_FAILSAFE)) { if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level for (i = 0; i < 3; i++) - rcData[i] = cfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec) + rcData[i] = mcfg.midrc; // after specified guard time after RC signal is lost (in 0.1sec) rcData[THROTTLE] = cfg.failsafe_throttle; if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) { // Turn OFF motors after specified Time (in 0.1sec) - f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents + mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm } failsafeEvents++; } if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) { // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm - f.ARMED = 0; // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents + mwDisarm(); // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents f.OK_TO_ARM = 0; // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm } failsafeCnt++; } + // end of failsafe routine - next change is made with RcOptions setting + + // ------------------ STICKS COMMAND HANDLER -------------------- + // checking sticks positions + for (i = 0; i < 4; i++) { + stTmp >>= 2; + if (rcData[i] > mcfg.mincheck) + stTmp |= 0x80; // check for MIN + if (rcData[i] < mcfg.maxcheck) + stTmp |= 0x40; // check for MAX + } + if (stTmp == rcSticks) { + if (rcDelayCommand < 250) + rcDelayCommand++; + } else + rcDelayCommand = 0; + rcSticks = stTmp; - if (rcData[THROTTLE] < cfg.mincheck) { + // perform actions + if (rcData[THROTTLE] < mcfg.mincheck) { errorGyroI[ROLL] = 0; errorGyroI[PITCH] = 0; errorGyroI[YAW] = 0; errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; - rcDelayCommand++; - if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && !f.ARMED) { - if (rcDelayCommand == 20) { + if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX + if (rcOptions[BOXARM] && f.OK_TO_ARM) + mwArm(); + else if (f.ARMED) + mwDisarm(); + } + } + + if (rcDelayCommand == 20) { + if (f.ARMED) { // actions during armed + // Disarm on throttle down + yaw + if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE)) + mwDisarm(); + // Disarm on roll (only when retarded_arm is enabled) + if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO)) + mwDisarm(); + } else { // actions during not armed + i = 0; + // GYRO calibration + if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { calibratingG = 1000; if (feature(FEATURE_GPS)) GPS_reset_home_position(); - } - } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (!f.ARMED && rcData[YAW] < cfg.mincheck && rcData[PITCH] > cfg.maxcheck && rcData[ROLL] > cfg.maxcheck)) { - if (rcDelayCommand == 20) { + if (sensors(SENSOR_BARO)) + calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) + // Inflight ACC Calibration + } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) { if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing AccInflightCalibrationMeasurementDone = 0; AccInflightCalibrationSavetoEEProm = 1; @@ -315,72 +380,61 @@ void loop(void) } } } - } else if (cfg.activate[BOXARM] > 0) { - if (rcOptions[BOXARM] && f.OK_TO_ARM) { - // TODO: feature(FEATURE_FAILSAFE) && failsafeCnt == 0 - f.ARMED = 1; - headFreeModeHold = heading; - } else if (f.ARMED) - f.ARMED = 0; - rcDelayCommand = 0; - } else if ((rcData[YAW] < cfg.mincheck || (cfg.retarded_arm == 1 && rcData[ROLL] < cfg.mincheck)) && f.ARMED) { - if (rcDelayCommand == 20) - f.ARMED = 0; // rcDelayCommand = 20 => 20x20ms = 0.4s = time to wait for a specific RC command to be acknowledged - } else if ((rcData[YAW] > cfg.maxcheck || (rcData[ROLL] > cfg.maxcheck && cfg.retarded_arm == 1)) && rcData[PITCH] < cfg.maxcheck && !f.ARMED && calibratingG == 0 && f.ACC_CALIBRATED) { - if (rcDelayCommand == 20) { - f.ARMED = 1; - headFreeModeHold = heading; + + // Multiple configuration profiles + if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1 + i = 1; + else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2 + i = 2; + else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3 + i = 3; + if (i) { + mcfg.current_profile = i - 1; + writeEEPROM(0, false); + blinkLED(2, 40, i); + // TODO alarmArray[0] = i; } - } else - rcDelayCommand = 0; - } else if (rcData[THROTTLE] > cfg.maxcheck && !f.ARMED) { - if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=left, pitch=min - if (rcDelayCommand == 20) + + // Arm via YAW + if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) + mwArm(); + // Arm via ROLL + else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI)) + mwArm(); + // Calibrating Acc + else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA = 400; - rcDelayCommand++; - } else if (rcData[YAW] > cfg.maxcheck && rcData[PITCH] < cfg.mincheck) { // throttle=max, yaw=right, pitch=min - if (rcDelayCommand == 20) - f.CALIBRATE_MAG = 1; // MAG calibration request - rcDelayCommand++; - } else if (rcData[PITCH] > cfg.maxcheck) { - cfg.angleTrim[PITCH] += 2; - writeParams(1); -#ifdef LEDRING - if (feature(FEATURE_LED_RING)) - ledringBlink(); -#endif - } else if (rcData[PITCH] < cfg.mincheck) { - cfg.angleTrim[PITCH] -= 2; - writeParams(1); -#ifdef LEDRING - if (feature(FEATURE_LED_RING)) - ledringBlink(); -#endif - } else if (rcData[ROLL] > cfg.maxcheck) { - cfg.angleTrim[ROLL] += 2; - writeParams(1); -#ifdef LEDRING - if (feature(FEATURE_LED_RING)) - ledringBlink(); -#endif - } else if (rcData[ROLL] < cfg.mincheck) { - cfg.angleTrim[ROLL] -= 2; - writeParams(1); -#ifdef LEDRING - if (feature(FEATURE_LED_RING)) - ledringBlink(); -#endif - } else { - rcDelayCommand = 0; + // Calibrating Mag + else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) + f.CALIBRATE_MAG = 1; + i = 0; + // Acc Trim + if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) { + cfg.angleTrim[PITCH] += 2; + i = 1; + } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) { + cfg.angleTrim[PITCH] -= 2; + i = 1; + } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) { + cfg.angleTrim[ROLL] += 2; + i = 1; + } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) { + cfg.angleTrim[ROLL] -= 2; + i = 1; + } + if (i) { + writeEEPROM(1, false); + rcDelayCommand = 0; // allow autorepetition + } } } if (feature(FEATURE_INFLIGHT_ACC_CAL)) { - if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > cfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement + if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement InflightcalibratingA = 50; AccInflightCalibrationArmed = 0; } - if (rcOptions[BOXPASSTHRU]) { // Use the Passthru Option to activate : Passthru = TRUE Meausrement started, Land and passtrhu = 0 measurement stored + if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone) InflightcalibratingA = 50; } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) { @@ -389,12 +443,13 @@ void loop(void) } } + // Check AUX switches for (i = 0; i < 4; i++) auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2); for (i = 0; i < CHECKBOXITEMS; i++) rcOptions[i] = (auxState & cfg.activate[i]) > 0; - // note: if FAILSAFE is disable, failsafeCnt > 5*FAILSAVE_DELAY is always false + // note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode if (!f.ANGLE_MODE) { @@ -407,6 +462,7 @@ void loop(void) } if (rcOptions[BOXHORIZON]) { + f.ANGLE_MODE = 0; if (!f.HORIZON_MODE) { errorAngleI[ROLL] = 0; errorAngleI[PITCH] = 0; @@ -426,6 +482,7 @@ void loop(void) #ifdef BARO if (sensors(SENSOR_BARO)) { + // Baro alt hold activate if (rcOptions[BOXBARO]) { if (!f.BARO_MODE) { f.BARO_MODE = 1; @@ -434,8 +491,19 @@ void loop(void) errorAltitudeI = 0; BaroPID = 0; } - } else + } else { f.BARO_MODE = 0; + } + // Vario signalling activate + if (feature(FEATURE_VARIO)) { + if (rcOptions[BOXVARIO]) { + if (!f.VARIO_MODE) { + f.VARIO_MODE = 1; + } + } else { + f.VARIO_MODE = 0; + } + } } #endif @@ -446,8 +514,9 @@ void loop(void) f.MAG_MODE = 1; magHold = heading; } - } else + } else { f.MAG_MODE = 0; + } if (rcOptions[BOXHEADFREE]) { if (!f.HEADFREE_MODE) { f.HEADFREE_MODE = 1; @@ -463,26 +532,39 @@ void loop(void) if (sensors(SENSOR_GPS)) { if (f.GPS_FIX && GPS_numSat >= 5) { + // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority if (rcOptions[BOXGPSHOME]) { if (!f.GPS_HOME_MODE) { f.GPS_HOME_MODE = 1; + f.GPS_HOLD_MODE = 0; + GPSNavReset = 0; GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]); nav_mode = NAV_MODE_WP; } } else { f.GPS_HOME_MODE = 0; - } - if (rcOptions[BOXGPSHOLD]) { - if (!f.GPS_HOLD_MODE) { - f.GPS_HOLD_MODE = 1; - GPS_hold[LAT] = GPS_coord[LAT]; - GPS_hold[LON] = GPS_coord[LON]; - GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); - nav_mode = NAV_MODE_POSHOLD; + if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) { + if (!f.GPS_HOLD_MODE) { + f.GPS_HOLD_MODE = 1; + GPSNavReset = 0; + GPS_hold[LAT] = GPS_coord[LAT]; + GPS_hold[LON] = GPS_coord[LON]; + GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); + nav_mode = NAV_MODE_POSHOLD; + } + } else { + f.GPS_HOLD_MODE = 0; + // both boxes are unselected here, nav is reset if not already done + if (GPSNavReset == 0) { + GPSNavReset = 1; + GPS_reset_nav(); + } } - } else { - f.GPS_HOLD_MODE = 0; } + } else { + f.GPS_HOME_MODE = 0; + f.GPS_HOLD_MODE = 0; + nav_mode = NAV_MODE_NONE; } } @@ -492,47 +574,49 @@ void loop(void) f.PASSTHRU_MODE = 0; } - if (cfg.mixerConfiguration == MULTITYPE_FLYING_WING || cfg.mixerConfiguration == MULTITYPE_AIRPLANE) { + if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) { f.HEADFREE_MODE = 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++ % 4) { + static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes + if (taskOrder > 3) + taskOrder -= 4; + switch (taskOrder) { case 0: + taskOrder++; #ifdef MAG - if (sensors(SENSOR_MAG)) - Mag_getADC(); + if (sensors(SENSOR_MAG) && Mag_getADC()) + break; #endif - break; case 1: + taskOrder++; #ifdef BARO - if (sensors(SENSOR_BARO)) - Baro_update(); + if (sensors(SENSOR_BARO) && Baro_update()) + break; #endif - break; case 2: + taskOrder++; #ifdef BARO - if (sensors(SENSOR_BARO)) - getEstimatedAltitude(); -#endif + if (sensors(SENSOR_BARO) && getEstimatedAltitude()) break; +#endif case 3: + taskOrder++; #ifdef SONAR if (sensors(SENSOR_SONAR)) { Sonar_update(); debug[2] = sonarAlt; } #endif - break; - default: - taskOrder = 0; + if (feature(FEATURE_VARIO) && f.VARIO_MODE) + mwVario(); break; } } currentTime = micros(); - if (cfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { - loopTime = currentTime + cfg.looptime; + if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { + loopTime = currentTime + mcfg.looptime; computeIMU(); // Measure loop rate just afer reading the sensors @@ -561,20 +645,44 @@ void loop(void) #ifdef BARO if (sensors(SENSOR_BARO)) { if (f.BARO_MODE) { - if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { - f.BARO_MODE = 0; // so that a new althold reference is defined + static uint8_t isAltHoldChanged = 0; + static int16_t AltHoldCorr = 0; + if (cfg.alt_hold_fast_change) { + // rapid alt changes + if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { + errorAltitudeI = 0; + isAltHoldChanged = 1; + rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral; + } else { + if (isAltHoldChanged) { + AltHold = EstAlt; + isAltHoldChanged = 0; + } + rcCommand[THROTTLE] = initialThrottleHold + BaroPID; + } + } else { + // slow alt changes for apfags + if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) { + // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms) + AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold; + if (abs(AltHoldCorr) > 500) { + AltHold += AltHoldCorr / 500; + AltHoldCorr %= 500; + } + errorAltitudeI = 0; + isAltHoldChanged = 1; + } else if (isAltHoldChanged) { + AltHold = EstAlt; + isAltHoldChanged = 0; + } + rcCommand[THROTTLE] = initialThrottleHold + BaroPID; } - rcCommand[THROTTLE] = initialThrottleHold + BaroPID; } } #endif if (sensors(SENSOR_GPS)) { - // Check that we really need to navigate ? - if ((!f.GPS_HOME_MODE && !f.GPS_HOLD_MODE) || !f.GPS_FIX_HOME) { - // If not. Reset nav loops and all nav related parameters - GPS_reset_nav(); - } else { + if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) { float sin_yaw_y = sinf(heading * 0.0174532925f); float cos_yaw_x = cosf(heading * 0.0174532925f); if (cfg.nav_slew_rate) { @@ -595,17 +703,13 @@ void loop(void) if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC // 50 degrees max inclination errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; -#ifdef LEVEL_PDF - PTermACC = -(int32_t)angle[axis] * cfg.P8[PIDLEVEL] / 100; -#else PTermACC = (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 PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5); errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp ITermACC = ((int32_t)errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12; } - if (!f.ANGLE_MODE || axis == 2) { // MODE relying on GYRO or YAW axis + if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis]; error -= gyroData[axis]; diff --git a/src/mw.h b/src/mw.h index 2f47a83b5..b91dc8561 100755 --- a/src/mw.h +++ b/src/mw.h @@ -4,7 +4,7 @@ #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations #define BARO_TAB_SIZE_MAX 48 -#define VERSION 211 +#define VERSION 220 #define LAT 0 #define LON 1 @@ -77,29 +77,51 @@ enum { }; enum { - BOXANGLE = 0, + BOXARM = 0, + BOXANGLE, BOXHORIZON, BOXBARO, + BOXVARIO, BOXMAG, + BOXHEADFREE, + BOXHEADADJ, BOXCAMSTAB, BOXCAMTRIG, - BOXARM, BOXGPSHOME, BOXGPSHOLD, BOXPASSTHRU, - BOXHEADFREE, BOXBEEPERON, BOXLEDMAX, + BOXLEDLOW, BOXLLIGHTS, - BOXHEADADJ, + BOXCALIB, + BOXGOV, + BOXOSD, CHECKBOXITEMS }; +#define ROL_LO (1 << (2 * ROLL)) +#define ROL_CE (3 << (2 * ROLL)) +#define ROL_HI (2 << (2 * ROLL)) +#define PIT_LO (1 << (2 * PITCH)) +#define PIT_CE (3 << (2 * PITCH)) +#define PIT_HI (2 << (2 * PITCH)) +#define YAW_LO (1 << (2 * YAW)) +#define YAW_CE (3 << (2 * YAW)) +#define YAW_HI (2 << (2 * YAW)) +#define THR_LO (1 << (2 * THROTTLE)) +#define THR_CE (3 << (2 * THROTTLE)) +#define THR_HI (2 << (2 * THROTTLE)) + #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))) +#define SERVO_NORMAL (1) +#define SERVO_REVERSE (-1) + +// Custom mixer data per motor typedef struct motorMixer_t { float throttle; float roll; @@ -107,12 +129,20 @@ typedef struct motorMixer_t { float yaw; } motorMixer_t; +// Custom mixer configuration typedef struct mixer_t { uint8_t numberMotor; uint8_t useServo; const motorMixer_t *motor; } mixer_t; +typedef struct servoParam_t { + int8_t direction; // servo direction + uint16_t middle; // servo middle + uint16_t min; // servo min + uint16_t max; // servo max +} servoParam_t; + enum { ALIGN_GYRO = 0, ALIGN_ACCEL = 1, @@ -120,14 +150,6 @@ enum { }; typedef struct config_t { - uint8_t version; - uint16_t size; - uint8_t magic_be; // magic number, should be 0xBE - uint8_t mixerConfiguration; - uint32_t enabledFeatures; - - uint16_t looptime; // imu loop time in us - uint8_t P8[PIDITEMS]; uint8_t I8[PIDITEMS]; uint8_t D8[PIDITEMS]; @@ -141,57 +163,30 @@ typedef struct config_t { uint8_t yawRate; uint8_t dynThrPID; - int16_t accZero[3]; - int16_t magZero[3]; int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ int16_t angleTrim[2]; // accelerometer trim // sensor-related stuff - int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y) - uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device uint8_t acc_lpf_factor; // Set the Low Pass Filter factor for ACC. Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter uint8_t acc_lpf_for_velocity; // ACC lowpass for AccZ height hold uint8_t accz_deadband; // ?? - uint16_t gyro_lpf; // mpuX050 LPF setting (TODO make it work on L3GD as well) - uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter. - 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 mpu6050_scale; // seems es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. fucking invenshit won't release chip IDs so I can't autodetect it. uint8_t baro_tab_size; // size of baro filter array float baro_noise_lpf; // additional LPF to reduce baro noise float baro_cf; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) - uint8_t moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. 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) - uint8_t power_adc_channel; // which channel is used for current sensor. Right now, only 2 places are supported: RC_CH2 (unused when in CPPM mode, = 1), RC_CH8 (last channel in PWM mode, = 9) // Radio/ESC-related configuration - uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero. uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. - uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-20 - uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit) - uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here - uint16_t mincheck; // minimum rc end - uint16_t maxcheck; // maximum rc end - uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right + uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 + uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement // Failsafe related configuration uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10) uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200) uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500. - // motor/esc/servo related stuff - 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 - uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) - uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) - int16_t servotrim[8]; // Adjust Servo MID Offset & Swash angles - int8_t servoreverse[8]; // Invert servos by setting -1 - // mixer-related configuration int8_t yaw_direction; uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim @@ -205,6 +200,7 @@ typedef struct config_t { uint16_t wing_right_min; uint16_t wing_right_mid; uint16_t wing_right_max; + int8_t pitch_direction_l; // left servo - pitch orientation int8_t pitch_direction_r; // right servo - pitch orientation (opposite sign to pitch_direction_l if servos are mounted mirrored) int8_t roll_direction_l; // left servo - roll orientation @@ -222,22 +218,71 @@ typedef struct config_t { uint16_t gimbal_roll_mid; // gimbal roll servo neutral value // gps-related stuff - uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ?? - uint32_t gps_baudrate; // GPS baudrate uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint8_t gps_lpf; // Low pass filter cut frequency for derivative calculation (default 20Hz) uint8_t nav_slew_rate; // Adds a rate control to nav output, will smoothen out nav angle spikes uint8_t nav_controls_heading; // copter faces toward the navigation point, maghold must be enabled for it uint16_t nav_speed_min; // cm/sec uint16_t nav_speed_max; // cm/sec + uint16_t ap_mode; // Temporarily Disables GPS_HOLD_MODE to be able to make it possible to adjust the Hold-position when moving the sticks, creating a deadspan for GPS +} config_t; + +// System-wide +typedef struct master_t { + uint8_t version; + uint16_t size; + uint8_t magic_be; // magic number, should be 0xBE + + uint8_t mixerConfiguration; + uint32_t enabledFeatures; + uint16_t looptime; // imu loop time in us + motorMixer_t customMixer[MAX_MOTORS]; // custom mixtable + + // motor/esc/servo related stuff + 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 + uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) + uint16_t servo_pwm_rate; // The update rate of servo outputs (50-498Hz) + + // global sensor-related stuff + int8_t align[3][3]; // acc, gyro, mag alignment (ex: with sensor output of X, Y, Z, align of 1 -3 2 would return X, -Z, Y) + uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device + uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. + uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter. + 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 moron_threshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. + uint8_t mpu6050_scale; // es/non-es variance between MPU6050 sensors, half my boards are mpu6000ES, need this to be dynamic. automatically set by mpu6050 driver. + int16_t accZero[3]; + int16_t magZero[3]; + + // Battery/ADC stuff + 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) + uint8_t power_adc_channel; // which channel is used for current sensor. Right now, only 2 places are supported: RC_CH2 (unused when in CPPM mode, = 1), RC_CH8 (last channel in PWM mode, = 9) + + // Radio/ESC-related configuration + uint8_t rcmap[8]; // mapping of radio channels to internal RPYTA+ order + uint8_t spektrum_hires; // spektrum high-resolution y/n (1024/2048bit) + uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here + uint16_t mincheck; // minimum rc end + uint16_t maxcheck; // maximum rc end + uint8_t retarded_arm; // allow disarsm/arm on throttle down + roll left/right + + // gps-related stuff + uint8_t gps_type; // Type of GPS hardware. 0: NMEA 1: UBX 2+ ?? + uint32_t gps_baudrate; // GPS baudrate // serial(uart1) baudrate uint32_t serial_baudrate; - motorMixer_t customMixer[MAX_MOTORS]; // custom mixtable + config_t profile[3]; // 3 separate profiles + uint8_t current_profile; // currently loaded profile + uint8_t magic_ef; // magic number, should be 0xEF uint8_t chk; // XOR checksum -} config_t; +} master_t; typedef struct flags_t { uint8_t OK_TO_ARM; @@ -255,6 +300,7 @@ typedef struct flags_t { uint8_t GPS_FIX_HOME; uint8_t SMALL_ANGLES_25; uint8_t CALIBRATE_MAG; + uint8_t VARIO_MODE; } flags_t; extern int16_t gyroZero[3]; @@ -272,6 +318,7 @@ extern uint32_t currentTime; extern uint32_t previousTime; extern uint16_t cycleTime; extern uint16_t calibratingA; +extern uint16_t calibratingB; extern uint16_t calibratingG; extern int16_t heading; extern int16_t annex650_overrun_count; @@ -281,12 +328,14 @@ extern int32_t EstAlt; extern int32_t AltHold; extern int16_t errorAltitudeI; extern int16_t BaroPID; +extern int16_t vario; extern int16_t headFreeModeHold; extern int16_t zVelocity; extern int16_t heading, magHold; extern int16_t motor[MAX_MOTORS]; extern int16_t servo[8]; extern int16_t rcData[8]; +extern uint16_t rssi; // range: [0;1023] extern uint8_t vbat; extern int16_t telemTemperature1; // gyro sensor temperature extern int16_t lookupPitchRollRC[6]; // lookup table for expo & RC rate PITCH+ROLL @@ -310,6 +359,7 @@ extern int16_t nav[2]; extern int8_t nav_mode; // Navigation mode extern int16_t nav_rated[2]; // Adding a rate controller to the navigation to make it smoother +extern master_t mcfg; extern config_t cfg; extern flags_t f; extern sensor_t acc; @@ -324,17 +374,17 @@ void imuInit(void); void annexCode(void); void computeIMU(void); void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat); -void getEstimatedAltitude(void); +int getEstimatedAltitude(void); // Sensors void sensorsAutodetect(void); void batteryInit(void); uint16_t batteryAdcToVoltage(uint16_t src); void ACC_getADC(void); -void Baro_update(void); +int Baro_update(void); void Gyro_getADC(void); void Mag_init(void); -void Mag_getADC(void); +int Mag_getADC(void); void Sonar_init(void); void Sonar_update(void); @@ -353,7 +403,7 @@ void serialCom(void); // Config void parseRcChannels(const char *input); void readEEPROM(void); -void writeParams(uint8_t b); +void writeEEPROM(uint8_t b, uint8_t updateProfile); void checkFirstTime(bool reset); bool sensors(uint32_t mask); void sensorsSet(uint32_t mask); diff --git a/src/sensors.c b/src/sensors.c index d636bc59a..82eb2cf80 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -1,9 +1,10 @@ #include "board.h" #include "mw.h" -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 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 calibratingB = 0; // baro calibration = get new ground pressure value uint16_t calibratingG = 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; @@ -33,52 +34,52 @@ void sensorsAutodetect(void) int16_t deg, min; drv_adxl345_config_t acc_params; bool haveMpu6k = false; - bool havel3g4200d = false; // Autodetect gyro hardware. We have MPU3050 or MPU6050. - if (mpu6050Detect(&acc, &gyro, &cfg.mpu6050_scale)) { + if (mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale)) { // this filled up acc.* struct with init values haveMpu6k = true; - } else if (l3g4200dDetect(&gyro)) { - havel3g4200d = true; - } else if (!mpu3050Detect(&gyro)) { + } else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) { + // well, we found our gyro + ; + } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) { // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. failureMode(3); } // Accelerometer. Fuck it. Let user break shit. retry: - switch (cfg.acc_hardware) { + switch (mcfg.acc_hardware) { case 0: // autodetect case 1: // ADXL345 acc_params.useFifo = false; acc_params.dataRate = 800; // unused currently if (adxl345Detect(&acc_params, &acc)) accHardware = ACC_ADXL345; - if (cfg.acc_hardware == ACC_ADXL345) + if (mcfg.acc_hardware == ACC_ADXL345) break; ; // fallthrough case 2: // MPU6050 if (haveMpu6k) { - mpu6050Detect(&acc, &gyro, &cfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct + mpu6050Detect(&acc, &gyro, mcfg.gyro_lpf, &mcfg.mpu6050_scale); // yes, i'm rerunning it again. re-fill acc struct accHardware = ACC_MPU6050; - if (cfg.acc_hardware == ACC_MPU6050) + if (mcfg.acc_hardware == ACC_MPU6050) break; } ; // fallthrough case 3: // MMA8452 if (mma8452Detect(&acc)) { accHardware = ACC_MMA8452; - if (cfg.acc_hardware == ACC_MMA8452) + if (mcfg.acc_hardware == ACC_MMA8452) break; } } // Found anything? Check if user fucked up or ACC is really missing. if (accHardware == ACC_DEFAULT) { - if (cfg.acc_hardware > ACC_DEFAULT) { + if (mcfg.acc_hardware > ACC_DEFAULT) { // Nothing was found and we have a forced sensor type. Stupid user probably chose a sensor that isn't present. - cfg.acc_hardware = ACC_DEFAULT; + mcfg.acc_hardware = ACC_DEFAULT; goto retry; } else { // We're really screwed @@ -103,14 +104,6 @@ retry: // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here. gyro.init(); - // todo: this is driver specific :( - if (havel3g4200d) { - l3g4200dConfig(cfg.gyro_lpf); - } else { - if (!haveMpu6k) - mpu3050Config(cfg.gyro_lpf); - } - #ifdef MAG if (!hmc5883lDetect()) sensorsClear(SENSOR_MAG); @@ -127,7 +120,7 @@ uint16_t batteryAdcToVoltage(uint16_t src) { // calculate battery voltage based on ADC reading // result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 4095 = 12bit adc, 110 = 11:1 voltage divider (10k:1k) * 10 for 0.1V - return (((src) * 3.3f) / 4095) * cfg.vbatscale; + return (((src) * 3.3f) / 4095) * mcfg.vbatscale; } void batteryInit(void) @@ -145,11 +138,11 @@ void batteryInit(void) // autodetect cell count, going from 2S..6S for (i = 2; i < 6; i++) { - if (voltage < i * cfg.vbatmaxcellvoltage) + if (voltage < i * mcfg.vbatmaxcellvoltage) break; } batteryCellCount = i; - batteryWarningVoltage = i * cfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI + batteryWarningVoltage = i * mcfg.vbatmincellvoltage; // 3.3V per cell minimum, configurable in CLI } // ALIGN_GYRO = 0, @@ -166,7 +159,7 @@ static void alignSensors(uint8_t type, int16_t *data) tmp[2] = data[2]; for (i = 0; i < 3; i++) { - int8_t axis = cfg.align[type][i]; + int8_t axis = mcfg.align[type][i]; if (axis > 0) data[axis - 1] = tmp[i]; else @@ -188,16 +181,16 @@ static void ACC_Common(void) a[axis] += accADC[axis]; // Clear global variables for next reading accADC[axis] = 0; - cfg.accZero[axis] = 0; + mcfg.accZero[axis] = 0; } // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (calibratingA == 1) { - cfg.accZero[ROLL] = a[ROLL] / 400; - cfg.accZero[PITCH] = a[PITCH] / 400; - cfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G + mcfg.accZero[ROLL] = a[ROLL] / 400; + mcfg.accZero[PITCH] = a[PITCH] / 400; + mcfg.accZero[YAW] = a[YAW] / 400 - acc_1G; // for nunchuk 200=1G cfg.angleTrim[ROLL] = 0; cfg.angleTrim[PITCH] = 0; - writeParams(1); // write accZero in EEPROM + writeEEPROM(1, true); // write accZero in EEPROM } calibratingA--; } @@ -208,9 +201,9 @@ static void ACC_Common(void) static int16_t angleTrim_saved[2] = { 0, 0 }; // Saving old zeropoints before measurement if (InflightcalibratingA == 50) { - accZero_saved[ROLL] = cfg.accZero[ROLL]; - accZero_saved[PITCH] = cfg.accZero[PITCH]; - accZero_saved[YAW] = cfg.accZero[YAW]; + accZero_saved[ROLL] = mcfg.accZero[ROLL]; + accZero_saved[PITCH] = mcfg.accZero[PITCH]; + accZero_saved[YAW] = mcfg.accZero[YAW]; angleTrim_saved[ROLL] = cfg.angleTrim[ROLL]; angleTrim_saved[PITCH] = cfg.angleTrim[PITCH]; } @@ -223,7 +216,7 @@ static void ACC_Common(void) b[axis] += accADC[axis]; // Clear global variables for next reading accADC[axis] = 0; - cfg.accZero[axis] = 0; + mcfg.accZero[axis] = 0; } // all values are measured if (InflightcalibratingA == 1) { @@ -231,9 +224,9 @@ static void ACC_Common(void) AccInflightCalibrationMeasurementDone = 1; 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]; - cfg.accZero[YAW] = accZero_saved[YAW]; + mcfg.accZero[ROLL] = accZero_saved[ROLL]; + mcfg.accZero[PITCH] = accZero_saved[PITCH]; + mcfg.accZero[YAW] = accZero_saved[YAW]; cfg.angleTrim[ROLL] = angleTrim_saved[ROLL]; cfg.angleTrim[PITCH] = angleTrim_saved[PITCH]; } @@ -242,25 +235,25 @@ static void ACC_Common(void) // Calculate average, shift Z down by acc_1G and store values in EEPROM at end of calibration if (AccInflightCalibrationSavetoEEProm == 1) { // the copter is landed, disarmed and the combo has been done again AccInflightCalibrationSavetoEEProm = 0; - cfg.accZero[ROLL] = b[ROLL] / 50; - cfg.accZero[PITCH] = b[PITCH] / 50; - cfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G + mcfg.accZero[ROLL] = b[ROLL] / 50; + mcfg.accZero[PITCH] = b[PITCH] / 50; + mcfg.accZero[YAW] = b[YAW] / 50 - acc_1G; // for nunchuk 200=1G cfg.angleTrim[ROLL] = 0; cfg.angleTrim[PITCH] = 0; - writeParams(1); // write accZero in EEPROM + writeEEPROM(1, true); // write accZero in EEPROM } } - accADC[ROLL] -= cfg.accZero[ROLL]; - accADC[PITCH] -= cfg.accZero[PITCH]; - accADC[YAW] -= cfg.accZero[YAW]; + accADC[ROLL] -= mcfg.accZero[ROLL]; + accADC[PITCH] -= mcfg.accZero[PITCH]; + accADC[YAW] -= mcfg.accZero[YAW]; } void ACC_getADC(void) { acc.read(accADC); // if we have CUSTOM alignment configured, user is "assumed" to know what they're doing - if (cfg.align[ALIGN_ACCEL][0]) + if (mcfg.align[ALIGN_ACCEL][0]) alignSensors(ALIGN_ACCEL, accADC); else acc.align(accADC); @@ -269,14 +262,14 @@ void ACC_getADC(void) } #ifdef BARO -void Baro_update(void) +int Baro_update(void) { static uint32_t baroDeadline = 0; static uint8_t state = 0; int32_t pressure; if ((int32_t)(currentTime - baroDeadline) < 0) - return; + return 0; baroDeadline = currentTime; @@ -303,6 +296,8 @@ void Baro_update(void) baroDeadline += baro.repeat_delay; break; } + + return 1; } #endif /* BARO */ @@ -364,7 +359,7 @@ static void GYRO_Common(void) if (calibratingG == 1) { float dev = devStandardDeviation(&var[axis]); // check deviation and startover if idiot was moving the model - if (cfg.moron_threshold && dev > cfg.moron_threshold) { + if (mcfg.moron_threshold && dev > mcfg.moron_threshold) { calibratingG = 1000; devClear(&var[0]); devClear(&var[1]); @@ -391,7 +386,7 @@ void Gyro_getADC(void) // range: +/- 8192; +/- 2000 deg/sec gyro.read(gyroADC); // if we have CUSTOM alignment configured, user is "assumed" to know what they're doing - if (cfg.align[ALIGN_GYRO][0]) + if (mcfg.align[ALIGN_GYRO][0]) alignSensors(ALIGN_GYRO, gyroADC); else gyro.align(gyroADC); @@ -491,15 +486,15 @@ void Mag_init(void) magInit = 1; } -void Mag_getADC(void) +int Mag_getADC(void) { static uint32_t t, tCal = 0; static int16_t magZeroTempMin[3]; static int16_t magZeroTempMax[3]; uint32_t axis; - + if ((int32_t)(currentTime - t) < 0) - return; //each read is spaced by 100ms + return 0; //each read is spaced by 100ms t = currentTime + 100000; // Read mag sensor @@ -512,7 +507,7 @@ void Mag_getADC(void) if (f.CALIBRATE_MAG) { tCal = t; for (axis = 0; axis < 3; axis++) { - cfg.magZero[axis] = 0; + mcfg.magZero[axis] = 0; magZeroTempMin[axis] = magADC[axis]; magZeroTempMax[axis] = magADC[axis]; } @@ -520,9 +515,9 @@ void Mag_getADC(void) } if (magInit) { // we apply offset only once mag calibration is done - magADC[ROLL] -= cfg.magZero[ROLL]; - magADC[PITCH] -= cfg.magZero[PITCH]; - magADC[YAW] -= cfg.magZero[YAW]; + magADC[ROLL] -= mcfg.magZero[ROLL]; + magADC[PITCH] -= mcfg.magZero[PITCH]; + magADC[YAW] -= mcfg.magZero[YAW]; } if (tCal != 0) { @@ -537,10 +532,12 @@ void Mag_getADC(void) } else { tCal = 0; for (axis = 0; axis < 3; axis++) - cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets - writeParams(1); + mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets + writeEEPROM(1, true); } } + + return 1; } #endif diff --git a/src/serial.c b/src/serial.c index 96eb798e9..db4ac12d6 100755 --- a/src/serial.c +++ b/src/serial.c @@ -3,68 +3,133 @@ // Multiwii Serial Protocol 0 #define MSP_VERSION 0 -#define PLATFORM_32BIT 0x80000000 +#define PLATFORM_32BIT ((uint32_t)1 << 31) -#define MSP_IDENT 100 //out message multitype + version -#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation +#define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable +#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number #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_RC 105 //out message 8 rc chan and more +#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course #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_ALTITUDE 109 //out message altitude, variometer +#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX #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_PID 112 //out message P I D coeff (9 are used currently) +#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup) +#define MSP_MISC 114 //out message powermeter trig #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI #define MSP_BOXNAMES 116 //out message the aux switch names #define MSP_PIDNAMES 117 //out message the PID names #define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold +#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes #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_PID 202 //in message P I D coeff (9 are used currently) +#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup) #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_WP_SET 209 //in message sets a given WP (WP#,lat, lon, alt, flags) +#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags) +#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) +#define MSP_SET_HEAD 211 //in message define a new heading hold direction + +// #define MSP_BIND 240 //in message no param #define MSP_EEPROM_WRITE 250 //in message no param #define MSP_DEBUGMSG 253 //out message debug string buffer #define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4 -#define MSP_ACC_TRIM 240 //out message get acc angle trim values -#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values - // Additional commands that are not compatible with MultiWii #define MSP_UID 160 //out message Unique device ID +#define MSP_ACC_TRIM 240 //out message get acc angle trim values +#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define INBUF_SIZE 64 +struct box_t { + const uint8_t boxIndex; // this is from boxnames enum + const char *boxName; // GUI-readable box name + const uint8_t permanentId; // +} boxes[] = { + { BOXARM, "ARM;", 0 }, + { BOXANGLE, "ANGLE;", 1 }, + { BOXHORIZON, "HORIZON;", 2 }, + { BOXBARO, "BARO;", 3 }, + { BOXVARIO, "VARIO;", 4 }, + { BOXMAG, "MAG;", 5 }, + { BOXHEADFREE, "HEADFREE;", 6 }, + { BOXHEADADJ, "HEADADJ;", 7 }, + { BOXCAMSTAB, "CAMSTAB;", 8 }, + { BOXCAMTRIG, "CAMTRIG;", 9 }, + { BOXGPSHOME, "GPS HOME;", 10 }, + { BOXGPSHOLD, "GPS HOLD;", 11 }, + { BOXPASSTHRU, "PASSTHRU;", 12 }, + { BOXBEEPERON, "BEEPER;", 13 }, + { BOXLEDMAX, "LEDMAX;", 14 }, + { BOXLEDLOW, "LEDLOW;", 15 }, + { BOXLLIGHTS, "LLIGHTS;", 16 }, + { BOXCALIB, "CALIB;", 17 }, + { BOXGOV, "GOVERNOR;", 18 }, + { BOXOSD, "OSD SW;", 19 }, + { CHECKBOXITEMS, NULL, 0xFF } +}; + +// this is calculated at startup based on enabled features. +static uint8_t availableBoxes[CHECKBOXITEMS]; +// this is the number of filled indexes in above array +static uint8_t numberBoxItems = 0; + static const char boxnames[] = + "ARM;" "ANGLE;" "HORIZON;" "BARO;" + "VARIO;" "MAG;" + "HEADFREE;" + "HEADADJ;" "CAMSTAB;" "CAMTRIG;" - "ARM;" "GPS HOME;" "GPS HOLD;" "PASSTHRU;" - "HEADFREE;" "BEEPER;" "LEDMAX;" + "LEDLOW;" "LLIGHTS;" - "HEADADJ;"; + "CALIB;" + "GOVERNOR;" + "OSD SW;"; + +const uint8_t boxids[] = { // permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function. + 0, // "ARM;" + 1, // "ANGLE;" + 2, // "HORIZON;" + 3, // "BARO;" + 4, // "VARIO;" + 5, // "MAG;" + 6, // "HEADFREE;" + 7, // "HEADADJ;" + 8, // "CAMSTAB;" + 9, // "CAMTRIG;" + 10, // "GPS HOME;" + 11, // "GPS HOLD;" + 12, // "PASSTHRU;" + 13, // "BEEPER;" + 14, // "LEDMAX;" + 15, // "LEDLOW;" + 16, // "LLIGHTS;" + 17, // "CALIB;" + 18, // "GOVERNOR;" + 19, // "OSD_SWITCH;" +}; static const char pidnames[] = "ROLL;" @@ -169,15 +234,68 @@ void serializeNames(const char *s) serialize8(*c); } +void serializeBoxNamesReply(void) +{ + char buf[256]; // no fucking idea + char *c; + int i, j; + + memset(buf, 0, sizeof(buf)); + for (i = 0; i < CHECKBOXITEMS; i++) { + for (j = 0; j < numberBoxItems; j++) { + if (boxes[i].boxIndex == availableBoxes[j]) + strcat(buf, boxes[i].boxName); + } + } + + headSerialReply(strlen(buf)); + for (c = buf; *c; c++) + serialize8(*c); +} + void serialInit(uint32_t baudrate) { + int idx; + uartInit(baudrate); + // calculate used boxes based on features and fill availableBoxes[] array + memset(availableBoxes, 0xFF, sizeof(availableBoxes)); + + idx = 0; + availableBoxes[idx++] = BOXARM; + if (sensors(SENSOR_ACC)) { + availableBoxes[idx++] = BOXANGLE; + availableBoxes[idx++] = BOXHORIZON; + } + if (sensors(SENSOR_BARO)) { + availableBoxes[idx++] = BOXBARO; + if (feature(FEATURE_VARIO)) + availableBoxes[idx++] = BOXVARIO; + } + if (sensors(SENSOR_MAG)) { + availableBoxes[idx++] = BOXMAG; + availableBoxes[idx++] = BOXHEADFREE; + availableBoxes[idx++] = BOXHEADADJ; + } + if (feature(FEATURE_SERVO_TILT)) + availableBoxes[idx++] = BOXCAMSTAB; + if (feature(FEATURE_GPS) && sensors(SENSOR_GPS)) { + availableBoxes[idx++] = BOXGPSHOME; + availableBoxes[idx++] = BOXGPSHOLD; + } + if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) + availableBoxes[idx++] = BOXPASSTHRU; + availableBoxes[idx++] = BOXBEEPERON; + if (feature(FEATURE_INFLIGHT_ACC_CAL)) + availableBoxes[idx++] = BOXCALIB; + numberBoxItems = idx; } static void evaluateCommand(void) { uint32_t i; uint8_t wp_no; + int32_t lat = 0, lon = 0, alt = 0; switch (cmdMSP) { case MSP_SET_RAW_RC: @@ -209,8 +327,8 @@ static void evaluateCommand(void) headSerialReply(0); break; case MSP_SET_BOX: - for (i = 0; i < CHECKBOXITEMS; i++) - cfg.activate[i] = read16(); + for (i = 0; i < numberBoxItems; i++) + cfg.activate[availableBoxes[i]] = read16(); headSerialReply(0); break; case MSP_SET_RC_TUNING: @@ -226,22 +344,46 @@ static void evaluateCommand(void) case MSP_SET_MISC: headSerialReply(0); break; + case MSP_SELECT_SETTING: + if (!f.ARMED) { + mcfg.current_profile = read8(); + if (mcfg.current_profile > 2) + mcfg.current_profile = 0; + // this writes new profile index and re-reads it + writeEEPROM(0, false); + } + headSerialReply(0); + break; + case MSP_SET_HEAD: + magHold = read16(); + headSerialReply(0); + break; case MSP_IDENT: headSerialReply(7); serialize8(VERSION); // multiwii version - serialize8(cfg.mixerConfiguration); // type of multicopter + serialize8(mcfg.mixerConfiguration); // type of multicopter serialize8(MSP_VERSION); // MultiWii Serial Protocol Version serialize32(PLATFORM_32BIT); // "capability" break; case MSP_STATUS: - headSerialReply(10); + headSerialReply(11); serialize16(cycleTime); serialize16(i2cGetErrorCounter()); serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4); - serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM | - rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG | - f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU | - rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ); + serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON | + f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ | + rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG | + f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | + f.PASSTHRU_MODE << BOXPASSTHRU | + rcOptions[BOXBEEPERON] << BOXBEEPERON | + rcOptions[BOXLEDMAX] << BOXLEDMAX | + rcOptions[BOXLLIGHTS] << BOXLLIGHTS | + rcOptions[BOXVARIO] << BOXVARIO | + rcOptions[BOXCALIB] << BOXCALIB | + rcOptions[BOXGOV] << BOXGOV | + rcOptions[BOXOSD] << BOXOSD | + f.ARMED << BOXARM); + serialize8(mcfg.current_profile); break; case MSP_RAW_IMU: headSerialReply(18); @@ -268,13 +410,14 @@ static void evaluateCommand(void) serialize16(rcData[i]); break; case MSP_RAW_GPS: - headSerialReply(14); + headSerialReply(16); serialize8(f.GPS_FIX); serialize8(GPS_numSat); serialize32(GPS_coord[LAT]); serialize32(GPS_coord[LON]); serialize16(GPS_altitude); serialize16(GPS_speed); + serialize16(GPS_ground_course); break; case MSP_COMP_GPS: headSerialReply(5); @@ -290,13 +433,15 @@ static void evaluateCommand(void) serialize16(headFreeModeHold); break; case MSP_ALTITUDE: - headSerialReply(4); + headSerialReply(6); serialize32(EstAlt); + serialize16(vario); break; - case MSP_BAT: - headSerialReply(3); + case MSP_ANALOG: + headSerialReply(5); serialize8(vbat); serialize16(0); // power meter trash + serialize16(rssi); break; case MSP_RC_TUNING: headSerialReply(7); @@ -316,18 +461,23 @@ static void evaluateCommand(void) serialize8(cfg.D8[i]); } break; + case MSP_PIDNAMES: + headSerialReply(sizeof(pidnames) - 1); + serializeNames(pidnames); + break; case MSP_BOX: - headSerialReply(2 * CHECKBOXITEMS); - for (i = 0; i < CHECKBOXITEMS; i++) - serialize16(cfg.activate[i]); + headSerialReply(2 * numberBoxItems); + for (i = 0; i < numberBoxItems; i++) + serialize16(cfg.activate[availableBoxes[i]]); break; case MSP_BOXNAMES: - headSerialReply(sizeof(boxnames) - 1); - serializeNames(boxnames); + // headSerialReply(sizeof(boxnames) - 1); + serializeBoxNamesReply(); break; - case MSP_PIDNAMES: - headSerialReply(sizeof(pidnames) - 1); - serializeNames(pidnames); + case MSP_BOXIDS: + headSerialReply(numberBoxItems); + for (i = 0; i < numberBoxItems; i++) + serialize8(availableBoxes[i]); break; case MSP_MISC: headSerialReply(2); @@ -340,54 +490,85 @@ static void evaluateCommand(void) break; case MSP_WP: wp_no = read8(); // get the wp number - headSerialReply(12); + headSerialReply(18); if (wp_no == 0) { - serialize8(0); // wp0 - serialize32(GPS_home[LAT]); - serialize32(GPS_home[LON]); - serialize16(0); // altitude will come here - serialize8(0); // nav flag will come here + lat = GPS_home[LAT]; + lon = GPS_home[LON]; } else if (wp_no == 16) { - serialize8(16); // wp16 - serialize32(GPS_hold[LAT]); - serialize32(GPS_hold[LON]); - serialize16(0); // altitude will come here - serialize8(0); // nav flag will come here + lat = GPS_hold[LAT]; + lon = GPS_hold[LON]; } + serialize8(wp_no); + serialize32(lat); + serialize32(lon); + serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps + serialize16(0); // heading will come here (deg) + serialize16(0); // time to stay (ms) will come here + serialize8(0); // nav flag will come here + break; + case MSP_SET_WP: + wp_no = read8(); //get the wp number + lat = read32(); + lon = read32(); + alt = read32(); // to set altitude (cm) + read16(); // future: to set heading (deg) + read16(); // future: to set time to stay (ms) + read8(); // future: to set nav flag + if (wp_no == 0) { + GPS_home[LAT] = lat; + GPS_home[LON] = lon; + f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS + f.GPS_FIX_HOME = 1; + if (alt != 0) + AltHold = alt; // temporary implementation to test feature with apps + } else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS + GPS_hold[LAT] = lat; + GPS_hold[LON] = lon; + if (alt != 0) + AltHold = alt; // temporary implementation to test feature with apps + nav_mode = NAV_MODE_WP; + GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); + } + headSerialReply(0); break; case MSP_RESET_CONF: - checkFirstTime(true); + if (!f.ARMED) + checkFirstTime(true); headSerialReply(0); break; case MSP_ACC_CALIBRATION: - calibratingA = 400; + if (!f.ARMED) + calibratingA = 400; headSerialReply(0); break; case MSP_MAG_CALIBRATION: - f.CALIBRATE_MAG = 1; + if (!f.ARMED) + f.CALIBRATE_MAG = 1; headSerialReply(0); break; case MSP_EEPROM_WRITE: - writeParams(0); + writeEEPROM(0, true); headSerialReply(0); break; - case MSP_ACC_TRIM: - headSerialReply(4); - serialize16(cfg.angleTrim[PITCH]); - serialize16(cfg.angleTrim[ROLL]); - break; case MSP_DEBUG: headSerialReply(8); for (i = 0; i < 4; i++) serialize16(debug[i]); // 4 variables are here for general monitoring purpose break; - // Additional commands that are not compatible with MultiWii + + // Additional commands that are not compatible with MultiWii + case MSP_ACC_TRIM: + headSerialReply(4); + serialize16(cfg.angleTrim[PITCH]); + serialize16(cfg.angleTrim[ROLL]); + break; case MSP_UID: headSerialReply(12); serialize32(U_ID_0); serialize32(U_ID_1); serialize32(U_ID_2); break; + default: // we do not know how to handle the (valid) message, indicate error MSP $M! headSerialError(0); break; diff --git a/src/spektrum.c b/src/spektrum.c index cdb6ee10d..215cccab9 100644 --- a/src/spektrum.c +++ b/src/spektrum.c @@ -17,7 +17,7 @@ extern int16_t failsafeCnt; void spektrumInit(void) { - if (cfg.spektrum_hires) { + if (mcfg.spektrum_hires) { // 11 bit frames spek_chan_shift = 3; spek_chan_mask = 0x07; @@ -75,12 +75,12 @@ uint16_t spektrumReadRawRC(uint8_t chan) } if (chan >= SPEK_MAX_CHANNEL || !spekDataIncoming) { - data = cfg.midrc; + data = mcfg.midrc; } else { - if (cfg.spektrum_hires) - data = 988 + (spekChannelData[cfg.rcmap[chan]] >> 1); // 2048 mode + if (mcfg.spektrum_hires) + data = 988 + (spekChannelData[mcfg.rcmap[chan]] >> 1); // 2048 mode else - data = 988 + spekChannelData[cfg.rcmap[chan]]; // 1024 mode + data = 988 + spekChannelData[mcfg.rcmap[chan]]; // 1024 mode } return data; diff --git a/src/telemetry.c b/src/telemetry.c index b5d815a5d..31be61c95 100644 --- a/src/telemetry.c +++ b/src/telemetry.c @@ -209,7 +209,7 @@ void initTelemetry(bool State) if (State) serialInit(9600); else - serialInit(cfg.serial_baudrate); + serialInit(mcfg.serial_baudrate); telemetryEnabled = State; } }