Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 33 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-06 00:34:38 +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;
}
src/modules/navigator/mission_block.cpp
+28 -26
View File
@@ -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;
src/modules/navigator/mission_feasibility_checker.cpp
+4 -4
View File
@@ -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;
}
src/modules/navigator/navigation.h
+1 -2
View File
@@ -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 <uORB/topics/mission.h>