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

Some mavlink fixes to use vehicle_control_mode, WIP

This commit is contained in:
Anton Babushkin 2013-12-29 22:38:22 +04:00
parent 95bdc1a9bd
commit fa20fae6fb
4 changed files with 75 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
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 */

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;