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

Merge remote-tracking branch 'px4/beta' into beta_mavlink

Browse Source 
Conflicts:
	src/modules/mavlink/mavlink.c
	src/modules/mavlink/mavlink_receiver.h
	src/modules/mavlink/orb_listener.c
This commit is contained in:
Julian Oes
2014-02-12 17:10:20 +01:00
parent 76ae004e5c 03cfb79b29
commit 3462054f73
172 changed files with 4794 additions and 3548 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink.c
+1 -1
View File
@@ -56,7 +56,7 @@
// #include <stdlib.h>
// #include <poll.h>
//
//
// #include <systemlib/systemlib.h>
// #include <systemlib/err.h>
// #include <mavlink/mavlink_log.h>
src/modules/mavlink/mavlink_main.cpp
+25 -17
View File
@@ -61,7 +61,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <systemlib/param/param.h>
@@ -502,28 +502,36 @@ Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_bas
*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;
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/mavlink_main.h
+4 -4
View File
@@ -57,7 +57,7 @@
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
@@ -233,7 +233,7 @@ public:
int get_sub;
int airspeed_sub;
int navigation_capabilities_sub;
int control_mode_sub;
int position_setpoint_triplet_sub;
int rc_sub;
int status_sub;
int home_sub;
@@ -252,12 +252,12 @@ public:
struct navigation_capabilities_s nav_cap;
/** Vehicle status */
struct vehicle_status_s v_status;
/** Vehicle control mode */
struct vehicle_control_mode_s control_mode;
/** RC channels */
struct rc_channels_s rc;
/** Actuator armed state */
struct actuator_armed_s armed;
/** Position setpoint triplet */
struct position_setpoint_triplet_s pos_sp_triplet;
protected:
/**
src/modules/mavlink/mavlink_orb_listener.cpp
+6 -26
View File
@@ -94,7 +94,6 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0);
add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0);
add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0);
add_listener(MavlinkOrbListener::l_control_mode, &(_mavlink->get_subs()->control_mode_sub), 0);
add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0);
add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0);
add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0);
@@ -277,6 +276,7 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
/* immediately communicate state _mavlink->get_chan()ges back to user */
orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status));
orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->position_setpoint_triplet_sub, &(l->mavlink->pos_sp_triplet));
/* enable or disable HIL */
if (l->listener->v_status.hil_state == HIL_STATE_ON)
@@ -317,10 +317,10 @@ MavlinkOrbListener::l_input_rc(const struct listener *l)
const unsigned port_width = 8;
for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) {
for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(),
l->listener->rc_raw.timestamp / 1000,
l->listener->rc_raw.timestamp_publication / 1000,
i,
(l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
(l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
@@ -644,26 +644,6 @@ MavlinkOrbListener::l_nav_cap(const struct listener *l)
}
void
MavlinkOrbListener::l_control_mode(const struct listener *l)
{
orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->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;
l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
void *
MavlinkOrbListener::uorb_receive_thread(void *arg)
{
@@ -759,9 +739,9 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */
/* --- CONTROL MODE --- */
mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(mavlink->get_subs()->control_mode_sub, 300); /* max 3.33 Hz updates */
/* --- POSITION SETPOINT TRIPLET --- */
mavlink->get_subs()->position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
orb_set_interval(mavlink->get_subs()->position_setpoint_triplet_sub, 0); /* not polled, don't limit */
/* --- RC CHANNELS VALUE --- */
mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels));
src/modules/mavlink/mavlink_orb_listener.h
-3
View File
@@ -57,7 +57,6 @@
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
@@ -158,13 +157,11 @@ private:
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);
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;
src/modules/mavlink/mavlink_receiver.cpp
+2 -2
View File
@@ -330,7 +330,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
tstatus.rxerrors = rstatus.rxerrors;
tstatus.fixed = rstatus.fixed;
if (telemetry_status_pub == 0) {
if (telemetry_status_pub <= 0) {
telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
@@ -613,7 +613,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
hil_global_pos.timestamp = timestamp;
hil_global_pos.valid = true;
hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
src/modules/mavlink/mavlink_receiver.h
+1 -1
View File
@@ -57,7 +57,6 @@
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
@@ -122,6 +121,7 @@ private:
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
struct battery_status_s hil_battery_status;
struct position_setpoint_triplet_s pos_sp_triplet;
orb_advert_t pub_hil_global_pos;
orb_advert_t pub_hil_local_pos;
orb_advert_t pub_hil_attitude;