1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00

Split escAndServoConfig into motor and servo parts, as per betaflight

This commit is contained in:
Martin Budden 2016-10-01 18:02:33 +01:00
parent b3eff8d832
commit f57c22f2a2
30 changed files with 157 additions and 108 deletions

View file

@ -55,7 +55,8 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/display.h" #include "io/display.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
@ -582,7 +583,7 @@ static void writeIntraframe(void)
* Write the throttle separately from the rest of the RC data so we can apply a predictor to it. * Write the throttle separately from the rest of the RC data so we can apply a predictor to it.
* Throttle lies in range [minthrottle..maxthrottle]: * Throttle lies in range [minthrottle..maxthrottle]:
*/ */
blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle); blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle);
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
/* /*
@ -626,7 +627,7 @@ static void writeIntraframe(void)
blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT); blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
//Motors can be below minthrottle when disarmed, but that doesn't happen much //Motors can be below minthrottle when disarmed, but that doesn't happen much
blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle); blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.motorConfig.minthrottle);
//Motors tend to be similar to each other so use the first motor's value as a predictor of the others //Motors tend to be similar to each other so use the first motor's value as a predictor of the others
for (x = 1; x < motorCount; x++) { for (x = 1; x < motorCount; x++) {
@ -1277,8 +1278,8 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime); BLACKBOX_PRINT_HEADER_LINE("Firmware date:%s %s", buildDate, buildTime);
BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom); BLACKBOX_PRINT_HEADER_LINE("P interval:%d/%d", masterConfig.blackbox_rate_num, masterConfig.blackbox_rate_denom);
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", 100); //For compatibility reasons write rc_rate 100 BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", 100); //For compatibility reasons write rc_rate 100
BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.escAndServoConfig.minthrottle); BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d", masterConfig.motorConfig.minthrottle);
BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.escAndServoConfig.maxthrottle); BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d", masterConfig.motorConfig.maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale)); BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x", castFloatBytesToInt(gyro.scale));
BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G); BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u", acc.acc_1G);

View file

@ -50,7 +50,8 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/display.h" #include "io/display.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"

View file

@ -50,7 +50,8 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
@ -244,16 +245,20 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; sensorAlignmentConfig->mag_align = ALIGN_DEFAULT;
} }
void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig) void resetMotorConfig(motorConfig_t *motorConfig)
{ {
#ifdef BRUSHED_MOTORS #ifdef BRUSHED_MOTORS
escAndServoConfig->minthrottle = 1000; motorConfig->minthrottle = 1000;
#else #else
escAndServoConfig->minthrottle = 1150; motorConfig->minthrottle = 1150;
#endif #endif
escAndServoConfig->maxthrottle = 1850; motorConfig->maxthrottle = 1850;
escAndServoConfig->mincommand = 1000; motorConfig->mincommand = 1000;
escAndServoConfig->servoCenterPulse = 1500; }
void resetServoConfig(servoConfig_t *servoConfig)
{
servoConfig->servoCenterPulse = 1500;
} }
void resetFlight3DConfig(flight3DConfig_t *flight3DConfig) void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
@ -390,7 +395,7 @@ void setControlRateProfile(uint8_t profileIndex)
uint16_t getCurrentMinthrottle(void) uint16_t getCurrentMinthrottle(void)
{ {
return masterConfig.escAndServoConfig.minthrottle; return masterConfig.motorConfig.minthrottle;
} }
// Default settings // Default settings
@ -481,7 +486,8 @@ static void resetConf(void)
#endif #endif
// Motor/ESC/Servo // Motor/ESC/Servo
resetEscAndServoConfig(&masterConfig.escAndServoConfig); resetMotorConfig(&masterConfig.motorConfig);
resetServoConfig(&masterConfig.servoConfig);
resetFlight3DConfig(&masterConfig.flight3DConfig); resetFlight3DConfig(&masterConfig.flight3DConfig);
#ifdef BRUSHED_MOTORS #ifdef BRUSHED_MOTORS
@ -615,8 +621,8 @@ static void resetConf(void)
masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
#endif #endif
masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.rxConfig.spektrum_sat_bind = 5;
masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.motorConfig.minthrottle = 1000;
masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motorConfig.maxthrottle = 2000;
masterConfig.motor_pwm_rate = 32000; masterConfig.motor_pwm_rate = 32000;
masterConfig.looptime = 2000; masterConfig.looptime = 2000;
currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[ROLL] = 36;
@ -694,7 +700,7 @@ static void resetConf(void)
void activateControlRateConfig(void) void activateControlRateConfig(void)
{ {
generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig); generateThrottleCurve(currentControlRateProfile, &masterConfig.motorConfig);
} }
void activateConfig(void) void activateConfig(void)
@ -707,7 +713,7 @@ void activateConfig(void)
useRcControlsConfig( useRcControlsConfig(
currentProfile->modeActivationConditions, currentProfile->modeActivationConditions,
&masterConfig.escAndServoConfig, &masterConfig.motorConfig,
&currentProfile->pidProfile &currentProfile->pidProfile
); );
@ -723,7 +729,7 @@ void activateConfig(void)
setAccelerationGain(&masterConfig.accGain); setAccelerationGain(&masterConfig.accGain);
setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz); setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz);
mixerUseConfigs(&masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, &masterConfig.mixerConfig, &masterConfig.rxConfig); mixerUseConfigs(&masterConfig.flight3DConfig, &masterConfig.motorConfig, &masterConfig.mixerConfig, &masterConfig.rxConfig);
#ifdef USE_SERVOS #ifdef USE_SERVOS
servosUseConfigs(&masterConfig.servoConfig, currentProfile->servoConf, &currentProfile->gimbalConfig); servosUseConfigs(&masterConfig.servoConfig, currentProfile->servoConf, &currentProfile->gimbalConfig);
#endif #endif
@ -744,7 +750,7 @@ void activateConfig(void)
navigationUseRcControlsConfig(&currentProfile->rcControlsConfig); navigationUseRcControlsConfig(&currentProfile->rcControlsConfig);
navigationUseRxConfig(&masterConfig.rxConfig); navigationUseRxConfig(&masterConfig.rxConfig);
navigationUseFlight3DConfig(&masterConfig.flight3DConfig); navigationUseFlight3DConfig(&masterConfig.flight3DConfig);
navigationUseEscAndServoConfig(&masterConfig.escAndServoConfig); navigationUsemotorConfig(&masterConfig.motorConfig);
#endif #endif
#ifdef BARO #ifdef BARO

View file

@ -45,7 +45,8 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"

View file

@ -36,7 +36,8 @@ typedef struct master_s {
servoMixer_t customServoMixer[MAX_SERVO_RULES]; servoMixer_t customServoMixer[MAX_SERVO_RULES];
#endif #endif
// motor/esc/servo related stuff // motor/esc/servo related stuff
escAndServoConfig_t escAndServoConfig; motorConfig_t motorConfig;
servoConfig_t servoConfig;
flight3DConfig_t flight3DConfig; flight3DConfig_t flight3DConfig;
uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz) uint16_t motor_pwm_rate; // The update rate of motor outputs (50-498Hz)

View file

@ -46,7 +46,8 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"

View file

@ -28,9 +28,13 @@
#include "timer.h" #include "timer.h"
#include "pwm_mapping.h" #include "pwm_mapping.h"
#include "pwm_output.h" #include "pwm_output.h"
#include "drivers/io_pca9685.h" #include "io_pca9685.h"
#include "io/pwmdriver_i2c.h" #include "io/pwmdriver_i2c.h"
#include "config/config.h" #include "config/config.h"
#include "config/feature.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors

View file

@ -49,7 +49,8 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gimbal.h" #include "io/gimbal.h"
@ -100,7 +101,7 @@ extern uint16_t cycleTime; // FIXME dependency on mw.c
extern uint16_t rssi; // FIXME dependency on mw.c extern uint16_t rssi; // FIXME dependency on mw.c
extern void resetPidProfile(pidProfile_t *pidProfile); extern void resetPidProfile(pidProfile_t *pidProfile);
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse);
static mspPostProcessFuncPtr mspPostProcessFn = NULL; static mspPostProcessFuncPtr mspPostProcessFn = NULL;
@ -738,9 +739,9 @@ static bool processOutCommand(uint8_t cmdMSP, sbuf_t *dst, sbuf_t *src)
case MSP_MISC: case MSP_MISC:
sbufWriteU16(dst, masterConfig.rxConfig.midrc); sbufWriteU16(dst, masterConfig.rxConfig.midrc);
sbufWriteU16(dst, masterConfig.escAndServoConfig.minthrottle); sbufWriteU16(dst, masterConfig.motorConfig.minthrottle);
sbufWriteU16(dst, masterConfig.escAndServoConfig.maxthrottle); sbufWriteU16(dst, masterConfig.motorConfig.maxthrottle);
sbufWriteU16(dst, masterConfig.escAndServoConfig.mincommand); sbufWriteU16(dst, masterConfig.motorConfig.mincommand);
sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle); sbufWriteU16(dst, masterConfig.failsafeConfig.failsafe_throttle);
@ -1148,7 +1149,7 @@ static mspResult_e processInCommand(uint8_t cmdMSP, sbuf_t *src)
mac->range.startStep = sbufReadU8(src); mac->range.startStep = sbufReadU8(src);
mac->range.endStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src);
useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, &currentProfile->pidProfile); useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile);
} else { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }
@ -1208,9 +1209,9 @@ static mspResult_e processInCommand(uint8_t cmdMSP, sbuf_t *src)
if (tmp < 1600 && tmp > 1400) if (tmp < 1600 && tmp > 1400)
masterConfig.rxConfig.midrc = tmp; masterConfig.rxConfig.midrc = tmp;
masterConfig.escAndServoConfig.minthrottle = sbufReadU16(src); masterConfig.motorConfig.minthrottle = sbufReadU16(src);
masterConfig.escAndServoConfig.maxthrottle = sbufReadU16(src); masterConfig.motorConfig.maxthrottle = sbufReadU16(src);
masterConfig.escAndServoConfig.mincommand = sbufReadU16(src); masterConfig.motorConfig.mincommand = sbufReadU16(src);
masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src); masterConfig.failsafeConfig.failsafe_throttle = sbufReadU16(src);

View file

@ -60,7 +60,8 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/display.h" #include "io/display.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
@ -588,10 +589,10 @@ void taskMainPidLoop(void)
} }
if (thrTiltCompStrength) { if (thrTiltCompStrength) {
rcCommand[THROTTLE] = constrain(masterConfig.escAndServoConfig.minthrottle rcCommand[THROTTLE] = constrain(masterConfig.motorConfig.minthrottle
+ (rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength), + (rcCommand[THROTTLE] - masterConfig.motorConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
masterConfig.escAndServoConfig.minthrottle, masterConfig.motorConfig.minthrottle,
masterConfig.escAndServoConfig.maxthrottle); masterConfig.motorConfig.maxthrottle);
} }
} }

View file

@ -42,7 +42,7 @@
#include "io/gps.h" #include "io/gps.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/escservo.h" #include "io/motors.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
@ -63,7 +63,7 @@
#define AIRMODE_DEADBAND 25 #define AIRMODE_DEADBAND 25
static escAndServoConfig_t *escAndServoConfig; static motorConfig_t *motorConfig;
static pidProfile_t *pidProfile; static pidProfile_t *pidProfile;
// true if arming is done via the sticks (as opposed to a switch) // true if arming is done via the sticks (as opposed to a switch)
@ -518,7 +518,7 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
case ADJUSTMENT_THROTTLE_EXPO: case ADJUSTMENT_THROTTLE_EXPO:
newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c newValue = constrain((int)controlRateConfig->thrExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->thrExpo8 = newValue; controlRateConfig->thrExpo8 = newValue;
generateThrottleCurve(controlRateConfig, escAndServoConfig); generateThrottleCurve(controlRateConfig, motorConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break; break;
case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_ROLL_RATE:
@ -705,9 +705,9 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
return MIN(ABS(rcData[axis] - midrc), 500); return MIN(ABS(rcData[axis] - midrc), 500);
} }
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse)
{ {
escAndServoConfig = escAndServoConfigToUse; motorConfig = motorConfigToUse;
pidProfile = pidProfileToUse; pidProfile = pidProfileToUse;
isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);

View file

@ -268,6 +268,6 @@ bool isUsingNavigationModes(void);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
struct escAndServoConfig_s; struct motorConfig_s;
struct pidProfile_s; struct pidProfile_s;
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct escAndServoConfig_s *escAndServoConfigToUse, struct pidProfile_s *pidProfileToUse); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse);

View file

@ -21,7 +21,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "io/escservo.h" #include "io/motors.h"
#define PITCH_LOOKUP_LENGTH 7 #define PITCH_LOOKUP_LENGTH 7
@ -31,9 +31,9 @@
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
int16_t lookupThrottleRCMid; // THROTTLE curve mid point int16_t lookupThrottleRCMid; // THROTTLE curve mid point
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig) void generateThrottleCurve(controlRateConfig_t *controlRateConfig, motorConfig_t *motorConfig)
{ {
lookupThrottleRCMid = escAndServoConfig->minthrottle + (int32_t)(escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE] lookupThrottleRCMid = motorConfig->minthrottle + (int32_t)(motorConfig->maxthrottle - motorConfig->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { for (int i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
const int16_t tmp = 10 * i - controlRateConfig->thrMid8; const int16_t tmp = 10 * i - controlRateConfig->thrMid8;
@ -43,7 +43,7 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
if (tmp < 0) if (tmp < 0)
y = controlRateConfig->thrMid8; y = controlRateConfig->thrMid8;
lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10;
lookupThrottleRC[i] = escAndServoConfig->minthrottle + (int32_t) (escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] lookupThrottleRC[i] = motorConfig->minthrottle + (int32_t) (motorConfig->maxthrottle - motorConfig->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE]
} }
} }

View file

@ -18,8 +18,8 @@
#pragma once #pragma once
struct controlRateConfig_s; struct controlRateConfig_s;
struct escAndServoConfig_s; struct motorConfig_s;
void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct escAndServoConfig_s *escAndServoConfig); void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig, struct motorConfig_s *motorConfig);
int16_t rcLookup(int32_t stickDeflection, uint8_t expo); int16_t rcLookup(int32_t stickDeflection, uint8_t expo);
int16_t rcLookupThrottle(int32_t tmp); int16_t rcLookupThrottle(int32_t tmp);

View file

@ -31,7 +31,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/escservo.h" #include "io/motors.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"

View file

@ -41,6 +41,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/motors.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
@ -72,8 +73,13 @@ bool motorLimitReached = false;
mixerConfig_t *mixerConfig; mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig; static flight3DConfig_t *flight3DConfig;
<<<<<<< 8e828b19d6c2105114ebe7dfa31f114733ca326d
static escAndServoConfig_t *escAndServoConfig; static escAndServoConfig_t *escAndServoConfig;
rxConfig_t *rxConfig; rxConfig_t *rxConfig;
=======
static motorConfig_t *motorConfig;
static rxConfig_t *rxConfig;
>>>>>>> Split escAndServoConfig into motor and servo parts, as per betaflight
mixerMode_e currentMixerMode; mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -270,12 +276,12 @@ static motorMixer_t *customMixers;
void mixerUseConfigs( void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse, flight3DConfig_t *flight3DConfigToUse,
escAndServoConfig_t *escAndServoConfigToUse, motorConfig_t *motorConfigToUse,
mixerConfig_t *mixerConfigToUse, mixerConfig_t *mixerConfigToUse,
rxConfig_t *rxConfigToUse) rxConfig_t *rxConfigToUse)
{ {
flight3DConfig = flight3DConfigToUse; flight3DConfig = flight3DConfigToUse;
escAndServoConfig = escAndServoConfigToUse; motorConfig = motorConfigToUse;
mixerConfig = mixerConfigToUse; mixerConfig = mixerConfigToUse;
rxConfig = rxConfigToUse; rxConfig = rxConfigToUse;
} }
@ -373,7 +379,7 @@ void mixerResetDisarmedMotors(void)
int i; int i;
// set disarmed motor values // set disarmed motor values
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand; motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand;
} }
void writeMotors(void) void writeMotors(void)
@ -401,7 +407,7 @@ void writeAllMotors(int16_t mc)
void stopMotors(void) void stopMotors(void)
{ {
writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand); writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->mincommand);
delay(50); // give the timers and ESCs a chance to react. delay(50); // give the timers and ESCs a chance to react.
} }
@ -447,23 +453,23 @@ void mixTable(void)
if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling
throttleMax = flight3DConfig->deadband3d_low; throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle; throttleMin = motorConfig->minthrottle;
throttlePrevious = throttleCommand = rcCommand[THROTTLE]; throttlePrevious = throttleCommand = rcCommand[THROTTLE];
} else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling
throttleMax = escAndServoConfig->maxthrottle; throttleMax = motorConfig->maxthrottle;
throttleMin = flight3DConfig->deadband3d_high; throttleMin = flight3DConfig->deadband3d_high;
throttlePrevious = throttleCommand = rcCommand[THROTTLE]; throttlePrevious = throttleCommand = rcCommand[THROTTLE];
} else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive
throttleCommand = throttleMax = flight3DConfig->deadband3d_low; throttleCommand = throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle; throttleMin = motorConfig->minthrottle;
} else { // Deadband handling from positive to negative } else { // Deadband handling from positive to negative
throttleMax = escAndServoConfig->maxthrottle; throttleMax = motorConfig->maxthrottle;
throttleCommand = throttleMin = flight3DConfig->deadband3d_high; throttleCommand = throttleMin = flight3DConfig->deadband3d_high;
} }
} else { } else {
throttleCommand = rcCommand[THROTTLE]; throttleCommand = rcCommand[THROTTLE];
throttleMin = escAndServoConfig->minthrottle; throttleMin = motorConfig->minthrottle;
throttleMax = escAndServoConfig->maxthrottle; throttleMax = motorConfig->maxthrottle;
} }
throttleRange = throttleMax - throttleMin; throttleRange = throttleMax - throttleMin;
@ -495,21 +501,21 @@ void mixTable(void)
motor[i] = rpyMix[i] + constrain(throttleCommand * currentMixer[i].throttle, throttleMin, throttleMax); motor[i] = rpyMix[i] + constrain(throttleCommand * currentMixer[i].throttle, throttleMin, throttleMax);
if (isFailsafeActive) { if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); motor[i] = constrain(motor[i], motorConfig->mincommand, motorConfig->maxthrottle);
} else if (feature(FEATURE_3D)) { } else if (feature(FEATURE_3D)) {
if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low); motor[i] = constrain(motor[i], motorConfig->minthrottle, flight3DConfig->deadband3d_low);
} else { } else {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, motorConfig->maxthrottle);
} }
} else { } else {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); motor[i] = constrain(motor[i], motorConfig->minthrottle, motorConfig->maxthrottle);
} }
// Motor stop handling // Motor stop handling
if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D)) { if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D)) {
if (((rcData[THROTTLE]) < rxConfig->mincheck)) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
motor[i] = escAndServoConfig->mincommand; motor[i] = motorConfig->mincommand;
} }
} }
} }

View file

@ -101,17 +101,16 @@ typedef struct flight3DConfig_s {
#define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF
extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern bool motorLimitReached; extern bool motorLimitReached;
struct escAndServoConfig_s; struct motorConfig_s;
struct rxConfig_s; struct rxConfig_s;
void mixerUseConfigs( void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse, flight3DConfig_t *flight3DConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse, struct motorConfig_s *motorConfigToUse,
mixerConfig_t *mixerConfigToUse, mixerConfig_t *mixerConfigToUse,
struct rxConfig_s *rxConfigToUse); struct rxConfig_s *rxConfigToUse);

View file

@ -2363,9 +2363,9 @@ void navigationUseRxConfig(rxConfig_t * initialRxConfig)
posControl.rxConfig = initialRxConfig; posControl.rxConfig = initialRxConfig;
} }
void navigationUseEscAndServoConfig(escAndServoConfig_t * initialEscAndServoConfig) void navigationUsemotorConfig(motorConfig_t * initialmotorConfig)
{ {
posControl.escAndServoConfig = initialEscAndServoConfig; posControl.motorConfig = initialmotorConfig;
} }
void navigationUsePIDs(pidProfile_t *initialPidProfile) void navigationUsePIDs(pidProfile_t *initialPidProfile)
@ -2416,7 +2416,7 @@ void navigationInit(navConfig_t *initialnavConfig,
rcControlsConfig_t *initialRcControlsConfig, rcControlsConfig_t *initialRcControlsConfig,
rxConfig_t * initialRxConfig, rxConfig_t * initialRxConfig,
flight3DConfig_t * initialFlight3DConfig, flight3DConfig_t * initialFlight3DConfig,
escAndServoConfig_t * initialEscAndServoConfig) motorConfig_t * initialmotorConfig)
{ {
/* Initial state */ /* Initial state */
posControl.navState = NAV_STATE_IDLE; posControl.navState = NAV_STATE_IDLE;
@ -2446,7 +2446,7 @@ void navigationInit(navConfig_t *initialnavConfig,
navigationUsePIDs(initialPidProfile); navigationUsePIDs(initialPidProfile);
navigationUseRcControlsConfig(initialRcControlsConfig); navigationUseRcControlsConfig(initialRcControlsConfig);
navigationUseRxConfig(initialRxConfig); navigationUseRxConfig(initialRxConfig);
navigationUseEscAndServoConfig(initialEscAndServoConfig); navigationUsemotorConfig(initialmotorConfig);
navigationUseFlight3DConfig(initialFlight3DConfig); navigationUseFlight3DConfig(initialFlight3DConfig);
} }

View file

@ -21,7 +21,7 @@
#include "common/filter.h" #include "common/filter.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/gps.h" #include "io/gps.h"
#include "flight/pid.h" #include "flight/pid.h"
@ -220,14 +220,14 @@ void navigationUsePIDs(pidProfile_t *pidProfile);
void navigationUseConfig(navConfig_t *navConfigToUse); void navigationUseConfig(navConfig_t *navConfigToUse);
void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig); void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig);
void navigationUseRxConfig(rxConfig_t * initialRxConfig); void navigationUseRxConfig(rxConfig_t * initialRxConfig);
void navigationUseEscAndServoConfig(escAndServoConfig_t * initialEscAndServoConfig); void navigationUsemotorConfig(motorConfig_t * initialmotorConfig);
void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig); void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig);
void navigationInit(navConfig_t *initialnavConfig, void navigationInit(navConfig_t *initialnavConfig,
pidProfile_t *initialPidProfile, pidProfile_t *initialPidProfile,
rcControlsConfig_t *initialRcControlsConfig, rcControlsConfig_t *initialRcControlsConfig,
rxConfig_t * initialRxConfig, rxConfig_t * initialRxConfig,
flight3DConfig_t * initialFlight3DConfig, flight3DConfig_t * initialFlight3DConfig,
escAndServoConfig_t * initialEscAndServoConfig); motorConfig_t * initialmotorConfig);
/* Navigation system updates */ /* Navigation system updates */
void updateWaypointsAndNavigationMode(void); void updateWaypointsAndNavigationMode(void);

View file

@ -439,7 +439,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) { if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) {
uint16_t correctedThrottleValue = constrain(posControl.navConfig->fw_cruise_throttle + throttleCorrection, posControl.navConfig->fw_min_throttle, posControl.navConfig->fw_max_throttle); uint16_t correctedThrottleValue = constrain(posControl.navConfig->fw_cruise_throttle + throttleCorrection, posControl.navConfig->fw_min_throttle, posControl.navConfig->fw_max_throttle);
rcCommand[THROTTLE] = constrain(correctedThrottleValue, posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); rcCommand[THROTTLE] = constrain(correctedThrottleValue, posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle);
} }
} }

View file

@ -38,7 +38,7 @@
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "io/escservo.h" #include "io/motors.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
@ -109,8 +109,8 @@ static void updateAltitudeVelocityController_MC(uint32_t deltaMicros)
static void updateAltitudeThrottleController_MC(uint32_t deltaMicros) static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
{ {
// Calculate min and max throttle boundaries (to compensate for integral windup) // Calculate min and max throttle boundaries (to compensate for integral windup)
int16_t thrAdjustmentMin = (int16_t)posControl.escAndServoConfig->minthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; int16_t thrAdjustmentMin = (int16_t)posControl.motorConfig->minthrottle - (int16_t)posControl.navConfig->mc_hover_throttle;
int16_t thrAdjustmentMax = (int16_t)posControl.escAndServoConfig->maxthrottle - (int16_t)posControl.navConfig->mc_hover_throttle; int16_t thrAdjustmentMax = (int16_t)posControl.motorConfig->maxthrottle - (int16_t)posControl.navConfig->mc_hover_throttle;
posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false); posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false);
@ -128,11 +128,11 @@ bool adjustMulticopterAltitudeFromRCInput(void)
// Make sure we can satisfy max_manual_climb_rate in both up and down directions // Make sure we can satisfy max_manual_climb_rate in both up and down directions
if (rcThrottleAdjustment > 0) { if (rcThrottleAdjustment > 0) {
// Scaling from altHoldThrottleRCZero to maxthrottle // Scaling from altHoldThrottleRCZero to maxthrottle
rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (posControl.escAndServoConfig->maxthrottle - altHoldThrottleRCZero); rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (posControl.motorConfig->maxthrottle - altHoldThrottleRCZero);
} }
else { else {
// Scaling from minthrottle to altHoldThrottleRCZero // Scaling from minthrottle to altHoldThrottleRCZero
rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.escAndServoConfig->minthrottle); rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.motorConfig->minthrottle);
} }
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET); updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET);
@ -168,8 +168,8 @@ void setupMulticopterAltitudeController(void)
// Make sure we are able to satisfy the deadband // Make sure we are able to satisfy the deadband
altHoldThrottleRCZero = constrain(altHoldThrottleRCZero, altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
posControl.escAndServoConfig->minthrottle + posControl.rcControlsConfig->alt_hold_deadband + 10, posControl.motorConfig->minthrottle + posControl.rcControlsConfig->alt_hold_deadband + 10,
posControl.escAndServoConfig->maxthrottle - posControl.rcControlsConfig->alt_hold_deadband - 10); posControl.motorConfig->maxthrottle - posControl.rcControlsConfig->alt_hold_deadband - 10);
/* Force AH controller to initialize althold integral for pending takeoff on reset */ /* Force AH controller to initialize althold integral for pending takeoff on reset */
if (throttleStatus == THROTTLE_LOW) { if (throttleStatus == THROTTLE_LOW) {
@ -229,7 +229,7 @@ static void applyMulticopterAltitudeController(uint32_t currentTime)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle);
// Save processed throttle for future use // Save processed throttle for future use
rcCommandAdjustedThrottle = rcCommand[THROTTLE]; rcCommandAdjustedThrottle = rcCommand[THROTTLE];
@ -586,7 +586,7 @@ static void applyMulticopterEmergencyLandingController(uint32_t currentTime)
} }
// Update throttle controller // Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle); rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.motorConfig->minthrottle, posControl.motorConfig->maxthrottle);
} }
else { else {
/* Sensors has gone haywire, attempt to land regardless */ /* Sensors has gone haywire, attempt to land regardless */
@ -596,7 +596,7 @@ static void applyMulticopterEmergencyLandingController(uint32_t currentTime)
rcCommand[THROTTLE] = failsafeConfig->failsafe_throttle; rcCommand[THROTTLE] = failsafeConfig->failsafe_throttle;
} }
else { else {
rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle; rcCommand[THROTTLE] = posControl.motorConfig->minthrottle;
} }
} }
} }

View file

@ -281,7 +281,7 @@ typedef struct {
pidProfile_t * pidProfile; pidProfile_t * pidProfile;
rxConfig_t * rxConfig; rxConfig_t * rxConfig;
flight3DConfig_t * flight3DConfig; flight3DConfig_t * flight3DConfig;
escAndServoConfig_t * escAndServoConfig; motorConfig_t * motorConfig;
} navigationPosControl_t; } navigationPosControl_t;
extern navigationPosControl_t posControl; extern navigationPosControl_t posControl;

View file

@ -41,7 +41,7 @@
#include "rx/rx.h" #include "rx/rx.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/escservo.h" #include "io/servos.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"

View file

@ -55,7 +55,8 @@
#include "io/ledstrip.h" #include "io/ledstrip.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/gps.h" #include "io/gps.h"

View file

@ -17,11 +17,9 @@
#pragma once #pragma once
typedef struct escAndServoConfig_s { typedef struct motorConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
// PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms)
uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. } motorConfig_t;
} escAndServoConfig_t;

View file

@ -56,7 +56,8 @@
#include "drivers/buf_writer.h" #include "drivers/buf_writer.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
@ -594,9 +595,9 @@ const clivalue_t valueTable[] = {
{ "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON }, 0 }, { "rc_smoothing", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.rcSmoothing, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON }, 0 }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.inputFilteringMode, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "max_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 },
{ "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "min_command", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, // FIXME upper limit should match code in the mixer, 1500 currently
{ "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, // FIXME lower limit should match code in the mixer, 1500 currently, { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, // FIXME lower limit should match code in the mixer, 1500 currently,
@ -759,7 +760,7 @@ const clivalue_t valueTable[] = {
{ "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.servoConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 }, { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.servoConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 },
{ "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 }, { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 },
{ "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE }, 0 }, { "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE }, 0 },
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 },
{ "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 }, 0 }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 }, 0 },
{ "fw_iterm_throw_limit", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.fixedWingItermThrowLimit, .config.minmax = { FW_ITERM_THROW_LIMIT_MIN, FW_ITERM_THROW_LIMIT_MAX}, 0 }, { "fw_iterm_throw_limit", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.fixedWingItermThrowLimit, .config.minmax = { FW_ITERM_THROW_LIMIT_MIN, FW_ITERM_THROW_LIMIT_MAX}, 0 },
#endif #endif

23
src/main/io/servos.h Normal file
View file

@ -0,0 +1,23 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
typedef struct servoConfig_s {
// PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500.
} servoConfig_t;

View file

@ -72,7 +72,8 @@
#include "io/serial.h" #include "io/serial.h"
#include "io/flashfs.h" #include "io/flashfs.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "io/gimbal.h" #include "io/gimbal.h"
@ -269,13 +270,13 @@ void init(void)
#ifdef USE_SERVOS #ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos(); pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = masterConfig.escAndServoConfig.servoCenterPulse; pwm_params.servoCenterPulse = masterConfig.servoConfig.servoCenterPulse;
pwm_params.servoPwmRate = masterConfig.servo_pwm_rate; pwm_params.servoPwmRate = masterConfig.servo_pwm_rate;
#endif #endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125); pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = masterConfig.motor_pwm_rate; pwm_params.motorPwmRate = masterConfig.motor_pwm_rate;
pwm_params.idlePulse = masterConfig.escAndServoConfig.mincommand; pwm_params.idlePulse = masterConfig.motorConfig.mincommand;
if (feature(FEATURE_3D)) if (feature(FEATURE_3D))
pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d;
if (pwm_params.motorPwmRate > 500) if (pwm_params.motorPwmRate > 500)
@ -495,7 +496,7 @@ void init(void)
&currentProfile->rcControlsConfig, &currentProfile->rcControlsConfig,
&masterConfig.rxConfig, &masterConfig.rxConfig,
&masterConfig.flight3DConfig, &masterConfig.flight3DConfig,
&masterConfig.escAndServoConfig &masterConfig.motorConfig
); );
#endif #endif

View file

@ -49,7 +49,8 @@
#include "io/beeper.h" #include "io/beeper.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/servos.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"

View file

@ -48,6 +48,8 @@
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
#include "io/motors.h"
#include "io/servos.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"

View file

@ -33,7 +33,7 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "io/escservo.h" #include "io/motors.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/serial.h" #include "io/serial.h"