Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

added gps for altitude estimation, remove most unused code, rename altitude.c to position.c, add hdop to nmea

Browse source
This commit is contained in:
Diego Basch 2018-04-24 11:11:35 -07:00
parent 75f82c7f2b
commit c46be03047
26 changed files with 154 additions and 550 deletions
Download patch file Download diff file Expand all files Collapse all files

2
make/source.mk
View file

@ -107,7 +107,7 @@ FC_SRC = \
fc/rc_adjustments.c \ fc/rc_adjustments.c \
fc/rc_controls.c \ fc/rc_controls.c \
fc/rc_modes.c \ fc/rc_modes.c \
flight/altitude.c \ flight/position.c \
flight/failsafe.c \ flight/failsafe.c \
flight/imu.c \ flight/imu.c \
flight/mixer.c \ flight/mixer.c \

12
src/main/fc/fc_core.c
View file

@ -83,7 +83,7 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
@ -867,16 +867,6 @@ static NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
} }
#endif #endif
#if defined(USE_ALT_HOLD)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE)) {
applyAltHold();
}
}
#endif
// If we're armed, at minimum throttle, and we do arming via the // 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 // 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. // motors do not spin up while we are trying to arm or disarm.

54
src/main/fc/fc_tasks.c
View file

@ -49,7 +49,7 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
@ -81,8 +81,8 @@
#include "sensors/compass.h" #include "sensors/compass.h"
#include "sensors/esc_sensor.h" #include "sensors/esc_sensor.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "sensors/rangefinder.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/rangefinder.h"
#include "scheduler/scheduler.h" #include "scheduler/scheduler.h"
@ -168,25 +168,9 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
} }
#endif #endif
#if !defined(USE_ALT_HOLD)
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands(); updateRcCommands();
#endif
updateArmingStatus(); updateArmingStatus();
#ifdef USE_ALT_HOLD
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
updateAltHoldState();
}
#endif
#ifdef USE_RANGEFINDER
if (sensors(SENSOR_RANGEFINDER)) {
updateRangefinderAltHoldState();
}
#endif
#endif // USE_ALT_HOLD
} }
#endif #endif
@ -204,20 +188,12 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
} }
#endif #endif
#if defined(USE_BARO) || defined(USE_RANGEFINDER) #if defined(USE_BARO) || defined(USE_GPS)
static void taskCalculateAltitude(timeUs_t currentTimeUs) static void taskCalculateAltitude(timeUs_t currentTimeUs)
{ {
if (false calculateEstimatedAltitude(currentTimeUs);
#if defined(USE_BARO) }
|| (sensors(SENSOR_BARO) && isBaroReady()) #endif // USE_BARO || USE_GPS
#endif
#if defined(USE_RANGEFINDER)
|| sensors(SENSOR_RANGEFINDER)
#endif
) {
calculateEstimatedAltitude(currentTimeUs);
}}
#endif // USE_BARO || USE_RANGEFINDER
#ifdef USE_TELEMETRY #ifdef USE_TELEMETRY
static void taskTelemetry(timeUs_t currentTimeUs) static void taskTelemetry(timeUs_t currentTimeUs)
@ -296,11 +272,8 @@ void fcTasksInit(void)
#ifdef USE_BARO #ifdef USE_BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO)); setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif #endif
#ifdef USE_RANGEFINDER #if defined(USE_BARO) || defined(USE_GPS)
setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER)); setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_GPS));
#endif
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER));
#endif #endif
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD
setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD)); setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
@ -502,16 +475,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
}, },
#endif #endif
#ifdef USE_RANGEFINDER #if defined(USE_BARO) || defined(USE_GPS)
[TASK_RANGEFINDER] = {
.taskName = "RANGEFINDER",
.taskFunc = rangefinderUpdate,
.desiredPeriod = TASK_PERIOD_MS(70), // XXX HCSR04 sonar specific value.
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
[TASK_ALTITUDE] = { [TASK_ALTITUDE] = {
.taskName = "ALTITUDE", .taskName = "ALTITUDE",
.taskFunc = taskCalculateAltitude, .taskFunc = taskCalculateAltitude,

317
src/main/flight/altitude.c
View file

@ -1,317 +0,0 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/altitude.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "rx/rx.h"
#include "sensors/sensors.h"
#include "sensors/barometer.h"
#include "sensors/rangefinder.h"
int32_t AltHold;
static int32_t estimatedVario = 0; // variometer in cm/s
static int32_t estimatedAltitude = 0; // in cm
enum {
DEBUG_ALTITUDE_ACC,
DEBUG_ALTITUDE_VEL,
DEBUG_ALTITUDE_HEIGHT
};
// 40hz update rate (20hz LPF on acc)
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#if defined(USE_ALT_HOLD)
PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig,
.fixedwing_althold_reversed = false
);
static int32_t setVelocity = 0;
static uint8_t velocityControl = 0;
static int32_t errorVelocityI = 0;
static int32_t altHoldThrottleAdjustment = 0;
static int16_t initialThrottleHold;
#define DEGREES_80_IN_DECIDEGREES 800
static void applyMultirotorAltHold(void)
{
static uint8_t isAltHoldChanged = 0;
// multirotor alt hold
if (rcControlsConfig()->alt_hold_fast_change) {
// rapid alt changes
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
errorVelocityI = 0;
isAltHoldChanged = 1;
rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband;
} else {
if (isAltHoldChanged) {
AltHold = estimatedAltitude;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX);
}
} else {
// slow alt changes, mostly used for aerial photography
if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
// set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
velocityControl = 1;
isAltHoldChanged = 1;
} else if (isAltHoldChanged) {
AltHold = estimatedAltitude;
velocityControl = 0;
isAltHoldChanged = 0;
}
rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX);
}
}
static void applyFixedWingAltHold(void)
{
// handle fixedwing-related althold. UNTESTED! and probably wrong
// most likely need to check changes on pitch channel and 'reset' althold similar to
// how throttle does it on multirotor
rcCommand[PITCH] += altHoldThrottleAdjustment * GET_DIRECTION(airplaneConfig()->fixedwing_althold_reversed);
}
void applyAltHold(void)
{
if (STATE(FIXED_WING)) {
applyFixedWingAltHold();
} else {
applyMultirotorAltHold();
}
}
void updateAltHoldState(void)
{
// Baro alt hold activate
if (!IS_RC_MODE_ACTIVE(BOXBARO)) {
DISABLE_FLIGHT_MODE(BARO_MODE);
return;
}
if (!FLIGHT_MODE(BARO_MODE)) {
ENABLE_FLIGHT_MODE(BARO_MODE);
AltHold = estimatedAltitude;
initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0;
altHoldThrottleAdjustment = 0;
}
}
void updateRangefinderAltHoldState(void)
{
// Sonar alt hold activate
if (!IS_RC_MODE_ACTIVE(BOXRANGEFINDER)) {
DISABLE_FLIGHT_MODE(RANGEFINDER_MODE);
return;
}
if (!FLIGHT_MODE(RANGEFINDER_MODE)) {
ENABLE_FLIGHT_MODE(RANGEFINDER_MODE);
AltHold = estimatedAltitude;
initialThrottleHold = rcData[THROTTLE];
errorVelocityI = 0;
altHoldThrottleAdjustment = 0;
}
}
bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude)
{
return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES;
}
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
int32_t result = 0;
int32_t error;
int32_t setVel;
if (!isThrustFacingDownwards(&attitude)) {
return result;
}
// Altitude P-Controller
if (!velocityControl) {
error = constrain(AltHold - estimatedAltitude, -500, 500);
error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
setVel = constrain((currentPidProfile->pid[PID_ALT].P * error / 128), -300, +300); // limit velocity to +/- 3 m/s
} else {
setVel = setVelocity;
}
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
result = constrain((currentPidProfile->pid[PID_VEL].P * error / 32), -300, +300);
// I
errorVelocityI += (currentPidProfile->pid[PID_VEL].I * error);
errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
result += errorVelocityI / 8192; // I in range +/-200
// D
result -= constrain(currentPidProfile->pid[PID_VEL].D * (accZ_tmp + accZ_old) / 512, -150, 150);
return result;
}
#endif // USE_ALT_HOLD
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs = 0;
const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
return;
}
previousTimeUs = currentTimeUs;
static float vel = 0.0f;
static float accAlt = 0.0f;
int32_t baroAlt = 0;
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
} else {
baroAlt = baroCalculateAltitude();
estimatedAltitude = baroAlt;
}
}
#endif
#ifdef USE_RANGEFINDER
if (sensors(SENSOR_RANGEFINDER) && rangefinderProcess(getCosTiltAngle())) {
int32_t rangefinderAlt = rangefinderGetLatestAltitude();
if (rangefinderAlt > 0 && rangefinderAlt >= rangefinderCfAltCm && rangefinderAlt <= rangefinderMaxAltWithTiltCm) {
// RANGEFINDER in range, so use complementary filter
float rangefinderTransition = (float)(rangefinderMaxAltWithTiltCm - rangefinderAlt) / (rangefinderMaxAltWithTiltCm - rangefinderCfAltCm);
rangefinderAlt = (float)rangefinderAlt * rangefinderTransition + baroAlt * (1.0f - rangefinderTransition);
estimatedAltitude = rangefinderAlt;
}
}
#endif
float accZ_tmp = 0;
#ifdef USE_ACC
if (sensors(SENSOR_ACC)) {
const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / accSumCount;
}
const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt)); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
estimatedAltitude = accAlt;
}
#endif
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt);
imuResetAccelerationSum();
int32_t baroVel = 0;
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
return;
}
static int32_t lastBaroAlt = 0;
baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
}
#endif // USE_BARO
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
int32_t vel_tmp = lrintf(vel);
// set vario
estimatedVario = applyDeadband(vel_tmp, 5);
#ifdef USE_ALT_HOLD
static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
#else
UNUSED(accZ_tmp);
#endif
}
#endif // USE_BARO || USE_RANGEFINDER
int32_t getEstimatedAltitude(void)
{
return estimatedAltitude;
}
int32_t getEstimatedVario(void)
{
return estimatedVario;
}

56
src/main/flight/imu.c
View file

@ -193,58 +193,6 @@ void imuResetAccelerationSum(void)
accTimeSum = 0; accTimeSum = 0;
} }
#if defined(USE_ALT_HOLD)
static void imuTransformVectorBodyToEarth(t_fp_vector * v)
{
// From body frame to earth frame
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
v->V.X = x;
v->V.Y = -y;
v->V.Z = z;
}
// rotate acc into Earth frame and calculate acceleration in it
static void imuCalculateAcceleration(timeDelta_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
// deltaT is measured in us ticks
const float dT = (float)deltaT * 1e-6f;
t_fp_vector accel_ned;
accel_ned.V.X = acc.accADC[X];
accel_ned.V.Y = acc.accADC[Y];
accel_ned.V.Z = acc.accADC[Z];
imuTransformVectorBodyToEarth(&accel_ned);
if (imuRuntimeConfig.acc_unarmedcal == 1) {
if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else {
accel_ned.V.Z -= acc.dev.acc_1G;
}
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
}
#endif // USE_ALT_HOLD
static float invSqrt(float x) static float invSqrt(float x)
{ {
return 1.0f / sqrtf(x); return 1.0f / sqrtf(x);
@ -531,6 +479,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
UNUSED(useCOG); UNUSED(useCOG);
UNUSED(canUseGPSHeading); UNUSED(canUseGPSHeading);
UNUSED(courseOverGround); UNUSED(courseOverGround);
UNUSED(deltaT);
#else #else
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC) #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
@ -552,9 +501,6 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
imuUpdateEulerAngles(); imuUpdateEulerAngles();
#endif #endif
#if defined(USE_ALT_HOLD)
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
#endif
} }
void imuUpdateAttitude(timeUs_t currentTimeUs) void imuUpdateAttitude(timeUs_t currentTimeUs)

117
src/main/flight/position.c Normal file
View file

@ -0,0 +1,117 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software: you can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include "build/debug.h"
#include "common/maths.h"
#include "fc/runtime_config.h"
#include "flight/position.h"
#include "flight/imu.h"
#include "flight/pid.h"
#include "io/gps.h"
#include "sensors/sensors.h"
#include "sensors/barometer.h"
static int32_t estimatedAltitude = 0; // in cm
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#if defined(USE_BARO) || defined(USE_GPS)
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs = 0;
const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
return;
}
previousTimeUs = currentTimeUs;
int32_t baroAlt = 0;
static int32_t baroAltOffset = 0;
static int32_t gpsAltOffset = 0;
int32_t gpsAlt = 0;
float gpsTrust = 0.3; //conservative default
bool haveBaroAlt = false;
bool haveGPSAlt = false;
#ifdef USE_BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
} else {
baroAlt = baroCalculateAltitude();
haveBaroAlt = true;
}
}
#endif
#ifdef USE_GPS
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
gpsAlt = gpsSol.llh.alt;
haveGPSAlt = true;
if (gpsSol.hdop != 0) {
gpsTrust = 100.0 / gpsSol.hdop;
}
// always use at least 10% of other sources besides gps if available
gpsTrust = MIN(gpsTrust, 0.9f);
}
#endif
if (!ARMING_FLAG(ARMED)) {
baroAltOffset = baroAlt;
gpsAltOffset = gpsAlt;
}
baroAlt -= baroAltOffset;
gpsAlt -= gpsAltOffset;
if (haveGPSAlt && haveBaroAlt) {
estimatedAltitude = gpsAlt * gpsTrust + baroAlt * (1-gpsTrust);
} else if (haveGPSAlt && !haveBaroAlt) {
estimatedAltitude = gpsAlt;
}
DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);
}
#endif
int32_t getEstimatedAltitude(void)
{
return estimatedAltitude;
}
// This should be removed or fixed, but it would require changing a lot of other things to get rid of.
int16_t getEstimatedVario(void)
{
return 0;
}

14
src/main/flight/altitude.h → src/main/flight/position.h
View file

@ -22,18 +22,6 @@
#include "common/time.h" #include "common/time.h"
extern int32_t AltHold;
typedef struct airplaneConfig_s {
bool fixedwing_althold_reversed; // false for negative pitch/althold gain. later check if need more than just sign
} airplaneConfig_t;
PG_DECLARE(airplaneConfig_t, airplaneConfig);
void calculateEstimatedAltitude(timeUs_t currentTimeUs); void calculateEstimatedAltitude(timeUs_t currentTimeUs);
int32_t getEstimatedAltitude(void); int32_t getEstimatedAltitude(void);
int32_t getEstimatedVario(void); int16_t getEstimatedVario(void);
void applyAltHold(void);
void updateAltHoldState(void);
void updateRangefinderAltHoldState(void);

2
src/main/interface/cli.c
View file

@ -91,7 +91,7 @@ extern uint8_t __config_end;
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"

61
src/main/interface/msp.c
View file

@ -69,7 +69,7 @@
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
@ -1324,29 +1324,6 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sb
} }
#endif // USE_OSD_SLAVE #endif // USE_OSD_SLAVE
#ifdef USE_NAV
static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
{
uint8_t wp_no;
int32_t lat = 0, lon = 0;
wp_no = sbufReadU8(src); // get the wp number
if (wp_no == 0) {
lat = GPS_home[LAT];
lon = GPS_home[LON];
} else if (wp_no == 16) {
lat = GPS_hold[LAT];
lon = GPS_hold[LON];
}
sbufWriteU8(dst, wp_no);
sbufWriteU32(dst, lat);
sbufWriteU32(dst, lon);
sbufWriteU32(dst, AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
sbufWriteU16(dst, 0); // heading will come here (deg)
sbufWriteU16(dst, 0); // time to stay (ms) will come here
sbufWriteU8(dst, 0); // nav flag will come here
}
#endif
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src) static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
{ {
@ -1399,10 +1376,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
uint32_t i; uint32_t i;
uint8_t value; uint8_t value;
const unsigned int dataSize = sbufBytesRemaining(src); const unsigned int dataSize = sbufBytesRemaining(src);
#ifdef USE_NAV
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
#endif
switch (cmdMSP) { switch (cmdMSP) {
case MSP_SELECT_SETTING: case MSP_SELECT_SETTING:
value = sbufReadU8(src); value = sbufReadU8(src);
@ -1869,33 +1842,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break; break;
#endif // USE_GPS #endif // USE_GPS
#ifdef USE_NAV
case MSP_SET_WP:
wp_no = sbufReadU8(src); //get the wp number
lat = sbufReadU32(src);
lon = sbufReadU32(src);
alt = sbufReadU32(src); // to set altitude (cm)
sbufReadU16(src); // future: to set heading (deg)
sbufReadU16(src); // future: to set time to stay (ms)
sbufReadU8(src); // future: to set nav flag
if (wp_no == 0) {
GPS_home[LAT] = lat;
GPS_home[LON] = lon;
DISABLE_FLIGHT_MODE(GPS_HOME_MODE); // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
ENABLE_STATE(GPS_FIX_HOME);
if (alt != 0)
AltHold = alt; // temporary implementation to test feature with apps
} else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
GPS_hold[LAT] = lat;
GPS_hold[LON] = lon;
if (alt != 0)
AltHold = alt; // temporary implementation to test feature with apps
nav_mode = NAV_MODE_WP;
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
}
break;
#endif // USE_NAV
case MSP_SET_FEATURE_CONFIG: case MSP_SET_FEATURE_CONFIG:
featureClearAll(); featureClearAll();
featureSet(sbufReadU32(src)); // features bitmap featureSet(sbufReadU32(src)); // features bitmap
@ -2307,11 +2253,6 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
mspFc4waySerialCommand(dst, src, mspPostProcessFn); mspFc4waySerialCommand(dst, src, mspPostProcessFn);
ret = MSP_RESULT_ACK; ret = MSP_RESULT_ACK;
#endif #endif
#ifdef USE_NAV
} else if (cmdMSP == MSP_WP) {
mspFcWpCommand(dst, src);
ret = MSP_RESULT_ACK;
#endif
#ifdef USE_FLASHFS #ifdef USE_FLASHFS
} else if (cmdMSP == MSP_DATAFLASH_READ) { } else if (cmdMSP == MSP_DATAFLASH_READ) {
mspFcDataFlashReadCommand(dst, src); mspFcDataFlashReadCommand(dst, src);

21
src/main/interface/settings.c
View file

@ -45,7 +45,7 @@
#include "fc/rc_adjustments.h" #include "fc/rc_adjustments.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
@ -692,16 +692,6 @@ const clivalue_t valueTable[] = {
{ "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_slew_rate) }, { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_slew_rate) },
#endif #endif
// PG_AIRPLANE_CONFIG
#if defined(USE_ALT_HOLD)
{ "fixedwing_althold_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_reversed) },
#endif
// PG_RC_CONTROLS_CONFIG
#if defined(USE_ALT_HOLD)
{ "alt_hold_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) },
{ "alt_hold_fast_change", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_fast_change) },
#endif
{ "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) }, { "deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) },
{ "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) }, { "yaw_deadband", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) },
{ "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) }, { "yaw_control_reversed", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
@ -758,15 +748,6 @@ const clivalue_t valueTable[] = {
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) }, { "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) }, { "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
#ifdef USE_ALT_HOLD
{ "p_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].P) },
{ "i_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].I) },
{ "d_alt", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].D) },
{ "p_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) },
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) },
#endif
{ "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) }, { "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
{ "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) }, { "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
{ "d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) }, { "d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },

7
src/main/io/gps.c
View file

@ -636,6 +636,7 @@ typedef struct gpsDataNmea_s {
uint8_t numSat; uint8_t numSat;
uint16_t altitude; uint16_t altitude;
uint16_t speed; uint16_t speed;
uint16_t hdop;
uint16_t ground_course; uint16_t ground_course;
uint32_t time; uint32_t time;
uint32_t date; uint32_t date;
@ -700,6 +701,9 @@ static bool gpsNewFrameNMEA(char c)
case 7: case 7:
gps_Msg.numSat = grab_fields(string, 0); gps_Msg.numSat = grab_fields(string, 0);
break; break;
case 8:
gps_Msg.hdop = grab_fields(string, 1) * 100; // hdop
break;
case 9: case 9:
gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis gps_Msg.altitude = grab_fields(string, 0); // altitude in meters added by Mis
break; break;
@ -793,6 +797,7 @@ static bool gpsNewFrameNMEA(char c)
gpsSol.llh.lon = gps_Msg.longitude; gpsSol.llh.lon = gps_Msg.longitude;
gpsSol.numSat = gps_Msg.numSat; gpsSol.numSat = gps_Msg.numSat;
gpsSol.llh.alt = gps_Msg.altitude; gpsSol.llh.alt = gps_Msg.altitude;
gpsSol.hdop = gps_Msg.hdop;
} }
break; break;
case FRAME_RMC: case FRAME_RMC:
@ -1015,7 +1020,7 @@ static bool UBLOX_parse_gps(void)
//i2c_dataset.time = _buffer.posllh.time; //i2c_dataset.time = _buffer.posllh.time;
gpsSol.llh.lon = _buffer.posllh.longitude; gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude; gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100; //alt in m gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10; //alt in cm
if (next_fix) { if (next_fix) {
ENABLE_STATE(GPS_FIX); ENABLE_STATE(GPS_FIX);
} else { } else {

2
src/main/io/gps.h
View file

@ -85,7 +85,7 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
typedef struct gpsLocation_s { typedef struct gpsLocation_s {
int32_t lat; // latitude * 1e+7 int32_t lat; // latitude * 1e+7
int32_t lon; // longitude * 1e+7 int32_t lon; // longitude * 1e+7
uint16_t alt; // altitude in 0.1m int32_t alt; // altitude in 0.1m
} gpsLocation_t; } gpsLocation_t;
typedef struct gpsSolutionData_s { typedef struct gpsSolutionData_s {

2
src/main/io/osd.c
View file

@ -65,7 +65,7 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/imu.h" #include "flight/imu.h"
#ifdef USE_ESC_SENSOR #ifdef USE_ESC_SENSOR
#include "flight/mixer.h" #include "flight/mixer.h"

2
src/main/scheduler/scheduler.h
View file

@ -82,7 +82,7 @@ typedef enum {
#ifdef USE_RANGEFINDER #ifdef USE_RANGEFINDER
TASK_RANGEFINDER, TASK_RANGEFINDER,
#endif #endif
#if defined(USE_BARO) || defined(USE_RANGEFINDER) #if defined(USE_BARO) || defined(USE_GPS)
TASK_ALTITUDE, TASK_ALTITUDE,
#endif #endif
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD

2
src/main/target/COLIBRI_RACE/i2c_bst.c
View file

@ -57,7 +57,7 @@
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"

5
src/main/target/common_fc_post.h
View file

@ -74,11 +74,6 @@
#undef USE_SPEKTRUM_CMS_TELEMETRY #undef USE_SPEKTRUM_CMS_TELEMETRY
#endif #endif
// undefine USE_ALT_HOLD if there is no baro or rangefinder to support it
#if defined(USE_ALT_HOLD) && !defined(USE_BARO) && !defined(USE_RANGEFINDER)
#undef USE_ALT_HOLD
#endif
/* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */ /* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */
#if !defined(USE_VTX_COMMON) || !defined(USE_VTX_CONTROL) #if !defined(USE_VTX_COMMON) || !defined(USE_VTX_CONTROL)
#undef USE_VTX_COMMON #undef USE_VTX_COMMON

1
src/main/target/common_fc_pre.h
View file

@ -198,7 +198,6 @@
#endif #endif
#if (FLASH_SIZE > 256) #if (FLASH_SIZE > 256)
#define USE_ALT_HOLD
#define USE_DASHBOARD #define USE_DASHBOARD
#define USE_GPS #define USE_GPS
#define USE_GPS_NMEA #define USE_GPS_NMEA

2
src/main/telemetry/frsky_hub.c
View file

@ -60,7 +60,7 @@
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "rx/rx.h" #include "rx/rx.h"

2
src/main/telemetry/hott.c
View file

@ -75,7 +75,7 @@
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "io/gps.h" #include "io/gps.h"

2
src/main/telemetry/ibus_shared.c
View file

@ -54,7 +54,7 @@ static uint16_t calculateChecksum(const uint8_t *ibusPacket);
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/barometer.h" #include "sensors/barometer.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "io/gps.h" #include "io/gps.h"

2
src/main/telemetry/jetiexbus.c
View file

@ -38,7 +38,7 @@
#include "drivers/serial_uart.h" #include "drivers/serial_uart.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "io/serial.h" #include "io/serial.h"

2
src/main/telemetry/ltm.c
View file

@ -72,7 +72,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "telemetry/telemetry.h" #include "telemetry/telemetry.h"
#include "telemetry/ltm.h" #include "telemetry/ltm.h"

2
src/main/telemetry/mavlink.c
View file

@ -52,7 +52,7 @@
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/navigation.h" #include "flight/navigation.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/gimbal.h" #include "io/gimbal.h"

2
src/main/telemetry/smartport.c
View file

@ -51,7 +51,7 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"

7
src/test/Makefile
View file

@ -26,11 +26,6 @@ alignsensor_unittest_SRC := \
$(USER_DIR)/sensors/boardalignment.c \ $(USER_DIR)/sensors/boardalignment.c \
$(USER_DIR)/common/maths.c $(USER_DIR)/common/maths.c
altitude_hold_unittest_SRC := \
$(USER_DIR)/flight/altitude.c
arming_prevention_unittest_SRC := \ arming_prevention_unittest_SRC := \
$(USER_DIR)/fc/fc_core.c \ $(USER_DIR)/fc/fc_core.c \
$(USER_DIR)/fc/rc_controls.c \ $(USER_DIR)/fc/rc_controls.c \
@ -120,7 +115,7 @@ flight_imu_unittest_SRC := \
$(USER_DIR)/common/maths.c \ $(USER_DIR)/common/maths.c \
$(USER_DIR)/config/feature.c \ $(USER_DIR)/config/feature.c \
$(USER_DIR)/fc/rc_modes.c \ $(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/flight/altitude.c \ $(USER_DIR)/flight/position.c \
$(USER_DIR)/flight/imu.c $(USER_DIR)/flight/imu.c

2
src/test/unit/altitude_hold_unittest.cc.txt
View file

@ -45,7 +45,7 @@ extern "C" {
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/altitude.h" #include "flight/position.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"