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

mavlink: AUTO states indication fixed

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/modules/mavlink/mavlink.c
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;

31
src/modules/mavlink/orb_listener.c
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));

6
src/modules/mavlink/orb_topics.h
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;

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

@ -189,6 +189,8 @@ private:
uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */
bool _pos_sp_triplet_updated;
char *nav_states_str[NAV_STATE_MAX];
struct {
@ -399,6 +401,7 @@ Navigator::Navigator() :
_mission_item_valid(false),
_need_takeoff(true),
_do_takeoff(false),
_pos_sp_triplet_updated(false),
_geofence_violation_warning_sent(false)
{
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
@ -835,6 +838,11 @@ Navigator::task_main()
}
}
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
publish_position_setpoint_triplet();
}
/* notify user about state changes */
if (myState != prevState) {
mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
@ -999,7 +1007,7 @@ Navigator::start_none()
_do_takeoff = false;
_rtl_state = RTL_STATE_NONE;
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1018,7 +1026,7 @@ Navigator::start_ready()
_rtl_state = RTL_STATE_NONE;
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1061,7 +1069,7 @@ Navigator::start_loiter()
_pos_sp_triplet.next.valid = false;
_mission_item_valid = false;
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1174,7 +1182,7 @@ Navigator::set_mission_item()
}
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1280,7 +1288,7 @@ Navigator::set_rtl_item()
_pos_sp_triplet.next.valid = false;
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", climb_alt - _home_pos.alt);
mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
break;
}
@ -1345,7 +1353,7 @@ Navigator::set_rtl_item()
}
}
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = true;
}
void
@ -1540,9 +1548,12 @@ Navigator::on_mission_item_reached()
void
Navigator::publish_position_setpoint_triplet()
{
/* lazily publish the mission triplet only once available */
/* update navigation state */
_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub > 0) {
/* publish the mission triplet */
/* publish the position setpoint triplet */
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
} else {

3
src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -45,6 +45,7 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics
@ -82,6 +83,8 @@ struct position_setpoint_triplet_s
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
nav_state_t nav_state; /**< navigation state */
};
/**