diff --git a/src/board.h b/src/board.h index a41e734c..3b94330d 100755 --- a/src/board.h +++ b/src/board.h @@ -75,6 +75,7 @@ typedef enum { FEATURE_POWERMETER = 1 << 12, FEATURE_VARIO = 1 << 13, FEATURE_3D = 1 << 14, + FEATURE_REMOTEGAINS = 1 << 15, } AvailableFeatures; typedef enum { diff --git a/src/cli.c b/src/cli.c index 8fc97945..3b19fae4 100644 --- a/src/cli.c +++ b/src/cli.c @@ -55,6 +55,7 @@ static const char * const featureNames[] = { "PPM", "VBAT", "INFLIGHT_ACC_CAL", "SERIALRX", "MOTOR_STOP", "SERVO_TILT", "SOFTSERIAL", "LED_RING", "GPS", "FAILSAFE", "SONAR", "TELEMETRY", "POWERMETER", "VARIO", "3D", + "REMOTEGAINS", NULL }; @@ -223,6 +224,26 @@ const clivalue_t valueTable[] = { { "p_vel", VAR_UINT8, &cfg.P8[PIDVEL], 0, 200 }, { "i_vel", VAR_UINT8, &cfg.I8[PIDVEL], 0, 200 }, { "d_vel", VAR_UINT8, &cfg.D8[PIDVEL], 0, 200 }, + { "rg_1_mode", VAR_UINT8, &mcfg.remote_gain_settings[0].mode, 0, 3 }, + { "rg_1_min", VAR_UINT8, &mcfg.remote_gain_settings[0].min, 0, 255 }, + { "rg_1_max", VAR_UINT8, &mcfg.remote_gain_settings[0].max, 0, 255 }, + { "rg_1_source", VAR_UINT8, &mcfg.remote_gain_settings[0].source, 1, 4 }, + { "rg_1_dest", VAR_UINT8, &mcfg.remote_gain_settings[0].dest, 0, (3 * PIDITEMS) }, + { "rg_2_mode", VAR_UINT8, &mcfg.remote_gain_settings[1].mode, 0, 3 }, + { "rg_2_min", VAR_UINT8, &mcfg.remote_gain_settings[1].min, 0, 255 }, + { "rg_2_max", VAR_UINT8, &mcfg.remote_gain_settings[1].max, 0, 255 }, + { "rg_2_source", VAR_UINT8, &mcfg.remote_gain_settings[1].source, 1, 4 }, + { "rg_2_dest", VAR_UINT8, &mcfg.remote_gain_settings[1].dest, 0, (3 * PIDITEMS) }, + { "rg_3_mode", VAR_UINT8, &mcfg.remote_gain_settings[2].mode, 0, 3 }, + { "rg_3_min", VAR_UINT8, &mcfg.remote_gain_settings[2].min, 0, 255 }, + { "rg_3_max", VAR_UINT8, &mcfg.remote_gain_settings[2].max, 0, 255 }, + { "rg_3_source", VAR_UINT8, &mcfg.remote_gain_settings[2].source, 1, 4 }, + { "rg_3_dest", VAR_UINT8, &mcfg.remote_gain_settings[2].dest, 0, (3 * PIDITEMS) }, + { "rg_4_mode", VAR_UINT8, &mcfg.remote_gain_settings[3].mode, 0, 3 }, + { "rg_4_min", VAR_UINT8, &mcfg.remote_gain_settings[3].min, 0, 255 }, + { "rg_4_max", VAR_UINT8, &mcfg.remote_gain_settings[3].max, 0, 255 }, + { "rg_4_source", VAR_UINT8, &mcfg.remote_gain_settings[3].source, 1, 4 }, + { "rg_4_dest", VAR_UINT8, &mcfg.remote_gain_settings[3].dest, 0, (3 * PIDITEMS) }, }; #define VALUE_COUNT (sizeof(valueTable) / sizeof(clivalue_t)) diff --git a/src/config.c b/src/config.c index 29a21d79..43984964 100755 --- a/src/config.c +++ b/src/config.c @@ -230,7 +230,15 @@ static void resetConf(void) mcfg.looptime = 3500; mcfg.emf_avoidance = 0; mcfg.rssi_aux_channel = 0; - + + for (i = 0; i < NUM_REMOTE_GAINS; i++) { + mcfg.remote_gain_settings[i].mode = REMOTE_GAIN_DISABLED; + mcfg.remote_gain_settings[i].min = 20; // Don't set to 0 by default just in case user has bad ideas + mcfg.remote_gain_settings[i].max = 200; + mcfg.remote_gain_settings[i].source = i + 1; + mcfg.remote_gain_settings[i].dest = 0; + } + cfg.pidController = 0; cfg.P8[ROLL] = 40; cfg.I8[ROLL] = 30; diff --git a/src/main.c b/src/main.c index 8726d492..049efff9 100755 --- a/src/main.c +++ b/src/main.c @@ -10,6 +10,9 @@ extern rcReadRawDataPtr rcReadRawFunc; // receiver read function extern uint16_t pwmReadRawRC(uint8_t chan); +// external vars (ugh) +extern int16_t failsafeCnt; + #ifdef USE_LAME_PRINTF // gcc/GNU version static void _putc(void *p, char c) @@ -99,6 +102,9 @@ int main(void) rcData[i] = 1502; rcReadRawFunc = pwmReadRawRC; core.numRCChannels = MAX_INPUTS; + if (feature(FEATURE_FAILSAFE)) { + failsafeCnt = 1000; // We're in failsafe until we get a valid signal for 1 second on powerup + } if (feature(FEATURE_SERIALRX)) { switch (mcfg.serialrx_type) { diff --git a/src/mw.c b/src/mw.c index c76664c2..2e9c175a 100755 --- a/src/mw.c +++ b/src/mw.c @@ -765,6 +765,57 @@ void loop(void) } else { f.PASSTHRU_MODE = 0; } + + if (feature(FEATURE_REMOTEGAINS)) { + int i; + for (i = 0; i < NUM_REMOTE_GAINS; i++) { + int workToDo = 0; + switch (mcfg.remote_gain_settings[i].mode) { + case REMOTE_GAIN_AUX: + if (rcOptions[BOXREMOTEGAINS]) { + workToDo = 1; + } + break; + case REMOTE_GAIN_DISARM: + if (!f.ARMED) { + workToDo = 1; + } + break; + case REMOTE_GAIN_ALWAYS: + workToDo = 1; + break; + } + // Inhibit any adjustment if we have no valid signal + if (feature(FEATURE_FAILSAFE)) { + if (failsafeCnt > 2) { + workToDo = 0; + } + } + if (workToDo) { + uint32_t val = rcData[AUX1 + mcfg.remote_gain_settings[i].source - 1]; + // Constrain input + val = min(val, mcfg.maxcheck); + val = max(val, mcfg.mincheck); + // Scale to defined range + val = (mcfg.remote_gain_settings[i].max - mcfg.remote_gain_settings[i].min) * ((val - mcfg.mincheck)); + val = val / ((mcfg.maxcheck - mcfg.mincheck)); + val = val + mcfg.remote_gain_settings[i].min; + + if (mcfg.remote_gain_settings[i].dest < PIDITEMS) { + cfg.P8[mcfg.remote_gain_settings[i].dest] = val; + } + else if (mcfg.remote_gain_settings[i].dest < (2 * PIDITEMS)) { + cfg.I8[mcfg.remote_gain_settings[i].dest - PIDITEMS] = val; + } + else if (mcfg.remote_gain_settings[i].dest < (3 * PIDITEMS)) { + cfg.D8[mcfg.remote_gain_settings[i].dest - (2 * PIDITEMS)] = val; + } + else if (mcfg.remote_gain_settings[i].dest == (3 * PIDITEMS)) { + cfg.dynThrPID = val; + } + } + } + } if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) { f.HEADFREE_MODE = 0; diff --git a/src/mw.h b/src/mw.h index 91a10e10..cf7af0ca 100755 --- a/src/mw.h +++ b/src/mw.h @@ -102,6 +102,7 @@ enum { BOXGOV, BOXOSD, BOXTELEMETRY, + BOXREMOTEGAINS, CHECKBOXITEMS }; @@ -150,6 +151,22 @@ enum { #define CALIBRATING_ACC_CYCLES 400 #define CALIBRATING_BARO_CYCLES 200 +#define NUM_REMOTE_GAINS 4 // 4 sets allow for one of each AUX channel +enum { + REMOTE_GAIN_DISABLED = 0, + REMOTE_GAIN_AUX, + REMOTE_GAIN_DISARM, + REMOTE_GAIN_ALWAYS +}; + +typedef struct remotegain_t { + uint8_t mode; // Enable remote adjustment (disabled, enabled by AUX channel, enabled when armed, always enabled - see enum above) + uint8_t min; // Value at min PWM + uint8_t max; // Value at max PWM + uint8_t source; // Which AUX channel to use (1-4) + uint8_t dest; // What to adjust (P * PIDITEMS, I * PIDITEMS, D * PIDITEMS) +} remotegain_t; + typedef struct config_t { uint8_t pidController; // 0 = multiwii original, 1 = rewrite from http://www.multiwii.com/forum/viewtopic.php?f=8&t=3671 uint8_t P8[PIDITEMS]; @@ -282,6 +299,8 @@ typedef struct master_t { uint32_t softserial_baudrate; // shared by both soft serial ports uint8_t softserial_1_inverted; // use inverted softserial input and output signals on port 1 uint8_t softserial_2_inverted; // use inverted softserial input and output signals on port 2 + + remotegain_t remote_gain_settings[NUM_REMOTE_GAINS]; // Settings for remote gain adjustments uint8_t telemetry_provider; // See TelemetryProvider enum. uint8_t telemetry_port; // See TelemetryPort enum. diff --git a/src/serial.c b/src/serial.c index ee59c8a7..4d6a53a3 100755 --- a/src/serial.c +++ b/src/serial.c @@ -62,6 +62,8 @@ #define MSP_ACC_TRIM 240 //out message get acc angle trim values #define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values #define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox) +#define MSP_REMOTEGAINS 170 //out message 4 remotegain_t gain adjustment sets +#define MSP_SET_REMOTEGAINS 171 //in message 4 remotegain_t gain adjustment sets #define INBUF_SIZE 64 @@ -91,6 +93,7 @@ struct box_t { { BOXGOV, "GOVERNOR;", 18 }, { BOXOSD, "OSD SW;", 19 }, { BOXTELEMETRY, "TELEMETRY;", 20 }, + { BOXREMOTEGAINS, "REMOTE GAINS;", 21 }, { CHECKBOXITEMS, NULL, 0xFF } }; @@ -272,6 +275,8 @@ void serialInit(uint32_t baudrate) availableBoxes[idx++] = BOXOSD; if (feature(FEATURE_TELEMETRY && mcfg.telemetry_switch)) availableBoxes[idx++] = BOXTELEMETRY; + if (feature(FEATURE_REMOTEGAINS)) + availableBoxes[idx++] = BOXREMOTEGAINS; numberBoxItems = idx; } @@ -388,6 +393,7 @@ static void evaluateCommand(void) rcOptions[BOXGOV] << BOXGOV | rcOptions[BOXOSD] << BOXOSD | rcOptions[BOXTELEMETRY] << BOXTELEMETRY | + rcOptions[BOXREMOTEGAINS] << BOXREMOTEGAINS | f.ARMED << BOXARM; for (i = 0; i < numberBoxItems; i++) { int flag = (tmp & (1 << availableBoxes[i])); @@ -627,6 +633,26 @@ static void evaluateCommand(void) serialize8(GPS_svinfo_cno[i]); } break; + case MSP_REMOTEGAINS: + headSerialReply(NUM_REMOTE_GAINS * sizeof(remotegain_t)); + for (i = 0; i < NUM_REMOTE_GAINS; i++) { + serialize8(mcfg.remote_gain_settings[i].mode); + serialize8(mcfg.remote_gain_settings[i].min); + serialize8(mcfg.remote_gain_settings[i].max); + serialize8(mcfg.remote_gain_settings[i].source); + serialize8(mcfg.remote_gain_settings[i].dest); + } + break; + case MSP_SET_REMOTEGAINS: + for (i = 0; i < NUM_REMOTE_GAINS; i++) { + mcfg.remote_gain_settings[i].mode = read8(); + mcfg.remote_gain_settings[i].min = read8(); + mcfg.remote_gain_settings[i].max = read8(); + mcfg.remote_gain_settings[i].source = read8(); + mcfg.remote_gain_settings[i].dest = read8(); + } + headSerialReply(0); + break; default: // we do not know how to handle the (valid) message, indicate error MSP $M! headSerialError(0); break;