1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-15 20:35:29 +03:00

Added motorConfig parameter group

This commit is contained in:
Martin Budden 2017-01-04 14:02:58 +00:00
parent c22695fbc7
commit d8efe67d64
18 changed files with 148 additions and 112 deletions

View file

@ -516,7 +516,6 @@ COMMON_SRC = \
flight/mixer.c \ flight/mixer.c \
flight/servos.c \ flight/servos.c \
flight/pid.c \ flight/pid.c \
io/beeper.c \
fc/fc_init.c \ fc/fc_init.c \
fc/fc_tasks.c \ fc/fc_tasks.c \
fc/fc_hardfaults.c \ fc/fc_hardfaults.c \
@ -524,6 +523,8 @@ COMMON_SRC = \
fc/rc_controls.c \ fc/rc_controls.c \
fc/rc_curves.c \ fc/rc_curves.c \
fc/serial_cli.c \ fc/serial_cli.c \
io/beeper.c \
io/motors.c \
io/serial.c \ io/serial.c \
io/serial_4way.c \ io/serial_4way.c \
io/serial_4way_avrootloader.c \ io/serial_4way_avrootloader.c \

View file

@ -91,7 +91,8 @@ static OSD_Entry menuMiscEntries[]=
{ {
{ "-- MISC --", OME_Label, NULL, NULL, 0 }, { "-- MISC --", OME_Label, NULL, NULL, 0 },
{ "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 }, //!!TODO - fix up CMS menu entries to use parameter groups
// { "MIN THR", OME_UINT16, NULL, &(OSD_UINT16_t){ &motorConfig()->minthrottle, 1000, 2000, 1 }, 0 },
{ "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatscale, 1, 250, 1 }, 0 }, { "VBAT SCALE", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatscale, 1, 250, 1 }, 0 },
{ "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage, 10, 50, 1 }, 0 }, { "VBAT CLMAX", OME_UINT8, NULL, &(OSD_UINT8_t) { &batteryConfig()->vbatmaxcellvoltage, 10, 50, 1 }, 0 },
{ "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0}, { "RC PREV", OME_Submenu, cmsMenuChange, &cmsx_menuRcPreview, 0},

View file

@ -48,7 +48,6 @@
#include "io/gps.h" #include "io/gps.h"
#include "io/osd.h" #include "io/osd.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
#include "io/motors.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/servos.h" #include "io/servos.h"
@ -58,7 +57,6 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#define motorConfig(x) (&masterConfig.motorConfig)
#define flight3DConfig(x) (&masterConfig.flight3DConfig) #define flight3DConfig(x) (&masterConfig.flight3DConfig)
#define servoConfig(x) (&masterConfig.servoConfig) #define servoConfig(x) (&masterConfig.servoConfig)
#define servoMixerConfig(x) (&masterConfig.servoMixerConfig) #define servoMixerConfig(x) (&masterConfig.servoMixerConfig)
@ -98,7 +96,6 @@ typedef struct master_s {
// motor/esc/servo related stuff // motor/esc/servo related stuff
motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS]; motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS];
motorConfig_t motorConfig;
flight3DConfig_t flight3DConfig; flight3DConfig_t flight3DConfig;
#ifdef USE_SERVOS #ifdef USE_SERVOS

View file

@ -21,7 +21,7 @@
//#define PG_GIMBAL_CONFIG 3 //#define PG_GIMBAL_CONFIG 3
//#define PG_MOTOR_MIXER 4 //#define PG_MOTOR_MIXER 4
//#define PG_BLACKBOX_CONFIG 5 //#define PG_BLACKBOX_CONFIG 5
//#define PG_MOTOR_AND_SERVO_CONFIG 6 #define PG_MOTOR_CONFIG 6
//#define PG_SENSOR_SELECTION_CONFIG 7 //#define PG_SENSOR_SELECTION_CONFIG 7
//#define PG_SENSOR_ALIGNMENT_CONFIG 8 //#define PG_SENSOR_ALIGNMENT_CONFIG 8
//#define PG_SENSOR_TRIMS 9 //#define PG_SENSOR_TRIMS 9

View file

@ -45,7 +45,6 @@
#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/motors.h"
#include "io/servos.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"
@ -229,22 +228,6 @@ void validateNavConfig(navConfig_t * navConfig)
} }
#endif #endif
void resetMotorConfig(motorConfig_t *motorConfig)
{
#ifdef BRUSHED_MOTORS
motorConfig->minthrottle = 1000;
motorConfig->motorPwmProtocol = PWM_TYPE_BRUSHED;
motorConfig->motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
#else
motorConfig->minthrottle = 1150;
motorConfig->motorPwmProtocol = PWM_TYPE_STANDARD;
motorConfig->motorPwmRate = BRUSHLESS_MOTORS_PWM_RATE;
#endif
motorConfig->maxthrottle = 1850;
motorConfig->mincommand = 1000;
}
#ifdef USE_SERVOS #ifdef USE_SERVOS
void resetServoConfig(servoConfig_t *servoConfig) void resetServoConfig(servoConfig_t *servoConfig)
{ {
@ -467,7 +450,6 @@ void createDefaultConfig(master_t *config)
resetServoConfig(&config->servoConfig); resetServoConfig(&config->servoConfig);
#endif #endif
resetMotorConfig(&config->motorConfig);
resetFlight3DConfig(&config->flight3DConfig); resetFlight3DConfig(&config->flight3DConfig);
#ifdef GPS #ifdef GPS
@ -672,7 +654,7 @@ void resetConfigs(void)
static void activateControlRateConfig(void) static void activateControlRateConfig(void)
{ {
generateThrottleCurve(currentControlRateProfile, &masterConfig.motorConfig); generateThrottleCurve(currentControlRateProfile);
} }
static void activateConfig(void) static void activateConfig(void)
@ -681,7 +663,7 @@ static void activateConfig(void)
resetAdjustmentStates(); resetAdjustmentStates();
useRcControlsConfig(masterConfig.modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile); useRcControlsConfig(masterConfig.modeActivationConditions, &currentProfile->pidProfile);
#ifdef TELEMETRY #ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig); telemetryUseConfig(&masterConfig.telemetryConfig);
@ -692,7 +674,7 @@ static void activateConfig(void)
setAccelerationCalibrationValues(); setAccelerationCalibrationValues();
setAccelerationFilter(); setAccelerationFilter();
mixerUseConfigs(&masterConfig.flight3DConfig, &masterConfig.motorConfig, &masterConfig.mixerConfig); mixerUseConfigs(&masterConfig.flight3DConfig, &masterConfig.mixerConfig);
#ifdef USE_SERVOS #ifdef USE_SERVOS
servosUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig); servosUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig);
#endif #endif
@ -707,7 +689,7 @@ static void activateConfig(void)
navigationUseRcControlsConfig(&masterConfig.rcControlsConfig); navigationUseRcControlsConfig(&masterConfig.rcControlsConfig);
navigationUseRxConfig(rxConfig()); navigationUseRxConfig(rxConfig());
navigationUseFlight3DConfig(&masterConfig.flight3DConfig); navigationUseFlight3DConfig(&masterConfig.flight3DConfig);
navigationUsemotorConfig(&masterConfig.motorConfig); navigationUsemotorConfig(motorConfig());
#endif #endif
} }

View file

@ -547,7 +547,7 @@ void init(void)
&masterConfig.rcControlsConfig, &masterConfig.rcControlsConfig,
rxConfig(), rxConfig(),
&masterConfig.flight3DConfig, &masterConfig.flight3DConfig,
&masterConfig.motorConfig motorConfig()
); );
#endif #endif

View file

@ -1313,7 +1313,7 @@ static mspResult_e mspFcProcessInCommand(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(masterConfig.modeActivationConditions, &masterConfig.motorConfig, &currentProfile->pidProfile); useRcControlsConfig(masterConfig.modeActivationConditions, &currentProfile->pidProfile);
} else { } else {
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }

View file

@ -57,6 +57,7 @@
#include "io/servos.h" #include "io/servos.h"
#include "io/gimbal.h" #include "io/gimbal.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/motors.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/statusindicator.h" #include "io/statusindicator.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/asyncfatfs/asyncfatfs.h"
@ -661,7 +662,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
} }
// Update PID coefficients // Update PID coefficients
updatePIDCoefficients(&currentProfile->pidProfile, currentControlRateProfile, &masterConfig.motorConfig); updatePIDCoefficients(&currentProfile->pidProfile, currentControlRateProfile, motorConfig());
// Calculate stabilisation // Calculate stabilisation
pidController(&currentProfile->pidProfile, currentControlRateProfile, rxConfig()); pidController(&currentProfile->pidProfile, currentControlRateProfile, rxConfig());

View file

@ -57,7 +57,6 @@
#define AIRMODE_DEADBAND 25 #define AIRMODE_DEADBAND 25
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)
@ -521,7 +520,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, motorConfig); generateThrottleCurve(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue); blackboxLogInflightAdjustmentEvent(ADJUSTMENT_THROTTLE_EXPO, newValue);
break; break;
case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_ROLL_RATE:
@ -720,9 +719,8 @@ 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, motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse)
{ {
motorConfig = motorConfigToUse;
pidProfile = pidProfileToUse; pidProfile = pidProfileToUse;
isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);

View file

@ -267,6 +267,5 @@ 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 motorConfig_s;
struct pidProfile_s; struct pidProfile_s;
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse);

View file

@ -33,9 +33,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, motorConfig_t *motorConfig) void generateThrottleCurve(controlRateConfig_t *controlRateConfig)
{ {
lookupThrottleRCMid = motorConfig->minthrottle + (int32_t)(motorConfig->maxthrottle - motorConfig->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;
@ -45,7 +45,7 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, motorConfig_t
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] = motorConfig->minthrottle + (int32_t) (motorConfig->maxthrottle - motorConfig->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,7 @@
#pragma once #pragma once
struct controlRateConfig_s; struct controlRateConfig_s;
struct motorConfig_s; void generateThrottleCurve(struct controlRateConfig_s *controlRateConfig);
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

@ -489,6 +489,7 @@ typedef struct {
} __attribute__((packed)) clivalue_t; } __attribute__((packed)) clivalue_t;
static const clivalue_t valueTable[] = { static const clivalue_t valueTable[] = {
// PG_GYRO_CONFIG
{ "looptime", VAR_UINT16 | MASTER_VALUE, .config.minmax = {0, 9000}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, looptime) }, { "looptime", VAR_UINT16 | MASTER_VALUE, .config.minmax = {0, 9000}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, looptime) },
{ "gyro_sync", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroSync) }, { "gyro_sync", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroSync) },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroSyncDenominator) }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 32 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroSyncDenominator) },
@ -505,6 +506,7 @@ static const clivalue_t valueTable[] = {
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = {1, 500 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = {1, 500 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
#endif #endif
// PG_ACCELEROMETER_CONFIG
{ "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) }, { "align_acc", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_align) },
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) }, { "acc_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACC_HARDWARE }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_hardware) },
{ "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = {0, 200 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) }, { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = {0, 200 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) },
@ -515,6 +517,7 @@ static const clivalue_t valueTable[] = {
{ "accgain_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Y]) }, { "accgain_y", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Y]) },
{ "accgain_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Z]) }, { "accgain_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Z]) },
// PG_COMPASS_CONFIG
#ifdef MAG #ifdef MAG
{ "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) }, { "align_mag", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_align) },
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAG_HARDWARE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hardware) }, { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MAG_HARDWARE }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hardware) },
@ -525,11 +528,13 @@ static const clivalue_t valueTable[] = {
{ "mag_hold_rate_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { MAG_HOLD_RATE_LIMIT_MIN, MAG_HOLD_RATE_LIMIT_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hold_rate_limit) }, { "mag_hold_rate_limit", VAR_UINT8 | MASTER_VALUE, .config.minmax = { MAG_HOLD_RATE_LIMIT_MIN, MAG_HOLD_RATE_LIMIT_MAX }, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_hold_rate_limit) },
#endif #endif
// PG_BAROMETER_CONFIG
#ifdef BARO #ifdef BARO
{ "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) }, { "baro_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) },
{ "baro_use_median_filter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, use_median_filtering) }, { "baro_use_median_filter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, use_median_filtering) },
#endif #endif
// PG_PITOTMETER_CONFIG
#ifdef PITOT #ifdef PITOT
{ "pitot_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PITOT_HARDWARE }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, pitot_hardware) }, { "pitot_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PITOT_HARDWARE }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, pitot_hardware) },
{ "pitot_use_median_filter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, use_median_filtering) }, { "pitot_use_median_filter", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, use_median_filtering) },
@ -537,6 +542,7 @@ static const clivalue_t valueTable[] = {
{ "pitot_scale", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, pitot_scale) }, { "pitot_scale", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PITOTMETER_CONFIG, offsetof(pitotmeterConfig_t, pitot_scale) },
#endif #endif
// PG_RX_CONFIG
{ "mid_rc", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1200, 1700 }, PG_RX_CONFIG, offsetof(rxConfig_t, midrc) }, { "mid_rc", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1200, 1700 }, PG_RX_CONFIG, offsetof(rxConfig_t, midrc) },
{ "min_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, mincheck) }, { "min_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, mincheck) },
{ "max_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, maxcheck) }, { "max_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, maxcheck) },
@ -557,6 +563,13 @@ static const clivalue_t valueTable[] = {
#endif #endif
{ "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) }, { "rx_min_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_min_usec) },
{ "rx_max_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_max_usec) }, { "rx_max_usec", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rx_max_usec) },
// PG_MOTOR_CONFIG
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, minthrottle) },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, maxthrottle) },
{ "min_command", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, mincommand) },
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 50, 32000 }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPwmRate) },
{ "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL }, PG_MOTOR_CONFIG, offsetof(motorConfig_t, motorPwmProtocol) },
}; };
#else #else
@ -579,18 +592,11 @@ const clivalue_t valueTable[] = {
{ "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &pwmRxConfig()->inputFilteringMode, .config.lookup = { TABLE_OFF_ON } }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &pwmRxConfig()->inputFilteringMode, .config.lookup = { TABLE_OFF_ON } },
{ "min_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->minthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "max_throttle", VAR_UINT16 | MASTER_VALUE, &motorConfig()->maxthrottle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "min_command", VAR_UINT16 | MASTER_VALUE, &motorConfig()->mincommand, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently { "3d_deadband_low", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_low, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME upper limit should match code in the mixer, 1500 currently
{ "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently, { "3d_deadband_high", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_high, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, // FIXME lower limit should match code in the mixer, 1500 currently,
{ "3d_neutral", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &flight3DConfig()->deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &motorConfig()->motorPwmRate, .config.minmax = { 50, 32000 } },
{ "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &motorConfig()->motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } },
{ "fixed_wing_auto_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->fixed_wing_auto_arm, .config.lookup = { TABLE_OFF_ON } }, { "fixed_wing_auto_arm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->fixed_wing_auto_arm, .config.lookup = { TABLE_OFF_ON } },
{ "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } }, { "disarm_kill_switch", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &armingConfig()->disarm_kill_switch, .config.lookup = { TABLE_OFF_ON } },
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &armingConfig()->auto_disarm_delay, .config.minmax = { 0, 60 } }, { "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, &armingConfig()->auto_disarm_delay, .config.minmax = { 0, 60 } },
@ -917,45 +923,45 @@ static void printValuePointer(const clivalue_t *var, void *valuePointer, uint32_
char buf[8]; char buf[8];
switch (var->type & VALUE_TYPE_MASK) { switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8: case VAR_UINT8:
value = *(uint8_t *)valuePointer; value = *(uint8_t *)valuePointer;
break; break;
case VAR_INT8: case VAR_INT8:
value = *(int8_t *)valuePointer; value = *(int8_t *)valuePointer;
break; break;
case VAR_UINT16: case VAR_UINT16:
value = *(uint16_t *)valuePointer; value = *(uint16_t *)valuePointer;
break; break;
case VAR_INT16: case VAR_INT16:
value = *(int16_t *)valuePointer; value = *(int16_t *)valuePointer;
break; break;
case VAR_UINT32: case VAR_UINT32:
value = *(uint32_t *)valuePointer; value = *(uint32_t *)valuePointer;
break; break;
case VAR_FLOAT: case VAR_FLOAT:
cliPrintf("%s", ftoa(*(float *)valuePointer, buf)); cliPrintf("%s", ftoa(*(float *)valuePointer, buf));
if (full && (var->type & VALUE_MODE_MASK) == MODE_DIRECT) { if (full && (var->type & VALUE_MODE_MASK) == MODE_DIRECT) {
cliPrintf(" %s", ftoa((float)var->config.minmax.min, buf)); cliPrintf(" %s", ftoa((float)var->config.minmax.min, buf));
cliPrintf(" %s", ftoa((float)var->config.minmax.max, buf)); cliPrintf(" %s", ftoa((float)var->config.minmax.max, buf));
} }
return; // return from case for float only return; // return from case for float only
} }
switch(var->type & VALUE_MODE_MASK) { switch(var->type & VALUE_MODE_MASK) {
case MODE_DIRECT: case MODE_DIRECT:
cliPrintf("%d", value); cliPrintf("%d", value);
if (full) { if (full) {
cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max); cliPrintf(" %d %d", var->config.minmax.min, var->config.minmax.max);
} }
break; break;
case MODE_LOOKUP: case MODE_LOOKUP:
cliPrintf(lookupTables[var->config.lookup.tableIndex].values[value]); cliPrintf(lookupTables[var->config.lookup.tableIndex].values[value]);
break; break;
} }
} }
@ -1077,6 +1083,7 @@ static pitotmeterConfig_t pitotmeterConfigCopy;
static rxConfig_t rxConfigCopy; static rxConfig_t rxConfigCopy;
static rxFailsafeChannelConfig_t rxFailsafeChannelConfigsCopy[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static rxFailsafeChannelConfig_t rxFailsafeChannelConfigsCopy[MAX_SUPPORTED_RC_CHANNEL_COUNT];
static rxChannelRangeConfig_t rxChannelRangeConfigsCopy[NON_AUX_CHANNEL_COUNT]; static rxChannelRangeConfig_t rxChannelRangeConfigsCopy[NON_AUX_CHANNEL_COUNT];
static motorConfig_t motorConfigCopy;
static void backupConfigs(void) static void backupConfigs(void)
{ {
@ -1099,6 +1106,7 @@ static void backupConfigs(void)
for (int ii = 0; ii < NON_AUX_CHANNEL_COUNT; ++ii) { for (int ii = 0; ii < NON_AUX_CHANNEL_COUNT; ++ii) {
rxChannelRangeConfigsCopy[ii] = *rxChannelRangeConfigs(ii); rxChannelRangeConfigsCopy[ii] = *rxChannelRangeConfigs(ii);
} }
motorConfigCopy = *motorConfig();
} }
static void restoreConfigs(void) static void restoreConfigs(void)
@ -1121,6 +1129,7 @@ static void restoreConfigs(void)
for (int ii = 0; ii < NON_AUX_CHANNEL_COUNT; ++ii) { for (int ii = 0; ii < NON_AUX_CHANNEL_COUNT; ++ii) {
*rxChannelRangeConfigs(ii) = rxChannelRangeConfigsCopy[ii]; *rxChannelRangeConfigs(ii) = rxChannelRangeConfigsCopy[ii];
} }
*motorConfig() = motorConfigCopy;
} }
static void *getDefaultPointer(const void *valuePointer, const master_t *defaultConfig) static void *getDefaultPointer(const void *valuePointer, const master_t *defaultConfig)
@ -1162,6 +1171,7 @@ static void dumpValues(uint16_t valueSection, uint8_t dumpMask, const master_t *
dumpPgValues(MASTER_VALUE, dumpMask, PG_PITOT_CONFIG, &pitotmeterConfigCopy, pitotmeterConfig()); dumpPgValues(MASTER_VALUE, dumpMask, PG_PITOT_CONFIG, &pitotmeterConfigCopy, pitotmeterConfig());
#endif #endif
dumpPgValues(MASTER_VALUE, dumpMask, PG_RX_CONFIG, &rxConfigCopy, rxConfig()); dumpPgValues(MASTER_VALUE, dumpMask, PG_RX_CONFIG, &rxConfigCopy, rxConfig());
dumpPgValues(MASTER_VALUE, dumpMask, PG_MOTOR_CONFIG, &motorConfigCopy, motorConfig());
return; return;
} }
#endif #endif
@ -1213,23 +1223,23 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value)
void *ptr = getValuePointer(var); void *ptr = getValuePointer(var);
switch (var->type & VALUE_TYPE_MASK) { switch (var->type & VALUE_TYPE_MASK) {
case VAR_UINT8: case VAR_UINT8:
case VAR_INT8: case VAR_INT8:
*(int8_t *)ptr = value.int_value; *(int8_t *)ptr = value.int_value;
break; break;
case VAR_UINT16: case VAR_UINT16:
case VAR_INT16: case VAR_INT16:
*(int16_t *)ptr = value.int_value; *(int16_t *)ptr = value.int_value;
break; break;
case VAR_UINT32: case VAR_UINT32:
*(uint32_t *)ptr = value.int_value; *(uint32_t *)ptr = value.int_value;
break; break;
case VAR_FLOAT: case VAR_FLOAT:
*(float *)ptr = (float)value.float_value; *(float *)ptr = (float)value.float_value;
break; break;
} }
} }

View file

@ -66,7 +66,6 @@ bool motorLimitReached = false;
mixerConfig_t *mixerConfig; mixerConfig_t *mixerConfig;
static flight3DConfig_t *flight3DConfig; static flight3DConfig_t *flight3DConfig;
static motorConfig_t *motorConfig;
mixerMode_e currentMixerMode; mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
@ -263,11 +262,9 @@ static motorMixer_t *customMixers;
void mixerUseConfigs( void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse, flight3DConfig_t *flight3DConfigToUse,
motorConfig_t *motorConfigToUse,
mixerConfig_t *mixerConfigToUse) mixerConfig_t *mixerConfigToUse)
{ {
flight3DConfig = flight3DConfigToUse; flight3DConfig = flight3DConfigToUse;
motorConfig = motorConfigToUse;
mixerConfig = mixerConfigToUse; mixerConfig = mixerConfigToUse;
} }
@ -364,7 +361,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 : motorConfig->mincommand; motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig()->mincommand;
} }
void writeMotors(void) void writeMotors(void)
@ -387,7 +384,7 @@ void writeAllMotors(int16_t mc)
void stopMotors(void) void stopMotors(void)
{ {
writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->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.
} }
@ -433,23 +430,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 = motorConfig->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 = motorConfig->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 = motorConfig->minthrottle; throttleMin = motorConfig()->minthrottle;
} else { // Deadband handling from positive to negative } else { // Deadband handling from positive to negative
throttleMax = motorConfig->maxthrottle; throttleMax = motorConfig()->maxthrottle;
throttleCommand = throttleMin = flight3DConfig->deadband3d_high; throttleCommand = throttleMin = flight3DConfig->deadband3d_high;
} }
} else { } else {
throttleCommand = rcCommand[THROTTLE]; throttleCommand = rcCommand[THROTTLE];
throttleMin = motorConfig->minthrottle; throttleMin = motorConfig()->minthrottle;
throttleMax = motorConfig->maxthrottle; throttleMax = motorConfig()->maxthrottle;
} }
throttleRange = throttleMax - throttleMin; throttleRange = throttleMax - throttleMin;
@ -481,21 +478,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], motorConfig->mincommand, motorConfig->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], motorConfig->minthrottle, flight3DConfig->deadband3d_low); motor[i] = constrain(motor[i], motorConfig()->minthrottle, flight3DConfig->deadband3d_low);
} else { } else {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, motorConfig->maxthrottle); motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, motorConfig()->maxthrottle);
} }
} else { } else {
motor[i] = constrain(motor[i], motorConfig->minthrottle, motorConfig->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) && !isFailsafeActive) { if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isFailsafeActive) {
if (((rcData[THROTTLE]) < rxConfig()->mincheck) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { if (((rcData[THROTTLE]) < rxConfig()->mincheck) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
motor[i] = motorConfig->mincommand; motor[i] = motorConfig()->mincommand;
} }
} }
} }

View file

@ -109,7 +109,6 @@ extern bool motorLimitReached;
struct motorConfig_s; struct motorConfig_s;
void mixerUseConfigs( void mixerUseConfigs(
flight3DConfig_t *flight3DConfigToUse, flight3DConfig_t *flight3DConfigToUse,
struct motorConfig_s *motorConfigToUse,
mixerConfig_t *mixerConfigToUse); mixerConfig_t *mixerConfigToUse);
void writeAllMotors(int16_t mc); void writeAllMotors(int16_t mc);

View file

@ -39,6 +39,7 @@
#include "flight/navigation_rewrite.h" #include "flight/navigation_rewrite.h"
#include "io/gps.h" #include "io/gps.h"
#include "io/motors.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -192,7 +193,7 @@ void schedulePidGainsUpdate(void)
pidGainsUpdateRequired = true; pidGainsUpdateRequired = true;
} }
void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const struct motorConfig_s *motorConfig) void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig)
{ {
static uint16_t prevThrottle = 0; static uint16_t prevThrottle = 0;

47
src/main/io/motors.c Normal file
View file

@ -0,0 +1,47 @@
/*
* 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/>.
*/
#include <stdint.h>
#include "platform.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "drivers/pwm_output.h"
#include "io/motors.h"
#ifdef BRUSHED_MOTORS
#define DEFAULT_PWM_PROTOCOL PWM_TYPE_BRUSHED
#define DEFAULT_PWM_RATE 16000
#define DEFAULT_MIN_THROTTLE 1000
#else
#define DEFAULT_PWM_PROTOCOL PWM_TYPE_STANDARD
#define DEFAULT_PWM_RATE 400
#define DEFAULT_MIN_THROTTLE 1150
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 1);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.minthrottle = DEFAULT_MIN_THROTTLE,
.motorPwmProtocol = DEFAULT_PWM_PROTOCOL,
.motorPwmRate = DEFAULT_PWM_RATE,
.maxthrottle = 1850,
.mincommand = 1000
);

View file

@ -17,6 +17,8 @@
#pragma once #pragma once
#include "config/parameter_group.h"
typedef struct motorConfig_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 (1ms 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.
@ -25,3 +27,5 @@ typedef struct motorConfig_s {
uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz)
uint8_t motorPwmProtocol; uint8_t motorPwmProtocol;
} motorConfig_t; } motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig);