AP_AHRS: move takeoff/touchdown flags to frontend

This commit is contained in:
Peter Barker 2021-08-11 16:58:36 +10:00 committed by Andrew Tridgell
parent c19391d540
commit e9c56a2b0a
5 changed files with 39 additions and 34 deletions

View File

@ -305,6 +305,9 @@ void AP_AHRS::update(bool skip_ins_update)
update_DCM(skip_ins_update);
// update takeoff/touchdown flags
update_flags();
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
update_SITL();
#endif

View File

@ -385,6 +385,22 @@ public:
void Log_Write_Home_And_Origin();
/*
* AHRS is used as a transport for vehicle-takeoff-expected and
* vehicle-landing-expected:
*/
void set_takeoff_expected(bool b);
bool get_takeoff_expected(void) const {
return takeoff_expected;
}
void set_touchdown_expected(bool b);
bool get_touchdown_expected(void) const {
return touchdown_expected;
}
protected:
// optional view class
AP_AHRS_View *_view;
@ -498,6 +514,17 @@ private:
// frame to autopilot body frame from _trim variables
void update_trim_rotation_matrices();
/*
* AHRS is used as a transport for vehicle-takeoff-expected and
* vehicle-landing-expected:
*/
// update takeoff/touchdown flags
void update_flags();
bool takeoff_expected; // true if the vehicle is in a state that takeoff might be expected. Ground effect may be in play.
uint32_t takeoff_expected_start_ms;
bool touchdown_expected; // true if the vehicle is in a state that touchdown might be expected. Ground effect may be in play.
uint32_t touchdown_expected_start_ms;
#if HAL_NMEA_OUTPUT_ENABLED
class AP_NMEA_Output* _nmea_out;
#endif

View File

@ -325,28 +325,28 @@ Vector3f AP_AHRS_Backend::get_vibration(void) const
return AP::ins().get_vibration_levels();
}
void AP_AHRS_Backend::set_takeoff_expected(bool b)
void AP_AHRS::set_takeoff_expected(bool b)
{
_flags.takeoff_expected = b;
takeoff_expected = b;
takeoff_expected_start_ms = AP_HAL::millis();
}
void AP_AHRS_Backend::set_touchdown_expected(bool b)
void AP_AHRS::set_touchdown_expected(bool b)
{
_flags.touchdown_expected = b;
touchdown_expected = b;
touchdown_expected_start_ms = AP_HAL::millis();
}
/*
update takeoff/touchdown flags
*/
void AP_AHRS_Backend::update_flags(void)
void AP_AHRS::update_flags(void)
{
const uint32_t timeout_ms = 1000;
if (_flags.takeoff_expected && AP_HAL::millis() - takeoff_expected_start_ms > timeout_ms) {
_flags.takeoff_expected = false;
if (takeoff_expected && AP_HAL::millis() - takeoff_expected_start_ms > timeout_ms) {
takeoff_expected = false;
}
if (_flags.touchdown_expected && AP_HAL::millis() - touchdown_expected_start_ms > timeout_ms) {
_flags.touchdown_expected = false;
if (touchdown_expected && AP_HAL::millis() - touchdown_expected_start_ms > timeout_ms) {
touchdown_expected = false;
}
}

View File

@ -67,18 +67,6 @@ public:
return _flags.fly_forward;
}
void set_takeoff_expected(bool b);
bool get_takeoff_expected(void) const {
return _flags.takeoff_expected;
}
void set_touchdown_expected(bool b);
bool get_touchdown_expected(void) const {
return _flags.touchdown_expected;
}
AHRS_VehicleClass get_vehicle_class(void) const {
return _vehicle_class;
}
@ -537,8 +525,6 @@ protected:
uint8_t fly_forward : 1; // 1 if we can assume the aircraft will be flying forward on its X axis
uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed)
uint8_t wind_estimation : 1; // 1 if we should do wind estimation
uint8_t takeoff_expected : 1; // 1 if the vehicle is in a state that takeoff might be expected. Ground effect may be in play.
uint8_t touchdown_expected : 1; // 1 if the vehicle is in a state that touchdown might be expected. Ground effect may be in play.
} _flags;
// calculate sin/cos of roll/pitch/yaw from rotation
@ -553,9 +539,6 @@ protected:
// update roll_sensor, pitch_sensor and yaw_sensor
void update_cd_values(void);
// update takeoff/touchdown flags
void update_flags();
// accelerometer values in the earth frame in m/s/s
Vector3f _accel_ef[INS_MAX_INSTANCES];
Vector3f _accel_ef_blended;
@ -576,9 +559,4 @@ protected:
// which accelerometer instance is active
uint8_t _active_accel_instance;
private:
uint32_t takeoff_expected_start_ms;
uint32_t touchdown_expected_start_ms;
};

View File

@ -108,9 +108,6 @@ AP_AHRS_DCM::update(bool skip_ins_update)
update_trig();
backup_attitude();
// update takeoff/touchdown flags
update_flags();
}
/*