Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

Fix servo middle/channel forwarding variable re-use. Adds a new

Browse source 
capability flag which is returned by MSP IDENT (CAP_CHANNEL_FORWARDING).
Adds two new MSP commands that are specifically for configuring channel
forwarding (MSP_CHANNEL_FORWARDING/MSP_SET_CHANNEL_FORWARDING).  Servo
'middle' and 'forward from channel' are now stored independently, as
they probably should always have been.
This commit is contained in:
Dominic Clifton 2014-04-18 19:11:25 +01:00
parent 43258c045d
commit b8e7c2d14f
5 changed files with 52 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/config.c
View file

@ -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;

43
src/flight_mixer.c
View file

@ -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) {

3
src/flight_mixer.h
View file

@ -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;

4
src/rx_common.h
View file

@ -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,

17
src/serial_msp.c
View file

@ -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;