1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00

naivation to navigation typo fixed

This commit is contained in:
Giacomo 2017-05-18 19:12:26 +02:00
parent 3b474954ed
commit b6321cde9e
5 changed files with 769 additions and 15 deletions

View file

@ -167,7 +167,7 @@ static void updatePreArmingChecks(void)
}
#if defined(NAV)
if (naivationBlockArming()) {
if (navigationBlockArming()) {
ENABLE_ARMING_FLAG(BLOCKED_NAVIGATION_SAFETY);
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
@ -397,7 +397,7 @@ void processRx(timeUs_t currentTimeUs)
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || naivationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
// bumpless transfer to Level mode
canUseHorizonMode = false;
@ -475,7 +475,7 @@ void processRx(timeUs_t currentTimeUs)
// Handle passthrough mode
if (STATE(FIXED_WING)) {
if ((IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !naivationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough
if ((IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !navigationRequiresAngleMode() && !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 {

754
src/main/fc/fc_core.c.orig Executable file
View file

@ -0,0 +1,754 @@
/*
* 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;
}

View file

@ -549,7 +549,7 @@ static uint8_t getHeadingHoldState()
}
#if defined(NAV)
int navHeadingState = naivationGetHeadingControlState();
int navHeadingState = navigationGetHeadingControlState();
// NAV will prevent MAG_MODE from activating, but require heading control
if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
// Apply maghold only if heading control is in auto mode
@ -722,7 +722,7 @@ void pidController(void)
}
#ifdef USE_FLM_TURN_ASSIST
if (FLIGHT_MODE(TURN_ASSISTANT) || naivationRequiresTurnAssistance()) {
if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
pidTurnAssistant(pidState);
}
#endif

View file

@ -2417,7 +2417,7 @@ bool navigationRequiresThrottleTiltCompensation(void)
/*-----------------------------------------------------------
* An indicator that ANGLE mode must be forced per NAV requirement
*-----------------------------------------------------------*/
bool naivationRequiresAngleMode(void)
bool navigationRequiresAngleMode(void)
{
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
@ -2426,7 +2426,7 @@ bool naivationRequiresAngleMode(void)
/*-----------------------------------------------------------
* An indicator that TURN ASSISTANCE is required for navigation
*-----------------------------------------------------------*/
bool naivationRequiresTurnAssistance(void)
bool navigationRequiresTurnAssistance(void)
{
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
if (STATE(FIXED_WING)) {
@ -2441,7 +2441,7 @@ bool naivationRequiresTurnAssistance(void)
/**
* An indicator that NAV is in charge of heading control (a signal to disable other heading controllers)
*/
int8_t naivationGetHeadingControlState(void)
int8_t navigationGetHeadingControlState(void)
{
// For airplanes report as manual heading control
if (STATE(FIXED_WING)) {
@ -2462,7 +2462,7 @@ int8_t naivationGetHeadingControlState(void)
}
}
bool naivationBlockArming(void)
bool navigationBlockArming(void)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD);
const bool navLaunchComboModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP));

View file

@ -255,11 +255,11 @@ void updatePositionEstimator(void);
void applyWaypointNavigationAndAltitudeHold(void);
/* Functions to signal navigation requirements to main loop */
bool naivationRequiresAngleMode(void);
bool navigationRequiresAngleMode(void);
bool navigationRequiresThrottleTiltCompensation(void);
bool naivationRequiresTurnAssistance(void);
int8_t naivationGetHeadingControlState(void);
bool naivationBlockArming(void);
bool navigationRequiresTurnAssistance(void);
int8_t navigationGetHeadingControlState(void);
bool navigationBlockArming(void);
bool navigationPositionEstimateIsHealthy(void);
bool navIsCalibrationComplete(void);
@ -315,8 +315,8 @@ extern int16_t navAccNEU[3];
#else
#define naivationRequiresAngleMode() (0)
#define naivationGetHeadingControlState() (0)
#define navigationRequiresAngleMode() (0)
#define navigationGetHeadingControlState() (0)
#define navigationRequiresThrottleTiltCompensation() (0)
#define getEstimatedActualVelocity(axis) (0)