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

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

@ -37,6 +37,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@ -91,7 +92,7 @@ static const int ERROR = -1;
#define MAX_DATA_RATE 10000 // max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 // 100 Hz
static Mavlink* _head = nullptr;
static Mavlink *_head = nullptr;
/* TODO: if this is a class member it crashes */
static struct file_operations fops;
@ -112,40 +113,47 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int uart = -1;
switch (channel) {
case MAVLINK_COMM_0:
uart = Mavlink::get_uart_fd(0);
case MAVLINK_COMM_0:
uart = Mavlink::get_uart_fd(0);
break;
case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1);
case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1);
break;
case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2);
case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2);
break;
case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3);
case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3);
break;
#ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4);
#ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4);
break;
#endif
#ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5);
#endif
#ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5);
break;
#endif
#ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6:
uart = Mavlink::get_uart_fd(6);
#endif
#ifdef MAVLINK_COMM_6
case MAVLINK_COMM_6:
uart = Mavlink::get_uart_fd(6);
break;
#endif
#endif
}
ssize_t desired = (sizeof(uint8_t) * length);
ssize_t ret = write(uart, ch, desired);
if (ret != desired)
if (ret != desired) {
warn("write err");
}
}
@ -168,7 +176,7 @@ Mavlink::Mavlink() :
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
{
wpm = &wpm_s;
fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
// _parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
}
@ -206,8 +214,9 @@ int
Mavlink::instance_count()
{
/* note: a local buffer count will help if this ever is called often */
Mavlink* inst = ::_head;
Mavlink *inst = ::_head;
unsigned inst_index = 0;
while (inst != nullptr) {
inst = inst->next;
inst_index++;
@ -225,14 +234,17 @@ Mavlink::new_instance()
/* create the first instance at _head */
if (::_head == nullptr) {
::_head = inst;
/* afterwards follow the next and append the instance */
/* afterwards follow the next and append the instance */
} else {
while (next->next != nullptr) {
next = next->next;
}
/* now parent has a null pointer, fill it */
next->next = inst;
}
return inst;
}
@ -241,6 +253,7 @@ Mavlink::get_instance(unsigned instance)
{
Mavlink *inst = ::_head;
unsigned inst_index = 0;
while (inst->next != nullptr && inst_index < instance) {
inst = inst->next;
inst_index++;
@ -277,6 +290,7 @@ Mavlink::destroy_all_instances()
unsigned iterations = 0;
warnx("waiting for instances to stop");
while (next_inst != nullptr) {
inst_to_del = next_inst;
@ -284,6 +298,7 @@ Mavlink::destroy_all_instances()
/* set flag to stop thread and wait for all threads to finish */
inst_to_del->_task_should_exit = true;
while (inst_to_del->thread_running) {
printf(".");
usleep(10000);
@ -294,6 +309,7 @@ Mavlink::destroy_all_instances()
return ERROR;
}
}
delete inst_to_del;
}
@ -308,23 +324,29 @@ Mavlink::destroy_all_instances()
bool
Mavlink::instance_exists(const char *device_name, Mavlink *self)
{
Mavlink* inst = ::_head;
Mavlink *inst = ::_head;
while (inst != nullptr) {
/* don't compare with itself */
if (inst != self && !strcmp(device_name, inst->device_name))
if (inst != self && !strcmp(device_name, inst->device_name)) {
return true;
}
inst = inst->next;
}
return false;
}
int
Mavlink::get_uart_fd(unsigned index)
{
Mavlink* inst = get_instance(index);
if (inst)
Mavlink *inst = get_instance(index);
if (inst) {
return inst->get_uart_fd();
}
return -1;
}
@ -353,21 +375,23 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
const char *txt = (const char *)arg;
const char *txt = (const char *)arg;
// printf("logmsg: %s\n", txt);
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
Mavlink* inst = ::_head;
while (inst != nullptr) {
Mavlink *inst = ::_head;
mavlink_logbuffer_write(&inst->lb, &msg);
inst->total_counter++;
inst = inst->next;
while (inst != nullptr) {
mavlink_logbuffer_write(&inst->lb, &msg);
inst->total_counter++;
inst = inst->next;
}
return OK;
}
return OK;
}
default:
return ENOTTY;
@ -582,7 +606,7 @@ int Mavlink::mavlink_pm_send_param_for_name(const char *name)
int Mavlink::mavlink_pm_send_param(param_t param)
{
if (param == PARAM_INVALID) return 1;
if (param == PARAM_INVALID) { return 1; }
/* buffers for param transmission */
static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
@ -714,38 +738,40 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
{
/* only support global waypoints for now */
switch (mavlink_mission_item->frame) {
case MAV_FRAME_GLOBAL:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = false;
break;
case MAV_FRAME_GLOBAL:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = false;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = true;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = true;
break;
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
default:
return MAV_MISSION_ERROR;
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
default:
return MAV_MISSION_ERROR;
}
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
break;
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
break;
}
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
@ -762,25 +788,27 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
{
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param2 = mission_item->pitch_min;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
break;
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param2 = mission_item->pitch_min;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
break;
}
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->param1 = mission_item->time_inside;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
@ -817,7 +845,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8
mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); }
}
/*
@ -846,7 +874,8 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
} else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
if (_verbose) warnx("ERROR: index out of bounds");
if (_verbose) { warnx("ERROR: index out of bounds"); }
}
}
@ -862,7 +891,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin
mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); }
}
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
@ -870,11 +899,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
struct mission_item_s mission_item;
ssize_t len = sizeof(struct mission_item_s);
dm_item_t dm_current;
if (wpm->current_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
@ -892,10 +922,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp);
mavlink_missionlib_send_message(&msg);
if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); }
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
if (_verbose) warnx("ERROR: could not read WP%u", seq);
if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
}
}
@ -910,11 +942,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity");
if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); }
}
}
@ -935,7 +968,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); }
}
void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
@ -945,7 +978,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
mavlink_missionlib_send_gcs_string("Operation timeout");
if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); }
wpm->current_state = MAVLINK_WPM_STATE_IDLE;
wpm->current_partner_sysid = 0;
@ -960,7 +993,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
switch (msg->msgid) {
case MAVLINK_MSG_ID_MISSION_ACK: {
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
@ -977,13 +1010,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch");
if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); }
}
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
@ -995,30 +1029,33 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
mission.current_index = wpc.seq;
publish_mission();
/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
// mavlink_wpm_send_waypoint_current(wpc.seq);
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
if (_verbose) warnx("IGN WP CURR CMD: Not in list");
if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); }
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
if (_verbose) warnx("IGN WP CURR CMD: Busy");
if (_verbose) { warnx("IGN WP CURR CMD: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) warnx("REJ. WP CMD: target id mismatch");
if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); }
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
@ -1027,14 +1064,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpm->size > 0) {
wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
wpm->current_wp_id = 0;
wpm->current_partner_sysid = msg->sysid;
wpm->current_partner_compid = msg->compid;
} else {
if (_verbose) warnx("No waypoints send");
if (_verbose) { warnx("No waypoints send"); }
}
wpm->current_count = wpm->size;
@ -1042,17 +1079,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
if (_verbose) warnx("IGN REQUEST LIST: Busy");
if (_verbose) { warnx("IGN REQUEST LIST: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch");
if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); }
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
@ -1062,22 +1102,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq >= wpm->size) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); }
break;
}
/*
* Ensure that we are in the correct state and that the first request has id 0
/*
* Ensure that we are in the correct state and that the first request has id 0
* and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
*/
if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) {
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
if (_verbose) warnx("REJ. WP CMD: First id != 0");
if (_verbose) { warnx("REJ. WP CMD: First id != 0"); }
break;
}
@ -1085,22 +1131,26 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq == wpm->current_wp_id) {
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else if (wpr.seq == wpm->current_wp_id + 1) {
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); }
break;
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); }
break;
}
@ -1109,11 +1159,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpr.seq < wpm->size) {
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id);
mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); }
}
@ -1122,18 +1173,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
@ -1143,18 +1197,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); }
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
break;
}
if (wpc.count == 0) {
mavlink_missionlib_send_gcs_string("COUNT 0");
if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); }
break;
}
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
wpm->current_wp_id = 0;
@ -1168,24 +1225,31 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); }
}
} else {
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy");
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); }
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
@ -1200,11 +1264,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
if (wp.seq != 0) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
break;
}
if (wp.seq != 0) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
break;
}
} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (wp.seq >= wpm->current_count) {
@ -1239,6 +1304,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (wpm->current_dataman_id == 0) {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
mission.dataman_id = 1;
} else {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.dataman_id = 0;
@ -1260,13 +1326,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
wpm->current_wp_id = wp.seq + 1;
if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); }
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
mission.count = wpm->current_count;
publish_mission();
wpm->current_dataman_id = mission.dataman_id;
@ -1280,13 +1346,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
@ -1305,21 +1372,24 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
} else {
mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
if (_verbose) warnx("IGN WP CLEAR CMD: Busy");
if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
}
} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); }
}
break;
@ -1352,8 +1422,9 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
while (i < len - 1) {
statustext.text[i] = string[i];
if (string[i] == '\0')
if (string[i] == '\0') {
break;
}
i++;
}
@ -1411,6 +1482,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
LL_DELETE(_streams, stream);
delete stream;
}
return OK;
}
}
@ -1466,18 +1538,22 @@ Mavlink::task_main(int argc, char *argv[])
switch (ch) {
case 'b':
_baudrate = strtoul(optarg, NULL, 10);
if (_baudrate < 9600 || _baudrate > 921600) {
warnx("invalid baud rate '%s'", optarg);
err_flag = true;
}
break;
case 'r':
_datarate = strtoul(optarg, NULL, 10);
if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
warnx("invalid data rate '%s'", optarg);
err_flag = true;
}
break;
case 'd':
@ -1501,6 +1577,7 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(optarg, "custom") == 0) {
_mode = MODE_CUSTOM;
}
break;
case 'v':
@ -1533,35 +1610,41 @@ Mavlink::task_main(int argc, char *argv[])
/* inform about mode */
switch (_mode) {
case MODE_CUSTOM:
warnx("mode: CUSTOM");
break;
case MODE_OFFBOARD:
warnx("mode: OFFBOARD");
break;
case MODE_ONBOARD:
warnx("mode: ONBOARD");
break;
case MODE_HIL:
warnx("mode: HIL");
break;
default:
warnx("ERROR: Unknown mode");
break;
case MODE_CUSTOM:
warnx("mode: CUSTOM");
break;
case MODE_OFFBOARD:
warnx("mode: OFFBOARD");
break;
case MODE_ONBOARD:
warnx("mode: ONBOARD");
break;
case MODE_HIL:
warnx("mode: HIL");
break;
default:
warnx("ERROR: Unknown mode");
break;
}
switch(_mode) {
case MODE_OFFBOARD:
case MODE_HIL:
case MODE_CUSTOM:
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
break;
case MODE_ONBOARD:
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
break;
default:
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
break;
switch (_mode) {
case MODE_OFFBOARD:
case MODE_HIL:
case MODE_CUSTOM:
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
break;
case MODE_ONBOARD:
_mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA;
break;
default:
_mavlink_wpm_comp_id = MAV_COMP_ID_ALL;
break;
}
warnx("data rate: %d bytes/s", _datarate);
@ -1576,8 +1659,9 @@ Mavlink::task_main(int argc, char *argv[])
/* default values for arguments */
_uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart);
if (_uart < 0)
if (_uart < 0) {
err(1, "could not open %s", device_name);
}
/* create the device node that's used for sending text log messages, etc. */
if (instance_count() == 1) {
@ -1615,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("HEARTBEAT", 1.0f);
switch(_mode) {
switch (_mode) {
case MODE_OFFBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
@ -1660,10 +1744,12 @@ Mavlink::task_main(int argc, char *argv[])
if (status_sub->update(t)) {
/* switch HIL mode if required */
if (status->hil_state == HIL_STATE_ON)
if (status->hil_state == HIL_STATE_ON) {
set_hil_enabled(true);
else if (status->hil_state == HIL_STATE_OFF)
} else if (status->hil_state == HIL_STATE_OFF) {
set_hil_enabled(false);
}
}
MavlinkStream *stream;
@ -1677,13 +1763,14 @@ Mavlink::task_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission);
if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); }
if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
}
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
} else {
if (slow_rate_limiter.check(t)) {
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
@ -1740,11 +1827,11 @@ Mavlink::start(int argc, char *argv[])
sprintf(buf, "mavlink_if%d", Mavlink::instance_count());
/*mavlink->_mavlink_task = */task_spawn_cmd(buf,
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&Mavlink::start_helper,
(const char **)argv);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2048,
(main_t)&Mavlink::start_helper,
(const char **)argv);
// while (!this->is_running()) {
// usleep(200);
@ -1775,7 +1862,7 @@ Mavlink::start(int argc, char *argv[])
}
void
Mavlink::status()
Mavlink::status()
{
warnx("running");
}
@ -1799,9 +1886,11 @@ Mavlink::stream(int argc, char *argv[])
switch (ch) {
case 'r':
rate = strtod(optarg, nullptr);
if (rate < 0.0f) {
err_flag = true;
}
break;
case 'd':
@ -1820,6 +1909,7 @@ Mavlink::stream(int argc, char *argv[])
if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
Mavlink *inst = get_instance_for_device(device_name);
if (inst != nullptr) {
if (OK == inst->configure_stream(stream_name, rate)) {
if (rate > 0.0f) {
@ -1865,15 +1955,15 @@ int mavlink_main(int argc, char *argv[])
} else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances();
// } else if (!strcmp(argv[1], "status")) {
// mavlink::g_mavlink->status();
// } else if (!strcmp(argv[1], "status")) {
// mavlink::g_mavlink->status();
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream(argc, argv);
} else {
usage();
}
} else {
usage();
}
return 0;
}

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

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

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

@ -1,8 +1,42 @@
/*
* mavlink_messages.cpp
/****************************************************************************
*
* Created on: 25.02.2014
* Author: ton
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_messages.cpp
* MAVLink 1.0 message formatters implementation.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdio.h>
@ -43,7 +77,7 @@
static uint16_t cm_uint16_from_m_float(float m);
static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
uint16_t
cm_uint16_from_m_float(float m)
@ -59,7 +93,7 @@ cm_uint16_from_m_float(float m)
}
void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
*mavlink_state = 0;
*mavlink_base_mode = 0;
@ -72,7 +106,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
/* arming state */
if (status->arming_state == ARMING_STATE_ARMED
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
@ -81,34 +115,44 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
/* use main state when navigator is not active */
/* use main state when navigator is not active */
if (status->main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (status->main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
} else if (status->main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
} else if (status->main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
}
} else {
/* use navigation state when navigator is active */
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
@ -118,24 +162,30 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
/* set system state */
if (status->arming_state == ARMING_STATE_INIT
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
|| status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;
} else if (status->arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL;
} else if (status->arming_state == ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
} else if (status->arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
} else {
*mavlink_state = MAV_STATE_CRITICAL;
}
}
class MavlinkStreamHeartbeat : public MavlinkStream {
class MavlinkStreamHeartbeat : public MavlinkStream
{
public:
const char *get_name()
{
@ -164,7 +214,8 @@ protected:
pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
status_sub->update(t);
pos_sp_triplet_sub->update(t);
@ -184,7 +235,8 @@ protected:
};
class MavlinkStreamSysStatus : public MavlinkStream {
class MavlinkStreamSysStatus : public MavlinkStream
{
public:
const char *get_name()
{
@ -207,28 +259,30 @@ protected:
status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
status_sub->update(t);
mavlink_msg_sys_status_send(_channel,
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
status->onboard_control_sensors_present,
status->onboard_control_sensors_enabled,
status->onboard_control_sensors_health,
status->load * 1000.0f,
status->battery_voltage * 1000.0f,
status->battery_current * 1000.0f,
status->battery_remaining,
status->drop_rate_comm,
status->errors_comm,
status->errors_count1,
status->errors_count2,
status->errors_count3,
status->errors_count4);
}
};
class MavlinkStreamHighresIMU : public MavlinkStream {
class MavlinkStreamHighresIMU : public MavlinkStream
{
public:
MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0)
{
@ -260,7 +314,8 @@ protected:
sensor = (struct sensor_combined_s *)sensor_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
sensor_sub->update(t);
uint16_t fields_updated = 0;
@ -290,18 +345,19 @@ protected:
}
mavlink_msg_highres_imu_send(_channel,
sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated);
sensor->timestamp,
sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
sensor->baro_pres_mbar, sensor->differential_pressure_pa,
sensor->baro_alt_meter, sensor->baro_temp_celcius,
fields_updated);
}
};
class MavlinkStreamAttitude : public MavlinkStream {
class MavlinkStreamAttitude : public MavlinkStream
{
public:
const char *get_name()
{
@ -324,18 +380,20 @@ protected:
att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
att_sub->update(t);
mavlink_msg_attitude_send(_channel,
att->timestamp / 1000,
att->roll, att->pitch, att->yaw,
att->rollspeed, att->pitchspeed, att->yawspeed);
att->timestamp / 1000,
att->roll, att->pitch, att->yaw,
att->rollspeed, att->pitchspeed, att->yawspeed);
}
};
class MavlinkStreamAttitudeQuaternion : public MavlinkStream {
class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public:
const char *get_name()
{
@ -358,23 +416,25 @@ protected:
att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
att_sub->update(t);
mavlink_msg_attitude_quaternion_send(_channel,
att->timestamp / 1000,
att->q[0],
att->q[1],
att->q[2],
att->q[3],
att->rollspeed,
att->pitchspeed,
att->yawspeed);
att->timestamp / 1000,
att->q[0],
att->q[1],
att->q[2],
att->q[3],
att->rollspeed,
att->pitchspeed,
att->yawspeed);
}
};
class MavlinkStreamVFRHUD : public MavlinkStream {
class MavlinkStreamVFRHUD : public MavlinkStream
{
public:
const char *get_name()
{
@ -421,7 +481,8 @@ protected:
airspeed = (struct airspeed_s *)airspeed_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
att_sub->update(t);
pos_sub->update(t);
armed_sub->update(t);
@ -433,17 +494,18 @@ protected:
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
airspeed->true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos->alt,
-pos->vel_d);
airspeed->true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos->alt,
-pos->vel_d);
}
};
class MavlinkStreamGPSRawInt : public MavlinkStream {
class MavlinkStreamGPSRawInt : public MavlinkStream
{
public:
const char *get_name()
{
@ -466,25 +528,27 @@ protected:
gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
gps_sub->update(t);
mavlink_msg_gps_raw_int_send(_channel,
gps->timestamp_position,
gps->fix_type,
gps->lat,
gps->lon,
gps->alt,
cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible);
gps->timestamp_position,
gps->fix_type,
gps->lat,
gps->lon,
gps->alt,
cm_uint16_from_m_float(gps->eph_m),
cm_uint16_from_m_float(gps->epv_m),
gps->vel_m_s * 100.0f,
_wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps->satellites_visible);
}
};
class MavlinkStreamGlobalPositionInt : public MavlinkStream {
class MavlinkStreamGlobalPositionInt : public MavlinkStream
{
public:
const char *get_name()
{
@ -513,25 +577,27 @@ protected:
home = (struct home_position_s *)home_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
pos_sub->update(t);
home_sub->update(t);
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
};
class MavlinkStreamLocalPositionNED : public MavlinkStream {
class MavlinkStreamLocalPositionNED : public MavlinkStream
{
public:
const char *get_name()
{
@ -554,22 +620,24 @@ protected:
pos = (struct vehicle_local_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
pos_sub->update(t);
mavlink_msg_local_position_ned_send(_channel,
pos->timestamp / 1000,
pos->x,
pos->y,
pos->z,
pos->vx,
pos->vy,
pos->vz);
pos->timestamp / 1000,
pos->x,
pos->y,
pos->z,
pos->vx,
pos->vy,
pos->vz);
}
};
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream {
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public:
const char *get_name()
{
@ -592,18 +660,20 @@ protected:
home = (struct home_position_s *)home_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
home_sub->update(t);
mavlink_msg_gps_global_origin_send(_channel,
(int32_t)(home->lat * 1e7),
(int32_t)(home->lon * 1e7),
(int32_t)(home->alt) * 1000.0f);
(int32_t)(home->lat * 1e7),
(int32_t)(home->lon * 1e7),
(int32_t)(home->alt) * 1000.0f);
}
};
class MavlinkStreamServoOutputRaw : public MavlinkStream {
class MavlinkStreamServoOutputRaw : public MavlinkStream
{
public:
MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
{
@ -641,25 +711,27 @@ protected:
act = (struct actuator_outputs_s *)act_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
act_sub->update(t);
mavlink_msg_servo_output_raw_send(_channel,
act->timestamp / 1000,
_n,
act->output[0],
act->output[1],
act->output[2],
act->output[3],
act->output[4],
act->output[5],
act->output[6],
act->output[7]);
act->timestamp / 1000,
_n,
act->output[0],
act->output[1],
act->output[2],
act->output[3],
act->output[4],
act->output[5],
act->output[6],
act->output[7]);
}
};
class MavlinkStreamHILControls : public MavlinkStream {
class MavlinkStreamHILControls : public MavlinkStream
{
public:
const char *get_name()
{
@ -694,7 +766,8 @@ protected:
act = (struct actuator_outputs_s *)act_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
status_sub->update(t);
pos_sp_triplet_sub->update(t);
act_sub->update(t);
@ -710,65 +783,66 @@ protected:
/* scale / assign outputs depending on system type */
if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
-1,
-1,
mavlink_base_mode,
0);
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
-1,
-1,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_base_mode,
0);
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
((act->output[6] - 900.0f) / 600.0f) / 2.0f,
((act->output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_base_mode,
0);
hrt_absolute_time(),
((act->output[0] - 900.0f) / 600.0f) / 2.0f,
((act->output[1] - 900.0f) / 600.0f) / 2.0f,
((act->output[2] - 900.0f) / 600.0f) / 2.0f,
((act->output[3] - 900.0f) / 600.0f) / 2.0f,
((act->output[4] - 900.0f) / 600.0f) / 2.0f,
((act->output[5] - 900.0f) / 600.0f) / 2.0f,
((act->output[6] - 900.0f) / 600.0f) / 2.0f,
((act->output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_base_mode,
0);
} else {
mavlink_msg_hil_controls_send(_channel,
hrt_absolute_time(),
(act->output[0] - 1500.0f) / 500.0f,
(act->output[1] - 1500.0f) / 500.0f,
(act->output[2] - 1500.0f) / 500.0f,
(act->output[3] - 1000.0f) / 1000.0f,
(act->output[4] - 1500.0f) / 500.0f,
(act->output[5] - 1500.0f) / 500.0f,
(act->output[6] - 1500.0f) / 500.0f,
(act->output[7] - 1500.0f) / 500.0f,
mavlink_base_mode,
0);
hrt_absolute_time(),
(act->output[0] - 1500.0f) / 500.0f,
(act->output[1] - 1500.0f) / 500.0f,
(act->output[2] - 1500.0f) / 500.0f,
(act->output[3] - 1000.0f) / 1000.0f,
(act->output[4] - 1500.0f) / 500.0f,
(act->output[5] - 1500.0f) / 500.0f,
(act->output[6] - 1500.0f) / 500.0f,
(act->output[7] - 1500.0f) / 500.0f,
mavlink_base_mode,
0);
}
}
};
class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream {
class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
{
public:
const char *get_name()
{
@ -791,7 +865,8 @@ protected:
pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
pos_sp_triplet_sub->update(t);
mavlink_msg_global_position_setpoint_int_send(_channel,
@ -804,7 +879,8 @@ protected:
};
class MavlinkStreamLocalPositionSetpoint : public MavlinkStream {
class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
{
public:
const char *get_name()
{
@ -827,7 +903,8 @@ protected:
pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
pos_sp_sub->update(t);
mavlink_msg_local_position_setpoint_send(_channel,
@ -840,7 +917,8 @@ protected:
};
class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream {
class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
{
public:
const char *get_name()
{
@ -863,7 +941,8 @@ protected:
att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
att_sp_sub->update(t);
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
@ -876,7 +955,8 @@ protected:
};
class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream {
class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
{
public:
const char *get_name()
{
@ -899,7 +979,8 @@ protected:
att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
att_rates_sp_sub->update(t);
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
@ -912,7 +993,8 @@ protected:
};
class MavlinkStreamRCChannelsRaw : public MavlinkStream {
class MavlinkStreamRCChannelsRaw : public MavlinkStream
{
public:
const char *get_name()
{
@ -935,7 +1017,8 @@ protected:
rc = (struct rc_input_values *)rc_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
rc_sub->update(t);
const unsigned port_width = 8;
@ -943,23 +1026,24 @@ protected:
for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel,
rc->timestamp_publication / 1000,
i,
(rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
rc->rssi);
rc->timestamp_publication / 1000,
i,
(rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
(rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
rc->rssi);
}
}
};
class MavlinkStreamManualControl : public MavlinkStream {
class MavlinkStreamManualControl : public MavlinkStream
{
public:
const char *get_name()
{
@ -982,43 +1066,44 @@ protected:
manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
}
void send(const hrt_abstime t) {
void send(const hrt_abstime t)
{
manual_sub->update(t);
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
manual->roll * 1000,
manual->pitch * 1000,
manual->yaw * 1000,
manual->throttle * 1000,
0);
mavlink_system.sysid,
manual->roll * 1000,
manual->pitch * 1000,
manual->yaw * 1000,
manual->throttle * 1000,
0);
}
};
MavlinkStream *streams_list[] = {
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
new MavlinkStreamHighresIMU(),
new MavlinkStreamAttitude(),
new MavlinkStreamAttitudeQuaternion(),
new MavlinkStreamVFRHUD(),
new MavlinkStreamGPSRawInt(),
new MavlinkStreamGlobalPositionInt(),
new MavlinkStreamLocalPositionNED(),
new MavlinkStreamGPSGlobalOrigin(),
new MavlinkStreamServoOutputRaw(0),
new MavlinkStreamServoOutputRaw(1),
new MavlinkStreamServoOutputRaw(2),
new MavlinkStreamServoOutputRaw(3),
new MavlinkStreamHILControls(),
new MavlinkStreamGlobalPositionSetpointInt(),
new MavlinkStreamLocalPositionSetpoint(),
new MavlinkStreamRollPitchYawThrustSetpoint(),
new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
new MavlinkStreamRCChannelsRaw(),
new MavlinkStreamManualControl(),
nullptr
new MavlinkStreamHeartbeat(),
new MavlinkStreamSysStatus(),
new MavlinkStreamHighresIMU(),
new MavlinkStreamAttitude(),
new MavlinkStreamAttitudeQuaternion(),
new MavlinkStreamVFRHUD(),
new MavlinkStreamGPSRawInt(),
new MavlinkStreamGlobalPositionInt(),
new MavlinkStreamLocalPositionNED(),
new MavlinkStreamGPSGlobalOrigin(),
new MavlinkStreamServoOutputRaw(0),
new MavlinkStreamServoOutputRaw(1),
new MavlinkStreamServoOutputRaw(2),
new MavlinkStreamServoOutputRaw(3),
new MavlinkStreamHILControls(),
new MavlinkStreamGlobalPositionSetpointInt(),
new MavlinkStreamLocalPositionSetpoint(),
new MavlinkStreamRollPitchYawThrustSetpoint(),
new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
new MavlinkStreamRCChannelsRaw(),
new MavlinkStreamManualControl(),
nullptr
};

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

@ -1,8 +1,41 @@
/*
* mavlink_messages.h
/****************************************************************************
*
* Created on: 25.02.2014
* Author: ton
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_messages.h
* MAVLink 1.0 message formatters definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_MESSAGES_H_

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

@ -1,10 +1,42 @@
/*
* mavlink_orb_subscription.cpp
/****************************************************************************
*
* Created on: 23.02.2014
* Author: ton
*/
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_orb_subscription.cpp
* uORB subscription implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <unistd.h>
#include <stdlib.h>
@ -45,10 +77,12 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
_last_check = t;
bool updated;
orb_check(_fd, &updated);
if (updated) {
orb_copy(_topic, _fd, _data);
return true;
}
}
return false;
}

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

@ -1,8 +1,41 @@
/*
* mavlink_orb_subscription.h
/****************************************************************************
*
* Created on: 23.02.2014
* Author: ton
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_orb_subscription.h
* uORB subscription definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
@ -12,7 +45,8 @@
#include <drivers/drv_hrt.h>
class MavlinkOrbSubscription {
class MavlinkOrbSubscription
{
public:
MavlinkOrbSubscription *next;

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

@ -1,10 +1,42 @@
/*
* mavlink_rate_limiter.cpp
/****************************************************************************
*
* Created on: 26.02.2014
* Author: ton
*/
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_rate_limiter.cpp
* Message rate limiter implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "mavlink_rate_limiter.h"
@ -30,9 +62,11 @@ bool
MavlinkRateLimiter::check(hrt_abstime t)
{
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
_last_sent = (t / _interval) * _interval;
return true;
}
return false;
}

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

@ -1,8 +1,41 @@
/*
* mavlink_rate_limiter.h
/****************************************************************************
*
* Created on: 26.02.2014
* Author: ton
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_rate_limiter.h
* Message rate limiter definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_RATE_LIMITER_H_
@ -11,7 +44,8 @@
#include <drivers/drv_hrt.h>
class MavlinkRateLimiter {
class MavlinkRateLimiter
{
private:
hrt_abstime _last_sent;
hrt_abstime _interval;

33
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;
/* go back to -PI..PI */
if (heading_rad > M_PI_F)
if (heading_rad > M_PI_F) {
heading_rad -= 2.0f * M_PI_F;
}
hil_gps.timestamp_velocity = gps.time_usec;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
@ -606,7 +607,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
// publish global position
// publish global position
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
@ -627,10 +628,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
if (pub_hil_local_pos > 0) {
float x;
float y;
bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve?
double lat = hil_state.lat*1e-7;
double lon = hil_state.lon*1e-7;
map_projection_project(lat, lon, &x, &y);
bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // XXX improve?
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
map_projection_project(lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
@ -638,10 +639,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_local_pos.v_z_valid = true;
hil_local_pos.x = x;
hil_local_pos.y = y;
hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
hil_local_pos.vx = hil_state.vx/100.0f;
hil_local_pos.vy = hil_state.vy/100.0f;
hil_local_pos.vz = hil_state.vz/100.0f;
hil_local_pos.z = alt0 - hil_state.alt / 1000.0f;
hil_local_pos.vx = hil_state.vx / 100.0f;
hil_local_pos.vy = hil_state.vy / 100.0f;
hil_local_pos.vz = hil_state.vz / 100.0f;
hil_local_pos.yaw = hil_attitude.yaw;
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
@ -651,6 +652,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
hil_local_pos.ref_alt = alt0;
hil_local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
} else {
pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
lat0 = hil_state.lat;
@ -661,12 +663,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Matrix<3,3> C_nb = q.to_dcm();
math::Matrix<3, 3> C_nb = q.to_dcm();
math::Vector<3> euler = C_nb.to_euler();
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) {
hil_attitude.R[i][j] = C_nb(i, j);
}
hil_attitude.R_valid = true;
@ -841,9 +844,9 @@ void MavlinkReceiver::print_status()
}
void * MavlinkReceiver::start_helper(void *context)
void *MavlinkReceiver::start_helper(void *context)
{
MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context);
MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);
return rcv->receive_thread(NULL);
}
@ -865,7 +868,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
pthread_attr_setstacksize(&receiveloop_attr, 3000);
pthread_t thread;
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent);
pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
pthread_attr_destroy(&receiveloop_attr);
return thread;

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

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

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

@ -1,8 +1,41 @@
/*
* mavlink_stream.cpp
/****************************************************************************
*
* Created on: 24.02.2014
* Author: ton
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_stream.cpp
* Mavlink messages stream implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <stdlib.h>
@ -43,6 +76,7 @@ int
MavlinkStream::update(const hrt_abstime t)
{
uint64_t dt = t - _last_sent;
if (dt > 0 && dt >= _interval) {
/* interval expired, send message */
send(t);

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

@ -1,8 +1,41 @@
/*
* mavlink_stream.h
/****************************************************************************
*
* Created on: 24.02.2014
* Author: ton
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mavlink_stream.cpp
* Mavlink messages stream definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef MAVLINK_STREAM_H_
@ -15,7 +48,8 @@ class MavlinkStream;
#include "mavlink_main.h"
class MavlinkStream {
class MavlinkStream
{
private:
hrt_abstime _last_sent;