aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/mavlink/mavlink_main.cpp448
-rw-r--r--src/modules/mavlink/mavlink_main.h35
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp495
-rw-r--r--src/modules/mavlink/mavlink_messages.h41
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.cpp44
-rw-r--r--src/modules/mavlink/mavlink_orb_subscription.h44
-rw-r--r--src/modules/mavlink/mavlink_rate_limiter.cpp44
-rw-r--r--src/modules/mavlink/mavlink_rate_limiter.h44
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp33
-rw-r--r--src/modules/mavlink/mavlink_receiver.h4
-rw-r--r--src/modules/mavlink/mavlink_stream.cpp42
-rw-r--r--src/modules/mavlink/mavlink_stream.h44
12 files changed, 867 insertions, 451 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 672daf641..c97786553 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -37,6 +37,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -91,7 +92,7 @@ static const int ERROR = -1;
#define MAX_DATA_RATE 10000 // max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 // 100 Hz
-static Mavlink* _head = nullptr;
+static Mavlink *_head = nullptr;
/* TODO: if this is a class member it crashes */
static struct file_operations fops;
@@ -112,40 +113,47 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int uart = -1;
switch (channel) {
- case MAVLINK_COMM_0:
- uart = Mavlink::get_uart_fd(0);
+ case MAVLINK_COMM_0:
+ uart = Mavlink::get_uart_fd(0);
break;
- case MAVLINK_COMM_1:
- uart = Mavlink::get_uart_fd(1);
+
+ case MAVLINK_COMM_1:
+ uart = Mavlink::get_uart_fd(1);
break;
- case MAVLINK_COMM_2:
- uart = Mavlink::get_uart_fd(2);
+
+ case MAVLINK_COMM_2:
+ uart = Mavlink::get_uart_fd(2);
break;
- case MAVLINK_COMM_3:
- uart = Mavlink::get_uart_fd(3);
+
+ case MAVLINK_COMM_3:
+ uart = Mavlink::get_uart_fd(3);
break;
- #ifdef MAVLINK_COMM_4
- case MAVLINK_COMM_4:
- uart = Mavlink::get_uart_fd(4);
+#ifdef MAVLINK_COMM_4
+
+ case MAVLINK_COMM_4:
+ uart = Mavlink::get_uart_fd(4);
break;
- #endif
- #ifdef MAVLINK_COMM_5
- case MAVLINK_COMM_5:
- uart = Mavlink::get_uart_fd(5);
+#endif
+#ifdef MAVLINK_COMM_5
+
+ case MAVLINK_COMM_5:
+ uart = Mavlink::get_uart_fd(5);
break;
- #endif
- #ifdef MAVLINK_COMM_6
- case MAVLINK_COMM_6:
- uart = Mavlink::get_uart_fd(6);
+#endif
+#ifdef MAVLINK_COMM_6
+
+ case MAVLINK_COMM_6:
+ uart = Mavlink::get_uart_fd(6);
break;
- #endif
+#endif
}
ssize_t desired = (sizeof(uint8_t) * length);
ssize_t ret = write(uart, ch, desired);
- if (ret != desired)
+ if (ret != desired) {
warn("write err");
+ }
}
@@ -168,7 +176,7 @@ Mavlink::Mavlink() :
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
wpm = &wpm_s;
- fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
+ fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
// _parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
}
@@ -206,8 +214,9 @@ int
Mavlink::instance_count()
{
/* note: a local buffer count will help if this ever is called often */
- Mavlink* inst = ::_head;
+ Mavlink *inst = ::_head;
unsigned inst_index = 0;
+
while (inst != nullptr) {
inst = inst->next;
inst_index++;
@@ -225,14 +234,17 @@ Mavlink::new_instance()
/* create the first instance at _head */
if (::_head == nullptr) {
::_head = inst;
- /* afterwards follow the next and append the instance */
+ /* afterwards follow the next and append the instance */
+
} else {
while (next->next != nullptr) {
next = next->next;
}
+
/* now parent has a null pointer, fill it */
next->next = inst;
}
+
return inst;
}
@@ -241,6 +253,7 @@ Mavlink::get_instance(unsigned instance)
{
Mavlink *inst = ::_head;
unsigned inst_index = 0;
+
while (inst->next != nullptr && inst_index < instance) {
inst = inst->next;
inst_index++;
@@ -277,6 +290,7 @@ Mavlink::destroy_all_instances()
unsigned iterations = 0;
warnx("waiting for instances to stop");
+
while (next_inst != nullptr) {
inst_to_del = next_inst;
@@ -284,6 +298,7 @@ Mavlink::destroy_all_instances()
/* set flag to stop thread and wait for all threads to finish */
inst_to_del->_task_should_exit = true;
+
while (inst_to_del->thread_running) {
printf(".");
usleep(10000);
@@ -294,6 +309,7 @@ Mavlink::destroy_all_instances()
return ERROR;
}
}
+
delete inst_to_del;
}
@@ -308,23 +324,29 @@ Mavlink::destroy_all_instances()
bool
Mavlink::instance_exists(const char *device_name, Mavlink *self)
{
- Mavlink* inst = ::_head;
+ Mavlink *inst = ::_head;
+
while (inst != nullptr) {
/* don't compare with itself */
- if (inst != self && !strcmp(device_name, inst->device_name))
+ if (inst != self && !strcmp(device_name, inst->device_name)) {
return true;
+ }
+
inst = inst->next;
}
+
return false;
}
int
Mavlink::get_uart_fd(unsigned index)
{
- Mavlink* inst = get_instance(index);
- if (inst)
+ Mavlink *inst = get_instance(index);
+
+ if (inst) {
return inst->get_uart_fd();
+ }
return -1;
}
@@ -353,21 +375,23 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
- const char *txt = (const char *)arg;
+ const char *txt = (const char *)arg;
// printf("logmsg: %s\n", txt);
- struct mavlink_logmessage msg;
- strncpy(msg.text, txt, sizeof(msg.text));
+ struct mavlink_logmessage msg;
+ strncpy(msg.text, txt, sizeof(msg.text));
+
+ Mavlink *inst = ::_head;
- Mavlink* inst = ::_head;
- while (inst != nullptr) {
+ while (inst != nullptr) {
- mavlink_logbuffer_write(&inst->lb, &msg);
- inst->total_counter++;
- inst = inst->next;
+ mavlink_logbuffer_write(&inst->lb, &msg);
+ inst->total_counter++;
+ inst = inst->next;
+ }
+
+ return OK;
}
- return OK;
- }
default:
return ENOTTY;
@@ -582,7 +606,7 @@ int Mavlink::mavlink_pm_send_param_for_name(const char *name)
int Mavlink::mavlink_pm_send_param(param_t param)
{
- if (param == PARAM_INVALID) return 1;
+ if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
@@ -714,38 +738,40 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
{
/* only support global waypoints for now */
switch (mavlink_mission_item->frame) {
- case MAV_FRAME_GLOBAL:
- mission_item->lat = (double)mavlink_mission_item->x;
- mission_item->lon = (double)mavlink_mission_item->y;
- mission_item->altitude = mavlink_mission_item->z;
- mission_item->altitude_is_relative = false;
- break;
+ case MAV_FRAME_GLOBAL:
+ mission_item->lat = (double)mavlink_mission_item->x;
+ mission_item->lon = (double)mavlink_mission_item->y;
+ mission_item->altitude = mavlink_mission_item->z;
+ mission_item->altitude_is_relative = false;
+ break;
- case MAV_FRAME_GLOBAL_RELATIVE_ALT:
- mission_item->lat = (double)mavlink_mission_item->x;
- mission_item->lon = (double)mavlink_mission_item->y;
- mission_item->altitude = mavlink_mission_item->z;
- mission_item->altitude_is_relative = true;
- break;
+ case MAV_FRAME_GLOBAL_RELATIVE_ALT:
+ mission_item->lat = (double)mavlink_mission_item->x;
+ mission_item->lon = (double)mavlink_mission_item->y;
+ mission_item->altitude = mavlink_mission_item->z;
+ mission_item->altitude_is_relative = true;
+ break;
- case MAV_FRAME_LOCAL_NED:
- case MAV_FRAME_LOCAL_ENU:
- return MAV_MISSION_UNSUPPORTED_FRAME;
- case MAV_FRAME_MISSION:
- default:
- return MAV_MISSION_ERROR;
+ case MAV_FRAME_LOCAL_NED:
+ case MAV_FRAME_LOCAL_ENU:
+ return MAV_MISSION_UNSUPPORTED_FRAME;
+
+ case MAV_FRAME_MISSION:
+ default:
+ return MAV_MISSION_ERROR;
}
switch (mavlink_mission_item->command) {
- case MAV_CMD_NAV_TAKEOFF:
- mission_item->pitch_min = mavlink_mission_item->param2;
- break;
- default:
- mission_item->acceptance_radius = mavlink_mission_item->param2;
- break;
+ case MAV_CMD_NAV_TAKEOFF:
+ mission_item->pitch_min = mavlink_mission_item->param2;
+ break;
+
+ default:
+ mission_item->acceptance_radius = mavlink_mission_item->param2;
+ break;
}
- mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
+ mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
@@ -762,25 +788,27 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
{
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
+
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
-
+
switch (mission_item->nav_cmd) {
- case NAV_CMD_TAKEOFF:
- mavlink_mission_item->param2 = mission_item->pitch_min;
- break;
- default:
- mavlink_mission_item->param2 = mission_item->acceptance_radius;
- break;
+ case NAV_CMD_TAKEOFF:
+ mavlink_mission_item->param2 = mission_item->pitch_min;
+ break;
+
+ default:
+ mavlink_mission_item->param2 = mission_item->acceptance_radius;
+ break;
}
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
- mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
- mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
+ mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
+ mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
@@ -817,7 +845,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
- if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
+ if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); }
}
/*
@@ -846,7 +874,8 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
} else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
- if (_verbose) warnx("ERROR: index out of bounds");
+
+ if (_verbose) { warnx("ERROR: index out of bounds"); }
}
}
@@ -862,7 +891,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
- if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
+ if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); }
}
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
@@ -870,11 +899,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
struct mission_item_s mission_item;
ssize_t len = sizeof(struct mission_item_s);
-
+
dm_item_t dm_current;
if (wpm->current_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
@@ -892,10 +922,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
mavlink_missionlib_send_message(&msg);
- if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
+ if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); }
+
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
- if (_verbose) warnx("ERROR: could not read WP%u", seq);
+
+ if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
}
}
@@ -910,11 +942,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
- if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
+ if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
- if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity");
+
+ if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
}
}
@@ -935,7 +968,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
- if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
+ if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); }
}
void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
@@ -945,7 +978,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
mavlink_missionlib_send_gcs_string("Operation timeout");
- if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
+ if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); }
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
wpm->current_partner_sysid = 0;
@@ -960,7 +993,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
switch (msg->msgid) {
- case MAVLINK_MSG_ID_MISSION_ACK: {
+ case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
@@ -977,13 +1010,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
- if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch");
+
+ if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
}
break;
}
- case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
+ case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
@@ -995,30 +1029,33 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mission.current_index = wpc.seq;
publish_mission();
-
+
/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
// mavlink_wpm_send_waypoint_current(wpc.seq);
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
- if (_verbose) warnx("IGN WP CURR CMD: Not in list");
+
+ if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
- if (_verbose) warnx("IGN WP CURR CMD: Busy");
+
+ if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
- if (_verbose) warnx("REJ. WP CMD: target id mismatch");
+
+ if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
}
break;
}
- case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
+ case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
@@ -1027,14 +1064,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpm->size > 0) {
-
+
wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
wpm->current_wp_id = 0;
wpm->current_partner_sysid = msg->sysid;
wpm->current_partner_compid = msg->compid;
} else {
- if (_verbose) warnx("No waypoints send");
+ if (_verbose) { warnx("No waypoints send"); }
}
wpm->current_count = wpm->size;
@@ -1042,17 +1079,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
- if (_verbose) warnx("IGN REQUEST LIST: Busy");
+
+ if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
}
+
} else {
mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
- if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch");
+
+ if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
}
break;
}
- case MAVLINK_MSG_ID_MISSION_REQUEST: {
+ case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
@@ -1062,22 +1102,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq >= wpm->size) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
- if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
+
+ if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
+
break;
}
- /*
- * Ensure that we are in the correct state and that the first request has id 0
+ /*
+ * Ensure that we are in the correct state and that the first request has id 0
* and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
*/
if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) {
- if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+ if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+
wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
+
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
- if (_verbose) warnx("REJ. WP CMD: First id != 0");
+
+ if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
+
break;
}
@@ -1085,22 +1131,26 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq == wpm->current_wp_id) {
- if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
+ if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else if (wpr.seq == wpm->current_wp_id + 1) {
- if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
-
+ if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
+
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
- if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
+
+ if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); }
+
break;
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
- if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
+
+ if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); }
+
break;
}
@@ -1109,11 +1159,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq < wpm->size) {
- mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id);
+ mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
- if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
+
+ if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
}
@@ -1122,18 +1173,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
- if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
+
+ if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
- if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+
+ if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
+
break;
}
- case MAVLINK_MSG_ID_MISSION_COUNT: {
+ case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
@@ -1143,18 +1197,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
- if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
+ if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); }
+
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
break;
}
if (wpc.count == 0) {
mavlink_missionlib_send_gcs_string("COUNT 0");
- if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
+
+ if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
+
break;
}
-
- if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
+
+ if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
wpm->current_wp_id = 0;
@@ -1168,24 +1225,31 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
- if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
+
+ if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
+
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
- if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
+
+ if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); }
}
+
} else {
- mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
- if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy");
+ mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
+
+ if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
}
+
} else {
mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
- if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+
+ if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
break;
- case MAVLINK_MSG_ID_MISSION_ITEM: {
+ case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
@@ -1200,11 +1264,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
- if (wp.seq != 0) {
- mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
- warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
- break;
- }
+ if (wp.seq != 0) {
+ mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
+ warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
+ break;
+ }
+
} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (wp.seq >= wpm->current_count) {
@@ -1239,6 +1304,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_dataman_id == 0) {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
mission.dataman_id = 1;
+
} else {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.dataman_id = 0;
@@ -1260,13 +1326,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
wpm->current_wp_id = wp.seq + 1;
if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-
- if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
+
+ if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); }
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
mission.count = wpm->current_count;
-
+
publish_mission();
wpm->current_dataman_id = mission.dataman_id;
@@ -1280,13 +1346,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
- if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+
+ if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
}
- case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
+ case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
@@ -1305,21 +1372,24 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
+
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
}
-
+
} else {
mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
- if (_verbose) warnx("IGN WP CLEAR CMD: Busy");
+
+ if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
}
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
- if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
+
+ if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
@@ -1352,8 +1422,9 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
while (i < len - 1) {
statustext.text[i] = string[i];
- if (string[i] == '\0')
+ if (string[i] == '\0') {
break;
+ }
i++;
}
@@ -1411,6 +1482,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
LL_DELETE(_streams, stream);
delete stream;
}
+
return OK;
}
}
@@ -1466,18 +1538,22 @@ Mavlink::task_main(int argc, char *argv[])
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
+
if (_baudrate < 9600 || _baudrate > 921600) {
warnx("invalid baud rate '%s'", optarg);
err_flag = true;
}
+
break;
case 'r':
_datarate = strtoul(optarg, NULL, 10);
+
if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
warnx("invalid data rate '%s'", optarg);
err_flag = true;
}
+
break;
case 'd':
@@ -1501,6 +1577,7 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(optarg, "custom") == 0) {
_mode = MODE_CUSTOM;
}
+
break;
case 'v':
@@ -1533,35 +1610,41 @@ Mavlink::task_main(int argc, char *argv[])
/* inform about mode */
switch (_mode) {
- case MODE_CUSTOM:
- warnx("mode: CUSTOM");
- break;
- case MODE_OFFBOARD:
- warnx("mode: OFFBOARD");
- break;
- case MODE_ONBOARD:
- warnx("mode: ONBOARD");
- break;
- case MODE_HIL:
- warnx("mode: HIL");
- break;
- default:
- warnx("ERROR: Unknown mode");
- break;
+ case MODE_CUSTOM:
+ warnx("mode: CUSTOM");
+ break;
+
+ case MODE_OFFBOARD:
+ warnx("mode: OFFBOARD");
+ break;
+
+ case MODE_ONBOARD:
+ warnx("mode: ONBOARD");
+ break;
+
+ case MODE_HIL:
+ warnx("mode: HIL");
+ break;
+
+ default:
+ warnx("ERROR: Unknown mode");
+ break;
}
- switch(_mode) {
- case MODE_OFFBOARD:
- case MODE_HIL:
- case MODE_CUSTOM:
- _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
- break;
- case MODE_ONBOARD:
- _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
- break;
- default:
- _mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
- break;
+ switch (_mode) {
+ case MODE_OFFBOARD:
+ case MODE_HIL:
+ case MODE_CUSTOM:
+ _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
+ break;
+
+ case MODE_ONBOARD:
+ _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
+ break;
+
+ default:
+ _mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
+ break;
}
warnx("data rate: %d bytes/s", _datarate);
@@ -1576,8 +1659,9 @@ Mavlink::task_main(int argc, char *argv[])
/* default values for arguments */
_uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart);
- if (_uart < 0)
+ if (_uart < 0) {
err(1, "could not open %s", device_name);
+ }
/* create the device node that's used for sending text log messages, etc. */
if (instance_count() == 1) {
@@ -1615,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("HEARTBEAT", 1.0f);
- switch(_mode) {
+ switch (_mode) {
case MODE_OFFBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
@@ -1660,10 +1744,12 @@ Mavlink::task_main(int argc, char *argv[])
if (status_sub->update(t)) {
/* switch HIL mode if required */
- if (status->hil_state == HIL_STATE_ON)
+ if (status->hil_state == HIL_STATE_ON) {
set_hil_enabled(true);
- else if (status->hil_state == HIL_STATE_OFF)
+
+ } else if (status->hil_state == HIL_STATE_OFF) {
set_hil_enabled(false);
+ }
}
MavlinkStream *stream;
@@ -1677,13 +1763,14 @@ Mavlink::task_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
- if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission);
+ if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); }
if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
}
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);
@@ -1740,11 +1827,11 @@ Mavlink::start(int argc, char *argv[])
sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
- SCHED_DEFAULT,
- SCHED_PRIORITY_DEFAULT,
- 2048,
- (main_t)&Mavlink::start_helper,
- (const char **)argv);
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_DEFAULT,
+ 2048,
+ (main_t)&Mavlink::start_helper,
+ (const char **)argv);
// while (!this->is_running()) {
// usleep(200);
@@ -1775,7 +1862,7 @@ Mavlink::start(int argc, char *argv[])
}
void
-Mavlink::status()
+Mavlink::status()
{
warnx("running");
}
@@ -1799,9 +1886,11 @@ Mavlink::stream(int argc, char *argv[])
switch (ch) {
case 'r':
rate = strtod(optarg, nullptr);
+
if (rate < 0.0f) {
err_flag = true;
}
+
break;
case 'd':
@@ -1820,6 +1909,7 @@ Mavlink::stream(int argc, char *argv[])
if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
Mavlink *inst = get_instance_for_device(device_name);
+
if (inst != nullptr) {
if (OK == inst->configure_stream(stream_name, rate)) {
if (rate > 0.0f) {
@@ -1865,15 +1955,15 @@ int mavlink_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances();
- // } else if (!strcmp(argv[1], "status")) {
- // mavlink::g_mavlink->status();
+ // } else if (!strcmp(argv[1], "status")) {
+ // mavlink::g_mavlink->status();
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream(argc, argv);
- } else {
- usage();
- }
+ } else {
+ usage();
+ }
return 0;
}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 41e781ee8..532c9bcee 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -36,6 +36,7 @@
* MAVLink 1.0 protocol interface definition.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
@@ -55,24 +56,24 @@
#include "mavlink_stream.h"
#include "mavlink_messages.h"
- // FIXME XXX - TO BE MOVED TO XML
+// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
- MAVLINK_WPM_STATE_IDLE = 0,
- MAVLINK_WPM_STATE_SENDLIST,
- MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
- MAVLINK_WPM_STATE_GETLIST,
- MAVLINK_WPM_STATE_GETLIST_GETWPS,
- MAVLINK_WPM_STATE_GETLIST_GOTALL,
- MAVLINK_WPM_STATE_ENUM_END
+ MAVLINK_WPM_STATE_IDLE = 0,
+ MAVLINK_WPM_STATE_SENDLIST,
+ MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
+ MAVLINK_WPM_STATE_GETLIST,
+ MAVLINK_WPM_STATE_GETLIST_GETWPS,
+ MAVLINK_WPM_STATE_GETLIST_GOTALL,
+ MAVLINK_WPM_STATE_ENUM_END
};
enum MAVLINK_WPM_CODES {
- MAVLINK_WPM_CODE_OK = 0,
- MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
- MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
- MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
- MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
- MAVLINK_WPM_CODE_ENUM_END
+ MAVLINK_WPM_CODE_OK = 0,
+ MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
+ MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
+ MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
+ MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
+ MAVLINK_WPM_CODE_ENUM_END
};
@@ -144,7 +145,7 @@ public:
const char *device_name;
enum MAVLINK_MODE {
- MODE_CUSTOM=0,
+ MODE_CUSTOM = 0,
MODE_OFFBOARD,
MODE_ONBOARD,
MODE_HIL
@@ -186,7 +187,7 @@ public:
bool _task_should_exit; /**< if true, mavlink task should exit */
protected:
- Mavlink* next;
+ Mavlink *next;
private:
int _mavlink_fd;
@@ -233,7 +234,7 @@ private:
unsigned int mavlink_param_queue_index;
bool mavlink_link_termination_allowed;
-
+
/**
* Send one parameter.
*
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 8097ecdb3..820faae1c 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1,8 +1,42 @@
-/*
- * mavlink_messages.cpp
+/****************************************************************************
*
- * Created on: 25.02.2014
- * Author: ton
+ * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_messages.cpp
+ * MAVLink 1.0 message formatters implementation.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdio.h>
@@ -43,7 +77,7 @@
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);
+ uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
uint16_t
cm_uint16_from_m_float(float m)
@@ -59,7 +93,7 @@ cm_uint16_from_m_float(float m)
}
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)
+ uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
*mavlink_state = 0;
*mavlink_base_mode = 0;
@@ -72,7 +106,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
/* arming state */
if (status->arming_state == ARMING_STATE_ARMED
- || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ || status->arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@@ -81,34 +115,44 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
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 */
+ /* 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;
}
@@ -118,24 +162,30 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
/* 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
+ || 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 {
+class MavlinkStreamHeartbeat : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -164,7 +214,8 @@ protected:
pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
status_sub->update(t);
pos_sp_triplet_sub->update(t);
@@ -184,7 +235,8 @@ protected:
};
-class MavlinkStreamSysStatus : public MavlinkStream {
+class MavlinkStreamSysStatus : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -207,28 +259,30 @@ protected:
status = (struct vehicle_status_s *)status_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
- status->onboard_control_sensors_present,
- status->onboard_control_sensors_enabled,
- status->onboard_control_sensors_health,
- status->load * 1000.0f,
- status->battery_voltage * 1000.0f,
- status->battery_current * 1000.0f,
- status->battery_remaining,
- status->drop_rate_comm,
- status->errors_comm,
- status->errors_count1,
- status->errors_count2,
- status->errors_count3,
- status->errors_count4);
+ status->onboard_control_sensors_present,
+ status->onboard_control_sensors_enabled,
+ status->onboard_control_sensors_health,
+ status->load * 1000.0f,
+ status->battery_voltage * 1000.0f,
+ status->battery_current * 1000.0f,
+ status->battery_remaining,
+ status->drop_rate_comm,
+ status->errors_comm,
+ status->errors_count1,
+ status->errors_count2,
+ status->errors_count3,
+ status->errors_count4);
}
};
-class MavlinkStreamHighresIMU : public MavlinkStream {
+class MavlinkStreamHighresIMU : public MavlinkStream
+{
public:
MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0)
{
@@ -260,7 +314,8 @@ protected:
sensor = (struct sensor_combined_s *)sensor_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
sensor_sub->update(t);
uint16_t fields_updated = 0;
@@ -290,18 +345,19 @@ protected:
}
mavlink_msg_highres_imu_send(_channel,
- sensor->timestamp,
- sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
- sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
- sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
- sensor->baro_pres_mbar, sensor->differential_pressure_pa,
- sensor->baro_alt_meter, sensor->baro_temp_celcius,
- fields_updated);
+ sensor->timestamp,
+ sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
+ sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
+ sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
+ sensor->baro_pres_mbar, sensor->differential_pressure_pa,
+ sensor->baro_alt_meter, sensor->baro_temp_celcius,
+ fields_updated);
}
};
-class MavlinkStreamAttitude : public MavlinkStream {
+class MavlinkStreamAttitude : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -324,18 +380,20 @@ protected:
att = (struct vehicle_attitude_s *)att_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
att_sub->update(t);
mavlink_msg_attitude_send(_channel,
- att->timestamp / 1000,
- att->roll, att->pitch, att->yaw,
- att->rollspeed, att->pitchspeed, att->yawspeed);
+ att->timestamp / 1000,
+ att->roll, att->pitch, att->yaw,
+ att->rollspeed, att->pitchspeed, att->yawspeed);
}
};
-class MavlinkStreamAttitudeQuaternion : public MavlinkStream {
+class MavlinkStreamAttitudeQuaternion : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -358,23 +416,25 @@ protected:
att = (struct vehicle_attitude_s *)att_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
att_sub->update(t);
mavlink_msg_attitude_quaternion_send(_channel,
- att->timestamp / 1000,
- att->q[0],
- att->q[1],
- att->q[2],
- att->q[3],
- att->rollspeed,
- att->pitchspeed,
- att->yawspeed);
+ att->timestamp / 1000,
+ att->q[0],
+ att->q[1],
+ att->q[2],
+ att->q[3],
+ att->rollspeed,
+ att->pitchspeed,
+ att->yawspeed);
}
};
-class MavlinkStreamVFRHUD : public MavlinkStream {
+class MavlinkStreamVFRHUD : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -421,7 +481,8 @@ protected:
airspeed = (struct airspeed_s *)airspeed_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
att_sub->update(t);
pos_sub->update(t);
armed_sub->update(t);
@@ -433,17 +494,18 @@ protected:
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
- airspeed->true_airspeed_m_s,
- groundspeed,
- heading,
- throttle,
- pos->alt,
- -pos->vel_d);
+ airspeed->true_airspeed_m_s,
+ groundspeed,
+ heading,
+ throttle,
+ pos->alt,
+ -pos->vel_d);
}
};
-class MavlinkStreamGPSRawInt : public MavlinkStream {
+class MavlinkStreamGPSRawInt : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -466,25 +528,27 @@ protected:
gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
gps_sub->update(t);
mavlink_msg_gps_raw_int_send(_channel,
- gps->timestamp_position,
- gps->fix_type,
- gps->lat,
- gps->lon,
- gps->alt,
- cm_uint16_from_m_float(gps->eph_m),
- cm_uint16_from_m_float(gps->epv_m),
- gps->vel_m_s * 100.0f,
- _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- gps->satellites_visible);
+ gps->timestamp_position,
+ gps->fix_type,
+ gps->lat,
+ gps->lon,
+ gps->alt,
+ cm_uint16_from_m_float(gps->eph_m),
+ cm_uint16_from_m_float(gps->epv_m),
+ gps->vel_m_s * 100.0f,
+ _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+ gps->satellites_visible);
}
};
-class MavlinkStreamGlobalPositionInt : public MavlinkStream {
+class MavlinkStreamGlobalPositionInt : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -513,25 +577,27 @@ protected:
home = (struct home_position_s *)home_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
pos_sub->update(t);
home_sub->update(t);
mavlink_msg_global_position_int_send(_channel,
- pos->timestamp / 1000,
- pos->lat * 1e7,
- pos->lon * 1e7,
- pos->alt * 1000.0f,
- (pos->alt - home->alt) * 1000.0f,
- pos->vel_n * 100.0f,
- pos->vel_e * 100.0f,
- pos->vel_d * 100.0f,
- _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+ pos->timestamp / 1000,
+ pos->lat * 1e7,
+ pos->lon * 1e7,
+ pos->alt * 1000.0f,
+ (pos->alt - home->alt) * 1000.0f,
+ pos->vel_n * 100.0f,
+ pos->vel_e * 100.0f,
+ pos->vel_d * 100.0f,
+ _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
};
-class MavlinkStreamLocalPositionNED : public MavlinkStream {
+class MavlinkStreamLocalPositionNED : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -554,22 +620,24 @@ protected:
pos = (struct vehicle_local_position_s *)pos_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
pos_sub->update(t);
mavlink_msg_local_position_ned_send(_channel,
- pos->timestamp / 1000,
- pos->x,
- pos->y,
- pos->z,
- pos->vx,
- pos->vy,
- pos->vz);
+ pos->timestamp / 1000,
+ pos->x,
+ pos->y,
+ pos->z,
+ pos->vx,
+ pos->vy,
+ pos->vz);
}
};
-class MavlinkStreamGPSGlobalOrigin : public MavlinkStream {
+class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -592,18 +660,20 @@ protected:
home = (struct home_position_s *)home_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
home_sub->update(t);
mavlink_msg_gps_global_origin_send(_channel,
- (int32_t)(home->lat * 1e7),
- (int32_t)(home->lon * 1e7),
- (int32_t)(home->alt) * 1000.0f);
+ (int32_t)(home->lat * 1e7),
+ (int32_t)(home->lon * 1e7),
+ (int32_t)(home->alt) * 1000.0f);
}
};
-class MavlinkStreamServoOutputRaw : public MavlinkStream {
+class MavlinkStreamServoOutputRaw : public MavlinkStream
+{
public:
MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
{
@@ -641,25 +711,27 @@ protected:
act = (struct actuator_outputs_s *)act_sub->get_data();
}
- void send(const hrt_abstime t) {
+ 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]);
+ 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 {
+class MavlinkStreamHILControls : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -694,7 +766,8 @@ protected:
act = (struct actuator_outputs_s *)act_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
status_sub->update(t);
pos_sp_triplet_sub->update(t);
act_sub->update(t);
@@ -710,65 +783,66 @@ protected:
/* 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);
+ 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);
+ 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);
+ 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);
+ 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);
}
}
};
-class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream {
+class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -791,7 +865,8 @@ protected:
pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
pos_sp_triplet_sub->update(t);
mavlink_msg_global_position_setpoint_int_send(_channel,
@@ -804,7 +879,8 @@ protected:
};
-class MavlinkStreamLocalPositionSetpoint : public MavlinkStream {
+class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -827,7 +903,8 @@ protected:
pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
pos_sp_sub->update(t);
mavlink_msg_local_position_setpoint_send(_channel,
@@ -840,7 +917,8 @@ protected:
};
-class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream {
+class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -863,7 +941,8 @@ protected:
att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
att_sp_sub->update(t);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
@@ -876,7 +955,8 @@ protected:
};
-class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream {
+class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -899,7 +979,8 @@ protected:
att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
att_rates_sp_sub->update(t);
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
@@ -912,7 +993,8 @@ protected:
};
-class MavlinkStreamRCChannelsRaw : public MavlinkStream {
+class MavlinkStreamRCChannelsRaw : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -935,7 +1017,8 @@ protected:
rc = (struct rc_input_values *)rc_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
rc_sub->update(t);
const unsigned port_width = 8;
@@ -943,23 +1026,24 @@ protected:
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel,
- rc->timestamp_publication / 1000,
- i,
- (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
- rc->rssi);
+ rc->timestamp_publication / 1000,
+ i,
+ (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
+ (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
+ (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
+ (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
+ (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
+ (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
+ (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
+ (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
+ rc->rssi);
}
}
};
-class MavlinkStreamManualControl : public MavlinkStream {
+class MavlinkStreamManualControl : public MavlinkStream
+{
public:
const char *get_name()
{
@@ -982,43 +1066,44 @@ protected:
manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
}
- void send(const hrt_abstime t) {
+ void send(const hrt_abstime t)
+ {
manual_sub->update(t);
mavlink_msg_manual_control_send(_channel,
- mavlink_system.sysid,
- manual->roll * 1000,
- manual->pitch * 1000,
- manual->yaw * 1000,
- manual->throttle * 1000,
- 0);
+ mavlink_system.sysid,
+ manual->roll * 1000,
+ manual->pitch * 1000,
+ manual->yaw * 1000,
+ manual->throttle * 1000,
+ 0);
}
};
MavlinkStream *streams_list[] = {
- new MavlinkStreamHeartbeat(),
- new MavlinkStreamSysStatus(),
- new MavlinkStreamHighresIMU(),
- new MavlinkStreamAttitude(),
- new MavlinkStreamAttitudeQuaternion(),
- new MavlinkStreamVFRHUD(),
- new MavlinkStreamGPSRawInt(),
- new MavlinkStreamGlobalPositionInt(),
- new MavlinkStreamLocalPositionNED(),
- new MavlinkStreamGPSGlobalOrigin(),
- new MavlinkStreamServoOutputRaw(0),
- new MavlinkStreamServoOutputRaw(1),
- new MavlinkStreamServoOutputRaw(2),
- new MavlinkStreamServoOutputRaw(3),
- new MavlinkStreamHILControls(),
- new MavlinkStreamGlobalPositionSetpointInt(),
- new MavlinkStreamLocalPositionSetpoint(),
- new MavlinkStreamRollPitchYawThrustSetpoint(),
- new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
- new MavlinkStreamRCChannelsRaw(),
- new MavlinkStreamManualControl(),
- nullptr
+ new MavlinkStreamHeartbeat(),
+ new MavlinkStreamSysStatus(),
+ new MavlinkStreamHighresIMU(),
+ new MavlinkStreamAttitude(),
+ new MavlinkStreamAttitudeQuaternion(),
+ new MavlinkStreamVFRHUD(),
+ new MavlinkStreamGPSRawInt(),
+ new MavlinkStreamGlobalPositionInt(),
+ new MavlinkStreamLocalPositionNED(),
+ new MavlinkStreamGPSGlobalOrigin(),
+ new MavlinkStreamServoOutputRaw(0),
+ new MavlinkStreamServoOutputRaw(1),
+ new MavlinkStreamServoOutputRaw(2),
+ new MavlinkStreamServoOutputRaw(3),
+ new MavlinkStreamHILControls(),
+ new MavlinkStreamGlobalPositionSetpointInt(),
+ new MavlinkStreamLocalPositionSetpoint(),
+ new MavlinkStreamRollPitchYawThrustSetpoint(),
+ new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
+ new MavlinkStreamRCChannelsRaw(),
+ new MavlinkStreamManualControl(),
+ nullptr
};
diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h
index 3dc6cb699..b8823263a 100644
--- a/src/modules/mavlink/mavlink_messages.h
+++ b/src/modules/mavlink/mavlink_messages.h
@@ -1,8 +1,41 @@
-/*
- * mavlink_messages.h
+/****************************************************************************
*
- * Created on: 25.02.2014
- * Author: ton
+ * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_messages.h
+ * MAVLink 1.0 message formatters definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_MESSAGES_H_
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 35470a19a..e1208bca9 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -1,10 +1,42 @@
-/*
- * mavlink_orb_subscription.cpp
+/****************************************************************************
*
- * Created on: 23.02.2014
- * Author: ton
- */
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mavlink_orb_subscription.cpp
+ * uORB subscription implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
#include <unistd.h>
#include <stdlib.h>
@@ -45,10 +77,12 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
_last_check = t;
bool updated;
orb_check(_fd, &updated);
+
if (updated) {
orb_copy(_topic, _fd, _data);
return true;
}
}
+
return false;
}
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 79ff3abdb..3cf33ccef 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -1,8 +1,41 @@
-/*
- * mavlink_orb_subscription.h
+/****************************************************************************
*
- * Created on: 23.02.2014
- * Author: ton
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_orb_subscription.h
+ * uORB subscription definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
@@ -12,7 +45,8 @@
#include <drivers/drv_hrt.h>
-class MavlinkOrbSubscription {
+class MavlinkOrbSubscription
+{
public:
MavlinkOrbSubscription *next;
diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp
index f5bb06ccd..f6ed6e662 100644
--- a/src/modules/mavlink/mavlink_rate_limiter.cpp
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -1,10 +1,42 @@
-/*
- * mavlink_rate_limiter.cpp
+/****************************************************************************
*
- * Created on: 26.02.2014
- * Author: ton
- */
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file mavlink_rate_limiter.cpp
+ * Message rate limiter implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
#include "mavlink_rate_limiter.h"
@@ -30,9 +62,11 @@ 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
index 6db65f638..0b37538e6 100644
--- a/src/modules/mavlink/mavlink_rate_limiter.h
+++ b/src/modules/mavlink/mavlink_rate_limiter.h
@@ -1,8 +1,41 @@
-/*
- * mavlink_rate_limiter.h
+/****************************************************************************
*
- * Created on: 26.02.2014
- * Author: ton
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_rate_limiter.h
+ * Message rate limiter definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_RATE_LIMITER_H_
@@ -11,7 +44,8 @@
#include <drivers/drv_hrt.h>
-class MavlinkRateLimiter {
+class MavlinkRateLimiter
+{
private:
hrt_abstime _last_sent;
hrt_abstime _interval;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index a3546e954..f85773ae0 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -566,8 +566,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
/* go back to -PI..PI */
- if (heading_rad > M_PI_F)
+ if (heading_rad > M_PI_F) {
heading_rad -= 2.0f * M_PI_F;
+ }
hil_gps.timestamp_velocity = gps.time_usec;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
@@ -606,7 +607,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
- // publish global position
+ // publish global position
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
@@ -627,10 +628,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
if (pub_hil_local_pos > 0) {
float x;
float y;
- bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve?
- double lat = hil_state.lat*1e-7;
- double lon = hil_state.lon*1e-7;
- map_projection_project(lat, lon, &x, &y);
+ bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // XXX improve?
+ double lat = hil_state.lat * 1e-7;
+ double lon = hil_state.lon * 1e-7;
+ map_projection_project(lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
@@ -638,10 +639,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_local_pos.v_z_valid = true;
hil_local_pos.x = x;
hil_local_pos.y = y;
- hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
- hil_local_pos.vx = hil_state.vx/100.0f;
- hil_local_pos.vy = hil_state.vy/100.0f;
- hil_local_pos.vz = hil_state.vz/100.0f;
+ hil_local_pos.z = alt0 - hil_state.alt / 1000.0f;
+ hil_local_pos.vx = hil_state.vx / 100.0f;
+ hil_local_pos.vy = hil_state.vy / 100.0f;
+ hil_local_pos.vz = hil_state.vz / 100.0f;
hil_local_pos.yaw = hil_attitude.yaw;
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
@@ -651,6 +652,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_local_pos.ref_alt = alt0;
hil_local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
+
} else {
pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
lat0 = hil_state.lat;
@@ -661,12 +663,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
- math::Matrix<3,3> C_nb = q.to_dcm();
+ math::Matrix<3, 3> C_nb = q.to_dcm();
math::Vector<3> euler = C_nb.to_euler();
/* set rotation matrix */
- for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) {
hil_attitude.R[i][j] = C_nb(i, j);
+ }
hil_attitude.R_valid = true;
@@ -841,9 +844,9 @@ void MavlinkReceiver::print_status()
}
-void * MavlinkReceiver::start_helper(void *context)
+void *MavlinkReceiver::start_helper(void *context)
{
- MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context);
+ MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);
return rcv->receive_thread(NULL);
}
@@ -865,7 +868,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_t thread;
- pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent);
+ pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
pthread_attr_destroy(&receiveloop_attr);
return thread;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index fca5de917..199e42689 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -97,13 +97,13 @@ public:
static pthread_t receive_start(Mavlink *parent);
- static void * start_helper(void *context);
+ static void *start_helper(void *context);
private:
perf_counter_t _loop_perf; /**< loop performance counter */
- Mavlink* _mavlink;
+ Mavlink *_mavlink;
void handle_message(mavlink_message_t *msg);
void *receive_thread(void *arg);
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 703f74b4c..869495098 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -1,8 +1,41 @@
-/*
- * mavlink_stream.cpp
+/****************************************************************************
*
- * Created on: 24.02.2014
- * Author: ton
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_stream.cpp
+ * Mavlink messages stream implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdlib.h>
@@ -43,6 +76,7 @@ int
MavlinkStream::update(const hrt_abstime t)
{
uint64_t dt = t - _last_sent;
+
if (dt > 0 && dt >= _interval) {
/* interval expired, send message */
send(t);
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index 9f175adbe..135e1bce0 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -1,8 +1,41 @@
-/*
- * mavlink_stream.h
+/****************************************************************************
*
- * Created on: 24.02.2014
- * Author: ton
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_stream.cpp
+ * Mavlink messages stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_STREAM_H_
@@ -15,7 +48,8 @@ class MavlinkStream;
#include "mavlink_main.h"
-class MavlinkStream {
+class MavlinkStream
+{
private:
hrt_abstime _last_sent;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-06 21:59:18 +0000