Removed a number of static config pointers
This commit is contained in:
parent
80700c2158
commit
f6c8319361
|
@ -30,6 +30,10 @@
|
||||||
#define M_PIf 3.14159265358979323846f
|
#define M_PIf 3.14159265358979323846f
|
||||||
|
|
||||||
#define RAD (M_PIf / 180.0f)
|
#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 MIN(a, b) ((a) < (b) ? (a) : (b))
|
||||||
#define MAX(a, b) ((a) > (b) ? (a) : (b))
|
#define MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||||
|
|
|
@ -325,7 +325,5 @@ typedef struct master_s {
|
||||||
} master_t;
|
} master_t;
|
||||||
|
|
||||||
extern master_t masterConfig;
|
extern master_t masterConfig;
|
||||||
extern profile_t *currentProfile;
|
|
||||||
extern controlRateConfig_t *currentControlRateProfile;
|
|
||||||
|
|
||||||
void createDefaultConfig(master_t *config);
|
void createDefaultConfig(master_t *config);
|
||||||
|
|
|
@ -25,7 +25,7 @@
|
||||||
static IO_t leds[LED_NUMBER];
|
static IO_t leds[LED_NUMBER];
|
||||||
static uint8_t ledPolarity = 0;
|
static uint8_t ledPolarity = 0;
|
||||||
|
|
||||||
void ledInit(statusLedConfig_t *statusLedConfig)
|
void ledInit(const statusLedConfig_t *statusLedConfig)
|
||||||
{
|
{
|
||||||
LED0_OFF;
|
LED0_OFF;
|
||||||
LED1_OFF;
|
LED1_OFF;
|
||||||
|
|
|
@ -17,6 +17,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "config/parameter_group.h"
|
||||||
#include "drivers/io_types.h"
|
#include "drivers/io_types.h"
|
||||||
|
|
||||||
#define LED_NUMBER 3
|
#define LED_NUMBER 3
|
||||||
|
@ -26,6 +27,8 @@ typedef struct statusLedConfig_s {
|
||||||
uint8_t polarity;
|
uint8_t polarity;
|
||||||
} statusLedConfig_t;
|
} statusLedConfig_t;
|
||||||
|
|
||||||
|
PG_DECLARE(statusLedConfig_t, statusLedConfig);
|
||||||
|
|
||||||
// Helpful macros
|
// Helpful macros
|
||||||
#ifdef LED0
|
#ifdef LED0
|
||||||
# define LED0_TOGGLE ledToggle(0)
|
# define LED0_TOGGLE ledToggle(0)
|
||||||
|
@ -57,6 +60,6 @@ typedef struct statusLedConfig_s {
|
||||||
# define LED2_ON do {} while(0)
|
# define LED2_ON do {} while(0)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void ledInit(statusLedConfig_t *statusLedConfig);
|
void ledInit(const statusLedConfig_t *statusLedConfig);
|
||||||
void ledToggle(int led);
|
void ledToggle(int led);
|
||||||
void ledSet(int led, bool state);
|
void ledSet(int led, bool state);
|
||||||
|
|
|
@ -892,7 +892,6 @@ void activateConfig(void)
|
||||||
|
|
||||||
useRcControlsConfig(
|
useRcControlsConfig(
|
||||||
modeActivationProfile()->modeActivationConditions,
|
modeActivationProfile()->modeActivationConditions,
|
||||||
&masterConfig.motorConfig,
|
|
||||||
¤tProfile->pidProfile
|
¤tProfile->pidProfile
|
||||||
);
|
);
|
||||||
|
|
||||||
|
@ -918,12 +917,7 @@ void activateConfig(void)
|
||||||
throttleCorrectionConfig()->throttle_correction_angle
|
throttleCorrectionConfig()->throttle_correction_angle
|
||||||
);
|
);
|
||||||
|
|
||||||
configureAltitudeHold(
|
configureAltitudeHold(¤tProfile->pidProfile);
|
||||||
¤tProfile->pidProfile,
|
|
||||||
&masterConfig.barometerConfig,
|
|
||||||
&masterConfig.rcControlsConfig,
|
|
||||||
&masterConfig.motorConfig
|
|
||||||
);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void validateAndFixConfig(void)
|
void validateAndFixConfig(void)
|
||||||
|
|
|
@ -66,6 +66,10 @@ typedef struct systemConfig_s {
|
||||||
} systemConfig_t;
|
} systemConfig_t;
|
||||||
|
|
||||||
//!!TODOPG_DECLARE(systemConfig_t, systemConfig);
|
//!!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 beeperOffSet(uint32_t mask);
|
||||||
void beeperOffSetAll(uint8_t beeperCount);
|
void beeperOffSetAll(uint8_t beeperCount);
|
||||||
|
|
|
@ -24,10 +24,10 @@
|
||||||
|
|
||||||
#include "blackbox/blackbox.h"
|
#include "blackbox/blackbox.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/utils.h"
|
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
|
#include "common/maths.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
#include "config/config_profile.h"
|
#include "config/config_profile.h"
|
||||||
#include "config/feature.h"
|
#include "config/feature.h"
|
||||||
|
@ -72,6 +72,7 @@
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
#include "flight/altitudehold.h"
|
#include "flight/altitudehold.h"
|
||||||
|
#include "flight/imu.h"
|
||||||
|
|
||||||
|
|
||||||
// June 2013 V2.2-dev
|
// June 2013 V2.2-dev
|
||||||
|
@ -296,7 +297,7 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
failsafeUpdateState();
|
failsafeUpdateState();
|
||||||
}
|
}
|
||||||
|
|
||||||
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, flight3DConfig()->deadband3d_throttle);
|
const throttleStatus_e throttleStatus = calculateThrottleStatus();
|
||||||
|
|
||||||
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
|
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
|
||||||
if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
|
if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
|
||||||
|
@ -362,7 +363,7 @@ void processRx(timeUs_t currentTimeUs)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
processRcStickPositions(&masterConfig.rxConfig, throttleStatus, armingConfig()->disarm_kill_switch);
|
processRcStickPositions(rxConfig(), throttleStatus, armingConfig()->disarm_kill_switch);
|
||||||
|
|
||||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||||
updateInflightCalibrationState();
|
updateInflightCalibrationState();
|
||||||
|
@ -493,7 +494,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
|
||||||
updateRcCommands();
|
updateRcCommands();
|
||||||
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
|
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
|
||||||
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
|
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
|
||||||
applyAltHold(&masterConfig.airplaneConfig);
|
applyAltHold();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -443,7 +443,7 @@ void init(void)
|
||||||
cliInit(serialConfig());
|
cliInit(serialConfig());
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
failsafeInit(flight3DConfig()->deadband3d_throttle);
|
failsafeInit();
|
||||||
|
|
||||||
rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
|
rxInit(rxConfig(), modeActivationProfile()->modeActivationConditions);
|
||||||
|
|
||||||
|
|
|
@ -1353,7 +1353,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(modeActivationConditions(0), motorConfig(), ¤tProfile->pidProfile);
|
useRcControlsConfig(modeActivationConditions(0), ¤tProfile->pidProfile);
|
||||||
} else {
|
} else {
|
||||||
return MSP_RESULT_ERROR;
|
return MSP_RESULT_ERROR;
|
||||||
}
|
}
|
||||||
|
|
|
@ -131,7 +131,7 @@ static void taskUpdateBattery(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
|
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
|
||||||
ibatLastServiced = currentTimeUs;
|
ibatLastServiced = currentTimeUs;
|
||||||
updateCurrentMeter(ibatTimeSinceLastServiced, rxConfig(), flight3DConfig()->deadband3d_throttle);
|
updateCurrentMeter(ibatTimeSinceLastServiced);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -60,7 +60,6 @@
|
||||||
#include "flight/failsafe.h"
|
#include "flight/failsafe.h"
|
||||||
|
|
||||||
|
|
||||||
static const 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)
|
||||||
|
@ -116,13 +115,13 @@ bool areSticksInApModePosition(uint16_t ap_mode)
|
||||||
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < 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 (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;
|
return THROTTLE_LOW;
|
||||||
} else {
|
} else {
|
||||||
if (rcData[THROTTLE] < rxConfig->mincheck)
|
if (rcData[THROTTLE] < rxConfig()->mincheck)
|
||||||
return THROTTLE_LOW;
|
return THROTTLE_LOW;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -728,9 +727,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(const modeActivationCondition_t *modeActivationConditions, const motorConfig_t *motorConfigToUse, pidProfile_t *pidProfileToUse)
|
void useRcControlsConfig(const modeActivationCondition_t *modeActivationConditions, pidProfile_t *pidProfileToUse)
|
||||||
{
|
{
|
||||||
motorConfig = motorConfigToUse;
|
|
||||||
pidProfile = pidProfileToUse;
|
pidProfile = pidProfileToUse;
|
||||||
|
|
||||||
isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
|
isUsingSticksToArm = !isModeActivationConditionPresent(modeActivationConditions, BOXARM);
|
||||||
|
|
|
@ -177,6 +177,15 @@ typedef struct rcControlsConfig_s {
|
||||||
|
|
||||||
PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
|
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 {
|
typedef struct armingConfig_s {
|
||||||
uint8_t gyro_cal_on_first_arm; // allow disarm/arm on throttle down + roll left/right
|
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
|
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
|
||||||
|
@ -189,7 +198,7 @@ bool areUsingSticksToArm(void);
|
||||||
|
|
||||||
bool areSticksInApModePosition(uint16_t ap_mode);
|
bool areSticksInApModePosition(uint16_t ap_mode);
|
||||||
struct rxConfig_s;
|
struct rxConfig_s;
|
||||||
throttleStatus_e calculateThrottleStatus(const struct rxConfig_s *rxConfig, uint16_t deadband3d_throttle);
|
throttleStatus_e calculateThrottleStatus(void);
|
||||||
void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
|
void processRcStickPositions(const struct rxConfig_s *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch);
|
||||||
|
|
||||||
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
|
bool isRangeActive(uint8_t auxChannelIndex, channelRange_t *range);
|
||||||
|
@ -294,4 +303,4 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
|
||||||
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
|
bool isModeActivationConditionPresent(const modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
|
||||||
struct pidProfile_s;
|
struct pidProfile_s;
|
||||||
struct motorConfig_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);
|
||||||
|
|
|
@ -21,25 +21,26 @@
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
|
#include "common/maths.h"
|
||||||
|
|
||||||
#include "sensors/barometer.h"
|
#include "config/parameter_group.h"
|
||||||
#include "sensors/sonar.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 "rx/rx.h"
|
||||||
|
|
||||||
#include "fc/rc_controls.h"
|
#include "sensors/barometer.h"
|
||||||
#include "io/motors.h"
|
#include "sensors/sonar.h"
|
||||||
|
|
||||||
#include "flight/pid.h"
|
|
||||||
#include "flight/imu.h"
|
|
||||||
|
|
||||||
#include "fc/runtime_config.h"
|
|
||||||
|
|
||||||
|
|
||||||
int32_t setVelocity = 0;
|
int32_t setVelocity = 0;
|
||||||
|
@ -50,22 +51,11 @@ int32_t AltHold;
|
||||||
int32_t vario = 0; // variometer in cm/s
|
int32_t vario = 0; // variometer in cm/s
|
||||||
|
|
||||||
|
|
||||||
static barometerConfig_t *barometerConfig;
|
|
||||||
static pidProfile_t *pidProfile;
|
static pidProfile_t *pidProfile;
|
||||||
static rcControlsConfig_t *rcControlsConfig;
|
|
||||||
static motorConfig_t *motorConfig;
|
|
||||||
|
|
||||||
void configureAltitudeHold(
|
void configureAltitudeHold(pidProfile_t *initialPidProfile)
|
||||||
pidProfile_t *initialPidProfile,
|
|
||||||
barometerConfig_t *intialBarometerConfig,
|
|
||||||
rcControlsConfig_t *initialRcControlsConfig,
|
|
||||||
motorConfig_t *initialMotorConfig
|
|
||||||
)
|
|
||||||
{
|
{
|
||||||
pidProfile = initialPidProfile;
|
pidProfile = initialPidProfile;
|
||||||
barometerConfig = intialBarometerConfig;
|
|
||||||
rcControlsConfig = initialRcControlsConfig;
|
|
||||||
motorConfig = initialMotorConfig;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(BARO) || defined(SONAR)
|
#if defined(BARO) || defined(SONAR)
|
||||||
|
@ -82,22 +72,22 @@ static void applyMultirotorAltHold(void)
|
||||||
{
|
{
|
||||||
static uint8_t isAltHoldChanged = 0;
|
static uint8_t isAltHoldChanged = 0;
|
||||||
// multirotor alt hold
|
// multirotor alt hold
|
||||||
if (rcControlsConfig->alt_hold_fast_change) {
|
if (rcControlsConfig()->alt_hold_fast_change) {
|
||||||
// rapid alt changes
|
// rapid alt changes
|
||||||
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
|
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
|
||||||
errorVelocityI = 0;
|
errorVelocityI = 0;
|
||||||
isAltHoldChanged = 1;
|
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 {
|
} else {
|
||||||
if (isAltHoldChanged) {
|
if (isAltHoldChanged) {
|
||||||
AltHold = EstAlt;
|
AltHold = EstAlt;
|
||||||
isAltHoldChanged = 0;
|
isAltHoldChanged = 0;
|
||||||
}
|
}
|
||||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig->minthrottle, motorConfig->maxthrottle);
|
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, motorConfig()->minthrottle, motorConfig()->maxthrottle);
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// slow alt changes, mostly used for aerial photography
|
// 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
|
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
|
||||||
setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
|
setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
|
||||||
velocityControl = 1;
|
velocityControl = 1;
|
||||||
|
@ -107,23 +97,23 @@ static void applyMultirotorAltHold(void)
|
||||||
velocityControl = 0;
|
velocityControl = 0;
|
||||||
isAltHoldChanged = 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
|
// handle fixedwing-related althold. UNTESTED! and probably wrong
|
||||||
// most likely need to check changes on pitch channel and 'reset' althold similar to
|
// most likely need to check changes on pitch channel and 'reset' althold similar to
|
||||||
// how throttle does it on multirotor
|
// 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)) {
|
if (STATE(FIXED_WING)) {
|
||||||
applyFixedWingAltHold(airplaneConfig);
|
applyFixedWingAltHold();
|
||||||
} else {
|
} else {
|
||||||
applyMultirotorAltHold();
|
applyMultirotorAltHold();
|
||||||
}
|
}
|
||||||
|
@ -272,7 +262,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
||||||
|
|
||||||
// Integrator - Altitude in cm
|
// Integrator - Altitude in cm
|
||||||
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
|
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;
|
vel += vel_acc;
|
||||||
|
|
||||||
#ifdef DEBUG_ALT_HOLD
|
#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).
|
// 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
|
// 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);
|
vel_tmp = lrintf(vel);
|
||||||
|
|
||||||
// set vario
|
// set vario
|
||||||
|
|
|
@ -23,13 +23,9 @@ extern int32_t vario;
|
||||||
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
|
void calculateEstimatedAltitude(timeUs_t currentTimeUs);
|
||||||
|
|
||||||
struct pidProfile_s;
|
struct pidProfile_s;
|
||||||
struct barometerConfig_s;
|
void configureAltitudeHold(struct pidProfile_s *initialPidProfile);
|
||||||
struct rcControlsConfig_s;
|
|
||||||
struct motorConfig_s;
|
|
||||||
void configureAltitudeHold(struct pidProfile_s *initialPidProfile, struct barometerConfig_s *intialBarometerConfig, struct rcControlsConfig_s *initialRcControlsConfig, struct motorConfig_s *initialMotorConfig);
|
|
||||||
|
|
||||||
struct airplaneConfig_s;
|
void applyAltHold(void);
|
||||||
void applyAltHold(struct airplaneConfig_s *airplaneConfig);
|
|
||||||
void updateAltHoldState(void);
|
void updateAltHoldState(void);
|
||||||
void updateSonarAltHoldState(void);
|
void updateSonarAltHoldState(void);
|
||||||
|
|
||||||
|
|
|
@ -53,8 +53,6 @@
|
||||||
|
|
||||||
static failsafeState_t failsafeState;
|
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.
|
* 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;
|
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
|
||||||
}
|
}
|
||||||
|
|
||||||
void failsafeInit(uint16_t deadband3d_throttle)
|
void failsafeInit(void)
|
||||||
{
|
{
|
||||||
deadband3dThrottle = deadband3d_throttle;
|
|
||||||
failsafeState.events = 0;
|
failsafeState.events = 0;
|
||||||
failsafeState.monitoring = false;
|
failsafeState.monitoring = false;
|
||||||
|
|
||||||
|
@ -180,7 +177,7 @@ void failsafeUpdateState(void)
|
||||||
case FAILSAFE_IDLE:
|
case FAILSAFE_IDLE:
|
||||||
if (armed) {
|
if (armed) {
|
||||||
// Track throttle command below minimum time
|
// 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;
|
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)
|
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
|
||||||
|
|
|
@ -74,7 +74,7 @@ typedef struct failsafeState_s {
|
||||||
failsafeRxLinkState_e rxLinkState;
|
failsafeRxLinkState_e rxLinkState;
|
||||||
} failsafeState_t;
|
} failsafeState_t;
|
||||||
|
|
||||||
void failsafeInit(uint16_t deadband3d_throttle);
|
void failsafeInit(void);
|
||||||
void failsafeReset(void);
|
void failsafeReset(void);
|
||||||
|
|
||||||
void failsafeStartMonitoring(void);
|
void failsafeStartMonitoring(void);
|
||||||
|
|
|
@ -32,11 +32,6 @@ extern float accVelScale;
|
||||||
extern int32_t accSum[XYZ_AXIS_COUNT];
|
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 {
|
typedef union {
|
||||||
int16_t raw[XYZ_AXIS_COUNT];
|
int16_t raw[XYZ_AXIS_COUNT];
|
||||||
struct {
|
struct {
|
||||||
|
|
|
@ -104,15 +104,6 @@ typedef struct mixerConfig_s {
|
||||||
|
|
||||||
PG_DECLARE(mixerConfig_t, mixerConfig);
|
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 {
|
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 minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed.
|
||||||
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
|
uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000
|
||||||
|
|
|
@ -93,6 +93,8 @@ typedef struct pidConfig_s {
|
||||||
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
|
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
|
||||||
} pidConfig_t;
|
} pidConfig_t;
|
||||||
|
|
||||||
|
PG_DECLARE(pidConfig_t, pidConfig);
|
||||||
|
|
||||||
union rollAndPitchTrims_u;
|
union rollAndPitchTrims_u;
|
||||||
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
|
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
|
||||||
|
|
||||||
|
|
|
@ -22,29 +22,23 @@
|
||||||
|
|
||||||
#include "build/debug.h"
|
#include "build/debug.h"
|
||||||
|
|
||||||
#include "common/maths.h"
|
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
|
#include "config/feature.h"
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
#include "config/parameter_group_ids.h"
|
#include "config/parameter_group_ids.h"
|
||||||
|
|
||||||
#include "drivers/adc.h"
|
#include "drivers/adc.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
|
|
||||||
#include "fc/config.h"
|
|
||||||
#include "fc/runtime_config.h"
|
#include "fc/runtime_config.h"
|
||||||
|
|
||||||
#include "config/feature.h"
|
#include "io/beeper.h"
|
||||||
|
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
#include "sensors/esc_sensor.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 VBAT_LPF_FREQ 0.4f
|
||||||
#define IBAT_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
|
uint16_t vbatLatest = 0; // most recent unsmoothed value
|
||||||
|
|
||||||
int32_t amperage = 0; // amperage read by current sensor in centiampere (1/100th A)
|
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
|
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) {
|
if (getBatteryState() != BATTERY_NOT_PRESENT) {
|
||||||
switch(batteryConfig()->currentMeterType) {
|
switch(batteryConfig()->currentMeterType) {
|
||||||
case CURRENT_SENSOR_ADC:
|
case CURRENT_SENSOR_ADC:
|
||||||
|
@ -279,7 +272,7 @@ void updateCurrentMeter(int32_t lastUpdateAt, const rxConfig_t *rxConfig, uint16
|
||||||
case CURRENT_SENSOR_VIRTUAL:
|
case CURRENT_SENSOR_VIRTUAL:
|
||||||
amperageLatest = (int32_t)batteryConfig()->currentMeterOffset;
|
amperageLatest = (int32_t)batteryConfig()->currentMeterOffset;
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), deadband3d_throttle);
|
throttleStatus_e throttleStatus = calculateThrottleStatus();
|
||||||
int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
|
int throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
|
||||||
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
|
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) {
|
||||||
throttleOffset = 0;
|
throttleOffset = 0;
|
||||||
|
|
|
@ -18,7 +18,6 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
#include "common/maths.h" // for fix12_t
|
|
||||||
|
|
||||||
#ifndef VBAT_SCALE_DEFAULT
|
#ifndef VBAT_SCALE_DEFAULT
|
||||||
#define VBAT_SCALE_DEFAULT 110
|
#define VBAT_SCALE_DEFAULT 110
|
||||||
|
@ -80,9 +79,6 @@ extern uint16_t batteryWarningVoltage;
|
||||||
extern int32_t amperageLatest;
|
extern int32_t amperageLatest;
|
||||||
extern int32_t amperage;
|
extern int32_t amperage;
|
||||||
extern int32_t mAhDrawn;
|
extern int32_t mAhDrawn;
|
||||||
#ifndef USE_PARAMETER_GROUPS
|
|
||||||
extern const batteryConfig_t *batteryConfig;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
batteryState_e getBatteryState(void);
|
batteryState_e getBatteryState(void);
|
||||||
const char * getBatteryStateString(void);
|
const char * getBatteryStateString(void);
|
||||||
|
@ -90,7 +86,7 @@ void updateBattery(void);
|
||||||
void batteryInit(void);
|
void batteryInit(void);
|
||||||
|
|
||||||
struct rxConfig_s;
|
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);
|
int32_t currentMeterToCentiamps(uint16_t src);
|
||||||
|
|
||||||
float calculateVbatPidCompensation(void);
|
float calculateVbatPidCompensation(void);
|
||||||
|
|
|
@ -1065,7 +1065,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
||||||
mac->range.startStep = bstRead8();
|
mac->range.startStep = bstRead8();
|
||||||
mac->range.endStep = bstRead8();
|
mac->range.endStep = bstRead8();
|
||||||
|
|
||||||
useRcControlsConfig(modeActivationProfile()->modeActivationConditions, &masterConfig.motorConfig, ¤tProfile->pidProfile);
|
useRcControlsConfig(modeActivationProfile()->modeActivationConditions, ¤tProfile->pidProfile);
|
||||||
} else {
|
} else {
|
||||||
ret = BST_FAILED;
|
ret = BST_FAILED;
|
||||||
}
|
}
|
||||||
|
|
|
@ -195,18 +195,15 @@ static void sendGpsAltitude(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
static void sendThrottleOrBatterySizeAsRpm(const rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
|
static void sendThrottleOrBatterySizeAsRpm(void)
|
||||||
{
|
{
|
||||||
sendDataHead(ID_RPM);
|
sendDataHead(ID_RPM);
|
||||||
#ifdef USE_ESC_SENSOR
|
#ifdef USE_ESC_SENSOR
|
||||||
UNUSED(rxConfig);
|
|
||||||
UNUSED(deadband3d_throttle);
|
|
||||||
|
|
||||||
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
|
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
|
||||||
serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0);
|
serialize16(escData->dataAge < ESC_DATA_INVALID ? escData->rpm : 0);
|
||||||
#else
|
#else
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle);
|
const throttleStatus_e throttleStatus = calculateThrottleStatus();
|
||||||
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
|
uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
|
||||||
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
|
if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP))
|
||||||
throttleForRPM = 0;
|
throttleForRPM = 0;
|
||||||
|
@ -538,7 +535,7 @@ void handleFrSkyTelemetry(void)
|
||||||
|
|
||||||
if ((cycleNum % 8) == 0) { // Sent every 1s
|
if ((cycleNum % 8) == 0) { // Sent every 1s
|
||||||
sendTemperature1();
|
sendTemperature1();
|
||||||
sendThrottleOrBatterySizeAsRpm(rxConfig(), flight3DConfig()->deadband3d_throttle);
|
sendThrottleOrBatterySizeAsRpm();
|
||||||
|
|
||||||
if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) {
|
if ((feature(FEATURE_VBAT) || feature(FEATURE_ESC_SENSOR)) && batteryCellCount > 0) {
|
||||||
sendVoltage();
|
sendVoltage();
|
||||||
|
|
Loading…
Reference in New Issue