diff --git a/msg/rtl_flight_time.msg b/msg/rtl_flight_time.msg index b21a37623e..1f16b67408 100644 --- a/msg/rtl_flight_time.msg +++ b/msg/rtl_flight_time.msg @@ -1,5 +1,4 @@ uint64 timestamp # time since system start (microseconds) -uint8 rtl_vehicle_type # from the vehicle_status message float32 rtl_time_s # how long in seconds will the RTL take float32 rtl_limit_fraction # what fraction of the allowable RTL time would be taken diff --git a/src/modules/navigator/RangeRTLTest.cpp b/src/modules/navigator/RangeRTLTest.cpp index fd3a993077..da5c51a8e9 100644 --- a/src/modules/navigator/RangeRTLTest.cpp +++ b/src/modules/navigator/RangeRTLTest.cpp @@ -50,6 +50,38 @@ TEST(Navigator_and_RTL, compiles_woohoooo) ASSERT_TRUE(_rtl_flight_time_sub.update()); auto msg = _rtl_flight_time_sub.get(); EXPECT_EQ(msg.rtl_time_s, 0); + + // WHEN: we set the vehicle type to multirotor + v_status.vehicle_type = + n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + float xy, z; + rtl.get_rtl_xy_z_speed(xy, z); + + // THEN: the RTL speed should correspond to multirotor parameters + float xy_desired, z_desired; + param_get(param_handle(px4::params::MPC_XY_CRUISE), &xy_desired); + param_get(param_handle(px4::params::MPC_Z_VEL_MAX_DN), &z_desired); + EXPECT_FLOAT_EQ(xy, xy_desired); + EXPECT_FLOAT_EQ(z, z_desired); + + // WHEN: it is a fixed wing + n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + rtl.get_rtl_xy_z_speed(xy, z); + + // THEN: it should be fixed wing parameters + param_get(param_handle(px4::params::FW_AIRSPD_TRIM), &xy_desired); + param_get(param_handle(px4::params::FW_T_SINK_MIN), &z_desired); + EXPECT_FLOAT_EQ(xy, xy_desired); + EXPECT_FLOAT_EQ(z, z_desired); + + // WHEN: it is rover + n.get_vstatus()->vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROVER; + rtl.get_rtl_xy_z_speed(xy, z); + + // THEN: it should be rover parameters, and z should just be large (no RTL time in Z -> high speed) + param_get(param_handle(px4::params::GND_SPEED_THR_SC), &xy_desired); + EXPECT_FLOAT_EQ(xy, xy_desired); + EXPECT_GT(z, 1000); } class RangeRTL_tth : public ::testing::Test diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index e28cb6e7ff..87c2d64bf9 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -230,16 +230,14 @@ void RTL::find_RTL_destination() map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &local_destination(0), &local_destination(1)); - uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type; float xy_speed, z_speed; - get_rtl_xy_z_speed(vehicle_type, xy_speed, z_speed); + get_rtl_xy_z_speed(xy_speed, z_speed); float time_to_home_s = time_to_home(local_pos, local_destination, get_wind(), xy_speed, z_speed); float rtl_flight_time_ratio = time_to_home_s / (60 * _param_rtl_flt_time.get()); rtl_flight_time_s rtl_flight_time; rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio; rtl_flight_time.rtl_time_s = time_to_home_s; - rtl_flight_time.rtl_vehicle_type = vehicle_type; _rtl_flight_time_pub.publish(rtl_flight_time); } } @@ -642,8 +640,9 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) return max(return_altitude_amsl, gpos.alt); } -void RTL::get_rtl_xy_z_speed(uint8_t vehicle_type, float &xy, float &z) +void RTL::get_rtl_xy_z_speed(float &xy, float &z) { + uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type; // Caution: here be dragons! // Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover @@ -658,7 +657,7 @@ void RTL::get_rtl_xy_z_speed(uint8_t vehicle_type, float &xy, float &z) case vehicle_status_s::VEHICLE_TYPE_FIXED_WING: _rtl_xy_speed = param_find("FW_AIRSPD_TRIM"); - _rtl_descent_speed = param_find("FW_T_SINK_MAX"); + _rtl_descent_speed = param_find("FW_T_SINK_MIN"); break; case vehicle_status_s::VEHICLE_TYPE_ROVER: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 9f445180e1..ecb04cab48 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -94,6 +94,8 @@ public: bool denyMissionLanding() { return _deny_mission_landing; } + void get_rtl_xy_z_speed(float &xy, float &z); + matrix::Vector2f get_wind(); private: /** * Set the RTL item @@ -105,8 +107,6 @@ private: */ void advance_rtl(); - void get_rtl_xy_z_speed(uint8_t vehicle_type, float &xy, float &z); - matrix::Vector2f get_wind(); float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);