Plane: support MAV_CMD_SET_HAGL

allow for external height above ground estimate
This commit is contained in:
Andrew Tridgell 2024-07-11 11:33:01 +10:00
parent fd6fe52dc7
commit d4f9843e55
4 changed files with 96 additions and 11 deletions

View File

@ -1083,6 +1083,12 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
case MAV_CMD_SET_HAGL:
plane.handle_external_hagl(packet);
return MAV_RESULT_ACCEPTED;
#endif
default:
return GCS_MAVLINK::handle_command_int_packet(packet, msg);
}

View File

@ -210,6 +210,20 @@ private:
AP_FixedWing::Rangefinder_State rangefinder_state;
#endif
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
struct {
// allow for external height above ground estimate
float hagl;
uint32_t last_update_ms;
uint32_t timeout_ms;
} external_hagl;
bool get_external_HAGL(float &height_agl);
void handle_external_hagl(const mavlink_command_int_t &packet);
#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
float get_landing_height(bool &using_rangefinder);
#if AP_RPM_ENABLED
AP_RPM rpm_sensor;
#endif

View File

@ -115,6 +115,14 @@ int32_t Plane::get_RTL_altitude_cm() const
*/
float Plane::relative_ground_altitude(bool use_rangefinder_if_available, bool use_terrain_if_available)
{
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
float height_AGL;
// use external HAGL if available
if (get_external_HAGL(height_AGL)) {
return height_AGL;
}
#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
#if AP_RANGEFINDER_ENABLED
if (use_rangefinder_if_available && rangefinder_state.in_range) {
return rangefinder_state.height_estimate;
@ -810,3 +818,67 @@ bool Plane::terrain_enabled_in_mode(Mode::Number num) const
return false;
}
#endif
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
/*
handle a MAV_CMD_SET_HAGL request. The accuracy is ignored
*/
void Plane::handle_external_hagl(const mavlink_command_int_t &packet)
{
auto &hagl = plane.external_hagl;
hagl.hagl = packet.param1;
hagl.last_update_ms = AP_HAL::millis();
hagl.timeout_ms = uint32_t(packet.param3 * 1000);
}
/*
get HAGL from external source if current
*/
bool Plane::get_external_HAGL(float &height_agl)
{
auto &hagl = plane.external_hagl;
if (hagl.last_update_ms != 0) {
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - hagl.last_update_ms <= hagl.timeout_ms) {
height_agl = hagl.hagl;
return true;
}
hagl.last_update_ms = 0;
}
return false;
}
#endif // AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
/*
get height for landing. Set using_rangefinder to true if a rangefinder
or external HAGL source is active
*/
float Plane::get_landing_height(bool &rangefinder_active)
{
float height;
#if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
// if external HAGL is active use that
if (get_external_HAGL(height)) {
// ensure no terrain correction is applied - that is the job
// of the external system if it is wanted
auto_state.terrain_correction = 0;
// an external HAGL is considered to be a type of rangefinder
rangefinder_active = true;
return height;
}
#endif
// get basic height above target
height = height_above_target();
rangefinder_active = false;
#if AP_RANGEFINDER_ENABLED
// possibly correct with rangefinder
height -= rangefinder_correction();
rangefinder_active = g.rangefinder_landing && rangefinder_state.in_range;
#endif
return height;
}

View File

@ -255,23 +255,16 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
} else {
// use rangefinder to correct if possible
#if AP_RANGEFINDER_ENABLED
float height = height_above_target() - rangefinder_correction();
#else
float height = height_above_target();
#endif
bool rangefinder_active = false;
float height = plane.get_landing_height(rangefinder_active);
// for flare calculations we don't want to use the terrain
// correction as otherwise we will flare early on rising
// ground
height -= auto_state.terrain_correction;
return landing.verify_land(prev_WP_loc, next_WP_loc, current_loc,
height, auto_state.sink_rate, auto_state.wp_proportion, auto_state.last_flying_ms, arming.is_armed(), is_flying(),
#if AP_RANGEFINDER_ENABLED
g.rangefinder_landing && rangefinder_state.in_range
#else
false
#endif
);
rangefinder_active);
}
case MAV_CMD_NAV_LOITER_UNLIM: