mirror of https://github.com/ArduPilot/ardupilot
AP_DAL: add takeoff and touchdown expected state
This commit is contained in:
parent
9eeec61cec
commit
f4f713a04f
|
@ -62,6 +62,8 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
|
|||
_RFRN.EAS2TAS = AP::baro().get_EAS2TAS();
|
||||
_RFRN.vehicle_class = ahrs.get_vehicle_class();
|
||||
_RFRN.fly_forward = ahrs.get_fly_forward();
|
||||
_RFRN.takeoff_expected = ahrs.get_takeoff_expected();
|
||||
_RFRN.touchdown_expected = ahrs.get_touchdown_expected();
|
||||
_RFRN.ahrs_airspeed_sensor_enabled = AP::ahrs().airspeed_sensor_enabled();
|
||||
_RFRN.available_memory = hal.util->available_memory();
|
||||
_RFRN.ahrs_trim = ahrs.get_trim();
|
||||
|
@ -99,6 +101,15 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype)
|
|||
#endif
|
||||
}
|
||||
|
||||
// for EKF usage to enable takeoff expected to true
|
||||
void AP_DAL::set_takeoff_expected()
|
||||
{
|
||||
#if !APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) && !APM_BUILD_TYPE(APM_BUILD_Replay)
|
||||
AP_AHRS &ahrs = AP::ahrs();
|
||||
ahrs.set_takeoff_expected(true);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
setup optional sensor backends
|
||||
*/
|
||||
|
|
|
@ -37,10 +37,10 @@ public:
|
|||
resetGyroBias = 0,
|
||||
resetHeightDatum = 1,
|
||||
//setInhibitGPS = 2, // removed
|
||||
setTakeoffExpected = 3,
|
||||
unsetTakeoffExpected = 4,
|
||||
setTouchdownExpected = 5,
|
||||
unsetTouchdownExpected = 6,
|
||||
//setTakeoffExpected = 3, // removed
|
||||
//unsetTakeoffExpected = 4, // removed
|
||||
//setTouchdownExpected = 5, // removed
|
||||
//unsetTouchdownExpected = 6, // removed
|
||||
//setInhibitGpsVertVelUse = 7, // removed
|
||||
//unsetInhibitGpsVertVelUse = 8, // removed
|
||||
setTerrainHgtStable = 9,
|
||||
|
@ -173,6 +173,17 @@ public:
|
|||
return _RFRN.fly_forward;
|
||||
}
|
||||
|
||||
bool get_takeoff_expected(void) const {
|
||||
return _RFRN.takeoff_expected;
|
||||
}
|
||||
|
||||
bool get_touchdown_expected(void) const {
|
||||
return _RFRN.touchdown_expected;
|
||||
}
|
||||
|
||||
// for EKF usage to enable takeoff expected to true
|
||||
void set_takeoff_expected();
|
||||
|
||||
// get ahrs trim
|
||||
const Vector3f &get_trim() const {
|
||||
return _RFRN.ahrs_trim;
|
||||
|
|
|
@ -67,6 +67,8 @@ struct log_RFRN {
|
|||
uint8_t ahrs_airspeed_sensor_enabled:1;
|
||||
uint8_t opticalflow_enabled:1;
|
||||
uint8_t wheelencoder_enabled:1;
|
||||
uint8_t takeoff_expected:1;
|
||||
uint8_t touchdown_expected:1;
|
||||
uint8_t _end;
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue