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

mavlink: AUTO states indication fixed

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-28 00:03:29 +01:00
parent 7d334ed54f
commit 3fdb082cb8
5 changed files with 55 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+25 -22
View File
@@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
}
/* arming state */
if (v_status.arming_state == ARMING_STATE_ARMED
|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
if (armed.armed) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@@ -208,33 +207,37 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} else if (v_status.main_state == MAIN_STATE_AUTO) {
if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
/* use main state when navigator is not active */
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} 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;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
}
} else {
/* use navigation state when navigator is active */
*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 get nav state
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
/*
if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen
if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (control_mode.nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (control_mode.nav_state == NAV_STATE_LOITER) {
} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (control_mode.nav_state == NAV_STATE_MISSION) {
} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (control_mode.nav_state == NAV_STATE_RTL) {
} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
*/
}
*mavlink_custom_mode = custom_mode.data;
src/modules/mavlink/orb_listener.c
+5 -26
View File
@@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos;
struct home_position_s home;
struct navigation_capabilities_s nav_cap;
struct vehicle_status_s v_status;
struct vehicle_control_mode_s control_mode;
struct position_setpoint_triplet_s pos_sp_triplet;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
@@ -127,7 +127,6 @@ 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},
@@ -154,7 +153,6 @@ 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]);
@@ -315,6 +313,7 @@ l_vehicle_status(const struct listener *l)
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet);
/* enable or disable HIL */
if (v_status.hil_state == HIL_STATE_ON)
@@ -682,26 +681,6 @@ 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)
{
@@ -777,9 +756,9 @@ 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 */
/* --- POSITION SETPOINT TRIPLET --- */
mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0); /* not polled, don't limit */
/* --- RC CHANNELS VALUE --- */
rc_sub = orb_subscribe(ORB_ID(rc_channels));
src/modules/mavlink/orb_topics.h
+3 -3
View File
@@ -94,7 +94,7 @@ struct mavlink_subscriptions {
int home_sub;
int airspeed_sub;
int navigation_capabilities_sub;
int control_mode_sub;
int position_setpoint_triplet_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
@@ -111,8 +111,8 @@ 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;
/** Position setpoint triplet */
extern struct position_setpoint_triplet_s pos_sp_triplet;
/** RC channels */
extern struct rc_channels_s rc;