1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00

Allow excluding of servo code at compilation time.

This is in preparation for backbox on CJMCU/64k.

Flight tested on CJMCU.
This commit is contained in:
Dominic Clifton 2015-02-22 16:20:04 +00:00
parent 5a15c3b271
commit 9057d70410
26 changed files with 131 additions and 48 deletions

View file

@ -397,6 +397,8 @@ CJMCU_SRC = \
drivers/timer.c \
drivers/timer_stm32f10x.c \
hardware_revision.c \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
$(COMMON_SRC)
CC3D_SRC = \

View file

@ -749,8 +749,10 @@ static void loadBlackboxState(void)
blackboxCurrent->BaroAlt = BaroAlt;
#endif
#ifdef USE_SERVOS
//Tail servo for tricopters
blackboxCurrent->servo[5] = servo[5];
#endif
}
/**

View file

@ -74,9 +74,17 @@
#define BRUSHLESS_MOTORS_PWM_RATE 400
void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse);
void mixerUseConfigs(
#ifdef USE_SERVOS
servoParam_t *servoConfToUse,
gimbalConfig_t *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
rxConfig_t *rxConfig
);
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
#define FLASH_TO_RESERVE_FOR_CONFIG 0x800
@ -319,7 +327,10 @@ static void setControlRateProfile(uint8_t profileIndex)
static void resetConf(void)
{
int i;
int8_t servoRates[8] = { 30, 30, 100, 100, 100, 100, 100, 100 };
#ifdef USE_SERVOS
int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
;
#endif
// Clear all configuration
memset(&masterConfig, 0, sizeof(master_t));
@ -434,8 +445,9 @@ static void resetConf(void)
currentProfile->failsafeConfig.failsafe_min_usec = 985; // any of first 4 channels below this value will trigger failsafe
currentProfile->failsafeConfig.failsafe_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe
#ifdef USE_SERVOS
// servos
for (i = 0; i < 8; i++) {
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
@ -450,6 +462,7 @@ static void resetConf(void)
// gimbal
currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
#endif
#ifdef GPS
resetGpsProfile(&currentProfile->gpsProfile);
@ -629,13 +642,15 @@ void activateConfig(void)
setAccelerationTrims(&masterConfig.accZero);
mixerUseConfigs(
#ifdef USE_SERVOS
currentProfile->servoConf,
&currentProfile->gimbalConfig,
#endif
&masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig,
&currentProfile->mixerConfig,
&masterConfig.airplaneConfig,
&masterConfig.rxConfig,
&currentProfile->gimbalConfig
&masterConfig.rxConfig
);
imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;

View file

@ -47,8 +47,12 @@ typedef struct profile_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.
#ifdef USE_SERVOS
// Servo-related stuff
servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration
// gimbal-related configuration
gimbalConfig_t gimbalConfig;
#endif
// Failsafe related configuration
failsafeConfig_t failsafeConfig;
@ -56,9 +60,6 @@ typedef struct profile_s {
// mixer-related configuration
mixerConfig_t mixerConfig;
// gimbal-related configuration
gimbalConfig_t gimbalConfig;
#ifdef GPS
gpsProfile_t gpsProfile;
#endif

View file

@ -419,6 +419,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
if (type == MAP_TO_PPM_INPUT && !init->usePPM)
continue;
#ifdef USE_SERVOS
if (init->useServos && !init->airplane) {
#if defined(NAZE)
// remap PWM9+10 as servos
@ -455,6 +456,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
if (timerIndex >= PWM5 && timerIndex <= PWM8)
type = MAP_TO_SERVO_OUTPUT;
}
#endif
#ifdef CC3D
if (init->useParallelPWM) {
@ -489,8 +491,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
}
pwmOutputConfiguration.motorCount++;
} else if (type == MAP_TO_SERVO_OUTPUT) {
#ifdef USE_SERVOS
pwmServoConfig(timerHardwarePtr, pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse);
pwmOutputConfiguration.servoCount++;
#endif
}
}

View file

@ -49,14 +49,16 @@ typedef struct drv_pwm_config_t {
bool useOneshot;
bool useSoftSerial;
bool useLEDStrip;
#ifdef USE_SERVOS
bool useServos;
bool extraServos; // configure additional 4 channels in PPM mode as servos, not motors
uint16_t servoPwmRate;
uint16_t servoCenterPulse;
#endif
bool airplane; // fixed wing hardware config, lots of servos etc
uint16_t motorPwmRate;
uint16_t servoPwmRate;
uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm),
// some higher value (used by 3d mode), or 0, for brushed pwm drivers.
uint16_t servoCenterPulse;
} drv_pwm_config_t;

View file

@ -43,8 +43,10 @@ typedef struct {
static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS];
static pwmOutputPort_t *motors[MAX_PWM_MOTORS];
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
#ifdef USE_SERVOS
static pwmOutputPort_t *servos[MAX_PWM_SERVOS];
#endif
#define PWM_BRUSHED_TIMER_MHZ 8
static uint8_t allocatedOutputPortCount = 0;
@ -160,13 +162,6 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount)
}
}
void pwmWriteServo(uint8_t index, uint16_t value)
{
if (servos[index] && index < MAX_SERVOS)
*servos[index]->ccr = value;
}
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
{
uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
@ -187,7 +182,15 @@ void pwmOneshotMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
}
#ifdef USE_SERVOS
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse)
{
servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse);
}
void pwmWriteServo(uint8_t index, uint16_t value)
{
if (servos[index] && index < MAX_SERVOS)
*servos[index]->ccr = value;
}
#endif

View file

@ -62,23 +62,24 @@ extern int16_t debug[4];
uint8_t motorCount = 0;
int16_t motor[MAX_SUPPORTED_MOTORS];
int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo;
static uint8_t servoCount;
static servoParam_t *servoConf;
static mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig;
static escAndServoConfig_t *escAndServoConfig;
static airplaneConfig_t *airplaneConfig;
static rxConfig_t *rxConfig;
static gimbalConfig_t *gimbalConfig;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static mixerMode_e currentMixerMode;
#ifdef USE_SERVOS
static gimbalConfig_t *gimbalConfig;
int16_t servo[MAX_SUPPORTED_SERVOS];
static int useServo;
static uint8_t servoCount;
static servoParam_t *servoConf;
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
#endif
static const motorMixer_t mixerQuadX[] = {
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
@ -230,17 +231,31 @@ const mixer_t mixers[] = {
};
#endif
void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse)
static motorMixer_t *customMixers;
void mixerUseConfigs(
#ifdef USE_SERVOS
servoParam_t *servoConfToUse,
gimbalConfig_t *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
rxConfig_t *rxConfigToUse)
{
#ifdef USE_SERVOS
servoConf = servoConfToUse;
gimbalConfig = gimbalConfigToUse;
#endif
flight3DConfig = flight3DConfigToUse;
escAndServoConfig = escAndServoConfigToUse;
mixerConfig = mixerConfigToUse;
airplaneConfig = airplaneConfigToUse;
rxConfig = rxConfigToUse;
gimbalConfig = gimbalConfigToUse;
}
#ifdef USE_SERVOS
int16_t determineServoMiddleOrForwardFromChannel(int nr)
{
uint8_t channelToForwardFrom = servoConf[nr].forwardFromChannel;
@ -269,9 +284,7 @@ int servoDirection(int nr, int lr)
else
return 1;
}
static motorMixer_t *customMixers;
#endif
#ifndef USE_QUAD_MIXER_ONLY
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
@ -361,15 +374,15 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
currentMixerMode = mixerMode;
customMixers = initialCustomMixers;
useServo = false;
}
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
{
UNUSED(pwmOutputConfiguration);
motorCount = 4;
#ifdef USE_SERVOS
servoCount = 0;
#endif
uint8_t i;
for (i = 0; i < motorCount; i++) {
@ -388,6 +401,7 @@ void mixerResetMotors(void)
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
}
#ifdef USE_SERVOS
static void updateGimbalServos(void)
{
pwmWriteServo(0, servo[0]);
@ -448,6 +462,7 @@ void writeServos(void)
break;
}
}
#endif
void writeMotors(void)
{
@ -541,7 +556,7 @@ void mixTable(void)
}
}
#ifndef USE_QUAD_MIXER_ONLY
#if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS)
// airplane / servo mixes
switch (currentMixerMode) {
case MIXER_BI:
@ -599,7 +614,6 @@ void mixTable(void)
default:
break;
}
#endif
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
@ -639,6 +653,7 @@ void mixTable(void)
pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]);
}
}
#endif
maxMotor = motor[0];
for (i = 1; i < motorCount; i++)
@ -668,14 +683,16 @@ void mixTable(void)
}
}
#ifdef USE_SERVOS
bool isMixerUsingServos(void)
{
return useServo;
}
#endif
void filterServos(void)
{
#ifdef USE_SERVOS
int16_t servoIdx;
#if defined(MIXER_DEBUG)
@ -693,5 +710,7 @@ void filterServos(void)
#if defined(MIXER_DEBUG)
debug[0] = (int16_t)(micros() - startTime);
#endif
#endif
}

View file

@ -65,9 +65,11 @@ typedef struct mixer_t {
typedef struct mixerConfig_s {
int8_t yaw_direction;
#ifdef USE_SERVOS
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq
int8_t servo_lowpass_enable; // enable/disable lowpass filter
#endif
} mixerConfig_t;
typedef struct flight3DConfig_s {
@ -84,6 +86,7 @@ typedef struct airplaneConfig_t {
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
#ifdef USE_SERVOS
typedef struct servoParam_t {
int16_t min; // servo min
int16_t max; // servo max
@ -92,15 +95,17 @@ typedef struct servoParam_t {
int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED
} servoParam_t;
extern int16_t servo[MAX_SUPPORTED_SERVOS];
bool isMixerUsingServos(void);
void writeServos(void);
void filterServos(void);
#endif
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int16_t servo[MAX_SUPPORTED_SERVOS];
bool isMixerUsingServos(void);
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerResetMotors(void);
void mixTable(void);
void writeServos(void);
void writeMotors(void);
void filterServos(void);

View file

@ -199,7 +199,7 @@ const clicmd_t cmdTable[] = {
{ "profile", "index (0 to 2)", cliProfile },
{ "rateprofile", "index (0 to 2)", cliRateProfile },
{ "save", "save and reboot", cliSave },
#ifndef CJMCU
#ifdef USE_SERVOS
{ "servo", "servo config", cliServo },
#endif
{ "set", "name=value or blank or * for list", cliSet },
@ -353,10 +353,11 @@ const clivalue_t valueTable[] = {
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, -1, 1 },
{ "yaw_direction", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.yaw_direction, -1, 1 },
#ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.tri_unarmed_servo, 0, 1 },
{ "servo_lowpass_freq", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_freq, 10, 400},
{ "servo_lowpass_enable", VAR_INT8 | PROFILE_VALUE, &masterConfig.profile[0].mixerConfig.servo_lowpass_enable, 0, 1 },
#endif
{ "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 },
{ "rc_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcRate8, 0, 250 },
{ "rc_expo", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rcExpo8, 0, 100 },
@ -373,7 +374,9 @@ const clivalue_t valueTable[] = {
{ "failsafe_min_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_min_usec, 100, PWM_RANGE_MAX },
{ "failsafe_max_usec", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].failsafeConfig.failsafe_max_usec, 100, PWM_RANGE_MAX + (PWM_RANGE_MAX - PWM_RANGE_MIN) },
#ifdef USE_SERVOS
{ "gimbal_flags", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].gimbalConfig.gimbal_flags, 0, 255},
#endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_NONE },
{ "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 },
@ -752,7 +755,7 @@ static void cliColor(char *cmdline)
static void cliServo(char *cmdline)
{
#ifdef CJMCU
#ifndef USE_SERVOS
UNUSED(cmdline);
#else
enum { SERVO_ARGUMENT_COUNT = 6 };

View file

@ -799,6 +799,7 @@ static bool processOutCommand(uint8_t cmdMSP)
for (i = 0; i < 3; i++)
serialize16(magADC[i]);
break;
#ifdef USE_SERVOS
case MSP_SERVO:
s_struct((uint8_t *)&servo, 16);
break;
@ -817,6 +818,7 @@ static bool processOutCommand(uint8_t cmdMSP)
serialize8(currentProfile->servoConf[i].forwardFromChannel);
}
break;
#endif
case MSP_MOTOR:
s_struct((uint8_t *)motor, 16);
break;
@ -1324,6 +1326,7 @@ static bool processInCommand(void)
motor_disarmed[i] = read16();
break;
case MSP_SET_SERVO_CONF:
#ifdef USE_SERVOS
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile->servoConf[i].min = read16();
currentProfile->servoConf[i].max = read16();
@ -1337,11 +1340,14 @@ static bool processInCommand(void)
}
currentProfile->servoConf[i].rate = read8();
}
#endif
break;
case MSP_SET_CHANNEL_FORWARDING:
#ifdef USE_SERVOS
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
currentProfile->servoConf[i].forwardFromChannel = read8();
}
#endif
break;
case MSP_RESET_CONF:
if (!ARMING_FLAG(ARMED)) {

View file

@ -197,18 +197,22 @@ void init(void)
&& masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.extraServos = currentProfile->gimbalConfig.gimbal_flags & GIMBAL_FORWARDAUX;
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = PULSE_1MS; // standard PWM for brushless ESC (default, overridden below)
if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse;
pwmRxInit(masterConfig.inputFilteringMode);

View file

@ -722,8 +722,12 @@ void loop(void)
);
mixTable();
#ifdef USE_SERVOS
filterServos();
writeServos();
#endif
writeMotors();
#ifdef BLACKBOX

View file

@ -92,6 +92,7 @@
#define GPS
//#define DISPLAY
#define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND

View file

@ -106,6 +106,7 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND
// USART3, PB11 (Flexport)

View file

@ -114,3 +114,4 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS

View file

@ -61,6 +61,8 @@
// #define SOFT_I2C_PB67
#define SERIAL_RX
//#define BLACKBOX
//#define USE_SERVOS
#define SPEKTRUM_BIND
// USART2, PA3

View file

@ -124,6 +124,7 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND
// USART2, PA3

View file

@ -144,6 +144,7 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND
// USART2, PA3

View file

@ -45,6 +45,7 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS
#define SPEKTRUM_BIND
// USART2, PA3

View file

@ -111,3 +111,4 @@
#define SERIAL_RX
#define AUTOTUNE
#define BLACKBOX
#define USE_SERVOS

View file

@ -135,4 +135,4 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS

View file

@ -90,6 +90,7 @@
#define SERIAL_RX
#define GPS
#define DISPLAY
#define USE_SERVOS
#define LED_STRIP
#if 1

View file

@ -126,3 +126,4 @@
#define SERIAL_RX
#define AUTOTUNE
#define DISPLAY
#define USE_SERVOS

View file

@ -88,3 +88,4 @@
#define TELEMETRY
#define SERIAL_RX
#define AUTOTUNE
#define USE_SERVOS

View file

@ -22,6 +22,7 @@
#define GPS
#define TELEMETRY
#define LED_STRIP
#define USE_SERVOS
#define SERIAL_PORT_COUNT 4