From 141982a3ac21e7a0437f1d7692e4024daf873c21 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 27 Feb 2014 13:54:55 +0400
Subject: [PATCH] mavlink: minor refactoring and cleanup, rate limiter class
implemented, new messages added
---
src/modules/mavlink/mavlink_main.cpp | 68 +--
src/modules/mavlink/mavlink_main.h | 9 +-
src/modules/mavlink/mavlink_messages.cpp | 438 ++++++++++--------
.../mavlink/mavlink_orb_subscription.cpp | 2 +-
.../mavlink/mavlink_orb_subscription.h | 4 +-
src/modules/mavlink/mavlink_rate_limiter.cpp | 38 ++
src/modules/mavlink/mavlink_rate_limiter.h | 28 ++
src/modules/mavlink/mavlink_stream.cpp | 2 +-
src/modules/mavlink/module.mk | 3 +-
9 files changed, 371 insertions(+), 221 deletions(-)
create mode 100644 src/modules/mavlink/mavlink_rate_limiter.cpp
create mode 100644 src/modules/mavlink/mavlink_rate_limiter.h
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3102c937da..8a026742c3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -79,6 +79,7 @@
#include "mavlink_main.h"
#include "mavlink_messages.h"
#include "mavlink_receiver.h"
+#include "mavlink_rate_limiter.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -321,7 +322,7 @@ Mavlink::get_uart_fd()
mavlink_channel_t
Mavlink::get_channel()
{
- return _chan;
+ return _channel;
}
/****************************************************************************
@@ -1321,7 +1322,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
- mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len);
+ mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
}
@@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
}
}
-MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size)
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size)
{
/* check if already subscribed to this topic */
MavlinkOrbSubscription *sub;
@@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[])
int ch;
_baudrate = 57600;
- _chan = MAVLINK_COMM_0;
+ _channel = MAVLINK_COMM_0;
_mode = MODE_OFFBOARD;
@@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[])
thread_running = true;
- unsigned lowspeed_counter = 0;
-
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s));
MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s));
@@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[])
warnx("started");
mavlink_log_info(_mavlink_fd, "[mavlink] started");
- /* add default streams, intervals depend on baud rate */
+ /* add default streams depending on mode, intervals depend on baud rate */
float rate_mult = _baudrate / 57600.0f;
if (rate_mult > 4.0f) {
rate_mult = 4.0f;
}
add_stream("HEARTBEAT", 1.0f);
- add_stream("SYS_STATUS", 1.0f * rate_mult);
- add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
- add_stream("HIGHRES_IMU", 1.0f * rate_mult);
- add_stream("ATTITUDE", 10.0f * rate_mult);
- add_stream("GPS_RAW_INT", 1.0f * rate_mult);
- add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
- add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+
+ switch(_mode) {
+ case MODE_OFFBOARD:
+ add_stream("SYS_STATUS", 1.0f * rate_mult);
+ add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
+ add_stream("HIGHRES_IMU", 1.0f * rate_mult);
+ add_stream("ATTITUDE", 10.0f * rate_mult);
+ add_stream("GPS_RAW_INT", 1.0f * rate_mult);
+ add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
+ add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+ add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult);
+ break;
+
+ case MODE_HIL:
+ add_stream("SYS_STATUS", 1.0f * rate_mult);
+ add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
+ add_stream("HIGHRES_IMU", 1.0f * rate_mult);
+ add_stream("ATTITUDE", 10.0f * rate_mult);
+ add_stream("GPS_RAW_INT", 1.0f * rate_mult);
+ add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
+ add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+ break;
+
+ default:
+ break;
+ }
+
+ MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult);
+ MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult);
while (!_task_should_exit) {
/* main loop */
@@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[])
stream->update(t);
}
- /* 0.5 Hz */
- if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) {
- mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
- }
-
- lowspeed_counter++;
-
bool updated;
orb_check(mission_result_sub, &updated);
@@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[])
}
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
+ } else {
+ if (slow_rate_limiter.check(t)) {
+ mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
+ }
}
- if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) {
- /* send parameters at 20 Hz (if queued for sending) */
+ if (fast_rate_limiter.check(t)) {
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(hrt_absolute_time());
- if (_baudrate > 57600) {
- mavlink_pm_queued_send();
- }
-
- /* send one string at 20 Hz */
if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index e7f3486da8..506b4317ad 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -201,7 +201,7 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
- MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size);
+ MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size);
mavlink_channel_t get_channel();
@@ -231,10 +231,7 @@ private:
MAVLINK_MODE _mode;
uint8_t _mavlink_wpm_comp_id;
- mavlink_channel_t _chan;
-
-// XXX probably should be in a header...
-// extern pthread_t receive_start(int uart);
+ mavlink_channel_t _channel;
struct mavlink_logbuffer lb;
unsigned int total_counter;
@@ -248,7 +245,7 @@ private:
bool _verbose;
int _uart;
int _baudrate;
- bool gcs_link;
+
/**
* If the queue index is not at 0, the queue sending
* logic will send parameters from the current index
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 48443fbdc0..9676068419 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -5,6 +5,7 @@
* Author: ton
*/
+#include <stdio.h>
#include <commander/px4_custom_mode.h>
#include <lib/geo/geo.h>
@@ -19,6 +20,10 @@
#include "mavlink_messages.h"
+static uint16_t cm_uint16_from_m_float(float m);
+static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
+ uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
uint16_t
cm_uint16_from_m_float(float m)
{
@@ -32,6 +37,83 @@ cm_uint16_from_m_float(float m)
return (uint16_t)(m * 100.0f);
}
+void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
+ uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
+{
+ *mavlink_state = 0;
+ *mavlink_base_mode = 0;
+ *mavlink_custom_mode = 0;
+
+ /* HIL */
+ if (status->hil_state == HIL_STATE_ON) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+ }
+
+ /* arming state */
+ if (status->arming_state == ARMING_STATE_ARMED
+ || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+ }
+
+ /* main state */
+ *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+ union px4_custom_mode custom_mode;
+ custom_mode.data = 0;
+ if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
+ /* use main state when navigator is not active */
+ if (status->main_state == MAIN_STATE_MANUAL) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+ } else if (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 (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 (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 (pos_sp_triplet->nav_state == NAV_STATE_READY) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+ } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+ } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+ } 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;
+
+ /* set system state */
+ if (status->arming_state == ARMING_STATE_INIT
+ || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
+ || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
+ *mavlink_state = MAV_STATE_UNINIT;
+ } else if (status->arming_state == ARMING_STATE_ARMED) {
+ *mavlink_state = MAV_STATE_ACTIVE;
+ } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ *mavlink_state = MAV_STATE_CRITICAL;
+ } else if (status->arming_state == ARMING_STATE_STANDBY) {
+ *mavlink_state = MAV_STATE_STANDBY;
+ } else if (status->arming_state == ARMING_STATE_REBOOT) {
+ *mavlink_state = MAV_STATE_POWEROFF;
+ } else {
+ *mavlink_state = MAV_STATE_CRITICAL;
+ }
+}
+
+
class MavlinkStreamHeartbeat : public MavlinkStream {
public:
const char *get_name()
@@ -68,78 +150,13 @@ protected:
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
-
- /* HIL */
- if (status->hil_state == HIL_STATE_ON) {
- mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
- }
-
- /* arming state */
- if (status->arming_state == ARMING_STATE_ARMED
- || status->arming_state == ARMING_STATE_ARMED_ERROR) {
- mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
- }
-
- /* main state */
- mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-
- union px4_custom_mode custom_mode;
- custom_mode.data = 0;
- if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
- /* use main state when navigator is not active */
- if (status->main_state == MAIN_STATE_MANUAL) {
- mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
- } else if (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 (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 (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 (pos_sp_triplet->nav_state == NAV_STATE_READY) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
- } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
- } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
- } 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;
- }
- }
-
- /* set system state */
- if (status->arming_state == ARMING_STATE_INIT
- || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
- || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
- mavlink_state = MAV_STATE_UNINIT;
- } else if (status->arming_state == ARMING_STATE_ARMED) {
- mavlink_state = MAV_STATE_ACTIVE;
- } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
- mavlink_state = MAV_STATE_CRITICAL;
- } else if (status->arming_state == ARMING_STATE_STANDBY) {
- mavlink_state = MAV_STATE_STANDBY;
- } else if (status->arming_state == ARMING_STATE_REBOOT) {
- mavlink_state = MAV_STATE_POWEROFF;
- } else {
- mavlink_state = MAV_STATE_CRITICAL;
- }
+ get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
- custom_mode.data,
+ mavlink_custom_mode,
mavlink_state);
}
@@ -164,7 +181,6 @@ private:
struct vehicle_status_s *status;
protected:
-
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
@@ -215,7 +231,6 @@ private:
uint32_t baro_counter = 0;
protected:
-
void subscribe(Mavlink *mavlink)
{
sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s));
@@ -281,7 +296,6 @@ private:
struct vehicle_attitude_s *att;
protected:
-
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s));
@@ -313,11 +327,9 @@ public:
private:
MavlinkOrbSubscription *gps_sub;
-
struct vehicle_gps_position_s *gps;
protected:
-
void subscribe(Mavlink *mavlink)
{
gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s));
@@ -356,13 +368,12 @@ public:
private:
MavlinkOrbSubscription *pos_sub;
- MavlinkOrbSubscription *home_sub;
-
struct vehicle_global_position_s *pos;
+
+ MavlinkOrbSubscription *home_sub;
struct home_position_s *home;
protected:
-
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s));
@@ -404,11 +415,9 @@ public:
private:
MavlinkOrbSubscription *pos_sub;
-
struct vehicle_local_position_s *pos;
protected:
-
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s));
@@ -444,11 +453,9 @@ public:
private:
MavlinkOrbSubscription *home_sub;
-
struct home_position_s *home;
protected:
-
void subscribe(Mavlink *mavlink)
{
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s));
@@ -466,6 +473,171 @@ protected:
};
+class MavlinkStreamServoOutputRaw : public MavlinkStream {
+public:
+ MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
+ {
+ sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
+ }
+
+ const char *get_name()
+ {
+ return _name;
+ }
+
+ MavlinkStream *new_instance()
+ {
+ return new MavlinkStreamServoOutputRaw(_n);
+ }
+
+private:
+ MavlinkOrbSubscription *act_sub;
+ struct actuator_outputs_s *act;
+
+ char _name[20];
+ unsigned int _n;
+
+protected:
+ void subscribe(Mavlink *mavlink)
+ {
+ orb_id_t act_topics[] = {
+ ORB_ID(actuator_outputs_0),
+ ORB_ID(actuator_outputs_1),
+ ORB_ID(actuator_outputs_2),
+ ORB_ID(actuator_outputs_3)
+ };
+
+ act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s));
+ act = (struct actuator_outputs_s *)act_sub->get_data();
+ }
+
+ void send(const hrt_abstime t) {
+ act_sub->update(t);
+
+ mavlink_msg_servo_output_raw_send(_channel,
+ act->timestamp / 1000,
+ _n,
+ act->output[0],
+ act->output[1],
+ act->output[2],
+ act->output[3],
+ act->output[4],
+ act->output[5],
+ act->output[6],
+ act->output[7]);
+ }
+};
+
+
+class MavlinkStreamHILControls : public MavlinkStream {
+public:
+ const char *get_name()
+ {
+ return "HIL_CONTROLS";
+ }
+
+ MavlinkStream *new_instance()
+ {
+ return new MavlinkStreamHILControls();
+ }
+
+private:
+ MavlinkOrbSubscription *status_sub;
+ struct vehicle_status_s *status;
+
+ MavlinkOrbSubscription *pos_sp_triplet_sub;
+ struct position_setpoint_triplet_s *pos_sp_triplet;
+
+ MavlinkOrbSubscription *act_sub;
+ struct actuator_outputs_s *act;
+
+protected:
+ void subscribe(Mavlink *mavlink)
+ {
+ status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
+ status = (struct vehicle_status_s *)status_sub->get_data();
+
+ pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s));
+ pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
+
+ act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s));
+ act = (struct actuator_outputs_s *)act_sub->get_data();
+ }
+
+ void send(const hrt_abstime t) {
+ status_sub->update(t);
+ pos_sp_triplet_sub->update(t);
+ act_sub->update(t);
+
+ /* translate the current syste state to mavlink state and mode */
+ uint8_t mavlink_state;
+ uint8_t mavlink_base_mode;
+ uint32_t mavlink_custom_mode;
+ get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+ /* HIL message as per MAVLink spec */
+
+ /* scale / assign outputs depending on system type */
+ if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
+ mavlink_msg_hil_controls_send(_channel,
+ hrt_absolute_time(),
+ ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
+ -1,
+ -1,
+ -1,
+ -1,
+ mavlink_base_mode,
+ 0);
+
+ } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
+ mavlink_msg_hil_controls_send(_channel,
+ hrt_absolute_time(),
+ ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[4] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[5] - 900.0f) / 600.0f) / 2.0f,
+ -1,
+ -1,
+ mavlink_base_mode,
+ 0);
+
+ } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+ mavlink_msg_hil_controls_send(_channel,
+ hrt_absolute_time(),
+ ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[4] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[5] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[6] - 900.0f) / 600.0f) / 2.0f,
+ ((act->output[7] - 900.0f) / 600.0f) / 2.0f,
+ mavlink_base_mode,
+ 0);
+
+ } else {
+ mavlink_msg_hil_controls_send(_channel,
+ hrt_absolute_time(),
+ (act->output[0] - 1500.0f) / 500.0f,
+ (act->output[1] - 1500.0f) / 500.0f,
+ (act->output[2] - 1500.0f) / 500.0f,
+ (act->output[3] - 1000.0f) / 1000.0f,
+ (act->output[4] - 1500.0f) / 500.0f,
+ (act->output[5] - 1500.0f) / 500.0f,
+ (act->output[6] - 1500.0f) / 500.0f,
+ (act->output[7] - 1500.0f) / 500.0f,
+ mavlink_base_mode,
+ 0);
+ }
+ }
+};
+
+
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
@@ -475,6 +647,11 @@ MavlinkStream *streams_list[] = {
new MavlinkStreamGlobalPositionInt(),
new MavlinkStreamLocalPositionNED(),
new MavlinkStreamGPSGlobalOrigin(),
+ new MavlinkStreamServoOutputRaw(0),
+ new MavlinkStreamServoOutputRaw(1),
+ new MavlinkStreamServoOutputRaw(2),
+ new MavlinkStreamServoOutputRaw(3),
+ new MavlinkStreamHILControls(),
nullptr
};
@@ -484,8 +661,6 @@ MavlinkStream *streams_list[] = {
-
-
//void
//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
//{
@@ -652,107 +827,6 @@ MavlinkStream *streams_list[] = {
//}
//
//void
-//MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
-//{
-// struct actuator_outputs_s act_outputs;
-//
-// orb_id_t ids[] = {
-// ORB_ID(actuator_outputs_0),
-// ORB_ID(actuator_outputs_1),
-// ORB_ID(actuator_outputs_2),
-// ORB_ID(actuator_outputs_3)
-// };
-//
-// /* copy actuator data into local buffer */
-// orb_copy(ids[l->arg], *l->subp, &act_outputs);
-//
-// 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],
-// act_outputs.output[1],
-// act_outputs.output[2],
-// act_outputs.output[3],
-// act_outputs.output[4],
-// act_outputs.output[5],
-// act_outputs.output[6],
-// act_outputs.output[7]);
-//
-// /* only send in HIL mode and only send first group for HIL */
-// if (l->mavlink->get_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;
-// 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);
-//
-// /* HIL message as per MAVLink spec */
-//
-// /* scale / assign outputs depending on system type */
-//
-// if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-// 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,
-// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-// -1,
-// -1,
-// -1,
-// -1,
-// mavlink_base_mode,
-// 0);
-//
-// } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-// 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,
-// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-// -1,
-// -1,
-// mavlink_base_mode,
-// 0);
-//
-// } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-// 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,
-// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
-// ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-// mavlink_base_mode,
-// 0);
-//
-// } else {
-// 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,
-// (act_outputs.output[2] - 1500.0f) / 500.0f,
-// (act_outputs.output[3] - 1000.0f) / 1000.0f,
-// (act_outputs.output[4] - 1500.0f) / 500.0f,
-// (act_outputs.output[5] - 1500.0f) / 500.0f,
-// (act_outputs.output[6] - 1500.0f) / 500.0f,
-// (act_outputs.output[7] - 1500.0f) / 500.0f,
-// mavlink_base_mode,
-// 0);
-// }
-// }
-// }
-//}
-//
-//void
//MavlinkOrbListener::l_actuator_armed(const struct listener *l)
//{
// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index b504b6955b..35470a19a1 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -13,7 +13,7 @@
#include "mavlink_orb_subscription.h"
-MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
{
_data = malloc(size);
memset(_data, 0, size);
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index c38a9cc431..79ff3abdbe 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -16,7 +16,7 @@ class MavlinkOrbSubscription {
public:
MavlinkOrbSubscription *next;
- MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size);
+ MavlinkOrbSubscription(const orb_id_t topic, size_t size);
~MavlinkOrbSubscription();
bool update(const hrt_abstime t);
@@ -24,7 +24,7 @@ public:
const struct orb_metadata *get_topic();
private:
- const struct orb_metadata *_topic;
+ const orb_id_t _topic;
int _fd;
void *_data;
hrt_abstime _last_check;
diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp
new file mode 100644
index 0000000000..f5bb06ccd2
--- /dev/null
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -0,0 +1,38 @@
+/*
+ * mavlink_rate_limiter.cpp
+ *
+ * Created on: 26.02.2014
+ * Author: ton
+ */
+
+
+#include "mavlink_rate_limiter.h"
+
+MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
+{
+}
+
+MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000)
+{
+}
+
+MavlinkRateLimiter::~MavlinkRateLimiter()
+{
+}
+
+void
+MavlinkRateLimiter::set_interval(unsigned int interval)
+{
+ _interval = interval * 1000;
+}
+
+bool
+MavlinkRateLimiter::check(hrt_abstime t)
+{
+ uint64_t dt = t - _last_sent;
+ if (dt > 0 && dt >= _interval) {
+ _last_sent = (t / _interval) * _interval;
+ return true;
+ }
+ return false;
+}
diff --git a/src/modules/mavlink/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h
new file mode 100644
index 0000000000..6db65f638f
--- /dev/null
+++ b/src/modules/mavlink/mavlink_rate_limiter.h
@@ -0,0 +1,28 @@
+/*
+ * mavlink_rate_limiter.h
+ *
+ * Created on: 26.02.2014
+ * Author: ton
+ */
+
+#ifndef MAVLINK_RATE_LIMITER_H_
+#define MAVLINK_RATE_LIMITER_H_
+
+#include <drivers/drv_hrt.h>
+
+
+class MavlinkRateLimiter {
+private:
+ hrt_abstime _last_sent;
+ hrt_abstime _interval;
+
+public:
+ MavlinkRateLimiter();
+ MavlinkRateLimiter(unsigned int interval);
+ ~MavlinkRateLimiter();
+ void set_interval(unsigned int interval);
+ bool check(hrt_abstime t);
+};
+
+
+#endif /* MAVLINK_RATE_LIMITER_H_ */
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 16407366e4..703f74b4c3 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -10,7 +10,7 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
-MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
+MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
{
}
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 5ea6c0f46a..f09efa2e6c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -41,6 +41,7 @@ SRCS += mavlink_main.cpp \
mavlink_receiver.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
- mavlink_stream.cpp
+ mavlink_stream.cpp \
+ mavlink_rate_limiter.cpp
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
--
GitLab