dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

navigator: RTL don't get invalid params

This commit is contained in:
Daniel Agar 2021-02-12 16:29:26 -05:00 committed by Lorenz Meier
parent 1981519aad
commit ce552a3364
2 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/navigator/rtl.cpp
View File

@ -642,27 +642,27 @@ void RTL::get_rtl_xy_z_speed(float &xy, float &z)
switch (vehicle_type) {
case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
_rtl_xy_speed = param_find("MPC_XY_CRUISE");
_rtl_descent_speed = param_find("MPC_Z_VEL_MAX_DN");
_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:
_rtl_xy_speed = param_find("FW_AIRSPD_TRIM");
_rtl_descent_speed = param_find("FW_T_SINK_MIN");
_param_rtl_xy_speed = param_find("FW_AIRSPD_TRIM");
_param_rtl_descent_speed = param_find("FW_T_SINK_MIN");
break;
case vehicle_status_s::VEHICLE_TYPE_ROVER:
_rtl_xy_speed = param_find("GND_SPEED_THR_SC");
_rtl_descent_speed = 65535;
_param_rtl_xy_speed = param_find("GND_SPEED_THR_SC");
_param_rtl_descent_speed = PARAM_INVALID;
break;
}
}
if (param_get(_rtl_xy_speed, &xy) != 0) {
if ((_param_rtl_xy_speed == PARAM_INVALID) || param_get(_param_rtl_xy_speed, &xy) != PX4_OK) {
xy = 1e6f;
}
if (param_get(_rtl_descent_speed, &z) != 0) {
if ((_param_rtl_descent_speed == PARAM_INVALID) || param_get(_param_rtl_descent_speed, &z) != PX4_OK) {
z = 1e6f;
}

5
src/modules/navigator/rtl.h
View File

@ -165,8 +165,9 @@ private:
// 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 _rtl_xy_speed{PARAM_INVALID};
param_t _rtl_descent_speed{PARAM_INVALID};
param_t _param_rtl_xy_speed{PARAM_INVALID};
param_t _param_rtl_descent_speed{PARAM_INVALID};
uORB::SubscriptionData<wind_estimate_s> _wind_estimate_sub{ORB_ID(wind_estimate)};
uORB::Publication<rtl_flight_time_s> _rtl_flight_time_pub{ORB_ID(rtl_flight_time)};