diff --git a/ArduPlane/AP_ExternalControl_Plane.cpp b/ArduPlane/AP_ExternalControl_Plane.cpp index c5afabf584321..47bad935faabc 100644 --- a/ArduPlane/AP_ExternalControl_Plane.cpp +++ b/ArduPlane/AP_ExternalControl_Plane.cpp @@ -19,4 +19,24 @@ bool AP_ExternalControl_Plane::set_global_position(const Location& loc) return plane.set_target_location(loc); } +bool AP_ExternalControl_Plane::set_path_position_tangent_and_curvature(const Location &position_on_path, + const Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) +{ + //! @todo(srmainwaring) consolidate duplicate code in GCS_MAVLINK_Plane::handle_set_position_target_global_int. + + // only accept position updates when in GUIDED mode. + if (!plane.control_mode->is_guided_mode()) { + return false; + } + + // update guided path + plane.mode_guided.handle_guided_path_request( + position_on_path, unit_path_tangent, path_curvature, direction_is_ccw); + + // update adjust_altitude_target immediately rather than wait for the scheduler. + plane.adjust_altitude_target(); + + return true; +} + #endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduPlane/AP_ExternalControl_Plane.h b/ArduPlane/AP_ExternalControl_Plane.h index 71308780d55fd..5c1c993cea0f5 100644 --- a/ArduPlane/AP_ExternalControl_Plane.h +++ b/ArduPlane/AP_ExternalControl_Plane.h @@ -16,6 +16,13 @@ class AP_ExternalControl_Plane : public AP_ExternalControl { Sets the target global position for a loiter point. */ bool set_global_position(const Location& loc) override WARN_IF_UNUSED; + + /* + Set the target setpoint for path guidance: the closest position on the + path, the unit tangent to the path, and path curvature and direction. + */ + bool set_path_position_tangent_and_curvature(const Location &position_on_path, + Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) override WARN_IF_UNUSED; }; #endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduPlane/GCS_MAVLink_Plane.cpp b/ArduPlane/GCS_MAVLink_Plane.cpp index 3338beff16586..0d6d297809499 100644 --- a/ArduPlane/GCS_MAVLink_Plane.cpp +++ b/ArduPlane/GCS_MAVLink_Plane.cpp @@ -1414,20 +1414,90 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess // Even though other parts of the command may be valid, reject the whole thing. return; } + const uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000; // Unexpectedly, the mask is expecting "ones" for dimensions that should // 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) - + const uint16_t alt_mask = ( + POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | + POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | + POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF; + + // bit mask for path following + const uint16_t path_mask = ( + POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | + POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF; + + bool msg_valid = true; AP_Mission::Mission_Command cmd = {0}; - - if (pos_target.type_mask & alt_mask) + + if (((pos_target.type_mask | POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF) == alt_mask) { const int32_t alt_cm = pos_target.alt * 100; cmd.content.location.set_alt_cm(alt_cm, frame); handle_change_alt_request(cmd); } + + // guided path following + if (((pos_target.type_mask | POSITION_TARGET_TYPEMASK_LAST_BYTE) ^ 0xFFFF) == 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) { + return; + } + + const Vector2f vel{pos_target.vx, pos_target.vy}; + const 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 6a3c0dd2ac40b..46159992e4869 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -357,12 +357,20 @@ 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(const Location& location_on_path, const 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; } #if AP_QUICKTUNE_ENABLED @@ -370,7 +378,14 @@ class ModeGuided : public Mode #endif 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 a97bedcd7dffb..519d2e6e8ce21 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->follow_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,35 @@ 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(const Location& location_on_path, const Vector2f& unit_path_tangent, const float path_curvature, const bool direction_is_ccw) +{ + Location location_on_path_abs_alt = location_on_path; + + // add home alt if needed + if (location_on_path_abs_alt.relative_alt) { + location_on_path_abs_alt.alt += plane.home.alt; + location_on_path_abs_alt.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 = location_on_path_abs_alt; + + _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; } diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index 259dcb51601c9..ab96262945c6b 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -9,11 +9,24 @@ #include // These are the Goal Interface constants. Because microxrceddsgen does not expose -// them in the generated code, they are manually maintained -// Position ignore flags -static constexpr uint16_t TYPE_MASK_IGNORE_LATITUDE = 1; -static constexpr uint16_t TYPE_MASK_IGNORE_LONGITUDE = 2; -static constexpr uint16_t TYPE_MASK_IGNORE_ALTITUDE = 4; +// them in the generated code, they are manually maintained. + +//! @todo(srmainwaring) make an enum class and use templates to enable as bitfield +// see: https://accu.org/journals/overload/24/132/williams_2228/ +typedef enum AP_DDS_PositionTargetTypeMask { + TYPE_MASK_IGNORE_LATITUDE = 1, + TYPE_MASK_IGNORE_LONGITUDE = 2, + TYPE_MASK_IGNORE_ALTITUDE = 4, + TYPE_MASK_IGNORE_VX = 8, + TYPE_MASK_IGNORE_VY = 16, + TYPE_MASK_IGNORE_VZ = 32, + TYPE_MASK_IGNORE_AFX = 64, + TYPE_MASK_IGNORE_AFY = 128, + TYPE_MASK_IGNORE_AFZ = 256, + TYPE_MASK_FORCE_SET = 512, + TYPE_MASK_YAW_IGNORE = 1024, + TYPE_MASK_YAW_RATE_IGNORE = 2048, +} AP_DDS_PositionTargetTypeMask; bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos) { @@ -31,19 +44,68 @@ bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_ return false; } - constexpr uint32_t MASK_POS_IGNORE = - TYPE_MASK_IGNORE_LATITUDE | - TYPE_MASK_IGNORE_LONGITUDE | - TYPE_MASK_IGNORE_ALTITUDE; + const uint16_t TYPE_MASK_LAST_BYTE = 0xF000; - if (!(cmd_pos.type_mask & MASK_POS_IGNORE)) { + // position setpoint mask + const uint16_t position_mask = + (TYPE_MASK_IGNORE_VX | TYPE_MASK_IGNORE_VY | TYPE_MASK_IGNORE_VZ | + TYPE_MASK_IGNORE_AFX | TYPE_MASK_IGNORE_AFY | TYPE_MASK_IGNORE_AFZ | + TYPE_MASK_YAW_IGNORE | TYPE_MASK_YAW_RATE_IGNORE | + TYPE_MASK_LAST_BYTE) ^ 0xFFFF; + + // path setpoint mask + const uint16_t path_mask = + (TYPE_MASK_YAW_IGNORE | TYPE_MASK_YAW_RATE_IGNORE | + TYPE_MASK_LAST_BYTE) ^ 0xFFFF; + + // position control + if (((cmd_pos.type_mask | TYPE_MASK_LAST_BYTE) ^ 0xFFFF) == position_mask) { Location loc(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame); if (!external_control->set_global_position(loc)) { return false; // Don't try sending other commands if this fails } } - // TODO add velocity and accel handling + // path guidance + if (((cmd_pos.type_mask | TYPE_MASK_LAST_BYTE) ^ 0xFFFF) == path_mask) { + Location position_on_path(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame); + + // convert velocty and acceleration setpoints from ENU to NED + Vector2f vel( + float(cmd_pos.velocity.linear.y), + float(cmd_pos.velocity.linear.x)); + Vector2f accel( + float(cmd_pos.acceleration_or_force.linear.y), + float(cmd_pos.acceleration_or_force.linear.x)); + 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; + } + } + + if (external_control->set_path_position_tangent_and_curvature( + position_on_path, unit_vel, path_curvature, dir_is_ccw)) { + return false; + } + } return true; } @@ -125,5 +187,4 @@ bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in, Locatio return true; } - #endif // AP_DDS_ENABLED \ No newline at end of file diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h index 57670008f4896..f3ef435da0465 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.h +++ b/libraries/AP_ExternalControl/AP_ExternalControl.h @@ -43,6 +43,15 @@ class AP_ExternalControl */ virtual bool disarm(AP_Arming::Method method, bool do_disarm_checks) WARN_IF_UNUSED; + /* + Set the target setpoint for path guidance: the closest position on the + path, the unit tangent to the path, and path curvature and direction. + */ + virtual bool set_path_position_tangent_and_curvature(const Location &position_on_path, + Vector2f unit_path_tangent, const float path_curvature, const bool direction_is_ccw) WARN_IF_UNUSED { + return false; + } + static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED { return singleton; } @@ -53,7 +62,6 @@ class AP_ExternalControl static AP_ExternalControl *singleton; }; - namespace AP { AP_ExternalControl *externalcontrol(); diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 4a5c83eb5d004..2c885054fa8db 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -147,6 +147,10 @@ float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const float AP_L1_Control::loiter_radius(const float radius) const { + if (_disable_loiter_radius_scaling) { + return radius; + } + // prevent an insane loiter bank limit float sanitized_bank_limit = constrain_float(_loiter_bank_limit, 0.0f, 89.0f); float lateral_accel_sea_level = tanf(radians(sanitized_bank_limit)) * GRAVITY_MSS; @@ -545,3 +549,34 @@ void AP_L1_Control::update_level_flight(void) _data_is_stale = false; // status are correctly updated with current waypoint data } + +// update L1 control for path following +void AP_L1_Control::follow_path(const Location &location_on_path, const Vector2f& unit_path_tangent, float path_curvature, int8_t direction) { + //! @note initial implementation uses existing functions + if (is_positive(path_curvature)) { + // moving along arc of circle - loiter about wp located at + // centre of curvature. + float radius_m = 1.0 / path_curvature; + auto center_wp = location_on_path; + Vector3p tangent_ned(unit_path_tangent.x, unit_path_tangent.y, 0.0); + Vector3p dn_ned(0.0, 0.0, 1.0); + auto ofs_ned = dn_ned.cross(tangent_ned) * radius_m * direction; + center_wp.offset(ofs_ned); + + // disable loiter radius scaling while updating loiter + _disable_loiter_radius_scaling = true; + update_loiter(center_wp, radius_m, direction); + _disable_loiter_radius_scaling = false; + } else { + // moving along a line segment - navigate to wp ahead of closest point + // in direction of path tangent + float ofs_m = 100.0; + Vector3p ofs_ned(ofs_m * unit_path_tangent.x, + ofs_m * unit_path_tangent.y, 0.0); + auto prev_wp = location_on_path; + auto next_wp = location_on_path; + next_wp.offset(ofs_ned); + + update_waypoint(prev_wp, next_wp); + } +} diff --git a/libraries/AP_L1_Control/AP_L1_Control.h b/libraries/AP_L1_Control/AP_L1_Control.h index 20eb6e577713c..52f917bfea74b 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.h +++ b/libraries/AP_L1_Control/AP_L1_Control.h @@ -53,6 +53,7 @@ class AP_L1_Control : public AP_Navigation { void update_loiter(const class Location ¢er_WP, float radius, int8_t loiter_direction) override; void update_heading_hold(int32_t navigation_heading_cd) override; void update_level_flight(void) override; + void follow_path(const Location &location_on_path, const Vector2f& unit_path_tangent, float path_curvature, int8_t direction) override; bool reached_loiter_target(void) override; // set the default NAVL1_PERIOD @@ -135,4 +136,7 @@ class AP_L1_Control : public AP_Navigation { bool _reverse = false; float get_yaw() const; int32_t get_yaw_sensor() const; + + // allow loiter radius scaling to be disabled (in path guidance). + bool _disable_loiter_radius_scaling; }; diff --git a/libraries/AP_Navigation/AP_Navigation.h b/libraries/AP_Navigation/AP_Navigation.h index 26d25ad19a8c8..ffae4e80b16e4 100644 --- a/libraries/AP_Navigation/AP_Navigation.h +++ b/libraries/AP_Navigation/AP_Navigation.h @@ -95,6 +95,16 @@ class AP_Navigation { // attitude/steering. virtual void update_level_flight(void) = 0; + // update the internal state of the navigation controller when + // the vehicle has been commanded with a path following setpoint, which + // includes the closest point on the path, the unit tangent to the path, + // and the curvature. This is the step function for navigation control when + // path following. This function is called at regular intervals + // (typically 10Hz). The main flight code will call an output function + // (such as nav_roll_cd()) after this function to ask for the new required + // navigation attitude/steering. + virtual void follow_path(const Location &location_on_path, const Vector2f& unit_path_tangent, float path_curvature, int8_t direction) {} + // return true if we have reached the target loiter location. This // may be a fuzzy decision, depending on internal navigation // parameters. For example the controller may return true only