Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plane: added MAV_CMD_SET_HAGL support #27529

Merged
merged 5 commits into from
Jul 16, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions ArduPlane/GCS_Mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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:
tridge marked this conversation as resolved.
Show resolved Hide resolved
plane.handle_external_hagl(packet);
return MAV_RESULT_ACCEPTED;
#endif

default:
return GCS_MAVLINK::handle_command_int_packet(packet, msg);
}
Expand Down
14 changes: 14 additions & 0 deletions ArduPlane/Plane.h
Original file line number Diff line number Diff line change
Expand Up @@ -210,6 +210,20 @@ class Plane : public AP_Vehicle {
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
Expand Down
72 changes: 72 additions & 0 deletions ArduPlane/altitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
15 changes: 4 additions & 11 deletions ArduPlane/commands_logic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
1 change: 1 addition & 0 deletions Tools/scripts/build_options.py
Original file line number Diff line number Diff line change
Expand Up @@ -344,6 +344,7 @@ def __init__(self,
Feature('MAVLink', 'MAV_MSG_SERIAL_CONTROL', 'AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'Enable handling of Serial Control mavlink messages', 0, None), # noqa
Feature('MAVLink', 'MAVLINK_MSG_MISSION_REQUEST', 'AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'Enable handling of MISSION_REQUEST mavlink messages', 0, None), # noqa
Feature('MAVLink', 'AP_MAVLINK_FTP_ENABLED', 'AP_MAVLINK_FTP_ENABLED', 'Enable MAVLink FTP Protocol', 0, None), # noqa
Feature('MAVLink', 'MAV_CMD_SEY_HAGL', 'AP_MAVLINK_MAV_CMD_SEY_HAGL_ENABLED', 'Enable MAVLink HAGL command', 0, None), # noqa

Feature('Developer', 'KILL_IMU', 'AP_INERTIALSENSOR_KILL_IMU_ENABLED', 'Allow IMUs to be disabled at runtime', 0, None),
Feature('Developer', 'CRASHCATCHER', 'AP_CRASHDUMP_ENABLED', 'Enable CrashCatcher', 0, None),
Expand Down
1 change: 1 addition & 0 deletions Tools/scripts/extract_features.py
Original file line number Diff line number Diff line change
Expand Up @@ -238,6 +238,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
('AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED', 'GCS_MAVLINK::handle_serial_control'),
('AP_MAVLINK_MSG_MISSION_REQUEST_ENABLED', 'GCS_MAVLINK::handle_mission_request\b'),
('AP_MAVLINK_FTP_ENABLED', 'GCS_MAVLINK::ftp_worker'),
('AP_MAVLINK_MAV_CMD_SEY_HAGL_ENABLED', 'Plane::get_external_HAGL'),

('AP_DRONECAN_HIMARK_SERVO_SUPPORT', 'AP_DroneCAN::SRV_send_himark'),
('AP_DRONECAN_HOBBYWING_ESC_SUPPORT', 'AP_DroneCAN::hobbywing_ESC_update'),
Expand Down
69 changes: 69 additions & 0 deletions libraries/AP_Scripting/examples/land_hagl.lua
Original file line number Diff line number Diff line change
@@ -0,0 +1,69 @@
--[[
demonstrate proving HAGL to plane code for landing
--]]

local MAV_CMD_SET_HAGL = 43005

local ROTATION_PITCH_90 = 24
local ROTATION_PITCH_270 = 25

-- for normal landing use PITCH_270, for inverted use PITCH_90
local RANGEFINDER_ORIENT = ROTATION_PITCH_270

local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}

local RNG_STATUS = { NotConnected = 0, NoData = 1, OutOfRangeLow = 2, OutOfRangeHigh = 3, Good = 4 }


--[[
create a NaN value
--]]
local function NaN()
return 0/0
end

local last_active = false

--[[
send HAGL data
--]]
local function send_HAGL()
local status = rangefinder:status_orient(RANGEFINDER_ORIENT)
if status ~= RNG_STATUS.Good then
last_active = false
return
end
local rangefinder_dist = rangefinder:distance_cm_orient(RANGEFINDER_ORIENT)*0.01
local correction = math.cos(ahrs:get_roll())*math.cos(ahrs:get_pitch())
local rangefinder_corrected = rangefinder_dist * correction
if RANGEFINDER_ORIENT == ROTATION_PITCH_90 then
rangefinder_corrected = -rangefinder_corrected
end
if rangefinder_corrected < 0 then
last_active = false
return
end

-- tell plane the height above ground level
local timeout_s = 0.2
local accuracy = NaN()
gcs:run_command_int(MAV_CMD_SET_HAGL, { p1 = rangefinder_corrected, p2 = accuracy, p3=timeout_s })

if not last_active then
last_active = true
gcs:send_text(MAV_SEVERITY.INFO, string.format("HAGL Active %.1f", rangefinder_corrected))
end

-- log it
logger:write("HAGL", "RDist,HAGL", "ff", rangefinder_dist, rangefinder_corrected)
end

local function update()
send_HAGL()
return update, 50
end

gcs:send_text(MAV_SEVERITY.INFO, "Loaded land_hagl")

return update, 1000

4 changes: 4 additions & 0 deletions libraries/GCS_MAVLink/GCS_config.h
Original file line number Diff line number Diff line change
Expand Up @@ -127,3 +127,7 @@
#ifndef AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
#define AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED (BOARD_FLASH_SIZE > 1024) && AP_INERTIALSENSOR_ENABLED
#endif

#ifndef AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED
#define AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED (BOARD_FLASH_SIZE > 1024)
#endif
Loading