diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5fed426c3c..dfa5dcfa31 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -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; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4d4effd6cc..aa7b48d6b2 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -83,18 +83,8 @@ MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { - // XXX: move handling to issue_command() as well - case NAV_CMD_DO_SET_SERVO: { - memset(&actuators, 0, sizeof(actuators)); - actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value; - actuators.timestamp = hrt_absolute_time(); - if (_actuator_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); - } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); - } + case NAV_CMD_DO_SET_SERVO: return true; - } case NAV_CMD_LAND: return _navigator->get_vstatus()->condition_landed; @@ -258,16 +248,34 @@ MissionBlock::issue_command(const struct mission_item_s *item) return; } - warnx("forwarding command %d\n", item->nav_cmd); - struct vehicle_command_s cmd = {}; - mission_item_to_vehicle_command(item, &cmd); - _action_start = hrt_absolute_time(); + if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) { + PX4_WARN("do_set_servo command"); + // XXX: we should issue a vehicle command and handle this somewhere else + memset(&actuators, 0, sizeof(actuators)); + // params[0] actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 + // params[1] new value for selected actuator in ms 900...2000 + actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1]; + actuators.timestamp = hrt_absolute_time(); - if (_cmd_pub != nullptr) { - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + if (_actuator_pub != nullptr) { + orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); + } } else { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); + PX4_WARN("forwarding command %d\n", item->nav_cmd); + struct vehicle_command_s cmd = {}; + mission_item_to_vehicle_command(item, &cmd); + _action_start = hrt_absolute_time(); + + if (_cmd_pub != nullptr) { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd); + + } else { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd); + } } } @@ -276,7 +284,8 @@ MissionBlock::item_contains_position(const struct mission_item_s *item) { // XXX: maybe extend that check onto item properties if (item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || - item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { + item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || + item->nav_cmd == NAV_CMD_DO_SET_SERVO) { return false; } @@ -303,13 +312,6 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->acceptance_radius = item->acceptance_radius; switch (item->nav_cmd) { - case NAV_CMD_DO_SET_SERVO: // XXX: actually also a non position item - /* Set current position for loitering set point*/ - sp->lat = _navigator->get_global_position()->lat; - sp->lon = _navigator->get_global_position()->lon; - sp->alt = _navigator->get_global_position()->alt; - sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; - break; case NAV_CMD_IDLE: sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE; break; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 5ec3bd8cb7..067aa74edc 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -347,14 +347,14 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO){ /* check actuator number */ - if (mission_item.actuator_num < 0 || mission_item.actuator_num > 5) { - mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.actuator_num); + if (mission_item.params[0] < 0 || mission_item.params[0] > 5) { + mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]); warning_issued = true; return false; } /* check actuator value */ - if (mission_item.actuator_value < -2000 || mission_item.actuator_value > 2000) { - mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.actuator_value); + if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) { + mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]); warning_issued = true; return false; } diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index f5175b3300..4f58256832 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -103,9 +103,8 @@ struct mission_item_s { int do_jump_mission_index; /**< index where the do jump will go to */ unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ unsigned do_jump_current_count; /**< count how many times the jump has been done */ - int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ - int actuator_value; /**< new value for selected actuator in ms 900...2000 */ float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/ + int8_t frame; /**< mission frame ***/ }; #pragma pack(pop) #include