From 033a8fbe3ffffc8cd554f25cc83f54acebc7728e Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 1 Jan 2014 09:16:41 +0100 Subject: [PATCH 1/7] ident/format --- src/board.h | 147 +++++++++++++++++++++++++--------------------------- 1 file changed, 71 insertions(+), 76 deletions(-) diff --git a/src/board.h b/src/board.h index 47bc77d770..0c6b540453 100755 --- a/src/board.h +++ b/src/board.h @@ -40,42 +40,42 @@ #define U_ID_2 (*(uint32_t*)0x1FFFF7F0) typedef enum { - SENSOR_GYRO = 1 << 0, // always present - SENSOR_ACC = 1 << 1, - SENSOR_BARO = 1 << 2, - SENSOR_MAG = 1 << 3, - SENSOR_SONAR = 1 << 4, - SENSOR_GPS = 1 << 5, - SENSOR_GPSMAG = 1 << 6, + SENSOR_GYRO = 1 << 0, // always present + SENSOR_ACC = 1 << 1, + SENSOR_BARO = 1 << 2, + SENSOR_MAG = 1 << 3, + SENSOR_SONAR = 1 << 4, + SENSOR_GPS = 1 << 5, + SENSOR_GPSMAG = 1 << 6, } AvailableSensors; // Type of accelerometer used/detected typedef enum AccelSensors { - ACC_DEFAULT = 0, - ACC_ADXL345 = 1, - ACC_MPU6050 = 2, - ACC_MMA8452 = 3, - ACC_BMA280 = 4, - ACC_NONE = 5 + ACC_DEFAULT = 0, + ACC_ADXL345 = 1, + ACC_MPU6050 = 2, + ACC_MMA8452 = 3, + ACC_BMA280 = 4, + ACC_NONE = 5 } AccelSensors; typedef enum { - FEATURE_PPM = 1 << 0, - FEATURE_VBAT = 1 << 1, - FEATURE_INFLIGHT_ACC_CAL = 1 << 2, - FEATURE_SERIALRX = 1 << 3, - FEATURE_MOTOR_STOP = 1 << 4, - FEATURE_SERVO_TILT = 1 << 5, - FEATURE_GYRO_SMOOTHING = 1 << 6, - FEATURE_LED_RING = 1 << 7, - FEATURE_GPS = 1 << 8, - FEATURE_FAILSAFE = 1 << 9, - FEATURE_SONAR = 1 << 10, - FEATURE_TELEMETRY = 1 << 11, - FEATURE_POWERMETER = 1 << 12, - FEATURE_VARIO = 1 << 13, - FEATURE_3D = 1 << 14, - FEATURE_SOFTSERIAL = 1 << 15, + FEATURE_PPM = 1 << 0, + FEATURE_VBAT = 1 << 1, + FEATURE_INFLIGHT_ACC_CAL = 1 << 2, + FEATURE_SERIALRX = 1 << 3, + FEATURE_MOTOR_STOP = 1 << 4, + FEATURE_SERVO_TILT = 1 << 5, + FEATURE_GYRO_SMOOTHING = 1 << 6, + FEATURE_LED_RING = 1 << 7, + FEATURE_GPS = 1 << 8, + FEATURE_FAILSAFE = 1 << 9, + FEATURE_SONAR = 1 << 10, + FEATURE_TELEMETRY = 1 << 11, + FEATURE_POWERMETER = 1 << 12, + FEATURE_VARIO = 1 << 13, + FEATURE_3D = 1 << 14, + FEATURE_SOFTSERIAL = 1 << 15, } AvailableFeatures; typedef enum { @@ -104,63 +104,60 @@ typedef enum { } sensor_axis_e; typedef enum { - ALIGN_DEFAULT = 0, // driver-provided alignment - CW0_DEG = 1, - CW90_DEG = 2, - CW180_DEG = 3, - CW270_DEG = 4, - CW0_DEG_FLIP = 5, - CW90_DEG_FLIP = 6, - CW180_DEG_FLIP = 7, - CW270_DEG_FLIP = 8 + ALIGN_DEFAULT = 0, // driver-provided alignment + CW0_DEG = 1, + CW90_DEG = 2, + CW180_DEG = 3, + CW270_DEG = 4, + CW0_DEG_FLIP = 5, + CW90_DEG_FLIP = 6, + CW180_DEG_FLIP = 7, + CW270_DEG_FLIP = 8 } sensor_align_e; enum { - GYRO_UPDATED = 1 << 0, - ACC_UPDATED = 1 << 1, - MAG_UPDATED = 1 << 2, - TEMP_UPDATED = 1 << 3 + GYRO_UPDATED = 1 << 0, + ACC_UPDATED = 1 << 1, + MAG_UPDATED = 1 << 2, + TEMP_UPDATED = 1 << 3 }; -typedef struct sensor_data_t -{ - int16_t gyro[3]; - int16_t acc[3]; - int16_t mag[3]; - float temperature; - int updated; +typedef struct sensor_data_t { + int16_t gyro[3]; + int16_t acc[3]; + int16_t mag[3]; + float temperature; + int updated; } sensor_data_t; -typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype -typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype -typedef void (* baroOpFuncPtr)(void); // baro start operation -typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) -typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app -typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data -typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype +typedef void (*sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype +typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype +typedef void (*baroOpFuncPtr)(void); // baro start operation +typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) +typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app +typedef uint16_t (*rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data +typedef void (*pidControllerFuncPtr)(void); // pid controller function prototype -typedef struct sensor_t -{ - sensorInitFuncPtr init; // initialize function - sensorReadFuncPtr read; // read 3 axis data function - sensorReadFuncPtr temperature; // read temperature if available - float scale; // scalefactor (currently used for gyro only, todo for accel) +typedef struct sensor_t { + sensorInitFuncPtr init; // initialize function + sensorReadFuncPtr read; // read 3 axis data function + sensorReadFuncPtr temperature; // read temperature if available + float scale; // scalefactor (currently used for gyro only, todo for accel) } sensor_t; -typedef struct baro_t -{ - uint16_t ut_delay; - uint16_t up_delay; - baroOpFuncPtr start_ut; - baroOpFuncPtr get_ut; - baroOpFuncPtr start_up; - baroOpFuncPtr get_up; - baroCalculateFuncPtr calculate; +typedef struct baro_t { + uint16_t ut_delay; + uint16_t up_delay; + baroOpFuncPtr start_ut; + baroOpFuncPtr get_ut; + baroOpFuncPtr start_up; + baroOpFuncPtr get_up; + baroCalculateFuncPtr calculate; } baro_t; // Hardware definitions and GPIO #ifdef FY90Q - // FY90Q +// FY90Q #define LED0_GPIO GPIOC #define LED0_PIN Pin_12 #define LED1_GPIO GPIOA @@ -181,7 +178,6 @@ typedef struct baro_t #define LED0_PIN Pin_1 // D3, PA1/USART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow #define LED1_GPIO GPIOA #define LED1_PIN Pin_5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green - #define GYRO #define ACC @@ -231,11 +227,10 @@ typedef struct baro_t #endif #undef SOFT_I2C // enable to test software i2c - #include "utils.h" #ifdef FY90Q - // FY90Q +// FY90Q #include "drv_adc.h" #include "drv_i2c.h" #include "drv_pwm.h" @@ -258,7 +253,7 @@ typedef struct baro_t #include "drv_softserial.h" #else - // AfroFlight32 +// AfroFlight32 #include "drv_adc.h" #include "drv_adxl345.h" #include "drv_bma280.h" From c76a9669f65d099365ddc631d10bff63c0fe4821 Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 1 Jan 2014 09:17:13 +0100 Subject: [PATCH 2/7] mixer coorection for hexa6x and hexa6p --- src/mixer.c | 978 ++++++++++++++++++++++++++-------------------------- 1 file changed, 489 insertions(+), 489 deletions(-) diff --git a/src/mixer.c b/src/mixer.c index f0c0ecf2c5..0cb5ee31e7 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -1,498 +1,498 @@ -#include "board.h" -#include "mw.h" - -static uint8_t numberMotor = 0; -int16_t motor[MAX_MOTORS]; -int16_t motor_disarmed[MAX_MOTORS]; -int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; - -static motorMixer_t currentMixer[MAX_MOTORS]; - -static const motorMixer_t mixerTri[] = { +#include "board.h" +#include "mw.h" + +static uint8_t numberMotor = 0; +int16_t motor[MAX_MOTORS]; +int16_t motor_disarmed[MAX_MOTORS]; +int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; + +static motorMixer_t currentMixer[MAX_MOTORS]; + +static const motorMixer_t mixerTri[] = { { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT -}; - -static const motorMixer_t mixerQuadP[] = { - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR - { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT - { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT - { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT -}; - -static const motorMixer_t mixerQuadX[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L -}; - -static const motorMixer_t mixerBi[] = { - { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT - { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT -}; - -static const motorMixer_t mixerY6[] = { +}; + +static const motorMixer_t mixerQuadP[] = { + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR + { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT + { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT + { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT +}; + +static const motorMixer_t mixerQuadX[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L +}; + +static const motorMixer_t mixerBi[] = { + { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT + { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT +}; + +static const motorMixer_t mixerY6[] = { { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT -}; - -static const motorMixer_t mixerHex6P[] = { - { 1.0f, -1.0f, 0.866025f, 1.0f }, // REAR_R - { 1.0f, -1.0f, -0.866025f, -1.0f }, // FRONT_R - { 1.0f, 1.0f, 0.866025f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -0.866025f, -1.0f }, // FRONT_L - { 1.0f, 0.0f, -0.866025f, 1.0f }, // FRONT - { 1.0f, 0.0f, 0.866025f, -1.0f }, // REAR -}; - -static const motorMixer_t mixerY4[] = { - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW - { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW - { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW - { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW -}; - -static const motorMixer_t mixerHex6X[] = { - { 1.0f, -0.866025f, 1.0f, 1.0f }, // REAR_R - { 1.0f, -0.866025f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 0.866025f, 1.0f, -1.0f }, // REAR_L - { 1.0f, 0.866025f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -0.866025f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 0.866025f, 0.0f, 1.0f }, // LEFT -}; - -static const motorMixer_t mixerOctoX8[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R - { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R - { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L - { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L -}; - -static const motorMixer_t mixerOctoFlatP[] = { - { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L - { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R - { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R - { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L - { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT - { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR - { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT -}; - -static const motorMixer_t mixerOctoFlatX[] = { - { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L - { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R - { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R - { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R - { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L -}; - -static const motorMixer_t mixerVtail4[] = { - { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L -}; - -static const motorMixer_t mixerHex6H[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT - { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT -}; - -static const motorMixer_t mixerDualcopter[] = { - { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT - { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT -}; - -// Keep this synced with MultiType struct in mw.h! -const mixer_t mixers[] = { -// Mo Se Mixtable - { 0, 0, NULL }, // entry 0 - { 3, 1, mixerTri }, // MULTITYPE_TRI - { 4, 0, mixerQuadP }, // MULTITYPE_QUADP - { 4, 0, mixerQuadX }, // MULTITYPE_QUADX - { 2, 1, mixerBi }, // MULTITYPE_BI - { 0, 1, NULL }, // * MULTITYPE_GIMBAL - { 6, 0, mixerY6 }, // MULTITYPE_Y6 - { 6, 0, mixerHex6P }, // MULTITYPE_HEX6 - { 1, 1, NULL }, // * MULTITYPE_FLYING_WING - { 4, 0, mixerY4 }, // MULTITYPE_Y4 - { 6, 0, mixerHex6X }, // MULTITYPE_HEX6X - { 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8 - { 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP - { 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX - { 1, 1, NULL }, // * MULTITYPE_AIRPLANE - { 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM - { 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG - { 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4 - { 6, 0, mixerHex6H }, // MULTITYPE_HEX6H - { 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO - { 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER - { 1, 1, NULL }, // MULTITYPE_SINGLECOPTER - { 0, 0, NULL }, // MULTITYPE_CUSTOM -}; - -int16_t servoMiddle(int nr) -{ - // Normally, servo.middle is a value between 1000..2000, but for the purposes of stupid, if it's less than - // the number of RC channels, it means the center value is taken FROM that RC channel (by its index) - if (cfg.servoConf[nr].middle < RC_CHANS && nr < MAX_SERVOS) - return rcData[cfg.servoConf[nr].middle]; - else - return cfg.servoConf[nr].middle; -} - -int servoDirection(int nr, int lr) -{ - // servo.rate is overloaded for servos that don't have a rate, but only need direction - // bit set = negative, clear = positive - // rate[2] = ???_direction - // rate[1] = roll_direction - // rate[0] = pitch_direction - // servo.rate is also used as gimbal gain multiplier (yeah) - if (cfg.servoConf[nr].rate & lr) - return -1; - else - return 1; -} - -void mixerInit(void) -{ - int i; - - // enable servos for mixes that require them. note, this shifts motor counts. - core.useServo = mixers[mcfg.mixerConfiguration].useServo; - // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (feature(FEATURE_SERVO_TILT)) - core.useServo = 1; - - if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) { - // load custom mixer into currentMixer - for (i = 0; i < MAX_MOTORS; i++) { - // check if done - if (mcfg.customMixer[i].throttle == 0.0f) - break; - currentMixer[i] = mcfg.customMixer[i]; - numberMotor++; - } - } else { - numberMotor = mixers[mcfg.mixerConfiguration].numberMotor; - // copy motor-based mixers - if (mixers[mcfg.mixerConfiguration].motor) { - for (i = 0; i < numberMotor; i++) - currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; - } - } - - // in 3D mode, mixer gain has to be halved - if (feature(FEATURE_3D)) { - if (numberMotor > 1) { - for (i = 0; i < numberMotor; i++) { - currentMixer[i].pitch *= 0.5f; - currentMixer[i].roll *= 0.5f; - currentMixer[i].yaw *= 0.5f; - } - } - } - mixerResetMotors(); -} - -void mixerResetMotors(void) -{ - int i; - // set disarmed motor values - for (i = 0; i < MAX_MOTORS; i++) - motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; -} - -void mixerLoadMix(int index) -{ - int i; - - // we're 1-based - index++; - // clear existing - for (i = 0; i < MAX_MOTORS; i++) - 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++) - mcfg.customMixer[i] = mixers[index].motor[i]; - } -} - -void writeServos(void) -{ - if (!core.useServo) - return; - - switch (mcfg.mixerConfiguration) { - case MULTITYPE_BI: - pwmWriteServo(0, servo[4]); - pwmWriteServo(1, servo[5]); - break; - - case MULTITYPE_TRI: - if (cfg.tri_unarmed_servo) { - // if unarmed flag set, we always move servo - pwmWriteServo(0, servo[5]); - } else { - // otherwise, only move servo when copter is armed - if (f.ARMED) - pwmWriteServo(0, servo[5]); - else - pwmWriteServo(0, 0); // kill servo signal completely. - } - break; - - case MULTITYPE_FLYING_WING: - pwmWriteServo(0, servo[3]); - pwmWriteServo(1, servo[4]); - break; - - case MULTITYPE_GIMBAL: - pwmWriteServo(0, servo[0]); - pwmWriteServo(1, servo[1]); - break; - - case MULTITYPE_DUALCOPTER: - pwmWriteServo(0, servo[4]); - pwmWriteServo(1, servo[5]); - break; - - case MULTITYPE_AIRPLANE: - case MULTITYPE_SINGLECOPTER: - pwmWriteServo(0, servo[3]); - pwmWriteServo(1, servo[4]); - pwmWriteServo(2, servo[5]); - pwmWriteServo(3, servo[6]); - break; - - default: - // Two servos for SERVO_TILT, if enabled - if (feature(FEATURE_SERVO_TILT)) { - pwmWriteServo(0, servo[0]); - pwmWriteServo(1, servo[1]); - } - break; - } -} - -extern uint8_t cliMode; - -void writeMotors(void) -{ - uint8_t i; - - for (i = 0; i < numberMotor; i++) - pwmWriteMotor(i, motor[i]); -} - -void writeAllMotors(int16_t mc) -{ - uint8_t i; - - // Sends commands to all motors - for (i = 0; i < numberMotor; i++) - motor[i] = mc; - writeMotors(); -} - -static void airplaneMixer(void) -{ - int16_t flapperons[2] = { 0, 0 }; - int i; - - if (!f.ARMED) - servo[7] = mcfg.mincommand; // Kill throttle when disarmed - else - servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); - motor[0] = servo[7]; - -#if 0 - if (cfg.flaperons) { - - - } -#endif - - if (mcfg.flaps_speed) { - // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control - // use servo min, servo max and servo rate for proper endpoints adjust - static int16_t slow_LFlaps; - int16_t lFlap = servoMiddle(2); - - lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); - lFlap = mcfg.midrc - lFlap; // shouldn't this be servoConf[2].middle? - if (slow_LFlaps < lFlap) - slow_LFlaps += mcfg.flaps_speed; - else if (slow_LFlaps > lFlap) - slow_LFlaps -= mcfg.flaps_speed; - - servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L; - servo[2] += mcfg.midrc; - } - - if (f.PASSTHRU_MODE) { // Direct passthru from RX - servo[3] = rcCommand[ROLL] + flapperons[0]; // Wing 1 - servo[4] = rcCommand[ROLL] + flapperons[1]; // Wing 2 - servo[5] = rcCommand[YAW]; // Rudder - servo[6] = rcCommand[PITCH]; // Elevator - } else { - // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui - servo[3] = axisPID[ROLL] + flapperons[0]; // Wing 1 - servo[4] = axisPID[ROLL] + flapperons[1]; // Wing 2 - servo[5] = axisPID[YAW]; // Rudder - servo[6] = axisPID[PITCH]; // Elevator - } - for (i = 3; i < 7; i++) { - servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates - servo[i] += servoMiddle(i); - } -} - -void mixTable(void) -{ - int16_t maxMotor; - uint32_t i; - - if (numberMotor > 3) { - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); - } - - // motors for non-servo mixes - if (numberMotor > 1) - for (i = 0; i < numberMotor; i++) - 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 (mcfg.mixerConfiguration) { - case MULTITYPE_BI: - servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + servoMiddle(4); // LEFT - servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + servoMiddle(5); // RIGHT - break; - - case MULTITYPE_TRI: - servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR - break; - - case MULTITYPE_GIMBAL: - servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0); - servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1); - break; - - case MULTITYPE_AIRPLANE: - airplaneMixer(); - break; - - case MULTITYPE_FLYING_WING: - if (!f.ARMED) - servo[7] = mcfg.mincommand; - else - servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); - motor[0] = servo[7]; - if (f.PASSTHRU_MODE) { - // do not use sensors for correction, simple 2 channel mixing - servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]); - servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]); - } else { - // use sensors to correct (gyro only or gyro + acc) - servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]); - servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]); - } - servo[3] += servoMiddle(3); - servo[4] += servoMiddle(4); - break; - - case MULTITYPE_DUALCOPTER: - for (i = 4; i < 6; i++ ) { - servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction - servo[i] += servoMiddle(i); - } - break; - - case MULTITYPE_SINGLECOPTER: - for (i = 3; i < 7; i++) { - servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction - servo[i] += servoMiddle(i); - } - motor[0] = rcCommand[THROTTLE]; - break; - } - - // do camstab - if (feature(FEATURE_SERVO_TILT)) { - // center at fixed position, or vary either pitch or roll by RC channel - servo[0] = servoMiddle(0); - servo[1] = servoMiddle(1); - - if (rcOptions[BOXCAMSTAB]) { - if (cfg.gimbal_flags & GIMBAL_MIXTILT) { - servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; - servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; - } else { - servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; - servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL] / 50; - } - } - } - - // constrain servos - for (i = 0; i < MAX_SERVOS; i++) - servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values - - // forward AUX1-4 to servo outputs (not constrained) - if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { - int offset = 0; - if (feature(FEATURE_SERVO_TILT)) - offset = 2; - for (i = 0; i < 4; i++) - pwmWriteServo(i + offset, rcData[AUX1 + i]); - } - - maxMotor = motor[0]; - for (i = 1; i < numberMotor; i++) - if (motor[i] > maxMotor) - maxMotor = motor[i]; - for (i = 0; i < numberMotor; i++) { - 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; - if (feature(FEATURE_3D)) { - if ((rcData[THROTTLE]) > 1500) { - motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); - } else { - motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); - } - } else { - motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); - if ((rcData[THROTTLE]) < mcfg.mincheck) { - if (!feature(FEATURE_MOTOR_STOP)) - motor[i] = mcfg.minthrottle; - else - motor[i] = mcfg.mincommand; - } - } - if (!f.ARMED) { - motor[i] = motor_disarmed[i]; - } - } -} +}; + +static const motorMixer_t mixerHex6P[] = { + { 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R + { 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R + { 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L + { 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L + { 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR +}; + +static const motorMixer_t mixerY4[] = { + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW + { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW + { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW + { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW +}; + +static const motorMixer_t mixerHex6X[] = { + { 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R + { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R + { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L + { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L + { 1.0f, -1f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 1f, 0.0f, 1.0f }, // LEFT +}; + +static const motorMixer_t mixerOctoX8[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R + { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R + { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L + { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L +}; + +static const motorMixer_t mixerOctoFlatP[] = { + { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L + { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R + { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R + { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L + { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT + { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR + { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT +}; + +static const motorMixer_t mixerOctoFlatX[] = { + { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L + { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R + { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R + { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R + { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L +}; + +static const motorMixer_t mixerVtail4[] = { + { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L +}; + +static const motorMixer_t mixerHex6H[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT + { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT +}; + +static const motorMixer_t mixerDualcopter[] = { + { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT + { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT +}; + +// Keep this synced with MultiType struct in mw.h! +const mixer_t mixers[] = { +// Mo Se Mixtable + { 0, 0, NULL }, // entry 0 + { 3, 1, mixerTri }, // MULTITYPE_TRI + { 4, 0, mixerQuadP }, // MULTITYPE_QUADP + { 4, 0, mixerQuadX }, // MULTITYPE_QUADX + { 2, 1, mixerBi }, // MULTITYPE_BI + { 0, 1, NULL }, // * MULTITYPE_GIMBAL + { 6, 0, mixerY6 }, // MULTITYPE_Y6 + { 6, 0, mixerHex6P }, // MULTITYPE_HEX6 + { 1, 1, NULL }, // * MULTITYPE_FLYING_WING + { 4, 0, mixerY4 }, // MULTITYPE_Y4 + { 6, 0, mixerHex6X }, // MULTITYPE_HEX6X + { 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8 + { 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP + { 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX + { 1, 1, NULL }, // * MULTITYPE_AIRPLANE + { 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM + { 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG + { 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4 + { 6, 0, mixerHex6H }, // MULTITYPE_HEX6H + { 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO + { 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER + { 1, 1, NULL }, // MULTITYPE_SINGLECOPTER + { 0, 0, NULL }, // MULTITYPE_CUSTOM +}; + +int16_t servoMiddle(int nr) +{ + // Normally, servo.middle is a value between 1000..2000, but for the purposes of stupid, if it's less than + // the number of RC channels, it means the center value is taken FROM that RC channel (by its index) + if (cfg.servoConf[nr].middle < RC_CHANS && nr < MAX_SERVOS) + return rcData[cfg.servoConf[nr].middle]; + else + return cfg.servoConf[nr].middle; +} + +int servoDirection(int nr, int lr) +{ + // servo.rate is overloaded for servos that don't have a rate, but only need direction + // bit set = negative, clear = positive + // rate[2] = ???_direction + // rate[1] = roll_direction + // rate[0] = pitch_direction + // servo.rate is also used as gimbal gain multiplier (yeah) + if (cfg.servoConf[nr].rate & lr) + return -1; + else + return 1; +} + +void mixerInit(void) +{ + int i; + + // enable servos for mixes that require them. note, this shifts motor counts. + core.useServo = mixers[mcfg.mixerConfiguration].useServo; + // if we want camstab/trig, that also enables servos, even if mixer doesn't + if (feature(FEATURE_SERVO_TILT)) + core.useServo = 1; + + if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) { + // load custom mixer into currentMixer + for (i = 0; i < MAX_MOTORS; i++) { + // check if done + if (mcfg.customMixer[i].throttle == 0.0f) + break; + currentMixer[i] = mcfg.customMixer[i]; + numberMotor++; + } + } else { + numberMotor = mixers[mcfg.mixerConfiguration].numberMotor; + // copy motor-based mixers + if (mixers[mcfg.mixerConfiguration].motor) { + for (i = 0; i < numberMotor; i++) + currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; + } + } + + // in 3D mode, mixer gain has to be halved + if (feature(FEATURE_3D)) { + if (numberMotor > 1) { + for (i = 0; i < numberMotor; i++) { + currentMixer[i].pitch *= 0.5f; + currentMixer[i].roll *= 0.5f; + currentMixer[i].yaw *= 0.5f; + } + } + } + mixerResetMotors(); +} + +void mixerResetMotors(void) +{ + int i; + // set disarmed motor values + for (i = 0; i < MAX_MOTORS; i++) + motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; +} + +void mixerLoadMix(int index) +{ + int i; + + // we're 1-based + index++; + // clear existing + for (i = 0; i < MAX_MOTORS; i++) + 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++) + mcfg.customMixer[i] = mixers[index].motor[i]; + } +} + +void writeServos(void) +{ + if (!core.useServo) + return; + + switch (mcfg.mixerConfiguration) { + case MULTITYPE_BI: + pwmWriteServo(0, servo[4]); + pwmWriteServo(1, servo[5]); + break; + + case MULTITYPE_TRI: + if (cfg.tri_unarmed_servo) { + // if unarmed flag set, we always move servo + pwmWriteServo(0, servo[5]); + } else { + // otherwise, only move servo when copter is armed + if (f.ARMED) + pwmWriteServo(0, servo[5]); + else + pwmWriteServo(0, 0); // kill servo signal completely. + } + break; + + case MULTITYPE_FLYING_WING: + pwmWriteServo(0, servo[3]); + pwmWriteServo(1, servo[4]); + break; + + case MULTITYPE_GIMBAL: + pwmWriteServo(0, servo[0]); + pwmWriteServo(1, servo[1]); + break; + + case MULTITYPE_DUALCOPTER: + pwmWriteServo(0, servo[4]); + pwmWriteServo(1, servo[5]); + break; + + case MULTITYPE_AIRPLANE: + case MULTITYPE_SINGLECOPTER: + pwmWriteServo(0, servo[3]); + pwmWriteServo(1, servo[4]); + pwmWriteServo(2, servo[5]); + pwmWriteServo(3, servo[6]); + break; + + default: + // Two servos for SERVO_TILT, if enabled + if (feature(FEATURE_SERVO_TILT)) { + pwmWriteServo(0, servo[0]); + pwmWriteServo(1, servo[1]); + } + break; + } +} + +extern uint8_t cliMode; + +void writeMotors(void) +{ + uint8_t i; + + for (i = 0; i < numberMotor; i++) + pwmWriteMotor(i, motor[i]); +} + +void writeAllMotors(int16_t mc) +{ + uint8_t i; + + // Sends commands to all motors + for (i = 0; i < numberMotor; i++) + motor[i] = mc; + writeMotors(); +} + +static void airplaneMixer(void) +{ + int16_t flapperons[2] = { 0, 0 }; + int i; + + if (!f.ARMED) + servo[7] = mcfg.mincommand; // Kill throttle when disarmed + else + servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); + motor[0] = servo[7]; + +#if 0 + if (cfg.flaperons) { + + + } +#endif + + if (mcfg.flaps_speed) { + // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control + // use servo min, servo max and servo rate for proper endpoints adjust + static int16_t slow_LFlaps; + int16_t lFlap = servoMiddle(2); + + lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); + lFlap = mcfg.midrc - lFlap; // shouldn't this be servoConf[2].middle? + if (slow_LFlaps < lFlap) + slow_LFlaps += mcfg.flaps_speed; + else if (slow_LFlaps > lFlap) + slow_LFlaps -= mcfg.flaps_speed; + + servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L; + servo[2] += mcfg.midrc; + } + + if (f.PASSTHRU_MODE) { // Direct passthru from RX + servo[3] = rcCommand[ROLL] + flapperons[0]; // Wing 1 + servo[4] = rcCommand[ROLL] + flapperons[1]; // Wing 2 + servo[5] = rcCommand[YAW]; // Rudder + servo[6] = rcCommand[PITCH]; // Elevator + } else { + // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui + servo[3] = axisPID[ROLL] + flapperons[0]; // Wing 1 + servo[4] = axisPID[ROLL] + flapperons[1]; // Wing 2 + servo[5] = axisPID[YAW]; // Rudder + servo[6] = axisPID[PITCH]; // Elevator + } + for (i = 3; i < 7; i++) { + servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates + servo[i] += servoMiddle(i); + } +} + +void mixTable(void) +{ + int16_t maxMotor; + uint32_t i; + + if (numberMotor > 3) { + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); + } + + // motors for non-servo mixes + if (numberMotor > 1) + for (i = 0; i < numberMotor; i++) + 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 (mcfg.mixerConfiguration) { + case MULTITYPE_BI: + servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + servoMiddle(4); // LEFT + servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + servoMiddle(5); // RIGHT + break; + + case MULTITYPE_TRI: + servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR + break; + + case MULTITYPE_GIMBAL: + servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0); + servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1); + break; + + case MULTITYPE_AIRPLANE: + airplaneMixer(); + break; + + case MULTITYPE_FLYING_WING: + if (!f.ARMED) + servo[7] = mcfg.mincommand; + else + servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); + motor[0] = servo[7]; + if (f.PASSTHRU_MODE) { + // do not use sensors for correction, simple 2 channel mixing + servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]); + servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]); + } else { + // use sensors to correct (gyro only or gyro + acc) + servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]); + servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]); + } + servo[3] += servoMiddle(3); + servo[4] += servoMiddle(4); + break; + + case MULTITYPE_DUALCOPTER: + for (i = 4; i < 6; i++ ) { + servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction + servo[i] += servoMiddle(i); + } + break; + + case MULTITYPE_SINGLECOPTER: + for (i = 3; i < 7; i++) { + servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction + servo[i] += servoMiddle(i); + } + motor[0] = rcCommand[THROTTLE]; + break; + } + + // do camstab + if (feature(FEATURE_SERVO_TILT)) { + // center at fixed position, or vary either pitch or roll by RC channel + servo[0] = servoMiddle(0); + servo[1] = servoMiddle(1); + + if (rcOptions[BOXCAMSTAB]) { + if (cfg.gimbal_flags & GIMBAL_MIXTILT) { + servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; + servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; + } else { + servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; + servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL] / 50; + } + } + } + + // constrain servos + for (i = 0; i < MAX_SERVOS; i++) + servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values + + // forward AUX1-4 to servo outputs (not constrained) + if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { + int offset = 0; + if (feature(FEATURE_SERVO_TILT)) + offset = 2; + for (i = 0; i < 4; i++) + pwmWriteServo(i + offset, rcData[AUX1 + i]); + } + + maxMotor = motor[0]; + for (i = 1; i < numberMotor; i++) + if (motor[i] > maxMotor) + maxMotor = motor[i]; + for (i = 0; i < numberMotor; i++) { + 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; + if (feature(FEATURE_3D)) { + if ((rcData[THROTTLE]) > 1500) { + motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); + } else { + motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); + } + } else { + motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); + if ((rcData[THROTTLE]) < mcfg.mincheck) { + if (!feature(FEATURE_MOTOR_STOP)) + motor[i] = mcfg.minthrottle; + else + motor[i] = mcfg.mincommand; + } + } + if (!f.ARMED) { + motor[i] = motor_disarmed[i]; + } + } +} From d885e39a9056e8daaf34f569e789c72b9be169b1 Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 1 Jan 2014 09:24:12 +0100 Subject: [PATCH 3/7] type float --- src/mixer.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mixer.c b/src/mixer.c index 0cb5ee31e7..f64493845e 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -63,8 +63,8 @@ static const motorMixer_t mixerHex6X[] = { { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L - { 1.0f, -1f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 1f, 0.0f, 1.0f }, // LEFT + { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT }; static const motorMixer_t mixerOctoX8[] = { From 2caa1fbb207084e62f39a4f5b623e940d28fce3a Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 1 Jan 2014 09:42:14 +0100 Subject: [PATCH 4/7] add newlib stubs --- Makefile | 1 + src/newlib_stubs.c | 278 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 279 insertions(+) create mode 100644 src/newlib_stubs.c diff --git a/Makefile b/Makefile index 174a62fe4f..34e8c1458a 100644 --- a/Makefile +++ b/Makefile @@ -42,6 +42,7 @@ BIN_DIR = $(ROOT)/obj # Source files common to all targets COMMON_SRC = startup_stm32f10x_md_gcc.S \ + newlib_stubs.c \ buzzer.c \ cli.c \ config.c \ diff --git a/src/newlib_stubs.c b/src/newlib_stubs.c new file mode 100644 index 0000000000..706e893fee --- /dev/null +++ b/src/newlib_stubs.c @@ -0,0 +1,278 @@ + +/* + * newlib_stubs.c + * + * Created on: 2 Nov 2010 + * Author: nanoage.co.uk + */ +#include +#include +#include +#include +#include "stm32f10x_usart.h" + + +#ifndef STDOUT_USART +#define STDOUT_USART 2 +#endif + +#ifndef STDERR_USART +#define STDERR_USART 2 +#endif + +#ifndef STDIN_USART +#define STDIN_USART 2 +#endif + +#undef errno +extern int errno; + +/* + environ + A pointer to a list of environment variables and their values. + For a minimal environment, this empty list is adequate: + */ +char *__env[1] = { 0 }; +char **environ = __env; + +int _write(int file, char *ptr, int len); + +void _exit(int status) { + _write(1, "exit", 4); + while (1) { + ; + } +} + +int _close(int file) { + return -1; +} +/* + execve + Transfer control to a new process. Minimal implementation (for a system without processes): + */ +int _execve(char *name, char **argv, char **env) { + errno = ENOMEM; + return -1; +} +/* + fork + Create a new process. Minimal implementation (for a system without processes): + */ + +int _fork() { + errno = EAGAIN; + return -1; +} +/* + fstat + Status of an open file. For consistency with other minimal implementations in these examples, + all files are regarded as character special devices. + The `sys/stat.h' header file required is distributed in the `include' subdirectory for this C library. + */ +int _fstat(int file, struct stat *st) { + st->st_mode = S_IFCHR; + return 0; +} + +/* + getpid + Process-ID; this is sometimes used to generate strings unlikely to conflict with other processes. Minimal implementation, for a system without processes: + */ + +int _getpid() { + return 1; +} + +/* + isatty + Query whether output stream is a terminal. For consistency with the other minimal implementations, + */ +int _isatty(int file) { + switch (file){ + case STDOUT_FILENO: + case STDERR_FILENO: + case STDIN_FILENO: + return 1; + default: + //errno = ENOTTY; + errno = EBADF; + return 0; + } +} + + +/* + kill + Send a signal. Minimal implementation: + */ +int _kill(int pid, int sig) { + errno = EINVAL; + return (-1); +} + +/* + link + Establish a new name for an existing file. Minimal implementation: + */ + +int _link(char *old, char *new) { + errno = EMLINK; + return -1; +} + +/* + lseek + Set position in a file. Minimal implementation: + */ +int _lseek(int file, int ptr, int dir) { + return 0; +} + +/* + sbrk + Increase program data space. + Malloc and related functions depend on this + */ +caddr_t _sbrk(int incr) { + + extern char _ebss; // Defined by the linker + static char *heap_end; + char *prev_heap_end; + + if (heap_end == 0) { + heap_end = &_ebss; + } + prev_heap_end = heap_end; + +char * stack = (char*) __get_MSP(); + if (heap_end + incr > stack) + { + _write (STDERR_FILENO, "Heap and stack collision\n", 25); + errno = ENOMEM; + return (caddr_t) -1; + //abort (); + } + + heap_end += incr; + return (caddr_t) prev_heap_end; + +} + +/* + read + Read a character to a file. `libc' subroutines will use this system routine for input from all files, including stdin + Returns -1 on error or blocks until the number of characters have been read. + */ + + +int _read(int file, char *ptr, int len) { + int n; + int num = 0; + switch (file) { + case STDIN_FILENO: + for (n = 0; n < len; n++) { +#if STDIN_USART == 1 + while ((USART1->SR & USART_FLAG_RXNE) == (uint16_t)RESET) {} + char c = (char)(USART1->DR & (uint16_t)0x01FF); +#elif STDIN_USART == 2 + while ((USART2->SR & USART_FLAG_RXNE) == (uint16_t) RESET) {} + char c = (char) (USART2->DR & (uint16_t) 0x01FF); +#elif STDIN_USART == 3 + while ((USART3->SR & USART_FLAG_RXNE) == (uint16_t)RESET) {} + char c = (char)(USART3->DR & (uint16_t)0x01FF); +#endif + *ptr++ = c; + num++; + } + break; + default: + errno = EBADF; + return -1; + } + return num; +} + +/* + stat + Status of a file (by name). Minimal implementation: + int _EXFUN(stat,( const char *__path, struct stat *__sbuf )); + */ + +int _stat(const char *filepath, struct stat *st) { + st->st_mode = S_IFCHR; + return 0; +} + +/* + times + Timing information for current process. Minimal implementation: + */ + +clock_t _times(struct tms *buf) { + return -1; +} + +/* + unlink + Remove a file's directory entry. Minimal implementation: + */ +int _unlink(char *name) { + errno = ENOENT; + return -1; +} + +/* + wait + Wait for a child process. Minimal implementation: + */ +int _wait(int *status) { + errno = ECHILD; + return -1; +} + +/* + write + Write a character to a file. `libc' subroutines will use this system routine for output to all files, including stdout + Returns -1 on error or number of bytes sent + */ +int _write(int file, char *ptr, int len) { + int n; + switch (file) { + case STDOUT_FILENO: /*stdout*/ + for (n = 0; n < len; n++) { +#if STDOUT_USART == 1 + while ((USART1->SR & USART_FLAG_TC) == (uint16_t)RESET) {} + USART1->DR = (*ptr++ & (uint16_t)0x01FF); +#elif STDOUT_USART == 2 + while ((USART2->SR & USART_FLAG_TC) == (uint16_t) RESET) { + } + USART2->DR = (*ptr++ & (uint16_t) 0x01FF); +#elif STDOUT_USART == 3 + while ((USART3->SR & USART_FLAG_TC) == (uint16_t)RESET) {} + USART3->DR = (*ptr++ & (uint16_t)0x01FF); +#endif + } + break; + case STDERR_FILENO: /* stderr */ + for (n = 0; n < len; n++) { +#if STDERR_USART == 1 + while ((USART1->SR & USART_FLAG_TC) == (uint16_t)RESET) {} + USART1->DR = (*ptr++ & (uint16_t)0x01FF); +#elif STDERR_USART == 2 + while ((USART2->SR & USART_FLAG_TC) == (uint16_t) RESET) { + } + USART2->DR = (*ptr++ & (uint16_t) 0x01FF); +#elif STDERR_USART == 3 + while ((USART3->SR & USART_FLAG_TC) == (uint16_t)RESET) {} + USART3->DR = (*ptr++ & (uint16_t)0x01FF); +#endif + } + break; + default: + errno = EBADF; + return -1; + } + return len; +} + From 239120ba4e59e4b269a9b342660618c5875923a2 Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 1 Jan 2014 10:14:23 +0100 Subject: [PATCH 5/7] ignore white space --- src/mixer.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/mixer.c b/src/mixer.c index f64493845e..44939ed7e1 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -9,9 +9,9 @@ int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; static motorMixer_t currentMixer[MAX_MOTORS]; static const motorMixer_t mixerTri[] = { - { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR - { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT - { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT + { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR + { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT + { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT }; static const motorMixer_t mixerQuadP[] = { @@ -34,12 +34,12 @@ static const motorMixer_t mixerBi[] = { }; static const motorMixer_t mixerY6[] = { - { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR - { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT - { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT - { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR - { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT - { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT + { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR + { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT + { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT + { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR + { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT + { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT }; static const motorMixer_t mixerHex6P[] = { From 01f079c25478d3cf69c9358530280cba8bcc0ee8 Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 1 Jan 2014 10:18:18 +0100 Subject: [PATCH 6/7] ed --- src/board.h | 147 ++++---- src/mixer.c | 996 ++++++++++++++++++++++++++-------------------------- 2 files changed, 574 insertions(+), 569 deletions(-) diff --git a/src/board.h b/src/board.h index 0c6b540453..47bc77d770 100755 --- a/src/board.h +++ b/src/board.h @@ -40,42 +40,42 @@ #define U_ID_2 (*(uint32_t*)0x1FFFF7F0) typedef enum { - SENSOR_GYRO = 1 << 0, // always present - SENSOR_ACC = 1 << 1, - SENSOR_BARO = 1 << 2, - SENSOR_MAG = 1 << 3, - SENSOR_SONAR = 1 << 4, - SENSOR_GPS = 1 << 5, - SENSOR_GPSMAG = 1 << 6, + SENSOR_GYRO = 1 << 0, // always present + SENSOR_ACC = 1 << 1, + SENSOR_BARO = 1 << 2, + SENSOR_MAG = 1 << 3, + SENSOR_SONAR = 1 << 4, + SENSOR_GPS = 1 << 5, + SENSOR_GPSMAG = 1 << 6, } AvailableSensors; // Type of accelerometer used/detected typedef enum AccelSensors { - ACC_DEFAULT = 0, - ACC_ADXL345 = 1, - ACC_MPU6050 = 2, - ACC_MMA8452 = 3, - ACC_BMA280 = 4, - ACC_NONE = 5 + ACC_DEFAULT = 0, + ACC_ADXL345 = 1, + ACC_MPU6050 = 2, + ACC_MMA8452 = 3, + ACC_BMA280 = 4, + ACC_NONE = 5 } AccelSensors; typedef enum { - FEATURE_PPM = 1 << 0, - FEATURE_VBAT = 1 << 1, - FEATURE_INFLIGHT_ACC_CAL = 1 << 2, - FEATURE_SERIALRX = 1 << 3, - FEATURE_MOTOR_STOP = 1 << 4, - FEATURE_SERVO_TILT = 1 << 5, - FEATURE_GYRO_SMOOTHING = 1 << 6, - FEATURE_LED_RING = 1 << 7, - FEATURE_GPS = 1 << 8, - FEATURE_FAILSAFE = 1 << 9, - FEATURE_SONAR = 1 << 10, - FEATURE_TELEMETRY = 1 << 11, - FEATURE_POWERMETER = 1 << 12, - FEATURE_VARIO = 1 << 13, - FEATURE_3D = 1 << 14, - FEATURE_SOFTSERIAL = 1 << 15, + FEATURE_PPM = 1 << 0, + FEATURE_VBAT = 1 << 1, + FEATURE_INFLIGHT_ACC_CAL = 1 << 2, + FEATURE_SERIALRX = 1 << 3, + FEATURE_MOTOR_STOP = 1 << 4, + FEATURE_SERVO_TILT = 1 << 5, + FEATURE_GYRO_SMOOTHING = 1 << 6, + FEATURE_LED_RING = 1 << 7, + FEATURE_GPS = 1 << 8, + FEATURE_FAILSAFE = 1 << 9, + FEATURE_SONAR = 1 << 10, + FEATURE_TELEMETRY = 1 << 11, + FEATURE_POWERMETER = 1 << 12, + FEATURE_VARIO = 1 << 13, + FEATURE_3D = 1 << 14, + FEATURE_SOFTSERIAL = 1 << 15, } AvailableFeatures; typedef enum { @@ -104,60 +104,63 @@ typedef enum { } sensor_axis_e; typedef enum { - ALIGN_DEFAULT = 0, // driver-provided alignment - CW0_DEG = 1, - CW90_DEG = 2, - CW180_DEG = 3, - CW270_DEG = 4, - CW0_DEG_FLIP = 5, - CW90_DEG_FLIP = 6, - CW180_DEG_FLIP = 7, - CW270_DEG_FLIP = 8 + ALIGN_DEFAULT = 0, // driver-provided alignment + CW0_DEG = 1, + CW90_DEG = 2, + CW180_DEG = 3, + CW270_DEG = 4, + CW0_DEG_FLIP = 5, + CW90_DEG_FLIP = 6, + CW180_DEG_FLIP = 7, + CW270_DEG_FLIP = 8 } sensor_align_e; enum { - GYRO_UPDATED = 1 << 0, - ACC_UPDATED = 1 << 1, - MAG_UPDATED = 1 << 2, - TEMP_UPDATED = 1 << 3 + GYRO_UPDATED = 1 << 0, + ACC_UPDATED = 1 << 1, + MAG_UPDATED = 1 << 2, + TEMP_UPDATED = 1 << 3 }; -typedef struct sensor_data_t { - int16_t gyro[3]; - int16_t acc[3]; - int16_t mag[3]; - float temperature; - int updated; +typedef struct sensor_data_t +{ + int16_t gyro[3]; + int16_t acc[3]; + int16_t mag[3]; + float temperature; + int updated; } sensor_data_t; -typedef void (*sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype -typedef void (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype -typedef void (*baroOpFuncPtr)(void); // baro start operation -typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) -typedef void (*serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app -typedef uint16_t (*rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data -typedef void (*pidControllerFuncPtr)(void); // pid controller function prototype +typedef void (* sensorInitFuncPtr)(sensor_align_e align); // sensor init prototype +typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype +typedef void (* baroOpFuncPtr)(void); // baro start operation +typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature) +typedef void (* serialReceiveCallbackPtr)(uint16_t data); // used by serial drivers to return frames to app +typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data +typedef void (* pidControllerFuncPtr)(void); // pid controller function prototype -typedef struct sensor_t { - sensorInitFuncPtr init; // initialize function - sensorReadFuncPtr read; // read 3 axis data function - sensorReadFuncPtr temperature; // read temperature if available - float scale; // scalefactor (currently used for gyro only, todo for accel) +typedef struct sensor_t +{ + sensorInitFuncPtr init; // initialize function + sensorReadFuncPtr read; // read 3 axis data function + sensorReadFuncPtr temperature; // read temperature if available + float scale; // scalefactor (currently used for gyro only, todo for accel) } sensor_t; -typedef struct baro_t { - uint16_t ut_delay; - uint16_t up_delay; - baroOpFuncPtr start_ut; - baroOpFuncPtr get_ut; - baroOpFuncPtr start_up; - baroOpFuncPtr get_up; - baroCalculateFuncPtr calculate; +typedef struct baro_t +{ + uint16_t ut_delay; + uint16_t up_delay; + baroOpFuncPtr start_ut; + baroOpFuncPtr get_ut; + baroOpFuncPtr start_up; + baroOpFuncPtr get_up; + baroCalculateFuncPtr calculate; } baro_t; // Hardware definitions and GPIO #ifdef FY90Q -// FY90Q + // FY90Q #define LED0_GPIO GPIOC #define LED0_PIN Pin_12 #define LED1_GPIO GPIOA @@ -178,6 +181,7 @@ typedef struct baro_t { #define LED0_PIN Pin_1 // D3, PA1/USART2_RTS/ADC1/TIM2_CH3 - "LED2" on silkscreen, Yellow #define LED1_GPIO GPIOA #define LED1_PIN Pin_5 // D13, PA5/SPI1_SCK/ADC5 - "LED1" on silkscreen, Green + #define GYRO #define ACC @@ -227,10 +231,11 @@ typedef struct baro_t { #endif #undef SOFT_I2C // enable to test software i2c + #include "utils.h" #ifdef FY90Q -// FY90Q + // FY90Q #include "drv_adc.h" #include "drv_i2c.h" #include "drv_pwm.h" @@ -253,7 +258,7 @@ typedef struct baro_t { #include "drv_softserial.h" #else -// AfroFlight32 + // AfroFlight32 #include "drv_adc.h" #include "drv_adxl345.h" #include "drv_bma280.h" diff --git a/src/mixer.c b/src/mixer.c index 44939ed7e1..f0c0ecf2c5 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -1,498 +1,498 @@ -#include "board.h" -#include "mw.h" - -static uint8_t numberMotor = 0; -int16_t motor[MAX_MOTORS]; -int16_t motor_disarmed[MAX_MOTORS]; -int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; - -static motorMixer_t currentMixer[MAX_MOTORS]; - -static const motorMixer_t mixerTri[] = { - { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR - { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT - { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT -}; - -static const motorMixer_t mixerQuadP[] = { - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR - { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT - { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT - { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT -}; - -static const motorMixer_t mixerQuadX[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L -}; - -static const motorMixer_t mixerBi[] = { - { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT - { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT -}; - -static const motorMixer_t mixerY6[] = { - { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR - { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT - { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT - { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR - { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT - { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT -}; - -static const motorMixer_t mixerHex6P[] = { - { 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R - { 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R - { 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L - { 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L - { 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR -}; - -static const motorMixer_t mixerY4[] = { - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW - { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW - { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW - { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW -}; - -static const motorMixer_t mixerHex6X[] = { - { 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R - { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R - { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L - { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT -}; - -static const motorMixer_t mixerOctoX8[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R - { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R - { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L - { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L -}; - -static const motorMixer_t mixerOctoFlatP[] = { - { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L - { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R - { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R - { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L - { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT - { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR - { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT -}; - -static const motorMixer_t mixerOctoFlatX[] = { - { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L - { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R - { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R - { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R - { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L -}; - -static const motorMixer_t mixerVtail4[] = { - { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L -}; - -static const motorMixer_t mixerHex6H[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT - { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT -}; - -static const motorMixer_t mixerDualcopter[] = { - { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT - { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT -}; - -// Keep this synced with MultiType struct in mw.h! -const mixer_t mixers[] = { -// Mo Se Mixtable - { 0, 0, NULL }, // entry 0 - { 3, 1, mixerTri }, // MULTITYPE_TRI - { 4, 0, mixerQuadP }, // MULTITYPE_QUADP - { 4, 0, mixerQuadX }, // MULTITYPE_QUADX - { 2, 1, mixerBi }, // MULTITYPE_BI - { 0, 1, NULL }, // * MULTITYPE_GIMBAL - { 6, 0, mixerY6 }, // MULTITYPE_Y6 - { 6, 0, mixerHex6P }, // MULTITYPE_HEX6 - { 1, 1, NULL }, // * MULTITYPE_FLYING_WING - { 4, 0, mixerY4 }, // MULTITYPE_Y4 - { 6, 0, mixerHex6X }, // MULTITYPE_HEX6X - { 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8 - { 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP - { 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX - { 1, 1, NULL }, // * MULTITYPE_AIRPLANE - { 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM - { 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG - { 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4 - { 6, 0, mixerHex6H }, // MULTITYPE_HEX6H - { 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO - { 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER - { 1, 1, NULL }, // MULTITYPE_SINGLECOPTER - { 0, 0, NULL }, // MULTITYPE_CUSTOM -}; - -int16_t servoMiddle(int nr) -{ - // Normally, servo.middle is a value between 1000..2000, but for the purposes of stupid, if it's less than - // the number of RC channels, it means the center value is taken FROM that RC channel (by its index) - if (cfg.servoConf[nr].middle < RC_CHANS && nr < MAX_SERVOS) - return rcData[cfg.servoConf[nr].middle]; - else - return cfg.servoConf[nr].middle; -} - -int servoDirection(int nr, int lr) -{ - // servo.rate is overloaded for servos that don't have a rate, but only need direction - // bit set = negative, clear = positive - // rate[2] = ???_direction - // rate[1] = roll_direction - // rate[0] = pitch_direction - // servo.rate is also used as gimbal gain multiplier (yeah) - if (cfg.servoConf[nr].rate & lr) - return -1; - else - return 1; -} - -void mixerInit(void) -{ - int i; - - // enable servos for mixes that require them. note, this shifts motor counts. - core.useServo = mixers[mcfg.mixerConfiguration].useServo; - // if we want camstab/trig, that also enables servos, even if mixer doesn't - if (feature(FEATURE_SERVO_TILT)) - core.useServo = 1; - - if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) { - // load custom mixer into currentMixer - for (i = 0; i < MAX_MOTORS; i++) { - // check if done - if (mcfg.customMixer[i].throttle == 0.0f) - break; - currentMixer[i] = mcfg.customMixer[i]; - numberMotor++; - } - } else { - numberMotor = mixers[mcfg.mixerConfiguration].numberMotor; - // copy motor-based mixers - if (mixers[mcfg.mixerConfiguration].motor) { - for (i = 0; i < numberMotor; i++) - currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; - } - } - - // in 3D mode, mixer gain has to be halved - if (feature(FEATURE_3D)) { - if (numberMotor > 1) { - for (i = 0; i < numberMotor; i++) { - currentMixer[i].pitch *= 0.5f; - currentMixer[i].roll *= 0.5f; - currentMixer[i].yaw *= 0.5f; - } - } - } - mixerResetMotors(); -} - -void mixerResetMotors(void) -{ - int i; - // set disarmed motor values - for (i = 0; i < MAX_MOTORS; i++) - motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; -} - -void mixerLoadMix(int index) -{ - int i; - - // we're 1-based - index++; - // clear existing - for (i = 0; i < MAX_MOTORS; i++) - 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++) - mcfg.customMixer[i] = mixers[index].motor[i]; - } -} - -void writeServos(void) -{ - if (!core.useServo) - return; - - switch (mcfg.mixerConfiguration) { - case MULTITYPE_BI: - pwmWriteServo(0, servo[4]); - pwmWriteServo(1, servo[5]); - break; - - case MULTITYPE_TRI: - if (cfg.tri_unarmed_servo) { - // if unarmed flag set, we always move servo - pwmWriteServo(0, servo[5]); - } else { - // otherwise, only move servo when copter is armed - if (f.ARMED) - pwmWriteServo(0, servo[5]); - else - pwmWriteServo(0, 0); // kill servo signal completely. - } - break; - - case MULTITYPE_FLYING_WING: - pwmWriteServo(0, servo[3]); - pwmWriteServo(1, servo[4]); - break; - - case MULTITYPE_GIMBAL: - pwmWriteServo(0, servo[0]); - pwmWriteServo(1, servo[1]); - break; - - case MULTITYPE_DUALCOPTER: - pwmWriteServo(0, servo[4]); - pwmWriteServo(1, servo[5]); - break; - - case MULTITYPE_AIRPLANE: - case MULTITYPE_SINGLECOPTER: - pwmWriteServo(0, servo[3]); - pwmWriteServo(1, servo[4]); - pwmWriteServo(2, servo[5]); - pwmWriteServo(3, servo[6]); - break; - - default: - // Two servos for SERVO_TILT, if enabled - if (feature(FEATURE_SERVO_TILT)) { - pwmWriteServo(0, servo[0]); - pwmWriteServo(1, servo[1]); - } - break; - } -} - -extern uint8_t cliMode; - -void writeMotors(void) -{ - uint8_t i; - - for (i = 0; i < numberMotor; i++) - pwmWriteMotor(i, motor[i]); -} - -void writeAllMotors(int16_t mc) -{ - uint8_t i; - - // Sends commands to all motors - for (i = 0; i < numberMotor; i++) - motor[i] = mc; - writeMotors(); -} - -static void airplaneMixer(void) -{ - int16_t flapperons[2] = { 0, 0 }; - int i; - - if (!f.ARMED) - servo[7] = mcfg.mincommand; // Kill throttle when disarmed - else - servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); - motor[0] = servo[7]; - -#if 0 - if (cfg.flaperons) { - - - } -#endif - - if (mcfg.flaps_speed) { - // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control - // use servo min, servo max and servo rate for proper endpoints adjust - static int16_t slow_LFlaps; - int16_t lFlap = servoMiddle(2); - - lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); - lFlap = mcfg.midrc - lFlap; // shouldn't this be servoConf[2].middle? - if (slow_LFlaps < lFlap) - slow_LFlaps += mcfg.flaps_speed; - else if (slow_LFlaps > lFlap) - slow_LFlaps -= mcfg.flaps_speed; - - servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L; - servo[2] += mcfg.midrc; - } - - if (f.PASSTHRU_MODE) { // Direct passthru from RX - servo[3] = rcCommand[ROLL] + flapperons[0]; // Wing 1 - servo[4] = rcCommand[ROLL] + flapperons[1]; // Wing 2 - servo[5] = rcCommand[YAW]; // Rudder - servo[6] = rcCommand[PITCH]; // Elevator - } else { - // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui - servo[3] = axisPID[ROLL] + flapperons[0]; // Wing 1 - servo[4] = axisPID[ROLL] + flapperons[1]; // Wing 2 - servo[5] = axisPID[YAW]; // Rudder - servo[6] = axisPID[PITCH]; // Elevator - } - for (i = 3; i < 7; i++) { - servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates - servo[i] += servoMiddle(i); - } -} - -void mixTable(void) -{ - int16_t maxMotor; - uint32_t i; - - if (numberMotor > 3) { - // prevent "yaw jump" during yaw correction - axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); - } - - // motors for non-servo mixes - if (numberMotor > 1) - for (i = 0; i < numberMotor; i++) - 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 (mcfg.mixerConfiguration) { - case MULTITYPE_BI: - servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + servoMiddle(4); // LEFT - servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + servoMiddle(5); // RIGHT - break; - - case MULTITYPE_TRI: - servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR - break; - - case MULTITYPE_GIMBAL: - servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0); - servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1); - break; - - case MULTITYPE_AIRPLANE: - airplaneMixer(); - break; - - case MULTITYPE_FLYING_WING: - if (!f.ARMED) - servo[7] = mcfg.mincommand; - else - servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); - motor[0] = servo[7]; - if (f.PASSTHRU_MODE) { - // do not use sensors for correction, simple 2 channel mixing - servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]); - servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]); - } else { - // use sensors to correct (gyro only or gyro + acc) - servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]); - servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]); - } - servo[3] += servoMiddle(3); - servo[4] += servoMiddle(4); - break; - - case MULTITYPE_DUALCOPTER: - for (i = 4; i < 6; i++ ) { - servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction - servo[i] += servoMiddle(i); - } - break; - - case MULTITYPE_SINGLECOPTER: - for (i = 3; i < 7; i++) { - servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction - servo[i] += servoMiddle(i); - } - motor[0] = rcCommand[THROTTLE]; - break; - } - - // do camstab - if (feature(FEATURE_SERVO_TILT)) { - // center at fixed position, or vary either pitch or roll by RC channel - servo[0] = servoMiddle(0); - servo[1] = servoMiddle(1); - - if (rcOptions[BOXCAMSTAB]) { - if (cfg.gimbal_flags & GIMBAL_MIXTILT) { - servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; - servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; - } else { - servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; - servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL] / 50; - } - } - } - - // constrain servos - for (i = 0; i < MAX_SERVOS; i++) - servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values - - // forward AUX1-4 to servo outputs (not constrained) - if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { - int offset = 0; - if (feature(FEATURE_SERVO_TILT)) - offset = 2; - for (i = 0; i < 4; i++) - pwmWriteServo(i + offset, rcData[AUX1 + i]); - } - - maxMotor = motor[0]; - for (i = 1; i < numberMotor; i++) - if (motor[i] > maxMotor) - maxMotor = motor[i]; - for (i = 0; i < numberMotor; i++) { - 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; - if (feature(FEATURE_3D)) { - if ((rcData[THROTTLE]) > 1500) { - motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); - } else { - motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); - } - } else { - motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); - if ((rcData[THROTTLE]) < mcfg.mincheck) { - if (!feature(FEATURE_MOTOR_STOP)) - motor[i] = mcfg.minthrottle; - else - motor[i] = mcfg.mincommand; - } - } - if (!f.ARMED) { - motor[i] = motor_disarmed[i]; - } - } -} +#include "board.h" +#include "mw.h" + +static uint8_t numberMotor = 0; +int16_t motor[MAX_MOTORS]; +int16_t motor_disarmed[MAX_MOTORS]; +int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; + +static motorMixer_t currentMixer[MAX_MOTORS]; + +static const motorMixer_t mixerTri[] = { + { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR + { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT + { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT +}; + +static const motorMixer_t mixerQuadP[] = { + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR + { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT + { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT + { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT +}; + +static const motorMixer_t mixerQuadX[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L +}; + +static const motorMixer_t mixerBi[] = { + { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT + { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT +}; + +static const motorMixer_t mixerY6[] = { + { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR + { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT + { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT + { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR + { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT + { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT +}; + +static const motorMixer_t mixerHex6P[] = { + { 1.0f, -1.0f, 0.866025f, 1.0f }, // REAR_R + { 1.0f, -1.0f, -0.866025f, -1.0f }, // FRONT_R + { 1.0f, 1.0f, 0.866025f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -0.866025f, -1.0f }, // FRONT_L + { 1.0f, 0.0f, -0.866025f, 1.0f }, // FRONT + { 1.0f, 0.0f, 0.866025f, -1.0f }, // REAR +}; + +static const motorMixer_t mixerY4[] = { + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW + { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW + { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW + { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW +}; + +static const motorMixer_t mixerHex6X[] = { + { 1.0f, -0.866025f, 1.0f, 1.0f }, // REAR_R + { 1.0f, -0.866025f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 0.866025f, 1.0f, -1.0f }, // REAR_L + { 1.0f, 0.866025f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -0.866025f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 0.866025f, 0.0f, 1.0f }, // LEFT +}; + +static const motorMixer_t mixerOctoX8[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R + { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R + { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L + { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L +}; + +static const motorMixer_t mixerOctoFlatP[] = { + { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L + { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R + { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R + { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L + { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT + { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR + { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT +}; + +static const motorMixer_t mixerOctoFlatX[] = { + { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L + { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R + { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R + { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R + { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L +}; + +static const motorMixer_t mixerVtail4[] = { + { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L +}; + +static const motorMixer_t mixerHex6H[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT + { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT +}; + +static const motorMixer_t mixerDualcopter[] = { + { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT + { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT +}; + +// Keep this synced with MultiType struct in mw.h! +const mixer_t mixers[] = { +// Mo Se Mixtable + { 0, 0, NULL }, // entry 0 + { 3, 1, mixerTri }, // MULTITYPE_TRI + { 4, 0, mixerQuadP }, // MULTITYPE_QUADP + { 4, 0, mixerQuadX }, // MULTITYPE_QUADX + { 2, 1, mixerBi }, // MULTITYPE_BI + { 0, 1, NULL }, // * MULTITYPE_GIMBAL + { 6, 0, mixerY6 }, // MULTITYPE_Y6 + { 6, 0, mixerHex6P }, // MULTITYPE_HEX6 + { 1, 1, NULL }, // * MULTITYPE_FLYING_WING + { 4, 0, mixerY4 }, // MULTITYPE_Y4 + { 6, 0, mixerHex6X }, // MULTITYPE_HEX6X + { 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8 + { 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP + { 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX + { 1, 1, NULL }, // * MULTITYPE_AIRPLANE + { 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM + { 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG + { 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4 + { 6, 0, mixerHex6H }, // MULTITYPE_HEX6H + { 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO + { 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER + { 1, 1, NULL }, // MULTITYPE_SINGLECOPTER + { 0, 0, NULL }, // MULTITYPE_CUSTOM +}; + +int16_t servoMiddle(int nr) +{ + // Normally, servo.middle is a value between 1000..2000, but for the purposes of stupid, if it's less than + // the number of RC channels, it means the center value is taken FROM that RC channel (by its index) + if (cfg.servoConf[nr].middle < RC_CHANS && nr < MAX_SERVOS) + return rcData[cfg.servoConf[nr].middle]; + else + return cfg.servoConf[nr].middle; +} + +int servoDirection(int nr, int lr) +{ + // servo.rate is overloaded for servos that don't have a rate, but only need direction + // bit set = negative, clear = positive + // rate[2] = ???_direction + // rate[1] = roll_direction + // rate[0] = pitch_direction + // servo.rate is also used as gimbal gain multiplier (yeah) + if (cfg.servoConf[nr].rate & lr) + return -1; + else + return 1; +} + +void mixerInit(void) +{ + int i; + + // enable servos for mixes that require them. note, this shifts motor counts. + core.useServo = mixers[mcfg.mixerConfiguration].useServo; + // if we want camstab/trig, that also enables servos, even if mixer doesn't + if (feature(FEATURE_SERVO_TILT)) + core.useServo = 1; + + if (mcfg.mixerConfiguration == MULTITYPE_CUSTOM) { + // load custom mixer into currentMixer + for (i = 0; i < MAX_MOTORS; i++) { + // check if done + if (mcfg.customMixer[i].throttle == 0.0f) + break; + currentMixer[i] = mcfg.customMixer[i]; + numberMotor++; + } + } else { + numberMotor = mixers[mcfg.mixerConfiguration].numberMotor; + // copy motor-based mixers + if (mixers[mcfg.mixerConfiguration].motor) { + for (i = 0; i < numberMotor; i++) + currentMixer[i] = mixers[mcfg.mixerConfiguration].motor[i]; + } + } + + // in 3D mode, mixer gain has to be halved + if (feature(FEATURE_3D)) { + if (numberMotor > 1) { + for (i = 0; i < numberMotor; i++) { + currentMixer[i].pitch *= 0.5f; + currentMixer[i].roll *= 0.5f; + currentMixer[i].yaw *= 0.5f; + } + } + } + mixerResetMotors(); +} + +void mixerResetMotors(void) +{ + int i; + // set disarmed motor values + for (i = 0; i < MAX_MOTORS; i++) + motor_disarmed[i] = feature(FEATURE_3D) ? mcfg.neutral3d : mcfg.mincommand; +} + +void mixerLoadMix(int index) +{ + int i; + + // we're 1-based + index++; + // clear existing + for (i = 0; i < MAX_MOTORS; i++) + 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++) + mcfg.customMixer[i] = mixers[index].motor[i]; + } +} + +void writeServos(void) +{ + if (!core.useServo) + return; + + switch (mcfg.mixerConfiguration) { + case MULTITYPE_BI: + pwmWriteServo(0, servo[4]); + pwmWriteServo(1, servo[5]); + break; + + case MULTITYPE_TRI: + if (cfg.tri_unarmed_servo) { + // if unarmed flag set, we always move servo + pwmWriteServo(0, servo[5]); + } else { + // otherwise, only move servo when copter is armed + if (f.ARMED) + pwmWriteServo(0, servo[5]); + else + pwmWriteServo(0, 0); // kill servo signal completely. + } + break; + + case MULTITYPE_FLYING_WING: + pwmWriteServo(0, servo[3]); + pwmWriteServo(1, servo[4]); + break; + + case MULTITYPE_GIMBAL: + pwmWriteServo(0, servo[0]); + pwmWriteServo(1, servo[1]); + break; + + case MULTITYPE_DUALCOPTER: + pwmWriteServo(0, servo[4]); + pwmWriteServo(1, servo[5]); + break; + + case MULTITYPE_AIRPLANE: + case MULTITYPE_SINGLECOPTER: + pwmWriteServo(0, servo[3]); + pwmWriteServo(1, servo[4]); + pwmWriteServo(2, servo[5]); + pwmWriteServo(3, servo[6]); + break; + + default: + // Two servos for SERVO_TILT, if enabled + if (feature(FEATURE_SERVO_TILT)) { + pwmWriteServo(0, servo[0]); + pwmWriteServo(1, servo[1]); + } + break; + } +} + +extern uint8_t cliMode; + +void writeMotors(void) +{ + uint8_t i; + + for (i = 0; i < numberMotor; i++) + pwmWriteMotor(i, motor[i]); +} + +void writeAllMotors(int16_t mc) +{ + uint8_t i; + + // Sends commands to all motors + for (i = 0; i < numberMotor; i++) + motor[i] = mc; + writeMotors(); +} + +static void airplaneMixer(void) +{ + int16_t flapperons[2] = { 0, 0 }; + int i; + + if (!f.ARMED) + servo[7] = mcfg.mincommand; // Kill throttle when disarmed + else + servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); + motor[0] = servo[7]; + +#if 0 + if (cfg.flaperons) { + + + } +#endif + + if (mcfg.flaps_speed) { + // configure SERVO3 middle point in GUI to using an AUX channel for FLAPS control + // use servo min, servo max and servo rate for proper endpoints adjust + static int16_t slow_LFlaps; + int16_t lFlap = servoMiddle(2); + + lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); + lFlap = mcfg.midrc - lFlap; // shouldn't this be servoConf[2].middle? + if (slow_LFlaps < lFlap) + slow_LFlaps += mcfg.flaps_speed; + else if (slow_LFlaps > lFlap) + slow_LFlaps -= mcfg.flaps_speed; + + servo[2] = ((int32_t)cfg.servoConf[2].rate * slow_LFlaps) / 100L; + servo[2] += mcfg.midrc; + } + + if (f.PASSTHRU_MODE) { // Direct passthru from RX + servo[3] = rcCommand[ROLL] + flapperons[0]; // Wing 1 + servo[4] = rcCommand[ROLL] + flapperons[1]; // Wing 2 + servo[5] = rcCommand[YAW]; // Rudder + servo[6] = rcCommand[PITCH]; // Elevator + } else { + // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui + servo[3] = axisPID[ROLL] + flapperons[0]; // Wing 1 + servo[4] = axisPID[ROLL] + flapperons[1]; // Wing 2 + servo[5] = axisPID[YAW]; // Rudder + servo[6] = axisPID[PITCH]; // Elevator + } + for (i = 3; i < 7; i++) { + servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates + servo[i] += servoMiddle(i); + } +} + +void mixTable(void) +{ + int16_t maxMotor; + uint32_t i; + + if (numberMotor > 3) { + // prevent "yaw jump" during yaw correction + axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW])); + } + + // motors for non-servo mixes + if (numberMotor > 1) + for (i = 0; i < numberMotor; i++) + 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 (mcfg.mixerConfiguration) { + case MULTITYPE_BI: + servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + servoMiddle(4); // LEFT + servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + servoMiddle(5); // RIGHT + break; + + case MULTITYPE_TRI: + servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR + break; + + case MULTITYPE_GIMBAL: + servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0); + servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1); + break; + + case MULTITYPE_AIRPLANE: + airplaneMixer(); + break; + + case MULTITYPE_FLYING_WING: + if (!f.ARMED) + servo[7] = mcfg.mincommand; + else + servo[7] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); + motor[0] = servo[7]; + if (f.PASSTHRU_MODE) { + // do not use sensors for correction, simple 2 channel mixing + servo[3] = (servoDirection(3, 1) * rcCommand[PITCH]) + (servoDirection(3, 2) * rcCommand[ROLL]); + servo[4] = (servoDirection(4, 1) * rcCommand[PITCH]) + (servoDirection(4, 2) * rcCommand[ROLL]); + } else { + // use sensors to correct (gyro only or gyro + acc) + servo[3] = (servoDirection(3, 1) * axisPID[PITCH]) + (servoDirection(3, 2) * axisPID[ROLL]); + servo[4] = (servoDirection(4, 1) * axisPID[PITCH]) + (servoDirection(4, 2) * axisPID[ROLL]); + } + servo[3] += servoMiddle(3); + servo[4] += servoMiddle(4); + break; + + case MULTITYPE_DUALCOPTER: + for (i = 4; i < 6; i++ ) { + servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction + servo[i] += servoMiddle(i); + } + break; + + case MULTITYPE_SINGLECOPTER: + for (i = 3; i < 7; i++) { + servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction + servo[i] += servoMiddle(i); + } + motor[0] = rcCommand[THROTTLE]; + break; + } + + // do camstab + if (feature(FEATURE_SERVO_TILT)) { + // center at fixed position, or vary either pitch or roll by RC channel + servo[0] = servoMiddle(0); + servo[1] = servoMiddle(1); + + if (rcOptions[BOXCAMSTAB]) { + if (cfg.gimbal_flags & GIMBAL_MIXTILT) { + servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; + servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; + } else { + servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; + servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL] / 50; + } + } + } + + // constrain servos + for (i = 0; i < MAX_SERVOS; i++) + servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values + + // forward AUX1-4 to servo outputs (not constrained) + if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { + int offset = 0; + if (feature(FEATURE_SERVO_TILT)) + offset = 2; + for (i = 0; i < 4; i++) + pwmWriteServo(i + offset, rcData[AUX1 + i]); + } + + maxMotor = motor[0]; + for (i = 1; i < numberMotor; i++) + if (motor[i] > maxMotor) + maxMotor = motor[i]; + for (i = 0; i < numberMotor; i++) { + 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; + if (feature(FEATURE_3D)) { + if ((rcData[THROTTLE]) > 1500) { + motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); + } else { + motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); + } + } else { + motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); + if ((rcData[THROTTLE]) < mcfg.mincheck) { + if (!feature(FEATURE_MOTOR_STOP)) + motor[i] = mcfg.minthrottle; + else + motor[i] = mcfg.mincommand; + } + } + if (!f.ARMED) { + motor[i] = motor_disarmed[i]; + } + } +} From bf5b70f4e2f2b65006bb4630097fb0ce18395fa7 Mon Sep 17 00:00:00 2001 From: treymarc Date: Wed, 1 Jan 2014 10:23:48 +0100 Subject: [PATCH 7/7] remove newlib stub , add mixer correction --- Makefile | 1 - src/mixer.c | 24 ++-- src/newlib_stubs.c | 278 --------------------------------------------- 3 files changed, 12 insertions(+), 291 deletions(-) delete mode 100644 src/newlib_stubs.c diff --git a/Makefile b/Makefile index 34e8c1458a..174a62fe4f 100644 --- a/Makefile +++ b/Makefile @@ -42,7 +42,6 @@ BIN_DIR = $(ROOT)/obj # Source files common to all targets COMMON_SRC = startup_stm32f10x_md_gcc.S \ - newlib_stubs.c \ buzzer.c \ cli.c \ config.c \ diff --git a/src/mixer.c b/src/mixer.c index f0c0ecf2c5..8c71a44c3a 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -43,12 +43,12 @@ static const motorMixer_t mixerY6[] = { }; static const motorMixer_t mixerHex6P[] = { - { 1.0f, -1.0f, 0.866025f, 1.0f }, // REAR_R - { 1.0f, -1.0f, -0.866025f, -1.0f }, // FRONT_R - { 1.0f, 1.0f, 0.866025f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -0.866025f, -1.0f }, // FRONT_L - { 1.0f, 0.0f, -0.866025f, 1.0f }, // FRONT - { 1.0f, 0.0f, 0.866025f, -1.0f }, // REAR + { 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R + { 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R + { 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L + { 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L + { 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR }; static const motorMixer_t mixerY4[] = { @@ -59,12 +59,12 @@ static const motorMixer_t mixerY4[] = { }; static const motorMixer_t mixerHex6X[] = { - { 1.0f, -0.866025f, 1.0f, 1.0f }, // REAR_R - { 1.0f, -0.866025f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 0.866025f, 1.0f, -1.0f }, // REAR_L - { 1.0f, 0.866025f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -0.866025f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 0.866025f, 0.0f, 1.0f }, // LEFT + { 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R + { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R + { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L + { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT }; static const motorMixer_t mixerOctoX8[] = { diff --git a/src/newlib_stubs.c b/src/newlib_stubs.c deleted file mode 100644 index 706e893fee..0000000000 --- a/src/newlib_stubs.c +++ /dev/null @@ -1,278 +0,0 @@ - -/* - * newlib_stubs.c - * - * Created on: 2 Nov 2010 - * Author: nanoage.co.uk - */ -#include -#include -#include -#include -#include "stm32f10x_usart.h" - - -#ifndef STDOUT_USART -#define STDOUT_USART 2 -#endif - -#ifndef STDERR_USART -#define STDERR_USART 2 -#endif - -#ifndef STDIN_USART -#define STDIN_USART 2 -#endif - -#undef errno -extern int errno; - -/* - environ - A pointer to a list of environment variables and their values. - For a minimal environment, this empty list is adequate: - */ -char *__env[1] = { 0 }; -char **environ = __env; - -int _write(int file, char *ptr, int len); - -void _exit(int status) { - _write(1, "exit", 4); - while (1) { - ; - } -} - -int _close(int file) { - return -1; -} -/* - execve - Transfer control to a new process. Minimal implementation (for a system without processes): - */ -int _execve(char *name, char **argv, char **env) { - errno = ENOMEM; - return -1; -} -/* - fork - Create a new process. Minimal implementation (for a system without processes): - */ - -int _fork() { - errno = EAGAIN; - return -1; -} -/* - fstat - Status of an open file. For consistency with other minimal implementations in these examples, - all files are regarded as character special devices. - The `sys/stat.h' header file required is distributed in the `include' subdirectory for this C library. - */ -int _fstat(int file, struct stat *st) { - st->st_mode = S_IFCHR; - return 0; -} - -/* - getpid - Process-ID; this is sometimes used to generate strings unlikely to conflict with other processes. Minimal implementation, for a system without processes: - */ - -int _getpid() { - return 1; -} - -/* - isatty - Query whether output stream is a terminal. For consistency with the other minimal implementations, - */ -int _isatty(int file) { - switch (file){ - case STDOUT_FILENO: - case STDERR_FILENO: - case STDIN_FILENO: - return 1; - default: - //errno = ENOTTY; - errno = EBADF; - return 0; - } -} - - -/* - kill - Send a signal. Minimal implementation: - */ -int _kill(int pid, int sig) { - errno = EINVAL; - return (-1); -} - -/* - link - Establish a new name for an existing file. Minimal implementation: - */ - -int _link(char *old, char *new) { - errno = EMLINK; - return -1; -} - -/* - lseek - Set position in a file. Minimal implementation: - */ -int _lseek(int file, int ptr, int dir) { - return 0; -} - -/* - sbrk - Increase program data space. - Malloc and related functions depend on this - */ -caddr_t _sbrk(int incr) { - - extern char _ebss; // Defined by the linker - static char *heap_end; - char *prev_heap_end; - - if (heap_end == 0) { - heap_end = &_ebss; - } - prev_heap_end = heap_end; - -char * stack = (char*) __get_MSP(); - if (heap_end + incr > stack) - { - _write (STDERR_FILENO, "Heap and stack collision\n", 25); - errno = ENOMEM; - return (caddr_t) -1; - //abort (); - } - - heap_end += incr; - return (caddr_t) prev_heap_end; - -} - -/* - read - Read a character to a file. `libc' subroutines will use this system routine for input from all files, including stdin - Returns -1 on error or blocks until the number of characters have been read. - */ - - -int _read(int file, char *ptr, int len) { - int n; - int num = 0; - switch (file) { - case STDIN_FILENO: - for (n = 0; n < len; n++) { -#if STDIN_USART == 1 - while ((USART1->SR & USART_FLAG_RXNE) == (uint16_t)RESET) {} - char c = (char)(USART1->DR & (uint16_t)0x01FF); -#elif STDIN_USART == 2 - while ((USART2->SR & USART_FLAG_RXNE) == (uint16_t) RESET) {} - char c = (char) (USART2->DR & (uint16_t) 0x01FF); -#elif STDIN_USART == 3 - while ((USART3->SR & USART_FLAG_RXNE) == (uint16_t)RESET) {} - char c = (char)(USART3->DR & (uint16_t)0x01FF); -#endif - *ptr++ = c; - num++; - } - break; - default: - errno = EBADF; - return -1; - } - return num; -} - -/* - stat - Status of a file (by name). Minimal implementation: - int _EXFUN(stat,( const char *__path, struct stat *__sbuf )); - */ - -int _stat(const char *filepath, struct stat *st) { - st->st_mode = S_IFCHR; - return 0; -} - -/* - times - Timing information for current process. Minimal implementation: - */ - -clock_t _times(struct tms *buf) { - return -1; -} - -/* - unlink - Remove a file's directory entry. Minimal implementation: - */ -int _unlink(char *name) { - errno = ENOENT; - return -1; -} - -/* - wait - Wait for a child process. Minimal implementation: - */ -int _wait(int *status) { - errno = ECHILD; - return -1; -} - -/* - write - Write a character to a file. `libc' subroutines will use this system routine for output to all files, including stdout - Returns -1 on error or number of bytes sent - */ -int _write(int file, char *ptr, int len) { - int n; - switch (file) { - case STDOUT_FILENO: /*stdout*/ - for (n = 0; n < len; n++) { -#if STDOUT_USART == 1 - while ((USART1->SR & USART_FLAG_TC) == (uint16_t)RESET) {} - USART1->DR = (*ptr++ & (uint16_t)0x01FF); -#elif STDOUT_USART == 2 - while ((USART2->SR & USART_FLAG_TC) == (uint16_t) RESET) { - } - USART2->DR = (*ptr++ & (uint16_t) 0x01FF); -#elif STDOUT_USART == 3 - while ((USART3->SR & USART_FLAG_TC) == (uint16_t)RESET) {} - USART3->DR = (*ptr++ & (uint16_t)0x01FF); -#endif - } - break; - case STDERR_FILENO: /* stderr */ - for (n = 0; n < len; n++) { -#if STDERR_USART == 1 - while ((USART1->SR & USART_FLAG_TC) == (uint16_t)RESET) {} - USART1->DR = (*ptr++ & (uint16_t)0x01FF); -#elif STDERR_USART == 2 - while ((USART2->SR & USART_FLAG_TC) == (uint16_t) RESET) { - } - USART2->DR = (*ptr++ & (uint16_t) 0x01FF); -#elif STDERR_USART == 3 - while ((USART3->SR & USART_FLAG_TC) == (uint16_t)RESET) {} - USART3->DR = (*ptr++ & (uint16_t)0x01FF); -#endif - } - break; - default: - errno = EBADF; - return -1; - } - return len; -} -