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

218
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;
@ -115,37 +116,44 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
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: case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1); uart = Mavlink::get_uart_fd(1);
break; break;
case MAVLINK_COMM_2: case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2); uart = Mavlink::get_uart_fd(2);
break; break;
case MAVLINK_COMM_3: case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3); uart = Mavlink::get_uart_fd(3);
break; break;
#ifdef MAVLINK_COMM_4 #ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4: case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4); uart = Mavlink::get_uart_fd(4);
break; break;
#endif #endif
#ifdef MAVLINK_COMM_5 #ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5: case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5); uart = Mavlink::get_uart_fd(5);
break; break;
#endif #endif
#ifdef MAVLINK_COMM_6 #ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6: case MAVLINK_COMM_6:
uart = Mavlink::get_uart_fd(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++;
@ -226,13 +235,16 @@ Mavlink::new_instance()
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;
} }
@ -358,7 +380,8 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
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) { while (inst != nullptr) {
mavlink_logbuffer_write(&inst->lb, &msg); mavlink_logbuffer_write(&inst->lb, &msg);
@ -366,6 +389,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
inst = inst->next; inst = inst->next;
} }
return OK; return OK;
} }
@ -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];
@ -731,6 +755,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
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: case MAV_FRAME_MISSION:
default: default:
return MAV_MISSION_ERROR; return MAV_MISSION_ERROR;
@ -740,12 +765,13 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
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: default:
mission_item->acceptance_radius = mavlink_mission_item->param2; mission_item->acceptance_radius = mavlink_mission_item->param2;
break; 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,6 +788,7 @@ 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;
} }
@ -770,6 +797,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
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: default:
mavlink_mission_item->param2 = mission_item->acceptance_radius; mavlink_mission_item->param2 = mission_item->acceptance_radius;
break; break;
@ -779,8 +807,8 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
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;
@ -977,7 +1010,8 @@ 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;
@ -1001,18 +1035,21 @@ 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;
@ -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,11 +1079,14 @@ 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;
@ -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,14 +1173,17 @@ 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;
} }
@ -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,19 +1225,26 @@ 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;
@ -1205,6 +1269,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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,7 +1346,8 @@ 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;
@ -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':
@ -1536,29 +1613,35 @@ Mavlink::task_main(int argc, char *argv[])
case MODE_CUSTOM: case MODE_CUSTOM:
warnx("mode: CUSTOM"); warnx("mode: CUSTOM");
break; break;
case MODE_OFFBOARD: case MODE_OFFBOARD:
warnx("mode: OFFBOARD"); warnx("mode: OFFBOARD");
break; break;
case MODE_ONBOARD: case MODE_ONBOARD:
warnx("mode: ONBOARD"); warnx("mode: ONBOARD");
break; break;
case MODE_HIL: case MODE_HIL:
warnx("mode: HIL"); warnx("mode: HIL");
break; break;
default: default:
warnx("ERROR: Unknown mode"); warnx("ERROR: Unknown mode");
break; 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: case MODE_ONBOARD:
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
break; break;
default: default:
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL; _mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
break; break;
@ -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,11 +1744,13 @@ 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;
LL_FOREACH(_streams, stream) { LL_FOREACH(_streams, 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);
@ -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) {

7
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,7 +56,7 @@
#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,
@ -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;

165
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>
@ -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;
} }
@ -121,21 +165,27 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
|| 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,7 +259,8 @@ 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,
@ -228,7 +281,8 @@ protected:
}; };
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;
@ -301,7 +356,8 @@ protected:
}; };
class MavlinkStreamAttitude : public MavlinkStream { class MavlinkStreamAttitude : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -324,7 +380,8 @@ 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,
@ -335,7 +392,8 @@ protected:
}; };
class MavlinkStreamAttitudeQuaternion : public MavlinkStream { class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -358,7 +416,8 @@ 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,
@ -374,7 +433,8 @@ protected:
}; };
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);
@ -443,7 +504,8 @@ protected:
}; };
class MavlinkStreamGPSRawInt : public MavlinkStream { class MavlinkStreamGPSRawInt : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -466,7 +528,8 @@ 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,
@ -484,7 +547,8 @@ protected:
}; };
class MavlinkStreamGlobalPositionInt : public MavlinkStream { class MavlinkStreamGlobalPositionInt : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -513,7 +577,8 @@ 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);
@ -531,7 +596,8 @@ protected:
}; };
class MavlinkStreamLocalPositionNED : public MavlinkStream { class MavlinkStreamLocalPositionNED : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -554,7 +620,8 @@ 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,
@ -569,7 +636,8 @@ protected:
}; };
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -592,7 +660,8 @@ 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,
@ -603,7 +672,8 @@ protected:
}; };
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,7 +711,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)
{
act_sub->update(t); act_sub->update(t);
mavlink_msg_servo_output_raw_send(_channel, mavlink_msg_servo_output_raw_send(_channel,
@ -659,7 +730,8 @@ protected:
}; };
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);
@ -768,7 +841,8 @@ protected:
}; };
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;
@ -959,7 +1042,8 @@ protected:
}; };
class MavlinkStreamManualControl : public MavlinkStream { class MavlinkStreamManualControl : public MavlinkStream
{
public: public:
const char *get_name() const char *get_name()
{ {
@ -982,7 +1066,8 @@ 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,

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;