From 6c60f93f5e6ffd1a359ff3071602c7b89dc377a2 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Sat, 14 Sep 2024 10:08:35 -0700 Subject: [PATCH 1/5] Sub: support ABOVE_TERRAIN frame in GUIDED mode posvel case --- ArduSub/GCS_Mavlink.cpp | 11 +++++++ ArduSub/Log.cpp | 8 +++-- ArduSub/Sub.h | 3 +- ArduSub/mode.h | 4 ++- ArduSub/mode_guided.cpp | 66 +++++++++++++++++++++++++++++++++++------ 5 files changed, 79 insertions(+), 13 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 63fb05f821e47..4ad35bf1c59ac 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -836,6 +836,17 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg) int32_t(packet.alt*100), frame, }; + + // posvel case supports ABOVE_TERRAIN frame + if (!vel_ignore && acc_ignore && frame == Location::AltFrame::ABOVE_TERRAIN) { + if (!loc.get_vector_xy_from_origin_NE(pos_neu_cm.xy())) { + break; + } + pos_neu_cm.z = loc.alt; + sub.mode_guided.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), frame); + break; + } + if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) { break; } diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 88642afe26e3d..841f48af7c138 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -176,6 +176,7 @@ struct PACKED log_GuidedTarget { LOG_PACKET_HEADER; uint64_t time_us; uint8_t type; + uint8_t alt_frame; float pos_target_x; float pos_target_y; float pos_target_z; @@ -185,12 +186,14 @@ struct PACKED log_GuidedTarget { }; // Write a Guided mode target -void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) +void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target, + Location::AltFrame alt_frame) { struct log_GuidedTarget pkt = { LOG_PACKET_HEADER_INIT(LOG_GUIDEDTARGET_MSG), time_us : AP_HAL::micros64(), type : target_type, + alt_frame : static_cast(alt_frame), pos_target_x : pos_target.x, pos_target_y : pos_target.y, pos_target_z : pos_target.z, @@ -251,6 +254,7 @@ void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target // @Description: Guided mode target information // @Field: TimeUS: Time since system startup // @Field: Type: Type of guided mode +// @Field: Frame: Location.AltFrame // @Field: pX: Target position, X-Axis // @Field: pY: Target position, Y-Axis // @Field: pZ: Target position, Z-Axis @@ -276,7 +280,7 @@ const struct LogStructure Sub::log_structure[] = { { LOG_DATA_FLOAT_MSG, sizeof(log_Data_Float), "DFLT", "QBf", "TimeUS,Id,Value", "s--", "F--" }, { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), - "GUIP", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ", "s-mmmnnn", "F-000000" }, + "GUIP", "QBBffffff", "TimeUS,Type,Frame,pX,pY,pZ,vX,vY,vZ", "s--mmmnnn", "F--000000" }, }; uint8_t Sub::get_num_log_structures() const diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 52cd205175f4e..6b429c9761938 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -422,7 +422,8 @@ class Sub : public AP_Vehicle { void Log_Write_Data(LogDataID id, int16_t value); void Log_Write_Data(LogDataID id, uint16_t value); void Log_Write_Data(LogDataID id, float value); - void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); + void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target, + Location::AltFrame alt_frame=Location::AltFrame::ABOVE_ORIGIN); void Log_Write_Vehicle_Startup_Messages(); #endif void load_parameters(void) override; diff --git a/ArduSub/mode.h b/ArduSub/mode.h index 11a6447167511..4ad5db6a5def9 100644 --- a/ArduSub/mode.h +++ b/ArduSub/mode.h @@ -326,7 +326,9 @@ class ModeGuided : public Mode void guided_set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads); void guided_set_angle(const Quaternion&, float); void guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm); - bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity); + void set_posvel_frame_above_home(); + void set_posvel_frame_above_terrain(float destination_z); + bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame alt_frame=Location::AltFrame::ABOVE_ORIGIN); bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw); bool guided_set_destination(const Vector3f& destination); bool guided_set_destination(const Location&); diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp index ff1bb07df5681..ff9f1bdb5a9d8 100644 --- a/ArduSub/mode_guided.cpp +++ b/ArduSub/mode_guided.cpp @@ -9,6 +9,7 @@ static Vector3p posvel_pos_target_cm; static Vector3f posvel_vel_target_cms; +static Location::AltFrame posvel_frame; static uint32_t update_time_ms; struct { @@ -114,6 +115,22 @@ void ModeGuided::guided_vel_control_start() set_auto_yaw_mode(AUTO_YAW_HOLD); } +// posvel helper function: use above home frame +void ModeGuided::set_posvel_frame_above_home() +{ + posvel_frame = Location::AltFrame::ABOVE_HOME; + position_control->set_pos_offset_z_cm(0); + position_control->set_pos_offset_target_z_cm(0); +} + +// posvel helper function: use above terrain frame +void ModeGuided::set_posvel_frame_above_terrain(float destination_z) +{ + posvel_frame = Location::AltFrame::ABOVE_TERRAIN; + position_control->set_pos_offset_z_cm(sub.rangefinder_state.inertial_alt_cm - destination_z); + position_control->set_pos_offset_target_z_cm(sub.rangefinder_state.rangefinder_terrain_offset_cm); +} + // initialise guided mode's posvel controller void ModeGuided::guided_posvel_control_start() { @@ -130,6 +147,9 @@ void ModeGuided::guided_posvel_control_start() // pilot always controls yaw set_auto_yaw_mode(AUTO_YAW_HOLD); + + // default frame is ABOVE_HOME + set_posvel_frame_above_home(); } // initialise guided mode's angle controller @@ -291,16 +311,35 @@ void ModeGuided::guided_set_velocity(const Vector3f& velocity, bool use_yaw, flo } // set guided mode posvel target -bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity) +bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, Location::AltFrame alt_frame) { - // check we are in velocity control mode + // check we are in posvel control mode if (sub.guided_mode != Guided_PosVel) { + // catch some configuration errors by requiring a healthy rangefinder to specify the ABOVE_TERRAIN frame + if (alt_frame == Location::AltFrame::ABOVE_TERRAIN && !sub.rangefinder_alt_ok()) { + gcs().send_text(MAV_SEVERITY_WARNING, "Terrain data (rangefinder) not available"); + return false; + } + guided_posvel_control_start(); + + if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) { + // initialize the terrain offset + // destination.z is the rangefinder target + set_posvel_frame_above_terrain(destination.z); + } + } else { + if (alt_frame == Location::AltFrame::ABOVE_TERRAIN) { + // rangefinder target may have changed + position_control->set_pos_offset_z_cm(position_control->get_pos_offset_z_cm() + posvel_pos_target_cm.z - destination.z); + } else { + set_posvel_frame_above_home(); + } } #if AP_FENCE_ENABLED // reject destination if outside the fence - const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); + const Location dest_loc(destination, alt_frame); if (!sub.fence.check_destination_within_fence(dest_loc)) { LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK @@ -312,14 +351,9 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons posvel_pos_target_cm = destination.topostype(); posvel_vel_target_cms = velocity; - position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); - float dz = posvel_pos_target_cm.z; - position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); - posvel_pos_target_cm.z = dz; - #if HAL_LOGGING_ENABLED // log target - sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); + sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity, alt_frame); #endif return true; @@ -634,9 +668,23 @@ void ModeGuided::guided_posvel_control_run() // send position and velocity targets to position controller position_control->input_pos_vel_accel_xy(posvel_pos_target_cm.xy(), posvel_vel_target_cms.xy(), Vector2f()); +#if AP_RANGEFINDER_ENABLED + if (posvel_frame == Location::AltFrame::ABOVE_TERRAIN) { + if (sub.rangefinder_alt_ok()) { + // surftrak: set the offset target to the current terrain altitude estimate + position_control->set_pos_offset_target_z_cm(sub.rangefinder_state.rangefinder_terrain_offset_cm); + } + position_control->set_pos_target_z_from_climb_rate_cm(posvel_vel_target_cms.z); + } else { + float pz = posvel_pos_target_cm.z; + position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0); + posvel_pos_target_cm.z = pz; + } +#else float pz = posvel_pos_target_cm.z; position_control->input_pos_vel_accel_z(pz, posvel_vel_target_cms.z, 0); posvel_pos_target_cm.z = pz; +#endif // run position controller position_control->update_xy_controller(); From 0160e63a01b7e5eab0c247a89e5dc4e9ae367eee Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Sat, 14 Sep 2024 10:13:53 -0700 Subject: [PATCH 2/5] AP_Vehicle: add set_target_posvel_terrain method for scripting --- libraries/AP_Vehicle/AP_Vehicle.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 5256dd82e281f..e2768f1e359c6 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -182,6 +182,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { virtual bool start_takeoff(float alt) { return false; } virtual bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) { return false; } virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; } + virtual bool set_target_posvel_terrain(const Vector3f& target_pos, const Vector3f& target_vel) { return false; } virtual bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; } virtual bool set_target_velocity_NED(const Vector3f& vel_ned) { return false; } virtual bool set_target_velaccel_NED(const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; } From b2e93f4cc9f78db3bdf74d931775278619e15617 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Sat, 14 Sep 2024 10:14:35 -0700 Subject: [PATCH 3/5] AP_Scripting: add Lua binding for set_target_posvel_terrain --- libraries/AP_Scripting/docs/docs.lua | 6 ++++++ libraries/AP_Scripting/generator/description/bindings.desc | 1 + 2 files changed, 7 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 889ffa089dca4..89611f0d1bc8c 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -2398,6 +2398,12 @@ function vehicle:set_target_posvelaccel_NED(target_pos, target_vel, target_accel ---@return boolean function vehicle:set_target_posvel_NED(target_pos, target_vel) end +-- Sets the target position and velocity in a guided mode, with ABOVE_TERRAIN frame. +---@param target_pos Vector3f_ud +---@param target_vel Vector3f_ud +---@return boolean +function vehicle:set_target_posvel_terrain(target_pos, target_vel) end + -- desc ---@param target_pos Vector3f_ud ---@param use_yaw boolean diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 30b5be0c766ef..c43c0096a81cd 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -325,6 +325,7 @@ singleton AP_Vehicle method get_target_location boolean Location'Null singleton AP_Vehicle method update_target_location boolean Location Location singleton AP_Vehicle method set_target_pos_NED boolean Vector3f boolean float -360 +360 boolean float'skip_check boolean boolean singleton AP_Vehicle method set_target_posvel_NED boolean Vector3f Vector3f +singleton AP_Vehicle method set_target_posvel_terrain boolean Vector3f Vector3f singleton AP_Vehicle method set_target_posvelaccel_NED boolean Vector3f Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean singleton AP_Vehicle method set_target_velaccel_NED boolean Vector3f Vector3f boolean float -360 +360 boolean float'skip_check boolean singleton AP_Vehicle method set_target_velocity_NED boolean Vector3f From 3485e918c6c1b3c1be081e5fd6dcab2fa3412b43 Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Sat, 14 Sep 2024 10:14:55 -0700 Subject: [PATCH 4/5] Sub: implement set_target_posvel methods --- ArduSub/Sub.cpp | 31 +++++++++++++++++++++++++++++++ ArduSub/Sub.h | 6 ++++++ 2 files changed, 37 insertions(+) diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 877f3bf8c17ca..88f1d0e25503d 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -49,6 +49,37 @@ Sub::Sub() _singleton = this; } +#if AP_SCRIPTING_ENABLED +// set target position and velocity, alt frame is Location::AltFrame::ABOVE_ORIGIN +bool Sub::set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, -target_pos.z * 100.0f); + const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f); + + return mode_guided.guided_set_destination_posvel(pos_neu_cm, vel_neu_cms, Location::AltFrame::ABOVE_ORIGIN); +} + +// set target position and velocity, alt frame is Location::AltFrame::ABOVE_TERRAIN +bool Sub::set_target_posvel_terrain(const Vector3f& target_pos, const Vector3f& target_vel) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + // pos.z is rf target, do not flip sign + const Vector3f pos_neu_cm(target_pos.x * 100.0f, target_pos.y * 100.0f, target_pos.z * 100.0f); + const Vector3f vel_neu_cms(target_vel.x * 100.0f, target_vel.y * 100.0f, -target_vel.z * 100.0f); + + return mode_guided.guided_set_destination_posvel(pos_neu_cm, vel_neu_cms, Location::AltFrame::ABOVE_TERRAIN); +} +#endif + Sub *Sub::_singleton = nullptr; Sub sub; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 6b429c9761938..d02341766a7c6 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -632,6 +632,12 @@ class Sub : public AP_Vehicle { float get_rangefinder_target_cm() const WARN_IF_UNUSED { return mode_surftrak.get_rangefinder_target_cm(); } bool set_rangefinder_target_cm(float new_target_cm) { return mode_surftrak.set_rangefinder_target_cm(new_target_cm); } #endif // AP_RANGEFINDER_ENABLED + + // set target position and velocity, alt frame is Location::AltFrame::ABOVE_ORIGIN + bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) override; + + // set target position and velocity, alt frame is Location::AltFrame::ABOVE_TERRAIN + bool set_target_posvel_terrain(const Vector3f& target_pos, const Vector3f& target_vel) override; #endif // AP_SCRIPTING_ENABLED }; From 35a723a320d267565ef669ba602fd385815ab88e Mon Sep 17 00:00:00 2001 From: Clyde McQueen Date: Sat, 14 Sep 2024 10:15:23 -0700 Subject: [PATCH 5/5] autotest: test GUIDED posvel frames --- Tools/autotest/ardusub.py | 94 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 94 insertions(+) diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 0e258f3da589b..e13d672bd489b 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -12,6 +12,7 @@ import sys from pymavlink import mavutil +from pysim import util import vehicle_test_suite from vehicle_test_suite import NotAchievedException @@ -440,6 +441,98 @@ def SimTerrainMission(self): self.context_pop() self.reboot_sitl() # e.g. revert rangefinder configuration + def GuidedPosVel(self): + """Send SET_POSITION_TARGET_INT msgs at 10Hz with position and velocity targets""" + + # GUIDED mode supports several sub-modes selected by the POSITION_TARGET_TYPE mask + # The Guided_PosVel sub-mode supports rapid updates and several altitude frames + mask = (mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE) + + seafloor_depth = 50 + speed_cms = 50 + + # Generate a synthetic seafloor at -50m + self.context_push() + self.prepare_synthetic_seafloor_test(seafloor_depth) + + # Guided_PosVel uses WPNAV_SPEED + self.set_parameter('WPNAV_SPEED', speed_cms) + + # Dive to -35m + start_altitude = -35 + pwm = 1300 if self.get_altitude(relative=True) > start_altitude else 1700 + self.wait_ready_to_arm() + self.arm_vehicle() + self.set_rc(Joystick.Throttle, pwm) + self.wait_altitude(altitude_min=start_altitude-1, altitude_max=start_altitude, relative=False, timeout=120) + self.set_rc(Joystick.Throttle, 1500) + self.delay_sim_time(1) + + # Request GLOBAL_POSITION_INT at 20Hz, this will be our clock + self.context_set_message_rate_hz('GLOBAL_POSITION_INT', 10) + + # Run between 2 locations, 30m apart + distance_m = 30 + timeout = distance_m * 100 / speed_cms + 5 # Add a little time to accelerate, etc. + + runs = [{ + 'frame': mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + 'bearing': 180, + 'target_alt': -35, # Altitude + }, { + 'frame': mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, + 'bearing': 0, + 'target_alt': 15, # Distance above seafloor + }] + + for run in runs: + # Face the direction of travel + self.reach_heading_manual(run['bearing']) + + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True) + start_loc = (msg.lat * 1e-7, msg.lon * 1e-7) + dest_loc = util.gps_newpos(start_loc[0], start_loc[1], run['bearing'], distance_m) + + # Start GUIDED mode + self.change_mode('GUIDED') + + # Go! + start_time = self.get_sim_time() + while msg := self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True): + current_loc = (msg.lat * 1e-7, msg.lon * 1e-7) + distance_remaining = util.gps_distance( + dest_loc[0], dest_loc[1], + current_loc[0], current_loc[1]) + + # self.progress('Frame %u, distance remaining %f m' % (run['frame'], distance_remaining)) + + if distance_remaining < 0.5: + self.progress('Frame %u reached destination at time %f' % + (run['frame'], self.get_sim_time_cached() - start_time)) + break + elif self.get_sim_time_cached() - start_time > timeout: + raise NotAchievedException('Frame %u took too long to reach the destination' % run['frame']) + + # TODO test altitude & rangefinder + + # Set target 10m ahead of current location + target_loc = util.gps_newpos(current_loc[0], current_loc[1], run['bearing'], 10) + + self.mav.mav.set_position_target_global_int_send( + 0, 1, 1, run['frame'], mask, int(target_loc[0] * 1e7), int(target_loc[1] * 1e7), run['target_alt'], + 0, 0, 0, 0, 0, 0, 0, 0) + + # Stop guided mode + self.change_mode('MANUAL') + + self.disarm_vehicle() + self.context_pop() + self.reboot_sitl() # e.g. revert rangefinder configuration + def ModeChanges(self, delta=0.2): """Check if alternating between ALTHOLD, STABILIZE, POSHOLD and SURFTRAK (mode 21) affects altitude""" self.wait_ready_to_arm() @@ -956,6 +1049,7 @@ def tests(self): self.Surftrak, self.SimTerrainSurftrak, self.SimTerrainMission, + self.GuidedPosVel, self.RngfndQuality, self.PositionHold, self.ModeChanges,