forked from Archive/PX4-Autopilot
Compute RTL time and react if lower than flight time
- Compute RTL time also during RTL - Calculate correct altitude when finding destination
This commit is contained in:
Matthias Grob
parent
5489005e0b
commit
c522a8b15a
|
|
@ -131,7 +131,7 @@ set(msg_files
|
|||
rc_channels.msg
|
||||
rc_parameter_map.msg
|
||||
rpm.msg
|
||||
rtl_flight_time.msg
|
||||
rtl_time_estimate.msg
|
||||
safety.msg
|
||||
satellite_info.msg
|
||||
sensor_accel.msg
|
||||
|
|
|
|||
|
|
@ -1,4 +0,0 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
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
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
bool valid # Flag indicating whether the time estiamtes are valid
|
||||
float32 time_estimate # [s] Estimated time for RTL
|
||||
float32 safe_time_estimate # [s] Same as time_estimate, but with safety factor and safety margin included (factor*t + margin)
|
||||
|
|
@ -3606,17 +3606,6 @@ void Commander::avoidance_check()
|
|||
|
||||
void Commander::battery_status_check()
|
||||
{
|
||||
battery_status_s batteries[_battery_status_subs.size()];
|
||||
size_t num_connected_batteries = 0;
|
||||
|
||||
for (auto &battery_sub : _battery_status_subs) {
|
||||
if (battery_sub.copy(&batteries[num_connected_batteries])) {
|
||||
if (batteries[num_connected_batteries].connected) {
|
||||
num_connected_batteries++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest
|
||||
// option is to check if ANY of them have a warning, and specifically find which one has the most
|
||||
// urgent warning.
|
||||
|
|
@ -3624,52 +3613,62 @@ void Commander::battery_status_check()
|
|||
// To make sure that all connected batteries are being regularly reported, we check which one has the
|
||||
// oldest timestamp.
|
||||
hrt_abstime oldest_update = hrt_absolute_time();
|
||||
size_t num_connected_batteries{0};
|
||||
float worst_battery_time_s{NAN};
|
||||
|
||||
_battery_current = 0.0f;
|
||||
float battery_level = 0.0f;
|
||||
|
||||
for (auto &battery_sub : _battery_status_subs) {
|
||||
battery_status_s battery;
|
||||
|
||||
// Only iterate over connected batteries. We don't care if a disconnected battery is not regularly publishing.
|
||||
for (size_t i = 0; i < num_connected_batteries; i++) {
|
||||
if (batteries[i].warning > worst_warning) {
|
||||
worst_warning = batteries[i].warning;
|
||||
if (!battery_sub.copy(&battery)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&batteries[i].timestamp) > hrt_elapsed_time(&oldest_update)) {
|
||||
oldest_update = batteries[i].timestamp;
|
||||
}
|
||||
if (battery.connected) {
|
||||
num_connected_batteries++;
|
||||
|
||||
// Sum up current from all batteries.
|
||||
_battery_current += batteries[i].current_filtered_a;
|
||||
if (battery.warning > worst_warning) {
|
||||
worst_warning = battery.warning;
|
||||
}
|
||||
|
||||
// average levels from all batteries
|
||||
battery_level += batteries[i].remaining;
|
||||
}
|
||||
if (battery.timestamp < oldest_update) {
|
||||
oldest_update = battery.timestamp;
|
||||
}
|
||||
|
||||
battery_level /= num_connected_batteries;
|
||||
if (PX4_ISFINITE(battery.time_remaining_s)
|
||||
&& (!PX4_ISFINITE(worst_battery_time_s)
|
||||
|| (PX4_ISFINITE(worst_battery_time_s) && (battery.time_remaining_s < worst_battery_time_s)))) {
|
||||
worst_battery_time_s = battery.time_remaining_s;
|
||||
}
|
||||
|
||||
_rtl_flight_time_sub.update();
|
||||
float battery_usage_to_home = 0;
|
||||
|
||||
if (hrt_absolute_time() - _rtl_flight_time_sub.get().timestamp < 2_s) {
|
||||
battery_usage_to_home = _rtl_flight_time_sub.get().rtl_limit_fraction;
|
||||
}
|
||||
|
||||
uint8_t battery_range_warning = battery_status_s::BATTERY_WARNING_NONE;
|
||||
|
||||
if (PX4_ISFINITE(battery_usage_to_home)) {
|
||||
float battery_at_home = battery_level - battery_usage_to_home;
|
||||
|
||||
if (battery_at_home < _param_bat_crit_thr.get()) {
|
||||
battery_range_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
|
||||
|
||||
} else if (battery_at_home < _param_bat_low_thr.get()) {
|
||||
battery_range_warning = battery_status_s::BATTERY_WARNING_LOW;
|
||||
// Sum up current from all batteries.
|
||||
_battery_current += battery.current_filtered_a;
|
||||
}
|
||||
}
|
||||
|
||||
if (battery_range_warning > worst_warning) {
|
||||
worst_warning = battery_range_warning;
|
||||
rtl_time_estimate_s rtl_time_estimate{};
|
||||
|
||||
// Compare estimate of RTL time to estimate of remaining flight time
|
||||
if (_rtl_time_estimate_sub.copy(&rtl_time_estimate)
|
||||
&& hrt_absolute_time() - rtl_time_estimate.timestamp < 2_s
|
||||
&& rtl_time_estimate.valid
|
||||
&& _armed.armed
|
||||
&& !_rtl_time_actions_done
|
||||
&& PX4_ISFINITE(worst_battery_time_s)
|
||||
&& rtl_time_estimate.safe_time_estimate >= worst_battery_time_s
|
||||
&& _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
|
||||
&& _internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
|
||||
// Try to trigger RTL
|
||||
if (main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags,
|
||||
_internal_state) == TRANSITION_CHANGED) {
|
||||
mavlink_log_emergency(&_mavlink_log_pub, "Flight time low, returning to land");
|
||||
|
||||
} else {
|
||||
mavlink_log_emergency(&_mavlink_log_pub, "Flight time low, land now!");
|
||||
}
|
||||
|
||||
_rtl_time_actions_done = true;
|
||||
}
|
||||
|
||||
bool battery_warning_level_increased_while_armed = false;
|
||||
|
|
|
|||
|
|
@ -77,7 +77,7 @@
|
|||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/power_button_state.h>
|
||||
#include <uORB/topics/rtl_flight_time.h>
|
||||
#include <uORB/topics/rtl_time_estimate.h>
|
||||
#include <uORB/topics/safety.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
|
|
@ -321,6 +321,8 @@ private:
|
|||
bool _geofence_warning_action_on{false};
|
||||
bool _geofence_violated_prev{false};
|
||||
|
||||
bool _rtl_time_actions_done{false};
|
||||
|
||||
FailureDetector _failure_detector;
|
||||
bool _flight_termination_triggered{false};
|
||||
bool _lockdown_triggered{false};
|
||||
|
|
@ -408,6 +410,7 @@ private:
|
|||
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _rtl_time_estimate_sub{ORB_ID(rtl_time_estimate)};
|
||||
uORB::Subscription _safety_sub{ORB_ID(safety)};
|
||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
|
|
@ -431,7 +434,6 @@ private:
|
|||
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::SubscriptionData<rtl_flight_time_s> _rtl_flight_time_sub{ORB_ID(rtl_flight_time)};
|
||||
|
||||
// Publications
|
||||
uORB::Publication<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_optional_topic("px4io_status");
|
||||
add_topic("radio_status");
|
||||
add_optional_topic("rpm", 500);
|
||||
add_topic("rtl_flight_time", 1000);
|
||||
add_topic("rtl_time_estimate", 1000);
|
||||
add_topic("safety");
|
||||
add_topic("sensor_combined");
|
||||
add_optional_topic("sensor_correction");
|
||||
|
|
|
|||
|
|
@ -57,6 +57,14 @@ RTL::RTL(Navigator *navigator) :
|
|||
MissionBlock(navigator),
|
||||
ModuleParams(navigator)
|
||||
{
|
||||
_param_mpc_z_vel_max_up = param_find("MPC_Z_VEL_MAX_UP");
|
||||
_param_mpc_z_vel_max_down = param_find("MPC_Z_VEL_MAX_DN");
|
||||
_param_mpc_land_speed = param_find("MPC_LAND_SPEED");
|
||||
_param_fw_climb_rate = param_find("FW_T_CLMB_R_SP");
|
||||
_param_fw_sink_rate = param_find("FW_T_SINK_R_SP");
|
||||
_param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM");
|
||||
_param_mpc_xy_cruise = param_find("MPC_XY_CRUISE");
|
||||
_param_rover_cruise_speed = param_find("GND_SPEED_THR_SC");
|
||||
}
|
||||
|
||||
void RTL::on_inactivation()
|
||||
|
|
@ -71,22 +79,20 @@ void RTL::on_inactive()
|
|||
// Reset RTL state.
|
||||
_rtl_state = RTL_STATE_NONE;
|
||||
|
||||
find_RTL_destination();
|
||||
// Limit inactive calculation to 1Hz
|
||||
if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
|
||||
_destination_check_time = hrt_absolute_time();
|
||||
|
||||
if (_navigator->home_position_valid()) {
|
||||
find_RTL_destination();
|
||||
}
|
||||
|
||||
calc_and_pub_rtl_time_estimate();
|
||||
}
|
||||
}
|
||||
|
||||
void RTL::find_RTL_destination()
|
||||
{
|
||||
// don't update RTL destination faster than 1 Hz
|
||||
if (hrt_elapsed_time(&_destination_check_time) < 1_s) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_navigator->home_position_valid()) {
|
||||
return;
|
||||
}
|
||||
|
||||
_destination_check_time = hrt_absolute_time();
|
||||
|
||||
// get home position:
|
||||
home_position_s &home_landing_position = *_navigator->get_home_position();
|
||||
|
||||
|
|
@ -225,23 +231,13 @@ void RTL::find_RTL_destination()
|
|||
}
|
||||
}
|
||||
|
||||
// figure out how long the RTL will take
|
||||
float rtl_xy_speed, rtl_z_speed;
|
||||
get_rtl_xy_z_speed(rtl_xy_speed, rtl_z_speed);
|
||||
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
|
||||
|
||||
matrix::Vector3f to_destination_vec;
|
||||
get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon,
|
||||
&to_destination_vec(0), &to_destination_vec(1));
|
||||
to_destination_vec(2) = _destination.alt - global_position.alt;
|
||||
|
||||
float time_to_home_s = time_to_home(to_destination_vec, get_wind(), rtl_xy_speed, rtl_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.timestamp = hrt_absolute_time();
|
||||
rtl_flight_time.rtl_limit_fraction = rtl_flight_time_ratio;
|
||||
rtl_flight_time.rtl_time_s = time_to_home_s;
|
||||
_rtl_flight_time_pub.publish(rtl_flight_time);
|
||||
} else {
|
||||
_rtl_alt = max(global_position.alt, max(_destination.alt,
|
||||
_navigator->get_home_position()->alt + _param_rtl_return_alt.get()));
|
||||
}
|
||||
}
|
||||
|
||||
void RTL::on_activation()
|
||||
|
|
@ -268,14 +264,6 @@ void RTL::on_activation()
|
|||
|
||||
_rtl_loiter_rad = _param_rtl_loiter_rad.get();
|
||||
|
||||
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
|
||||
|
||||
} else {
|
||||
_rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get());
|
||||
}
|
||||
|
||||
|
||||
if (_navigator->get_land_detected()->landed) {
|
||||
// For safety reasons don't go into RTL if landed.
|
||||
_rtl_state = RTL_STATE_LANDED;
|
||||
|
|
@ -316,6 +304,12 @@ void RTL::on_active()
|
|||
} else if (_navigator->get_precland()->is_activated()) {
|
||||
_navigator->get_precland()->on_inactivation();
|
||||
}
|
||||
|
||||
// Limit rtl time calculation to 1Hz
|
||||
if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
|
||||
_destination_check_time = hrt_absolute_time();
|
||||
calc_and_pub_rtl_time_estimate();
|
||||
}
|
||||
}
|
||||
|
||||
void RTL::set_rtl_item()
|
||||
|
|
@ -730,41 +724,148 @@ 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(float &xy, float &z)
|
||||
void RTL::calc_and_pub_rtl_time_estimate()
|
||||
{
|
||||
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
|
||||
rtl_time_estimate_s rtl_time_estimate{};
|
||||
|
||||
if (vehicle_type != _rtl_vehicle_type) {
|
||||
_rtl_vehicle_type = vehicle_type;
|
||||
// Calculate RTL time estimate only when there is a valid home position
|
||||
// TODO: Also check if vehicle position is valid
|
||||
if (!_navigator->home_position_valid()) {
|
||||
rtl_time_estimate.valid = false;
|
||||
|
||||
} else {
|
||||
rtl_time_estimate.valid = true;
|
||||
|
||||
const vehicle_global_position_s &gpos = *_navigator->get_global_position();
|
||||
|
||||
// Sum up time estimate for various segments of the landing procedure
|
||||
switch (_rtl_state) {
|
||||
case RTL_STATE_NONE:
|
||||
case RTL_STATE_CLIMB: {
|
||||
// Climb segment is only relevant if the drone is below return altitude
|
||||
const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0;
|
||||
|
||||
if (climb_dist > 0) {
|
||||
rtl_time_estimate.time_estimate += climb_dist / getClimbRate();
|
||||
}
|
||||
}
|
||||
|
||||
// FALLTHROUGH
|
||||
case RTL_STATE_RETURN:
|
||||
|
||||
// Add cruise segment to home
|
||||
rtl_time_estimate.time_estimate += get_distance_to_next_waypoint(
|
||||
_destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed();
|
||||
|
||||
// FALLTHROUGH
|
||||
case RTL_STATE_HEAD_TO_CENTER:
|
||||
case RTL_STATE_TRANSITION_TO_MC:
|
||||
case RTL_STATE_DESCEND: {
|
||||
// when descending, the target altitude is stored in the current mission item
|
||||
float initial_altitude = 0;
|
||||
float loiter_altitude = 0;
|
||||
|
||||
if (_rtl_state == RTL_STATE_DESCEND) {
|
||||
// Take current vehicle altitude as the starting point for calculation
|
||||
initial_altitude = gpos.alt; // TODO: Check if this is in the right frame
|
||||
loiter_altitude = _mission_item.altitude; // Next waypoint = loiter
|
||||
|
||||
|
||||
} else {
|
||||
// Take the return altitude as the starting point for the calculation
|
||||
initial_altitude = _rtl_alt; // CLIMB and RETURN
|
||||
loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
|
||||
}
|
||||
|
||||
// Add descend segment (first landing phase: return alt to loiter alt)
|
||||
rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate();
|
||||
}
|
||||
|
||||
// FALLTHROUGH
|
||||
case RTL_STATE_LOITER:
|
||||
// Add land delay (the short pause for deploying landing gear)
|
||||
// TODO: Check if landing gear is deployed or not
|
||||
rtl_time_estimate.time_estimate += _param_rtl_land_delay.get();
|
||||
|
||||
// FALLTHROUGH
|
||||
case RTL_MOVE_TO_LAND_HOVER_VTOL:
|
||||
case RTL_STATE_LAND: {
|
||||
float initial_altitude;
|
||||
|
||||
// Add land segment (second landing phase) which comes after LOITER
|
||||
if (_rtl_state == RTL_STATE_LAND) {
|
||||
// If we are in this phase, use the current vehicle altitude instead
|
||||
// of the altitude paramteter to get a continous time estimate
|
||||
initial_altitude = gpos.alt;
|
||||
|
||||
|
||||
} else {
|
||||
// If this phase is not active yet, simply use the loiter altitude,
|
||||
// which is where the LAND phase will start
|
||||
const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
|
||||
initial_altitude = loiter_altitude;
|
||||
}
|
||||
|
||||
// Prevent negative times when close to the ground
|
||||
if (initial_altitude > _destination.alt) {
|
||||
rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
switch (vehicle_type) {
|
||||
case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
|
||||
_param_rtl_xy_speed = param_find("MPC_XY_CRUISE");
|
||||
_param_rtl_descent_speed = param_find("MPC_Z_VEL_MAX_DN");
|
||||
break;
|
||||
|
||||
case vehicle_status_s::VEHICLE_TYPE_FIXED_WING:
|
||||
_param_rtl_xy_speed = param_find("FW_AIRSPD_TRIM");
|
||||
_param_rtl_descent_speed = param_find("FW_T_SINK_MIN");
|
||||
case RTL_STATE_LANDED:
|
||||
// Remaining time is 0
|
||||
break;
|
||||
}
|
||||
|
||||
case vehicle_status_s::VEHICLE_TYPE_ROVER:
|
||||
_param_rtl_xy_speed = param_find("GND_SPEED_THR_SC");
|
||||
_param_rtl_descent_speed = PARAM_INVALID;
|
||||
break;
|
||||
// Prevent negative durations as phyiscally they make no sense. These can
|
||||
// occur during the last phase of landing when close to the ground.
|
||||
rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate);
|
||||
|
||||
// Use actual time estimate to compute the safer time estimate with additional scale factor and a margin
|
||||
rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate
|
||||
+ _param_rtl_time_margin.get();
|
||||
}
|
||||
|
||||
// Publish message
|
||||
rtl_time_estimate.timestamp = hrt_absolute_time();
|
||||
_rtl_time_estimate_pub.publish(rtl_time_estimate);
|
||||
}
|
||||
|
||||
float RTL::getCruiseSpeed()
|
||||
{
|
||||
float ret = 1e6f;
|
||||
|
||||
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
|
||||
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
|
||||
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
|
||||
if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
}
|
||||
|
||||
if ((_param_rtl_xy_speed == PARAM_INVALID) || param_get(_param_rtl_xy_speed, &xy) != PX4_OK) {
|
||||
xy = 1e6f;
|
||||
}
|
||||
|
||||
if ((_param_rtl_descent_speed == PARAM_INVALID) || param_get(_param_rtl_descent_speed, &z) != PX4_OK) {
|
||||
z = 1e6f;
|
||||
return ret;
|
||||
}
|
||||
|
||||
float RTL::getHoverLandSpeed()
|
||||
{
|
||||
float ret = 1e6f;
|
||||
|
||||
if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
matrix::Vector2f RTL::get_wind()
|
||||
|
|
@ -780,27 +881,67 @@ matrix::Vector2f RTL::get_wind()
|
|||
return wind;
|
||||
}
|
||||
|
||||
float time_to_home(const matrix::Vector3f &to_home_vec,
|
||||
const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, float vehicle_descent_speed_m_s)
|
||||
float RTL::getClimbRate()
|
||||
{
|
||||
const matrix::Vector2f to_home = to_home_vec.xy();
|
||||
const float alt_change = to_home_vec(2);
|
||||
const matrix::Vector2f to_home_dir = to_home.unit_or_zero();
|
||||
const float dist_to_home = to_home.norm();
|
||||
float ret = 1e6f;
|
||||
|
||||
const float wind_towards_home = wind_velocity.dot(to_home_dir);
|
||||
const float wind_across_home = matrix::Vector2f(wind_velocity - to_home_dir * wind_towards_home).norm();
|
||||
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
if (_param_mpc_z_vel_max_up == PARAM_INVALID || param_get(_param_mpc_z_vel_max_up, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
|
||||
// Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient
|
||||
const float cruise_speed = sqrtf(vehicle_speed_m_s * vehicle_speed_m_s - wind_across_home * wind_across_home) + fminf(
|
||||
0.f, wind_towards_home);
|
||||
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
|
||||
if (!PX4_ISFINITE(cruise_speed) || cruise_speed <= 0) {
|
||||
return INFINITY; // we never reach home if the wind is stronger than vehicle speed
|
||||
if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
}
|
||||
|
||||
// assume horizontal and vertical motions happen serially, so their time adds
|
||||
float horiz = dist_to_home / cruise_speed;
|
||||
float descent = fabsf(alt_change) / vehicle_descent_speed_m_s;
|
||||
return horiz + descent;
|
||||
return ret;
|
||||
}
|
||||
|
||||
float RTL::getDescendRate()
|
||||
{
|
||||
float ret = 1e6f;
|
||||
|
||||
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
if (_param_mpc_z_vel_max_down == PARAM_INVALID || param_get(_param_mpc_z_vel_max_down, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
|
||||
} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) {
|
||||
ret = 1e6f;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
float RTL::getCruiseGroundSpeed()
|
||||
{
|
||||
float cruise_speed = getCruiseSpeed();
|
||||
|
||||
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
|
||||
matrix::Vector2f wind = get_wind();
|
||||
|
||||
matrix::Vector2f to_destination_vec;
|
||||
get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon,
|
||||
&to_destination_vec(0), &to_destination_vec(1));
|
||||
|
||||
const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero();
|
||||
|
||||
const float wind_towards_home = wind.dot(to_home_dir);
|
||||
const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm();
|
||||
|
||||
|
||||
// Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient
|
||||
const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf(
|
||||
0.f, wind_towards_home);
|
||||
|
||||
cruise_speed = ground_speed;
|
||||
}
|
||||
|
||||
return cruise_speed;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -48,7 +48,7 @@
|
|||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/rtl_flight_time.h>
|
||||
#include <uORB/topics/rtl_time_estimate.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/wind.h>
|
||||
#include <matrix/math.hpp>
|
||||
|
|
@ -109,6 +109,17 @@ private:
|
|||
void advance_rtl();
|
||||
|
||||
float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
|
||||
void calc_and_pub_rtl_time_estimate();
|
||||
|
||||
float getCruiseGroundSpeed();
|
||||
|
||||
float getClimbRate();
|
||||
|
||||
float getDescendRate();
|
||||
|
||||
float getCruiseSpeed();
|
||||
|
||||
float getHoverLandSpeed();
|
||||
|
||||
enum RTLState {
|
||||
RTL_STATE_NONE = 0,
|
||||
|
|
@ -159,21 +170,25 @@ private:
|
|||
(ParamFloat<px4::params::RTL_MIN_DIST>) _param_rtl_min_dist,
|
||||
(ParamInt<px4::params::RTL_TYPE>) _param_rtl_type,
|
||||
(ParamInt<px4::params::RTL_CONE_ANG>) _param_rtl_cone_half_angle_deg,
|
||||
(ParamFloat<px4::params::RTL_FLT_TIME>) _param_rtl_flt_time,
|
||||
(ParamInt<px4::params::RTL_PLD_MD>) _param_rtl_pld_md,
|
||||
(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad,
|
||||
(ParamInt<px4::params::RTL_HDG_MD>) _param_rtl_hdg_md
|
||||
(ParamInt<px4::params::RTL_HDG_MD>) _param_rtl_hdg_md,
|
||||
(ParamFloat<px4::params::RTL_TIME_FACTOR>) _param_rtl_time_factor,
|
||||
(ParamInt<px4::params::RTL_TIME_MARGIN>) _param_rtl_time_margin
|
||||
)
|
||||
|
||||
// These need to point at different parameters depending on vehicle type.
|
||||
// Can't hard-code them because we have non-MC/FW/Rover builds
|
||||
uint8_t _rtl_vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN};
|
||||
param_t _param_mpc_z_vel_max_up{PARAM_INVALID};
|
||||
param_t _param_mpc_z_vel_max_down{PARAM_INVALID};
|
||||
param_t _param_mpc_land_speed{PARAM_INVALID};
|
||||
param_t _param_fw_climb_rate{PARAM_INVALID};
|
||||
param_t _param_fw_sink_rate{PARAM_INVALID};
|
||||
|
||||
param_t _param_rtl_xy_speed{PARAM_INVALID};
|
||||
param_t _param_rtl_descent_speed{PARAM_INVALID};
|
||||
param_t _param_fw_airspeed_trim{PARAM_INVALID};
|
||||
param_t _param_mpc_xy_cruise{PARAM_INVALID};
|
||||
param_t _param_rover_cruise_speed{PARAM_INVALID};
|
||||
|
||||
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
|
||||
uORB::Publication<rtl_flight_time_s> _rtl_flight_time_pub{ORB_ID(rtl_flight_time)};
|
||||
uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
|
||||
};
|
||||
|
||||
float time_to_home(const matrix::Vector3f &to_home_vec,
|
||||
|
|
|
|||
|
|
@ -137,17 +137,6 @@ PARAM_DEFINE_INT32(RTL_TYPE, 0);
|
|||
*/
|
||||
PARAM_DEFINE_INT32(RTL_CONE_ANG, 45);
|
||||
|
||||
/**
|
||||
* Maximum allowed RTL flight in minutes
|
||||
*
|
||||
* This is used to determine when the vehicle should be switched to RTL due to low battery.
|
||||
* Note, particularly for multirotors this should reflect flight time at cruise speed, not while stationary
|
||||
*
|
||||
* @unit min
|
||||
* @group Commander
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_FLT_TIME, 15);
|
||||
|
||||
/**
|
||||
* RTL precision land mode
|
||||
*
|
||||
|
|
@ -185,3 +174,32 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
|
|||
* @group Return Mode
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RTL_HDG_MD, 0);
|
||||
|
||||
/**
|
||||
* RTL time estimate safety margin factor
|
||||
*
|
||||
* Safety factor that is used to scale the actual RTL time estiamte.
|
||||
* Time with margin = RTL_TIME_FACTOR * time + RTL_TIME_MARGIN
|
||||
*
|
||||
* @min 1.0
|
||||
* @max 2.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group Return To Land
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(RTL_TIME_FACTOR, 1.1f);
|
||||
|
||||
/**
|
||||
* RTL time estimate safety margin offset
|
||||
*
|
||||
* Margin that is added to the time estimate, after it has already been scaled
|
||||
* Time with margin = RTL_TIME_FACTOR * time + RTL_TIME_MARGIN
|
||||
*
|
||||
* @unit s
|
||||
* @min 0
|
||||
* @max 300
|
||||
* @decimal 1
|
||||
* @increment 1
|
||||
* @group Return To Land
|
||||
*/
|
||||
PARAM_DEFINE_INT32(RTL_TIME_MARGIN, 110);
|
||||
|
|
|
|||
Loading…
Reference in New Issue