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

refactored do_set_servo handling and generalized formatting of CMD mavlink mission items, fixes #3644

Browse Source
This commit is contained in:
Andreas Antener
2016-02-08 11:09:12 +01:00
parent 46bd1cbacf
commit baa11e357d
4 changed files with 56 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_mission.cpp
+23 -32
View File
@@ -793,6 +793,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
return MAV_MISSION_UNSUPPORTED_FRAME;
}
mission_item->frame = mavlink_mission_item->frame;
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param1;
@@ -804,12 +806,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
case MAV_CMD_DO_SET_SERVO:
mission_item->actuator_num = mavlink_mission_item->param1;
mission_item->actuator_value = mavlink_mission_item->param2;
mission_item->time_inside=0.0f;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
@@ -842,52 +838,47 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
int
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
{
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
mavlink_mission_item->frame = mission_item->frame;
mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
/* default mappings for generic commands */
if (mission_item->frame == MAV_FRAME_MISSION) {
mavlink_mission_item->param1 = mission_item->params[0];
mavlink_mission_item->param2 = mission_item->params[1];
mavlink_mission_item->param3 = mission_item->params[2];
mavlink_mission_item->param4 = mission_item->params[3];
mavlink_mission_item->x = mission_item->params[4];
mavlink_mission_item->y = mission_item->params[5];
mavlink_mission_item->z = mission_item->params[6];
// default mappings for regular waypoints
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
}
// specific item handling
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
case NAV_CMD_DO_SET_SERVO:
mavlink_mission_item->frame = MAV_FRAME_MISSION;
mavlink_mission_item->param1 = mission_item->actuator_num;
mavlink_mission_item->param2 = mission_item->actuator_value;
break;
case NAV_CMD_DO_JUMP:
mavlink_mission_item->frame = MAV_FRAME_MISSION;
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
break;
case NAV_CMD_DO_VTOL_TRANSITION:
mavlink_mission_item->frame = MAV_FRAME_MISSION;
mavlink_mission_item->param1 = mission_item->params[0];
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
return OK;
}