From f8b4c5e5f72152efaec0e185ea2f54ae84e8185b Mon Sep 17 00:00:00 2001 From: Andreas Tacke Date: Thu, 26 Feb 2015 12:28:15 +0100 Subject: [PATCH] Made formatting consistent with baseflight coding style guidelines. --- src/board.h | 2 +- src/buzzer.c | 16 +- src/cli.c | 61 +-- src/drv_adc.c | 6 +- src/drv_ak8975.c | 20 +- src/drv_hmc5883l.c | 4 +- src/drv_i2c.c | 36 +- src/drv_i2c.h | 2 +- src/drv_i2c_soft.c | 4 +- src/drv_ledring.c | 4 +- src/drv_mpu6050.h | 2 +- src/drv_pwm.c | 4 +- src/drv_pwm.h | 6 +- src/drv_softserial.c | 2 +- src/drv_softserial.h | 4 +- src/drv_timer.c | 12 +- src/drv_uart.c | 6 +- src/gps.c | 201 ++++----- src/ibus.c | 4 +- src/imu.c | 8 +- src/main.c | 8 +- src/mixer.c | 23 +- src/mw.c | 80 ++-- src/mw.h | 12 +- src/printf.c | 57 +-- src/printf.h | 8 +- src/sensors.c | 45 +- src/serial.c | 970 +++++++++++++++++++++---------------------- src/spektrum.c | 2 +- src/telemetry_hott.c | 14 +- 30 files changed, 820 insertions(+), 803 deletions(-) diff --git a/src/board.h b/src/board.h index 2371215b..4ba715f0 100755 --- a/src/board.h +++ b/src/board.h @@ -234,7 +234,7 @@ typedef struct baro_t { // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB67 - // AfroFlight32 +// AfroFlight32 #include "drv_adc.h" #include "drv_adxl345.h" #include "drv_bma280.h" diff --git a/src/buzzer.c b/src/buzzer.c index aca57f0d..f5feef71 100644 --- a/src/buzzer.c +++ b/src/buzzer.c @@ -18,11 +18,11 @@ */ // fast beep: static const uint8_t buzz_shortBeep[] = { - 5,5, 0xFF + 5, 5, 0xFF }; // fast beep: static const uint8_t buzz_BatteryBeep[] = { - 10,5, 0xFF + 10, 5, 0xFF }; // medium beep static const uint8_t buzz_mediumBeepFast[] = { @@ -38,23 +38,23 @@ static const uint8_t buzz_longBeep[] = { }; // SOS morse code: static const uint8_t buzz_sos[] = { - 10,10, 10,10, 10,40, 40,10, 40,10, 40,40, 10,10, 10,10, 10,70, 0xFF + 10, 10, 10, 10, 10, 40, 40, 10, 40, 10, 40, 40, 10, 10, 10, 10, 10, 70, 0xFF }; // Arming when GPS is fixed static const uint8_t buzz_armed[] = { - 5,5, 15,5, 5,5, 15,30, 0xFF + 5, 5, 15, 5, 5, 5, 15, 30, 0xFF }; // Ready beeps. When gps has fix and copter is ready to fly. static const uint8_t buzz_readyBeep[] = { - 4,5, 4,5, 8,5, 15,5, 8,5, 4,5, 4,5, 0xFF + 4, 5, 4, 5, 8, 5, 15, 5, 8, 5, 4, 5, 4, 5, 0xFF }; // 2 fast short beeps static const uint8_t buzz_2shortBeeps[] = { - 5,5, 5,5, 0xFF + 5, 5, 5, 5, 0xFF }; // 3 fast short beeps static const uint8_t buzz_3shortBeeps[] = { - 5,5, 5,5, 5,5, 0xFF + 5, 5, 5, 5, 5, 5, 0xFF }; // Array used for beeps when reporting GPS satellite count (up to 10 satellites) static uint8_t buzz_countSats[22]; @@ -64,7 +64,7 @@ static uint8_t buzzerMode = BUZZER_STOPPED; // Buzzer off = 0 Buzzer on = 1 static uint8_t buzzerIsOn = 0; // Pointer to current sequence -static const uint8_t* buzzerPtr = NULL; +static const uint8_t *buzzerPtr = NULL; // Place in current sequence static uint16_t buzzerPos = 0; // Time when buzzer routine must act next time diff --git a/src/cli.c b/src/cli.c index 57733c35..2e545999 100644 --- a/src/cli.c +++ b/src/cli.c @@ -49,17 +49,17 @@ static float _atof(const char *p); static char *ftoa(float x, char *floatString); // sync this with MultiType enum from mw.h -static const char * const mixerNames[] = { +static const char *const mixerNames[] = { "TRI", "QUADP", "QUADX", "BI", "GIMBAL", "Y6", "HEX6", "FLYING_WING", "Y4", "HEX6X", "OCTOX8", "OCTOFLATP", "OCTOFLATX", - "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", + "AIRPLANE", "HELI_120_CCPM", "HELI_90_DEG", "VTAIL4", "HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER", "ATAIL4", "CUSTOM", "CUSTOMPLANE", NULL }; // sync this with AvailableFeatures enum from board.h -static const char * const featureNames[] = { +static const char *const featureNames[] = { "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", @@ -68,17 +68,17 @@ static const char * const featureNames[] = { }; // sync this with AvailableSensors enum from board.h -static const char * const sensorNames[] = { +static const char *const sensorNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL }; // sync this with AccelSensors enum from board.h -static const char * const accNames[] = { +static const char *const accNames[] = { "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "MPU6500", "None", NULL }; // sync this with HardwareRevision in board.h -static const char * const hwNames[] = { +static const char *const hwNames[] = { "", "Naze 32", "Naze32 rev.5", "Naze32 SP" }; @@ -404,8 +404,14 @@ static float _atof(const char *p) // Calculate scaling factor. // while (expon >= 50) { scale *= 1E50f; expon -= 50; } - while (expon >= 8) { scale *= 1E8f; expon -= 8; } - while (expon > 0) { scale *= 10.0f; expon -= 1; } + while (expon >= 8) { + scale *= 1E8f; + expon -= 8; + } + while (expon > 0) { + scale *= 10.0f; + expon -= 1; + } } // Return signed and scaled floating point result. @@ -645,7 +651,7 @@ static void cliServoMix(char *cmdline) printf("change the direction a servo reacts to a input channel: \r\nservo input -1|1\r\n"); printf("s"); for (channel = 0; channel < INPUT_ITEMS; channel++) - printf("\ti%d",channel + 1); + printf("\ti%d", channel + 1); printf("\r\n"); for (servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++) { @@ -662,12 +668,12 @@ static void cliServoMix(char *cmdline) args[check++] = atoi(ptr); ptr = strtok(NULL, " "); } - + if (ptr != NULL || check != ARGS_COUNT) { printf("Wrong number of arguments, needs servo input direction\r\n"); return; } - + if (args[SERVO] >= 1 && args[SERVO] <= MAX_SERVOS && args[INPUT] >= 1 && args[INPUT] <= INPUT_ITEMS && (args[DIRECTION] == -1 || args[DIRECTION] == 1)) { args[SERVO] -= 1; args[INPUT] -= 1; @@ -679,20 +685,19 @@ static void cliServoMix(char *cmdline) printf("ERR: Wrong range for arguments\r\n"); cliServoMix("direction"); - } - else { + } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, MIN, MAX, BOX, ARGS_COUNT}; ptr = strtok(cmdline, " "); while (ptr != NULL && check < ARGS_COUNT) { args[check++] = atoi(ptr); ptr = strtok(NULL, " "); } - + if (ptr != NULL || check != ARGS_COUNT) { printf("ERR: Wrong number of arguments, needs rule target_channel input_channel rate speed min max box\r\n"); return; } - + i = args[RULE] - 1; if (i >= 0 && i < MAX_SERVO_RULES && args[TARGET] > 0 && args[TARGET] <= MAX_SERVOS && @@ -702,14 +707,14 @@ static void cliServoMix(char *cmdline) args[MIN] >= 0 && args[MIN] <= 100 && args[MAX] >= 0 && args[MAX] <= 100 && args[MIN] < args[MAX] && args[BOX] >= 0 && args[BOX] <= MAX_SERVO_BOXES) { - mcfg.customServoMixer[i].targetChannel = args[TARGET] - 1; - mcfg.customServoMixer[i].fromChannel = args[INPUT] - 1; - mcfg.customServoMixer[i].rate = args[RATE]; - mcfg.customServoMixer[i].speed = args[SPEED]; - mcfg.customServoMixer[i].min = args[MIN]; - mcfg.customServoMixer[i].max = args[MAX]; - mcfg.customServoMixer[i].box = args[BOX]; - cliServoMix(""); + mcfg.customServoMixer[i].targetChannel = args[TARGET] - 1; + mcfg.customServoMixer[i].fromChannel = args[INPUT] - 1; + mcfg.customServoMixer[i].rate = args[RATE]; + mcfg.customServoMixer[i].speed = args[SPEED]; + mcfg.customServoMixer[i].min = args[MIN]; + mcfg.customServoMixer[i].max = args[MAX]; + mcfg.customServoMixer[i].box = args[BOX]; + cliServoMix(""); } else printf("ERR: Wrong range for arguments\r\n"); } @@ -768,7 +773,7 @@ static void cliDump(char *cmdline) } printf("cmix %d 0 0 0 0\r\n", i + 1); } - + // print custom servo mixer if exists if (mcfg.customServoMixer[0].rate != 0) { for (i = 0; i < MAX_SERVO_RULES; i++) { @@ -785,13 +790,13 @@ static void cliDump(char *cmdline) } printf("smix %d 0 0 0 0\r\n", i + 1); } - + // print servo directions for (i = 0; i < 8; i++) for (channel = 0; channel < INPUT_ITEMS; channel++) if (cfg.servoConf[i].direction & (1 << channel)) - printf("smix direction %d %d -1\r\n",i + 1 ,channel + 1); - + printf("smix direction %d %d -1\r\n", i + 1 , channel + 1); + // print enabled features mask = featureMask(); for (i = 0; ; i++) { // disable all feature first @@ -1198,7 +1203,7 @@ static void cliStatus(char *cmdline) uint32_t mask; printf("System Uptime: %d seconds, Voltage: %d * 0.1V (%dS battery)\r\n", - millis() / 1000, vbat, batteryCellCount); + millis() / 1000, vbat, batteryCellCount); mask = sensorsMask(); printf("Hardware: %s @ %dMHz, detected sensors: ", hwNames[hw_revision], (SystemCoreClock / 1000000)); diff --git a/src/drv_adc.c b/src/drv_adc.c index 84b5f300..c72350ee 100755 --- a/src/drv_adc.c +++ b/src/drv_adc.c @@ -3,7 +3,7 @@ // Driver for STM32F103CB onboard ADC // VBAT is connected to PA4 (ADC1_IN4) with 10k:1k divider // rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board -// Additional channel can be stolen from RC_CH2 (PA1, ADC1_IN1) or +// Additional channel can be stolen from RC_CH2 (PA1, ADC1_IN1) or // RC_CH8 (PB1, ADC1_IN9) by using set power_adc_channel=1|9 typedef struct adc_config_t { @@ -77,9 +77,9 @@ void adcInit(drv_adc_config_t *init) // Calibrate ADC ADC_ResetCalibration(ADC1); - while(ADC_GetResetCalibrationStatus(ADC1)); + while (ADC_GetResetCalibrationStatus(ADC1)); ADC_StartCalibration(ADC1); - while(ADC_GetCalibrationStatus(ADC1)); + while (ADC_GetCalibrationStatus(ADC1)); // Fire off ADC ADC_SoftwareStartConvCmd(ADC1, ENABLE); diff --git a/src/drv_ak8975.c b/src/drv_ak8975.c index 197412fd..af5c8fff 100644 --- a/src/drv_ak8975.c +++ b/src/drv_ak8975.c @@ -2,7 +2,7 @@ * This file is part of baseflight * Licensed under GPL V3 or modified DCL - see https://github.com/multiwii/baseflight/blob/master/README.md */ - + #include "board.h" // This sensor is available on MPU-9150. The accel/gyro in that chip use the same driver as MPU-6050. @@ -12,14 +12,14 @@ #define AK8975_MAG_ID_ADDRESS 0x00 #define AK8975_MAG_DATA_ADDRESS 0x03 #define AK8975_MAG_CONTROL_ADDRESS 0x0A - + static sensor_align_e magAlign = CW180_DEG; - -bool ak8975detect(sensor_t * mag) + +bool ak8975detect(sensor_t *mag) { bool ack = false; uint8_t sig = 0; - + // device ID is in register 0 and is equal to 'H' ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_ID_ADDRESS, 1, &sig); if (!ack || sig != 'H') @@ -27,7 +27,7 @@ bool ak8975detect(sensor_t * mag) mag->init = ak8975Init; mag->read = ak8975Read; - + return true; } @@ -35,7 +35,7 @@ void ak8975Init(sensor_align_e align) { if (align > 0) magAlign = align; - + i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading } @@ -43,7 +43,7 @@ void ak8975Read(int16_t *magData) { uint8_t buf[6]; int16_t mag[3]; - + i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_DATA_ADDRESS, 6, buf); // align sensors to match MPU6050: // x -> y @@ -52,8 +52,8 @@ void ak8975Read(int16_t *magData) mag[X] = -(int16_t)(buf[3] << 8 | buf[2]) * 4; mag[Y] = -(int16_t)(buf[1] << 8 | buf[0]) * 4; mag[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4; - + alignSensors(mag, magData, magAlign); - + i2cWrite(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_CONTROL_ADDRESS, 0x01); // start reading again } diff --git a/src/drv_hmc5883l.c b/src/drv_hmc5883l.c index 8eb17e0d..cb1a6da7 100755 --- a/src/drv_hmc5883l.c +++ b/src/drv_hmc5883l.c @@ -92,7 +92,7 @@ bool hmc5883lDetect(sensor_t *mag) mag->init = hmc5883lInit; mag->read = hmc5883lRead; - + return true; } @@ -106,7 +106,7 @@ void hmc5883lInit(sensor_align_e align) if (align > 0) magAlign = align; - + if (hw_revision == NAZE32) { // PB12 - MAG_DRDY output on rev4 hardware gpio.pin = Pin_12; diff --git a/src/drv_i2c.c b/src/drv_i2c.c index 3decd2cb..4bd76f33 100755 --- a/src/drv_i2c.c +++ b/src/drv_i2c.c @@ -68,8 +68,8 @@ static volatile uint8_t reg; static volatile uint8_t bytes; static volatile uint8_t writing; static volatile uint8_t reading; -static volatile uint8_t* write_p; -static volatile uint8_t* read_p; +static volatile uint8_t *write_p; +static volatile uint8_t *read_p; static bool i2cHandleHardwareFailure(void) { @@ -92,13 +92,15 @@ bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) bytes = len_; busy = 1; error = false; - + if (!I2Cx) return false; if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start - while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending + while (I2Cx->CR1 & 0x0200 && --timeout > 0) { + ; // wait for any stop to finish sending + } if (timeout == 0) return i2cHandleHardwareFailure(); I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job @@ -107,7 +109,9 @@ bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) } timeout = I2C_DEFAULT_TIMEOUT; - while (busy && --timeout > 0) { ; } + while (busy && --timeout > 0) { + ; + } if (timeout == 0) return i2cHandleHardwareFailure(); @@ -119,7 +123,7 @@ bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data) return i2cWriteBuffer(addr_, reg_, 1, &data); } -bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t *buf) { uint32_t timeout = I2C_DEFAULT_TIMEOUT; @@ -138,7 +142,9 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start - while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending + while (I2Cx->CR1 & 0x0200 && --timeout > 0) { + ; // wait for any stop to finish sending + } if (timeout == 0) return i2cHandleHardwareFailure(); I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job @@ -147,7 +153,9 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) } timeout = I2C_DEFAULT_TIMEOUT; - while (busy && --timeout > 0) { ; } + while (busy && --timeout > 0) { + ; + } if (timeout == 0) return i2cHandleHardwareFailure(); @@ -168,9 +176,13 @@ static void i2c_er_handler(void) I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { // if we dont have an ARLO error, ensure sending of a stop if (I2Cx->CR1 & 0x0100) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral - while (I2Cx->CR1 & 0x0100) { ; } // wait for any start to finish sending + while (I2Cx->CR1 & 0x0100) { + ; // wait for any start to finish sending + } I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction - while (I2Cx->CR1 & 0x0200) { ; } // wait for stop to finish sending + while (I2Cx->CR1 & 0x0200) { + ; // wait for stop to finish sending + } i2cInit(I2Cx_index); // reset and configure the hardware } else { I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus @@ -255,7 +267,9 @@ void i2c_ev_handler(void) } } // we must wait for the start to clear, otherwise we get constant BTF - while (I2Cx->CR1 & 0x0100) { ; } + while (I2Cx->CR1 & 0x0100) { + ; + } } else if (SReg_1 & 0x0040) { // Byte received - EV7 read_p[index++] = (uint8_t)I2Cx->DR; if (bytes == (index + 3)) diff --git a/src/drv_i2c.h b/src/drv_i2c.h index b7d7a92f..13f6cbf4 100755 --- a/src/drv_i2c.h +++ b/src/drv_i2c.h @@ -13,5 +13,5 @@ typedef enum I2CDevice { void i2cInit(I2CDevice index); bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data); -bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); +bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t *buf); uint16_t i2cGetErrorCounter(void); diff --git a/src/drv_i2c_soft.c b/src/drv_i2c_soft.c index acef8d6b..b5e615ea 100644 --- a/src/drv_i2c_soft.c +++ b/src/drv_i2c_soft.c @@ -152,7 +152,7 @@ static uint8_t I2C_ReceiveByte(void) return byte; } -void i2cInit(I2C_TypeDef * I2C) +void i2cInit(I2C_TypeDef *I2C) { gpio_config_t gpio; @@ -162,7 +162,7 @@ void i2cInit(I2C_TypeDef * I2C) gpioInit(I2C_GPIO, &gpio); } -bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data) +bool i2cWriteBuffer(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *data) { int i; if (!I2C_Start()) diff --git a/src/drv_ledring.c b/src/drv_ledring.c index 3e943838..8765e4cb 100644 --- a/src/drv_ledring.c +++ b/src/drv_ledring.c @@ -22,7 +22,7 @@ void ledringState(void) if (state == 0) { b[0] = 'z'; - b[1] = (180 - heading) / 2; // 1 unit = 2 degrees; + b[1] = (180 - heading) / 2; // 1 unit = 2 degrees; i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 2, b); state = 1; } else if (state == 1) { @@ -32,7 +32,7 @@ void ledringState(void) i2cWriteBuffer(LED_RING_ADDRESS, 0xFF, 3, b); state = 2; } else if (state == 2) { - b[0] = 'd'; // all unicolor GREEN + b[0] = 'd'; // all unicolor GREEN b[1] = 1; if (f.ARMED) b[2] = 1; diff --git a/src/drv_mpu6050.h b/src/drv_mpu6050.h index c9501f27..bbd8bb81 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, uint16_t lpf, 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/drv_pwm.c b/src/drv_pwm.c index 77d3f407..121bcbf0 100755 --- a/src/drv_pwm.c +++ b/src/drv_pwm.c @@ -162,7 +162,7 @@ static const uint8_t airPWM[] = { 0xFF }; -static const uint8_t * const hardwareMaps[] = { +static const uint8_t *const hardwareMaps[] = { multiPWM, multiPPM, airPWM, @@ -389,7 +389,7 @@ bool pwmInit(drv_pwm_config_t *init) } if (init->extraServos && !init->airplane) { - // remap PWM5..8 as servos when used in extended servo mode. + // remap PWM5..8 as servos when used in extended servo mode. // condition for airplane because airPPM already has these as servos if (port >= PWM5 && port <= PWM8) mask = TYPE_S; diff --git a/src/drv_pwm.h b/src/drv_pwm.h index f6ead9e9..241947ff 100755 --- a/src/drv_pwm.h +++ b/src/drv_pwm.h @@ -23,11 +23,11 @@ typedef struct drv_pwm_config_t { uint8_t adcChannel; // steal one RC input for current sensor uint16_t motorPwmRate; uint16_t servoPwmRate; - uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), - // some higher value (used by 3d mode), or 0, for brushed pwm drivers. + uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), + // some higher value (used by 3d mode), or 0, for brushed pwm drivers. uint16_t servoCenterPulse; uint16_t failsafeThreshold; - + // OUT parameters, filled by driver uint8_t numServos; } drv_pwm_config_t; diff --git a/src/drv_softserial.c b/src/drv_softserial.c index dde4b9b8..7271fb71 100644 --- a/src/drv_softserial.c +++ b/src/drv_softserial.c @@ -29,7 +29,7 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state) } } -softSerial_t* lookupSoftSerial(uint8_t reference) +softSerial_t *lookupSoftSerial(uint8_t reference) { assert_param(reference >= 0 && reference <= MAX_SOFTSERIAL_PORTS); diff --git a/src/drv_softserial.h b/src/drv_softserial.h index 9f7e3a0b..66adc7dc 100644 --- a/src/drv_softserial.h +++ b/src/drv_softserial.h @@ -19,7 +19,7 @@ typedef struct softSerial_s { const timerHardware_t *txTimerHardware; volatile uint8_t txBuffer[SOFT_SERIAL_BUFFER_SIZE]; - + uint8_t isSearchingForStartBit; uint8_t rxBitIndex; uint8_t rxLastLeadingEdgeAtBitIndex; @@ -37,7 +37,7 @@ typedef struct softSerial_s { uint16_t receiveErrors; } softSerial_t; -extern timerHardware_t* serialTimerHardware; +extern timerHardware_t *serialTimerHardware; extern softSerial_t softSerialPorts[]; extern const struct serialPortVTable softSerialVTable[]; diff --git a/src/drv_timer.c b/src/drv_timer.c index 124931a6..9ab655bb 100644 --- a/src/drv_timer.c +++ b/src/drv_timer.c @@ -72,14 +72,14 @@ enum { #define CC_CHANNELS_PER_TIMER 4 // TIM_Channel_1..4 -static const TIM_TypeDef * const timers[MAX_TIMERS] = { +static const TIM_TypeDef *const timers[MAX_TIMERS] = { [TIM1_IDX] = TIM1, [TIM2_IDX] = TIM2, [TIM3_IDX] = TIM3, [TIM4_IDX] = TIM4 }; typedef struct channelConfig_s { uint16_t channel; uint16_t interruptBit; - uint16_t (*TIM_GetCaptureFn)(TIM_TypeDef* TIMx); + uint16_t (*TIM_GetCaptureFn)(TIM_TypeDef *TIMx); } channelConfig_t; static const channelConfig_t channels[CC_CHANNELS_PER_TIMER] = { @@ -101,7 +101,7 @@ timerConfig_t timerConfigs[MAX_TIMERS][CC_CHANNELS_PER_TIMER]; static int lookupTimerIndex(const TIM_TypeDef *tim) { int timerIndex; - for (timerIndex=0; timerIndex < MAX_TIMERS; timerIndex++ ) { + for (timerIndex = 0; timerIndex < MAX_TIMERS; timerIndex++ ) { if (timers[timerIndex] == tim) break; } @@ -111,7 +111,7 @@ static int lookupTimerIndex(const TIM_TypeDef *tim) static int lookupChannelIndex(const int channel) { int channelIndex; - for (channelIndex=0; channelIndex < CC_CHANNELS_PER_TIMER; channelIndex++ ) { + for (channelIndex = 0; channelIndex < CC_CHANNELS_PER_TIMER; channelIndex++ ) { if (channels[channelIndex].channel == channel) break; } @@ -202,12 +202,12 @@ static timerConfig_t *findTimerConfig(unsigned int timerIndex, unsigned int chan return &(timerConfigs[timerIndex][channelIndex]); } -static void timCCxHandler(TIM_TypeDef * const tim, unsigned int timerIndex) +static void timCCxHandler(TIM_TypeDef *const tim, unsigned int timerIndex) { unsigned int channelIndex; for (channelIndex = 0; channelIndex < CC_CHANNELS_PER_TIMER; channelIndex++) { - channelConfig_t const * const channel = &channels[channelIndex]; + channelConfig_t const *const channel = &channels[channelIndex]; if (TIM_GetITStatus(tim, channel->interruptBit) == SET) { TIM_ClearITPendingBit(tim, channel->interruptBit); diff --git a/src/drv_uart.c b/src/drv_uart.c index ded7fd20..4745e81d 100755 --- a/src/drv_uart.c +++ b/src/drv_uart.c @@ -291,7 +291,7 @@ static void uartStartTxDMA(uartPort_t *s) uint8_t uartTotalBytesWaiting(serialPort_t *instance) { - uartPort_t *s = (uartPort_t*)instance; + uartPort_t *s = (uartPort_t *)instance; // FIXME always returns 1 or 0, not the amount of bytes waiting if (s->rxDMAChannel) return s->rxDMAChannel->CNDTR != s->rxDMAPos; @@ -341,8 +341,8 @@ void uartWrite(serialPort_t *instance, uint8_t ch) } const struct serialPortVTable uartVTable[] = { - { - uartWrite, + { + uartWrite, uartTotalBytesWaiting, uartRead, uartSetBaudRate, diff --git a/src/gps.c b/src/gps.c index 1feacc59..85330308 100644 --- a/src/gps.c +++ b/src/gps.c @@ -54,7 +54,7 @@ static const uint8_t ubloxInit[] = { }; static const uint8_t poll_svinfo[] = { - 0xB5, 0x62, 0x01, 0x30, 0x00, 0x00, 0x31, 0x94 // Poll: UBX-NAV-SVINFO (0x01 0x30) + 0xB5, 0x62, 0x01, 0x30, 0x00, 0x00, 0x31, 0x94 // Poll: UBX-NAV-SVINFO (0x01 0x30) }; static uint8_t ubloxSbasInit[] = { @@ -160,10 +160,10 @@ void gpsInit(uint8_t baudrateIndex) gpsSetPIDs(); if (feature(FEATURE_SERIALRX) && feature(FEATURE_SOFTSERIAL) && !mcfg.spektrum_sat_on_flexport) { if (gpsInitData[baudrateIndex].baudrate > SOFT_SERIAL_MAX_BAUD_RATE) { - mcfg.softserial_baudrate = SOFT_SERIAL_MAX_BAUD_RATE; - for (i = 0; i < GPS_INIT_ENTRIES; i++) - if (SOFT_SERIAL_MAX_BAUD_RATE == gpsInitData[i].baudrate) - mcfg.gps_baudrate = gpsInitData[i].index; + mcfg.softserial_baudrate = SOFT_SERIAL_MAX_BAUD_RATE; + for (i = 0; i < GPS_INIT_ENTRIES; i++) + if (SOFT_SERIAL_MAX_BAUD_RATE == gpsInitData[i].baudrate) + mcfg.gps_baudrate = gpsInitData[i].index; } else mcfg.softserial_baudrate = gpsInitData[baudrateIndex].baudrate; // If SerialRX is in use then use soft serial ports for GPS (pin 5 & 6) @@ -382,11 +382,11 @@ void gpsPollSvinfo(void) #define GPS_LOW_SPEED_D_FILTER 1 // below .5m/s speed ignore D term for POSHOLD_RATE, theoretically this also removed D term induced noise static bool check_missed_wp(void); -static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * dist, int32_t * bearing); +static void GPS_distance_cm_bearing(int32_t *lat1, int32_t *lon1, int32_t *lat2, int32_t *lon2, int32_t *dist, int32_t *bearing); //static void GPS_distance(int32_t lat1, int32_t lon1, int32_t lat2, int32_t lon2, uint16_t* dist, int16_t* bearing); static void GPS_calc_longitude_scaling(int32_t lat); static void GPS_calc_velocity(void); -static void GPS_calc_location_error(int32_t * target_lat, int32_t * target_lng, int32_t * gps_lat, int32_t * gps_lng); +static void GPS_calc_location_error(int32_t *target_lat, int32_t *target_lng, int32_t *gps_lat, int32_t *gps_lng); static void GPS_calc_poshold(void); static void GPS_calc_nav_rate(int max_speed); static void GPS_update_crosstrack(void); @@ -442,7 +442,7 @@ static int32_t get_P(int32_t error, PID_PARAM *pid) static int32_t get_I(int32_t error, float *dt, PID *pid, PID_PARAM *pid_param) { - pid->integrator += ((float)error * pid_param->kI) * *dt; + pid->integrator += ((float)error * pid_param->kI) **dt; pid->integrator = constrain(pid->integrator, -pid_param->Imax, pid_param->Imax); return pid->integrator; } @@ -604,37 +604,37 @@ static void gpsNewData(uint16_t c) GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); - if(f.FIXED_WING) + if (f.FIXED_WING) nav_mode = NAV_MODE_WP; // Planes always navigate in Wp mode. switch (nav_mode) { - case NAV_MODE_POSHOLD: - // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 - GPS_calc_poshold(); - break; - - case NAV_MODE_WP: - speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); // slow navigation - // use error as the desired rate towards the target - // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 - GPS_calc_nav_rate(speed); - - // Tail control - if (cfg.nav_controls_heading) { - if (NAV_TAIL_FIRST) { - magHold = wrap_18000(nav_bearing - 18000) / 100; - } else { - magHold = nav_bearing / 100; + case NAV_MODE_POSHOLD: + // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 + GPS_calc_poshold(); + break; + + case NAV_MODE_WP: + speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV); // slow navigation + // use error as the desired rate towards the target + // Desired output is in nav_lat and nav_lon where 1deg inclination is 100 + GPS_calc_nav_rate(speed); + + // Tail control + if (cfg.nav_controls_heading) { + if (NAV_TAIL_FIRST) { + magHold = wrap_18000(nav_bearing - 18000) / 100; + } else { + magHold = nav_bearing / 100; + } } - } - // Are we there yet ?(within x meters of the destination) - if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode - nav_mode = NAV_MODE_POSHOLD; - if (NAV_SET_TAKEOFF_HEADING) { - magHold = nav_takeoff_bearing; + // Are we there yet ?(within x meters of the destination) + if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) { // if yes switch to poshold mode + nav_mode = NAV_MODE_POSHOLD; + if (NAV_SET_TAKEOFF_HEADING) { + magHold = nav_takeoff_bearing; + } } - } - break; + break; } } //end of gps calcs } @@ -691,9 +691,9 @@ void gpsSetPIDs(void) navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100; if (f.FIXED_WING) { - altPID_PARAM.kP = (float)cfg.P8[PIDALT] / 10.0f; - altPID_PARAM.kI = (float)cfg.I8[PIDALT] / 100.0f; - altPID_PARAM.kD = (float)cfg.D8[PIDALT] / 1000.0f; + altPID_PARAM.kP = (float)cfg.P8[PIDALT] / 10.0f; + altPID_PARAM.kI = (float)cfg.I8[PIDALT] / 100.0f; + altPID_PARAM.kD = (float)cfg.D8[PIDALT] / 1000.0f; } } @@ -705,7 +705,7 @@ int8_t gpsSetPassthrough(void) LED0_OFF; LED1_OFF; - while(1) { + while (1) { if (serialTotalBytesWaiting(core.gpsport)) { LED0_ON; serialWrite(core.mainport, serialRead(core.gpsport)); @@ -768,7 +768,7 @@ static bool check_missed_wp(void) //////////////////////////////////////////////////////////////////////////////////// // Get distance between two points in cm // Get bearing from pos1 to pos2, returns an 1deg = 100 precision -static void GPS_distance_cm_bearing(int32_t * lat1, int32_t * lon1, int32_t * lat2, int32_t * lon2, int32_t * dist, int32_t * bearing) +static void GPS_distance_cm_bearing(int32_t *lat1, int32_t *lon1, int32_t *lat2, int32_t *lon2, int32_t *dist, int32_t *bearing) { float dLat = *lat2 - *lat1; // difference of latitude in 1/10 000 000 degrees float dLon = (float)(*lon2 - *lon1) * GPS_scaleLonDown; @@ -1001,7 +1001,7 @@ static uint32_t GPS_coord_to_degrees(char *s) */ #define DIGIT_TO_VAL(_x) (_x - '0') -uint32_t GPS_coord_to_degrees(char* s) +uint32_t GPS_coord_to_degrees(char *s) { char *p, *q; uint8_t deg = 0, min = 0; @@ -1043,7 +1043,8 @@ uint32_t GPS_coord_to_degrees(char* s) // helper functions static uint32_t grab_fields(char *src, uint8_t mult) -{ // convert string to uint32 +{ + // convert string to uint32 uint32_t i; uint32_t tmp = 0; for (i = 0; src[i] != 0; i++) { @@ -1066,7 +1067,7 @@ static uint32_t grab_fields(char *src, uint8_t mult) /* This is a light implementation of a GPS frame decoding This should work with most of modern GPS devices configured to output NMEA frames. It assumes there are some NMEA GGA frames to decode on the serial bus - Now verifies checksum correctly before applying data + Now verifies checksum correctly before applying data Here we use only the following data : - latitude @@ -1084,7 +1085,7 @@ static uint32_t grab_fields(char *src, uint8_t mult) typedef struct gpsMessage_t { int32_t latitude; - int32_t longitude; + int32_t longitude; uint8_t numSat; uint16_t altitude; uint16_t speed; @@ -1172,8 +1173,8 @@ static bool gpsNewFrameNMEA(char c) if (checksum == parity) { switch (gps_frame) { case FRAME_GGA: - frameOK = 1; - if (f.GPS_FIX) { + frameOK = 1; + if (f.GPS_FIX) { GPS_coord[LAT] = gps_msg.latitude; GPS_coord[LON] = gps_msg.longitude; GPS_numSat = gps_msg.numSat; @@ -1436,59 +1437,59 @@ static bool UBLOX_parse_gps(void) { int i; switch (_msg_id) { - case MSG_POSLLH: - //i2c_dataset.time = _buffer.posllh.time; - GPS_coord[LON] = _buffer.posllh.longitude; - GPS_coord[LAT] = _buffer.posllh.latitude; - GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m - GPS_HorizontalAcc = _buffer.posllh.horizontal_accuracy; - GPS_VerticalAcc = _buffer.posllh.vertical_accuracy; - f.GPS_FIX = next_fix; - _new_position = true; - if (!sensors(SENSOR_BARO) && f.FIXED_WING) - EstAlt = (GPS_altitude - GPS_home[ALT]) * 100; // Use values Based on GPS - // Update GPS update rate table. - GPS_update_rate[0] = GPS_update_rate[1]; - GPS_update_rate[1] = millis(); - break; - case MSG_STATUS: - next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); - if (!next_fix) - f.GPS_FIX = false; - break; - case MSG_SOL: - next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); - if (!next_fix) - f.GPS_FIX = false; - GPS_numSat = _buffer.solution.satellites; - // GPS_hdop = _buffer.solution.position_DOP; - break; - case MSG_VELNED: - // speed_3d = _buffer.velned.speed_3d; // cm/s - GPS_speed = _buffer.velned.speed_2d; // cm/s - GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 - _new_speed = true; - if (!sensors(SENSOR_MAG) && GPS_speed > 100) { - GPS_ground_course = wrap_18000(GPS_ground_course * 10) / 10; - heading = GPS_ground_course / 10; // Use values Based on GPS if we are moving. - } - break; - case MSG_SVINFO: - GPS_numCh = _buffer.svinfo.numCh; - if (GPS_numCh > 32) - GPS_numCh = 32; - for (i = 0; i < GPS_numCh; i++){ - GPS_svinfo_chn[i]= _buffer.svinfo.channel[i].chn; - GPS_svinfo_svid[i]= _buffer.svinfo.channel[i].svid; - GPS_svinfo_quality[i]=_buffer.svinfo.channel[i].quality; - GPS_svinfo_cno[i]= _buffer.svinfo.channel[i].cno; - } - // Update GPS SVIFO update rate table. - GPS_svinfo_rate[0] = GPS_svinfo_rate[1]; - GPS_svinfo_rate[1] = millis(); - break; - default: - return false; + case MSG_POSLLH: + //i2c_dataset.time = _buffer.posllh.time; + GPS_coord[LON] = _buffer.posllh.longitude; + GPS_coord[LAT] = _buffer.posllh.latitude; + GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m + GPS_HorizontalAcc = _buffer.posllh.horizontal_accuracy; + GPS_VerticalAcc = _buffer.posllh.vertical_accuracy; + f.GPS_FIX = next_fix; + _new_position = true; + if (!sensors(SENSOR_BARO) && f.FIXED_WING) + EstAlt = (GPS_altitude - GPS_home[ALT]) * 100; // Use values Based on GPS + // Update GPS update rate table. + GPS_update_rate[0] = GPS_update_rate[1]; + GPS_update_rate[1] = millis(); + break; + case MSG_STATUS: + next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); + if (!next_fix) + f.GPS_FIX = false; + break; + case MSG_SOL: + next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); + if (!next_fix) + f.GPS_FIX = false; + GPS_numSat = _buffer.solution.satellites; + // GPS_hdop = _buffer.solution.position_DOP; + break; + case MSG_VELNED: + // speed_3d = _buffer.velned.speed_3d; // cm/s + GPS_speed = _buffer.velned.speed_2d; // cm/s + GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + _new_speed = true; + if (!sensors(SENSOR_MAG) && GPS_speed > 100) { + GPS_ground_course = wrap_18000(GPS_ground_course * 10) / 10; + heading = GPS_ground_course / 10; // Use values Based on GPS if we are moving. + } + break; + case MSG_SVINFO: + GPS_numCh = _buffer.svinfo.numCh; + if (GPS_numCh > 32) + GPS_numCh = 32; + for (i = 0; i < GPS_numCh; i++) { + GPS_svinfo_chn[i] = _buffer.svinfo.channel[i].chn; + GPS_svinfo_svid[i] = _buffer.svinfo.channel[i].svid; + GPS_svinfo_quality[i] = _buffer.svinfo.channel[i].quality; + GPS_svinfo_cno[i] = _buffer.svinfo.channel[i].cno; + } + // Update GPS SVIFO update rate table. + GPS_svinfo_rate[0] = GPS_svinfo_rate[1]; + GPS_svinfo_rate[1] = millis(); + break; + default: + return false; } // we only return true when we get new position and speed data @@ -1504,12 +1505,12 @@ static bool UBLOX_parse_gps(void) void gpsInit(uint8_t baudrateIndex) { - + } void gpsThread(void) { - + } #endif /* GPS */ diff --git a/src/ibus.c b/src/ibus.c index 02397f57..6d8c7fc7 100755 --- a/src/ibus.c +++ b/src/ibus.c @@ -53,7 +53,7 @@ static void ibusDataReceive(uint16_t c) bool ibusFrameComplete(void) { uint8_t i; - uint16_t chksum,rxsum; + uint16_t chksum, rxsum; if (ibusFrameDone) { ibusFrameDone = false; @@ -66,7 +66,7 @@ bool ibusFrameComplete(void) rxsum = ibus[30] + (ibus[31] << 8); if (chksum == rxsum) { - ibusChannelData[0] = (ibus[ 3] << 8) + ibus[ 2]; + ibusChannelData[0] = (ibus[ 3] << 8) + ibus[ 2]; ibusChannelData[1] = (ibus[ 5] << 8) + ibus[ 4]; ibusChannelData[2] = (ibus[ 7] << 8) + ibus[ 6]; ibusChannelData[3] = (ibus[ 9] << 8) + ibus[ 8]; diff --git a/src/imu.c b/src/imu.c index 3aca05cd..6dbb9331 100644 --- a/src/imu.c +++ b/src/imu.c @@ -12,7 +12,7 @@ uint32_t baroPressureSum = 0; int32_t BaroAlt = 0; float sonarTransition = 0; int32_t baroAlt_offset = 0; -int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range +int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range int32_t EstAlt; // in cm int32_t BaroPID = 0; int32_t AltHold; @@ -41,7 +41,7 @@ void imuInit(void) smallAngle = lrintf(acc_1G * cosf(RAD * cfg.small_angle)); accVelScale = 9.80665f / acc_1G / 10000.0f; throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle); - + fc_acc = 0.5f / (M_PI * cfg.accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf #ifdef MAG @@ -206,7 +206,7 @@ void acc_calc(uint32_t deltaT) accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband); accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband); accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband); - + accTimeSum += deltaT; accSumCount++; } @@ -426,7 +426,7 @@ int getEstimatedAltitude(void) } else { setVel = setVelocity; } - + // Velocity PID-Controller // P error = setVel - vel_tmp; diff --git a/src/main.c b/src/main.c index 88ab97c3..74060296 100755 --- a/src/main.c +++ b/src/main.c @@ -43,8 +43,8 @@ int main(void) drv_adc_config_t adc_params; bool sensorsOK = false; #ifdef SOFTSERIAL_LOOPBACK - serialPort_t* loopbackPort1 = NULL; - serialPort_t* loopbackPort2 = NULL; + serialPort_t *loopbackPort1 = NULL; + serialPort_t *loopbackPort2 = NULL; #endif initEEPROM(); @@ -227,10 +227,10 @@ int main(void) setupSoftSerialSecondary(mcfg.softserial_2_inverted); #ifdef SOFTSERIAL_LOOPBACK - loopbackPort1 = (serialPort_t*)&(softSerialPorts[0]); + loopbackPort1 = (serialPort_t *)(&softSerialPorts[0])); serialPrint(loopbackPort1, "SOFTSERIAL 1 - LOOPBACK ENABLED\r\n"); - loopbackPort2 = (serialPort_t*)&(softSerialPorts[1]); + loopbackPort2 = (serialPort_t *)(&softSerialPorts[1])); serialPrint(loopbackPort2, "SOFTSERIAL 2 - LOOPBACK ENABLED\r\n"); #endif //core.mainport = (serialPort_t*)&(softSerialPorts[0]); // Uncomment to switch the main port to use softserial. diff --git a/src/mixer.c b/src/mixer.c index a226e3a2..dd8b3abb 100644 --- a/src/mixer.c +++ b/src/mixer.c @@ -291,7 +291,7 @@ void mixerInit(void) currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; } } - + if (core.useServo) { numberRules = servoMixers[mcfg.mixerConfiguration].numberRules; if (servoMixers[mcfg.mixerConfiguration].rule) { @@ -314,7 +314,7 @@ void mixerInit(void) // set flag that we're on something with wings if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE || mcfg.mixerConfiguration == MULTITYPE_CUSTOM_PLANE) { f.FIXED_WING = 1; - + if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM_PLANE) { // load custom mixer into currentServoMixer for (i = 0; i < MAX_SERVO_RULES; i++) { @@ -325,8 +325,7 @@ void mixerInit(void) numberRules++; } } - } - else + } else f.FIXED_WING = 0; mixerResetMotors(); @@ -340,7 +339,7 @@ void mixerResetMotors(void) motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; } -void servoMixerLoadMix(int index) +void servoMixerLoadMix(int index) { int i; @@ -409,7 +408,7 @@ void writeServos(void) pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); break; - + case MULTITYPE_AIRPLANE: case MULTITYPE_SINGLECOPTER: pwmWriteServo(0, servo[3]); @@ -417,7 +416,7 @@ void writeServos(void) pwmWriteServo(2, servo[5]); pwmWriteServo(3, servo[6]); break; - + case MULTITYPE_CUSTOM_PLANE: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); @@ -464,7 +463,7 @@ static void servoMixer(void) int16_t input[INPUT_ITEMS]; static int16_t currentOutput[MAX_SERVO_RULES]; uint8_t i; - + if (f.PASSTHRU_MODE) { // Direct passthru from RX input[INPUT_ROLL] = rcCommand[ROLL]; @@ -486,20 +485,20 @@ static void servoMixer(void) input[INPUT_RC_PITCH] = mcfg.midrc - rcData[PITCH]; input[INPUT_RC_YAW] = mcfg.midrc - rcData[YAW]; input[INPUT_RC_THROTTLE] = mcfg.midrc - rcData[THROTTLE]; - + for (i = 0; i < MAX_SERVOS; i++) servo[i] = servoMiddle(i); // mix servos according to rules for (i = 0; i < numberRules; i++) { // consider rule if no box assigned or box is active - if (currentServoMixer[i].box == 0 || rcOptions[BOXSERVO1+currentServoMixer[i].box-1]) { + if (currentServoMixer[i].box == 0 || rcOptions[BOXSERVO1 + currentServoMixer[i].box - 1]) { uint8_t target = currentServoMixer[i].targetChannel; uint8_t from = currentServoMixer[i].fromChannel; uint16_t servo_width = cfg.servoConf[target].max - cfg.servoConf[target].min; int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; - + if (currentServoMixer[i].speed == 0) currentOutput[i] = input[from]; else { @@ -607,7 +606,7 @@ void mixTable(void) if ((rcData[THROTTLE]) < mcfg.mincheck) { if (!feature(FEATURE_MOTOR_STOP)) motor[i] = mcfg.minthrottle; - else { + else { motor[i] = mcfg.mincommand; f.MOTORS_STOPPED = 1; } diff --git a/src/mw.c b/src/mw.c index 028bdcf1..b2e20b46 100755 --- a/src/mw.c +++ b/src/mw.c @@ -173,7 +173,7 @@ void annexCode(void) vbatRaw -= vbatRaw / 8; vbatRaw += adcGetChannel(ADC_BATTERY); vbat = batteryAdcToVoltage(vbatRaw / 8); - + if (mcfg.power_adc_channel > 0) { amperageRaw -= amperageRaw / 8; amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT); @@ -182,7 +182,7 @@ void annexCode(void) mAhdrawn = mAhdrawnRaw / (3600 * 100); vbatCycleTime = 0; } - + } // Buzzers for low and critical battery levels if (vbat <= batteryCriticalVoltage) @@ -432,7 +432,7 @@ static void pidRewrite(void) // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings - errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13); + errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)(-GYRO_I_MAX) << 13, (int32_t)(+GYRO_I_MAX) << 13); ITerm = errorGyroI[axis] >> 13; //-----calculate D-term @@ -607,7 +607,7 @@ void loop(void) calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) if (!sensors(SENSOR_MAG)) heading = 0; // reset heading to zero after gyro calibration - // Inflight ACC Calibration + // 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 = false; @@ -707,7 +707,7 @@ void loop(void) } else { f.ANGLE_MODE = 0; // failsafe support f.FW_FAILSAFE_RTH_ENABLE = 0; - } + } if (rcOptions[BOXHORIZON]) { f.ANGLE_MODE = 0; @@ -836,7 +836,7 @@ void loop(void) if (feature(FEATURE_FAILSAFE) && failsafeCnt > (6 * cfg.failsafe_delay)) { f.PASSTHRU_MODE = 0; f.ANGLE_MODE = 1; - for (i = 0; i < 3; i++) + for (i = 0; i < 3; i++) rcData[i] = mcfg.midrc; rcData[THROTTLE] = cfg.failsafe_throttle; // No GPS? Force a soft left turn. @@ -861,45 +861,45 @@ void loop(void) } else { // not in rc loop static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes switch (taskOrder) { - case 0: - taskOrder++; + case 0: + taskOrder++; #ifdef MAG - if (sensors(SENSOR_MAG) && Mag_getADC()) - break; + if (sensors(SENSOR_MAG) && Mag_getADC()) + break; #endif - case 1: - taskOrder++; + case 1: + taskOrder++; #ifdef BARO - if (sensors(SENSOR_BARO) && Baro_update()) - break; + if (sensors(SENSOR_BARO) && Baro_update()) + break; #endif - case 2: - taskOrder++; + case 2: + taskOrder++; #ifdef BARO - if (sensors(SENSOR_BARO) && getEstimatedAltitude()) - break; + if (sensors(SENSOR_BARO) && getEstimatedAltitude()) + break; #endif - case 3: - // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck - // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will - // change this based on available hardware - taskOrder++; + case 3: + // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck + // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will + // change this based on available hardware + taskOrder++; #ifdef GPS - if (feature(FEATURE_GPS)) { - gpsThread(); - break; - } + if (feature(FEATURE_GPS)) { + gpsThread(); + break; + } #endif - case 4: - taskOrder = 0; + case 4: + taskOrder = 0; #ifdef SONAR - if (sensors(SENSOR_SONAR)) { - Sonar_update(); - } + if (sensors(SENSOR_SONAR)) { + Sonar_update(); + } #endif - if (feature(FEATURE_VARIO) && f.VARIO_MODE) - mwVario(); - break; + if (feature(FEATURE_VARIO) && f.VARIO_MODE) + mwVario(); + break; } } @@ -983,7 +983,7 @@ void loop(void) float sin_yaw_y = sinf(heading * 0.0174532925f); float cos_yaw_x = cosf(heading * 0.0174532925f); if (!f.FIXED_WING) { - if (cfg.nav_slew_rate) { + if (cfg.nav_slew_rate) { nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8 nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate); GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10; @@ -991,12 +991,12 @@ void loop(void) } else { GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10; GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10; - } + } } else fw_nav(); } else { - GPS_angle[ROLL] = 0; - GPS_angle[PITCH] = 0; - GPS_angle[YAW] = 0; + GPS_angle[ROLL] = 0; + GPS_angle[PITCH] = 0; + GPS_angle[YAW] = 0; } } #endif diff --git a/src/mw.h b/src/mw.h index 686fb999..9b420328 100755 --- a/src/mw.h +++ b/src/mw.h @@ -19,16 +19,14 @@ // Serial GPS only variables // navigation mode -typedef enum NavigationMode -{ +typedef enum NavigationMode { NAV_MODE_NONE = 0, NAV_MODE_POSHOLD, NAV_MODE_WP } NavigationMode; // Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization. -typedef enum MultiType -{ +typedef enum MultiType { MULTITYPE_TRI = 1, MULTITYPE_QUADP = 2, MULTITYPE_QUADX = 3, @@ -47,7 +45,7 @@ typedef enum MultiType MULTITYPE_HELI_90_DEG = 16, MULTITYPE_VTAIL4 = 17, MULTITYPE_HEX6H = 18, - MULTITYPE_PPM_TO_SERVO = 19, // PPM -> servo relay + MULTITYPE_PPM_TO_SERVO = 19, // PPM -> servo relay MULTITYPE_DUALCOPTER = 20, MULTITYPE_SINGLECOPTER = 21, MULTITYPE_ATAIL4 = 22, @@ -486,7 +484,7 @@ extern int32_t GPS_hold[3]; extern uint8_t GPS_numSat; extern uint16_t GPS_distanceToHome; // distance to home or hold point in meters extern int16_t GPS_directionToHome; // direction to home or hol point in degrees -extern uint16_t GPS_altitude,GPS_speed; // altitude in 0.1m and speed in 0.1m/s +extern uint16_t GPS_altitude, GPS_speed; // altitude in 0.1m and speed in 0.1m/s extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update extern int16_t GPS_angle[3]; // it's the angles that must be applied for GPS correction extern uint16_t GPS_ground_course; // degrees*10 @@ -603,7 +601,7 @@ int8_t gpsSetPassthrough(void); void gpsPollSvinfo(void); void GPS_reset_home_position(void); void GPS_reset_nav(void); -void GPS_set_next_wp(int32_t* lat, int32_t* lon); +void GPS_set_next_wp(int32_t *lat, int32_t *lon); int32_t wrap_18000(int32_t error); void fw_nav(void); diff --git a/src/printf.c b/src/printf.c index d4fa8102..3916558f 100644 --- a/src/printf.c +++ b/src/printf.c @@ -148,7 +148,7 @@ void tfp_format(void *putp, putcf putf, char *fmt, va_list va) putf(putp, ch); else { char lz = 0; -#ifdef PRINTF_LONG_SUPPORT +#ifdef PRINTF_LONG_SUPPORT char lng = 0; #endif int w = 0; @@ -160,17 +160,17 @@ void tfp_format(void *putp, putcf putf, char *fmt, va_list va) if (ch >= '0' && ch <= '9') { ch = a2i(ch, &fmt, 10, &w); } -#ifdef PRINTF_LONG_SUPPORT +#ifdef PRINTF_LONG_SUPPORT if (ch == 'l') { ch = *(fmt++); lng = 1; } #endif switch (ch) { - case 0: - goto abort; - case 'u':{ -#ifdef PRINTF_LONG_SUPPORT + case 0: + goto abort; + case 'u': { +#ifdef PRINTF_LONG_SUPPORT if (lng) uli2a(va_arg(va, unsigned long int), 10, 0, bf); else @@ -179,8 +179,8 @@ void tfp_format(void *putp, putcf putf, char *fmt, va_list va) putchw(putp, putf, w, lz, bf); break; } - case 'd':{ -#ifdef PRINTF_LONG_SUPPORT + case 'd': { +#ifdef PRINTF_LONG_SUPPORT if (lng) li2a(va_arg(va, unsigned long int), bf); else @@ -189,30 +189,31 @@ void tfp_format(void *putp, putcf putf, char *fmt, va_list va) putchw(putp, putf, w, lz, bf); break; } - case 'x': - case 'X': -#ifdef PRINTF_LONG_SUPPORT - if (lng) - uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); - else + case 'x': + case 'X': +#ifdef PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf); + else #endif - ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); - putchw(putp, putf, w, lz, bf); - break; - case 'c': - putf(putp, (char) (va_arg(va, int))); - break; - case 's': - putchw(putp, putf, w, 0, va_arg(va, char *)); - break; - case '%': - putf(putp, ch); - default: - break; + ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf); + putchw(putp, putf, w, lz, bf); + break; + case 'c': + putf(putp, (char) (va_arg(va, int))); + break; + case 's': + putchw(putp, putf, w, 0, va_arg(va, char *)); + break; + case '%': + putf(putp, ch); + default: + break; } } } - abort:; +abort: + ; } diff --git a/src/printf.h b/src/printf.h index f93a226d..10b2e1aa 100644 --- a/src/printf.h +++ b/src/printf.h @@ -66,10 +66,10 @@ To use the printf you need to supply your own character output function, something like : void putc ( void* p, char c) - { - while (!SERIAL_PORT_EMPTY) ; - SERIAL_PORT_TX_REGISTER = c; - } + { + while (!SERIAL_PORT_EMPTY) ; + SERIAL_PORT_TX_REGISTER = c; + } Before you can call printf you need to initialize it to use your character output function with something like: diff --git a/src/sensors.c b/src/sensors.c index 5cacf772..1994f6d8 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -47,14 +47,14 @@ bool sensorsAutodetect(void) if (hw_revision == NAZE32_SP && mpu6500Detect(&acc, &gyro, mcfg.gyro_lpf)) haveMpu65 = true; else if (l3g4200dDetect(&gyro, mcfg.gyro_lpf)) { - // well, we found our gyro - ; - } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) + // well, we found our gyro + ; + } else if (!mpu3050Detect(&gyro, mcfg.gyro_lpf)) #endif - { - // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. - return false; - } + { + // if this fails, we get a beep + blink pattern. we're doomed, no gyro or i2c error. + return false; + } // Accelerometer. Fuck it. Let user break shit. retry: @@ -121,7 +121,7 @@ bool sensorsAutodetect(void) #ifdef BARO // Detect what pressure sensors are available. baro->update() is set to sensor-specific update function if (!bmp085Detect(&baro)) { - // ms5611 disables BMP085, and tries to initialize + check PROM crc. + // ms5611 disables BMP085, and tries to initialize + check PROM crc. // moved 5611 init here because there have been some reports that calibration data in BMP180 // has been "passing" ms5611 PROM crc check if (!ms5611Detect(&baro)) { @@ -138,7 +138,7 @@ bool sensorsAutodetect(void) gyro.init(mcfg.gyro_align); #ifdef MAG - retryMag: +retryMag: switch (mcfg.mag_hardware) { case MAG_NONE: // disable MAG sensorsClear(SENSOR_MAG); @@ -147,22 +147,22 @@ bool sensorsAutodetect(void) case MAG_HMC5883L: if (hmc5883lDetect(&mag)) { - magHardware = MAG_HMC5883L; - if (mcfg.mag_hardware == MAG_HMC5883L) - break; - } - ; // fallthrough - -#ifdef NAZE + magHardware = MAG_HMC5883L; + if (mcfg.mag_hardware == MAG_HMC5883L) + break; + } + ; // fallthrough + +#ifdef NAZE case MAG_AK8975: if (ak8975detect(&mag)) { magHardware = MAG_AK8975; if (mcfg.mag_hardware == MAG_AK8975) break; } -#endif +#endif } - + // Found anything? Check if user fucked up or mag is really missing. if (magHardware == MAG_DEFAULT) { if (mcfg.mag_hardware > MAG_DEFAULT && mcfg.mag_hardware < MAG_NONE) { @@ -216,11 +216,11 @@ uint16_t batteryAdcToVoltage(uint16_t src) int32_t currentSensorToCentiamps(uint16_t src) { int32_t millivolts; - + millivolts = ((uint32_t)src * ADCVREF * 100) / 4095; millivolts -= mcfg.currentoffset; - - return (millivolts * 1000) / (int32_t)mcfg.currentscale; // current in 0.01A steps + + return (millivolts * 1000) / (int32_t)mcfg.currentscale; // current in 0.01A steps } void batteryInit(void) @@ -378,8 +378,7 @@ int Baro_update(void) } #endif /* BARO */ -typedef struct stdev_t -{ +typedef struct stdev_t { float m_oldM, m_newM, m_oldS, m_newS; int m_n; } stdev_t; diff --git a/src/serial.c b/src/serial.c index 5d13bcf0..4696e605 100755 --- a/src/serial.c +++ b/src/serial.c @@ -319,14 +319,14 @@ void serialInit(uint32_t baudrate) availableBoxes[idx++] = BOXSERVO2; availableBoxes[idx++] = BOXSERVO3; } - + numberBoxItems = idx; } static uint8_t getOldServoConfig(uint8_t servoIndex) { uint8_t tmpRate = cfg.servoConf[servoIndex].rate; - + switch (mcfg.mixerConfiguration) { case MULTITYPE_BI: if (servoIndex == 4) { @@ -335,20 +335,20 @@ static uint8_t getOldServoConfig(uint8_t servoIndex) if (cfg.servoConf[servoIndex].direction & (1 << INPUT_YAW)) tmpRate |= 2; } - + if (servoIndex == 5) { if (cfg.servoConf[servoIndex].direction & (1 << INPUT_PITCH)) tmpRate |= 1; if (cfg.servoConf[servoIndex].direction & (1 << INPUT_YAW)) tmpRate |= 2; } - + break; case MULTITYPE_TRI: if (servoIndex == 5) if (cfg.servoConf[servoIndex].direction & (1 << INPUT_YAW)) tmpRate |= 1; - + break; case MULTITYPE_FLYING_WING: if (servoIndex == 3) { @@ -357,7 +357,7 @@ static uint8_t getOldServoConfig(uint8_t servoIndex) if (cfg.servoConf[servoIndex].direction & (1 << INPUT_ROLL)) tmpRate |= 2; } - + if (servoIndex == 4) { if (cfg.servoConf[servoIndex].direction & (1 << INPUT_PITCH)) tmpRate |= 1; @@ -370,12 +370,12 @@ static uint8_t getOldServoConfig(uint8_t servoIndex) if (servoIndex == 4) if (cfg.servoConf[servoIndex].direction & (1 << INPUT_PITCH)) tmpRate |= 1; - + if (servoIndex == 5) if (cfg.servoConf[servoIndex].direction & (1 << INPUT_ROLL)) tmpRate |= 1; - - + + break; case MULTITYPE_SINGLECOPTER: if (servoIndex == 3) { @@ -384,28 +384,28 @@ static uint8_t getOldServoConfig(uint8_t servoIndex) if (cfg.servoConf[servoIndex].direction & (1 << INPUT_YAW)) tmpRate |= 2; } - + if (servoIndex == 4) { if (cfg.servoConf[servoIndex].direction & (1 << INPUT_PITCH)) tmpRate |= 1; if (cfg.servoConf[servoIndex].direction & (1 << INPUT_YAW)) tmpRate |= 2; } - + if (servoIndex == 5) { if (cfg.servoConf[servoIndex].direction & (1 << INPUT_ROLL)) tmpRate |= 1; if (cfg.servoConf[servoIndex].direction & (1 << INPUT_YAW)) tmpRate |= 2; } - + if (servoIndex == 6) { if (cfg.servoConf[servoIndex].direction & (1 << INPUT_ROLL)) tmpRate |= 1; if (cfg.servoConf[servoIndex].direction & (1 << INPUT_YAW)) tmpRate |= 2; } - + break; } return tmpRate; @@ -431,492 +431,492 @@ static void evaluateCommand(void) const char *build = __DATE__; switch (currentPortState->cmdMSP) { - case MSP_SET_RAW_RC: - for (i = 0; i < 8; i++) - rcData[i] = read16(); - headSerialReply(0); - mspFrameRecieve(); - break; - case MSP_SET_ACC_TRIM: - cfg.angleTrim[PITCH] = read16(); - cfg.angleTrim[ROLL] = read16(); - headSerialReply(0); - break; + case MSP_SET_RAW_RC: + for (i = 0; i < 8; i++) + rcData[i] = read16(); + headSerialReply(0); + mspFrameRecieve(); + break; + case MSP_SET_ACC_TRIM: + cfg.angleTrim[PITCH] = read16(); + cfg.angleTrim[ROLL] = read16(); + headSerialReply(0); + break; #ifdef GPS - case MSP_SET_RAW_GPS: - f.GPS_FIX = read8(); - GPS_numSat = read8(); - GPS_coord[LAT] = read32(); - GPS_coord[LON] = read32(); - GPS_altitude = read16(); - GPS_speed = read16(); - GPS_update |= 2; // New data signalisation to GPS functions - headSerialReply(0); - break; + case MSP_SET_RAW_GPS: + f.GPS_FIX = read8(); + GPS_numSat = read8(); + GPS_coord[LAT] = read32(); + GPS_coord[LON] = read32(); + GPS_altitude = read16(); + GPS_speed = read16(); + GPS_update |= 2; // New data signalisation to GPS functions + headSerialReply(0); + break; #endif - case MSP_SET_PID: - for (i = 0; i < PIDITEMS; i++) { - cfg.P8[i] = read8(); - cfg.I8[i] = read8(); - cfg.D8[i] = read8(); - } - headSerialReply(0); - break; - case MSP_SET_BOX: - for (i = 0; i < numberBoxItems; i++) - cfg.activate[availableBoxes[i]] = read16(); - headSerialReply(0); - break; - case MSP_SET_RC_TUNING: - cfg.rcRate8 = read8(); - cfg.rcExpo8 = read8(); - cfg.rollPitchRate = read8(); - cfg.yawRate = read8(); - cfg.dynThrPID = read8(); - cfg.thrMid8 = read8(); - cfg.thrExpo8 = read8(); - headSerialReply(0); - break; - case MSP_SET_MISC: - tmp = read16(); - // sanity check - if (tmp < 1600 && tmp > 1400) - mcfg.midrc = tmp; - mcfg.minthrottle = read16(); - mcfg.maxthrottle = read16(); - mcfg.mincommand = read16(); - cfg.failsafe_throttle = read16(); - mcfg.gps_type = read8(); - mcfg.gps_baudrate = read8(); - mcfg.gps_ubx_sbas = read8(); - mcfg.multiwiicurrentoutput = read8(); - mcfg.rssi_aux_channel = read8(); - read8(); - cfg.mag_declination = read16() * 10; - mcfg.vbatscale = read8(); // actual vbatscale as intended - mcfg.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI - mcfg.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI - mcfg.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert - headSerialReply(0); - break; - case MSP_SET_MOTOR: - for (i = 0; i < 8; i++) - motor_disarmed[i] = read16(); - 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(mcfg.mixerConfiguration); // type of multicopter - serialize8(MSP_VERSION); // MultiWii Serial Protocol Version - serialize32(CAP_PLATFORM_32BIT | CAP_BASEFLIGHT_CONFIG | CAP_DYNBALANCE | CAP_FW_FLAPS); // "capability" - break; - case MSP_STATUS: - 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); - // OK, so you waste all the fucking time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all - // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE FUCKING LOGIC IN THIS, FUCKWADS. - // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit - junk = 0; - tmp = 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 | - rcOptions[BOXTELEMETRY] << BOXTELEMETRY | - rcOptions[BOXSERVO1] << BOXSERVO1 | - rcOptions[BOXSERVO2] << BOXSERVO2 | - rcOptions[BOXSERVO3] << BOXSERVO3 | - f.ARMED << BOXARM; - for (i = 0; i < numberBoxItems; i++) { - int flag = (tmp & (1 << availableBoxes[i])); - if (flag) - junk |= 1 << i; - } - serialize32(junk); - serialize8(mcfg.current_profile); - break; - case MSP_RAW_IMU: - headSerialReply(18); - // Retarded hack until multiwiidorks start using real units for sensor data - if (acc_1G > 1024) { + case MSP_SET_PID: + for (i = 0; i < PIDITEMS; i++) { + cfg.P8[i] = read8(); + cfg.I8[i] = read8(); + cfg.D8[i] = read8(); + } + headSerialReply(0); + break; + case MSP_SET_BOX: + for (i = 0; i < numberBoxItems; i++) + cfg.activate[availableBoxes[i]] = read16(); + headSerialReply(0); + break; + case MSP_SET_RC_TUNING: + cfg.rcRate8 = read8(); + cfg.rcExpo8 = read8(); + cfg.rollPitchRate = read8(); + cfg.yawRate = read8(); + cfg.dynThrPID = read8(); + cfg.thrMid8 = read8(); + cfg.thrExpo8 = read8(); + headSerialReply(0); + break; + case MSP_SET_MISC: + tmp = read16(); + // sanity check + if (tmp < 1600 && tmp > 1400) + mcfg.midrc = tmp; + mcfg.minthrottle = read16(); + mcfg.maxthrottle = read16(); + mcfg.mincommand = read16(); + cfg.failsafe_throttle = read16(); + mcfg.gps_type = read8(); + mcfg.gps_baudrate = read8(); + mcfg.gps_ubx_sbas = read8(); + mcfg.multiwiicurrentoutput = read8(); + mcfg.rssi_aux_channel = read8(); + read8(); + cfg.mag_declination = read16() * 10; + mcfg.vbatscale = read8(); // actual vbatscale as intended + mcfg.vbatmincellvoltage = read8(); // vbatlevel_warn1 in MWC2.3 GUI + mcfg.vbatmaxcellvoltage = read8(); // vbatlevel_warn2 in MWC2.3 GUI + mcfg.vbatwarningcellvoltage = read8(); // vbatlevel when buzzer starts to alert + headSerialReply(0); + break; + case MSP_SET_MOTOR: + for (i = 0; i < 8; i++) + motor_disarmed[i] = read16(); + 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(mcfg.mixerConfiguration); // type of multicopter + serialize8(MSP_VERSION); // MultiWii Serial Protocol Version + serialize32(CAP_PLATFORM_32BIT | CAP_BASEFLIGHT_CONFIG | CAP_DYNBALANCE | CAP_FW_FLAPS); // "capability" + break; + case MSP_STATUS: + 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); + // OK, so you waste all the fucking time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all + // the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE FUCKING LOGIC IN THIS, FUCKWADS. + // Serialize the boxes in the order we delivered them, until multiwii retards fix their shit + junk = 0; + tmp = 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 | + rcOptions[BOXTELEMETRY] << BOXTELEMETRY | + rcOptions[BOXSERVO1] << BOXSERVO1 | + rcOptions[BOXSERVO2] << BOXSERVO2 | + rcOptions[BOXSERVO3] << BOXSERVO3 | + f.ARMED << BOXARM; + for (i = 0; i < numberBoxItems; i++) { + int flag = (tmp & (1 << availableBoxes[i])); + if (flag) + junk |= 1 << i; + } + serialize32(junk); + serialize8(mcfg.current_profile); + break; + case MSP_RAW_IMU: + headSerialReply(18); + // Retarded hack until multiwiidorks start using real units for sensor data + if (acc_1G > 1024) { + for (i = 0; i < 3; i++) + serialize16(accSmooth[i] / 8); + } else { + for (i = 0; i < 3; i++) + serialize16(accSmooth[i]); + } for (i = 0; i < 3; i++) - serialize16(accSmooth[i] / 8); - } else { + serialize16(gyroData[i]); for (i = 0; i < 3; i++) - serialize16(accSmooth[i]); - } - for (i = 0; i < 3; i++) - serialize16(gyroData[i]); - for (i = 0; i < 3; i++) - serialize16(magADC[i]); - break; - case MSP_SERVO: - s_struct((uint8_t *)&servo, 16); - break; - case MSP_SERVO_CONF: - headSerialReply(56); - for (i = 0; i < MAX_SERVOS; i++) { - serialize16(cfg.servoConf[i].min); - serialize16(cfg.servoConf[i].max); - serialize16(cfg.servoConf[i].middle); - serialize8(getOldServoConfig(i)); - } - break; - case MSP_SET_SERVO_CONF: - headSerialReply(0); - for (i = 0; i < MAX_SERVOS; i++) { - cfg.servoConf[i].min = read16(); - cfg.servoConf[i].max = read16(); - cfg.servoConf[i].middle = read16(); - tmp = read8(); - // trash old servo direction - cfg.servoConf[i].rate = tmp & 0xfc; - - // store old style servo direction depending on current mixer configuration - switch (mcfg.mixerConfiguration) { - case MULTITYPE_BI: - storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); - storeOldServoConfig(i, tmp, 5, 1, INPUT_PITCH); - - storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW); - storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW); - - break; - case MULTITYPE_TRI: - storeOldServoConfig(i, tmp, 5, 1, INPUT_YAW); - - break; - case MULTITYPE_FLYING_WING: - storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH); - storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); - - storeOldServoConfig(i, tmp, 3, 2, INPUT_ROLL); - storeOldServoConfig(i, tmp, 4, 2, INPUT_ROLL); - - break; - case MULTITYPE_DUALCOPTER: - storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); - storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL); - - break; - case MULTITYPE_SINGLECOPTER: - storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH); - storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); - storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL); - storeOldServoConfig(i, tmp, 6, 1, INPUT_ROLL); - - storeOldServoConfig(i, tmp, 3, 2, INPUT_YAW); - storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW); - storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW); - storeOldServoConfig(i, tmp, 6, 2, INPUT_YAW); - - break; + serialize16(magADC[i]); + break; + case MSP_SERVO: + s_struct((uint8_t *)&servo, 16); + break; + case MSP_SERVO_CONF: + headSerialReply(56); + for (i = 0; i < MAX_SERVOS; i++) { + serialize16(cfg.servoConf[i].min); + serialize16(cfg.servoConf[i].max); + serialize16(cfg.servoConf[i].middle); + serialize8(getOldServoConfig(i)); } - } - break; - case MSP_MOTOR: - s_struct((uint8_t *)motor, 16); - break; - case MSP_RC: - headSerialReply(16); - for (i = 0; i < 8; i++) - serialize16(rcData[i]); - break; + break; + case MSP_SET_SERVO_CONF: + headSerialReply(0); + for (i = 0; i < MAX_SERVOS; i++) { + cfg.servoConf[i].min = read16(); + cfg.servoConf[i].max = read16(); + cfg.servoConf[i].middle = read16(); + tmp = read8(); + // trash old servo direction + cfg.servoConf[i].rate = tmp & 0xfc; + + // store old style servo direction depending on current mixer configuration + switch (mcfg.mixerConfiguration) { + case MULTITYPE_BI: + storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); + storeOldServoConfig(i, tmp, 5, 1, INPUT_PITCH); + + storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW); + storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW); + + break; + case MULTITYPE_TRI: + storeOldServoConfig(i, tmp, 5, 1, INPUT_YAW); + + break; + case MULTITYPE_FLYING_WING: + storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH); + storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); + + storeOldServoConfig(i, tmp, 3, 2, INPUT_ROLL); + storeOldServoConfig(i, tmp, 4, 2, INPUT_ROLL); + + break; + case MULTITYPE_DUALCOPTER: + storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); + storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL); + + break; + case MULTITYPE_SINGLECOPTER: + storeOldServoConfig(i, tmp, 3, 1, INPUT_PITCH); + storeOldServoConfig(i, tmp, 4, 1, INPUT_PITCH); + storeOldServoConfig(i, tmp, 5, 1, INPUT_ROLL); + storeOldServoConfig(i, tmp, 6, 1, INPUT_ROLL); + + storeOldServoConfig(i, tmp, 3, 2, INPUT_YAW); + storeOldServoConfig(i, tmp, 4, 2, INPUT_YAW); + storeOldServoConfig(i, tmp, 5, 2, INPUT_YAW); + storeOldServoConfig(i, tmp, 6, 2, INPUT_YAW); + + break; + } + } + break; + case MSP_MOTOR: + s_struct((uint8_t *)motor, 16); + break; + case MSP_RC: + headSerialReply(16); + for (i = 0; i < 8; i++) + serialize16(rcData[i]); + break; #ifdef GPS - case MSP_RAW_GPS: - 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); - serialize16(GPS_distanceToHome); - serialize16(GPS_directionToHome); - serialize8(GPS_update & 1); - break; + case MSP_RAW_GPS: + 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); + serialize16(GPS_distanceToHome); + serialize16(GPS_directionToHome); + serialize8(GPS_update & 1); + break; #endif - case MSP_ATTITUDE: - headSerialReply(6); - for (i = 0; i < 2; i++) - serialize16(angle[i]); - serialize16(heading); - break; - case MSP_ALTITUDE: - headSerialReply(6); - serialize32(EstAlt); - serialize16(vario); - break; - case MSP_ANALOG: - headSerialReply(7); - serialize8((uint8_t)constrain(vbat, 0, 255)); - serialize16((uint16_t)constrain(mAhdrawn, 0, 0xFFFF)); // milliamphours drawn from battery - serialize16(rssi); - if (mcfg.multiwiicurrentoutput) - serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps - else - serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps - break; - case MSP_RC_TUNING: - headSerialReply(7); - serialize8(cfg.rcRate8); - serialize8(cfg.rcExpo8); - serialize8(cfg.rollPitchRate); - serialize8(cfg.yawRate); - serialize8(cfg.dynThrPID); - serialize8(cfg.thrMid8); - serialize8(cfg.thrExpo8); - break; - case MSP_PID: - headSerialReply(3 * PIDITEMS); - for (i = 0; i < PIDITEMS; i++) { - serialize8(cfg.P8[i]); - serialize8(cfg.I8[i]); - serialize8(cfg.D8[i]); - } - break; - case MSP_PIDNAMES: - headSerialReply(sizeof(pidnames) - 1); - serializeNames(pidnames); - break; - case MSP_BOX: - headSerialReply(2 * numberBoxItems); - for (i = 0; i < numberBoxItems; i++) - serialize16(cfg.activate[availableBoxes[i]]); - break; - case MSP_BOXNAMES: - // headSerialReply(sizeof(boxnames) - 1); - serializeBoxNamesReply(); - break; - case MSP_BOXIDS: - headSerialReply(numberBoxItems); - for (i = 0; i < numberBoxItems; i++) { - for (j = 0; j < CHECKBOXITEMS; j++) { - if (boxes[j].permanentId == availableBoxes[i]) - serialize8(boxes[j].permanentId); + case MSP_ATTITUDE: + headSerialReply(6); + for (i = 0; i < 2; i++) + serialize16(angle[i]); + serialize16(heading); + break; + case MSP_ALTITUDE: + headSerialReply(6); + serialize32(EstAlt); + serialize16(vario); + break; + case MSP_ANALOG: + headSerialReply(7); + serialize8((uint8_t)constrain(vbat, 0, 255)); + serialize16((uint16_t)constrain(mAhdrawn, 0, 0xFFFF)); // milliamphours drawn from battery + serialize16(rssi); + if (mcfg.multiwiicurrentoutput) + serialize16((uint16_t)constrain((abs(amperage) * 10), 0, 0xFFFF)); // send amperage in 0.001 A steps + else + serialize16((uint16_t)constrain(abs(amperage), 0, 0xFFFF)); // send amperage in 0.01 A steps + break; + case MSP_RC_TUNING: + headSerialReply(7); + serialize8(cfg.rcRate8); + serialize8(cfg.rcExpo8); + serialize8(cfg.rollPitchRate); + serialize8(cfg.yawRate); + serialize8(cfg.dynThrPID); + serialize8(cfg.thrMid8); + serialize8(cfg.thrExpo8); + break; + case MSP_PID: + headSerialReply(3 * PIDITEMS); + for (i = 0; i < PIDITEMS; i++) { + serialize8(cfg.P8[i]); + serialize8(cfg.I8[i]); + serialize8(cfg.D8[i]); } - } - break; - case MSP_MISC: - headSerialReply(2 * 6 + 4 + 2 + 4); - serialize16(mcfg.midrc); - serialize16(mcfg.minthrottle); - serialize16(mcfg.maxthrottle); - serialize16(mcfg.mincommand); - serialize16(cfg.failsafe_throttle); - serialize8(mcfg.gps_type); - serialize8(mcfg.gps_baudrate); - serialize8(mcfg.gps_ubx_sbas); - serialize8(mcfg.multiwiicurrentoutput); - serialize8(mcfg.rssi_aux_channel); - serialize8(0); - serialize16(cfg.mag_declination / 10); // TODO check this shit - serialize8(mcfg.vbatscale); - serialize8(mcfg.vbatmincellvoltage); - serialize8(mcfg.vbatmaxcellvoltage); - serialize8(mcfg.vbatwarningcellvoltage); - break; - case MSP_MOTOR_PINS: - headSerialReply(8); - for (i = 0; i < 8; i++) - serialize8(i + 1); - break; + break; + case MSP_PIDNAMES: + headSerialReply(sizeof(pidnames) - 1); + serializeNames(pidnames); + break; + case MSP_BOX: + headSerialReply(2 * numberBoxItems); + for (i = 0; i < numberBoxItems; i++) + serialize16(cfg.activate[availableBoxes[i]]); + break; + case MSP_BOXNAMES: + // headSerialReply(sizeof(boxnames) - 1); + serializeBoxNamesReply(); + break; + case MSP_BOXIDS: + headSerialReply(numberBoxItems); + for (i = 0; i < numberBoxItems; i++) { + for (j = 0; j < CHECKBOXITEMS; j++) { + if (boxes[j].permanentId == availableBoxes[i]) + serialize8(boxes[j].permanentId); + } + } + break; + case MSP_MISC: + headSerialReply(2 * 6 + 4 + 2 + 4); + serialize16(mcfg.midrc); + serialize16(mcfg.minthrottle); + serialize16(mcfg.maxthrottle); + serialize16(mcfg.mincommand); + serialize16(cfg.failsafe_throttle); + serialize8(mcfg.gps_type); + serialize8(mcfg.gps_baudrate); + serialize8(mcfg.gps_ubx_sbas); + serialize8(mcfg.multiwiicurrentoutput); + serialize8(mcfg.rssi_aux_channel); + serialize8(0); + serialize16(cfg.mag_declination / 10); // TODO check this shit + serialize8(mcfg.vbatscale); + serialize8(mcfg.vbatmincellvoltage); + serialize8(mcfg.vbatmaxcellvoltage); + serialize8(mcfg.vbatwarningcellvoltage); + break; + case MSP_MOTOR_PINS: + headSerialReply(8); + for (i = 0; i < 8; i++) + serialize8(i + 1); + break; #ifdef GPS - case MSP_WP: - wp_no = read8(); // get the wp number - headSerialReply(18); - if (wp_no == 0) { - lat = GPS_home[LAT]; - lon = GPS_home[LON]; - } else if (wp_no == 16) { - 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_WP: + wp_no = read8(); // get the wp number + headSerialReply(18); + if (wp_no == 0) { + lat = GPS_home[LAT]; + lon = GPS_home[LON]; + } else if (wp_no == 16) { + 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; #endif /* GPS */ - case MSP_RESET_CONF: - if (!f.ARMED) - checkFirstTime(true); - headSerialReply(0); - break; - case MSP_ACC_CALIBRATION: - if (!f.ARMED) - calibratingA = CALIBRATING_ACC_CYCLES; - headSerialReply(0); - break; - case MSP_MAG_CALIBRATION: - if (!f.ARMED) - f.CALIBRATE_MAG = 1; - headSerialReply(0); - break; - case MSP_EEPROM_WRITE: - if (f.ARMED) { - headSerialError(0); - } else { - writeEEPROM(0, true); + case MSP_RESET_CONF: + if (!f.ARMED) + checkFirstTime(true); headSerialReply(0); - } - break; - case MSP_DEBUG: - headSerialReply(8); - // make use of this crap, output some useful QA statistics - debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] - 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 - 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; + break; + case MSP_ACC_CALIBRATION: + if (!f.ARMED) + calibratingA = CALIBRATING_ACC_CYCLES; + headSerialReply(0); + break; + case MSP_MAG_CALIBRATION: + if (!f.ARMED) + f.CALIBRATE_MAG = 1; + headSerialReply(0); + break; + case MSP_EEPROM_WRITE: + if (f.ARMED) { + headSerialError(0); + } else { + writeEEPROM(0, true); + headSerialReply(0); + } + break; + case MSP_DEBUG: + headSerialReply(8); + // make use of this crap, output some useful QA statistics + debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] + 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 + 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; #ifdef GPS - case MSP_GPSSVINFO: - headSerialReply(1 + (GPS_numCh * 4)); - serialize8(GPS_numCh); - for (i = 0; i < GPS_numCh; i++){ - serialize8(GPS_svinfo_chn[i]); - serialize8(GPS_svinfo_svid[i]); - serialize8(GPS_svinfo_quality[i]); - serialize8(GPS_svinfo_cno[i]); - } - // Poll new SVINFO from GPS - gpsPollSvinfo(); - break; - case MSP_GPSDEBUGINFO: - headSerialReply(16); - if (sensors(SENSOR_GPS)) { - serialize32(GPS_update_rate[1] - GPS_update_rate[0]); - serialize32(GPS_svinfo_rate[1] - GPS_svinfo_rate[0]); - } else { - serialize32(0); - serialize32(0); - } - serialize32(GPS_HorizontalAcc); - serialize32(GPS_VerticalAcc); - break; + case MSP_GPSSVINFO: + headSerialReply(1 + (GPS_numCh * 4)); + serialize8(GPS_numCh); + for (i = 0; i < GPS_numCh; i++) { + serialize8(GPS_svinfo_chn[i]); + serialize8(GPS_svinfo_svid[i]); + serialize8(GPS_svinfo_quality[i]); + serialize8(GPS_svinfo_cno[i]); + } + // Poll new SVINFO from GPS + gpsPollSvinfo(); + break; + case MSP_GPSDEBUGINFO: + headSerialReply(16); + if (sensors(SENSOR_GPS)) { + serialize32(GPS_update_rate[1] - GPS_update_rate[0]); + serialize32(GPS_svinfo_rate[1] - GPS_svinfo_rate[0]); + } else { + serialize32(0); + serialize32(0); + } + serialize32(GPS_HorizontalAcc); + serialize32(GPS_VerticalAcc); + break; #endif /* GPS */ - case MSP_SET_CONFIG: - headSerialReply(0); - mcfg.mixerConfiguration = read8(); // multitype - featureClearAll(); - featureSet(read32()); // features bitmap - mcfg.serialrx_type = read8(); // serialrx_type - mcfg.board_align_roll = read16(); // board_align_roll - mcfg.board_align_pitch = read16(); // board_align_pitch - mcfg.board_align_yaw = read16(); // board_align_yaw - mcfg.currentscale = read16(); - mcfg.currentoffset = read16(); - /// ??? - break; - case MSP_CONFIG: - headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 4); - serialize8(mcfg.mixerConfiguration); - serialize32(featureMask()); - serialize8(mcfg.serialrx_type); - serialize16(mcfg.board_align_roll); - serialize16(mcfg.board_align_pitch); - serialize16(mcfg.board_align_yaw); - serialize16(mcfg.currentscale); - serialize16(mcfg.currentoffset); - /// ??? - break; - - case MSP_RCMAP: - headSerialReply(MAX_INPUTS); // TODO fix this - for (i = 0; i < MAX_INPUTS; i++) - serialize8(mcfg.rcmap[i]); - break; - case MSP_SET_RCMAP: - headSerialReply(0); - for (i = 0; i < MAX_INPUTS; i++) - mcfg.rcmap[i] = read8(); - break; - - case MSP_REBOOT: - headSerialReply(0); - pendReboot = true; - break; - - case MSP_BUILDINFO: - headSerialReply(11 + 4 + 4); - for (i = 0; i < 11; i++) - serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc - serialize32(0); // future exp - serialize32(0); // future exp - break; - - default: // we do not know how to handle the (valid) message, indicate error MSP $M! - headSerialError(0); - break; + case MSP_SET_CONFIG: + headSerialReply(0); + mcfg.mixerConfiguration = read8(); // multitype + featureClearAll(); + featureSet(read32()); // features bitmap + mcfg.serialrx_type = read8(); // serialrx_type + mcfg.board_align_roll = read16(); // board_align_roll + mcfg.board_align_pitch = read16(); // board_align_pitch + mcfg.board_align_yaw = read16(); // board_align_yaw + mcfg.currentscale = read16(); + mcfg.currentoffset = read16(); + /// ??? + break; + case MSP_CONFIG: + headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 4); + serialize8(mcfg.mixerConfiguration); + serialize32(featureMask()); + serialize8(mcfg.serialrx_type); + serialize16(mcfg.board_align_roll); + serialize16(mcfg.board_align_pitch); + serialize16(mcfg.board_align_yaw); + serialize16(mcfg.currentscale); + serialize16(mcfg.currentoffset); + /// ??? + break; + + case MSP_RCMAP: + headSerialReply(MAX_INPUTS); // TODO fix this + for (i = 0; i < MAX_INPUTS; i++) + serialize8(mcfg.rcmap[i]); + break; + case MSP_SET_RCMAP: + headSerialReply(0); + for (i = 0; i < MAX_INPUTS; i++) + mcfg.rcmap[i] = read8(); + break; + + case MSP_REBOOT: + headSerialReply(0); + pendReboot = true; + break; + + case MSP_BUILDINFO: + headSerialReply(11 + 4 + 4); + for (i = 0; i < 11; i++) + serialize8(build[i]); // MMM DD YYYY as ascii, MMM = Jan/Feb... etc + serialize32(0); // future exp + serialize32(0); // future exp + break; + + default: // we do not know how to handle the (valid) message, indicate error MSP $M! + headSerialError(0); + break; } tailSerialReply(); } diff --git a/src/spektrum.c b/src/spektrum.c index dbc68b25..d2446887 100644 --- a/src/spektrum.c +++ b/src/spektrum.c @@ -111,7 +111,7 @@ static uint16_t spektrumReadRawRC(uint8_t chan) * Function must be called immediately after startup so that we don't miss satellite bind window. * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX. * 9 = DSMX 11ms / DSMX 22ms - * 5 = DSM2 11ms 2048 / DSM2 22ms 1024 + * 5 = DSM2 11ms 2048 / DSM2 22ms 1024 */ void spektrumBind(void) { diff --git a/src/telemetry_hott.c b/src/telemetry_hott.c index 37a6c120..083c33fa 100644 --- a/src/telemetry_hott.c +++ b/src/telemetry_hott.c @@ -82,7 +82,7 @@ void hottV4FormatAndSendGPSResponse(void) hottV4GPSUpdate(); - hottV4Respond((uint8_t*)&HoTTV4GPSModule, sizeof(HoTTV4GPSModule)); + hottV4Respond((uint8_t *)&HoTTV4GPSModule, sizeof(HoTTV4GPSModule)); } void hottV4GPSUpdate(void) @@ -212,7 +212,7 @@ void hottV4FormatAndSendEAMResponse(void) HoTTV4ElectricAirModule.m2s = OFFSET_M2S; HoTTV4ElectricAirModule.m3s = OFFSET_M3S; - hottV4Respond((uint8_t*)&HoTTV4ElectricAirModule, sizeof(HoTTV4ElectricAirModule)); + hottV4Respond((uint8_t *)&HoTTV4ElectricAirModule, sizeof(HoTTV4ElectricAirModule)); } static void hottV4Respond(uint8_t *data, uint8_t size) @@ -264,13 +264,13 @@ void handleHoTTTelemetry(void) delay(5); switch (c) { - case 0x8A: + case 0x8A: if (sensors(SENSOR_GPS)) hottV4FormatAndSendGPSResponse(); - break; - case 0x8E: - hottV4FormatAndSendEAMResponse(); - break; + break; + case 0x8E: + hottV4FormatAndSendEAMResponse(); + break; } } }