AP_AHRS: move takeoff/touchdown flags to frontend
This commit is contained in:
parent
c19391d540
commit
e9c56a2b0a
@ -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
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
@ -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;
|
||||
};
|
||||
|
@ -108,9 +108,6 @@ AP_AHRS_DCM::update(bool skip_ins_update)
|
||||
update_trig();
|
||||
|
||||
backup_attitude();
|
||||
|
||||
// update takeoff/touchdown flags
|
||||
update_flags();
|
||||
}
|
||||
|
||||
/*
|
||||
|
Loading…
Reference in New Issue
Block a user