ArduCopter: add Airspeed support
This commit is contained in:
parent
442cff4c60
commit
b917a71431
@ -62,6 +62,9 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
|
||||
& gcs_failsafe_check(display_failure)
|
||||
& winch_checks(display_failure)
|
||||
& alt_checks(display_failure)
|
||||
#if AP_AIRSPEED_ENABLED
|
||||
& AP_Arming::airspeed_checks(display_failure)
|
||||
#endif
|
||||
& AP_Arming::pre_arm_checks(display_failure);
|
||||
}
|
||||
|
||||
|
@ -195,6 +195,16 @@ void GCS_MAVLINK_Copter::send_nav_controller_output() const
|
||||
|
||||
float GCS_MAVLINK_Copter::vfr_hud_airspeed() const
|
||||
{
|
||||
#if AP_AIRSPEED_ENABLED
|
||||
// airspeed sensors are best. While the AHRS airspeed_estimate
|
||||
// will use an airspeed sensor, that value is constrained by the
|
||||
// ground speed. When reporting we should send the true airspeed
|
||||
// value if possible:
|
||||
if (copter.airspeed.enabled() && copter.airspeed.healthy()) {
|
||||
return copter.airspeed.get_airspeed();
|
||||
}
|
||||
#endif
|
||||
|
||||
Vector3f airspeed_vec_bf;
|
||||
if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
|
||||
// we are running the EKF3 wind estimation code which can give
|
||||
|
@ -111,6 +111,10 @@ void Copter::init_ardupilot()
|
||||
AP::compass().set_log_bit(MASK_LOG_COMPASS);
|
||||
AP::compass().init();
|
||||
|
||||
#if AP_AIRSPEED_ENABLED
|
||||
airspeed.set_log_bit(MASK_LOG_IMU);
|
||||
#endif
|
||||
|
||||
#if AC_OAPATHPLANNER_ENABLED == ENABLED
|
||||
g2.oa.init();
|
||||
#endif
|
||||
|
Loading…
Reference in New Issue
Block a user