aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink_main.cpp18
-rw-r--r--src/modules/mavlink/mavlink_main.h50
-rw-r--r--src/modules/mavlink/mavlink_orb_listener.cpp557
-rw-r--r--src/modules/mavlink/mavlink_orb_listener.h94
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp24
-rw-r--r--src/modules/mavlink/mavlink_receiver.h4
6 files changed, 426 insertions, 321 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1c7986cbb..cd37c5437 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -56,7 +56,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
-#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -196,6 +195,11 @@ Mavlink::~Mavlink()
}
}
+void Mavlink::set_mode(enum MAVLINK_MODE mode)
+{
+ _mode = mode;
+}
+
int Mavlink::instance_count()
{
/* note: a local buffer count will help if this ever is called often */
@@ -1506,7 +1510,7 @@ Mavlink::task_main(int argc, char *argv[])
break;
case 'o':
- mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
+ _mode = MODE_ONBOARD;
break;
default:
@@ -1523,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[])
warnx("MAVLink v1.0 serial interface starting...");
/* inform about mode */
- warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
+ warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
/* Flush stdout in case MAVLink is about to take it over */
fflush(stdout);
@@ -1541,12 +1545,12 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system();
/* start the MAVLink receiver */
- MavlinkReceiver rcv(this);
- receive_thread = rcv.receive_start(uart);
+ // MavlinkReceiver rcv(this);
+ receive_thread = MavlinkReceiver::receive_start(this);
/* start the ORB receiver */
- MavlinkOrbListener listener(this);
- uorb_receive_thread = listener.uorb_receive_start();
+ //MavlinkOrbListener listener(this);
+ uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this);
/* initialize waypoint manager */
mavlink_wpm_init(wpm);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 244af04a6..3b6714559 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -54,9 +54,7 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -72,7 +70,8 @@
#include <uORB/topics/battery_status.h>
#include <drivers/drv_rc_input.h>
#include <uORB/topics/navigation_capabilities.h>
-
+#include <uORB/topics/mission.h>
+#include <systemlib/param/param.h>
#include <nuttx/fs/fs.h>
@@ -139,6 +138,7 @@ struct mavlink_wpm_storage {
class Mavlink
{
+
public:
/**
* Constructor
@@ -170,6 +170,31 @@ public:
static int get_uart_fd(unsigned index);
+ int get_uart_fd() { return _mavlink_fd; }
+
+ enum MAVLINK_MODE {
+ MODE_TX_HEARTBEAT_ONLY=0,
+ MODE_OFFBOARD,
+ MODE_ONBOARD,
+ MODE_HIL
+ };
+
+ void set_mode(enum MAVLINK_MODE);
+ enum MAVLINK_MODE get_mode();
+
+ bool hil_enabled() { return _mavlink_hil_enabled; };
+
+ /**
+ * Handle waypoint related messages.
+ */
+ void mavlink_wpm_message_handler(const mavlink_message_t *msg);
+
+ /**
+ * Handle parameter related messages.
+ */
+ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+
+
struct mavlink_subscriptions {
int sensor_sub;
int att_sub;
@@ -196,10 +221,15 @@ public:
int airspeed_sub;
int navigation_capabilities_sub;
int control_mode_sub;
+ int rc_sub;
+ int status_sub;
};
struct mavlink_subscriptions subs;
+ struct mavlink_subscriptions* get_subs() { return &subs; }
+ mavlink_channel_t get_chan() { return chan; }
+
/** Global position */
struct vehicle_global_position_s global_pos;
/** Local position */
@@ -243,6 +273,7 @@ private:
uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
bool thread_running;
bool thread_should_exit;
+ MAVLINK_MODE _mode;
uint8_t mavlink_wpm_comp_id;
mavlink_channel_t chan;
@@ -269,12 +300,6 @@ private:
*/
unsigned int mavlink_param_queue_index;
- /* interface mode */
- enum {
- MAVLINK_INTERFACE_MODE_OFFBOARD,
- MAVLINK_INTERFACE_MODE_ONBOARD
- } mavlink_link_mode;
-
struct mavlink_logbuffer lb;
bool mavlink_link_termination_allowed;
@@ -293,12 +318,6 @@ private:
*/
int set_hil_on_off(bool hil_enabled);
-
- /**
- * Handle parameter related messages.
- */
- void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
-
/**
* Send all parameters at once.
*
@@ -352,7 +371,6 @@ private:
void mavlink_update_system();
- void mavlink_wpm_message_handler(const mavlink_message_t *msg);
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index 44bf77bb0..fa1a6887e 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -62,11 +62,7 @@
void *uorb_receive_thread(void *arg);
-struct listener {
- void (*callback)(const struct listener *l);
- int *subp;
- uintptr_t arg;
-};
+
uint16_t cm_uint16_from_m_float(float m);
@@ -86,86 +82,97 @@ cm_uint16_from_m_float(float m)
MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) :
- _task_should_exit(false),
+ thread_should_exit(false),
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")),
_mavlink(parent),
+ _listeners(nullptr),
_n_listeners(0)
{
- static const struct listener listeners[] = {
- {l_sensor_combined, &mavlink_subs.sensor_sub, 0},
- {l_vehicle_attitude, &mavlink_subs.att_sub, 0},
- {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0},
- {l_vehicle_status, &status_sub, 0},
- {l_rc_channels, &rc_sub, 0},
- {l_input_rc, &mavlink_subs.input_rc_sub, 0},
- {l_global_position, &mavlink_subs.global_pos_sub, 0},
- {l_local_position, &mavlink_subs.local_pos_sub, 0},
- {l_global_position_setpoint, &mavlink_subs.triplet_sub, 0},
- {l_local_position_setpoint, &mavlink_subs.spl_sub, 0},
- {l_attitude_setpoint, &mavlink_subs.spa_sub, 0},
- {l_actuator_outputs, &mavlink_subs.act_0_sub, 0},
- {l_actuator_outputs, &mavlink_subs.act_1_sub, 1},
- {l_actuator_outputs, &mavlink_subs.act_2_sub, 2},
- {l_actuator_outputs, &mavlink_subs.act_3_sub, 3},
- {l_actuator_armed, &mavlink_subs.armed_sub, 0},
- {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
- {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
- {l_debug_key_value, &mavlink_subs.debug_key_value, 0},
- {l_optical_flow, &mavlink_subs.optical_flow, 0},
- {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
- {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},
-};
-
- _n_listeners = sizeof(listeners) / sizeof(listeners[0]);
+ add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0);
+ add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0);
+ add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0);
+ add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0);
+ add_listener(MavlinkOrbListener::l_rc_channels, &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);
+ add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0);
+ add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0);
+ add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0);
+ add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0);
+ add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0);
+ add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1);
+ add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2);
+ add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3);
+ add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0);
+ add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0);
+ add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0);
+ add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0);
+ add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0);
+ add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0);
+ add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0);
+ add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0);
+ add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0);
+ add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0);
}
+void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg)
+{
+ struct listener *nl = new listener;
+
+ nl->callback = callback;
+ nl->subp = subp;
+ nl->arg = arg;
+ nl->next = nullptr;
+
+ // Register it
+ struct listener *next = _listeners;
+ while (next->next != nullptr) {
+ next = next->next;
+ }
+
+ // Attach
+ next->next = nl;
+ _n_listeners++;
+}
+
void
MavlinkOrbListener::l_sensor_combined(const struct listener *l)
{
struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */
- orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw);
-
- last_sensor_timestamp = raw.timestamp;
+ orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw);
- /* mark individual fields as changed */
+ /* mark individual fields as _mavlink->get_chan()ged */
uint16_t fields_updated = 0;
- static unsigned accel_counter = 0;
- static unsigned gyro_counter = 0;
- static unsigned mag_counter = 0;
- static unsigned baro_counter = 0;
-
- if (accel_counter != raw.accelerometer_counter) {
- /* mark first three dimensions as changed */
- fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
- accel_counter = raw.accelerometer_counter;
- }
-
- if (gyro_counter != raw.gyro_counter) {
- /* mark second group dimensions as changed */
- fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
- gyro_counter = raw.gyro_counter;
- }
-
- if (mag_counter != raw.magnetometer_counter) {
- /* mark third group dimensions as changed */
- fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
- mag_counter = raw.magnetometer_counter;
- }
-
- if (baro_counter != raw.baro_counter) {
- /* mark last group dimensions as changed */
- fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
- baro_counter = raw.baro_counter;
- }
- if (gcs_link)
- mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp,
+ // if (accel_counter != raw.accelerometer_counter) {
+ // /* mark first three dimensions as _mavlink->get_chan()ged */
+ // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
+ // accel_counter = raw.accelerometer_counter;
+ // }
+
+ // if (gyro_counter != raw.gyro_counter) {
+ // /* mark second group dimensions as _mavlink->get_chan()ged */
+ // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
+ // gyro_counter = raw.gyro_counter;
+ // }
+
+ // if (mag_counter != raw.magnetometer_counter) {
+ // /* mark third group dimensions as _mavlink->get_chan()ged */
+ // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
+ // mag_counter = raw.magnetometer_counter;
+ // }
+
+ // if (baro_counter != raw.baro_counter) {
+ // /* mark last group dimensions as _mavlink->get_chan()ged */
+ // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
+ // baro_counter = raw.baro_counter;
+ // }
+
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+ mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp,
raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
raw.gyro_rad_s[1], raw.gyro_rad_s[2],
@@ -175,51 +182,51 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l)
raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated);
- sensors_raw_counter++;
+ l->listener->sensors_raw_counter++;
}
void
MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
{
/* copy attitude data into local buffer */
- orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
+ orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att);
- if (gcs_link) {
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
/* send sensor values */
- mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(),
- last_sensor_timestamp / 1000,
- att.roll,
- att.pitch,
- att.yaw,
- att.rollspeed,
- att.pitchspeed,
- att.yawspeed);
+ mavlink_msg_attitude_send(l->mavlink->get_chan(),
+ l->listener->last_sensor_timestamp / 1000,
+ l->listener->att.roll,
+ l->listener->att.pitch,
+ l->listener->att.yaw,
+ l->listener->att.rollspeed,
+ l->listener->att.pitchspeed,
+ l->listener->att.yawspeed);
/* limit VFR message rate to 10Hz */
hrt_abstime t = hrt_absolute_time();
- if (t >= last_sent_vfr + 100000) {
- last_sent_vfr = t;
- float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
- uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
- float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
- mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
+ if (t >= l->listener->last_sent_vfr + 100000) {
+ l->listener->last_sent_vfr = t;
+ float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e);
+ uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F;
+ float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f;
+ mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d);
}
/* send quaternion values if it exists */
- if(att.q_valid) {
- mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(),
- last_sensor_timestamp / 1000,
- att.q[0],
- att.q[1],
- att.q[2],
- att.q[3],
- att.rollspeed,
- att.pitchspeed,
- att.yawspeed);
+ if(l->listener->att.q_valid) {
+ mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(),
+ l->listener->last_sensor_timestamp / 1000,
+ l->listener->att.q[0],
+ l->listener->att.q[1],
+ l->listener->att.q[2],
+ l->listener->att.q[3],
+ l->listener->att.rollspeed,
+ l->listener->att.pitchspeed,
+ l->listener->att.yawspeed);
}
}
- attitude_counter++;
+ l->listener->attitude_counter++;
}
void
@@ -228,13 +235,13 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
struct vehicle_gps_position_s gps;
/* copy gps data into local buffer */
- orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
+ orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps);
/* GPS COG is 0..2PI in degrees * 1e2 */
float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
/* GPS position */
- mavlink_msg_gps_raw_int_send(_mavlink->get_mavlink_chan(),
+ mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(),
gps.timestamp_position,
gps.fix_type,
gps.lat,
@@ -247,8 +254,8 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
gps.satellites_visible);
/* update SAT info every 10 seconds */
- if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
- mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(),
+ if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) {
+ mavlink_msg_gps_status_send(l->mavlink->get_chan(),
gps.satellites_visible,
gps.satellite_prn,
gps.satellite_used,
@@ -257,30 +264,30 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l)
gps.satellite_snr);
}
- gps_counter++;
+ l->listener->gps_counter++;
}
void
MavlinkOrbListener::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);
+ /* immediately communicate state _mavlink->get_chan()ges back to user */
+ orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status);
+ orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
/* enable or disable HIL */
- if (v_status.hil_state == HIL_STATE_ON)
- set_hil_on_off(true);
- else if (v_status.hil_state == HIL_STATE_OFF)
- set_hil_on_off(false);
+ if (l->listener->v_status.hil_state == HIL_STATE_ON)
+ l->mavlink->set_hil_on_off(true);
+ else if (l->listener->v_status.hil_state == HIL_STATE_OFF)
+ l->mavlink->set_hil_on_off(false);
/* 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);
+ l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
- mavlink_msg_heartbeat_send(chan,
+ mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
@@ -289,37 +296,37 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l)
}
void
-MavlinkOrbListener::l_rc_channels(const struct listener *l)
+MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l)
{
- /* copy rc channels into local buffer */
- orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
- // XXX Add RC channels scaled message here
+ /* copy rc _mavlink->get_chan()nels into local buffer */
+ orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc);
+ // XXX Add RC _mavlink->get_chan()nels scaled message here
}
void
MavlinkOrbListener::l_input_rc(const struct listener *l)
{
- /* copy rc channels into local buffer */
- orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
+ /* copy rc _mavlink->get_chan()nels into local buffer */
+ orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw);
- if (gcs_link) {
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
const unsigned port_width = 8;
- for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
+ for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
- mavlink_msg_rc_channels_raw_send(chan,
- rc_raw.timestamp / 1000,
+ mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(),
+ l->listener->rc_raw.timestamp / 1000,
i,
- (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
- (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
- (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
- (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
- (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
- (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
- (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
- (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
- rc_raw.rssi);
+ (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,
+ (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
+ (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
+ (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
+ (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
+ (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
+ (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
+ l->listener->rc_raw.rssi);
}
}
}
@@ -328,48 +335,48 @@ void
MavlinkOrbListener::l_global_position(const struct listener *l)
{
/* copy global position data into local buffer */
- orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
-
- mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(),
- global_pos.timestamp / 1000,
- global_pos.lat * 1e7,
- global_pos.lon * 1e7,
- global_pos.alt * 1000.0f,
- (global_pos.alt - home.alt) * 1000.0f,
- global_pos.vel_n * 100.0f,
- global_pos.vel_e * 100.0f,
- global_pos.vel_d * 100.0f,
- _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
+ orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos);
+
+ mavlink_msg_global_position_int_send(l->mavlink->get_chan(),
+ l->listener->global_pos.timestamp / 1000,
+ l->listener->global_pos.lat * 1e7,
+ l->listener->global_pos.lon * 1e7,
+ l->listener->global_pos.alt * 1000.0f,
+ (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f,
+ l->listener->global_pos.vel_n * 100.0f,
+ l->listener->global_pos.vel_e * 100.0f,
+ l->listener->global_pos.vel_d * 100.0f,
+ _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
void
MavlinkOrbListener::l_local_position(const struct listener *l)
{
/* copy local position data into local buffer */
- orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
-
- if (gcs_link)
- mavlink_msg_local_position_ned_send(_mavlink->get_mavlink_chan(),
- local_pos.timestamp / 1000,
- local_pos.x,
- local_pos.y,
- local_pos.z,
- local_pos.vx,
- local_pos.vy,
- local_pos.vz);
+ orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos);
+
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+ mavlink_msg_local_position_ned_send(l->mavlink->get_chan(),
+ l->listener->local_pos.timestamp / 1000,
+ l->listener->local_pos.x,
+ l->listener->local_pos.y,
+ l->listener->local_pos.z,
+ l->listener->local_pos.vx,
+ l->listener->local_pos.vy,
+ l->listener->local_pos.vz);
}
void
MavlinkOrbListener::l_global_position_setpoint(const struct listener *l)
{
struct position_setpoint_triplet_s triplet;
- orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
+ orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet);
if (!triplet.current.valid)
return;
- if (gcs_link)
- mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(),
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+ mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(),
MAV_FRAME_GLOBAL,
(int32_t)(triplet.current.lat * 1e7d),
(int32_t)(triplet.current.lon * 1e7d),
@@ -383,10 +390,10 @@ MavlinkOrbListener::l_local_position_setpoint(const struct listener *l)
struct vehicle_local_position_setpoint_s local_sp;
/* copy local position data into local buffer */
- orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
+ orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp);
- if (gcs_link)
- mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(),
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+ mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(),
MAV_FRAME_LOCAL_NED,
local_sp.x,
local_sp.y,
@@ -400,10 +407,10 @@ MavlinkOrbListener::l_attitude_setpoint(const struct listener *l)
struct vehicle_attitude_setpoint_s att_sp;
/* copy local position data into local buffer */
- orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
+ orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp);
- if (gcs_link)
- mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(),
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+ mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(),
att_sp.timestamp / 1000,
att_sp.roll_body,
att_sp.pitch_body,
@@ -417,10 +424,10 @@ MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l)
struct vehicle_rates_setpoint_s rates_sp;
/* copy local position data into local buffer */
- orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
+ orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp);
- if (gcs_link)
- mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(),
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+ mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(),
rates_sp.timestamp / 1000,
rates_sp.roll,
rates_sp.pitch,
@@ -443,8 +450,8 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
/* copy actuator data into local buffer */
orb_copy(ids[l->arg], *l->subp, &act_outputs);
- if (gcs_link) {
- mavlink_msg_servo_output_raw_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000,
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
+ mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000,
l->arg /* port number - needs GCS support */,
/* QGC has port number support already */
act_outputs.output[0],
@@ -457,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
act_outputs.output[7]);
/* only send in HIL mode and only send first group for HIL */
- if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
+ if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -470,7 +477,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
/* scale / assign outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
- mavlink_msg_hil_controls_send(chan,
+ mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
@@ -484,7 +491,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
- mavlink_msg_hil_controls_send(chan,
+ mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
@@ -498,7 +505,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
- mavlink_msg_hil_controls_send(chan,
+ mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
@@ -512,7 +519,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
0);
} else {
- mavlink_msg_hil_controls_send(chan,
+ mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 500.0f,
(act_outputs.output[1] - 1500.0f) / 500.0f,
@@ -532,7 +539,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
void
MavlinkOrbListener::l_actuator_armed(const struct listener *l)
{
- orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+ orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
}
void
@@ -541,10 +548,10 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l)
struct manual_control_setpoint_s man_control;
/* copy manual control data into local buffer */
- orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
+ orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control);
- if (gcs_link)
- mavlink_msg_manual_control_send(_mavlink->get_mavlink_chan(),
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD)
+ mavlink_msg_manual_control_send(l->mavlink->get_chan(),
mavlink_system.sysid,
man_control.roll * 1000,
man_control.pitch * 1000,
@@ -556,26 +563,26 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l)
void
MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l)
{
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0);
- if (gcs_link) {
+ if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
/* send, add spaces so that string buffer is at least 10 chars long */
- mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
- last_sensor_timestamp / 1000,
+ mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+ l->listener->last_sensor_timestamp / 1000,
"ctrl0 ",
- actuators_0.control[0]);
- mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
- last_sensor_timestamp / 1000,
+ l->listener->actuators_0.control[0]);
+ mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+ l->listener->last_sensor_timestamp / 1000,
"ctrl1 ",
- actuators_0.control[1]);
- mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
- last_sensor_timestamp / 1000,
+ l->listener->actuators_0.control[1]);
+ mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+ l->listener->last_sensor_timestamp / 1000,
"ctrl2 ",
- actuators_0.control[2]);
- mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
- last_sensor_timestamp / 1000,
+ l->listener->actuators_0.control[2]);
+ mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+ l->listener->last_sensor_timestamp / 1000,
"ctrl3 ",
- actuators_0.control[3]);
+ l->listener->actuators_0.control[3]);
}
}
@@ -584,13 +591,13 @@ MavlinkOrbListener::l_debug_key_value(const struct listener *l)
{
struct debug_key_value_s debug;
- orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug);
+ orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug);
/* Enforce null termination */
debug.key[sizeof(debug.key) - 1] = '\0';
- mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
- last_sensor_timestamp / 1000,
+ mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
+ l->listener->last_sensor_timestamp / 1000,
debug.key,
debug.value);
}
@@ -600,52 +607,52 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l)
{
struct optical_flow_s flow;
- orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
+ orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow);
- mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
+ mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
}
void
MavlinkOrbListener::l_home(const struct listener *l)
{
- orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
+ orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home);
- mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
+ mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
}
void
MavlinkOrbListener::l_airspeed(const struct listener *l)
{
- orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
+ orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed);
}
void
MavlinkOrbListener::l_nav_cap(const struct listener *l)
{
- orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
+ orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap);
- mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(),
+ mavlink_msg_named_value_float_send(l->mavlink->get_chan(),
hrt_absolute_time() / 1000,
"turn dist",
- nav_cap.turn_distance);
+ l->listener->nav_cap.turn_distance);
}
void
MavlinkOrbListener::l_control_mode(const struct listener *l)
{
- orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
+ 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;
- get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+ l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
- mavlink_msg_heartbeat_send(chan,
+ mavlink_msg_heartbeat_send(l->mavlink->get_chan(),
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
@@ -657,7 +664,9 @@ void *
MavlinkOrbListener::uorb_receive_thread(void *arg)
{
/* Set thread name */
- prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
+ char buf[32];
+ sprintf(buf, "mavlink rcv%d", Mavlink::instance_count());
+ prctl(PR_SET_NAME, buf, getpid());
/*
* set up poll to block for new data,
@@ -670,15 +679,21 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
*
* Might want to invoke each listener once to set initial state.
*/
- struct pollfd fds[_n_listeners];
+ struct pollfd fds[_max_listeners];
+
+ struct listener* next = _listeners;
+ unsigned i = 0;
- for (unsigned i = 0; i < _n_listeners; i++) {
- fds[i].fd = *listeners[i].subp;
+ while (next != nullptr) {
+
+ fds[i].fd = *next->subp;
fds[i].events = POLLIN;
+ next = next->next;
+ i++;
+ }
/* Invoke callback to set initial state */
//listeners[i].callback(&listener[i]);
- }
while (!thread_should_exit) {
@@ -689,13 +704,19 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
/* silent */
} else if (poll_ret < 0) {
- mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
+ //mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
} else {
- for (unsigned i = 0; i < _n_listeners; i++) {
+ unsigned i = 0;
+ struct listener* cb = _listeners;
+ while (cb != nullptr) {
+
if (fds[i].revents & POLLIN)
- listeners[i].callback(&listeners[i]);
+ cb->callback(cb);
+
+ cb = cb->next;
+ i++;
}
}
}
@@ -703,107 +724,113 @@ MavlinkOrbListener::uorb_receive_thread(void *arg)
return NULL;
}
+void * MavlinkOrbListener::uorb_start_helper(void *context)
+{
+ return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL);
+}
+
pthread_t
-MavlinkOrbListener::uorb_receive_start(void)
+MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink)
{
+ MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink);
+
/* --- SENSORS RAW VALUE --- */
- mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+ mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
/* rate limit set externally based on interface speed, set a basic default here */
- orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */
+ orb_set_interval(mavlink->get_subs()->sensor_sub, 200); /* 5Hz updates */
/* --- ATTITUDE VALUE --- */
- mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ mavlink->get_subs()->att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
/* rate limit set externally based on interface speed, set a basic default here */
- orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */
+ orb_set_interval(mavlink->get_subs()->att_sub, 200); /* 5Hz updates */
/* --- GPS VALUE --- */
- mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
- orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */
+ mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+ orb_set_interval(mavlink->get_subs()->gps_sub, 200); /* 5Hz updates */
/* --- HOME POSITION --- */
- mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
- orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */
+ mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position));
+ orb_set_interval(mavlink->get_subs()->home_sub, 1000); /* 1Hz updates */
/* --- SYSTEM STATE --- */
- status_sub = orb_subscribe(ORB_ID(vehicle_status));
- orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
+ 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_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 */
+ 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 */
/* --- RC CHANNELS VALUE --- */
- rc_sub = orb_subscribe(ORB_ID(rc_channels));
- orb_set_interval(rc_sub, 100); /* 10Hz updates */
+ mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels));
+ orb_set_interval(mavlink->get_subs()->rc_sub, 100); /* 10Hz updates */
/* --- RC RAW VALUE --- */
- mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
- orb_set_interval(mavlink_subs.input_rc_sub, 100);
+ mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc));
+ orb_set_interval(mavlink->get_subs()->input_rc_sub, 100);
/* --- GLOBAL POS VALUE --- */
- mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
- orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */
+ mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ orb_set_interval(mavlink->get_subs()->global_pos_sub, 100); /* 10 Hz active updates */
/* --- LOCAL POS VALUE --- */
- mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
- orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */
+ mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+ orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000); /* 1Hz active updates */
/* --- GLOBAL SETPOINT VALUE --- */
- mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
- orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */
+ mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+ orb_set_interval(mavlink->get_subs()->triplet_sub, 2000); /* 0.5 Hz updates */
/* --- LOCAL SETPOINT VALUE --- */
- mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
- orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */
+ mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+ orb_set_interval(mavlink->get_subs()->spl_sub, 2000); /* 0.5 Hz updates */
/* --- ATTITUDE SETPOINT VALUE --- */
- mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
- orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */
+ mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+ orb_set_interval(mavlink->get_subs()->spa_sub, 2000); /* 0.5 Hz updates */
/* --- RATES SETPOINT VALUE --- */
- mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
- orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */
+ mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+ orb_set_interval(mavlink->get_subs()->rates_setpoint_sub, 2000); /* 0.5 Hz updates */
/* --- ACTUATOR OUTPUTS --- */
- mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
- mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
- mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
- mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
+ mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
+ mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
+ mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
+ mavlink->get_subs()->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
/* rate limits set externally based on interface speed, set a basic default here */
- orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */
- orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */
- orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */
- orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */
+ orb_set_interval(mavlink->get_subs()->act_0_sub, 100); /* 10Hz updates */
+ orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */
+ orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */
+ orb_set_interval(mavlink->get_subs()->act_3_sub, 100); /* 10Hz updates */
/* --- ACTUATOR ARMED VALUE --- */
- mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
- orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */
+ mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+ orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */
/* --- MAPPED MANUAL CONTROL INPUTS --- */
- mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+ mavlink->get_subs()->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* rate limits set externally based on interface speed, set a basic default here */
- orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
+ orb_set_interval(mavlink->get_subs()->man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */
- mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
- orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
+ mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+ orb_set_interval(mavlink->get_subs()->actuators_sub, 100); /* 10Hz updates */
/* --- DEBUG VALUE OUTPUT --- */
- mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
- orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */
+ mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
+ orb_set_interval(mavlink->get_subs()->debug_key_value, 100); /* 10Hz updates */
/* --- FLOW SENSOR --- */
- mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
- orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */
+ mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow));
+ orb_set_interval(mavlink->get_subs()->optical_flow, 200); /* 5Hz updates */
/* --- AIRSPEED --- */
- mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
- orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */
+ mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+ orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); /* 5Hz updates */
/* --- NAVIGATION CAPABILITIES --- */
- mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
- orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */
- nav_cap.turn_distance = 0.0f;
+ mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+ orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */
/* start the listener loop */
pthread_attr_t uorb_attr;
@@ -813,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(void)
pthread_attr_setstacksize(&uorb_attr, 2048);
pthread_t thread;
- pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
+ pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv);
pthread_attr_destroy(&uorb_attr);
return thread;
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
index 29e081b36..3988103bc 100644
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ b/src/modules/mavlink/mavlink_orb_listener.h
@@ -38,8 +38,40 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
+#include <systemlib/perf_counter.h>
+
#pragma once
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#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>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <drivers/drv_rc_input.h>
+#include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/mission.h>
+
class Mavlink;
class MavlinkOrbListener
@@ -67,18 +99,32 @@ public:
*/
void status();
- pthread_t uorb_receive_start(void);
+ static pthread_t uorb_receive_start(Mavlink *mavlink);
void * uorb_receive_thread(void *arg);
+ struct listener {
+ void (*callback)(const struct listener *l);
+ int *subp;
+ uintptr_t arg;
+ struct listener* next;
+ Mavlink *mavlink;
+ MavlinkOrbListener* listener;
+ };
+
+ void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg);
+ static void * uorb_start_helper(void *context);
+
private:
- bool _task_should_exit; /**< if true, sensor task should exit */
+ bool thread_should_exit; /**< if true, sensor task should exit */
perf_counter_t _loop_perf; /**< loop performance counter */
Mavlink* _mavlink;
+ struct listener *_listeners;
unsigned _n_listeners;
+ static const unsigned _max_listeners = 32;
/**
* Shim for calling task_main from task_create.
@@ -90,28 +136,28 @@ private:
*/
void task_main() __attribute__((noreturn));
- void l_sensor_combined(const struct listener *l);
- void l_vehicle_attitude(const struct listener *l);
- void l_vehicle_gps_position(const struct listener *l);
- void l_vehicle_status(const struct listener *l);
- void l_rc_channels(const struct listener *l);
- void l_input_rc(const struct listener *l);
- void l_global_position(const struct listener *l);
- void l_local_position(const struct listener *l);
- void l_global_position_setpoint(const struct listener *l);
- void l_local_position_setpoint(const struct listener *l);
- void l_attitude_setpoint(const struct listener *l);
- void l_actuator_outputs(const struct listener *l);
- void l_actuator_armed(const struct listener *l);
- void l_manual_control_setpoint(const struct listener *l);
- void l_vehicle_attitude_controls(const struct listener *l);
- void l_debug_key_value(const struct listener *l);
- void l_optical_flow(const struct listener *l);
- void l_vehicle_rates_setpoint(const struct listener *l);
- void l_home(const struct listener *l);
- void l_airspeed(const struct listener *l);
- void l_nav_cap(const struct listener *l);
- void l_control_mode(const struct listener *l);
+ static void l_sensor_combined(const struct listener *l);
+ static void l_vehicle_attitude(const struct listener *l);
+ static void l_vehicle_gps_position(const struct listener *l);
+ static void l_vehicle_status(const struct listener *l);
+ static void l_rc_channels(const struct listener *l);
+ static void l_input_rc(const struct listener *l);
+ static void l_global_position(const struct listener *l);
+ static void l_local_position(const struct listener *l);
+ static void l_global_position_setpoint(const struct listener *l);
+ static void l_local_position_setpoint(const struct listener *l);
+ static void l_attitude_setpoint(const struct listener *l);
+ static void l_actuator_outputs(const struct listener *l);
+ static void l_actuator_armed(const struct listener *l);
+ static void l_manual_control_setpoint(const struct listener *l);
+ static void l_vehicle_attitude_controls(const struct listener *l);
+ static void l_debug_key_value(const struct listener *l);
+ static void l_optical_flow(const struct listener *l);
+ 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);
struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 4f763c3c6..982d6c1d8 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -83,7 +83,8 @@ __BEGIN_DECLS
__END_DECLS
-void MavlinkReceiver::MavlinkReceiver() :
+MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
+ _mavlink(parent),
pub_hil_global_pos(-1),
pub_hil_local_pos(-1),
pub_hil_attitude(-1),
@@ -790,7 +791,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
void *
MavlinkReceiver::receive_thread(void *arg)
{
- int uart_fd = *((int *)arg);
+ int uart_fd = _mavlink->get_uart_fd();
const int timeout = 1000;
uint8_t buf[32];
@@ -822,10 +823,10 @@ MavlinkReceiver::receive_thread(void *arg)
handle_message(&msg);
/* handle packet with waypoint component */
- mavlink_wpm_message_handler(&msg);
+ _mavlink->mavlink_wpm_message_handler(&msg);
/* handle packet with parameter component */
- mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
+ _mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
}
}
}
@@ -839,15 +840,22 @@ void MavlinkReceiver::print_status()
}
+void * MavlinkReceiver::start_helper(void *context)
+{
+ return ((MavlinkReceiver *)context)->receive_thread(NULL);
+}
+
pthread_t
-MavlinkReceiver::receive_start(int uart)
+MavlinkReceiver::receive_start(Mavlink *mavlink)
{
+ MavlinkReceiver *rcv = new MavlinkReceiver(mavlink);
+
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
// set to non-blocking read
- int flags = fcntl(uart, F_GETFL, 0);
- fcntl(uart, F_SETFL, flags | O_NONBLOCK);
+ int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0);
+ fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK);
struct sched_param param;
(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
@@ -857,7 +865,7 @@ MavlinkReceiver::receive_start(int uart)
pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_t thread;
- pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
+ pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv);
pthread_attr_destroy(&receiveloop_attr);
return thread;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index ea57714d2..483d91e72 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -96,7 +96,9 @@ public:
*/
void print_status();
- pthread_t receive_start(int uart);
+ static pthread_t receive_start(Mavlink* mavlink);
+
+ static void * start_helper(void *context);
private:
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 16:37:06 +0000