From d60a5ee05f1aa05e21329b90542be1a86d6b1fdb Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 15 Sep 2024 12:37:55 +0930 Subject: [PATCH] All squashed --- ArduCopter/Copter.cpp | 38 ++ ArduCopter/Copter.h | 3 + ArduCopter/mode.h | 4 + ArduCopter/mode_auto.cpp | 66 +-- ArduSub/mode_auto.cpp | 4 +- .../AC_AttitudeControl/AC_PosControl.cpp | 187 +++++++-- libraries/AC_AttitudeControl/AC_PosControl.h | 53 ++- libraries/AC_WPNav/AC_Circle.cpp | 35 +- libraries/AC_WPNav/AC_Circle.h | 4 +- libraries/AC_WPNav/AC_Loiter.cpp | 2 + libraries/AC_WPNav/AC_WPNav.cpp | 16 +- .../applets/copter-slung-payload.lua | 396 ++++++++++++++++++ .../applets/copter-slung-payload.md | 30 ++ libraries/AP_Scripting/docs/docs.lua | 19 + .../examples/copter-posoffset.lua | 103 +++++ .../generator/description/bindings.desc | 3 + .../mavlink_msg_GLOBAL_POSITION_INT.lua | 14 + .../modules/MAVLink/mavlink_msg_HEARTBEAT.lua | 11 + .../modules/MAVLink/mavlink_msgs.lua | Bin 2853 -> 3114 bytes libraries/AP_Vehicle/AP_Vehicle.h | 7 + 20 files changed, 903 insertions(+), 92 deletions(-) create mode 100644 libraries/AP_Scripting/applets/copter-slung-payload.lua create mode 100644 libraries/AP_Scripting/applets/copter-slung-payload.md create mode 100644 libraries/AP_Scripting/examples/copter-posoffset.lua create mode 100644 libraries/AP_Scripting/modules/MAVLink/mavlink_msg_GLOBAL_POSITION_INT.lua create mode 100644 libraries/AP_Scripting/modules/MAVLink/mavlink_msg_HEARTBEAT.lua diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a5aace188f4ba..7569a75519983 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -413,6 +413,44 @@ bool Copter::set_desired_speed(float speed) } #if MODE_AUTO_ENABLED +// add an additional offset to vehicle's target position, velocity and acceleration +// units are m, m/s and m/s/s in NED frame +// Z-axis is not currently supported and is ignored +bool Copter::set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) +{ + // only supported in Auto mode + if (flightmode != &mode_auto) { + return false; + } + + pos_control->set_posvelaccel_offset_target_xy_cm(pos_offset_NED.topostype().xy() * 100.0, vel_offset_NED.xy() * 100.0, accel_offset_NED.xy() * 100.0); + return true; +} + +// add an additional offset to vehicle's target velocity and acceleration +// units are m/s and m/s/s in NED frame +// Z-axis is not currently supported and is ignored +bool Copter::set_velaccel_offset(const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) +{ + // only supported in Auto mode + if (flightmode != &mode_auto) { + return false; + } + + pos_control->set_velaccel_offset_target_xy_cms(vel_offset_NED.xy() * 100.0, accel_offset_NED.xy() * 100.0); + return true; +} + +// get position and velocity offset to vehicle's target velocity and acceleration +// units are m and m/s in NED frame +bool Copter::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED) +{ + pos_offset_NED = pos_control->get_pos_offset_cm().tofloat() * 0.01; + vel_offset_NED = pos_control->get_vel_offset_cms() * 0.01; + accel_offset_NED = pos_control->get_accel_offset_cmss() * 0.01; + return true; +} + // returns true if mode supports NAV_SCRIPT_TIME mission commands bool Copter::nav_scripting_enable(uint8_t mode) { diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8a9822d165179..97b083dd228c5 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -688,6 +688,9 @@ class Copter : public AP_Vehicle { #endif bool set_desired_speed(float speed) override; #if MODE_AUTO_ENABLED + bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) override; + bool set_velaccel_offset(const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) override; + bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED) override; bool nav_scripting_enable(uint8_t mode) override; bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4) override; void nav_script_time_done(uint16_t id) override; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 77d72c7073bdc..04777e1cfd6f5 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -631,6 +631,10 @@ class ModeAuto : public Mode { bool shift_alt_to_current_alt(Location& target_loc) const; + // subtract position controller offsets from target location + // should be used when the location will be used as a target for the position controller + void subtract_pos_offsets(Location& target_loc) const; + void do_takeoff(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index d95687bf12df2..80c94f3dbea9c 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -498,7 +498,7 @@ void ModeAuto::land_start() set_submode(SubMode::LAND); } -// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location +// circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location // we assume the caller has performed all required GPS_ok checks void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn) { @@ -517,8 +517,8 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi // check our distance from edge of circle Vector3f circle_edge_neu; - copter.circle_nav->get_closest_point_on_circle(circle_edge_neu); - float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length(); + float dist_to_edge; + copter.circle_nav->get_closest_point_on_circle(circle_edge_neu, dist_to_edge); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { @@ -1488,6 +1488,14 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const return true; } +// subtract position controller offsets from target location +// should be used when the location will be used as a target for the position controller +void ModeAuto::subtract_pos_offsets(Location& target_loc) const +{ + // subtract position controller offsets from target location + target_loc.offset(-pos_control->get_pos_offset_cm() * 0.01); +} + /********************************************************************************/ // Nav (Must) commands /********************************************************************************/ @@ -1528,6 +1536,10 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) { // calculate default location used when lat, lon or alt is zero Location default_loc = copter.current_loc; + + // subtract position offsets + subtract_pos_offsets(default_loc); + if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { if (!wp_nav->get_wp_destination_loc(default_loc)) { // this should never happen @@ -1648,33 +1660,23 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) // note: caller should set yaw_mode void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) { - // convert back to location - Location target_loc(cmd.content.location); + // calculate default location used when lat, lon or alt is zero + Location default_loc = copter.current_loc; - // use current location if not provided - if (target_loc.lat == 0 && target_loc.lng == 0) { - // To-Do: make this simpler - Vector3f temp_pos; - copter.wp_nav->get_wp_stopping_point_xy(temp_pos.xy()); - const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN); - target_loc.lat = temp_loc.lat; - target_loc.lng = temp_loc.lng; - } - - // use current altitude if not provided - // To-Do: use z-axis stopping point instead of current alt - if (target_loc.alt == 0) { - // set to current altitude but in command's alt frame - int32_t curr_alt; - if (copter.current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) { - target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame()); - } else { - // default to current altitude as alt-above-home - target_loc.set_alt_cm(copter.current_loc.alt, - copter.current_loc.get_alt_frame()); + // subtract position offsets + subtract_pos_offsets(default_loc); + + // use previous waypoint destination as default if available + if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { + if (!wp_nav->get_wp_destination_loc(default_loc)) { + // this should never happen + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); } } + // get waypoint's location from command and send to wp_nav + const Location target_loc = loc_from_cmd(cmd, default_loc); + // start way point navigator and provide it the desired location if (!wp_start(target_loc)) { // failure to set next destination can only be because of missing terrain data @@ -1686,7 +1688,13 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) // do_circle - initiate moving in a circle void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) { - const Location circle_center = loc_from_cmd(cmd, copter.current_loc); + // calculate default location used when lat, lon or alt is zero + Location default_loc = copter.current_loc; + + // subtract position offsets + subtract_pos_offsets(default_loc); + + const Location circle_center = loc_from_cmd(cmd, default_loc); // calculate radius uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 @@ -1754,6 +1762,10 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd) { // calculate default location used when lat, lon or alt is zero Location default_loc = copter.current_loc; + + // subtract position offsets + subtract_pos_offsets(default_loc); + if (wp_nav->is_active() && wp_nav->reached_wp_destination()) { if (!wp_nav->get_wp_destination_loc(default_loc)) { // this should never happen diff --git a/ArduSub/mode_auto.cpp b/ArduSub/mode_auto.cpp index d1fdc42f2aba7..67f9e4d78264d 100644 --- a/ArduSub/mode_auto.cpp +++ b/ArduSub/mode_auto.cpp @@ -187,8 +187,8 @@ void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float // check our distance from edge of circle Vector3f circle_edge_neu; - sub.circle_nav.get_closest_point_on_circle(circle_edge_neu); - float dist_to_edge = (inertial_nav.get_position_neu_cm() - circle_edge_neu).length(); + float dist_to_edge; + sub.circle_nav.get_closest_point_on_circle(circle_edge_neu, dist_to_edge); // if more than 3m then fly to edge if (dist_to_edge > 300.0f) { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 452680bdc1a07..7afcf1eb1da38 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -74,6 +74,9 @@ extern const AP_HAL::HAL& hal; #define POSCONTROL_VIBE_COMP_P_GAIN 0.250f #define POSCONTROL_VIBE_COMP_I_GAIN 0.125f +// velocity offset targets timeout if not updated within 3 seconds +#define POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS 3000 + const AP_Param::GroupInfo AC_PosControl::var_info[] = { // 0 was used for HOVER @@ -363,10 +366,8 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float // Terrain following velocity scalar must be calculated before we remove the position offset const float offset_z_scaler = pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer); - // remove terrain offsets for flat earth assumption - _pos_target.z -= _pos_offset_z; - _vel_desired.z -= _vel_offset_z; - _accel_desired.z -= _accel_offset_z; + // remove offsets including terrain offset for flat earth assumption + remove_offsets_xy(); // calculated increased maximum acceleration and jerk if over speed const float overspeed_gain = calculate_overspeed_gain(); @@ -406,13 +407,12 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float -constrain_float(accel_max_z_cmss, 0.0f, 750.0f), accel_max_z_cmss, jerk_max_z_cmsss, _dt, false); - // update the vertical position, velocity and acceleration offsets + // update the position, velocity and acceleration offsets + update_xy_offsets(); update_pos_offset_z(pos_offset_z); - // add terrain offsets - _pos_target.z += _pos_offset_z; - _vel_desired.z += _vel_offset_z; - _accel_desired.z += _accel_offset_z; + // add back offsets including terrain offset + add_offsets_xy(); } @@ -422,7 +422,7 @@ float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_ if (is_zero(pos_offset_z_buffer)) { return 1.0; } - float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z - _pos_offset_z + pos_offset_z); + float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z - _pos_offset.z + pos_offset_z); return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0); } @@ -542,6 +542,10 @@ void AC_PosControl::init_xy_controller() // initialise ekf xy reset handler init_ekf_xy_reset(); + // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated. + update_xy_offsets(); + set_posvelaccel_offsets_xy_cm(_pos_offset_target.xy(), _vel_offset_target, _accel_offset_target); + // initialise z_controller time out _last_update_xy_ticks = AP::scheduler().ticks32(); } @@ -556,8 +560,17 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel) // check for ekf xy position reset handle_ekf_xy_reset(); + // remove offsets + remove_offsets_xy(); + update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error()); shape_accel_xy(accel, _accel_desired, _jerk_max_xy_cmsss, _dt); + + // update the position, velocity and acceleration offsets + update_xy_offsets(); + + // add back offsets + add_offsets_xy(); } /// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. @@ -567,12 +580,21 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel) /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output) { + // remove offsets including terrain offset for flat earth assumption + remove_offsets_xy(); + update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error()); shape_vel_accel_xy(vel, accel, _vel_desired.xy(), _accel_desired.xy(), _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output); update_vel_accel_xy(vel, accel, _dt, Vector2f(), Vector2f()); + + // update the position, velocity and acceleration offsets + update_xy_offsets(); + + // add back offsets including terrain offset + add_offsets_xy(); } /// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. @@ -582,12 +604,63 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, boo /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output) { + // remove offsets including terrain offset for flat earth assumption + remove_offsets_xy(); + update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error()); shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output); update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f(), Vector2f(), Vector2f()); + + // update the position, velocity and acceleration offsets + update_xy_offsets(); + + // add back offsets including terrain offset + add_offsets_xy(); +} + +/// update the horizontal position and velocity offsets +/// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target, _vel_offset_target, _accel_offset_target) +void AC_PosControl::update_xy_offsets() +{ + // return immediately if offsets have never been set + if (_xy_offset_type == XYOffsetType::NONE) { + return; + } + + // check for offset target timeout + uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _posvelaccel_offset_target_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { + _xy_offset_type = XYOffsetType::POS_VEL_ACCEL; + _pos_offset_target.zero(); + _vel_offset_target.zero(); + _accel_offset_target.zero(); + } + + // update position, velocity, accel offsets for this iteration + update_pos_vel_accel_xy(_pos_offset.xy(), _vel_offset.xy(), _accel_offset.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error()); + + switch (_xy_offset_type) { + + case XYOffsetType::NONE: + break; + + case XYOffsetType::POS_VEL_ACCEL: + // input shape horizontal position, velocity and acceleration offsets + shape_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target, _accel_offset_target, + _pos_offset.xy(), _vel_offset.xy(), _accel_offset.xy(), + _vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false); + break; + + case XYOffsetType::VEL_ACCEL: + // input shape horizontal velocity and acceleration offsets + shape_vel_accel_xy(_vel_offset_target, _accel_offset_target, + _vel_offset.xy(), _accel_offset.xy(), + _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false); + break; + } } /// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system @@ -804,10 +877,10 @@ void AC_PosControl::init_z_controller() _pid_accel_z.reset_filter(); // initialise vertical offsets - _pos_offset_target_z = 0.0; - _pos_offset_z = 0.0; - _vel_offset_z = 0.0; - _accel_offset_z = 0.0; + _pos_offset_target.z = 0.0; + _pos_offset.z = 0.0; + _vel_offset.z = 0.0; + _accel_offset.z = 0.0; // Set accel PID I term based on the current throttle // Remove the expected P term due to _accel_desired.z being constrained to _accel_max_z_cmss @@ -864,20 +937,20 @@ void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool limit_output void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel) { // remove terrain offsets for flat earth assumption - _pos_target.z -= _pos_offset_z; - _vel_desired.z -= _vel_offset_z; - _accel_desired.z -= _accel_offset_z; + _pos_target.z -= _pos_offset.z; + _vel_desired.z -= _vel_offset.z; + _accel_desired.z -= _accel_offset.z; float vel_temp = vel; input_vel_accel_z(vel_temp, 0.0); // update the vertical position, velocity and acceleration offsets - update_pos_offset_z(_pos_offset_target_z); + update_pos_offset_z(_pos_offset_target.z); // add terrain offsets - _pos_target.z += _pos_offset_z; - _vel_desired.z += _vel_offset_z; - _accel_desired.z += _accel_offset_z; + _pos_target.z += _pos_offset.z; + _vel_desired.z += _vel_offset.z; + _accel_desired.z += _accel_offset.z; } /// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s @@ -930,13 +1003,13 @@ void AC_PosControl::set_alt_target_with_slew(float pos) /// update_pos_offset_z - updates the vertical offsets used by terrain following void AC_PosControl::update_pos_offset_z(float pos_offset_z) { - postype_t p_offset_z = _pos_offset_z; - update_pos_vel_accel(p_offset_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error()); - _pos_offset_z = p_offset_z; + postype_t p_offset_z = _pos_offset.z; + update_pos_vel_accel(p_offset_z, _vel_offset.z, _accel_offset.z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error()); + _pos_offset.z = p_offset_z; // input shape the terrain offset shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f, - _pos_offset_z, _vel_offset_z, _accel_offset_z, + _pos_offset.z, _vel_offset.z, _accel_offset.z, get_max_speed_down_cms(), get_max_speed_up_cms(), -get_max_accel_z_cmss(), get_max_accel_z_cmss(), _jerk_max_z_cmsss, _dt, false); @@ -1079,6 +1152,67 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c }; } +/// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame +/// this is used to initiate the offsets when initialise the position controller or do an offset reset +void AC_PosControl::set_posvelaccel_offsets_xy_cm(const Vector2p& pos_offset_xy_cm, const Vector2f& vel_offset_xy_cms, const Vector2f& accel_offset_xy_cmss) +{ + // set position offset to target + _pos_offset.xy() = pos_offset_xy_cm; + + // set velocity offset to target + _vel_offset.xy() = vel_offset_xy_cms; + + // set acceleration offset to target + _accel_offset.xy() = accel_offset_xy_cmss; +} + +void AC_PosControl::remove_offsets_xy() +{ + // remove offsets + _pos_target.xy() -= _pos_offset.xy(); + _vel_desired.xy() -= _vel_offset.xy(); + _accel_desired.xy() -= _accel_offset.xy(); +} + +void AC_PosControl::add_offsets_xy() +{ + // add back offsets + _pos_target.xy() += _pos_offset.xy(); + _vel_desired.xy() += _vel_offset.xy(); + _accel_desired.xy() += _accel_offset.xy(); +} + +/// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame +/// these must be set every 3 seconds (or less) or they will timeout and return to zero +void AC_PosControl::set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss) +{ + // set position offset target + _xy_offset_type = XYOffsetType::POS_VEL_ACCEL; + _pos_offset_target.xy() = pos_offset_target_xy_cm; + + // set velocity offset target and limit to 1/2 maximum + _vel_offset_target = vel_offset_target_xy_cms; + _vel_offset_target.limit_length(get_max_speed_xy_cms() * 0.5); + + // set acceleration offset target and limit to 1/2 maximum + _accel_offset_target = accel_offset_target_xy_cmss; + _accel_offset_target.limit_length(get_max_accel_xy_cmss() * 0.5); + + // record time of update so we can detect timeouts + _posvelaccel_offset_target_ms = AP_HAL::millis(); +} + +/// set the horizontal velocity and acceleration offset targets in cm/s and cm/s/s in NE frame +/// these must be set every 3 seconds (or less) or they will timeout and return to zero +void AC_PosControl::set_velaccel_offset_target_xy_cms(const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss) +{ + // re-use set_posvelaccel_offset_target_xy_cm but with zero position offset target + set_posvelaccel_offset_target_xy_cm(Vector2p(), vel_offset_target_xy_cms, accel_offset_target_xy_cmss); + + // override offset type + _xy_offset_type = XYOffsetType::VEL_ACCEL; +} + // returns the NED target acceleration vector for attitude control Vector3f AC_PosControl::get_thrust_vector() const { @@ -1091,10 +1225,12 @@ Vector3f AC_PosControl::get_thrust_vector() const /// function does not change the z axis void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const { + // todo: we should use the current target position and velocity if we are currently running the position controller stopping_point = _inav.get_position_xy_cm().topostype(); float kP = _p_pos_xy.kP(); Vector2f curr_vel = _inav.get_velocity_xy_cms(); + curr_vel -= _vel_offset.xy(); // calculate current velocity float vel_total = curr_vel.length(); @@ -1111,6 +1247,7 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const // convert the stopping distance into a stopping point using velocity vector const float t = stopping_dist / vel_total; stopping_point += (curr_vel * t).topostype(); + stopping_point -= _pos_offset.xy(); } /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 0681900efad95..3555e6ee72684 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -129,6 +129,10 @@ class AC_PosControl /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output = true); + /// update the horizontal position and velocity offsets + /// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target or _vel_offset_target) + void update_xy_offsets(); + // is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times bool is_active_xy() const; @@ -310,21 +314,37 @@ class AC_PosControl /// Offset + /// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame + /// this is used to initiate the offsets when initialise the position controller or do an offset reset + void set_posvelaccel_offsets_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss); + void remove_offsets_xy(); + void add_offsets_xy(); + + /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame + /// these must be set every 3 seconds (or less) or they will timeout and return to zero + void set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss); + + /// set the horizontal velocity and acceleration offset targets in cm/s and cm/s/s in NE frame + /// these must be set every 3 seconds (or less) or they will timeout and return to zero + void set_velaccel_offset_target_xy_cms(const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss); + + /// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame + const Vector3p& get_pos_offset_target_cm() const { return _pos_offset_target; } + const Vector3p& get_pos_offset_cm() const { return _pos_offset; } + const Vector3f& get_vel_offset_cms() const { return _vel_offset; } + const Vector3f& get_accel_offset_cmss() const { return _accel_offset; } + /// set_pos_offset_target_z_cm - set altitude offset target in cm above the EKF origin - void set_pos_offset_target_z_cm(float pos_offset_target_z) { _pos_offset_target_z = pos_offset_target_z; } + void set_pos_offset_target_z_cm(float pos_offset_target_z) { _pos_offset_target.z = pos_offset_target_z; } /// set_pos_offset_z_cm - set altitude offset in cm above the EKF origin - void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; } + void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset.z = pos_offset_z; } /// get_pos_offset_z_cm - returns altitude offset in cm above the EKF origin - float get_pos_offset_z_cm() const { return _pos_offset_z; } + float get_pos_offset_z_cm() const { return _pos_offset.z; } /// get_vel_offset_z_cm - returns current vertical offset speed in cm/s - float get_vel_offset_z_cms() const { return _vel_offset_z; } - - /// get_accel_offset_z_cm - returns current vertical offset acceleration in cm/s/s - float get_accel_offset_z_cmss() const { return _accel_offset_z; } - + float get_vel_offset_z_cms() const { return _vel_offset.z; } /// Outputs @@ -481,10 +501,19 @@ class AC_PosControl bool _fwd_pitch_is_limited; // true when the forward pitch demand is being limited to meet acceleration limits - float _pos_offset_target_z; // vertical position offset target, frame NEU in cm relative to the EKF origin - float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin - float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step - float _accel_offset_z; // vertical acceleration offset in NEU cm/s/s + // offset handling variables + enum class XYOffsetType { + NONE = 0, // offset target have never been set + POS_VEL_ACCEL, // position and (optionally) velocity and acceleration offset targets have been set + VEL_ACCEL // velocity and (optionally) acceleration offset targets have been set + } _xy_offset_type; // type of offset being applied. position OR velocity offsets are supported but not both at the same time + Vector3p _pos_offset_target; // position offset target in cm relative to the EKF origin in NEU frame + Vector3p _pos_offset; // position offset in cm from the EKF origin in NEU frame. this offset moves towards _pos_offset_target + Vector2f _vel_offset_target; // velocity offset target in cm/s in NEU frame + Vector3f _vel_offset; // velocity offset in NEU cm/s calculated by pos_to_rate step. this offset moves towards _vel_offset_target + Vector2f _accel_offset_target; // acceleration offset target in cm/s/s in NEU frame + Vector3f _accel_offset; // acceleration offset in NEU cm/s/s + uint32_t _posvelaccel_offset_target_ms; // system time that pos, vel, accel targets were set (used to implement timeouts) // ekf reset handling uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index d5ed82397b62b..bcc70ca67a1c4 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms) target.y += - _radius * sinf(-_angle); // heading is from vehicle to center of circle - _yaw = get_bearing_cd(_inav.get_position_xy_cm(), _center.tofloat().xy()); + _yaw = get_bearing_cd(_inav.get_position_xy_cm() - _pos_control.get_pos_offset_cm().xy().tofloat(), _center.tofloat().xy()); if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) { _yaw += is_positive(_rate)?-9000.0f:9000.0f; @@ -234,27 +234,28 @@ bool AC_Circle::update(float climb_rate_cms) // get_closest_point_on_circle - returns closest point on the circle // circle's center should already have been set -// closest point on the circle will be placed in result +// closest point on the circle will be placed in result, dist_cm will be updated with the 3D distance to the center // result's altitude (i.e. z) will be set to the circle_center's altitude // if vehicle is at the center of the circle, the edge directly behind vehicle will be returned -void AC_Circle::get_closest_point_on_circle(Vector3f &result) const +void AC_Circle::get_closest_point_on_circle(Vector3f& result, float& dist_cm) const { + // get current position + Vector3p stopping_point; + _pos_control.get_stopping_point_xy_cm(stopping_point.xy()); + _pos_control.get_stopping_point_z_cm(stopping_point.z); + + // calc vector from stopping point to circle center + Vector3f vec = (stopping_point - _center).tofloat(); + dist_cm = vec.length(); + // return center if radius is zero if (_radius <= 0) { result = _center.tofloat(); return; } - // get current position - Vector2p stopping_point; - _pos_control.get_stopping_point_xy_cm(stopping_point); - - // calc vector from stopping point to circle center - Vector2f vec = (stopping_point - _center.xy()).tofloat(); - float dist = vec.length(); - // if current location is exactly at the center of the circle return edge directly behind vehicle - if (is_zero(dist)) { + if (is_zero(dist_cm)) { result.x = _center.x - _radius * _ahrs.cos_yaw(); result.y = _center.y - _radius * _ahrs.sin_yaw(); result.z = _center.z; @@ -262,8 +263,8 @@ void AC_Circle::get_closest_point_on_circle(Vector3f &result) const } // calculate closest point on edge of circle - result.x = _center.x + vec.x / dist * _radius; - result.y = _center.y + vec.y / dist * _radius; + result.x = _center.x + vec.x / dist_cm * _radius; + result.y = _center.y + vec.y / dist_cm * _radius; result.z = _center.z; } @@ -313,12 +314,12 @@ void AC_Circle::init_start_angle(bool use_heading) _angle = wrap_PI(_ahrs.yaw-M_PI); } else { // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading) - const Vector3f &curr_pos = _inav.get_position_neu_cm(); - if (is_equal(curr_pos.x,float(_center.x)) && is_equal(curr_pos.y,float(_center.y))) { + const Vector3f &curr_pos_minus_offset = _inav.get_position_neu_cm() - _pos_control.get_pos_offset_cm().tofloat(); + if (is_equal(curr_pos_minus_offset.x,float(_center.x)) && is_equal(curr_pos_minus_offset.y,float(_center.y))) { _angle = wrap_PI(_ahrs.yaw-M_PI); } else { // get bearing from circle center to vehicle in radians - float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x); + float bearing_rad = atan2f(curr_pos_minus_offset.y-_center.y, curr_pos_minus_offset.x-_center.x); _angle = wrap_PI(bearing_rad); } } diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index d76fcc90ce64e..a03b23e178816 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -75,10 +75,10 @@ class AC_Circle // get_closest_point_on_circle - returns closest point on the circle // circle's center should already have been set - // closest point on the circle will be placed in result + // closest point on the circle will be placed in result, dist_cm will be updated with the distance to the center // result's altitude (i.e. z) will be set to the circle_center's altitude // if vehicle is at the center of the circle, the edge directly behind vehicle will be returned - void get_closest_point_on_circle(Vector3f &result) const; + void get_closest_point_on_circle(Vector3f& result, float& dist_cm) const; /// get horizontal distance to loiter target in cm float get_distance_to_target() const { return _pos_control.get_pos_error_xy_cm(); } diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index aab6fa27f3ea1..94aaae8b5914f 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -189,7 +189,9 @@ float AC_Loiter::get_angle_max_cd() const /// run the loiter controller void AC_Loiter::update(bool avoidance_on) { + _pos_control.remove_offsets_xy(); calc_desired_velocity(avoidance_on); + _pos_control.add_offsets_xy(); _pos_control.update_xy_controller(); } diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 3504e90f1f06b..2011272ad8a28 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -465,11 +465,16 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) } const float offset_z_scaler = _pos_control.pos_offset_z_scaler(terr_offset, get_terrain_margin() * 100.0); - // input shape the terrain offset + // input shape the horizontal and terrain offsets + _pos_control.update_xy_offsets(); _pos_control.update_pos_offset_z(terr_offset); + // get position controller's position offset (post input shaping) so it can be used in position error calculation + const Vector3p& psc_pos_offset_target = _pos_control.get_pos_offset_target_cm(); + // get current position and adjust altitude to origin and destination's frame (i.e. _frame) - const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset}; + // question: shouldn't this be a Vector3p + const Vector3f &curr_pos = _inav.get_position_neu_cm() - psc_pos_offset_target.tofloat(); Vector3f curr_target_vel = _pos_control.get_vel_desired_cms(); curr_target_vel.z -= _pos_control.get_vel_offset_z_cms(); @@ -504,6 +509,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc); // target position, velocity and acceleration from straight line or spline calculators +// question: doesn't target_pos need to be Vector3p Vector3f target_pos, target_vel, target_accel; bool s_finished; @@ -528,13 +534,9 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) target_accel *= sq(vel_scaler_dt); target_accel += accel_offset; - // convert final_target.z to altitude above the ekf origin - target_pos.z += _pos_control.get_pos_offset_z_cm(); - target_vel.z += _pos_control.get_vel_offset_z_cms(); - target_accel.z += _pos_control.get_accel_offset_z_cmss(); - // pass new target to the position controller _pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel); + _pos_control.add_offsets_xy(); // check if we've reached the waypoint if (!_flags.reached_destination) { diff --git a/libraries/AP_Scripting/applets/copter-slung-payload.lua b/libraries/AP_Scripting/applets/copter-slung-payload.lua new file mode 100644 index 0000000000000..d60a560e8c00a --- /dev/null +++ b/libraries/AP_Scripting/applets/copter-slung-payload.lua @@ -0,0 +1,396 @@ +-- Move a Copter so as to reduce a slung payload's oscillation. Requires the payload be capable of sending its position and velocity to the main vehicle +-- +-- How To Use +-- 1. copy this script to the autopilot's "scripts" directory +-- 2. within the "scripts" directory create a "modules" directory +-- 3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory +-- 4. add an autopilot and GPS to the payload and configure it to send GLOBAL_POSITION_INT messages at 10hz to the vehicle +-- 5. create a mission with a SCRIPT_TIME or PAYLOAD_PLACE command included +-- 6. fly the mission and the vehicle should move so as to reduce the payload's oscillations while executing the SCRIPT_TIME or PAYLOAD_PLACE commands +-- 7. optionally set SLUP_SYSID to the system id of the payload autopilot +-- 8. optionally set WP_YAW_BEHAVIOR to 0 to prevent the vehicle from yawing while moving to the payload + +-- load mavlink message definitions from modules/MAVLink directory +local mavlink_msgs = require("MAVLink/mavlink_msgs") + +-- global definitions +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local UPDATE_INTERVAL_MS = 10 -- update at about 100hz +local COPTER_MODE_AUTO = 3 +local MAV_CMD_NAV_PAYLOAD_PLACE = 94 +local MAV_CMD_NAV_SCRIPT_TIME = 42702 +local PAYLOAD_OFFSET_COMP_VEL_MAX = 1 -- payload offset compensation will be active when the payload's horizontal velocity is no more than this speed in m/s +local PAYLOAD_UPDATE_TIMEOUT_MS = 1000 -- payload update timeout, used to warn user on loss of connection +local CONTROL_TIMEOUT_MS = 3000 -- control timeout, used to reset offsets if they have not been set for more than 3 seconds +local TEXT_PREFIX_STR = "copter-slung-payload:" -- prefix for all text messages + + -- setup script specific parameters +local PARAM_TABLE_KEY = 82 +local PARAM_TABLE_PREFIX = "SLUP_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 7), 'could not add param table') + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', PARAM_TABLE_PREFIX .. name)) + return Parameter(PARAM_TABLE_PREFIX .. name) + end + +--[[ + // @Param: SLUP_ENABLE + // @DisplayName: Slung Payload enable + // @Description: Slung Payload enable + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local SLUP_ENABLE = bind_add_param("ENABLE", 1, 1) + +--[[ + // @Param: SLUP_VEL_P + // @DisplayName: Slung Payload Velocity P gain + // @Description: Slung Payload Velocity P gain, higher values will result in faster movements in sync with payload + // @Range: 0 0.8 + // @User: Standard +--]] +local SLUP_VEL_P = bind_add_param("VEL_P", 2, 0.5) + +--[[ + // @Param: SLUP_DIST_MAX + // @DisplayName: Slung Payload horizontal distance max + // @Description: Oscillation is suppressed when vehicle and payload are no more than this distance horizontally. Set to 0 to always suppress + // @Range: 0 30 + // @User: Standard +--]] +local SLUP_DIST_MAX = bind_add_param("DIST_MAX", 3, 15) + +--[[ + // @Param: SLUP_SYSID + // @DisplayName: Slung Payload mavlink system id + // @Description: Slung Payload mavlink system id. 0 to use any/all system ids + // @Range: 0 255 + // @User: Standard +--]] +local SLUP_SYSID = bind_add_param("SYSID", 4, 0) + +--[[ + // @Param: SLUP_WP_POS_P + // @DisplayName: Slung Payload return to WP position P gain + // @Description: WP position P gain. higher values will result in vehicle moving more quickly back to the original waypoint + // @Range: 0 1 + // @User: Standard +--]] +local SLUP_WP_POS_P = bind_add_param("WP_POS_P", 5, 0.05) + +--[[ + // @Param: SLUP_RESTOFS_TC + // @DisplayName: Slung Payload resting offset estimate filter time constant + // @Description: payload's position estimator's time constant used to compensate for GPS errors and wind. Higher values result in smoother estimate but slower response + // @Range: 1 20 + // @User: Standard +--]] +local SLUP_RESTOFS_TC = bind_add_param("RESTOFS_TC", 6, 10) + +--[[ + // @Param: SLUP_DEBUG + // @DisplayName: Slung Payload debug output + // @Description: Slung payload debug output, set to 1 to enable debug + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local SLUP_DEBUG = bind_add_param("DEBUG", 7, 0) + +-- mavlink definitions +local GLOBAL_POSITION_INT_ID = 33 +local msg_map = {} +msg_map[GLOBAL_POSITION_INT_ID] = "GLOBAL_POSITION_INT" + +-- initialize MAVLink rx with number of messages, and buffer depth +mavlink:init(2, 5) + +-- register message id to receive +mavlink:register_rx_msgid(GLOBAL_POSITION_INT_ID) + +-- variables +local payload_sysid = nil -- holds sysid of payload once a matching global position int message has been received +local payload_loc = Location() -- payload location +local payload_vel = Vector3f() -- payload velocity +local payload_acc = Vector3f() -- payload acceleration +local payload_update_timeout_prev = true -- payload update timeout state from previous iteration, used to detect loss/recovery of payload updates +local payload_loc_update_ms = uint32_t(0) -- system time that payload_loc was last updated +local payload_dist_NED = Vector3f() -- distance between vehicle and payload in NED frame +local global_pos_int_timebootms_prev = 0 -- global position int message's time_boot_ms field from previous iteration (used to calc dt) +local resting_offset_NED = Vector3f() -- estimated position offset between payload and vehicle +local resting_vel_NED = Vector3f() -- estimated velocity offset. should be near zero when hovering +local resting_offset_valid = false -- true if resting_offset_NED and resting_vel_NED can be used +local resting_offset_update_ms = uint32_t(0) -- system time that resting_offset_NED was last updated +local resting_offset_notify_ms = uint32_t(0) -- system time that the user was sent the resting_offset_NED +local send_velocity_offsets = false -- true if we should send vehicle velocity offset commands to reduce payload oscillation +local sent_velocity_offsets_ms = uint32_t(0) -- system time that the last velocity offset was sent to the vehicle +local control_timeout_ms = uint32_t(0) -- system time that the control timeout occurred +local payload_vel_prev = Vector3f() -- previous iterations payload velocity used to calculate acceleration +local print_warning_ms = uint32_t(0) -- system time that the last warning was printed. used to prevent spamming the user with warnings + +-- display a text warning to the user. only displays a warning every second +-- prefix is automatically added +function print_warning(text_warning) + local now_ms = millis() + if (now_ms - print_warning_ms > 1000) then + gcs:send_text(MAV_SEVERITY.WARNING, string.format("%s %s", TEXT_PREFIX_STR, text_warning)) + print_warning_ms = now_ms + end +end + +-- calculate sign of a number. 1 if positive, -1 if negative, 0 if exactly zero +function get_sign(value) + if value > 0 then + return 1 + elseif value < 0 then + return -1 + end + return 0 +end + +-- calculate an alpha for a first order low pass filter +function calc_lowpass_alpha(dt, time_constant) + local rc = time_constant/(math.pi*2) + return dt/(dt+rc) +end + +-- handle global position int message +-- returns true if the message was from the payload and updates payload_loc, payload_vel, payload_acc and payload_loc_update_ms +function handle_global_position_int(msg) + -- check if message is from the correct system id + if (SLUP_SYSID:get() > 0 and msg.sysid ~= SLUP_SYSID:get()) then + return false + end + + -- lock onto the first matching system id + if payload_sysid == nil then + payload_sysid = msg.sysid + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s found sysid:%d", TEXT_PREFIX_STR, msg.sysid)) + elseif payload_sysid ~= msg.sysid then + return false + end + + -- check for duplicate messages and calculate dt + local time_boot_ms = msg.time_boot_ms + local dt = (time_boot_ms - global_pos_int_timebootms_prev) * 0.001 + global_pos_int_timebootms_prev = time_boot_ms + if dt <= 0 or dt > 1 then + return false + end + + -- update payload location + payload_loc:lat(msg.lat) + payload_loc:lng(msg.lon) + payload_loc:alt(msg.alt * 0.1) + + -- update payload velocity + payload_vel:x(msg.vx * 0.01) + payload_vel:y(msg.vy * 0.01) + payload_vel:z(msg.vz * 0.01) + + -- calc payload acceleration + payload_acc:x((payload_vel:x() - payload_vel_prev:x()) / dt) + payload_acc:y((payload_vel:y() - payload_vel_prev:y()) / dt) + payload_acc:z((payload_vel:z() - payload_vel_prev:z()) / dt) + payload_vel_prev = payload_vel:copy() + + -- record time of update + payload_loc_update_ms = millis() + return true +end + +-- estimate the payload's resting position offset based on its current offset and velocity +-- relies on payload_dist_NED and payload_vel being updated +function update_payload_resting_offset() + + -- calculate dt since last update + local now_ms = millis() + local dt = (now_ms - resting_offset_update_ms):tofloat() * 0.001 + resting_offset_update_ms = now_ms + + -- sanity check dt + if (dt <= 0) then + resting_offset_valid = false + do return end + end + + -- if not updated for more than 1 second, reset resting offset to current offset + if (dt > 1) then + resting_offset_NED = payload_dist_NED + resting_vel_NED = payload_vel + resting_offset_valid = false + do return end + end + + -- use a low-pass filter to move the resting offset NED towards the pos_offset_NED + local alpha = calc_lowpass_alpha(dt, SLUP_RESTOFS_TC:get()) + resting_offset_NED:x(resting_offset_NED:x() + (payload_dist_NED:x() - resting_offset_NED:x()) * alpha) + resting_offset_NED:y(resting_offset_NED:y() + (payload_dist_NED:y() - resting_offset_NED:y()) * alpha) + resting_offset_NED:z(resting_offset_NED:z() + (payload_dist_NED:z() - resting_offset_NED:z()) * alpha) + resting_vel_NED:x(resting_vel_NED:x() + (payload_vel:x() - resting_vel_NED:x()) * alpha) + resting_vel_NED:y(resting_vel_NED:y() + (payload_vel:y() - resting_vel_NED:y()) * alpha) + resting_vel_NED:z(resting_vel_NED:z() + (payload_vel:z() - resting_vel_NED:z()) * alpha) + + -- debug output every 3 seconds + local print_debug = false + if (SLUP_DEBUG:get() > 0) and (now_ms - resting_offset_notify_ms > 3000) then + print_debug = true + resting_offset_notify_ms = now_ms + end + + -- validate that resting offsets are valid + -- resting velocity should be low to ensure the resting position estimate is accurate + if (resting_vel_NED:xy():length() > PAYLOAD_OFFSET_COMP_VEL_MAX) then + resting_offset_valid = false + if print_debug then + print_warning("resting velocity too high") + end + return + end + + -- resting position should be within SLUP_DIST_MAX of the vehicle + if resting_offset_NED:xy():length() > SLUP_DIST_MAX:get() then + resting_offset_valid = false + if print_debug then + print_warning("payload resting pos too far, ignoring"); + end + return + end + + -- update user + if (print_debug) then + gcs:send_text(MAV_SEVERITY.INFO, string.format("resting a:%f px:%4.1f py:%4.1f pz:%4.1f vx:%4.1f vy:%4.1f vz:%4.1f", alpha, resting_offset_NED:x(), resting_offset_NED:y(), resting_offset_NED:z(), resting_vel_NED:x(), resting_vel_NED:y(), resting_vel_NED:z())) + end + + -- if set got this far the resting offsets must be valid + resting_offset_valid = true +end + +-- move vehicle to reduce payload oscillation +-- relies on payload_dist_NED, payload_vel, payload_acc, resting_offset_NED, resting_vel_NED being updated +function move_vehicle() + + -- check horizontal distance is less than SLUP_DIST_MAX + if SLUP_DIST_MAX:get() > 0 then + local dist_xy = payload_dist_NED:xy():length() + if (dist_xy > SLUP_DIST_MAX:get()) then + print_warning(string.format("payload too far %4.1fm", dist_xy)); + do return end + end + end + + -- get long-term payload offset used to compensate for GPS errors and wind + local payload_offset_NED = Vector3f() + if resting_offset_valid then + payload_offset_NED = resting_offset_NED + end + + -- get position offset (cumulative effect of velocity offsets) and use to slowly move back to waypoint + local pos_offset_NED, _, _ = vehicle:get_posvelaccel_offset() + if pos_offset_NED == nil then + print_warning("unable to get dist to waypoint") + pos_offset_NED = Vector3f() + end + + -- calculate send velocity offsets in m/s in NED frame + local vel_offset_NED = Vector3f() + vel_offset_NED:x(-payload_acc:x() * SLUP_VEL_P:get() + (-pos_offset_NED:x() - payload_offset_NED:x()) * SLUP_WP_POS_P:get()) + vel_offset_NED:y(-payload_acc:y() * SLUP_VEL_P:get() + (-pos_offset_NED:y() - payload_offset_NED:y()) * SLUP_WP_POS_P:get()) + if vehicle:set_velaccel_offset(vel_offset_NED, Vector3f()) then + sent_velocity_offsets_ms = millis() + end +end + +-- display welcome message +gcs:send_text(MAV_SEVERITY.INFO, "copter-slung-payload script loaded") + +-- update function to receive location from payload and move vehicle to reduce payload's oscillation +function update() + + -- exit immediately if not enabled + if (SLUP_ENABLE:get() <= 0) then + return update, 1000 + end + + -- get vehicle location + local curr_loc = ahrs:get_location() + if curr_loc == nil then + return update, UPDATE_INTERVAL_MS + end + + -- consume all available mavlink messages + local payload_update_received = false + local msg + repeat + msg, _ = mavlink:receive_chan() + if (msg ~= nil) then + local parsed_msg = mavlink_msgs.decode(msg, msg_map) + if (parsed_msg ~= nil) then + if parsed_msg.msgid == GLOBAL_POSITION_INT_ID then + if handle_global_position_int(parsed_msg) then + payload_update_received = true + end + end + end + end + until msg == nil + + -- warn user on loss of recovery of telemetry from payload + local payload_timeout = millis() - payload_loc_update_ms > PAYLOAD_UPDATE_TIMEOUT_MS + if payload_timeout ~= payload_update_timeout_prev then + if payload_timeout then + gcs:send_text(MAV_SEVERITY.WARNING, string.format("%s payload updates lost", TEXT_PREFIX_STR)) + else + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s payload updates received", TEXT_PREFIX_STR)) + end + end + payload_update_timeout_prev = payload_timeout + + if payload_update_received then + -- calculate position difference vs vehicle + payload_dist_NED = curr_loc:get_distance_NED(payload_loc) + + -- estimate the payload's resting position offset based on its current offset and velocity + -- relies on payload_dist_NED and payload_vel being updated + update_payload_resting_offset() + end + + -- check if we can control the vehicle + -- vehicle must be in Auto mode executing a SCRIPT_TIME or PAYLOAD_PLACE command + local send_velocity_offsets_prev = send_velocity_offsets + local armed_and_flying = arming:is_armed() and vehicle:get_likely_flying() + local takingoff_or_landing = vehicle:is_landing() or vehicle:is_taking_off() + local auto_mode = (vehicle:get_mode() == COPTER_MODE_AUTO) + local scripting_or_payloadplace = (mission:get_current_nav_id() == MAV_CMD_NAV_SCRIPT_TIME) or (mission:get_current_nav_id() == MAV_CMD_NAV_PAYLOAD_PLACE) + send_velocity_offsets = armed_and_flying and not takingoff_or_landing and auto_mode and scripting_or_payloadplace and not payload_timeout + + -- alert user if we start or stop sending velocity offsets + if (send_velocity_offsets and not send_velocity_offsets_prev) then + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s activated", TEXT_PREFIX_STR)) + end + if (not send_velocity_offsets and send_velocity_offsets_prev) then + gcs:send_text(MAV_SEVERITY.INFO, string.format("%s deactivated", TEXT_PREFIX_STR)) + vehicle:set_posvelaccel_offset(Vector3f(), Vector3f(), Vector3f()) + end + + -- move vehicle to reduce payload oscillation + if send_velocity_offsets then + move_vehicle() + + -- check for unexpected control timeout + -- reset vehicle offsets if not sent within last 3 seconds + local now_ms = millis() + local time_since_vel_offset_sent = now_ms - sent_velocity_offsets_ms + local time_since_last_control_timeout = now_ms - control_timeout_ms + if (time_since_vel_offset_sent > CONTROL_TIMEOUT_MS) and (time_since_last_control_timeout > CONTROL_TIMEOUT_MS) then + vehicle:set_posvelaccel_offset(Vector3f(), Vector3f(), Vector3f()) + control_timeout_ms = now_ms + print_warning("control timeout, clearing offsets") + end + end + + return update, UPDATE_INTERVAL_MS +end + +return update() diff --git a/libraries/AP_Scripting/applets/copter-slung-payload.md b/libraries/AP_Scripting/applets/copter-slung-payload.md new file mode 100644 index 0000000000000..32d76256c43a7 --- /dev/null +++ b/libraries/AP_Scripting/applets/copter-slung-payload.md @@ -0,0 +1,30 @@ +# Slung Payload + +This script moves a Copter so as to reduce a slung payload's oscillation. Requires the payload be capable of sending its position and velocity to the main vehicle + +# Parameters + +SLUP_ENABLE : Set to 1 to enable this script +SLUP_VEL_P : Oscillation controller velocity P gain. Higher values will result in the vehicle moving more quickly in sync with the payload +SLUP_DIST_MAX : maximum acceptable distance between vehicle and payload. Within this distance oscillation suppression will operate +SLUP_SYSID : System id of payload's autopilot. If zero any system id is accepted +SLUP_WP_POS_P : Return to waypoint position P gain. Higher values result in the vehicle returning more quickly to the latest waypoint +SLUP_RESTOFS_TC : Slung Payload resting offset estimate filter time constant. Higher values result in smoother estimate but slower response +SLUP_DEBUG : Slung payload debug output, set to 1 to enable debug + +# How To Use + +1. mount an autopilot on the payload connected to the main vehicle using telemetry +2. ensure the vehicle and payload autopilots have unique system ids +3. copy this script to the vehicle autopilot's "scripts" directory +4. within the "scripts" directory create a "modules" directory +5. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory + +# How It Works + +The script's algorithm is implemented as follows + +1. Consume GLOBAL_POSITION_INT messages from the payload +2. Calculate the payload's position vs the vehicle position +3. Use a P controller to move the vehicle towards the payload to reduce oscillation +4. Simultaneously the vehicle moves back towards the original location. The speed depends upon the SLUP_WP_POS_P parameter diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 6644ba2e940d9..b3cade5294a06 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2354,6 +2354,25 @@ function vehicle:set_circle_rate(rate_dps) end ---@return number|nil function vehicle:get_circle_radius() end +-- add an offset to vehicle's target position, velocity and acceleration in autonomous modes +---@param pos_offset_NED Vector3f_ud +---@param vel_offset_NED Vector3f_ud +---@param accel_offset_NED Vector3f_ud +---@return boolean +function vehicle:set_posvelaccel_offset(pos_offset_NED, vel_offset_NED, accel_offset_NED) end + +-- add an offset to vehicle's target velocity and acceleration in autonomous modes +---@param vel_offset_NED Vector3f_ud +---@param accel_offset_NED Vector3f_ud +---@return boolean +function vehicle:set_velaccel_offset(vel_offset_NED, accel_offset_NED) end + +-- get position, velocity and acceleration offsets to vehicle's target position in autonomous modes +---@return Vector3f_ud|nil +---@return Vector3f_ud|nil +---@return Vector3f_ud|nil +function vehicle:get_posvelaccel_offset() end + -- desc ---@param roll_deg number ---@param pitch_deg number diff --git a/libraries/AP_Scripting/examples/copter-posoffset.lua b/libraries/AP_Scripting/examples/copter-posoffset.lua new file mode 100644 index 0000000000000..1f40fcb16e861 --- /dev/null +++ b/libraries/AP_Scripting/examples/copter-posoffset.lua @@ -0,0 +1,103 @@ +-- Example showing how a position offset can be added to a Copter's auto mission plan +-- +-- CAUTION: This script only works for Copter +-- this script waits for the vehicle to be armed and in auto mode and then +-- adds an offset to the position or velocity target +-- +-- How To Use +-- 1. copy this script to the autopilot's "scripts" directory +-- 2. within the "scripts" directory create a "modules" directory +-- 3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory +-- + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +-- setup script specific parameters +local PARAM_TABLE_KEY = 71 +local PARAM_TABLE_PREFIX = "PSC_OFS_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add param table') + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', PARAM_TABLE_PREFIX .. name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +--[[ + // @Param: PSC_OFS_POS_N + // @DisplayName: Position Controller Position Offset North + // @Description: Position controller offset North + // @Range: 0 1000 + // @Units: m + // @User: Standard +--]] +local PSC_OFS_POS_N = bind_add_param("POS_N", 1, 0) + +--[[ + // @Param: PSC_OFS_POS_E + // @DisplayName: Position Controller Position Offset East + // @Description: Position controller offset North + // @Range: 0 1000 + // @Units: m + // @User: Standard +--]] +local PSC_OFS_POS_E = bind_add_param("POS_E", 2, 0) + +--[[ + // @Param: PSC_OFS_VEL_N + // @DisplayName: Position Controller Velocity Offset North + // @Description: Position controller velocity offset North + // @Range: 0 10 + // @Units: m/s + // @User: Standard +--]] +local PSC_OFS_VEL_N = bind_add_param("VEL_N", 3, 0) + +--[[ + // @Param: PSC_OFS_VEL_E + // @DisplayName: Position Controller Velocity Offset East + // @Description: Position controller velocity offset East + // @Range: 0 10 + // @Units: m/s + // @User: Standard +--]] +local PSC_OFS_VEL_E = bind_add_param("VEL_E", 4, 0) + +-- welcome message to user +gcs:send_text(MAV_SEVERITY.INFO, "copter-posoffset.lua loaded") + +function update() + + -- must be armed, flying and in auto mode + if (not arming:is_armed()) or (not vehicle:get_likely_flying()) then + return update, 1000 + end + + local pos_offsets_zero = PSC_OFS_POS_N:get() == 0 and PSC_OFS_POS_E:get() == 0 + local vel_offsets_zero = PSC_OFS_VEL_N:get() == 0 and PSC_OFS_VEL_E:get() == 0 + + -- if position offsets are non-zero or all offsets are zero then send position offsets + if not pos_offsets_zero or vel_offsets_zero then + -- set the position offset in meters in NED frame + local pos_offset_NED = Vector3f() + pos_offset_NED:x(PSC_OFS_POS_N:get()) + pos_offset_NED:y(PSC_OFS_POS_E:get()) + if not vehicle:set_posvelaccel_offset(pos_offset_NED, Vector3f(), Vector3f()) then + gcs:send_text(MAV_SEVERITY.ERROR, "copter-posoffset: failed to set pos offset") + end + test_position = not pos_offsets_zero + else + -- test velocity offsets in m/s in NED frame + local vel_offset_NED = Vector3f() + vel_offset_NED:x(PSC_OFS_VEL_N:get()) + vel_offset_NED:y(PSC_OFS_VEL_E:get()) + if not vehicle:set_velaccel_offset(vel_offset_NED, Vector3f()) then + gcs:send_text(MAV_SEVERITY.ERROR, "copter-posoffset: failed to set vel offset") + end + end + + -- update at 1hz + return update, 1000 +end + +return update() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 6868e797d7899..830e2600fda30 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -330,6 +330,9 @@ singleton AP_Vehicle method set_target_velocity_NED boolean Vector3f singleton AP_Vehicle method set_target_angle_and_climbrate boolean float -180 180 float -90 90 float -360 360 float'skip_check boolean float'skip_check singleton AP_Vehicle method get_circle_radius boolean float'Null singleton AP_Vehicle method set_circle_rate boolean float'skip_check +singleton AP_Vehicle method set_posvelaccel_offset boolean Vector3f Vector3f Vector3f +singleton AP_Vehicle method set_velaccel_offset boolean Vector3f Vector3f +singleton AP_Vehicle method get_posvelaccel_offset boolean Vector3f'Null Vector3f'Null Vector3f'Null singleton AP_Vehicle method set_steering_and_throttle boolean float -1 1 float -1 1 singleton AP_Vehicle method get_steering_and_throttle boolean float'Null float'Null singleton AP_Vehicle method get_wp_distance_m boolean float'Null diff --git a/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_GLOBAL_POSITION_INT.lua b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_GLOBAL_POSITION_INT.lua new file mode 100644 index 0000000000000..e566ad52b53c2 --- /dev/null +++ b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_GLOBAL_POSITION_INT.lua @@ -0,0 +1,14 @@ +local GLOBAL_POSITION_INT = {} +GLOBAL_POSITION_INT.id = 33 +GLOBAL_POSITION_INT.fields = { + { "time_boot_ms", "S5cZ%H`T*kbBOq;HE1G)_J$cbWK5Q(vteg8;K`@);z|01ZXrsC zVHjq+AdWjo6WplFpPR)e;NBq`()mH++j^>s z+7zb9QI=RWD`-!=#9T@G+3Soqh&bIWckb_ZcmH*>t6(sQltO? delta 13 UcmZ1_u~clMCM)}9E)E4203DM9O#lD@ diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 5256dd82e281f..206639b2086aa 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -201,6 +201,13 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool get_circle_radius(float &radius_m) { return false; } virtual bool set_circle_rate(float rate_dps) { return false; } + // position, velocity and acceleration offset target (only used by scripting with Copter) + // gets or sets an additional offset to the vehicle's target position, velocity and acceleration + // units are m, m/s and m/s/s in NED frame + virtual bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) { return false; } + virtual bool set_velaccel_offset(const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) { return false; } + virtual bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED) { return false; } + // get or set steering and throttle (-1 to +1) (for use by scripting with Rover) virtual bool set_steering_and_throttle(float steering, float throttle) { return false; } virtual bool get_steering_and_throttle(float& steering, float& throttle) { return false; }