mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 04:53:33 +08:00
mission feasibility checker: use param_find() to check fixed-wing land angle
This commit is contained in:
committed by
Daniel Agar
parent
4b0a8565fe
commit
ceb432aacb
@@ -487,6 +487,19 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
|
|||||||
if (missionitem.nav_cmd == NAV_CMD_LAND) {
|
if (missionitem.nav_cmd == NAV_CMD_LAND) {
|
||||||
mission_item_s missionitem_previous {};
|
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, ¶m_fw_lnd_ang);
|
||||||
|
}
|
||||||
|
|
||||||
if (i > 0) {
|
if (i > 0) {
|
||||||
landing_approach_index = i - 1;
|
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
|
// rounding on next check with small (arbitrary) 0.1 deg buffer, as the
|
||||||
// landing angle parameter is what is typically used for steepest glide
|
// landing angle parameter is what is typically used for steepest glide
|
||||||
// in landing config
|
// 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) {
|
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_left_of_decimal = (uint8_t)param_fw_lnd_ang;
|
||||||
const uint8_t land_angle_first_after_decimal = (uint8_t)((_param_fw_lnd_ang.get() - floorf(
|
const uint8_t land_angle_first_after_decimal = (uint8_t)((param_fw_lnd_ang - floorf(
|
||||||
_param_fw_lnd_ang.get())) * 10.0f);
|
param_fw_lnd_ang)) * 10.0f);
|
||||||
|
|
||||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
|
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",
|
"Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t",
|
||||||
|
|||||||
@@ -90,8 +90,4 @@ public:
|
|||||||
bool checkMissionFeasible(const mission_s &mission,
|
bool checkMissionFeasible(const mission_s &mission,
|
||||||
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
|
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
|
||||||
bool land_start_req);
|
bool land_start_req);
|
||||||
|
|
||||||
DEFINE_PARAMETERS(
|
|
||||||
(ParamFloat<px4::params::FW_LND_ANG>) _param_fw_lnd_ang
|
|
||||||
)
|
|
||||||
};
|
};
|
||||||
|
|||||||
Reference in New Issue
Block a user