mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-02 03:49:12 +08:00
add minimal pitch field to mission item
This commit is contained in:
@@ -103,11 +103,20 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
|
|||||||
return MAV_MISSION_ERROR;
|
return MAV_MISSION_ERROR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
switch (mavlink_mission_item->command) {
|
||||||
|
case MAV_CMD_NAV_TAKEOFF:
|
||||||
|
mission_item->pitch_min = mavlink_mission_item->param1;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
mission_item->radius = mavlink_mission_item->param1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
|
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
|
||||||
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
|
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
|
||||||
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
|
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
|
||||||
mission_item->nav_cmd = mavlink_mission_item->command;
|
mission_item->nav_cmd = mavlink_mission_item->command;
|
||||||
mission_item->radius = mavlink_mission_item->param1;
|
|
||||||
mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
|
mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
|
||||||
mission_item->autocontinue = mavlink_mission_item->autocontinue;
|
mission_item->autocontinue = mavlink_mission_item->autocontinue;
|
||||||
mission_item->index = mavlink_mission_item->seq;
|
mission_item->index = mavlink_mission_item->seq;
|
||||||
|
|||||||
@@ -89,6 +89,7 @@ struct mission_item_s
|
|||||||
enum NAV_CMD nav_cmd; /**< navigation command */
|
enum NAV_CMD nav_cmd; /**< navigation command */
|
||||||
float radius; /**< radius in which the mission is accepted as reached in meters */
|
float radius; /**< radius in which the mission is accepted as reached in meters */
|
||||||
float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */
|
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 */
|
||||||
bool autocontinue; /**< true if next waypoint should follow after this one */
|
bool autocontinue; /**< true if next waypoint should follow after this one */
|
||||||
int index; /**< index matching the mavlink waypoint */
|
int index; /**< index matching the mavlink waypoint */
|
||||||
enum ORIGIN origin; /**< where the waypoint has been generated */
|
enum ORIGIN origin; /**< where the waypoint has been generated */
|
||||||
|
|||||||
Reference in New Issue
Block a user