fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mission feasibility checker: use param_find() to check fixed-wing land angle

Browse Source
This commit is contained in:
Thomas Stastny 2022-07-05 10:52:37 +02:00 committed by Daniel Agar
parent 4b0a8565fe
commit ceb432aacb
2 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -487,6 +487,19 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
if (missionitem.nav_cmd == NAV_CMD_LAND) {
mission_item_s missionitem_previous {};
float param_fw_lnd_ang = 0.0f;
const param_t param_handle_fw_lnd_ang = param_find("FW_LND_ANG");
if (param_handle_fw_lnd_ang == PARAM_INVALID) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: FW_LND_ANG parameter is missing.\t");
events::send(events::ID("navigator_mis_land_angle_param_missing"), {events::Log::Error, events::LogInternal::Info},
"Mission rejected: FW_LND_ANG parameter is missing");
return false;
} else {
param_get(param_handle_fw_lnd_ang, &param_fw_lnd_ang);
}
if (i > 0) {
landing_approach_index = i - 1;
@ -553,13 +566,13 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
// rounding on next check with small (arbitrary) 0.1 deg buffer, as the
// landing angle parameter is what is typically used for steepest glide
// in landing config
const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get() + 0.1f));
const float max_glide_slope = tanf(math::radians(param_fw_lnd_ang + 0.1f));
if (glide_slope > max_glide_slope) {
const uint8_t land_angle_left_of_decimal = (uint8_t)_param_fw_lnd_ang.get();
const uint8_t land_angle_first_after_decimal = (uint8_t)((_param_fw_lnd_ang.get() - floorf(
_param_fw_lnd_ang.get())) * 10.0f);
const uint8_t land_angle_left_of_decimal = (uint8_t)param_fw_lnd_ang;
const uint8_t land_angle_first_after_decimal = (uint8_t)((param_fw_lnd_ang - floorf(
param_fw_lnd_ang)) * 10.0f);
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t",

4
src/modules/navigator/mission_feasibility_checker.h
View File

@ -90,8 +90,4 @@ public:
bool checkMissionFeasible(const mission_s &mission,
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
bool land_start_req);
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang
)
};