2012-08-21 02:58:09 -03:00
|
|
|
/*
|
2013-08-26 03:55:30 -03:00
|
|
|
APM_AHRS.cpp
|
2012-08-21 02:58:09 -03:00
|
|
|
|
2013-08-26 03:55:30 -03:00
|
|
|
This program 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.
|
|
|
|
|
|
|
|
This program 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.
|
2015-09-23 04:29:43 -03:00
|
|
|
|
2013-08-26 03:55:30 -03:00
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
2012-08-21 02:58:09 -03:00
|
|
|
*/
|
2015-08-11 03:28:42 -03:00
|
|
|
#include "AP_AHRS.h"
|
2017-02-11 04:15:34 -04:00
|
|
|
#include "AP_AHRS_View.h"
|
2015-08-11 03:28:42 -03:00
|
|
|
#include <AP_HAL/AP_HAL.h>
|
2019-01-18 00:23:42 -04:00
|
|
|
#include <AP_Logger/AP_Logger.h>
|
2019-06-14 00:15:53 -03:00
|
|
|
#include <AP_GPS/AP_GPS.h>
|
2019-06-26 23:37:09 -03:00
|
|
|
#include <AP_Baro/AP_Baro.h>
|
2017-02-11 04:15:34 -04:00
|
|
|
|
2012-11-14 12:10:15 -04:00
|
|
|
extern const AP_HAL::HAL& hal;
|
2012-08-21 02:58:09 -03:00
|
|
|
|
2019-03-04 14:16:06 -04:00
|
|
|
// init sets up INS board orientation
|
2021-07-20 10:04:20 -03:00
|
|
|
void AP_AHRS_Backend::init()
|
2019-03-04 14:16:06 -04:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
2017-03-02 07:33:13 -04:00
|
|
|
// return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
|
2021-07-20 10:04:20 -03:00
|
|
|
Vector3f AP_AHRS_Backend::get_gyro_latest(void) const
|
2017-03-02 07:33:13 -04:00
|
|
|
{
|
2017-12-03 06:10:19 -04:00
|
|
|
const uint8_t primary_gyro = get_primary_gyro_index();
|
2018-03-10 05:35:03 -04:00
|
|
|
return AP::ins().get_gyro(primary_gyro) + get_gyro_drift();
|
2017-03-02 07:33:13 -04:00
|
|
|
}
|
|
|
|
|
2013-02-19 05:50:57 -04:00
|
|
|
// set_trim
|
2021-07-31 03:24:05 -03:00
|
|
|
void AP_AHRS::set_trim(const Vector3f &new_trim)
|
2013-02-19 05:50:57 -04:00
|
|
|
{
|
|
|
|
Vector3f trim;
|
2013-05-01 21:25:40 -03:00
|
|
|
trim.x = constrain_float(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
|
|
|
|
trim.y = constrain_float(new_trim.y, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
|
2013-02-19 05:50:57 -04:00
|
|
|
_trim.set_and_save(trim);
|
|
|
|
}
|
|
|
|
|
2012-11-05 00:29:00 -04:00
|
|
|
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
|
2021-07-31 03:24:05 -03:00
|
|
|
void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
|
2012-11-05 00:29:00 -04:00
|
|
|
{
|
|
|
|
Vector3f trim = _trim.get();
|
|
|
|
|
|
|
|
// add new trim
|
2013-05-01 21:25:40 -03:00
|
|
|
trim.x = constrain_float(trim.x + roll_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
|
|
|
|
trim.y = constrain_float(trim.y + pitch_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
|
2012-11-05 00:29:00 -04:00
|
|
|
|
2012-12-19 11:06:20 -04:00
|
|
|
// set new trim values
|
|
|
|
_trim.set(trim);
|
|
|
|
|
|
|
|
// save to eeprom
|
|
|
|
if( save_to_eeprom ) {
|
|
|
|
_trim.save();
|
|
|
|
}
|
2012-11-19 07:49:36 -04:00
|
|
|
}
|
2013-03-29 03:23:22 -03:00
|
|
|
|
2018-03-08 22:26:56 -04:00
|
|
|
// Set the board mounting orientation, may be called while disarmed
|
2021-07-25 21:32:10 -03:00
|
|
|
void AP_AHRS::update_orientation()
|
2018-03-08 22:26:56 -04:00
|
|
|
{
|
2018-12-20 05:11:16 -04:00
|
|
|
const enum Rotation orientation = (enum Rotation)_board_orientation.get();
|
2018-03-08 22:26:56 -04:00
|
|
|
if (orientation != ROTATION_CUSTOM) {
|
|
|
|
AP::ins().set_board_orientation(orientation);
|
2021-07-25 21:32:10 -03:00
|
|
|
AP::compass().set_board_orientation(orientation);
|
2018-03-08 22:26:56 -04:00
|
|
|
} else {
|
2019-09-17 07:36:44 -03:00
|
|
|
_custom_rotation.from_euler(radians(_custom_roll), radians(_custom_pitch), radians(_custom_yaw));
|
2018-03-08 22:26:56 -04:00
|
|
|
AP::ins().set_board_orientation(orientation, &_custom_rotation);
|
2021-07-25 21:32:10 -03:00
|
|
|
AP::compass().set_board_orientation(orientation, &_custom_rotation);
|
2018-03-08 22:26:56 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-03-29 04:22:03 -03:00
|
|
|
// return a ground speed estimate in m/s
|
2021-08-19 23:44:05 -03:00
|
|
|
Vector2f AP_AHRS_DCM::groundspeed_vector(void)
|
2013-03-29 03:23:22 -03:00
|
|
|
{
|
2013-03-29 06:42:26 -03:00
|
|
|
// Generate estimate of ground speed vector using air data system
|
|
|
|
Vector2f gndVelADS;
|
|
|
|
Vector2f gndVelGPS;
|
2018-09-04 19:01:59 -03:00
|
|
|
float airspeed = 0;
|
2020-01-06 20:45:58 -04:00
|
|
|
const bool gotAirspeed = airspeed_estimate_true(airspeed);
|
2017-12-01 19:50:46 -04:00
|
|
|
const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D);
|
2013-03-29 06:42:26 -03:00
|
|
|
if (gotAirspeed) {
|
2017-12-02 09:02:37 -04:00
|
|
|
const Vector3f wind = wind_estimate();
|
|
|
|
const Vector2f wind2d(wind.x, wind.y);
|
2018-09-04 19:01:59 -03:00
|
|
|
const Vector2f airspeed_vector(_cos_yaw * airspeed, _sin_yaw * airspeed);
|
2019-06-12 06:31:25 -03:00
|
|
|
gndVelADS = airspeed_vector + wind2d;
|
2013-03-29 06:42:26 -03:00
|
|
|
}
|
2015-09-23 04:29:43 -03:00
|
|
|
|
2013-03-29 06:42:26 -03:00
|
|
|
// Generate estimate of ground speed vector using GPS
|
|
|
|
if (gotGPS) {
|
2021-01-21 18:19:38 -04:00
|
|
|
const float cog = radians(AP::gps().ground_course());
|
2017-12-01 19:50:46 -04:00
|
|
|
gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed();
|
2013-03-29 06:42:26 -03:00
|
|
|
}
|
|
|
|
// If both ADS and GPS data is available, apply a complementary filter
|
|
|
|
if (gotAirspeed && gotGPS) {
|
2015-09-23 04:29:43 -03:00
|
|
|
// The LPF is applied to the GPS and the HPF is applied to the air data estimate
|
|
|
|
// before the two are summed
|
|
|
|
//Define filter coefficients
|
|
|
|
// alpha and beta must sum to one
|
|
|
|
// beta = dt/Tau, where
|
|
|
|
// dt = filter time step (0.1 sec if called by nav loop)
|
|
|
|
// Tau = cross-over time constant (nominal 2 seconds)
|
|
|
|
// More lag on GPS requires Tau to be bigger, less lag allows it to be smaller
|
|
|
|
// To-Do - set Tau as a function of GPS lag.
|
|
|
|
const float alpha = 1.0f - beta;
|
|
|
|
// Run LP filters
|
|
|
|
_lp = gndVelGPS * beta + _lp * alpha;
|
|
|
|
// Run HP filters
|
|
|
|
_hp = (gndVelADS - _lastGndVelADS) + _hp * alpha;
|
|
|
|
// Save the current ADS ground vector for the next time step
|
|
|
|
_lastGndVelADS = gndVelADS;
|
|
|
|
// Sum the HP and LP filter outputs
|
|
|
|
return _hp + _lp;
|
2013-03-29 06:42:26 -03:00
|
|
|
}
|
|
|
|
// Only ADS data is available return ADS estimate
|
|
|
|
if (gotAirspeed && !gotGPS) {
|
2015-09-23 04:29:43 -03:00
|
|
|
return gndVelADS;
|
2013-03-29 06:42:26 -03:00
|
|
|
}
|
|
|
|
// Only GPS data is available so return GPS estimate
|
|
|
|
if (!gotAirspeed && gotGPS) {
|
2015-09-23 04:29:43 -03:00
|
|
|
return gndVelGPS;
|
2013-03-29 06:42:26 -03:00
|
|
|
}
|
2018-09-04 19:01:59 -03:00
|
|
|
|
|
|
|
if (airspeed > 0) {
|
|
|
|
// we have a rough airspeed, and we have a yaw. For
|
|
|
|
// dead-reckoning purposes we can create a estimated
|
|
|
|
// groundspeed vector
|
|
|
|
Vector2f ret(cosf(yaw), sinf(yaw));
|
|
|
|
ret *= airspeed;
|
|
|
|
// adjust for estimated wind
|
2018-12-20 05:11:16 -04:00
|
|
|
const Vector3f wind = wind_estimate();
|
2018-09-04 19:01:59 -03:00
|
|
|
ret.x += wind.x;
|
|
|
|
ret.y += wind.y;
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2013-03-29 06:42:26 -03:00
|
|
|
return Vector2f(0.0f, 0.0f);
|
2013-03-29 03:23:22 -03:00
|
|
|
}
|
2013-05-16 19:32:21 -03:00
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
/*
|
|
|
|
calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix
|
|
|
|
*/
|
2021-07-20 10:04:20 -03:00
|
|
|
void AP_AHRS_Backend::calc_trig(const Matrix3f &rot,
|
2017-02-11 04:15:34 -04:00
|
|
|
float &cr, float &cp, float &cy,
|
|
|
|
float &sr, float &sp, float &sy) const
|
2014-02-08 02:52:03 -04:00
|
|
|
{
|
2017-12-02 09:02:37 -04:00
|
|
|
Vector2f yaw_vector(rot.a.x, rot.b.x);
|
2014-02-08 02:52:03 -04:00
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
if (fabsf(yaw_vector.x) > 0 ||
|
|
|
|
fabsf(yaw_vector.y) > 0) {
|
|
|
|
yaw_vector.normalize();
|
|
|
|
}
|
2017-12-02 09:02:37 -04:00
|
|
|
sy = constrain_float(yaw_vector.y, -1.0f, 1.0f);
|
|
|
|
cy = constrain_float(yaw_vector.x, -1.0f, 1.0f);
|
2014-02-08 02:52:03 -04:00
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
// sanity checks
|
|
|
|
if (yaw_vector.is_inf() || yaw_vector.is_nan()) {
|
|
|
|
sy = 0.0f;
|
|
|
|
cy = 1.0f;
|
|
|
|
}
|
2017-12-03 06:10:19 -04:00
|
|
|
|
|
|
|
const float cx2 = rot.c.x * rot.c.x;
|
2014-10-24 21:21:04 -03:00
|
|
|
if (cx2 >= 1.0f) {
|
2017-02-11 04:15:34 -04:00
|
|
|
cp = 0;
|
|
|
|
cr = 1.0f;
|
2014-10-24 21:21:04 -03:00
|
|
|
} else {
|
2017-02-11 04:15:34 -04:00
|
|
|
cp = safe_sqrt(1 - cx2);
|
|
|
|
cr = rot.c.z / cp;
|
2014-10-24 21:21:04 -03:00
|
|
|
}
|
2017-12-02 09:02:37 -04:00
|
|
|
cp = constrain_float(cp, 0.0f, 1.0f);
|
|
|
|
cr = constrain_float(cr, -1.0f, 1.0f); // this relies on constrain_float() of infinity doing the right thing
|
2014-02-08 02:52:03 -04:00
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
sp = -rot.c.x;
|
2015-03-22 21:03:54 -03:00
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
if (!is_zero(cp)) {
|
|
|
|
sr = rot.c.y / cp;
|
2015-03-22 21:03:54 -03:00
|
|
|
}
|
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
if (is_zero(cp) || isinf(cr) || isnan(cr) || isinf(sr) || isnan(sr)) {
|
|
|
|
float r, p, y;
|
|
|
|
rot.to_euler(&r, &p, &y);
|
|
|
|
cr = cosf(r);
|
|
|
|
sr = sinf(r);
|
2015-03-22 21:03:54 -03:00
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
}
|
2015-03-22 21:03:54 -03:00
|
|
|
|
2017-02-11 04:15:34 -04:00
|
|
|
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
|
|
|
|
// should be called after _dcm_matrix is updated
|
2021-07-20 10:04:20 -03:00
|
|
|
void AP_AHRS_Backend::update_trig(void)
|
2017-02-11 04:15:34 -04:00
|
|
|
{
|
|
|
|
calc_trig(get_rotation_body_to_ned(),
|
|
|
|
_cos_roll, _cos_pitch, _cos_yaw,
|
|
|
|
_sin_roll, _sin_pitch, _sin_yaw);
|
2014-02-08 02:52:03 -04:00
|
|
|
}
|
2014-10-14 23:18:08 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
update the centi-degree values
|
|
|
|
*/
|
2021-07-20 10:04:20 -03:00
|
|
|
void AP_AHRS_Backend::update_cd_values(void)
|
2014-10-14 23:18:08 -03:00
|
|
|
{
|
|
|
|
roll_sensor = degrees(roll) * 100;
|
|
|
|
pitch_sensor = degrees(pitch) * 100;
|
|
|
|
yaw_sensor = degrees(yaw) * 100;
|
|
|
|
if (yaw_sensor < 0)
|
|
|
|
yaw_sensor += 36000;
|
|
|
|
}
|
2017-02-11 04:15:34 -04:00
|
|
|
|
|
|
|
/*
|
2018-11-12 00:58:54 -04:00
|
|
|
create a rotated view of AP_AHRS with optional pitch trim
|
2017-02-11 04:15:34 -04:00
|
|
|
*/
|
2018-11-12 00:58:54 -04:00
|
|
|
AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation, float pitch_trim_deg)
|
2017-02-11 04:15:34 -04:00
|
|
|
{
|
|
|
|
if (_view != nullptr) {
|
|
|
|
// can only have one
|
|
|
|
return nullptr;
|
|
|
|
}
|
2018-11-12 00:58:54 -04:00
|
|
|
_view = new AP_AHRS_View(*this, rotation, pitch_trim_deg);
|
2017-02-11 04:15:34 -04:00
|
|
|
return _view;
|
|
|
|
}
|
2017-04-09 08:17:05 -03:00
|
|
|
|
|
|
|
/*
|
|
|
|
* Update AOA and SSA estimation based on airspeed, velocity vector and wind vector
|
|
|
|
*
|
|
|
|
* Based on:
|
|
|
|
* "On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors" by
|
|
|
|
* Tor A. Johansen, Andrea Cristofaro, Kim Sorensen, Jakob M. Hansen, Thor I. Fossen
|
|
|
|
*
|
|
|
|
* "Multi-Stage Fusion Algorithm for Estimation of Aerodynamic Angles in Mini Aerial Vehicle" by
|
|
|
|
* C.Ramprasadh and Hemendra Arya
|
|
|
|
*
|
|
|
|
* "ANGLE OF ATTACK AND SIDESLIP ESTIMATION USING AN INERTIAL REFERENCE PLATFORM" by
|
|
|
|
* JOSEPH E. ZEIS, JR., CAPTAIN, USAF
|
|
|
|
*/
|
2021-07-22 08:54:35 -03:00
|
|
|
void AP_AHRS::update_AOA_SSA(void)
|
2017-04-09 08:17:05 -03:00
|
|
|
{
|
|
|
|
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
2017-12-03 06:10:19 -04:00
|
|
|
const uint32_t now = AP_HAL::millis();
|
2017-04-09 08:17:05 -03:00
|
|
|
if (now - _last_AOA_update_ms < 50) {
|
|
|
|
// don't update at more than 20Hz
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
_last_AOA_update_ms = now;
|
|
|
|
|
|
|
|
Vector3f aoa_velocity, aoa_wind;
|
|
|
|
|
|
|
|
// get velocity and wind
|
|
|
|
if (get_velocity_NED(aoa_velocity) == false) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
aoa_wind = wind_estimate();
|
|
|
|
|
|
|
|
// Rotate vectors to the body frame and calculate velocity and wind
|
|
|
|
const Matrix3f &rot = get_rotation_body_to_ned();
|
|
|
|
aoa_velocity = rot.mul_transpose(aoa_velocity);
|
|
|
|
aoa_wind = rot.mul_transpose(aoa_wind);
|
|
|
|
|
|
|
|
// calculate relative velocity in body coordinates
|
|
|
|
aoa_velocity = aoa_velocity - aoa_wind;
|
2017-12-03 06:10:19 -04:00
|
|
|
const float vel_len = aoa_velocity.length();
|
2017-04-09 08:17:05 -03:00
|
|
|
|
|
|
|
// do not calculate if speed is too low
|
|
|
|
if (vel_len < 2.0) {
|
|
|
|
_AOA = 0;
|
|
|
|
_SSA = 0;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Calculate AOA and SSA
|
|
|
|
if (aoa_velocity.x > 0) {
|
|
|
|
_AOA = degrees(atanf(aoa_velocity.z / aoa_velocity.x));
|
|
|
|
} else {
|
|
|
|
_AOA = 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
_SSA = degrees(safe_asin(aoa_velocity.y / vel_len));
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
2017-09-04 03:53:56 -03:00
|
|
|
// rotate a 2D vector from earth frame to body frame
|
2021-07-20 10:04:20 -03:00
|
|
|
Vector2f AP_AHRS_Backend::earth_to_body2D(const Vector2f &ef) const
|
2017-09-04 03:53:56 -03:00
|
|
|
{
|
|
|
|
return Vector2f(ef.x * _cos_yaw + ef.y * _sin_yaw,
|
|
|
|
-ef.x * _sin_yaw + ef.y * _cos_yaw);
|
|
|
|
}
|
|
|
|
|
|
|
|
// rotate a 2D vector from earth frame to body frame
|
2021-07-20 10:04:20 -03:00
|
|
|
Vector2f AP_AHRS_Backend::body_to_earth2D(const Vector2f &bf) const
|
2017-09-04 03:53:56 -03:00
|
|
|
{
|
|
|
|
return Vector2f(bf.x * _cos_yaw - bf.y * _sin_yaw,
|
|
|
|
bf.x * _sin_yaw + bf.y * _cos_yaw);
|
|
|
|
}
|
2018-01-01 08:03:28 -04:00
|
|
|
|
2019-02-11 04:15:24 -04:00
|
|
|
// log ahrs home and EKF origin
|
2021-07-24 20:08:28 -03:00
|
|
|
void AP_AHRS::Log_Write_Home_And_Origin()
|
2018-05-16 00:28:31 -03:00
|
|
|
{
|
2019-02-11 04:15:24 -04:00
|
|
|
AP_Logger *logger = AP_Logger::get_singleton();
|
|
|
|
if (logger == nullptr) {
|
2018-05-16 00:28:31 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
Location ekf_orig;
|
|
|
|
if (get_origin(ekf_orig)) {
|
2021-01-08 23:44:17 -04:00
|
|
|
Write_Origin(LogOriginType::ekf_origin, ekf_orig);
|
2018-05-16 00:28:31 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
if (home_is_set()) {
|
2021-01-08 23:44:17 -04:00
|
|
|
Write_Origin(LogOriginType::ahrs_home, _home);
|
2018-05-16 00:28:31 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-06-26 23:37:09 -03:00
|
|
|
// get apparent to true airspeed ratio
|
2021-07-20 10:04:20 -03:00
|
|
|
float AP_AHRS_Backend::get_EAS2TAS(void) const {
|
2019-06-26 23:37:09 -03:00
|
|
|
return AP::baro().get_EAS2TAS();
|
|
|
|
}
|
2018-05-16 00:28:31 -03:00
|
|
|
|
2020-03-06 06:11:25 -04:00
|
|
|
// return current vibration vector for primary IMU
|
2021-07-20 10:04:20 -03:00
|
|
|
Vector3f AP_AHRS_Backend::get_vibration(void) const
|
2020-03-06 06:11:25 -04:00
|
|
|
{
|
|
|
|
return AP::ins().get_vibration_levels();
|
|
|
|
}
|
|
|
|
|
2021-08-11 03:58:36 -03:00
|
|
|
void AP_AHRS::set_takeoff_expected(bool b)
|
2021-05-26 23:08:59 -03:00
|
|
|
{
|
2021-08-11 03:58:36 -03:00
|
|
|
takeoff_expected = b;
|
2021-05-26 23:08:59 -03:00
|
|
|
takeoff_expected_start_ms = AP_HAL::millis();
|
|
|
|
}
|
|
|
|
|
2021-08-11 03:58:36 -03:00
|
|
|
void AP_AHRS::set_touchdown_expected(bool b)
|
2021-05-26 23:08:59 -03:00
|
|
|
{
|
2021-08-11 03:58:36 -03:00
|
|
|
touchdown_expected = b;
|
2021-05-26 23:08:59 -03:00
|
|
|
touchdown_expected_start_ms = AP_HAL::millis();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
update takeoff/touchdown flags
|
|
|
|
*/
|
2021-08-11 03:58:36 -03:00
|
|
|
void AP_AHRS::update_flags(void)
|
2021-05-26 23:08:59 -03:00
|
|
|
{
|
|
|
|
const uint32_t timeout_ms = 1000;
|
2021-08-11 03:58:36 -03:00
|
|
|
if (takeoff_expected && AP_HAL::millis() - takeoff_expected_start_ms > timeout_ms) {
|
|
|
|
takeoff_expected = false;
|
2021-05-26 23:08:59 -03:00
|
|
|
}
|
2021-08-11 03:58:36 -03:00
|
|
|
if (touchdown_expected && AP_HAL::millis() - touchdown_expected_start_ms > timeout_ms) {
|
|
|
|
touchdown_expected = false;
|
2021-05-26 23:08:59 -03:00
|
|
|
}
|
|
|
|
}
|