mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 01:05:21 +03:00
Fix bug with non-working switch-ARM if using mode_range_operator=AND
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
32b13fdad6
commit
e1865ef699
2 changed files with 19 additions and 768 deletions
|
|
@ -1,754 +0,0 @@
|
||||||
/*
|
|
||||||
* 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 <stdbool.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
|
||||||
|
|
||||||
#include "blackbox/blackbox.h"
|
|
||||||
|
|
||||||
#include "build/debug.h"
|
|
||||||
|
|
||||||
#include "common/maths.h"
|
|
||||||
#include "common/axis.h"
|
|
||||||
#include "common/color.h"
|
|
||||||
#include "common/utils.h"
|
|
||||||
#include "common/filter.h"
|
|
||||||
|
|
||||||
#include "drivers/light_led.h"
|
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/serial.h"
|
|
||||||
#include "drivers/time.h"
|
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
|
||||||
#include "sensors/diagnostics.h"
|
|
||||||
#include "sensors/boardalignment.h"
|
|
||||||
#include "sensors/acceleration.h"
|
|
||||||
#include "sensors/barometer.h"
|
|
||||||
#include "sensors/pitotmeter.h"
|
|
||||||
#include "sensors/gyro.h"
|
|
||||||
#include "sensors/battery.h"
|
|
||||||
|
|
||||||
#include "fc/fc_core.h"
|
|
||||||
#include "fc/cli.h"
|
|
||||||
#include "fc/config.h"
|
|
||||||
#include "fc/controlrate_profile.h"
|
|
||||||
#include "fc/rc_adjustments.h"
|
|
||||||
#include "fc/rc_controls.h"
|
|
||||||
#include "fc/rc_curves.h"
|
|
||||||
#include "fc/runtime_config.h"
|
|
||||||
|
|
||||||
#include "io/beeper.h"
|
|
||||||
#include "io/dashboard.h"
|
|
||||||
#include "io/gimbal.h"
|
|
||||||
#include "io/gps.h"
|
|
||||||
#include "io/serial.h"
|
|
||||||
#include "io/statusindicator.h"
|
|
||||||
#include "io/asyncfatfs/asyncfatfs.h"
|
|
||||||
|
|
||||||
#include "msp/msp_serial.h"
|
|
||||||
|
|
||||||
#include "navigation/navigation.h"
|
|
||||||
|
|
||||||
#include "rx/rx.h"
|
|
||||||
#include "rx/msp.h"
|
|
||||||
|
|
||||||
#include "scheduler/scheduler.h"
|
|
||||||
|
|
||||||
#include "telemetry/telemetry.h"
|
|
||||||
|
|
||||||
#include "flight/mixer.h"
|
|
||||||
#include "flight/servos.h"
|
|
||||||
#include "flight/pid.h"
|
|
||||||
#include "flight/imu.h"
|
|
||||||
|
|
||||||
#include "flight/failsafe.h"
|
|
||||||
|
|
||||||
#include "config/feature.h"
|
|
||||||
|
|
||||||
// June 2013 V2.2-dev
|
|
||||||
|
|
||||||
enum {
|
|
||||||
ALIGN_GYRO = 0,
|
|
||||||
ALIGN_ACCEL = 1,
|
|
||||||
ALIGN_MAG = 2
|
|
||||||
};
|
|
||||||
|
|
||||||
#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro
|
|
||||||
|
|
||||||
timeDelta_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
|
|
||||||
|
|
||||||
float dT;
|
|
||||||
|
|
||||||
int16_t headFreeModeHold;
|
|
||||||
|
|
||||||
uint8_t motorControlEnable = false;
|
|
||||||
|
|
||||||
int16_t telemTemperature1; // gyro sensor temperature
|
|
||||||
static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
|
|
||||||
|
|
||||||
static bool isRXDataNew;
|
|
||||||
static disarmReason_t lastDisarmReason = DISARM_NONE;
|
|
||||||
|
|
||||||
bool isCalibrating(void)
|
|
||||||
{
|
|
||||||
#ifdef BARO
|
|
||||||
if (sensors(SENSOR_BARO) && !baroIsCalibrationComplete()) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef PITOT
|
|
||||||
if (sensors(SENSOR_PITOT) && !pitotIsCalibrationComplete()) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef NAV
|
|
||||||
if (!navIsCalibrationComplete()) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!gyroIsCalibrationComplete()) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
|
|
||||||
{
|
|
||||||
int16_t stickDeflection;
|
|
||||||
|
|
||||||
stickDeflection = constrain(rawData - rxConfig()->midrc, -500, 500);
|
|
||||||
stickDeflection = applyDeadband(stickDeflection, deadband);
|
|
||||||
|
|
||||||
return rcLookup(stickDeflection, rate);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void updatePreArmingChecks(void)
|
|
||||||
{
|
|
||||||
DISABLE_ARMING_FLAG(BLOCKED_ALL_FLAGS);
|
|
||||||
|
|
||||||
if (!STATE(SMALL_ANGLE)) {
|
|
||||||
ENABLE_ARMING_FLAG(BLOCKED_UAV_NOT_LEVEL);
|
|
||||||
DISABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (isCalibrating()) {
|
|
||||||
ENABLE_ARMING_FLAG(BLOCKED_SENSORS_CALIBRATING);
|
|
||||||
DISABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (isSystemOverloaded()) {
|
|
||||||
ENABLE_ARMING_FLAG(BLOCKED_SYSTEM_OVERLOADED);
|
|
||||||
DISABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(NAV)
|
|
||||||
if (navigationBlockArming()) {
|
|
||||||
ENABLE_ARMING_FLAG(BLOCKED_NAVIGATION_SAFETY);
|
|
||||||
DISABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#if defined(MAG)
|
|
||||||
if (sensors(SENSOR_MAG) && !STATE(COMPASS_CALIBRATED)) {
|
|
||||||
ENABLE_ARMING_FLAG(BLOCKED_COMPASS_NOT_CALIBRATED);
|
|
||||||
DISABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (sensors(SENSOR_ACC) && !STATE(ACCELEROMETER_CALIBRATED)) {
|
|
||||||
ENABLE_ARMING_FLAG(BLOCKED_ACCELEROMETER_NOT_CALIBRATED);
|
|
||||||
DISABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!isHardwareHealthy()) {
|
|
||||||
ENABLE_ARMING_FLAG(BLOCKED_HARDWARE_FAILURE);
|
|
||||||
DISABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void annexCode(void)
|
|
||||||
{
|
|
||||||
int32_t throttleValue;
|
|
||||||
|
|
||||||
if (failsafeShouldApplyControlInput()) {
|
|
||||||
// Failsafe will apply rcCommand for us
|
|
||||||
failsafeApplyControlInput();
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
// Compute ROLL PITCH and YAW command
|
|
||||||
rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
|
|
||||||
rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, rcControlsConfig()->deadband);
|
|
||||||
rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, rcControlsConfig()->yaw_deadband);
|
|
||||||
|
|
||||||
//Compute THROTTLE command
|
|
||||||
throttleValue = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX);
|
|
||||||
throttleValue = (uint32_t)(throttleValue - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); // [MINCHECK;2000] -> [0;1000]
|
|
||||||
rcCommand[THROTTLE] = rcLookupThrottle(throttleValue);
|
|
||||||
|
|
||||||
// Signal updated rcCommand values to Failsafe system
|
|
||||||
failsafeUpdateRcCommandValues();
|
|
||||||
|
|
||||||
if (FLIGHT_MODE(HEADFREE_MODE)) {
|
|
||||||
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
|
|
||||||
const float cosDiff = cos_approx(radDiff);
|
|
||||||
const float sinDiff = sin_approx(radDiff);
|
|
||||||
const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
|
|
||||||
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
|
|
||||||
rcCommand[PITCH] = rcCommand_PITCH;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ARMING_FLAG(ARMED)) {
|
|
||||||
LED0_ON;
|
|
||||||
} else {
|
|
||||||
if (!IS_RC_MODE_ACTIVE(BOXARM) && failsafeIsReceivingRxData()) {
|
|
||||||
ENABLE_ARMING_FLAG(OK_TO_ARM);
|
|
||||||
}
|
|
||||||
|
|
||||||
updatePreArmingChecks();
|
|
||||||
|
|
||||||
if (ARMING_FLAG(OK_TO_ARM)) {
|
|
||||||
warningLedDisable();
|
|
||||||
} else {
|
|
||||||
warningLedFlash();
|
|
||||||
}
|
|
||||||
|
|
||||||
warningLedUpdate();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void mwDisarm(disarmReason_t disarmReason)
|
|
||||||
{
|
|
||||||
if (ARMING_FLAG(ARMED)) {
|
|
||||||
lastDisarmReason = disarmReason;
|
|
||||||
DISABLE_ARMING_FLAG(ARMED);
|
|
||||||
|
|
||||||
#ifdef BLACKBOX
|
|
||||||
if (feature(FEATURE_BLACKBOX)) {
|
|
||||||
blackboxFinish();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
beeper(BEEPER_DISARMING); // emit disarm tone
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
disarmReason_t getDisarmReason(void)
|
|
||||||
{
|
|
||||||
return lastDisarmReason;
|
|
||||||
}
|
|
||||||
|
|
||||||
#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
|
|
||||||
|
|
||||||
void releaseSharedTelemetryPorts(void) {
|
|
||||||
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
|
|
||||||
while (sharedPort) {
|
|
||||||
mspSerialReleasePortIfAllocated(sharedPort);
|
|
||||||
sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void mwArm(void)
|
|
||||||
{
|
|
||||||
if (ARMING_FLAG(OK_TO_ARM)) {
|
|
||||||
if (ARMING_FLAG(ARMED)) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if (!ARMING_FLAG(PREVENT_ARMING)) {
|
|
||||||
ENABLE_ARMING_FLAG(ARMED);
|
|
||||||
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
|
|
||||||
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
|
|
||||||
|
|
||||||
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
|
|
||||||
|
|
||||||
#ifdef BLACKBOX
|
|
||||||
if (feature(FEATURE_BLACKBOX)) {
|
|
||||||
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
|
|
||||||
if (sharedBlackboxAndMspPort) {
|
|
||||||
mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
|
|
||||||
}
|
|
||||||
blackboxStart();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
|
|
||||||
|
|
||||||
//beep to indicate arming
|
|
||||||
#ifdef NAV
|
|
||||||
if (navigationPositionEstimateIsHealthy())
|
|
||||||
beeper(BEEPER_ARMING_GPS_FIX);
|
|
||||||
else
|
|
||||||
beeper(BEEPER_ARMING);
|
|
||||||
#else
|
|
||||||
beeper(BEEPER_ARMING);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!ARMING_FLAG(ARMED)) {
|
|
||||||
beeperConfirmationBeeps(1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void processRx(timeUs_t currentTimeUs)
|
|
||||||
{
|
|
||||||
static bool armedBeeperOn = false;
|
|
||||||
|
|
||||||
calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
|
|
||||||
|
|
||||||
// in 3D mode, we need to be able to disarm by switch at any time
|
|
||||||
if (feature(FEATURE_3D)) {
|
|
||||||
if (!IS_RC_MODE_ACTIVE(BOXARM))
|
|
||||||
mwDisarm(DISARM_SWITCH_3D);
|
|
||||||
}
|
|
||||||
|
|
||||||
updateRSSI(currentTimeUs);
|
|
||||||
|
|
||||||
// Update failsafe monitoring system
|
|
||||||
if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
|
|
||||||
failsafeStartMonitoring();
|
|
||||||
}
|
|
||||||
|
|
||||||
failsafeUpdateState();
|
|
||||||
|
|
||||||
const throttleStatus_e throttleStatus = calculateThrottleStatus();
|
|
||||||
|
|
||||||
// When armed and motors aren't spinning, do beeps and then disarm
|
|
||||||
// board after delay so users without buzzer won't lose fingers.
|
|
||||||
// mixTable constrains motor commands, so checking throttleStatus is enough
|
|
||||||
if (ARMING_FLAG(ARMED)
|
|
||||||
&& feature(FEATURE_MOTOR_STOP)
|
|
||||||
&& !STATE(FIXED_WING)
|
|
||||||
) {
|
|
||||||
if (isUsingSticksForArming()) {
|
|
||||||
if (throttleStatus == THROTTLE_LOW) {
|
|
||||||
if (armingConfig()->auto_disarm_delay != 0
|
|
||||||
&& (int32_t)(disarmAt - millis()) < 0
|
|
||||||
) {
|
|
||||||
// auto-disarm configured and delay is over
|
|
||||||
mwDisarm(DISARM_TIMEOUT);
|
|
||||||
armedBeeperOn = false;
|
|
||||||
} else {
|
|
||||||
// still armed; do warning beeps while armed
|
|
||||||
beeper(BEEPER_ARMED);
|
|
||||||
armedBeeperOn = true;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// throttle is not low
|
|
||||||
if (armingConfig()->auto_disarm_delay != 0) {
|
|
||||||
// extend disarm time
|
|
||||||
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (armedBeeperOn) {
|
|
||||||
beeperSilence();
|
|
||||||
armedBeeperOn = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
// arming is via AUX switch; beep while throttle low
|
|
||||||
if (throttleStatus == THROTTLE_LOW) {
|
|
||||||
beeper(BEEPER_ARMED);
|
|
||||||
armedBeeperOn = true;
|
|
||||||
} else if (armedBeeperOn) {
|
|
||||||
beeperSilence();
|
|
||||||
armedBeeperOn = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
processRcStickPositions(throttleStatus, armingConfig()->disarm_kill_switch, armingConfig()->fixed_wing_auto_arm);
|
|
||||||
|
|
||||||
updateActivatedModes();
|
|
||||||
|
|
||||||
if (!cliMode) {
|
|
||||||
updateAdjustmentStates();
|
|
||||||
processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile));
|
|
||||||
}
|
|
||||||
|
|
||||||
bool canUseHorizonMode = true;
|
|
||||||
|
|
||||||
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
|
|
||||||
// bumpless transfer to Level mode
|
|
||||||
canUseHorizonMode = false;
|
|
||||||
|
|
||||||
if (!FLIGHT_MODE(ANGLE_MODE)) {
|
|
||||||
ENABLE_FLIGHT_MODE(ANGLE_MODE);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
|
|
||||||
}
|
|
||||||
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
|
|
||||||
|
|
||||||
DISABLE_FLIGHT_MODE(ANGLE_MODE);
|
|
||||||
|
|
||||||
if (!FLIGHT_MODE(HORIZON_MODE)) {
|
|
||||||
ENABLE_FLIGHT_MODE(HORIZON_MODE);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(HORIZON_MODE);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
|
|
||||||
LED1_ON;
|
|
||||||
} else {
|
|
||||||
LED1_OFF;
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
/* Flaperon mode */
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
|
|
||||||
if (!FLIGHT_MODE(FLAPERON)) {
|
|
||||||
ENABLE_FLIGHT_MODE(FLAPERON);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(FLAPERON);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef USE_FLM_TURN_ASSIST
|
|
||||||
/* Turn assistant mode */
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
|
|
||||||
if (!FLIGHT_MODE(TURN_ASSISTANT)) {
|
|
||||||
ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (sensors(SENSOR_ACC)) {
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXHEADINGHOLD)) {
|
|
||||||
if (!FLIGHT_MODE(HEADING_MODE)) {
|
|
||||||
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
|
|
||||||
ENABLE_FLIGHT_MODE(HEADING_MODE);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(HEADING_MODE);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(MAG)
|
|
||||||
if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
|
|
||||||
if (!FLIGHT_MODE(HEADFREE_MODE)) {
|
|
||||||
ENABLE_FLIGHT_MODE(HEADFREE_MODE);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
|
||||||
}
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
|
|
||||||
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
<<<<<<< HEAD
|
|
||||||
// Handle passthrough mode
|
|
||||||
if (STATE(FIXED_WING)) {
|
|
||||||
if ((IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !naivationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough
|
|
||||||
(!ARMING_FLAG(ARMED) && isCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating
|
|
||||||
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
|
|
||||||
}
|
|
||||||
=======
|
|
||||||
// Navigation may override PASSTHRU_MODE
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) {
|
|
||||||
ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
|
|
||||||
} else {
|
|
||||||
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
|
|
||||||
>>>>>>> naivation to navigation typo fix
|
|
||||||
}
|
|
||||||
|
|
||||||
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
|
|
||||||
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
|
|
||||||
Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
|
|
||||||
if (FLIGHT_MODE(PASSTHRU_MODE) || !ARMING_FLAG(ARMED)) {
|
|
||||||
/* In PASSTHRU mode we reset integrators prevent I-term wind-up (PID output is not used in PASSTHRU) */
|
|
||||||
pidResetErrorAccumulators();
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
if (throttleStatus == THROTTLE_LOW) {
|
|
||||||
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
|
|
||||||
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
|
|
||||||
|
|
||||||
// ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs
|
|
||||||
if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) {
|
|
||||||
ENABLE_STATE(ANTI_WINDUP);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
DISABLE_STATE(ANTI_WINDUP);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
DISABLE_STATE(ANTI_WINDUP);
|
|
||||||
pidResetErrorAccumulators();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
DISABLE_STATE(ANTI_WINDUP);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE) {
|
|
||||||
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(AUTOTUNE_FIXED_WING) || defined(AUTOTUNE_MULTIROTOR)
|
|
||||||
autotuneUpdateState();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef TELEMETRY
|
|
||||||
if (feature(FEATURE_TELEMETRY)) {
|
|
||||||
if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
|
|
||||||
(telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
|
|
||||||
|
|
||||||
releaseSharedTelemetryPorts();
|
|
||||||
} else {
|
|
||||||
// the telemetry state must be checked immediately so that shared serial ports are released.
|
|
||||||
telemetryCheckState();
|
|
||||||
mspSerialAllocatePorts();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void filterRc(bool isRXDataNew)
|
|
||||||
{
|
|
||||||
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
|
|
||||||
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
|
|
||||||
static int16_t factor, rcInterpolationFactor;
|
|
||||||
static biquadFilter_t filteredCycleTimeState;
|
|
||||||
static bool filterInitialised;
|
|
||||||
|
|
||||||
// Calculate average cycle time (1Hz LPF on cycle time)
|
|
||||||
if (!filterInitialised) {
|
|
||||||
biquadFilterInitLPF(&filteredCycleTimeState, 1, getPidUpdateRate());
|
|
||||||
filterInitialised = true;
|
|
||||||
}
|
|
||||||
|
|
||||||
const timeDelta_t filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime);
|
|
||||||
rcInterpolationFactor = rxGetRefreshRate() / filteredCycleTime + 1;
|
|
||||||
|
|
||||||
if (isRXDataNew) {
|
|
||||||
for (int channel=0; channel < 4; channel++) {
|
|
||||||
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
|
|
||||||
lastCommand[channel] = rcCommand[channel];
|
|
||||||
}
|
|
||||||
|
|
||||||
factor = rcInterpolationFactor - 1;
|
|
||||||
} else {
|
|
||||||
factor--;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Interpolate steps of rcCommand
|
|
||||||
if (factor > 0) {
|
|
||||||
for (int channel=0; channel < 4; channel++) {
|
|
||||||
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
factor = 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Function for loop trigger
|
|
||||||
void taskGyro(timeUs_t currentTimeUs) {
|
|
||||||
// getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
|
|
||||||
// To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
|
|
||||||
const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
|
|
||||||
|
|
||||||
if (gyroConfig()->gyroSync) {
|
|
||||||
while (true) {
|
|
||||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + cmpTimeUs(micros(), currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Update actual hardware readings */
|
|
||||||
gyroUpdate();
|
|
||||||
|
|
||||||
#ifdef ASYNC_GYRO_PROCESSING
|
|
||||||
/* Update IMU for better accuracy */
|
|
||||||
imuUpdateGyroscope((timeUs_t)currentDeltaTime + (micros() - currentTimeUs));
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompensationStrength)
|
|
||||||
{
|
|
||||||
if (throttleTiltCompensationStrength) {
|
|
||||||
float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f); // max tilt about 50 deg
|
|
||||||
return 1.0f + (tiltCompFactor - 1.0f) * (throttleTiltCompensationStrength / 100.f);
|
|
||||||
} else {
|
|
||||||
return 1.0f;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void taskMainPidLoop(timeUs_t currentTimeUs)
|
|
||||||
{
|
|
||||||
cycleTime = getTaskDeltaTime(TASK_SELF);
|
|
||||||
dT = (float)cycleTime * 0.000001f;
|
|
||||||
|
|
||||||
#ifdef ASYNC_GYRO_PROCESSING
|
|
||||||
if (getAsyncMode() == ASYNC_MODE_NONE) {
|
|
||||||
taskGyro(currentTimeUs);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (getAsyncMode() != ASYNC_MODE_ALL && sensors(SENSOR_ACC)) {
|
|
||||||
imuUpdateAccelerometer();
|
|
||||||
imuUpdateAttitude(currentTimeUs);
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
/* Update gyroscope */
|
|
||||||
taskGyro(currentTimeUs);
|
|
||||||
imuUpdateAccelerometer();
|
|
||||||
imuUpdateAttitude(currentTimeUs);
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
annexCode();
|
|
||||||
|
|
||||||
if (rxConfig()->rcSmoothing) {
|
|
||||||
filterRc(isRXDataNew);
|
|
||||||
}
|
|
||||||
|
|
||||||
#if defined(NAV)
|
|
||||||
if (isRXDataNew) {
|
|
||||||
updateWaypointsAndNavigationMode();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
isRXDataNew = false;
|
|
||||||
|
|
||||||
#if defined(NAV)
|
|
||||||
updatePositionEstimator();
|
|
||||||
applyWaypointNavigationAndAltitudeHold();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// If we're armed, at minimum throttle, and we do arming via the
|
|
||||||
// sticks, do not process yaw input from the rx. We do this so the
|
|
||||||
// motors do not spin up while we are trying to arm or disarm.
|
|
||||||
// Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
|
|
||||||
if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
&& !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
|
|
||||||
#endif
|
|
||||||
&& mixerConfig()->mixerMode != MIXER_AIRPLANE
|
|
||||||
&& mixerConfig()->mixerMode != MIXER_FLYING_WING
|
|
||||||
&& mixerConfig()->mixerMode != MIXER_CUSTOM_AIRPLANE
|
|
||||||
#endif
|
|
||||||
) {
|
|
||||||
rcCommand[YAW] = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Apply throttle tilt compensation
|
|
||||||
if (!STATE(FIXED_WING)) {
|
|
||||||
int16_t thrTiltCompStrength = 0;
|
|
||||||
|
|
||||||
if (navigationRequiresThrottleTiltCompensation()) {
|
|
||||||
thrTiltCompStrength = 100;
|
|
||||||
}
|
|
||||||
else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
|
|
||||||
thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (thrTiltCompStrength) {
|
|
||||||
rcCommand[THROTTLE] = constrain(motorConfig()->minthrottle
|
|
||||||
+ (rcCommand[THROTTLE] - motorConfig()->minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
|
|
||||||
motorConfig()->minthrottle,
|
|
||||||
motorConfig()->maxthrottle);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
// FIXME: throttle pitch comp for FW
|
|
||||||
}
|
|
||||||
|
|
||||||
// Update PID coefficients
|
|
||||||
updatePIDCoefficients();
|
|
||||||
|
|
||||||
// Calculate stabilisation
|
|
||||||
pidController();
|
|
||||||
|
|
||||||
#ifdef HIL
|
|
||||||
if (hilActive) {
|
|
||||||
hilUpdateControlState();
|
|
||||||
motorControlEnable = false;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
mixTable();
|
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
|
||||||
if (isMixerUsingServos()) {
|
|
||||||
servoMixer();
|
|
||||||
}
|
|
||||||
if (feature(FEATURE_SERVO_TILT)) {
|
|
||||||
processServoTilt();
|
|
||||||
}
|
|
||||||
processServoAutotrim();
|
|
||||||
//Servos should be filtered or written only when mixer is using servos or special feaures are enabled
|
|
||||||
if (isServoOutputEnabled()) {
|
|
||||||
writeServos();
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (motorControlEnable) {
|
|
||||||
writeMotors();
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef USE_SDCARD
|
|
||||||
afatfs_poll();
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef BLACKBOX
|
|
||||||
if (!cliMode && feature(FEATURE_BLACKBOX)) {
|
|
||||||
blackboxUpdate(micros());
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
bool taskUpdateRxCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
|
|
||||||
{
|
|
||||||
UNUSED(currentDeltaTime);
|
|
||||||
|
|
||||||
return rxUpdateCheck(currentTimeUs, currentDeltaTime);
|
|
||||||
}
|
|
||||||
|
|
||||||
void taskUpdateRxMain(timeUs_t currentTimeUs)
|
|
||||||
{
|
|
||||||
processRx(currentTimeUs);
|
|
||||||
isRXDataNew = true;
|
|
||||||
}
|
|
||||||
|
|
@ -23,6 +23,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "build/debug.h"
|
||||||
|
|
||||||
#include "blackbox/blackbox.h"
|
#include "blackbox/blackbox.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
|
|
@ -64,6 +66,9 @@
|
||||||
// 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)
|
||||||
static bool isUsingSticksToArm = true;
|
static bool isUsingSticksToArm = true;
|
||||||
|
|
||||||
|
// Count of mode activation ranged (per box mode)
|
||||||
|
static uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
|
||||||
|
|
||||||
#ifdef NAV
|
#ifdef NAV
|
||||||
// true if pilot has any of GPS modes configured
|
// true if pilot has any of GPS modes configured
|
||||||
static bool isUsingNAVModes = false;
|
static bool isUsingNAVModes = false;
|
||||||
|
|
@ -373,20 +378,13 @@ void updateActivatedModes(void)
|
||||||
// Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode.
|
// Unfortunately for AND logic it's not enough to simply check if any of the specified channel range conditions are valid for a mode.
|
||||||
// We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid.
|
// We need to count the total number of conditions specified for each mode, and check that all those conditions are currently valid.
|
||||||
|
|
||||||
uint8_t specifiedConditionCountPerMode[CHECKBOX_ITEM_COUNT];
|
uint8_t activeConditionCountPerMode[CHECKBOX_ITEM_COUNT];
|
||||||
uint8_t validConditionCountPerMode[CHECKBOX_ITEM_COUNT];
|
memset(activeConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
|
||||||
|
|
||||||
memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
|
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
|
||||||
memset(validConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
|
if (isRangeActive(modeActivationConditions(index)->auxChannelIndex, &modeActivationConditions(index)->range)) {
|
||||||
|
|
||||||
for (int modeIndex = 0; modeIndex < MAX_MODE_ACTIVATION_CONDITION_COUNT; modeIndex++) {
|
|
||||||
|
|
||||||
// Increment the number of specified conditions for this mode
|
|
||||||
specifiedConditionCountPerMode[modeActivationConditions(modeIndex)->modeId]++;
|
|
||||||
|
|
||||||
if (isRangeActive(modeActivationConditions(modeIndex)->auxChannelIndex, &modeActivationConditions(modeIndex)->range)) {
|
|
||||||
// Increment the number of valid conditions for this mode
|
// Increment the number of valid conditions for this mode
|
||||||
validConditionCountPerMode[modeActivationConditions(modeIndex)->modeId]++;
|
activeConditionCountPerMode[modeActivationConditions(index)->modeId]++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -402,13 +400,13 @@ void updateActivatedModes(void)
|
||||||
|
|
||||||
if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
|
if (modeActivationOperatorConfig()->modeActivationOperator == MODE_OPERATOR_AND) {
|
||||||
// AND the conditions
|
// AND the conditions
|
||||||
if (validConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
|
if (activeConditionCountPerMode[modeIndex] == specifiedConditionCountPerMode[modeIndex]) {
|
||||||
ACTIVATE_RC_MODE(modeIndex);
|
ACTIVATE_RC_MODE(modeIndex);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
// OR the conditions
|
// OR the conditions
|
||||||
if (validConditionCountPerMode[modeIndex] > 0) {
|
if (activeConditionCountPerMode[modeIndex] > 0) {
|
||||||
ACTIVATE_RC_MODE(modeIndex);
|
ACTIVATE_RC_MODE(modeIndex);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -422,6 +420,13 @@ int32_t getRcStickDeflection(int32_t axis, uint16_t midrc) {
|
||||||
|
|
||||||
void updateUsedModeActivationConditionFlags(void)
|
void updateUsedModeActivationConditionFlags(void)
|
||||||
{
|
{
|
||||||
|
memset(specifiedConditionCountPerMode, 0, CHECKBOX_ITEM_COUNT);
|
||||||
|
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
|
||||||
|
if (IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
|
||||||
|
specifiedConditionCountPerMode[modeActivationConditions(index)->modeId]++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
|
isUsingSticksToArm = !isModeActivationConditionPresent(BOXARM);
|
||||||
|
|
||||||
#ifdef NAV
|
#ifdef NAV
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue