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:
parent
c22695fbc7
commit
d8efe67d64
18 changed files with 148 additions and 112 deletions
3
Makefile
3
Makefile
|
@ -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 \
|
||||||
|
|
|
@ -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},
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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, ¤tProfile->pidProfile);
|
useRcControlsConfig(masterConfig.modeActivationConditions, ¤tProfile->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
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -547,7 +547,7 @@ void init(void)
|
||||||
&masterConfig.rcControlsConfig,
|
&masterConfig.rcControlsConfig,
|
||||||
rxConfig(),
|
rxConfig(),
|
||||||
&masterConfig.flight3DConfig,
|
&masterConfig.flight3DConfig,
|
||||||
&masterConfig.motorConfig
|
motorConfig()
|
||||||
);
|
);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -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, ¤tProfile->pidProfile);
|
useRcControlsConfig(masterConfig.modeActivationConditions, ¤tProfile->pidProfile);
|
||||||
} else {
|
} else {
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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(¤tProfile->pidProfile, currentControlRateProfile, &masterConfig.motorConfig);
|
updatePIDCoefficients(¤tProfile->pidProfile, currentControlRateProfile, motorConfig());
|
||||||
|
|
||||||
// Calculate stabilisation
|
// Calculate stabilisation
|
||||||
pidController(¤tProfile->pidProfile, currentControlRateProfile, rxConfig());
|
pidController(¤tProfile->pidProfile, currentControlRateProfile, rxConfig());
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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
47
src/main/io/motors.c
Normal 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
|
||||||
|
);
|
|
@ -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);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue