diff --git a/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan b/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan index e8dd595993..03986f692e 100644 --- a/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan +++ b/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan @@ -22,7 +22,7 @@ "doJumpId": 1, "frame": 3, "params": [ - 15, + 0, 0, 0, null, diff --git a/integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan b/integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan index 8e2b37540b..f1b04ffd63 100644 --- a/integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan +++ b/integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan @@ -22,7 +22,7 @@ "doJumpId": 1, "frame": 3, "params": [ - 15, + 0, 0, 0, null, diff --git a/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan b/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan index fc0f17560b..8068b43127 100644 --- a/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan +++ b/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan @@ -22,7 +22,7 @@ "doJumpId": 1, "frame": 3, "params": [ - 15, + 0, 0, 0, null, diff --git a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan index 88c9d0f4d5..1698c6b2bc 100644 --- a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan +++ b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan @@ -22,7 +22,7 @@ "doJumpId": 1, "frame": 3, "params": [ - 15, + 0, 0, 0, null, diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg index b7857f2640..403ece57b0 100644 --- a/msg/position_setpoint.msg +++ b/msg/position_setpoint.msg @@ -42,7 +42,6 @@ int8 landing_gear # landing gear: see definition of the states in landing_gear. float32 loiter_radius # loiter radius (only for fixed wing), in m int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW -float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints float32 a_x # acceleration x setpoint float32 a_y # acceleration y setpoint diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 49a1b3b4b4..5de4f3d56d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1222,7 +1222,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here? _param_fw_thr_cruise.get(), _runway_takeoff.climbout(), - radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _param_fw_p_lim_min.get())), + radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())), tecs_status_s::TECS_MODE_TAKEOFF); // assign values @@ -1293,7 +1293,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f takeoff_throttle, _param_fw_thr_cruise.get(), true, - max(radians(pos_sp_curr.pitch_min), radians(10.0f)), + radians(_takeoff_pitch_min.get()), tecs_status_s::TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ @@ -1320,7 +1320,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = max(radians(pos_sp_curr.pitch_min), radians(10.0f)); + _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); } } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 28ef26b1d7..ade6314bf6 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -417,7 +417,9 @@ private: (ParamFloat) _param_fw_man_p_max, (ParamFloat) _param_fw_man_r_max, - (ParamFloat) _param_nav_loiter_rad + (ParamFloat) _param_nav_loiter_rad, + + (ParamFloat) _takeoff_pitch_min ) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 6d988bc9be..229b60b33b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -268,6 +268,18 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); */ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); +/** + * Minimum pitch during takeoff. + * + * @unit deg + * @min -5.0 + * @max 30.0 + * @decimal 1 + * @increment 0.5 + * @group FW L1 Control + */ +PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); + /** * * diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index 4e33bd82a5..fc85cc2f90 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -221,13 +221,12 @@ bool RunwayTakeoff::resetIntegrators() * the climbtout minimum pitch (parameter). * Otherwise use the minimum that is enforced generally (parameter). */ -float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +float RunwayTakeoff::getMinPitch(float climbout_min, float min) { if (_state < RunwayTakeoffState::FLY) { - return math::max(sp_min, climbout_min); - } + return climbout_min; - else { + } else { return min; } } diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index b619f8b384..b42ced184b 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -86,7 +86,7 @@ public: float getYaw(float navigatorYaw); float getThrottle(const hrt_abstime &now, float tecsThrottle); bool resetIntegrators(); - float getMinPitch(float sp_min, float climbout_min, float min); + float getMinPitch(float climbout_min, float min); float getMaxPitch(float max); matrix::Vector2f getStartWP(); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7de65addc3..5d50dc2c29 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1305,13 +1305,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mission_item->altitude_is_relative = true; } - /* this field is shared with pitch_min (and circle_radius for geofence) in memory and - * exclusive in the MAVLink spec. Set it to 0 first - * and then set minimum pitch later only for the - * corresponding item - */ - mission_item->time_inside = 0.0f; - switch (mavlink_mission_item->command) { case MAV_CMD_NAV_WAYPOINT: mission_item->nav_cmd = NAV_CMD_WAYPOINT; @@ -1345,9 +1338,17 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * break; case MAV_CMD_NAV_TAKEOFF: - mission_item->nav_cmd = NAV_CMD_TAKEOFF; - mission_item->pitch_min = mavlink_mission_item->param1; - mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + + // reject takeoff item if minimum pitch (parameter 1) is set + if (PX4_ISFINITE(mavlink_mission_item->param1) && (fabsf(mavlink_mission_item->param1) > FLT_EPSILON)) { + _mavlink->send_statustext_critical("Takeoff rejected, remove deprecated minimum pitch"); + return MAV_MISSION_INVALID_PARAM1; + + } else { + mission_item->nav_cmd = NAV_CMD_TAKEOFF; + mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4)); + } + break; case MAV_CMD_NAV_LOITER_TO_ALT: @@ -1652,7 +1653,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * break; case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; mavlink_mission_item->param4 = math::degrees(mission_item->yaw); break; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d563644e3e..d436e740fe 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -752,11 +752,6 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; - /* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero - * since in NAV_CMD_TAKEOFF mode there is currently no time_inside. - * Note also that resetting time_inside to zero will cause pitch_min to be zero as well. - */ - _mission_item.time_inside = 0.0f; } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_DEFAULT @@ -783,10 +778,6 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; - /* since _mission_item.time_inside and and _mission_item.pitch_min build a union, we need to set time_inside to zero - * since in NAV_CMD_TAKEOFF mode there is currently no time_inside. - */ - _mission_item.time_inside = 0.0f; } /* if we just did a VTOL takeoff, prepare transition */ @@ -1864,7 +1855,6 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit (p1->yawspeed_valid == p2->yawspeed_valid) && (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && (p1->loiter_direction == p2->loiter_direction) && - (fabsf(p1->pitch_min - p2->pitch_min) < FLT_EPSILON) && (fabsf(p1->a_x - p2->a_x) < FLT_EPSILON) && (fabsf(p1->a_y - p2->a_y) < FLT_EPSILON) && (fabsf(p1->a_z - p2->a_z) < FLT_EPSILON) && diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1d4163dd9e..a37c2226c1 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -582,9 +582,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi } else { sp->type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - - // set pitch and ensure that the hold time is zero - sp->pitch_min = item.pitch_min; } break; @@ -667,7 +664,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) } void -MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch) +MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude) { item->nav_cmd = NAV_CMD_TAKEOFF; @@ -680,7 +677,6 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude, item->altitude_is_relative = false; item->loiter_radius = _navigator->get_loiter_radius(); - item->pitch_min = min_pitch; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index b432e9f2cd..be87ea307a 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -116,7 +116,7 @@ protected: /** * Set a takeoff mission item */ - void set_takeoff_item(struct mission_item_s *item, float abs_altitude, float min_pitch = 0.0f); + void set_takeoff_item(struct mission_item_s *item, float abs_altitude); /** * Set a land mission item diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index deef0b5827..50cd362f26 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -153,7 +153,6 @@ struct mission_item_s { struct { union { float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ - float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ float circle_radius; /**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */ }; float acceptance_radius; /**< default radius in which the mission is accepted as reached in meters */