diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 78fd819d77..5283d6eba4 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -30,6 +30,10 @@ #define M_PIf 3.14159265358979323846f #define RAD (M_PIf / 180.0f) +#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) +#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) +#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) +#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f) #define MIN(a, b) ((a) < (b) ? (a) : (b)) #define MAX(a, b) ((a) > (b) ? (a) : (b)) diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 584c18876e..21cf9d4218 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -325,7 +325,5 @@ typedef struct master_s { } master_t; extern master_t masterConfig; -extern profile_t *currentProfile; -extern controlRateConfig_t *currentControlRateProfile; void createDefaultConfig(master_t *config); diff --git a/src/main/drivers/light_led.c b/src/main/drivers/light_led.c index 8011f04cf4..8f3fcd9d15 100644 --- a/src/main/drivers/light_led.c +++ b/src/main/drivers/light_led.c @@ -25,7 +25,7 @@ static IO_t leds[LED_NUMBER]; static uint8_t ledPolarity = 0; -void ledInit(statusLedConfig_t *statusLedConfig) +void ledInit(const statusLedConfig_t *statusLedConfig) { LED0_OFF; LED1_OFF; diff --git a/src/main/drivers/light_led.h b/src/main/drivers/light_led.h index 25961579e4..341bbb81d9 100644 --- a/src/main/drivers/light_led.h +++ b/src/main/drivers/light_led.h @@ -17,6 +17,7 @@ #pragma once +#include "config/parameter_group.h" #include "drivers/io_types.h" #define LED_NUMBER 3 @@ -26,6 +27,8 @@ typedef struct statusLedConfig_s { uint8_t polarity; } statusLedConfig_t; +PG_DECLARE(statusLedConfig_t, statusLedConfig); + // Helpful macros #ifdef LED0 # define LED0_TOGGLE ledToggle(0) @@ -57,6 +60,6 @@ typedef struct statusLedConfig_s { # define LED2_ON do {} while(0) #endif -void ledInit(statusLedConfig_t *statusLedConfig); +void ledInit(const statusLedConfig_t *statusLedConfig); void ledToggle(int led); void ledSet(int led, bool state); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 58a847ecf8..e4eb8c3c10 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -892,7 +892,6 @@ void activateConfig(void) useRcControlsConfig( modeActivationProfile()->modeActivationConditions, - &masterConfig.motorConfig, ¤tProfile->pidProfile ); @@ -918,12 +917,7 @@ void activateConfig(void) throttleCorrectionConfig()->throttle_correction_angle ); - configureAltitudeHold( - ¤tProfile->pidProfile, - &masterConfig.barometerConfig, - &masterConfig.rcControlsConfig, - &masterConfig.motorConfig - ); + configureAltitudeHold(¤tProfile->pidProfile); } void validateAndFixConfig(void) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 3b69a1f189..b092f15f80 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -66,6 +66,10 @@ typedef struct systemConfig_s { } systemConfig_t; //!!TODOPG_DECLARE(systemConfig_t, systemConfig); +struct profile_s; +extern struct profile_s *currentProfile; +struct controlRateConfig_s; +extern struct controlRateConfig_s *currentControlRateProfile; void beeperOffSet(uint32_t mask); void beeperOffSetAll(uint8_t beeperCount); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 57b926142c..47535bb6f3 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -24,10 +24,10 @@ #include "blackbox/blackbox.h" -#include "common/maths.h" #include "common/axis.h" -#include "common/utils.h" #include "common/filter.h" +#include "common/maths.h" +#include "common/utils.h" #include "config/config_profile.h" #include "config/feature.h" @@ -72,6 +72,7 @@ #include "flight/pid.h" #include "flight/failsafe.h" #include "flight/altitudehold.h" +#include "flight/imu.h" // June 2013 V2.2-dev @@ -287,7 +288,7 @@ void processRx(timeUs_t currentTimeUs) failsafeUpdateState(); } - throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); if (isAirmodeActive() && ARMING_FLAG(ARMED)) { if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset @@ -353,7 +354,7 @@ void processRx(timeUs_t currentTimeUs) } } - processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch); + processRcStickPositions(throttleStatus); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); @@ -363,7 +364,7 @@ void processRx(timeUs_t currentTimeUs) if (!cliMode) { updateAdjustmentStates(adjustmentProfile()->adjustmentRanges); - processRcAdjustments(currentControlRateProfile, rxConfig()); + processRcAdjustments(currentControlRateProfile); } bool canUseHorizonMode = true; @@ -484,7 +485,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs) updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { - applyAltHold(&masterConfig.airplaneConfig); + applyAltHold(); } } #endif diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index f1652946b6..5abdcaee74 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -443,7 +443,7 @@ void init(void) cliInit(serialConfig()); #endif - failsafeInit(flight3DConfig()->deadband3d_throttle); + failsafeInit(); rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 43175c83c5..b8a7d5aeb5 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1356,7 +1356,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) mac->range.startStep = sbufReadU8(src); mac->range.endStep = sbufReadU8(src); - useRcControlsConfig(modeActivationConditions(0), motorConfig(), ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationConditions(0), ¤tProfile->pidProfile); } else { return MSP_RESULT_ERROR; } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index d37278147e..ecbb07659d 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -131,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs) if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTimeUs; - updateCurrentMeter(ibatTimeSinceLastServiced, rxConfig(), flight3DConfig()->deadband3d_throttle); + updateCurrentMeter(ibatTimeSinceLastServiced); } } } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 943be00b87..2f09eed281 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -60,7 +60,6 @@ #include "flight/failsafe.h" -static const motorConfig_t *motorConfig; static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) @@ -116,20 +115,20 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } -throttleStatus_e calculateThrottleStatus(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +throttleStatus_e calculateThrottleStatus(void) { if (feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) { - if ((rcData[THROTTLE] > (rxConfig->midrc - deadband3d_throttle) && rcData[THROTTLE] < (rxConfig->midrc + deadband3d_throttle))) + if ((rcData[THROTTLE] > (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) && rcData[THROTTLE] < (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle))) return THROTTLE_LOW; } else { - if (rcData[THROTTLE] < rxConfig->mincheck) + if (rcData[THROTTLE] < rxConfig()->mincheck) return THROTTLE_LOW; } return THROTTLE_HIGH; } -void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch) +void processRcStickPositions(throttleStatus_e throttleStatus) { static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors static uint8_t rcSticks; // this hold sticks position for command combos @@ -141,9 +140,9 @@ void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e thrott // checking sticks positions for (i = 0; i < 4; i++) { stTmp >>= 2; - if (rcData[i] > rxConfig->mincheck) + if (rcData[i] > rxConfig()->mincheck) stTmp |= 0x80; // check for MIN - if (rcData[i] < rxConfig->maxcheck) + if (rcData[i] < rxConfig()->maxcheck) stTmp |= 0x40; // check for MAX } if (stTmp == rcSticks) { @@ -170,7 +169,7 @@ void processRcStickPositions(const rxConfig_t *rxConfig, throttleStatus_e thrott if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) { rcDisarmTicks++; if (rcDisarmTicks > 3) { - if (disarm_kill_switch) { + if (armingConfig()->disarm_kill_switch) { mwDisarm(); } else if (throttleStatus == THROTTLE_LOW) { mwDisarm(); @@ -647,7 +646,7 @@ static void applySelectAdjustment(uint8_t adjustmentFunction, uint8_t position) #define RESET_FREQUENCY_2HZ (1000 / 2) -void processRcAdjustments(controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig) +void processRcAdjustments(controlRateConfig_t *controlRateConfig) { uint8_t adjustmentIndex; uint32_t now = millis(); @@ -682,9 +681,9 @@ void processRcAdjustments(controlRateConfig_t *controlRateConfig, const rxConfig if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) { int delta; - if (rcData[channelIndex] > rxConfig->midrc + 200) { + if (rcData[channelIndex] > rxConfig()->midrc + 200) { delta = adjustmentState->config->data.stepConfig.step; - } else if (rcData[channelIndex] < rxConfig->midrc - 200) { + } else if (rcData[channelIndex] < rxConfig()->midrc - 200) { delta = 0 - adjustmentState->config->data.stepConfig.step; } else { // returning the switch to the middle immediately resets the ready state @@ -728,9 +727,8 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) { return MIN(ABS(rcData[axis] - midrc), 500); } -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse) +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse) { - motorConfig = motorConfigToUse; pidProfile = pidProfileToUse; isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index bf6ac276b6..24024da319 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -178,6 +178,15 @@ typedef struct rcControlsConfig_s { PG_DECLARE(rcControlsConfig_t, rcControlsConfig); +typedef struct flight3DConfig_s { + uint16_t deadband3d_low; // min 3d value + uint16_t deadband3d_high; // max 3d value + uint16_t neutral3d; // center 3d value + uint16_t deadband3d_throttle; // default throttle deadband from MIDRC +} flight3DConfig_t; + +PG_DECLARE(flight3DConfig_t, flight3DConfig); + typedef struct armingConfig_s { uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value @@ -189,9 +198,8 @@ PG_DECLARE(armingConfig_t, armingConfig); bool areUsingSticksToArm(void); bool areSticksInApModePosition(uint16_t ap_mode); -struct rxConfig_s; -throttleStatus_e calculateThrottleStatus(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); -void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch); +throttleStatus_e calculateThrottleStatus(void); +void processRcStickPositions(throttleStatus_e throttleStatus); bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); @@ -287,7 +295,8 @@ typedef struct adjustmentProfile_s { bool isAirmodeActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); -void processRcAdjustments(controlRateConfig_t *controlRateConfig, const struct rxConfig_s *rxConfig); +struct rxConfig_s; +void processRcAdjustments(controlRateConfig_t *controlRateConfig); bool isUsingSticksForArming(void); @@ -295,4 +304,4 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId); struct pidProfile_s; struct motorConfig_s; -void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, const struct motorConfig_s *motorConfigToUse, struct pidProfile_s *pidProfileToUse); +void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, struct pidProfile_s *pidProfileToUse); diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 9689b24b69..d0157bd9eb 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -21,25 +21,26 @@ #include #include - #include "platform.h" + #include "build/debug.h" -#include "common/maths.h" #include "common/axis.h" +#include "common/maths.h" -#include "sensors/barometer.h" -#include "sensors/sonar.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" +#include "flight/pid.h" #include "rx/rx.h" -#include "fc/rc_controls.h" -#include "io/motors.h" - -#include "flight/pid.h" -#include "flight/imu.h" - -#include "fc/runtime_config.h" +#include "sensors/barometer.h" +#include "sensors/sonar.h" int32_t setVelocity = 0; @@ -50,22 +51,11 @@ int32_t AltHold; int32_t vario = 0; // variometer in cm/s -static barometerConfig_t *barometerConfig; static pidProfile_t *pidProfile; -static rcControlsConfig_t *rcControlsConfig; -static motorConfig_t *motorConfig; -void configureAltitudeHold( - pidProfile_t *initialPidProfile, - barometerConfig_t *intialBarometerConfig, - rcControlsConfig_t *initialRcControlsConfig, - motorConfig_t *initialMotorConfig -) +void configureAltitudeHold(pidProfile_t *initialPidProfile) { pidProfile = initialPidProfile; - barometerConfig = intialBarometerConfig; - rcControlsConfig = initialRcControlsConfig; - motorConfig = initialMotorConfig; } #if defined(BARO) || defined(SONAR) @@ -82,22 +72,22 @@ static void applyMultirotorAltHold(void) { static uint8_t isAltHoldChanged = 0; // multirotor alt hold - if (rcControlsConfig->alt_hold_fast_change) { + if (rcControlsConfig()->alt_hold_fast_change) { // rapid alt changes - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { errorVelocityI = 0; isAltHoldChanged = 1; - rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband; + rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband; } else { if (isAltHoldChanged) { AltHold = EstAlt; isAltHoldChanged = 0; } - rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig->minthrottle, motorConfig->maxthrottle); + rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig()->minthrottle, motorConfig()->maxthrottle); } } else { // slow alt changes, mostly used for aerial photography - if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) { + if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) { // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2; velocityControl = 1; @@ -107,23 +97,23 @@ static void applyMultirotorAltHold(void) velocityControl = 0; isAltHoldChanged = 0; } - rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig->minthrottle, motorConfig->maxthrottle); + rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig()->minthrottle, motorConfig()->maxthrottle); } } -static void applyFixedWingAltHold(airplaneConfig_t *airplaneConfig) +static void applyFixedWingAltHold(void) { // handle fixedwing-related althold. UNTESTED! and probably wrong // most likely need to check changes on pitch channel and 'reset' althold similar to // how throttle does it on multirotor - rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig->fixedwing_althold_dir; + rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig()->fixedwing_althold_dir; } -void applyAltHold(airplaneConfig_t *airplaneConfig) +void applyAltHold(void) { if (STATE(FIXED_WING)) { - applyFixedWingAltHold(airplaneConfig); + applyFixedWingAltHold(); } else { applyMultirotorAltHold(); } @@ -272,7 +262,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // Integrator - Altitude in cm accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2) - accAlt = accAlt * barometerConfig->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) + accAlt = accAlt * barometerConfig()->baro_cf_alt + (float)baro.BaroAlt * (1.0f - barometerConfig()->baro_cf_alt); // complementary filter for altitude estimation (baro & acc) vel += vel_acc; #ifdef DEBUG_ALT_HOLD @@ -308,7 +298,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs) // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay - vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel); + vel = vel * barometerConfig()->baro_cf_vel + baroVel * (1.0f - barometerConfig()->baro_cf_vel); vel_tmp = lrintf(vel); // set vario diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 3201f71b9a..8dceceb093 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -23,13 +23,9 @@ extern int32_t vario; void calculateEstimatedAltitude(timeUs_t currentTimeUs); struct pidProfile_s; -struct barometerConfig_s; -struct rcControlsConfig_s; -struct motorConfig_s; -void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct motorConfig_s *initialMotorConfig); +void configureAltitudeHold(struct pidProfile_s *initialPidProfile); -struct airplaneConfig_s; -void applyAltHold(struct airplaneConfig_s *airplaneConfig); +void applyAltHold(void); void updateAltHoldState(void); void updateSonarAltHoldState(void); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2ef72459b1..ba604c309c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -53,8 +53,6 @@ static failsafeState_t failsafeState; -static uint16_t deadband3dThrottle; // default throttle deadband from MIDRC - /* * Should called when the failsafe config needs to be changed - e.g. a different profile has been selected. */ @@ -71,9 +69,8 @@ void failsafeReset(void) failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN; } -void failsafeInit(uint16_t deadband3d_throttle) +void failsafeInit(void) { - deadband3dThrottle = deadband3d_throttle; failsafeState.events = 0; failsafeState.monitoring = false; @@ -180,7 +177,7 @@ void failsafeUpdateState(void) case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time - if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig(), deadband3dThrottle)) { + if (THROTTLE_HIGH == calculateThrottleStatus()) { failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 2feefb297d..d11d61421a 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -74,7 +74,7 @@ typedef struct failsafeState_s { failsafeRxLinkState_e rxLinkState; } failsafeState_t; -void failsafeInit(uint16_t deadband3d_throttle); +void failsafeInit(void); void failsafeReset(void); void failsafeStartMonitoring(void); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 3b4d91ffbe..0deec6c89d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -32,11 +32,6 @@ extern float accVelScale; extern int32_t accSum[XYZ_AXIS_COUNT]; -#define DEGREES_TO_DECIDEGREES(angle) (angle * 10) -#define DECIDEGREES_TO_DEGREES(angle) (angle / 10) -#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f) -#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f) - typedef union { int16_t raw[XYZ_AXIS_COUNT]; struct { diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 66baf8172b..86fd8f3d2c 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -104,15 +104,6 @@ typedef struct mixerConfig_s { PG_DECLARE(mixerConfig_t, mixerConfig); -typedef struct flight3DConfig_s { - uint16_t deadband3d_low; // min 3d value - uint16_t deadband3d_high; // max 3d value - uint16_t neutral3d; // center 3d value - uint16_t deadband3d_throttle; // default throttle deadband from MIDRC -} flight3DConfig_t; - -PG_DECLARE(flight3DConfig_t, flight3DConfig); - typedef struct motorConfig_s { 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 diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d88a937a5b..1e093cdfc1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -93,6 +93,8 @@ typedef struct pidConfig_s { uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate } pidConfig_t; +PG_DECLARE(pidConfig_t, pidConfig); + union rollAndPitchTrims_u; void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 02f53ffa93..e54a0a1ae3 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -22,29 +22,23 @@ #include "build/debug.h" -#include "common/maths.h" #include "common/filter.h" +#include "common/utils.h" +#include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" #include "drivers/adc.h" #include "drivers/system.h" -#include "fc/config.h" #include "fc/runtime_config.h" -#include "config/feature.h" +#include "io/beeper.h" #include "sensors/battery.h" #include "sensors/esc_sensor.h" -#include "fc/rc_controls.h" -#include "io/beeper.h" - -#include "rx/rx.h" - -#include "common/utils.h" #define VBAT_LPF_FREQ 0.4f #define IBAT_LPF_FREQ 0.4f @@ -64,7 +58,7 @@ uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) uint16_t vbatLatest = 0; // most recent unsmoothed value int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A) -int32_t amperageLatest = 0; // most recent value +int32_t amperageLatest = 0; // most recent value int32_t mAhDrawn = 0; // milliampere hours drawn from the battery since start @@ -266,9 +260,8 @@ void updateConsumptionWarning(void) } } -void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +void updateCurrentMeter(int32_t lastUpdateAt) { - (void)(rxConfig); if (getBatteryState() != BATTERY_NOT_PRESENT) { switch(batteryConfig()->currentMeterType) { case CURRENT_SENSOR_ADC: @@ -279,7 +272,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16 case CURRENT_SENSOR_VIRTUAL: amperageLatest = (int32_t)batteryConfig()->currentMeterOffset; if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle); + throttleStatus_e throttleStatus = calculateThrottleStatus(); int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) { throttleOffset = 0; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index a8e21145ff..91b59568ce 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -18,7 +18,6 @@ #pragma once #include "config/parameter_group.h" -#include "common/maths.h" // for fix12_t #ifndef VBAT_SCALE_DEFAULT #define VBAT_SCALE_DEFAULT 110 @@ -80,9 +79,6 @@ extern uint16_t batteryWarningVoltage; extern int32_t amperageLatest; extern int32_t amperage; extern int32_t mAhDrawn; -#ifndef USE_PARAMETER_GROUPS -extern const batteryConfig_t *batteryConfig; -#endif batteryState_e getBatteryState(void); const char * getBatteryStateString(void); @@ -90,7 +86,7 @@ void updateBattery(void); void batteryInit(void); struct rxConfig_s; -void updateCurrentMeter(int32_t lastUpdateAt, const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle); +void updateCurrentMeter(int32_t lastUpdateAt); int32_t currentMeterToCentiamps(uint16_t src); float calculateVbatPidCompensation(void); diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 4912bc15f8..550f69b97c 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -1065,7 +1065,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand) mac->range.startStep = bstRead8(); mac->range.endStep = bstRead8(); - useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile); + useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile); } else { ret = BST_FAILED; } diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index b764353caa..ef1877e82e 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -195,18 +195,15 @@ static void sendGpsAltitude(void) } #endif -static void sendThrottleOrBatterySizeAsRpm(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +static void sendThrottleOrBatterySizeAsRpm(void) { sendDataHead(ID_RPM); #ifdef USE_ESC_SENSOR - UNUSED(rxConfig); - UNUSED(deadband3d_throttle); - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0); #else if (ARMING_FLAG(ARMED)) { - throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + const throttleStatus_e throttleStatus = calculateThrottleStatus(); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; @@ -538,7 +535,7 @@ void handleFrSkyTelemetry(void) if ((cycleNum % 8) == 0) { // Sent every 1s sendTemperature1(); - sendThrottleOrBatterySizeAsRpm(rxConfig(), flight3DConfig()->deadband3d_throttle); + sendThrottleOrBatterySizeAsRpm(); if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) { sendVoltage();