diff --git a/src/cli.c b/src/cli.c index 60c85a8c..c43eb093 100644 --- a/src/cli.c +++ b/src/cli.c @@ -194,13 +194,18 @@ const clivalue_t valueTable[] = { { "alt_hold_fast_change", VAR_UINT8, &cfg.alt_hold_fast_change, 0, 1 }, { "throttle_correction_value", VAR_UINT8, &cfg.throttle_correction_value, 0, 150 }, { "throttle_correction_angle", VAR_UINT16, &cfg.throttle_correction_angle, 1, 900 }, - { "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 }, - { "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 }, + { "rc_rate", VAR_UINT8, &cfg.rcRate8[ACRO_OFF], 0, 250 }, + { "rc_expo", VAR_UINT8, &cfg.rcExpo8[ACRO_OFF], 0, 100 }, + { "rc_rate_acro", VAR_UINT8, &cfg.rcRate8[ACRO_ON], 0, 250 }, + { "rc_expo_acro", VAR_UINT8, &cfg.rcExpo8[ACRO_ON], 0, 100 }, { "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 }, { "thr_expo", VAR_UINT8, &cfg.thrExpo8, 0, 100 }, - { "roll_rate", VAR_UINT8, &cfg.rollPitchRate[0], 0, 100 }, - { "pitch_rate", VAR_UINT8, &cfg.rollPitchRate[1], 0, 100 }, - { "yaw_rate", VAR_UINT8, &cfg.yawRate, 0, 100 }, + { "roll_rate", VAR_UINT8, &cfg.rollPitchRate[ACRO_OFF][0], 0, 100 }, + { "pitch_rate", VAR_UINT8, &cfg.rollPitchRate[ACRO_OFF][1], 0, 100 }, + { "roll_rate_acro", VAR_UINT8, &cfg.rollPitchRate[ACRO_ON][0], 0, 100 }, + { "pitch_rate_acro", VAR_UINT8, &cfg.rollPitchRate[ACRO_ON][1], 0, 100 }, + { "yaw_rate", VAR_UINT8, &cfg.yawRate[ACRO_OFF], 0, 100 }, + { "yaw_rate_acro", VAR_UINT8, &cfg.yawRate[ACRO_ON], 0, 100 }, { "tpa_rate", VAR_UINT8, &cfg.dynThrPID, 0, 100}, { "tpa_breakpoint", VAR_UINT16, &cfg.tpa_breakpoint, 1000, 2000}, { "failsafe_delay", VAR_UINT8, &cfg.failsafe_delay, 0, 200 }, @@ -1046,7 +1051,7 @@ static void cliMixer(char *cmdline) // Really Ugly Hack if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) { cfg.dynThrPID = 50; - cfg.rcExpo8 = 0; + cfg.rcExpo8[ACRO_OFF] = 0; cfg.P8[PIDALT] = 30; cfg.I8[PIDALT] = 20; cfg.D8[PIDALT] = 45; diff --git a/src/config.c b/src/config.c index 0668a212..38e18e23 100755 --- a/src/config.c +++ b/src/config.c @@ -90,7 +90,10 @@ void activateConfig(void) { uint8_t i; for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) - lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t)cfg.rcRate8 / 2500; + lookupPitchRollRC[ACRO_OFF][i] = (2500 + cfg.rcExpo8[ACRO_OFF] * (i * i - 25)) * i * (int32_t)cfg.rcRate8[ACRO_OFF] / 2500; + + for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) + lookupPitchRollRC[ACRO_ON][i] = (2500 + cfg.rcExpo8[ACRO_ON] * (i * i - 25)) * i * (int32_t)cfg.rcRate8[ACRO_ON] / 2500; for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { int16_t tmp = 10 * i - cfg.thrMid8; @@ -283,9 +286,12 @@ static void resetConf(void) cfg.P8[PIDVEL] = 120; cfg.I8[PIDVEL] = 45; cfg.D8[PIDVEL] = 1; - cfg.rcRate8 = 90; - cfg.rcExpo8 = 65; - cfg.yawRate = 0; + cfg.rcRate8[ACRO_OFF] = 90; + cfg.rcExpo8[ACRO_OFF] = 65; + cfg.yawRate[ACRO_OFF] = 0; + cfg.rcRate8[ACRO_ON] = 90; + cfg.rcExpo8[ACRO_ON] = 65; + cfg.yawRate[ACRO_ON] = 0; cfg.dynThrPID = 0; cfg.tpa_breakpoint = 1500; cfg.thrMid8 = 50; diff --git a/src/mw.c b/src/mw.c index 28d6987a..4548a7ae 100755 --- a/src/mw.c +++ b/src/mw.c @@ -26,7 +26,7 @@ int16_t failsafeCnt = 0; int16_t failsafeEvents = 0; int16_t rcData[RC_CHANS]; // interval [1000;2000] int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL +int16_t lookupPitchRollRC[2][PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE uint16_t rssi; // range: [0;1023] rcReadRawDataPtr rcReadRawFunc = NULL; // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers) @@ -40,6 +40,8 @@ uint8_t rcOptions[CHECKBOXITEMS]; int16_t axisPID[3]; +bool acroState; + // ********************** // GPS // ********************** @@ -109,6 +111,8 @@ void annexCode(void) static int64_t mAhdrawnRaw = 0; static int32_t vbatCycleTime = 0; + // Set the state to avoid changing mid loop + acroState = rcOptions[BOXACROSWITCH]; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < cfg.tpa_breakpoint) { prop2 = 100; @@ -132,8 +136,8 @@ void annexCode(void) } tmp2 = tmp / 100; - rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; - prop1 = 100 - (uint16_t)cfg.rollPitchRate[axis] * tmp / 500; + rcCommand[axis] = lookupPitchRollRC[acroState][tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[acroState][tmp2 + 1] - lookupPitchRollRC[acroState][tmp2]) / 100; + prop1 = 100 - (uint16_t)cfg.rollPitchRate[acroState][axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else { // YAW if (cfg.yawdeadband) { @@ -144,7 +148,7 @@ void annexCode(void) } } rcCommand[axis] = tmp * -mcfg.yaw_control_direction; - prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500; + prop1 = 100 - (uint16_t)cfg.yawRate[acroState] * abs(tmp) / 500; } dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; @@ -400,12 +404,12 @@ static void pidRewrite(void) for (axis = 0; axis < 3; axis++) { // -----Get the desired angle rate depending on flight mode if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand) - AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[YAW]) >> 5); + AngleRateTmp = (((int32_t)(cfg.yawRate[acroState] + 27) * rcCommand[YAW]) >> 5); } else { // calculate error and limit the angle to 50 degrees max inclination errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]) / 10.0f; // 16 bits is ok here if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID - AngleRateTmp = ((int32_t)(cfg.rollPitchRate[axis] + 27) * rcCommand[axis]) >> 4; + AngleRateTmp = ((int32_t)(cfg.rollPitchRate[acroState][axis] + 27) * rcCommand[axis]) >> 4; if (f.HORIZON_MODE) { // mix up angle error to desired AngleRateTmp to add a little auto-level feel diff --git a/src/mw.h b/src/mw.h index 01d4e934..03319441 100755 --- a/src/mw.h +++ b/src/mw.h @@ -17,6 +17,9 @@ #define RC_CHANS (18) +#define ACRO_OFF 0 +#define ACRO_ON 1 + // Serial GPS only variables // navigation mode typedef enum NavigationMode { @@ -121,6 +124,7 @@ enum { BOXSERVO1, BOXSERVO2, BOXSERVO3, + BOXACROSWITCH, CHECKBOXITEMS }; @@ -220,13 +224,13 @@ typedef struct config_t { uint8_t I8[PIDITEMS]; uint8_t D8[PIDITEMS]; - uint8_t rcRate8; - uint8_t rcExpo8; + uint8_t rcRate8[2]; + uint8_t rcExpo8[2]; uint8_t thrMid8; uint8_t thrExpo8; - uint8_t rollPitchRate[2]; - uint8_t yawRate; + uint8_t rollPitchRate[2][2]; + uint8_t yawRate[2]; uint8_t dynThrPID; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated @@ -476,8 +480,9 @@ extern int32_t mAhdrawn; // milli ampere hours drawn from battery s #define PITCH_LOOKUP_LENGTH 7 #define THROTTLE_LOOKUP_LENGTH 12 -extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL +extern int16_t lookupPitchRollRC[2][PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE +extern bool acroState; // GPS stuff extern int32_t GPS_coord[2]; diff --git a/src/serial.c b/src/serial.c index cbd93e0f..6006e040 100755 --- a/src/serial.c +++ b/src/serial.c @@ -115,6 +115,7 @@ static const box_t boxes[] = { { BOXSERVO1, "SERVO1;", 21 }, { BOXSERVO2, "SERVO2;", 22 }, { BOXSERVO3, "SERVO3;", 23 }, + { BOXACROSWITCH, "ACROSWITCH;", 24 }, { CHECKBOXITEMS, NULL, 0xFF } }; @@ -323,7 +324,7 @@ void serialInit(uint32_t baudrate) availableBoxes[idx++] = BOXSERVO2; availableBoxes[idx++] = BOXSERVO3; } - + availableBoxes[idx++] = BOXACROSWITCH; numberBoxItems = idx; } @@ -374,10 +375,10 @@ static void evaluateCommand(void) headSerialReply(0); break; case MSP_SET_RC_TUNING: - cfg.rcRate8 = read8(); - cfg.rcExpo8 = read8(); + cfg.rcRate8[ACRO_OFF] = read8(); + cfg.rcExpo8[ACRO_OFF] = read8(); read8(); // Legacy pitch-roll rate, read but not set. - cfg.yawRate = read8(); + cfg.yawRate[ACRO_OFF] = read8(); cfg.dynThrPID = read8(); cfg.thrMid8 = read8(); cfg.thrExpo8 = read8(); @@ -456,6 +457,7 @@ static void evaluateCommand(void) rcOptions[BOXSERVO1] << BOXSERVO1 | rcOptions[BOXSERVO2] << BOXSERVO2 | rcOptions[BOXSERVO3] << BOXSERVO3 | + rcOptions[BOXACROSWITCH] << BOXACROSWITCH | f.ARMED << BOXARM; for (i = 0; i < numberBoxItems; i++) { int flag = (tmp & (1 << availableBoxes[i])); @@ -621,10 +623,10 @@ static void evaluateCommand(void) break; case MSP_RC_TUNING: headSerialReply(7); - serialize8(cfg.rcRate8); - serialize8(cfg.rcExpo8); - serialize8(cfg.rollPitchRate[0]); // here for legacy support - serialize8(cfg.yawRate); + serialize8(cfg.rcRate8[ACRO_OFF]); + serialize8(cfg.rcExpo8[ACRO_OFF]); + serialize8(cfg.rollPitchRate[ACRO_OFF][0]); // here for legacy support + serialize8(cfg.yawRate[ACRO_OFF]); serialize8(cfg.dynThrPID); serialize8(cfg.thrMid8); serialize8(cfg.thrExpo8); @@ -810,8 +812,13 @@ static void evaluateCommand(void) mcfg.currentscale = read16(); mcfg.currentoffset = read16(); mcfg.motor_pwm_rate = read16(); - cfg.rollPitchRate[0] = read8(); - cfg.rollPitchRate[1] = read8(); + cfg.rollPitchRate[ACRO_OFF][0] = read8(); + cfg.rollPitchRate[ACRO_OFF][1] = read8(); + cfg.rollPitchRate[ACRO_ON][0] = read8(); + cfg.rollPitchRate[ACRO_ON][1] = read8(); + cfg.yawRate[ACRO_ON] = read8(); + cfg.rcExpo8[ACRO_ON] = read8(); + cfg.rcRate8[ACRO_ON] = read8(); mcfg.power_adc_channel = read8(); cfg.small_angle = read8(); mcfg.looptime = read16(); @@ -819,7 +826,7 @@ static void evaluateCommand(void) /// ??? break; case MSP_CONFIG: - headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 1 + 1 + 2 + 1); + headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 1 + 1 + 1 + 1 + 1 + 2 + 1); serialize8(mcfg.mixerConfiguration); serialize32(featureMask()); serialize8(mcfg.serialrx_type); @@ -829,8 +836,13 @@ static void evaluateCommand(void) serialize16(mcfg.currentscale); serialize16(mcfg.currentoffset); serialize16(mcfg.motor_pwm_rate); - serialize8(cfg.rollPitchRate[0]); - serialize8(cfg.rollPitchRate[1]); + serialize8(cfg.rollPitchRate[ACRO_OFF][0]); + serialize8(cfg.rollPitchRate[ACRO_OFF][1]); + serialize8(cfg.rollPitchRate[ACRO_ON][0]); + serialize8(cfg.rollPitchRate[ACRO_ON][1]); + serialize8(cfg.yawRate[ACRO_ON]); + serialize8(cfg.rcExpo8[ACRO_ON]); + serialize8(cfg.rcRate8[ACRO_ON]); serialize8(mcfg.power_adc_channel); serialize8(cfg.small_angle); serialize16(mcfg.looptime);