diff --git a/src/cli.c b/src/cli.c index bcc7b01026..b65d79220c 100644 --- a/src/cli.c +++ b/src/cli.c @@ -150,28 +150,7 @@ const clivalue_t valueTable[] = { { "rssi_aux_channel", VAR_INT8, &mcfg.rssi_aux_channel, 0, 4 }, { "yaw_direction", VAR_INT8, &cfg.yaw_direction, -1, 1 }, { "tri_unarmed_servo", VAR_INT8, &cfg.tri_unarmed_servo, 0, 1 }, - { "tri_yaw_middle", VAR_UINT16, &cfg.tri_yaw_middle, 0, 2000 }, - { "tri_yaw_min", VAR_UINT16, &cfg.tri_yaw_min, 0, 2000 }, - { "tri_yaw_max", VAR_UINT16, &cfg.tri_yaw_max, 0, 2000 }, - { "wing_left_min", VAR_UINT16, &cfg.wing_left_min, 0, 2000 }, - { "wing_left_mid", VAR_UINT16, &cfg.wing_left_mid, 0, 2000 }, - { "wing_left_max", VAR_UINT16, &cfg.wing_left_max, 0, 2000 }, - { "wing_right_min", VAR_UINT16, &cfg.wing_right_min, 0, 2000 }, - { "wing_right_mid", VAR_UINT16, &cfg.wing_right_mid, 0, 2000 }, - { "wing_right_max", VAR_UINT16, &cfg.wing_right_max, 0, 2000 }, - { "pitch_direction_l", VAR_INT8, &cfg.pitch_direction_l, -1, 1 }, - { "pitch_direction_r", VAR_INT8, &cfg.pitch_direction_r, -1, 1 }, - { "roll_direction_l", VAR_INT8, &cfg.roll_direction_l, -1, 1 }, - { "roll_direction_r", VAR_INT8, &cfg.roll_direction_r, -1, 1 }, { "gimbal_flags", VAR_UINT8, &cfg.gimbal_flags, 0, 255}, - { "gimbal_pitch_gain", VAR_INT8, &cfg.gimbal_pitch_gain, -100, 100 }, - { "gimbal_roll_gain", VAR_INT8, &cfg.gimbal_roll_gain, -100, 100 }, - { "gimbal_pitch_min", VAR_UINT16, &cfg.gimbal_pitch_min, 100, 3000 }, - { "gimbal_pitch_max", VAR_UINT16, &cfg.gimbal_pitch_max, 100, 3000 }, - { "gimbal_pitch_mid", VAR_UINT16, &cfg.gimbal_pitch_mid, 100, 3000 }, - { "gimbal_roll_min", VAR_UINT16, &cfg.gimbal_roll_min, 100, 3000 }, - { "gimbal_roll_max", VAR_UINT16, &cfg.gimbal_roll_max, 100, 3000 }, - { "gimbal_roll_mid", VAR_UINT16, &cfg.gimbal_roll_mid, 100, 3000 }, { "acc_lpf_factor", VAR_UINT8, &cfg.acc_lpf_factor, 0, 250 }, { "accxy_deadband", VAR_UINT8, &cfg.accxy_deadband, 0, 100 }, { "accz_deadband", VAR_UINT8, &cfg.accz_deadband, 0, 100 }, diff --git a/src/config.c b/src/config.c index 2cbbc22722..d9091c9c61 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static const uint8_t EEPROM_CONF_VERSION = 51; +static const uint8_t EEPROM_CONF_VERSION = 52; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -83,7 +83,6 @@ void readEEPROM(void) lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE] } - cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR setPIDController(cfg.pidController); GPS_set_pids(); } @@ -286,32 +285,9 @@ static void resetConf(void) cfg.yaw_direction = 1; cfg.tri_unarmed_servo = 1; - cfg.tri_yaw_middle = 1500; - cfg.tri_yaw_min = 1020; - cfg.tri_yaw_max = 2000; - - // flying wing - cfg.wing_left_min = 1020; - cfg.wing_left_mid = 1500; - cfg.wing_left_max = 2000; - cfg.wing_right_min = 1020; - cfg.wing_right_mid = 1500; - cfg.wing_right_max = 2000; - cfg.pitch_direction_l = 1; - cfg.pitch_direction_r = -1; - cfg.roll_direction_l = 1; - cfg.roll_direction_r = 1; // gimbal - cfg.gimbal_pitch_gain = 10; - cfg.gimbal_roll_gain = 10; cfg.gimbal_flags = GIMBAL_NORMAL; - cfg.gimbal_pitch_min = 1020; - cfg.gimbal_pitch_max = 2000; - cfg.gimbal_pitch_mid = 1500; - cfg.gimbal_roll_min = 1020; - cfg.gimbal_roll_max = 2000; - cfg.gimbal_roll_mid = 1500; // gps/nav stuff cfg.gps_wp_radius = 200; diff --git a/src/main.c b/src/main.c index 36f369a846..e9b582204d 100755 --- a/src/main.c +++ b/src/main.c @@ -81,6 +81,8 @@ int main(void) pwmInit(&pwm_params); // configure PWM/CPPM read function and max number of channels. spektrum or sbus below will override both of these, if enabled + for (i = 0; i < RC_CHANS; i++) + rcData[i] = 1502; rcReadRawFunc = pwmReadRawRC; core.numRCChannels = MAX_INPUTS; diff --git a/src/mixer.c b/src/mixer.c index 25ceabeda1..1b1e6bb52a 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -3,7 +3,7 @@ static uint8_t numberMotor = 0; int16_t motor[MAX_MOTORS]; -int16_t servo[8] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; +int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; static motorMixer_t currentMixer[MAX_MOTORS]; @@ -130,6 +130,30 @@ const mixer_t mixers[] = { { 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; @@ -319,17 +343,17 @@ void mixTable(void) // airplane / servo mixes switch (mcfg.mixerConfiguration) { case MULTITYPE_BI: - servo[4] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT - servo[5] = constrain(1500 + (-cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT + 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] = constrain(cfg.tri_yaw_middle + -cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR + servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + servoMiddle(5); // REAR break; case MULTITYPE_GIMBAL: - servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max); - servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max); + 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: @@ -337,47 +361,47 @@ void mixTable(void) break; case MULTITYPE_FLYING_WING: - motor[0] = rcCommand[THROTTLE]; + 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[0] = cfg.pitch_direction_l * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - mcfg.midrc); - servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - mcfg.midrc); + 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[0] = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL]; - servo[1] = cfg.pitch_direction_r * axisPID[PITCH] + cfg.roll_direction_r * axisPID[ROLL]; + 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[0] = constrain(servo[0] + cfg.wing_left_mid, cfg.wing_left_min, cfg.wing_left_max); - servo[1] = constrain(servo[1] + cfg.wing_right_mid, cfg.wing_right_min, cfg.wing_right_max); + servo[3] += servoMiddle(3); + servo[4] += servoMiddle(4); break; } // do camstab if (feature(FEATURE_SERVO_TILT)) { - uint16_t aux[2] = { 0, 0 }; - - if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY)) - aux[0] = rcData[AUX3] - mcfg.midrc; - if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34)) - aux[1] = rcData[AUX4] - mcfg.midrc; - - servo[0] = cfg.gimbal_pitch_mid + aux[0]; - servo[1] = cfg.gimbal_roll_mid + aux[1]; + // 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] -= (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 - cfg.gimbal_roll_gain * angle[ROLL] / 16; - servo[1] += (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 + cfg.gimbal_roll_gain * angle[ROLL] / 16; + 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] += cfg.gimbal_pitch_gain * angle[PITCH] / 16; - servo[1] += cfg.gimbal_roll_gain * angle[ROLL] / 16; + servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; + servo[1] += (int32_t)cfg.servoConf[0].rate * angle[ROLL] / 50; } } - - servo[0] = constrain(servo[0], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max); - servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max); } + // 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)) diff --git a/src/mw.c b/src/mw.c index 8d4ea51e29..97a2442cbb 100755 --- a/src/mw.c +++ b/src/mw.c @@ -16,7 +16,7 @@ int16_t telemTemperature1; // gyro sensor temperature int16_t failsafeCnt = 0; int16_t failsafeEvents = 0; -int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000] +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[6]; // lookup table for expo & RC rate PITCH+ROLL int16_t lookupThrottleRC[11]; // lookup table for expo & mid THROTTLE @@ -802,9 +802,6 @@ void loop(void) currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; -#ifdef MPU6050_DMP - mpu6050DmpLoop(); -#endif #ifdef MAG if (sensors(SENSOR_MAG)) { diff --git a/src/mw.h b/src/mw.h index df724a994e..ad8dd31e3c 100755 --- a/src/mw.h +++ b/src/mw.h @@ -4,11 +4,13 @@ #define VBATFREQ 6 // to read battery voltage - nth number of loop iterations #define BARO_TAB_SIZE_MAX 48 -#define VERSION 220 +#define VERSION 230 #define LAT 0 #define LON 1 +#define RC_CHANS (18) + // Serial GPS only variables // navigation mode typedef enum NavigationMode @@ -44,10 +46,8 @@ typedef enum MultiType typedef enum GimbalFlags { GIMBAL_NORMAL = 1 << 0, - GIMBAL_TILTONLY = 1 << 1, - GIMBAL_DISABLEAUX34 = 1 << 2, - GIMBAL_FORWARDAUX = 1 << 3, - GIMBAL_MIXTILT = 1 << 4, + GIMBAL_MIXTILT = 1 << 1, + GIMBAL_FORWARDAUX = 1 << 2, } GimbalFlags; /*********** RC alias *****************/ @@ -194,33 +194,9 @@ typedef struct config_t { // mixer-related configuration int8_t yaw_direction; uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - uint16_t tri_yaw_middle; // tail servo center pos. - use this for initial trim - uint16_t tri_yaw_min; // tail servo min - uint16_t tri_yaw_max; // tail servo max - - // flying wing related configuration - uint16_t wing_left_min; // min/mid/max servo travel - uint16_t wing_left_mid; - uint16_t wing_left_max; - uint16_t wing_right_min; - uint16_t wing_right_mid; - uint16_t wing_right_max; - - int8_t pitch_direction_l; // left servo - pitch orientation - int8_t pitch_direction_r; // right servo - pitch orientation (opposite sign to pitch_direction_l if servos are mounted mirrored) - int8_t roll_direction_l; // left servo - roll orientation - int8_t roll_direction_r; // right servo - roll orientation (same sign as ROLL_DIRECTION_L, if servos are mounted in mirrored orientation) // gimbal-related configuration - int8_t gimbal_pitch_gain; // gimbal pitch servo gain (tied to angle) can be negative to invert movement - int8_t gimbal_roll_gain; // gimbal roll servo gain (tied to angle) can be negative to invert movement uint8_t gimbal_flags; // in servotilt mode, various things that affect stuff - uint16_t gimbal_pitch_min; // gimbal pitch servo min travel - uint16_t gimbal_pitch_max; // gimbal pitch servo max travel - uint16_t gimbal_pitch_mid; // gimbal pitch servo neutral value - uint16_t gimbal_roll_min; // gimbal roll servo min travel - uint16_t gimbal_roll_max; // gimbal roll servo max travel - uint16_t gimbal_roll_mid; // gimbal roll servo neutral value // gps-related stuff uint16_t gps_wp_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) @@ -305,6 +281,7 @@ typedef struct core_t { serialPort_t *telemport; serialPort_t *rcvrport; bool useServo; + uint8_t numRCChannels; } core_t; @@ -362,8 +339,8 @@ extern int16_t throttleAngleCorrection; extern int16_t headFreeModeHold; extern int16_t heading, magHold; extern int16_t motor[MAX_MOTORS]; -extern int16_t servo[8]; -extern int16_t rcData[8]; +extern int16_t servo[MAX_SERVOS]; +extern int16_t rcData[RC_CHANS]; extern uint16_t rssi; // range: [0;1023] extern uint8_t vbat; extern int16_t telemTemperature1; // gyro sensor temperature diff --git a/src/serial.c b/src/serial.c index 4da63d505c..07a1ce4bcd 100755 --- a/src/serial.c +++ b/src/serial.c @@ -149,7 +149,6 @@ static const char pidnames[] = static uint8_t checksum, indRX, inBuf[INBUF_SIZE]; static uint8_t cmdMSP; -static bool guiConnected = false; // signal that we're in cli mode uint8_t cliMode = 0; @@ -468,17 +467,28 @@ static void evaluateCommand(void) serialize16(magADC[i]); break; case MSP_SERVO: - headSerialReply(16); - for (i = 0; i < 8; i++) - serialize16(servo[i]); + s_struct((uint8_t *)&servo, 16); break; case MSP_SERVO_CONF: - s_struct((uint8_t *)&cfg.servoConf, 56); // struct servoConf is 7 bytes length: min:2 / max:2 / middle:2 / rate:1 ---- 8 servo => 8x7 = 56 + headSerialReply(56); + for (i = 0; i < MAX_SERVOS; i++) { + serialize16(cfg.servoConf[i].min); + serialize16(cfg.servoConf[i].max); + serialize16(cfg.servoConf[i].middle); + serialize8(cfg.servoConf[i].rate); + } + break; + case MSP_SET_SERVO_CONF: + headSerialReply(0); + for (i = 0; i < MAX_SERVOS; i++) { + cfg.servoConf[i].min = read16(); + cfg.servoConf[i].max = read16(); + cfg.servoConf[i].middle = read16(); + cfg.servoConf[i].rate = read8(); + } break; case MSP_MOTOR: - headSerialReply(16); - for (i = 0; i < 8; i++) - serialize16(motor[i]); + s_struct((uint8_t *)motor, 16); break; case MSP_RC: headSerialReply(16); @@ -718,7 +728,6 @@ void serialCom(void) indRX = 0; checksum ^= c; c_state = HEADER_SIZE; // the command is to follow - guiConnected = true; } else if (c_state == HEADER_SIZE) { cmdMSP = c; checksum ^= c;