dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'navigator_new' into navigator_new_vector

This commit is contained in:
Anton Babushkin 2013-12-30 14:42:00 +04:00
parent 35e62a13f8 c35c0a90d3
commit 6c30eebeb8
7 changed files with 166 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/modules/commander/commander.cpp
View File

@ -193,7 +193,7 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
/**
* Mainloop of commander.
@ -345,10 +345,11 @@ void print_status()
static orb_advert_t status_pub;
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
bool ret = false;
/* only handle high-priority commands here */
@ -375,6 +376,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
if (hil_ret == OK)
ret = true;
// TODO remove debug code
//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
@ -407,6 +410,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
}
}
if (arming_res == TRANSITION_CHANGED)
ret = true;
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
@ -447,6 +452,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
}
if (main_res == TRANSITION_CHANGED)
ret = true;
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
@ -473,6 +480,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
} else {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
@ -482,12 +490,36 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
case VEHICLE_CMD_OVERRIDE_GOTO: {
// TODO listen vehicle_command topic directly from navigator (?)
unsigned int mav_goto = cmd->param1;
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->set_nav_state = NAV_STATE_LOITER;
status->set_nav_state_timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status->set_nav_state = NAV_STATE_MISSION;
status->set_nav_state_timestamp = hrt_absolute_time();
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
} else {
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
}
}
break;
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
} else {
/* reject parachute depoyment not armed */
@ -1151,7 +1183,8 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
handle_command(&status, &safety, &cmd, &armed);
if (handle_command(&status, &safety, &cmd, &armed))
status_changed = true;
}
/* check which state machines for changes, clear "changed" flag */
@ -1359,7 +1392,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
current_status->mode_switch = MODE_SWITCH_ASSISTED;
}
/* land switch */
/* return switch */
if (!isfinite(sp_man->return_switch)) {
current_status->return_switch = RETURN_SWITCH_NONE;
@ -1367,7 +1400,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
current_status->return_switch = RETURN_SWITCH_RETURN;
} else {
current_status->return_switch = RETURN_SWITCH_NONE;
current_status->return_switch = RETURN_SWITCH_NORMAL;
}
/* assisted switch */
@ -1383,10 +1416,10 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
/* mission switch */
if (!isfinite(sp_man->mission_switch)) {
current_status->mission_switch = MISSION_SWITCH_MISSION;
current_status->mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
current_status->mission_switch = MISSION_SWITCH_NONE;
current_status->mission_switch = MISSION_SWITCH_LOITER;
} else {
current_status->mission_switch = MISSION_SWITCH_MISSION;

20
src/modules/mavlink/mavlink.c
View File

@ -220,22 +220,18 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
// TODO use control_mode topic
/*
if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
// TODO review
if (control_mode.nav_state == NAV_STATE_NONE) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
} else if (control_mode.nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
} else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
*/
}
*mavlink_custom_mode = custom_mode.data;

27
src/modules/mavlink/orb_listener.c
View File

@ -69,6 +69,7 @@ struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct navigation_capabilities_s nav_cap;
struct vehicle_status_s v_status;
struct vehicle_control_mode_s control_mode;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
@ -125,6 +126,7 @@ static void l_vehicle_rates_setpoint(const struct listener *l);
static void l_home(const struct listener *l);
static void l_airspeed(const struct listener *l);
static void l_nav_cap(const struct listener *l);
static void l_control_mode(const struct listener *l);
static const struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@ -151,6 +153,7 @@ static const struct listener listeners[] = {
{l_home, &mavlink_subs.home_sub, 0},
{l_airspeed, &mavlink_subs.airspeed_sub, 0},
{l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0},
{l_control_mode, &mavlink_subs.control_mode_sub, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@ -678,6 +681,26 @@ l_nav_cap(const struct listener *l)
}
void
l_control_mode(const struct listener *l)
{
orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
static void *
uorb_receive_thread(void *arg)
{
@ -753,6 +776,10 @@ uorb_receive_start(void)
status_sub = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
/* --- CONTROL MODE --- */
mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */
/* --- RC CHANNELS VALUE --- */
rc_sub = orb_subscribe(ORB_ID(rc_channels));
orb_set_interval(rc_sub, 100); /* 10Hz updates */

4
src/modules/mavlink/orb_topics.h
View File

@ -95,6 +95,7 @@ struct mavlink_subscriptions {
int home_sub;
int airspeed_sub;
int navigation_capabilities_sub;
int control_mode_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
@ -111,6 +112,9 @@ extern struct navigation_capabilities_s nav_cap;
/** Vehicle status */
extern struct vehicle_status_s v_status;
/** Vehicle control mode */
extern struct vehicle_control_mode_s control_mode;
/** RC channels */
extern struct rc_channels_s rc;

106
src/modules/navigator/navigator_main.cpp
View File

@ -526,6 +526,8 @@ Navigator::task_main()
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
unsigned prevState = 0;
/* wakeup source(s) */
struct pollfd fds[7];
@ -545,7 +547,6 @@ Navigator::task_main()
fds[6].fd = _vstatus_sub;
fds[6].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 100ms for data */
@ -571,41 +572,75 @@ Navigator::task_main()
/* Evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.main_state == MAIN_STATE_AUTO) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) {
case NAV_STATE_INIT:
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
dispatch(EVENT_MISSION_REQUESTED);
break;
case NAV_STATE_RTL:
bool stick_mode = false;
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
dispatch(EVENT_RTL_REQUESTED);
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* try mission, if none is available fallback to loiter instead */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
stick_mode = true;
} else {
dispatch(EVENT_LOITER_REQUESTED);
/* MISSION switch */
if (!stick_mode) {
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
/* switch to mission only if available */
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
stick_mode = true;
}
}
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
dispatch(EVENT_LOITER_REQUESTED);
stick_mode = true;
}
}
}
if (!stick_mode) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) {
case NAV_STATE_INIT:
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAV_STATE_LOITER:
dispatch(EVENT_LOITER_REQUESTED);
break;
case NAV_STATE_MISSION:
dispatch(EVENT_MISSION_REQUESTED);
break;
case NAV_STATE_RTL:
dispatch(EVENT_RTL_REQUESTED);
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* on first switch to AUTO try mission, if none is available fallback to loiter instead */
if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) {
if (_mission.current_mission_available()) {
dispatch(EVENT_MISSION_REQUESTED);
} else {
dispatch(EVENT_LOITER_REQUESTED);
}
}
}
break;
}
} else {
@ -673,6 +708,11 @@ Navigator::task_main()
}
}
if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
prevState = myState;
}
publish_control_mode();
perf_end(_loop_perf);

24
src/modules/sensors/sensors.cpp
View File

@ -1370,18 +1370,6 @@ Sensors::rc_poll()
manual_control.yaw *= _parameters.rc_scale_yaw;
}
/* mode switch input */
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
/* land switch input */
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
/* assisted switch input */
manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
/* mission switch input */
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
/* flaps */
if (_rc.function[FLAPS] >= 0) {
@ -1392,14 +1380,26 @@ Sensors::rc_poll()
}
}
/* mode switch input */
if (_rc.function[MODE] >= 0) {
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
}
/* assisted switch input */
if (_rc.function[ASSISTED] >= 0) {
manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
}
/* mission switch input */
if (_rc.function[MISSION] >= 0) {
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
/* return switch input */
if (_rc.function[RETURN] >= 0) {
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
}
// if (_rc.function[OFFBOARD_MODE] >= 0) {
// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
// }

2
src/modules/uORB/topics/vehicle_status.h
View File

@ -99,11 +99,13 @@ typedef enum {
typedef enum {
RETURN_SWITCH_NONE = 0,
RETURN_SWITCH_NORMAL,
RETURN_SWITCH_RETURN
} return_switch_pos_t;
typedef enum {
MISSION_SWITCH_NONE = 0,
MISSION_SWITCH_LOITER,
MISSION_SWITCH_MISSION
} mission_switch_pos_t;