diff --git a/src/config.c b/src/config.c index 9bfa334dea..691e8a6014 100755 --- a/src/config.c +++ b/src/config.c @@ -39,7 +39,7 @@ void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct -static const uint8_t EEPROM_CONF_VERSION = 62; +static const uint8_t EEPROM_CONF_VERSION = 63; static void resetConf(void); static uint8_t validEEPROM(void) @@ -291,10 +291,11 @@ static void resetConf(void) // servos for (i = 0; i < 8; i++) { - cfg.servoConf[i].min = 1020; - cfg.servoConf[i].max = 2000; - cfg.servoConf[i].middle = 1500; + cfg.servoConf[i].min = DEFAULT_SERVO_MIN; + cfg.servoConf[i].max = DEFAULT_SERVO_MAX; + cfg.servoConf[i].middle = DEFAULT_SERVO_MIDDLE; cfg.servoConf[i].rate = servoRates[i]; + cfg.servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } cfg.yaw_direction = 1; diff --git a/src/flight_mixer.c b/src/flight_mixer.c index 0f2739b754..7fe508ebd6 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -155,14 +155,19 @@ const mixer_t mixers[] = { { 0, 0, NULL }, // MULTITYPE_CUSTOM }; -int16_t servoMiddle(int nr) +int16_t determineServoMiddleOrForwardFromChannel(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 < MAX_SUPPORTED_RC_CHANNEL_COUNT && nr < MAX_SERVOS) - return rcData[cfg.servoConf[nr].middle]; - else + uint8_t channelToForwardFrom = cfg.servoConf[nr].forwardFromChannel; + + if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom << MAX_SUPPORTED_RC_CHANNEL_COUNT) { + return rcData[channelToForwardFrom]; + } + + if (nr < MAX_SERVOS) { return cfg.servoConf[nr].middle; + } + + return DEFAULT_SERVO_MIDDLE; } int servoDirection(int nr, int lr) @@ -350,7 +355,7 @@ static void airplaneMixer(void) // 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); + int16_t lFlap = determineServoMiddleOrForwardFromChannel(2); lFlap = constrain(lFlap, cfg.servoConf[2].min, cfg.servoConf[2].max); lFlap = mcfg.rxConfig.midrc - lFlap; // shouldn't this be servoConf[2].middle? @@ -377,7 +382,7 @@ static void airplaneMixer(void) } for (i = 3; i < 7; i++) { servo[i] = ((int32_t)cfg.servoConf[i].rate * servo[i]) / 100L; // servo rates - servo[i] += servoMiddle(i); + servo[i] += determineServoMiddleOrForwardFromChannel(i); } } @@ -399,17 +404,17 @@ void mixTable(void) // 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 + servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4); // LEFT + servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5); // RIGHT break; case MULTITYPE_TRI: - servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR + servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(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); + servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + determineServoMiddleOrForwardFromChannel(0); + servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + determineServoMiddleOrForwardFromChannel(1); break; case MULTITYPE_AIRPLANE: @@ -431,21 +436,21 @@ void mixTable(void) 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); + servo[3] += determineServoMiddleOrForwardFromChannel(3); + servo[4] += determineServoMiddleOrForwardFromChannel(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); + servo[i] += determineServoMiddleOrForwardFromChannel(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); + servo[i] += determineServoMiddleOrForwardFromChannel(i); } motor[0] = rcCommand[THROTTLE]; break; @@ -454,8 +459,8 @@ void mixTable(void) // 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); + servo[0] = determineServoMiddleOrForwardFromChannel(0); + servo[1] = determineServoMiddleOrForwardFromChannel(1); if (rcOptions[BOXCAMSTAB]) { if (cfg.gimbal_flags & GIMBAL_MIXTILT) { diff --git a/src/flight_mixer.h b/src/flight_mixer.h index 264916bfca..d5c1919bcd 100644 --- a/src/flight_mixer.h +++ b/src/flight_mixer.h @@ -43,9 +43,12 @@ typedef struct mixer_t { const motorMixer_t *motor; } mixer_t; +#define CHANNEL_FORWARDING_DISABLED 0xFF + typedef struct servoParam_t { int16_t min; // servo min int16_t max; // servo max int16_t middle; // servo middle int8_t rate; // range [-100;+100] ; can be used to ajust a rate 0-100% and a direction + int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED } servoParam_t; diff --git a/src/rx_common.h b/src/rx_common.h index 31b300fae7..ef442078db 100644 --- a/src/rx_common.h +++ b/src/rx_common.h @@ -24,6 +24,10 @@ typedef enum rc_alias { #define THR_CE (3 << (2 * THROTTLE)) #define THR_HI (2 << (2 * THROTTLE)) +#define DEFAULT_SERVO_MIN 1020 +#define DEFAULT_SERVO_MIDDLE 1500 +#define DEFAULT_SERVO_MAX 2000 + typedef enum { SERIALRX_SPEKTRUM1024 = 0, SERIALRX_SPEKTRUM2048 = 1, diff --git a/src/serial_msp.c b/src/serial_msp.c index a60bd1e6bc..0efb0d4b6a 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -12,6 +12,7 @@ #define CAP_PLATFORM_32BIT ((uint32_t)1 << 31) #define CAP_DYNBALANCE ((uint32_t)1 << 2) #define CAP_FLAPS ((uint32_t)1 << 3) +#define CAP_CHANNEL_FORWARDING ((uint32_t)1 << 4) #define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable #define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number @@ -36,6 +37,7 @@ #define MSP_SERVO_CONF 120 //out message Servo settings #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters +#define MSP_CHANNEL_FORWARDING 123 //out message Returns channel forwarding settings #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -50,6 +52,7 @@ #define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2) #define MSP_SET_HEAD 211 //in message define a new heading hold direction #define MSP_SET_SERVO_CONF 212 //in message Servo settings +#define MSP_SET_CHANNEL_FORWARDING 213 //in message Channel forwarding settings #define MSP_SET_MOTOR 214 //in message PropBalance function #define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom @@ -367,7 +370,7 @@ static void evaluateCommand(void) serialize8(VERSION); // multiwii version serialize8(mcfg.mixerConfiguration); // type of multicopter serialize8(MSP_VERSION); // MultiWii Serial Protocol Version - serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0)); // "capability" + serialize32(CAP_PLATFORM_32BIT | CAP_DYNBALANCE | (mcfg.flaps_speed ? CAP_FLAPS : 0) | CAP_CHANNEL_FORWARDING); // "capability" break; case MSP_STATUS: headSerialReply(11); @@ -436,6 +439,18 @@ static void evaluateCommand(void) cfg.servoConf[i].rate = read8(); } break; + case MSP_CHANNEL_FORWARDING: + headSerialReply(8); + for (i = 0; i < MAX_SERVOS; i++) { + serialize8(cfg.servoConf[i].forwardFromChannel); + } + break; + case MSP_SET_CHANNEL_FORWARDING: + headSerialReply(0); + for (i = 0; i < MAX_SERVOS; i++) { + cfg.servoConf[i].forwardFromChannel = read8(); + } + break; case MSP_MOTOR: s_struct((uint8_t *)motor, 16); break;