aboutsummaryrefslogtreecommitdiff
path: root/apps/mavlink/mavlink.c
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2012-08-22 14:35:22 +0200
committerLorenz Meier <lm@inf.ethz.ch>2012-08-22 14:35:22 +0200
commitfa9f145b08cace0f48c808cf307daf6cd43d9bd6 (patch)
tree06e8e7482f12ac072aaf8b2573632eee44118bcb /apps/mavlink/mavlink.c
parent44ff4d4ee29f3da53f64b2f24b822f2f2f7032c0 (diff)
downloadpx4-firmware-fa9f145b08cace0f48c808cf307daf6cd43d9bd6.tar.gz
px4-firmware-fa9f145b08cace0f48c808cf307daf6cd43d9bd6.tar.bz2
px4-firmware-fa9f145b08cace0f48c808cf307daf6cd43d9bd6.zip
Fixed a bunch of issues in the arming state machine for multirotors, arming / disarming works fine now. Porting of various processes needed
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r--apps/mavlink/mavlink.c285
1 files changed, 248 insertions, 37 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 84ea0aae2..1b23f6751 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -73,6 +73,8 @@
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/manual_control_setpoint.h>
#include "waypoints.h"
#include "mavlink_log.h"
@@ -129,9 +131,6 @@ static struct vehicle_command_s vcmd;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t ardrone_motors_pub = -1;
static orb_advert_t cmd_pub = -1;
-static int sensor_sub = -1;
-static int att_sub = -1;
-static int global_pos_sub = -1;
static int local_pos_sub = -1;
static orb_advert_t flow_pub = -1;
static orb_advert_t global_position_setpoint_pub = -1;
@@ -146,6 +145,30 @@ static enum {
MAVLINK_INTERFACE_MODE_ONBOARD
} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
+static struct mavlink_subscriptions {
+ int sensor_sub;
+ int att_sub;
+ int global_pos_sub;
+ int act_0_sub;
+ int act_1_sub;
+ int act_2_sub;
+ int act_3_sub;
+ int gps_sub;
+ int man_control_sp_sub;
+ bool initialized;
+} mavlink_subs = {
+ .sensor_sub = 0,
+ .att_sub = 0,
+ .global_pos_sub = 0,
+ .act_0_sub = 0,
+ .act_1_sub = 0,
+ .act_2_sub = 0,
+ .act_3_sub = 0,
+ .gps_sub = 0,
+ .man_control_sp_sub = 0,
+ .initialized = false
+};
+
/* 3: Define waypoint helper functions */
void mavlink_wpm_send_message(mavlink_message_t *msg);
@@ -459,23 +482,33 @@ static void *receiveloop(void *arg)
return NULL;
}
-static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
+static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
{
int ret = OK;
switch (mavlink_msg_id) {
case MAVLINK_MSG_ID_SCALED_IMU:
/* senser sub triggers scaled IMU */
- orb_set_interval(sensor_sub, min_interval);
+ if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
break;
case MAVLINK_MSG_ID_RAW_IMU:
/* senser sub triggers RAW IMU */
- orb_set_interval(sensor_sub, min_interval);
+ if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
break;
case MAVLINK_MSG_ID_ATTITUDE:
/* attitude sub triggers attitude */
- orb_set_interval(att_sub, min_interval);
+ if (subs->att_sub) orb_set_interval(subs->att_sub, min_interval);
+ break;
+ case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
+ /* actuator_outputs triggers this message */
+ if (subs->act_0_sub) orb_set_interval(subs->act_0_sub, min_interval);
+ if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval);
+ if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval);
+ if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval);
break;
+ case MAVLINK_MSG_ID_MANUAL_CONTROL:
+ /* manual_control_setpoint triggers this message */
+ if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval);
default:
/* not found */
ret = ERROR;
@@ -494,13 +527,16 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
*/
static void *uorb_receiveloop(void *arg)
{
+ /* obtain reference to task's subscriptions */
+ struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg;
+
/* Set thread name */
prctl(PR_SET_NAME, "mavlink uORB", getpid());
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
- const ssize_t fdsc = 15;
+ const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@@ -515,28 +551,30 @@ static void *uorb_receiveloop(void *arg)
struct vehicle_local_position_setpoint_s local_sp;
struct vehicle_global_position_setpoint_s global_sp;
struct vehicle_attitude_setpoint_s att_sp;
+ struct actuator_outputs_s act_outputs;
+ struct manual_control_setpoint_s man_control;
} buf;
/* --- SENSORS RAW VALUE --- */
/* subscribe to ORB for sensors raw */
- sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
- fds[fdsc_count].fd = sensor_sub;
+ subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+ fds[fdsc_count].fd = subs->sensor_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ATTITUDE VALUE --- */
/* subscribe to ORB for attitude */
- att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
- orb_set_interval(att_sub, 100);
- fds[fdsc_count].fd = att_sub;
+ subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+ orb_set_interval(subs->att_sub, 100);
+ fds[fdsc_count].fd = subs->att_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GPS VALUE --- */
/* subscribe to ORB for attitude */
- int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
- orb_set_interval(gps_sub, 1000); /* 1Hz updates */
- fds[fdsc_count].fd = gps_sub;
+ subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+ orb_set_interval(subs->gps_sub, 1000); /* 1Hz updates */
+ fds[fdsc_count].fd = subs->gps_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -577,7 +615,7 @@ static void *uorb_receiveloop(void *arg)
/* --- GLOBAL POS VALUE --- */
/* struct already globally allocated and topic already subscribed */
- fds[fdsc_count].fd = global_pos_sub;
+ fds[fdsc_count].fd = subs->global_pos_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@@ -608,12 +646,39 @@ static void *uorb_receiveloop(void *arg)
/* --- ATTITUDE SETPOINT VALUE --- */
/* subscribe to ORB for attitude setpoint */
/* struct already allocated */
- int spa_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+ int spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_set_interval(spa_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = spa_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
+ /** --- ACTUATOR OUTPUTS --- */
+ subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
+ fds[fdsc_count].fd = subs->act_0_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+ subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
+ fds[fdsc_count].fd = subs->act_1_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+ subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
+ fds[fdsc_count].fd = subs->act_2_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+ subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
+ fds[fdsc_count].fd = subs->act_3_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
+ /** --- MAPPED MANUAL CONTROL INPUTS --- */
+ subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+ fds[fdsc_count].fd = subs->man_control_sp_sub;
+ fds[fdsc_count].events = POLLIN;
+ fdsc_count++;
+
+ /* all subscriptions initialized, return success */
+ subs->initialized = true;
+
unsigned int sensors_raw_counter = 0;
unsigned int attitude_counter = 0;
unsigned int gps_counter = 0;
@@ -651,7 +716,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy sensors raw data into local buffer */
- orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw);
+ orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
/* send raw imu data */
mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
@@ -667,7 +732,7 @@ static void *uorb_receiveloop(void *arg)
if (fds[ifds++].revents & POLLIN) {
/* copy attitude data into local buffer */
- orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att);
+ orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att);
/* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
@@ -678,7 +743,7 @@ static void *uorb_receiveloop(void *arg)
/* --- GPS VALUE --- */
if (fds[ifds++].revents & POLLIN) {
/* copy gps data into local buffer */
- orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps);
+ orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps);
/* GPS position */
mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible);
@@ -801,7 +866,7 @@ static void *uorb_receiveloop(void *arg)
/* --- VEHICLE GLOBAL POSITION --- */
if (fds[ifds++].revents & POLLIN) {
/* copy global position data into local buffer */
- orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+ orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos);
uint64_t timestamp = global_pos.timestamp;
int32_t lat = global_pos.lat;
int32_t lon = global_pos.lon;
@@ -845,6 +910,126 @@ static void *uorb_receiveloop(void *arg)
orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust);
}
+
+ /* --- ACTUATOR OUTPUTS 0 --- */
+ if (fds[ifds++].revents & POLLIN) {
+ /* copy actuator data into local buffer */
+ orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs);
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ 0 /* port 0 */,
+ buf.act_outputs.output[0],
+ buf.act_outputs.output[1],
+ buf.act_outputs.output[2],
+ buf.act_outputs.output[3],
+ buf.act_outputs.output[4],
+ buf.act_outputs.output[5],
+ buf.act_outputs.output[6],
+ buf.act_outputs.output[7]);
+ // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+ // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ // 1 /* port 1 */,
+ // buf.act_outputs.output[ 8],
+ // buf.act_outputs.output[ 9],
+ // buf.act_outputs.output[10],
+ // buf.act_outputs.output[11],
+ // buf.act_outputs.output[12],
+ // buf.act_outputs.output[13],
+ // buf.act_outputs.output[14],
+ // buf.act_outputs.output[15]);
+ // }
+ }
+
+ /* --- ACTUATOR OUTPUTS 1 --- */
+ if (fds[ifds++].revents & POLLIN) {
+ /* copy actuator data into local buffer */
+ orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs);
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/,
+ buf.act_outputs.output[0],
+ buf.act_outputs.output[1],
+ buf.act_outputs.output[2],
+ buf.act_outputs.output[3],
+ buf.act_outputs.output[4],
+ buf.act_outputs.output[5],
+ buf.act_outputs.output[6],
+ buf.act_outputs.output[7]);
+ if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ 3 /* port 3 */,
+ buf.act_outputs.output[ 8],
+ buf.act_outputs.output[ 9],
+ buf.act_outputs.output[10],
+ buf.act_outputs.output[11],
+ buf.act_outputs.output[12],
+ buf.act_outputs.output[13],
+ buf.act_outputs.output[14],
+ buf.act_outputs.output[15]);
+ }
+ }
+
+ /* --- ACTUATOR OUTPUTS 2 --- */
+ if (fds[ifds++].revents & POLLIN) {
+ /* copy actuator data into local buffer */
+ orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs);
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */,
+ buf.act_outputs.output[0],
+ buf.act_outputs.output[1],
+ buf.act_outputs.output[2],
+ buf.act_outputs.output[3],
+ buf.act_outputs.output[4],
+ buf.act_outputs.output[5],
+ buf.act_outputs.output[6],
+ buf.act_outputs.output[7]);
+ if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ 5 /* port 5 */,
+ buf.act_outputs.output[ 8],
+ buf.act_outputs.output[ 9],
+ buf.act_outputs.output[10],
+ buf.act_outputs.output[11],
+ buf.act_outputs.output[12],
+ buf.act_outputs.output[13],
+ buf.act_outputs.output[14],
+ buf.act_outputs.output[15]);
+ }
+ }
+
+ /* --- ACTUATOR OUTPUTS 3 --- */
+ if (fds[ifds++].revents & POLLIN) {
+ /* copy actuator data into local buffer */
+ orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs);
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */,
+ buf.act_outputs.output[0],
+ buf.act_outputs.output[1],
+ buf.act_outputs.output[2],
+ buf.act_outputs.output[3],
+ buf.act_outputs.output[4],
+ buf.act_outputs.output[5],
+ buf.act_outputs.output[6],
+ buf.act_outputs.output[7]);
+ if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+ mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+ 7 /* port 7 */,
+ buf.act_outputs.output[ 8],
+ buf.act_outputs.output[ 9],
+ buf.act_outputs.output[10],
+ buf.act_outputs.output[11],
+ buf.act_outputs.output[12],
+ buf.act_outputs.output[13],
+ buf.act_outputs.output[14],
+ buf.act_outputs.output[15]);
+ }
+ }
+
+ /* --- MAPPED MANUAL CONTROL INPUTS --- */
+ if (fds[ifds++].revents & POLLIN) {
+ /* copy local position data into local buffer */
+ orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control);
+ mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
+ buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1);
+ }
}
}
@@ -891,7 +1076,8 @@ void handleMessage(mavlink_message_t *msg)
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
- if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+ if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+ || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */ //TODO: check what happens with global_data buffers that are read by the mavlink app
@@ -1299,8 +1485,8 @@ int mavlink_thread_main(int argc, char *argv[])
/* topics to subscribe globally */
/* subscribe to ORB for global position */
- global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
- orb_set_interval(global_pos_sub, 1000); /* 1Hz active updates */
+ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */
/* subscribe to ORB for local position */
local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_set_interval(local_pos_sub, 1000); /* 1Hz active updates */
@@ -1315,7 +1501,7 @@ int mavlink_thread_main(int argc, char *argv[])
pthread_attr_init(&uorb_attr);
/* Set stack size, needs more than 4000 bytes */
pthread_attr_setstacksize(&uorb_attr, 4096);
- pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
+ pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, &mavlink_subs);
/* initialize waypoint manager */
mavlink_wpm_init(wpm);
@@ -1323,27 +1509,52 @@ int mavlink_thread_main(int argc, char *argv[])
uint16_t counter = 0;
int lowspeed_counter = 0;
+ /* make sure all threads have registered their subscriptions */
+ while (!mavlink_subs.initialized) {
+ usleep(500);
+ }
+
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 921600) {
/* 500 Hz / 2 ms */
- set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2);
- set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 2);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
+ /* 200 Hz / 5 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
+ /* 5 Hz */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
} else if (baudrate >= 460800) {
/* 250 Hz / 4 ms */
- set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 5);
- set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 5);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
+ /* 50 Hz / 20 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
+ /* 2 Hz */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */
- set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 50);
- set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
+ /* 10 Hz / 100 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
+ /* 1 Hz */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */
- set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100);
- set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 100);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
+ /* 5 Hz / 200 ms */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
+ /* 0.2 Hz */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
} else {
/* very low baud rate, limit to 1 Hz / 1000 ms */
- set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000);
- set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
+ /* 0.5 Hz */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
+ /* 0.1 Hz */
+ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
}
while (!thread_should_exit) {
@@ -1351,7 +1562,7 @@ int mavlink_thread_main(int argc, char *argv[])
if (mavlink_exit_requested) break;
/* get local and global position */
- orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
/* check if waypoint has been reached against the last positions */