dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: code style and copyright fixes

This commit is contained in:
Anton Babushkin 2014-03-01 00:16:51 +04:00
parent 77d1989aba
commit 836f7c435f
12 changed files with 867 additions and 451 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

424
src/modules/mavlink/mavlink_main.cpp
View File

@ -37,6 +37,7 @@
* *
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#include <nuttx/config.h> #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 MAX_DATA_RATE 10000 // max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 // 100 Hz #define MAIN_LOOP_DELAY 10000 // 100 Hz
static Mavlink* _head = nullptr; static Mavlink *_head = nullptr;
/* TODO: if this is a class member it crashes */ /* TODO: if this is a class member it crashes */
static struct file_operations fops; 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; int uart = -1;
switch (channel) { switch (channel) {
case MAVLINK_COMM_0: case MAVLINK_COMM_0:
uart = Mavlink::get_uart_fd(0); uart = Mavlink::get_uart_fd(0);
break; break;
case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1); case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1);
break; break;
case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2); case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2);
break; break;
case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3); case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3);
break; break;
#ifdef MAVLINK_COMM_4 #ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4); case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4);
break; break;
#endif #endif
#ifdef MAVLINK_COMM_5 #ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5); case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5);
break; break;
#endif #endif
#ifdef MAVLINK_COMM_6 #ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6:
uart = Mavlink::get_uart_fd(6); case MAVLINK_COMM_6:
uart = Mavlink::get_uart_fd(6);
break; break;
#endif #endif
} }
ssize_t desired = (sizeof(uint8_t) * length); ssize_t desired = (sizeof(uint8_t) * length);
ssize_t ret = write(uart, ch, desired); ssize_t ret = write(uart, ch, desired);
if (ret != desired) if (ret != desired) {
warn("write err"); warn("write err");
}
} }
@ -168,7 +176,7 @@ Mavlink::Mavlink() :
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) _loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{ {
wpm = &wpm_s; 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"); // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
} }
@ -206,8 +214,9 @@ int
Mavlink::instance_count() Mavlink::instance_count()
{ {
/* note: a local buffer count will help if this ever is called often */ /* note: a local buffer count will help if this ever is called often */
Mavlink* inst = ::_head; Mavlink *inst = ::_head;
unsigned inst_index = 0; unsigned inst_index = 0;
while (inst != nullptr) { while (inst != nullptr) {
inst = inst->next; inst = inst->next;
inst_index++; inst_index++;
@ -225,14 +234,17 @@ Mavlink::new_instance()
/* create the first instance at _head */ /* create the first instance at _head */
if (::_head == nullptr) { if (::_head == nullptr) {
::_head = inst; ::_head = inst;
/* afterwards follow the next and append the instance */ /* afterwards follow the next and append the instance */
} else { } else {
while (next->next != nullptr) { while (next->next != nullptr) {
next = next->next; next = next->next;
} }
/* now parent has a null pointer, fill it */ /* now parent has a null pointer, fill it */
next->next = inst; next->next = inst;
} }
return inst; return inst;
} }
@ -241,6 +253,7 @@ Mavlink::get_instance(unsigned instance)
{ {
Mavlink *inst = ::_head; Mavlink *inst = ::_head;
unsigned inst_index = 0; unsigned inst_index = 0;
while (inst->next != nullptr && inst_index < instance) { while (inst->next != nullptr && inst_index < instance) {
inst = inst->next; inst = inst->next;
inst_index++; inst_index++;
@ -277,6 +290,7 @@ Mavlink::destroy_all_instances()
unsigned iterations = 0; unsigned iterations = 0;
warnx("waiting for instances to stop"); warnx("waiting for instances to stop");
while (next_inst != nullptr) { while (next_inst != nullptr) {
inst_to_del = next_inst; 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 */ /* set flag to stop thread and wait for all threads to finish */
inst_to_del->_task_should_exit = true; inst_to_del->_task_should_exit = true;
while (inst_to_del->thread_running) { while (inst_to_del->thread_running) {
printf("."); printf(".");
usleep(10000); usleep(10000);
@ -294,6 +309,7 @@ Mavlink::destroy_all_instances()
return ERROR; return ERROR;
} }
} }
delete inst_to_del; delete inst_to_del;
} }
@ -308,23 +324,29 @@ Mavlink::destroy_all_instances()
bool bool
Mavlink::instance_exists(const char *device_name, Mavlink *self) Mavlink::instance_exists(const char *device_name, Mavlink *self)
{ {
Mavlink* inst = ::_head; Mavlink *inst = ::_head;
while (inst != nullptr) { while (inst != nullptr) {
/* don't compare with itself */ /* 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; return true;
}
inst = inst->next; inst = inst->next;
} }
return false; return false;
} }
int int
Mavlink::get_uart_fd(unsigned index) Mavlink::get_uart_fd(unsigned index)
{ {
Mavlink* inst = get_instance(index); Mavlink *inst = get_instance(index);
if (inst)
if (inst) {
return inst->get_uart_fd(); return inst->get_uart_fd();
}
return -1; 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_CRITICAL:
case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
const char *txt = (const char *)arg; const char *txt = (const char *)arg;
// printf("logmsg: %s\n", txt); // printf("logmsg: %s\n", txt);
struct mavlink_logmessage msg; struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text)); strncpy(msg.text, txt, sizeof(msg.text));
Mavlink* inst = ::_head; Mavlink *inst = ::_head;
while (inst != nullptr) {
mavlink_logbuffer_write(&inst->lb, &msg); while (inst != nullptr) {
inst->total_counter++;
inst = inst->next;
mavlink_logbuffer_write(&inst->lb, &msg);
inst->total_counter++;
inst = inst->next;
}
return OK;
} }
return OK;
}
default: default:
return ENOTTY; 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) 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 */ /* buffers for param transmission */
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; 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 */ /* only support global waypoints for now */
switch (mavlink_mission_item->frame) { switch (mavlink_mission_item->frame) {
case MAV_FRAME_GLOBAL: case MAV_FRAME_GLOBAL:
mission_item->lat = (double)mavlink_mission_item->x; mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y; mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z; mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = false; mission_item->altitude_is_relative = false;
break; break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT: case MAV_FRAME_GLOBAL_RELATIVE_ALT:
mission_item->lat = (double)mavlink_mission_item->x; mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y; mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z; mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = true; mission_item->altitude_is_relative = true;
break; break;
case MAV_FRAME_LOCAL_NED: case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_ENU: case MAV_FRAME_LOCAL_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME; return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
default: case MAV_FRAME_MISSION:
return MAV_MISSION_ERROR; default:
return MAV_MISSION_ERROR;
} }
switch (mavlink_mission_item->command) { switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param2; mission_item->pitch_min = mavlink_mission_item->param2;
break; break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2; default:
break; 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_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->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; 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) { if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL; mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
} else { } else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
} }
switch (mission_item->nav_cmd) { switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF: case NAV_CMD_TAKEOFF:
mavlink_mission_item->param2 = mission_item->pitch_min; mavlink_mission_item->param2 = mission_item->pitch_min;
break; break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius; default:
break; mavlink_mission_item->param2 = mission_item->acceptance_radius;
break;
} }
mavlink_mission_item->x = (float)mission_item->lat; mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon; mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude; mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; 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->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->autocontinue = mission_item->autocontinue; 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_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
mavlink_missionlib_send_message(&msg); 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 { } else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); 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_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg); 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) void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
@ -875,6 +904,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
if (wpm->current_dataman_id == 0) { if (wpm->current_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else { } else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; 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_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
mavlink_missionlib_send_message(&msg); 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 { } else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); 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_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
mavlink_missionlib_send_message(&msg); 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 { } else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); 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_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg); 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) 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"); 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_state = MAVLINK_WPM_STATE_IDLE;
wpm->current_partner_sysid = 0; wpm->current_partner_sysid = 0;
@ -960,7 +993,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_MISSION_ACK: { case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_mission_ack_t wpa; mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa); mavlink_msg_mission_ack_decode(msg, &wpa);
@ -977,13 +1010,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); 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; break;
} }
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
mavlink_mission_set_current_t wpc; mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc); mavlink_msg_mission_set_current_decode(msg, &wpc);
@ -1001,24 +1035,27 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else { } else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); 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 { } else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); 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 { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); 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; break;
} }
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
mavlink_mission_request_list_t wprl; mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl); mavlink_msg_mission_request_list_decode(msg, &wprl);
@ -1034,7 +1071,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
wpm->current_partner_compid = msg->compid; wpm->current_partner_compid = msg->compid;
} else { } else {
if (_verbose) warnx("No waypoints send"); if (_verbose) { warnx("No waypoints send"); }
} }
wpm->current_count = wpm->size; wpm->current_count = wpm->size;
@ -1042,17 +1079,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else { } else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
if (_verbose) warnx("IGN REQUEST LIST: Busy");
if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
} }
} else { } else {
mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); 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; break;
} }
case MAVLINK_MSG_ID_MISSION_REQUEST: { case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t wpr; mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr); mavlink_msg_mission_request_decode(msg, &wpr);
@ -1062,7 +1102,9 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq >= wpm->size) { if (wpr.seq >= wpm->size) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); 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; break;
} }
@ -1073,11 +1115,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) { 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; wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); 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; break;
} }
@ -1085,22 +1131,26 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq == wpm->current_wp_id) { 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) { } 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 { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); 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; break;
} }
} else { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); 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; break;
} }
@ -1109,11 +1159,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq < wpm->size) { 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 { } else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); 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)) { 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"); 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 { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); 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; break;
} }
case MAVLINK_MSG_ID_MISSION_COUNT: { case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t wpc; mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &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 (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.count > NUM_MISSIONS_SUPPORTED) { 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); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
break; break;
} }
if (wpc.count == 0) { if (wpc.count == 0) {
mavlink_missionlib_send_gcs_string("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; 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_state = MAVLINK_WPM_STATE_GETLIST;
wpm->current_wp_id = 0; 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) { if (wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); 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 { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); 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 { } else {
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy");
if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
} }
} else { } else {
mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); 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; break;
case MAVLINK_MSG_ID_MISSION_ITEM: { case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t wp; mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &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 (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
if (wp.seq != 0) { if (wp.seq != 0) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 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); warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
break; break;
} }
} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (wp.seq >= wpm->current_count) { 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) { if (wpm->current_dataman_id == 0) {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
mission.dataman_id = 1; mission.dataman_id = 1;
} else { } else {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.dataman_id = 0; mission.dataman_id = 0;
@ -1261,7 +1327,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { 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); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
@ -1280,13 +1346,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else { } else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); 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; break;
} }
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
mavlink_mission_clear_all_t wpca; mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca); mavlink_msg_mission_clear_all_decode(msg, &wpca);
@ -1305,6 +1372,7 @@ 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) { 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); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
} else { } else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
} }
@ -1312,14 +1380,16 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else { } else {
mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); 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) { } 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"); 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; break;
@ -1352,8 +1422,9 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
while (i < len - 1) { while (i < len - 1) {
statustext.text[i] = string[i]; statustext.text[i] = string[i];
if (string[i] == '\0') if (string[i] == '\0') {
break; break;
}
i++; i++;
} }
@ -1411,6 +1482,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
LL_DELETE(_streams, stream); LL_DELETE(_streams, stream);
delete stream; delete stream;
} }
return OK; return OK;
} }
} }
@ -1466,18 +1538,22 @@ Mavlink::task_main(int argc, char *argv[])
switch (ch) { switch (ch) {
case 'b': case 'b':
_baudrate = strtoul(optarg, NULL, 10); _baudrate = strtoul(optarg, NULL, 10);
if (_baudrate < 9600 || _baudrate > 921600) { if (_baudrate < 9600 || _baudrate > 921600) {
warnx("invalid baud rate '%s'", optarg); warnx("invalid baud rate '%s'", optarg);
err_flag = true; err_flag = true;
} }
break; break;
case 'r': case 'r':
_datarate = strtoul(optarg, NULL, 10); _datarate = strtoul(optarg, NULL, 10);
if (_datarate < 10 || _datarate > MAX_DATA_RATE) { if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
warnx("invalid data rate '%s'", optarg); warnx("invalid data rate '%s'", optarg);
err_flag = true; err_flag = true;
} }
break; break;
case 'd': case 'd':
@ -1501,6 +1577,7 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(optarg, "custom") == 0) { } else if (strcmp(optarg, "custom") == 0) {
_mode = MODE_CUSTOM; _mode = MODE_CUSTOM;
} }
break; break;
case 'v': case 'v':
@ -1533,35 +1610,41 @@ Mavlink::task_main(int argc, char *argv[])
/* inform about mode */ /* inform about mode */
switch (_mode) { switch (_mode) {
case MODE_CUSTOM: case MODE_CUSTOM:
warnx("mode: CUSTOM"); warnx("mode: CUSTOM");
break; break;
case MODE_OFFBOARD:
warnx("mode: OFFBOARD"); case MODE_OFFBOARD:
break; warnx("mode: OFFBOARD");
case MODE_ONBOARD: break;
warnx("mode: ONBOARD");
break; case MODE_ONBOARD:
case MODE_HIL: warnx("mode: ONBOARD");
warnx("mode: HIL"); break;
break;
default: case MODE_HIL:
warnx("ERROR: Unknown mode"); warnx("mode: HIL");
break; break;
default:
warnx("ERROR: Unknown mode");
break;
} }
switch(_mode) { switch (_mode) {
case MODE_OFFBOARD: case MODE_OFFBOARD:
case MODE_HIL: case MODE_HIL:
case MODE_CUSTOM: case MODE_CUSTOM:
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
break; break;
case MODE_ONBOARD:
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; case MODE_ONBOARD:
break; _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
default: break;
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
break; default:
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
break;
} }
warnx("data rate: %d bytes/s", _datarate); warnx("data rate: %d bytes/s", _datarate);
@ -1576,8 +1659,9 @@ Mavlink::task_main(int argc, char *argv[])
/* default values for arguments */ /* default values for arguments */
_uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); _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); err(1, "could not open %s", device_name);
}
/* create the device node that's used for sending text log messages, etc. */ /* create the device node that's used for sending text log messages, etc. */
if (instance_count() == 1) { if (instance_count() == 1) {
@ -1615,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("HEARTBEAT", 1.0f); configure_stream("HEARTBEAT", 1.0f);
switch(_mode) { switch (_mode) {
case MODE_OFFBOARD: case MODE_OFFBOARD:
configure_stream("SYS_STATUS", 1.0f); configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); 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)) { if (status_sub->update(t)) {
/* switch HIL mode if required */ /* switch HIL mode if required */
if (status->hil_state == HIL_STATE_ON) if (status->hil_state == HIL_STATE_ON) {
set_hil_enabled(true); set_hil_enabled(true);
else if (status->hil_state == HIL_STATE_OFF)
} else if (status->hil_state == HIL_STATE_OFF) {
set_hil_enabled(false); set_hil_enabled(false);
}
} }
MavlinkStream *stream; MavlinkStream *stream;
@ -1677,13 +1763,14 @@ Mavlink::task_main(int argc, char *argv[])
if (updated) { if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); 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) { if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_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); mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
} else { } else {
if (slow_rate_limiter.check(t)) { if (slow_rate_limiter.check(t)) {
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); 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()); sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
/*mavlink->_mavlink_task = */task_spawn_cmd(buf, /*mavlink->_mavlink_task = */task_spawn_cmd(buf,
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
2048, 2048,
(main_t)&Mavlink::start_helper, (main_t)&Mavlink::start_helper,
(const char **)argv); (const char **)argv);
// while (!this->is_running()) { // while (!this->is_running()) {
// usleep(200); // usleep(200);
@ -1799,9 +1886,11 @@ Mavlink::stream(int argc, char *argv[])
switch (ch) { switch (ch) {
case 'r': case 'r':
rate = strtod(optarg, nullptr); rate = strtod(optarg, nullptr);
if (rate < 0.0f) { if (rate < 0.0f) {
err_flag = true; err_flag = true;
} }
break; break;
case 'd': case 'd':
@ -1820,6 +1909,7 @@ Mavlink::stream(int argc, char *argv[])
if (!err_flag && rate >= 0.0 && stream_name != nullptr) { if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
Mavlink *inst = get_instance_for_device(device_name); Mavlink *inst = get_instance_for_device(device_name);
if (inst != nullptr) { if (inst != nullptr) {
if (OK == inst->configure_stream(stream_name, rate)) { if (OK == inst->configure_stream(stream_name, rate)) {
if (rate > 0.0f) { if (rate > 0.0f) {
@ -1865,15 +1955,15 @@ int mavlink_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "stop-all")) { } else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances(); return Mavlink::destroy_all_instances();
// } else if (!strcmp(argv[1], "status")) { // } else if (!strcmp(argv[1], "status")) {
// mavlink::g_mavlink->status(); // mavlink::g_mavlink->status();
} else if (!strcmp(argv[1], "stream")) { } else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream(argc, argv); return Mavlink::stream(argc, argv);
} else { } else {
usage(); usage();
} }
return 0; return 0;
} }

33
src/modules/mavlink/mavlink_main.h
View File

@ -36,6 +36,7 @@
* MAVLink 1.0 protocol interface definition. * MAVLink 1.0 protocol interface definition.
* *
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#pragma once #pragma once
@ -55,24 +56,24 @@
#include "mavlink_stream.h" #include "mavlink_stream.h"
#include "mavlink_messages.h" #include "mavlink_messages.h"
// FIXME XXX - TO BE MOVED TO XML // FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES { enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0, MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST, MAVLINK_WPM_STATE_SENDLIST,
MAVLINK_WPM_STATE_SENDLIST_SENDWPS, MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
MAVLINK_WPM_STATE_GETLIST, MAVLINK_WPM_STATE_GETLIST,
MAVLINK_WPM_STATE_GETLIST_GETWPS, MAVLINK_WPM_STATE_GETLIST_GETWPS,
MAVLINK_WPM_STATE_GETLIST_GOTALL, MAVLINK_WPM_STATE_GETLIST_GOTALL,
MAVLINK_WPM_STATE_ENUM_END MAVLINK_WPM_STATE_ENUM_END
}; };
enum MAVLINK_WPM_CODES { enum MAVLINK_WPM_CODES {
MAVLINK_WPM_CODE_OK = 0, MAVLINK_WPM_CODE_OK = 0,
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
MAVLINK_WPM_CODE_ENUM_END MAVLINK_WPM_CODE_ENUM_END
}; };
@ -144,7 +145,7 @@ public:
const char *device_name; const char *device_name;
enum MAVLINK_MODE { enum MAVLINK_MODE {
MODE_CUSTOM=0, MODE_CUSTOM = 0,
MODE_OFFBOARD, MODE_OFFBOARD,
MODE_ONBOARD, MODE_ONBOARD,
MODE_HIL MODE_HIL
@ -186,7 +187,7 @@ public:
bool _task_should_exit; /**< if true, mavlink task should exit */ bool _task_should_exit; /**< if true, mavlink task should exit */
protected: protected:
Mavlink* next; Mavlink *next;
private: private:
int _mavlink_fd; int _mavlink_fd;

495
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1,8 +1,42 @@
/* /****************************************************************************
* mavlink_messages.cpp
* *
* Created on: 25.02.2014 * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: ton *
* 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> #include <stdio.h>
@ -43,7 +77,7 @@
static uint16_t cm_uint16_from_m_float(float m); 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, 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 uint16_t
cm_uint16_from_m_float(float m) 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, 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_state = 0;
*mavlink_base_mode = 0; *mavlink_base_mode = 0;
@ -72,7 +106,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
/* arming state */ /* arming state */
if (status->arming_state == ARMING_STATE_ARMED 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; *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; union px4_custom_mode custom_mode;
custom_mode.data = 0; custom_mode.data = 0;
if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { 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) { 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); *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; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (status->main_state == MAIN_STATE_SEATBELT) { } else if (status->main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (status->main_state == MAIN_STATE_EASY) { } 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; *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; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} else if (status->main_state == MAIN_STATE_AUTO) { } 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; *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.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} }
} else { } else {
/* use navigation state when navigator is active */ /* 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; *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.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
if (pos_sp_triplet->nav_state == NAV_STATE_READY) { if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_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 */ /* set system state */
if (status->arming_state == ARMING_STATE_INIT if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE || status->arming_state == ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT; *mavlink_state = MAV_STATE_UNINIT;
} else if (status->arming_state == ARMING_STATE_ARMED) { } else if (status->arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE; *mavlink_state = MAV_STATE_ACTIVE;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL; *mavlink_state = MAV_STATE_CRITICAL;
} else if (status->arming_state == ARMING_STATE_STANDBY) { } else if (status->arming_state == ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY; *mavlink_state = MAV_STATE_STANDBY;
} else if (status->arming_state == ARMING_STATE_REBOOT) { } else if (status->arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF; *mavlink_state = MAV_STATE_POWEROFF;
} else { } else {
*mavlink_state = MAV_STATE_CRITICAL; *mavlink_state = MAV_STATE_CRITICAL;
} }
} }
class MavlinkStreamHeartbeat : public MavlinkStream { class MavlinkStreamHeartbeat : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -164,7 +214,8 @@ protected:
pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); 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); status_sub->update(t);
pos_sp_triplet_sub->update(t); pos_sp_triplet_sub->update(t);
@ -184,7 +235,8 @@ protected:
}; };
class MavlinkStreamSysStatus : public MavlinkStream { class MavlinkStreamSysStatus : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -207,28 +259,30 @@ protected:
status = (struct vehicle_status_s *)status_sub->get_data(); 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); status_sub->update(t);
mavlink_msg_sys_status_send(_channel, mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present, status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled, status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health, status->onboard_control_sensors_health,
status->load * 1000.0f, status->load * 1000.0f,
status->battery_voltage * 1000.0f, status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f, status->battery_current * 1000.0f,
status->battery_remaining, status->battery_remaining,
status->drop_rate_comm, status->drop_rate_comm,
status->errors_comm, status->errors_comm,
status->errors_count1, status->errors_count1,
status->errors_count2, status->errors_count2,
status->errors_count3, status->errors_count3,
status->errors_count4); status->errors_count4);
} }
}; };
class MavlinkStreamHighresIMU : public MavlinkStream { class MavlinkStreamHighresIMU : public MavlinkStream
{
public: public:
MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) 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(); 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); sensor_sub->update(t);
uint16_t fields_updated = 0; uint16_t fields_updated = 0;
@ -290,18 +345,19 @@ protected:
} }
mavlink_msg_highres_imu_send(_channel, mavlink_msg_highres_imu_send(_channel,
sensor->timestamp, sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], 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->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->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa, sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius, sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated); fields_updated);
} }
}; };
class MavlinkStreamAttitude : public MavlinkStream { class MavlinkStreamAttitude : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -324,18 +380,20 @@ protected:
att = (struct vehicle_attitude_s *)att_sub->get_data(); 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); att_sub->update(t);
mavlink_msg_attitude_send(_channel, mavlink_msg_attitude_send(_channel,
att->timestamp / 1000, att->timestamp / 1000,
att->roll, att->pitch, att->yaw, att->roll, att->pitch, att->yaw,
att->rollspeed, att->pitchspeed, att->yawspeed); att->rollspeed, att->pitchspeed, att->yawspeed);
} }
}; };
class MavlinkStreamAttitudeQuaternion : public MavlinkStream { class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -358,23 +416,25 @@ protected:
att = (struct vehicle_attitude_s *)att_sub->get_data(); 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); att_sub->update(t);
mavlink_msg_attitude_quaternion_send(_channel, mavlink_msg_attitude_quaternion_send(_channel,
att->timestamp / 1000, att->timestamp / 1000,
att->q[0], att->q[0],
att->q[1], att->q[1],
att->q[2], att->q[2],
att->q[3], att->q[3],
att->rollspeed, att->rollspeed,
att->pitchspeed, att->pitchspeed,
att->yawspeed); att->yawspeed);
} }
}; };
class MavlinkStreamVFRHUD : public MavlinkStream { class MavlinkStreamVFRHUD : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -421,7 +481,8 @@ protected:
airspeed = (struct airspeed_s *)airspeed_sub->get_data(); airspeed = (struct airspeed_s *)airspeed_sub->get_data();
} }
void send(const hrt_abstime t) { void send(const hrt_abstime t)
{
att_sub->update(t); att_sub->update(t);
pos_sub->update(t); pos_sub->update(t);
armed_sub->update(t); armed_sub->update(t);
@ -433,17 +494,18 @@ protected:
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel, mavlink_msg_vfr_hud_send(_channel,
airspeed->true_airspeed_m_s, airspeed->true_airspeed_m_s,
groundspeed, groundspeed,
heading, heading,
throttle, throttle,
pos->alt, pos->alt,
-pos->vel_d); -pos->vel_d);
} }
}; };
class MavlinkStreamGPSRawInt : public MavlinkStream { class MavlinkStreamGPSRawInt : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -466,25 +528,27 @@ protected:
gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); 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); gps_sub->update(t);
mavlink_msg_gps_raw_int_send(_channel, mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position, gps->timestamp_position,
gps->fix_type, gps->fix_type,
gps->lat, gps->lat,
gps->lon, gps->lon,
gps->alt, gps->alt,
cm_uint16_from_m_float(gps->eph_m), cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m), cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f, gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible); gps->satellites_visible);
} }
}; };
class MavlinkStreamGlobalPositionInt : public MavlinkStream { class MavlinkStreamGlobalPositionInt : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -513,25 +577,27 @@ protected:
home = (struct home_position_s *)home_sub->get_data(); 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); pos_sub->update(t);
home_sub->update(t); home_sub->update(t);
mavlink_msg_global_position_int_send(_channel, mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000, pos->timestamp / 1000,
pos->lat * 1e7, pos->lat * 1e7,
pos->lon * 1e7, pos->lon * 1e7,
pos->alt * 1000.0f, pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f, (pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f, pos->vel_n * 100.0f,
pos->vel_e * 100.0f, pos->vel_e * 100.0f,
pos->vel_d * 100.0f, pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
} }
}; };
class MavlinkStreamLocalPositionNED : public MavlinkStream { class MavlinkStreamLocalPositionNED : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -554,22 +620,24 @@ protected:
pos = (struct vehicle_local_position_s *)pos_sub->get_data(); 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); pos_sub->update(t);
mavlink_msg_local_position_ned_send(_channel, mavlink_msg_local_position_ned_send(_channel,
pos->timestamp / 1000, pos->timestamp / 1000,
pos->x, pos->x,
pos->y, pos->y,
pos->z, pos->z,
pos->vx, pos->vx,
pos->vy, pos->vy,
pos->vz); pos->vz);
} }
}; };
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -592,18 +660,20 @@ protected:
home = (struct home_position_s *)home_sub->get_data(); 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); home_sub->update(t);
mavlink_msg_gps_global_origin_send(_channel, mavlink_msg_gps_global_origin_send(_channel,
(int32_t)(home->lat * 1e7), (int32_t)(home->lat * 1e7),
(int32_t)(home->lon * 1e7), (int32_t)(home->lon * 1e7),
(int32_t)(home->alt) * 1000.0f); (int32_t)(home->alt) * 1000.0f);
} }
}; };
class MavlinkStreamServoOutputRaw : public MavlinkStream { class MavlinkStreamServoOutputRaw : public MavlinkStream
{
public: public:
MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
{ {
@ -641,25 +711,27 @@ protected:
act = (struct actuator_outputs_s *)act_sub->get_data(); 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); act_sub->update(t);
mavlink_msg_servo_output_raw_send(_channel, mavlink_msg_servo_output_raw_send(_channel,
act->timestamp / 1000, act->timestamp / 1000,
_n, _n,
act->output[0], act->output[0],
act->output[1], act->output[1],
act->output[2], act->output[2],
act->output[3], act->output[3],
act->output[4], act->output[4],
act->output[5], act->output[5],
act->output[6], act->output[6],
act->output[7]); act->output[7]);
} }
}; };
class MavlinkStreamHILControls : public MavlinkStream { class MavlinkStreamHILControls : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -694,7 +766,8 @@ protected:
act = (struct actuator_outputs_s *)act_sub->get_data(); 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); status_sub->update(t);
pos_sp_triplet_sub->update(t); pos_sp_triplet_sub->update(t);
act_sub->update(t); act_sub->update(t);
@ -710,65 +783,66 @@ protected:
/* scale / assign outputs depending on system type */ /* scale / assign outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR) { if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
mavlink_msg_hil_controls_send(_channel, mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(), hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f, ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 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[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f, ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
-1, -1,
-1, -1,
-1, -1,
-1, -1,
mavlink_base_mode, mavlink_base_mode,
0); 0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
mavlink_msg_hil_controls_send(_channel, mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(), hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f, ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 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[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 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[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 900.0f) / 600.0f) / 2.0f, ((act->output[5] - 900.0f) / 600.0f) / 2.0f,
-1, -1,
-1, -1,
mavlink_base_mode, mavlink_base_mode,
0); 0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
mavlink_msg_hil_controls_send(_channel, mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(), hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f, ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 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[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 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[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 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[6] - 900.0f) / 600.0f) / 2.0f,
((act->output[7] - 900.0f) / 600.0f) / 2.0f, ((act->output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_base_mode, mavlink_base_mode,
0); 0);
} else { } else {
mavlink_msg_hil_controls_send(_channel, mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(), hrt_absolute_time(),
(act->output[0] - 1500.0f) / 500.0f, (act->output[0] - 1500.0f) / 500.0f,
(act->output[1] - 1500.0f) / 500.0f, (act->output[1] - 1500.0f) / 500.0f,
(act->output[2] - 1500.0f) / 500.0f, (act->output[2] - 1500.0f) / 500.0f,
(act->output[3] - 1000.0f) / 1000.0f, (act->output[3] - 1000.0f) / 1000.0f,
(act->output[4] - 1500.0f) / 500.0f, (act->output[4] - 1500.0f) / 500.0f,
(act->output[5] - 1500.0f) / 500.0f, (act->output[5] - 1500.0f) / 500.0f,
(act->output[6] - 1500.0f) / 500.0f, (act->output[6] - 1500.0f) / 500.0f,
(act->output[7] - 1500.0f) / 500.0f, (act->output[7] - 1500.0f) / 500.0f,
mavlink_base_mode, mavlink_base_mode,
0); 0);
} }
} }
}; };
class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream { class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -791,7 +865,8 @@ protected:
pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); 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); pos_sp_triplet_sub->update(t);
mavlink_msg_global_position_setpoint_int_send(_channel, mavlink_msg_global_position_setpoint_int_send(_channel,
@ -804,7 +879,8 @@ protected:
}; };
class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -827,7 +903,8 @@ protected:
pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); 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); pos_sp_sub->update(t);
mavlink_msg_local_position_setpoint_send(_channel, mavlink_msg_local_position_setpoint_send(_channel,
@ -840,7 +917,8 @@ protected:
}; };
class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream { class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -863,7 +941,8 @@ protected:
att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); 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); att_sp_sub->update(t);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
@ -876,7 +955,8 @@ protected:
}; };
class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream { class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -899,7 +979,8 @@ protected:
att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); 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); att_rates_sp_sub->update(t);
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
@ -912,7 +993,8 @@ protected:
}; };
class MavlinkStreamRCChannelsRaw : public MavlinkStream { class MavlinkStreamRCChannelsRaw : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -935,7 +1017,8 @@ protected:
rc = (struct rc_input_values *)rc_sub->get_data(); 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); rc_sub->update(t);
const unsigned port_width = 8; const unsigned port_width = 8;
@ -943,23 +1026,24 @@ protected:
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */ /* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel, mavlink_msg_rc_channels_raw_send(_channel,
rc->timestamp_publication / 1000, rc->timestamp_publication / 1000,
i, i,
(rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, (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) + 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) + 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) + 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) + 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) + 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) + 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->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
rc->rssi); rc->rssi);
} }
} }
}; };
class MavlinkStreamManualControl : public MavlinkStream { class MavlinkStreamManualControl : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -982,43 +1066,44 @@ protected:
manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); 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); manual_sub->update(t);
mavlink_msg_manual_control_send(_channel, mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid, mavlink_system.sysid,
manual->roll * 1000, manual->roll * 1000,
manual->pitch * 1000, manual->pitch * 1000,
manual->yaw * 1000, manual->yaw * 1000,
manual->throttle * 1000, manual->throttle * 1000,
0); 0);
} }
}; };
MavlinkStream *streams_list[] = { MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(), new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(), new MavlinkStreamSysStatus(),
new MavlinkStreamHighresIMU(), new MavlinkStreamHighresIMU(),
new MavlinkStreamAttitude(), new MavlinkStreamAttitude(),
new MavlinkStreamAttitudeQuaternion(), new MavlinkStreamAttitudeQuaternion(),
new MavlinkStreamVFRHUD(), new MavlinkStreamVFRHUD(),
new MavlinkStreamGPSRawInt(), new MavlinkStreamGPSRawInt(),
new MavlinkStreamGlobalPositionInt(), new MavlinkStreamGlobalPositionInt(),
new MavlinkStreamLocalPositionNED(), new MavlinkStreamLocalPositionNED(),
new MavlinkStreamGPSGlobalOrigin(), new MavlinkStreamGPSGlobalOrigin(),
new MavlinkStreamServoOutputRaw(0), new MavlinkStreamServoOutputRaw(0),
new MavlinkStreamServoOutputRaw(1), new MavlinkStreamServoOutputRaw(1),
new MavlinkStreamServoOutputRaw(2), new MavlinkStreamServoOutputRaw(2),
new MavlinkStreamServoOutputRaw(3), new MavlinkStreamServoOutputRaw(3),
new MavlinkStreamHILControls(), new MavlinkStreamHILControls(),
new MavlinkStreamGlobalPositionSetpointInt(), new MavlinkStreamGlobalPositionSetpointInt(),
new MavlinkStreamLocalPositionSetpoint(), new MavlinkStreamLocalPositionSetpoint(),
new MavlinkStreamRollPitchYawThrustSetpoint(), new MavlinkStreamRollPitchYawThrustSetpoint(),
new MavlinkStreamRollPitchYawRatesThrustSetpoint(), new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
new MavlinkStreamRCChannelsRaw(), new MavlinkStreamRCChannelsRaw(),
new MavlinkStreamManualControl(), new MavlinkStreamManualControl(),
nullptr nullptr
}; };

41
src/modules/mavlink/mavlink_messages.h
View File

@ -1,8 +1,41 @@
/* /****************************************************************************
* mavlink_messages.h
* *
* Created on: 25.02.2014 * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: ton *
* 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_ #ifndef MAVLINK_MESSAGES_H_

44
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -1,10 +1,42 @@
/* /****************************************************************************
* mavlink_orb_subscription.cpp
* *
* Created on: 23.02.2014 * Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Author: ton *
*/ * 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 <unistd.h>
#include <stdlib.h> #include <stdlib.h>
@ -45,10 +77,12 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
_last_check = t; _last_check = t;
bool updated; bool updated;
orb_check(_fd, &updated); orb_check(_fd, &updated);
if (updated) { if (updated) {
orb_copy(_topic, _fd, _data); orb_copy(_topic, _fd, _data);
return true; return true;
} }
} }
return false; return false;
} }

44
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -1,8 +1,41 @@
/* /****************************************************************************
* mavlink_orb_subscription.h
* *
* Created on: 23.02.2014 * Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Author: ton *
* 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_ #ifndef MAVLINK_ORB_SUBSCRIPTION_H_
@ -12,7 +45,8 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
class MavlinkOrbSubscription { class MavlinkOrbSubscription
{
public: public:
MavlinkOrbSubscription *next; MavlinkOrbSubscription *next;

44
src/modules/mavlink/mavlink_rate_limiter.cpp
View File

@ -1,10 +1,42 @@
/* /****************************************************************************
* mavlink_rate_limiter.cpp
* *
* Created on: 26.02.2014 * Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Author: ton *
*/ * 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" #include "mavlink_rate_limiter.h"
@ -30,9 +62,11 @@ bool
MavlinkRateLimiter::check(hrt_abstime t) MavlinkRateLimiter::check(hrt_abstime t)
{ {
uint64_t dt = t - _last_sent; uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) { if (dt > 0 && dt >= _interval) {
_last_sent = (t / _interval) * _interval; _last_sent = (t / _interval) * _interval;
return true; return true;
} }
return false; return false;
} }

44
src/modules/mavlink/mavlink_rate_limiter.h
View File

@ -1,8 +1,41 @@
/* /****************************************************************************
* mavlink_rate_limiter.h
* *
* Created on: 26.02.2014 * Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Author: ton *
* 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_ #ifndef MAVLINK_RATE_LIMITER_H_
@ -11,7 +44,8 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
class MavlinkRateLimiter { class MavlinkRateLimiter
{
private: private:
hrt_abstime _last_sent; hrt_abstime _last_sent;
hrt_abstime _interval; hrt_abstime _interval;

29
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -566,8 +566,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
/* go back to -PI..PI */ /* go back to -PI..PI */
if (heading_rad > M_PI_F) if (heading_rad > M_PI_F) {
heading_rad -= 2.0f * M_PI_F; heading_rad -= 2.0f * M_PI_F;
}
hil_gps.timestamp_velocity = gps.time_usec; hil_gps.timestamp_velocity = gps.time_usec;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
@ -627,9 +628,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
if (pub_hil_local_pos > 0) { if (pub_hil_local_pos > 0) {
float x; float x;
float y; float y;
bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve? bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // XXX improve?
double lat = hil_state.lat*1e-7; double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon*1e-7; double lon = hil_state.lon * 1e-7;
map_projection_project(lat, lon, &x, &y); map_projection_project(lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp; hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true; hil_local_pos.xy_valid = true;
@ -638,10 +639,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_local_pos.v_z_valid = true; hil_local_pos.v_z_valid = true;
hil_local_pos.x = x; hil_local_pos.x = x;
hil_local_pos.y = y; hil_local_pos.y = y;
hil_local_pos.z = alt0 - hil_state.alt/1000.0f; hil_local_pos.z = alt0 - hil_state.alt / 1000.0f;
hil_local_pos.vx = hil_state.vx/100.0f; hil_local_pos.vx = hil_state.vx / 100.0f;
hil_local_pos.vy = hil_state.vy/100.0f; hil_local_pos.vy = hil_state.vy / 100.0f;
hil_local_pos.vz = hil_state.vz/100.0f; hil_local_pos.vz = hil_state.vz / 100.0f;
hil_local_pos.yaw = hil_attitude.yaw; hil_local_pos.yaw = hil_attitude.yaw;
hil_local_pos.xy_global = true; hil_local_pos.xy_global = true;
hil_local_pos.z_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.ref_alt = alt0;
hil_local_pos.landed = landed; hil_local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
} else { } else {
pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
lat0 = hil_state.lat; lat0 = hil_state.lat;
@ -661,12 +663,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
/* Calculate Rotation Matrix */ /* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion); 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(); math::Vector<3> euler = C_nb.to_euler();
/* set rotation matrix */ /* 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[i][j] = C_nb(i, j);
}
hil_attitude.R_valid = true; 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); return rcv->receive_thread(NULL);
} }
@ -865,7 +868,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_t thread; 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); pthread_attr_destroy(&receiveloop_attr);
return thread; return thread;

4
src/modules/mavlink/mavlink_receiver.h
View File

@ -97,13 +97,13 @@ public:
static pthread_t receive_start(Mavlink *parent); static pthread_t receive_start(Mavlink *parent);
static void * start_helper(void *context); static void *start_helper(void *context);
private: private:
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
Mavlink* _mavlink; Mavlink *_mavlink;
void handle_message(mavlink_message_t *msg); void handle_message(mavlink_message_t *msg);
void *receive_thread(void *arg); void *receive_thread(void *arg);

42
src/modules/mavlink/mavlink_stream.cpp
View File

@ -1,8 +1,41 @@
/* /****************************************************************************
* mavlink_stream.cpp
* *
* Created on: 24.02.2014 * Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Author: ton *
* 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> #include <stdlib.h>
@ -43,6 +76,7 @@ int
MavlinkStream::update(const hrt_abstime t) MavlinkStream::update(const hrt_abstime t)
{ {
uint64_t dt = t - _last_sent; uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) { if (dt > 0 && dt >= _interval) {
/* interval expired, send message */ /* interval expired, send message */
send(t); send(t);

44
src/modules/mavlink/mavlink_stream.h
View File

@ -1,8 +1,41 @@
/* /****************************************************************************
* mavlink_stream.h
* *
* Created on: 24.02.2014 * Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Author: ton *
* 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_ #ifndef MAVLINK_STREAM_H_
@ -15,7 +48,8 @@ class MavlinkStream;
#include "mavlink_main.h" #include "mavlink_main.h"
class MavlinkStream { class MavlinkStream
{
private: private:
hrt_abstime _last_sent; hrt_abstime _last_sent;