From 916dac47b257bf0bbee25bf3e4b429dfaa2cf207 Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 26 Feb 2024 11:30:51 +0000 Subject: [PATCH] Plane: add guided path navigation - Add guided path sub-mode - Extend handling of SET_POSITION_TARGET_GLOBAL_INT to include path guidance - Check for non-zero velocity and acceleration - Calculate path curvature and direction - Force update of adjust_altitude_target. - Remove duplicate check for guided mode. Signed-off-by: Rhys Mainwaring --- ArduPlane/GCS_Mavlink.cpp | 65 +++++++++++++++++++++++++++++++++++++-- ArduPlane/mode.h | 15 +++++++++ ArduPlane/mode_guided.cpp | 40 +++++++++++++++++++++++- 3 files changed, 116 insertions(+), 4 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 24e832ee8b0a3a..01ec6266222538 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1373,10 +1373,13 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess // be IGNORNED rather than INCLUDED. See mavlink documentation of the // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field. const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3) - + + // bit mask for path following: ignore force_set, yaw, yaw_rate + const uint16_t path_mask = 0b1111111000000000; + bool msg_valid = true; AP_Mission::Mission_Command cmd = {0}; - + if (pos_target.type_mask & alt_mask) { cmd.content.location.alt = pos_target.alt * 100; @@ -1405,7 +1408,63 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess if (msg_valid) { handle_change_alt_request(cmd); } - } // end if alt_mask + } // end if alt_mask + + // guided path following + if (pos_target.type_mask & path_mask) { + cmd.content.location.lat = pos_target.lat_int; + cmd.content.location.lng = pos_target.lon_int; + + cmd.content.location.alt = pos_target.alt * 100; + cmd.content.location.relative_alt = false; + cmd.content.location.terrain_alt = false; + switch (pos_target.coordinate_frame) + { + case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + cmd.content.location.relative_alt = true; + break; + default: + gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT"); + msg_valid = false; + break; + } + + if (msg_valid) { + Vector2f vel(pos_target.vx, pos_target.vy); + Vector2f accel(pos_target.afx, pos_target.afy); + Vector2f unit_vel; + + float path_curvature{0.0}; + bool dir_is_ccw{false}; + + if (!vel.is_zero()) { + unit_vel = vel.normalized(); + + if (!accel.is_zero()) { + // curvature is determined from the acceleration normal + // to the planar velocity and the equation for uniform + // circular motion: a = v^2 / r. + float accel_proj = accel.dot(unit_vel); + Vector2f accel_lat = accel - unit_vel * accel_proj; + Vector2f accel_lat_unit = accel_lat.normalized(); + + path_curvature = accel_lat.length() / vel.length_squared(); + + // % is cross product, direction: cw:= 1, ccw:= -1 + float dir = accel_lat_unit % unit_vel; + dir_is_ccw = dir < 0.0; + } + } + + // update path guidance + plane.mode_guided.handle_guided_path_request( + cmd.content.location, unit_vel, path_curvature, dir_is_ccw); + + // update adjust_altitude_target immediately rather than wait + // for the scheduler. + plane.adjust_altitude_target(); + } + } } MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet) diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index a217c2bf7d8e18..8ba3a27f4fa1b9 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -296,17 +296,32 @@ class ModeGuided : public Mode // handle a guided target request from GCS bool handle_guided_request(Location target_loc) override; + // handle a guided path following request + bool handle_guided_path_request(Location position_on_path, Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw); + void set_radius_and_direction(const float radius, const bool direction_is_ccw); void update_target_altitude() override; protected: + enum class SubMode: uint8_t { + WP, + Path + }; + bool _enter() override; bool _pre_arm_checks(size_t buflen, char *buffer) const override { return true; } private: + + SubMode _guided_mode; + float active_radius_m; + + // used in path mode + float _path_curvature; + Vector2f _unit_path_tangent; }; class ModeCircle: public Mode diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 85462166d1bc7f..80235c3a855643 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -103,7 +103,18 @@ void ModeGuided::update() void ModeGuided::navigate() { - plane.update_loiter(active_radius_m); + switch (_guided_mode) { + case SubMode::WP: + plane.update_loiter(active_radius_m); + break; + case SubMode::Path: + plane.nav_controller->update_path(plane.next_WP_loc, + _unit_path_tangent, _path_curvature, plane.loiter.direction); + break; + default: + gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); + break; + } } bool ModeGuided::handle_guided_request(Location target_loc) @@ -116,6 +127,33 @@ bool ModeGuided::handle_guided_request(Location target_loc) plane.set_guided_WP(target_loc); + // use waypoint navigation sub-mode + _guided_mode = SubMode::WP; + + return true; +} + +bool ModeGuided::handle_guided_path_request(Location position_on_path, Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) +{ + // add home alt if needed + if (position_on_path.relative_alt) { + position_on_path.alt += plane.home.alt; + position_on_path.relative_alt = 0; + } + + // copy the current location into the OldWP slot + plane.prev_WP_loc = plane.current_loc; + + // load the next_WP slot + plane.next_WP_loc = position_on_path; + + _unit_path_tangent = unit_path_tangent; + _path_curvature = path_curvature; + plane.loiter.direction = direction_is_ccw ? -1 : 1; + + // use path navigation sub-mode + _guided_mode = SubMode::Path; + return true; }