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:
parent
5a15c3b271
commit
9057d70410
26 changed files with 131 additions and 48 deletions
2
Makefile
2
Makefile
|
@ -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 = \
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -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(¤tProfile->gpsProfile);
|
||||
|
@ -629,13 +642,15 @@ void activateConfig(void)
|
|||
setAccelerationTrims(&masterConfig.accZero);
|
||||
|
||||
mixerUseConfigs(
|
||||
#ifdef USE_SERVOS
|
||||
currentProfile->servoConf,
|
||||
¤tProfile->gimbalConfig,
|
||||
#endif
|
||||
&masterConfig.flight3DConfig,
|
||||
&masterConfig.escAndServoConfig,
|
||||
¤tProfile->mixerConfig,
|
||||
&masterConfig.airplaneConfig,
|
||||
&masterConfig.rxConfig,
|
||||
¤tProfile->gimbalConfig
|
||||
&masterConfig.rxConfig
|
||||
);
|
||||
|
||||
imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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 };
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -722,8 +722,12 @@ void loop(void)
|
|||
);
|
||||
|
||||
mixTable();
|
||||
|
||||
#ifdef USE_SERVOS
|
||||
filterServos();
|
||||
writeServos();
|
||||
#endif
|
||||
|
||||
writeMotors();
|
||||
|
||||
#ifdef BLACKBOX
|
||||
|
|
|
@ -92,6 +92,7 @@
|
|||
#define GPS
|
||||
//#define DISPLAY
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
|
|
|
@ -106,6 +106,7 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART3, PB11 (Flexport)
|
||||
|
|
|
@ -114,3 +114,4 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
|
|
|
@ -61,6 +61,8 @@
|
|||
// #define SOFT_I2C_PB67
|
||||
|
||||
#define SERIAL_RX
|
||||
//#define BLACKBOX
|
||||
//#define USE_SERVOS
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART2, PA3
|
||||
|
|
|
@ -124,6 +124,7 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART2, PA3
|
||||
|
|
|
@ -144,6 +144,7 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART2, PA3
|
||||
|
|
|
@ -45,6 +45,7 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART2, PA3
|
||||
|
|
|
@ -111,3 +111,4 @@
|
|||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define BLACKBOX
|
||||
#define USE_SERVOS
|
||||
|
|
|
@ -135,4 +135,4 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
|
||||
#define USE_SERVOS
|
||||
|
|
|
@ -90,6 +90,7 @@
|
|||
#define SERIAL_RX
|
||||
#define GPS
|
||||
#define DISPLAY
|
||||
#define USE_SERVOS
|
||||
|
||||
#define LED_STRIP
|
||||
#if 1
|
||||
|
|
|
@ -126,3 +126,4 @@
|
|||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define DISPLAY
|
||||
#define USE_SERVOS
|
||||
|
|
|
@ -88,3 +88,4 @@
|
|||
#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
|
|
|
@ -22,6 +22,7 @@
|
|||
#define GPS
|
||||
#define TELEMETRY
|
||||
#define LED_STRIP
|
||||
#define USE_SERVOS
|
||||
|
||||
#define SERIAL_PORT_COUNT 4
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue