mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
Cleanup project structure. Update unit test Makefile to place object
files in obj/test
This commit is contained in:
Dominic Clifton
parent
fb9e3a2358
commit
d19a5e7046
330 changed files with 657 additions and 638 deletions
738
src/main/mw.c
Executable file
738
src/main/mw.c
Executable file
|
|
@ -0,0 +1,738 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/axis.h"
|
||||
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/light_ledring.h"
|
||||
#include "drivers/light_led.h"
|
||||
#include "drivers/gpio.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/serial.h"
|
||||
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "io/battery.h"
|
||||
#include "io/buzzer.h"
|
||||
#include "io/escservo.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/flight.h"
|
||||
#include "flight/autotune.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/gps.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "io/serial_cli.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/statusindicator.h"
|
||||
#include "io/rc_controls.h"
|
||||
#include "io/rc_curves.h"
|
||||
#include "rx/rx.h"
|
||||
#include "rx/msp.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
// June 2013 V2.2-dev
|
||||
|
||||
enum {
|
||||
ALIGN_GYRO = 0,
|
||||
ALIGN_ACCEL = 1,
|
||||
ALIGN_MAG = 2
|
||||
};
|
||||
|
||||
/* for VBAT monitoring frequency */
|
||||
#define VBATFREQ 6 // to read battery voltage - nth number of loop iterations
|
||||
|
||||
int16_t debug[4];
|
||||
uint32_t currentTime = 0;
|
||||
uint32_t previousTime = 0;
|
||||
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
|
||||
int16_t headFreeModeHold;
|
||||
|
||||
int16_t telemTemperature1; // gyro sensor temperature
|
||||
|
||||
extern uint8_t dynP8[3], dynI8[3], dynD8[3];
|
||||
extern failsafe_t *failsafe;
|
||||
|
||||
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
|
||||
|
||||
extern pidControllerFuncPtr pid_controller;
|
||||
|
||||
// Automatic ACC Offset Calibration
|
||||
bool AccInflightCalibrationArmed = false;
|
||||
bool AccInflightCalibrationMeasurementDone = false;
|
||||
bool AccInflightCalibrationSavetoEEProm = false;
|
||||
bool AccInflightCalibrationActive = false;
|
||||
uint16_t InflightcalibratingA = 0;
|
||||
|
||||
void updateAutotuneState(void)
|
||||
{
|
||||
static bool landedAfterAutoTuning = false;
|
||||
static bool autoTuneWasUsed = false;
|
||||
|
||||
if (rcOptions[BOXAUTOTUNE]) {
|
||||
if (!f.AUTOTUNE_MODE) {
|
||||
if (f.ARMED) {
|
||||
if (isAutotuneIdle() || landedAfterAutoTuning) {
|
||||
autotuneReset();
|
||||
landedAfterAutoTuning = false;
|
||||
}
|
||||
autotuneBeginNextPhase(¤tProfile.pidProfile, currentProfile.pidController);
|
||||
f.AUTOTUNE_MODE = 1;
|
||||
autoTuneWasUsed = true;
|
||||
} else {
|
||||
if (havePidsBeenUpdatedByAutotune()) {
|
||||
saveAndReloadCurrentProfileToCurrentProfileSlot();
|
||||
autotuneReset();
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (f.AUTOTUNE_MODE) {
|
||||
autotuneEndPhase();
|
||||
f.AUTOTUNE_MODE = 0;
|
||||
}
|
||||
|
||||
if (!f.ARMED && autoTuneWasUsed) {
|
||||
landedAfterAutoTuning = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool isCalibrating()
|
||||
{
|
||||
#ifdef BARO
|
||||
if (sensors(SENSOR_ACC) && !isBaroCalibrationComplete()) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
|
||||
|
||||
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
|
||||
}
|
||||
|
||||
static uint32_t warningLedTimer = 0;
|
||||
|
||||
void enableWarningLed(uint32_t currentTime)
|
||||
{
|
||||
if (warningLedTimer != 0) {
|
||||
return; // already enabled
|
||||
}
|
||||
warningLedTimer = currentTime + 500000;
|
||||
LED0_ON;
|
||||
}
|
||||
|
||||
void disableWarningLed(void)
|
||||
{
|
||||
warningLedTimer = 0;
|
||||
LED0_OFF;
|
||||
}
|
||||
|
||||
void updateWarningLed(uint32_t currentTime)
|
||||
{
|
||||
if (warningLedTimer && (int32_t)(currentTime - warningLedTimer) >= 0) {
|
||||
LED0_TOGGLE;
|
||||
warningLedTimer = warningLedTimer + 500000;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void annexCode(void)
|
||||
{
|
||||
int32_t tmp, tmp2;
|
||||
int32_t axis, prop1, prop2;
|
||||
|
||||
static uint8_t batteryWarningEnabled = false;
|
||||
static uint8_t vbatTimer = 0;
|
||||
static uint32_t vbatCycleTime = 0;
|
||||
|
||||
// PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value
|
||||
if (rcData[THROTTLE] < currentProfile.tpa_breakpoint) {
|
||||
prop2 = 100;
|
||||
} else {
|
||||
if (rcData[THROTTLE] < 2000) {
|
||||
prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpa_breakpoint) / (2000 - currentProfile.tpa_breakpoint);
|
||||
} else {
|
||||
prop2 = 100 - currentProfile.dynThrPID;
|
||||
}
|
||||
}
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
|
||||
if (axis == ROLL || axis == PITCH) {
|
||||
if (currentProfile.deadband) {
|
||||
if (tmp > currentProfile.deadband) {
|
||||
tmp -= currentProfile.deadband;
|
||||
} else {
|
||||
tmp = 0;
|
||||
}
|
||||
}
|
||||
|
||||
tmp2 = tmp / 100;
|
||||
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
|
||||
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
|
||||
prop1 = (uint16_t)prop1 * prop2 / 100;
|
||||
}
|
||||
if (axis == YAW) {
|
||||
if (currentProfile.yaw_deadband) {
|
||||
if (tmp > currentProfile.yaw_deadband) {
|
||||
tmp -= currentProfile.yaw_deadband;
|
||||
} else {
|
||||
tmp = 0;
|
||||
}
|
||||
}
|
||||
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
|
||||
prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500;
|
||||
}
|
||||
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
|
||||
dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100;
|
||||
dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100;
|
||||
dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[axis] * prop1 / 100;
|
||||
if (rcData[axis] < masterConfig.rxConfig.midrc)
|
||||
rcCommand[axis] = -rcCommand[axis];
|
||||
}
|
||||
|
||||
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
|
||||
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
|
||||
tmp2 = tmp / 100;
|
||||
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
|
||||
|
||||
if (f.HEADFREE_MODE) {
|
||||
float radDiff = degreesToRadians(heading - headFreeModeHold);
|
||||
float cosDiff = cosf(radDiff);
|
||||
float sinDiff = sinf(radDiff);
|
||||
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
|
||||
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
|
||||
rcCommand[PITCH] = rcCommand_PITCH;
|
||||
}
|
||||
|
||||
if (feature(FEATURE_VBAT || FEATURE_CURRENT_METER)) {
|
||||
vbatCycleTime += cycleTime;
|
||||
if (!(++vbatTimer % VBATFREQ)) {
|
||||
|
||||
if (feature(FEATURE_VBAT)) {
|
||||
updateBatteryVoltage();
|
||||
batteryWarningEnabled = shouldSoundBatteryAlarm();
|
||||
}
|
||||
|
||||
if (feature(FEATURE_CURRENT_METER)) {
|
||||
updateCurrentMeter(vbatCycleTime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
buzzer(batteryWarningEnabled); // external buzzer routine that handles buzzer events globally now
|
||||
|
||||
if (f.ARMED) {
|
||||
LED0_ON;
|
||||
} else {
|
||||
if (isCalibrating()) {
|
||||
LED0_TOGGLE;
|
||||
f.OK_TO_ARM = 0;
|
||||
}
|
||||
|
||||
f.OK_TO_ARM = 1;
|
||||
|
||||
if (!f.SMALL_ANGLE) {
|
||||
f.OK_TO_ARM = 0;
|
||||
}
|
||||
|
||||
if (rcOptions[BOXAUTOTUNE]) {
|
||||
f.OK_TO_ARM = 0;
|
||||
}
|
||||
|
||||
if (f.OK_TO_ARM) {
|
||||
disableWarningLed();
|
||||
} else {
|
||||
enableWarningLed(currentTime);
|
||||
}
|
||||
|
||||
updateWarningLed(currentTime);
|
||||
}
|
||||
|
||||
checkTelemetryState();
|
||||
|
||||
#ifdef LEDRING
|
||||
if (feature(FEATURE_LED_RING)) {
|
||||
static uint32_t LEDTime;
|
||||
if ((int32_t)(currentTime - LEDTime) >= 0) {
|
||||
LEDTime = currentTime + 50000;
|
||||
ledringState(f.ARMED, inclination.values.pitchDeciDegrees, inclination.values.rollDeciDegrees);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
handleSerial();
|
||||
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
updateGpsIndicator(currentTime);
|
||||
}
|
||||
|
||||
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
|
||||
if (gyro.temperature)
|
||||
gyro.temperature(&telemTemperature1);
|
||||
else {
|
||||
// TODO MCU temp
|
||||
}
|
||||
}
|
||||
|
||||
void mwDisarm(void)
|
||||
{
|
||||
if (f.ARMED)
|
||||
f.ARMED = 0;
|
||||
}
|
||||
|
||||
void mwArm(void)
|
||||
{
|
||||
if (f.OK_TO_ARM) {
|
||||
if (f.ARMED) {
|
||||
return;
|
||||
}
|
||||
if (!f.PREVENT_ARMING) {
|
||||
f.ARMED = 1;
|
||||
headFreeModeHold = heading;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (!f.ARMED) {
|
||||
blinkLedAndSoundBeeper(2, 255, 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void mwVario(void)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
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
|
||||
uint8_t stTmp = 0;
|
||||
int i;
|
||||
#ifdef BARO
|
||||
static int16_t initialThrottleHold;
|
||||
#endif
|
||||
static uint32_t loopTime;
|
||||
uint16_t auxState = 0;
|
||||
bool isThrottleLow = false;
|
||||
|
||||
updateRx();
|
||||
|
||||
if (shouldProcessRx(currentTime)) {
|
||||
calculateRxChannelsAndUpdateFailsafe(currentTime);
|
||||
|
||||
// in 3D mode, we need to be able to disarm by switch at any time
|
||||
if (feature(FEATURE_3D)) {
|
||||
if (!rcOptions[BOXARM])
|
||||
mwDisarm();
|
||||
}
|
||||
|
||||
updateRSSI(currentTime);
|
||||
|
||||
if (feature(FEATURE_FAILSAFE)) {
|
||||
|
||||
if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) {
|
||||
failsafe->vTable->enable();
|
||||
}
|
||||
|
||||
failsafe->vTable->updateState();
|
||||
}
|
||||
|
||||
// ------------------ STICKS COMMAND HANDLER --------------------
|
||||
// checking sticks positions
|
||||
for (i = 0; i < 4; i++) {
|
||||
stTmp >>= 2;
|
||||
if (rcData[i] > masterConfig.rxConfig.mincheck)
|
||||
stTmp |= 0x80; // check for MIN
|
||||
if (rcData[i] < masterConfig.rxConfig.maxcheck)
|
||||
stTmp |= 0x40; // check for MAX
|
||||
}
|
||||
if (stTmp == rcSticks) {
|
||||
if (rcDelayCommand < 250)
|
||||
rcDelayCommand++;
|
||||
} else
|
||||
rcDelayCommand = 0;
|
||||
rcSticks = stTmp;
|
||||
|
||||
// perform actions
|
||||
if (feature(FEATURE_3D) && (rcData[THROTTLE] > (masterConfig.rxConfig.midrc - masterConfig.flight3DConfig.deadband3d_throttle) && rcData[THROTTLE] < (masterConfig.rxConfig.midrc + masterConfig.flight3DConfig.deadband3d_throttle)))
|
||||
isThrottleLow = true;
|
||||
else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < masterConfig.rxConfig.mincheck))
|
||||
isThrottleLow = true;
|
||||
if (isThrottleLow) {
|
||||
resetErrorAngle();
|
||||
resetErrorGyro();
|
||||
if (currentProfile.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
|
||||
if (rcOptions[BOXARM] && f.OK_TO_ARM)
|
||||
mwArm();
|
||||
else if (f.ARMED)
|
||||
mwDisarm();
|
||||
}
|
||||
}
|
||||
|
||||
if (rcDelayCommand == 20) {
|
||||
if (f.ARMED) { // actions during armed
|
||||
// Disarm on throttle down + yaw
|
||||
if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
|
||||
mwDisarm();
|
||||
// Disarm on roll (only when retarded_arm is enabled)
|
||||
if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
|
||||
mwDisarm();
|
||||
} else { // actions during not armed
|
||||
i = 0;
|
||||
// GYRO calibration
|
||||
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
|
||||
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||||
if (feature(FEATURE_GPS))
|
||||
GPS_reset_home_position();
|
||||
#ifdef BARO
|
||||
if (sensors(SENSOR_BARO))
|
||||
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
|
||||
#endif
|
||||
if (!sensors(SENSOR_MAG))
|
||||
heading = 0; // reset heading to zero after gyro calibration
|
||||
// Inflight ACC Calibration
|
||||
} else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
|
||||
if (AccInflightCalibrationMeasurementDone) { // trigger saving into eeprom after landing
|
||||
AccInflightCalibrationMeasurementDone = false;
|
||||
AccInflightCalibrationSavetoEEProm = true;
|
||||
} else {
|
||||
AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
|
||||
if (AccInflightCalibrationArmed) {
|
||||
queueConfirmationBeep(2);
|
||||
} else {
|
||||
queueConfirmationBeep(3);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Multiple configuration profiles
|
||||
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
|
||||
i = 1;
|
||||
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
|
||||
i = 2;
|
||||
else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI) // ROLL right -> Profile 3
|
||||
i = 3;
|
||||
if (i) {
|
||||
masterConfig.current_profile_index = i - 1;
|
||||
writeEEPROM();
|
||||
readEEPROM();
|
||||
blinkLedAndSoundBeeper(2, 40, i);
|
||||
// TODO alarmArray[0] = i;
|
||||
}
|
||||
|
||||
// Arm via YAW
|
||||
if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
|
||||
mwArm();
|
||||
// Arm via ROLL
|
||||
else if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
|
||||
mwArm();
|
||||
// Calibrating Acc
|
||||
else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
|
||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||
// Calibrating Mag
|
||||
else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
|
||||
f.CALIBRATE_MAG = 1;
|
||||
i = 0;
|
||||
// Acc Trim
|
||||
if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
|
||||
currentProfile.accelerometerTrims.values.pitch += 2;
|
||||
i = 1;
|
||||
} else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
|
||||
currentProfile.accelerometerTrims.values.pitch -= 2;
|
||||
i = 1;
|
||||
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
|
||||
currentProfile.accelerometerTrims.values.roll += 2;
|
||||
i = 1;
|
||||
} else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
|
||||
currentProfile.accelerometerTrims.values.roll -= 2;
|
||||
i = 1;
|
||||
}
|
||||
if (i) {
|
||||
copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
|
||||
writeEEPROM();
|
||||
readEEPROMAndNotify();
|
||||
rcDelayCommand = 0; // allow autorepetition
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
|
||||
if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) { // Copter is airborne and you are turning it off via boxarm : start measurement
|
||||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationArmed = false;
|
||||
}
|
||||
if (rcOptions[BOXCALIB]) { // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
|
||||
if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
|
||||
InflightcalibratingA = 50;
|
||||
AccInflightCalibrationActive = true;
|
||||
} else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
|
||||
AccInflightCalibrationMeasurementDone = false;
|
||||
AccInflightCalibrationSavetoEEProm = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Check AUX switches
|
||||
|
||||
// auxState is a bitmask, 3 bits per channel. aux1 is first.
|
||||
//
|
||||
// the three bits are as follows:
|
||||
// bit 1 is SET when the stick is less than 1300
|
||||
// bit 2 is SET when the stick is between 1300 and 1700
|
||||
// bit 3 is SET when the stick is above 1700
|
||||
// if the value is 1300 or 1700 NONE of the three bits are set.
|
||||
|
||||
for (i = 0; i < 4; i++)
|
||||
auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
|
||||
for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
|
||||
rcOptions[i] = (auxState & currentProfile.activate[i]) > 0;
|
||||
|
||||
if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
|
||||
// bumpless transfer to Level mode
|
||||
if (!f.ANGLE_MODE) {
|
||||
resetErrorAngle();
|
||||
f.ANGLE_MODE = 1;
|
||||
}
|
||||
} else {
|
||||
f.ANGLE_MODE = 0; // failsafe support
|
||||
}
|
||||
|
||||
if (rcOptions[BOXHORIZON]) {
|
||||
f.ANGLE_MODE = 0;
|
||||
if (!f.HORIZON_MODE) {
|
||||
resetErrorAngle();
|
||||
f.HORIZON_MODE = 1;
|
||||
}
|
||||
} else {
|
||||
f.HORIZON_MODE = 0;
|
||||
}
|
||||
|
||||
if (f.ANGLE_MODE || f.HORIZON_MODE) {
|
||||
LED1_ON;
|
||||
} else {
|
||||
LED1_OFF;
|
||||
}
|
||||
|
||||
#ifdef BARO
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
// Baro alt hold activate
|
||||
if (rcOptions[BOXBARO]) {
|
||||
if (!f.BARO_MODE) {
|
||||
f.BARO_MODE = 1;
|
||||
AltHold = EstAlt;
|
||||
initialThrottleHold = rcCommand[THROTTLE];
|
||||
errorAltitudeI = 0;
|
||||
BaroPID = 0;
|
||||
}
|
||||
} else {
|
||||
f.BARO_MODE = 0;
|
||||
}
|
||||
// Vario signalling activate
|
||||
if (feature(FEATURE_VARIO)) {
|
||||
if (rcOptions[BOXVARIO]) {
|
||||
if (!f.VARIO_MODE) {
|
||||
f.VARIO_MODE = 1;
|
||||
}
|
||||
} else {
|
||||
f.VARIO_MODE = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef MAG
|
||||
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
|
||||
if (rcOptions[BOXMAG]) {
|
||||
if (!f.MAG_MODE) {
|
||||
f.MAG_MODE = 1;
|
||||
magHold = heading;
|
||||
}
|
||||
} else {
|
||||
f.MAG_MODE = 0;
|
||||
}
|
||||
if (rcOptions[BOXHEADFREE]) {
|
||||
if (!f.HEADFREE_MODE) {
|
||||
f.HEADFREE_MODE = 1;
|
||||
}
|
||||
} else {
|
||||
f.HEADFREE_MODE = 0;
|
||||
}
|
||||
if (rcOptions[BOXHEADADJ]) {
|
||||
headFreeModeHold = heading; // acquire new heading
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
updateGpsWaypointsAndMode();
|
||||
}
|
||||
|
||||
if (rcOptions[BOXPASSTHRU]) {
|
||||
f.PASSTHRU_MODE = 1;
|
||||
} else {
|
||||
f.PASSTHRU_MODE = 0;
|
||||
}
|
||||
|
||||
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
|
||||
f.HEADFREE_MODE = 0;
|
||||
}
|
||||
} else { // not in rc loop
|
||||
static int taskOrder = 0; // never call all function in the same loop, to avoid high delay spikes
|
||||
switch (taskOrder) {
|
||||
case 0:
|
||||
taskOrder++;
|
||||
#ifdef MAG
|
||||
if (sensors(SENSOR_MAG) && compassGetADC(&masterConfig.magZero))
|
||||
break;
|
||||
#endif
|
||||
case 1:
|
||||
taskOrder++;
|
||||
#ifdef BARO
|
||||
if (sensors(SENSOR_BARO) && baroUpdate(currentTime) != BAROMETER_ACTION_NOT_READY)
|
||||
break;
|
||||
#endif
|
||||
case 2:
|
||||
taskOrder++;
|
||||
#ifdef BARO
|
||||
if (sensors(SENSOR_BARO) && getEstimatedAltitude())
|
||||
break;
|
||||
#endif
|
||||
case 3:
|
||||
// if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
|
||||
// hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
|
||||
// change this based on available hardware
|
||||
taskOrder++;
|
||||
if (feature(FEATURE_GPS)) {
|
||||
gpsThread();
|
||||
break;
|
||||
}
|
||||
case 4:
|
||||
taskOrder = 0;
|
||||
#ifdef SONAR
|
||||
if (sensors(SENSOR_SONAR)) {
|
||||
Sonar_update();
|
||||
}
|
||||
#endif
|
||||
if (feature(FEATURE_VARIO) && f.VARIO_MODE)
|
||||
mwVario();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
currentTime = micros();
|
||||
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
|
||||
loopTime = currentTime + masterConfig.looptime;
|
||||
|
||||
computeIMU();
|
||||
annexCode();
|
||||
// Measure loop rate just afer reading the sensors
|
||||
currentTime = micros();
|
||||
cycleTime = (int32_t)(currentTime - previousTime);
|
||||
previousTime = currentTime;
|
||||
|
||||
updateAutotuneState();
|
||||
|
||||
#ifdef MAG
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
|
||||
int16_t dif = heading - magHold;
|
||||
if (dif <= -180)
|
||||
dif += 360;
|
||||
if (dif >= +180)
|
||||
dif -= 360;
|
||||
dif *= -masterConfig.yaw_control_direction;
|
||||
if (f.SMALL_ANGLE)
|
||||
rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30; // 18 deg
|
||||
} else
|
||||
magHold = heading;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef BARO
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
if (f.BARO_MODE) {
|
||||
static uint8_t isAltHoldChanged = 0;
|
||||
static int16_t AltHoldCorr = 0;
|
||||
if (!f.FIXED_WING) {
|
||||
// multirotor alt hold
|
||||
if (currentProfile.alt_hold_fast_change) {
|
||||
// rapid alt changes
|
||||
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) {
|
||||
errorAltitudeI = 0;
|
||||
isAltHoldChanged = 1;
|
||||
rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_throttle_neutral : currentProfile.alt_hold_throttle_neutral;
|
||||
} else {
|
||||
if (isAltHoldChanged) {
|
||||
AltHold = EstAlt;
|
||||
isAltHoldChanged = 0;
|
||||
}
|
||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
|
||||
}
|
||||
} else {
|
||||
// slow alt changes for apfags
|
||||
if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) {
|
||||
// Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
|
||||
AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
|
||||
AltHold += AltHoldCorr / 2000;
|
||||
AltHoldCorr %= 2000;
|
||||
isAltHoldChanged = 1;
|
||||
} else if (isAltHoldChanged) {
|
||||
AltHold = EstAlt;
|
||||
AltHoldCorr = 0;
|
||||
isAltHoldChanged = 0;
|
||||
}
|
||||
rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
|
||||
}
|
||||
} else {
|
||||
// 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] += BaroPID * masterConfig.fixedwing_althold_dir;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
|
||||
rcCommand[THROTTLE] += throttleAngleCorrection;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
|
||||
updateGpsStateForHomeAndHoldMode();
|
||||
}
|
||||
}
|
||||
|
||||
// PID - note this is function pointer set by setPIDController()
|
||||
pid_controller(¤tProfile.pidProfile, ¤tProfile.controlRateConfig, masterConfig.max_angle_inclination, ¤tProfile.accelerometerTrims);
|
||||
|
||||
mixTable();
|
||||
writeServos();
|
||||
writeMotors();
|
||||
}
|
||||
|
||||
if (!cliMode && feature(FEATURE_TELEMETRY)) {
|
||||
handleTelemetry();
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue