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 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;
@ -115,37 +116,44 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
case MAVLINK_COMM_0:
uart = Mavlink::get_uart_fd(0);
break;
case MAVLINK_COMM_1:
uart = Mavlink::get_uart_fd(1);
break;
case MAVLINK_COMM_2:
uart = Mavlink::get_uart_fd(2);
break;
case MAVLINK_COMM_3:
uart = Mavlink::get_uart_fd(3);
break;
#ifdef MAVLINK_COMM_4
#ifdef MAVLINK_COMM_4
case MAVLINK_COMM_4:
uart = Mavlink::get_uart_fd(4);
break;
#endif
#ifdef MAVLINK_COMM_5
#endif
#ifdef MAVLINK_COMM_5
case MAVLINK_COMM_5:
uart = Mavlink::get_uart_fd(5);
break;
#endif
#ifdef MAVLINK_COMM_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++;
@ -226,13 +235,16 @@ Mavlink::new_instance()
if (::_head == nullptr) {
::_head = inst;
/* 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;
}
@ -358,7 +380,8 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
Mavlink* inst = ::_head;
Mavlink *inst = ::_head;
while (inst != nullptr) {
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;
}
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)
{
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];
@ -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_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
default:
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:
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,6 +788,7 @@ 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;
}
@ -770,6 +797,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param2 = mission_item->pitch_min;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
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->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)
@ -875,6 +904,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
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;
@ -977,7 +1010,8 @@ 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;
@ -1001,18 +1035,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} 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;
@ -1034,7 +1071,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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,11 +1079,14 @@ 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;
@ -1062,7 +1102,9 @@ 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;
}
@ -1073,11 +1115,15 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
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,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)) {
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;
}
@ -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,19 +1225,26 @@ 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");
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;
@ -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);
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;
@ -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 (_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);
@ -1280,7 +1346,8 @@ 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;
@ -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) {
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);
}
@ -1312,14 +1380,16 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
} 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':
@ -1536,29 +1613,35 @@ Mavlink::task_main(int argc, char *argv[])
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) {
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;
@ -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,11 +1744,13 @@ 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;
LL_FOREACH(_streams, 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);
@ -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) {

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

165
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>
@ -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 */
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;
}
@ -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_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,7 +259,8 @@ 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,
@ -228,7 +281,8 @@ protected:
};
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;
@ -301,7 +356,8 @@ protected:
};
class MavlinkStreamAttitude : public MavlinkStream {
class MavlinkStreamAttitude : public MavlinkStream
{
public:
const char *get_name()
{
@ -324,7 +380,8 @@ 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,
@ -335,7 +392,8 @@ protected:
};
class MavlinkStreamAttitudeQuaternion : public MavlinkStream {
class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public:
const char *get_name()
{
@ -358,7 +416,8 @@ 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,
@ -374,7 +433,8 @@ protected:
};
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);
@ -443,7 +504,8 @@ protected:
};
class MavlinkStreamGPSRawInt : public MavlinkStream {
class MavlinkStreamGPSRawInt : public MavlinkStream
{
public:
const char *get_name()
{
@ -466,7 +528,8 @@ 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,
@ -484,7 +547,8 @@ protected:
};
class MavlinkStreamGlobalPositionInt : public MavlinkStream {
class MavlinkStreamGlobalPositionInt : public MavlinkStream
{
public:
const char *get_name()
{
@ -513,7 +577,8 @@ 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);
@ -531,7 +596,8 @@ protected:
};
class MavlinkStreamLocalPositionNED : public MavlinkStream {
class MavlinkStreamLocalPositionNED : public MavlinkStream
{
public:
const char *get_name()
{
@ -554,7 +620,8 @@ 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,
@ -569,7 +636,8 @@ protected:
};
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream {
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public:
const char *get_name()
{
@ -592,7 +660,8 @@ 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,
@ -603,7 +672,8 @@ protected:
};
class MavlinkStreamServoOutputRaw : public MavlinkStream {
class MavlinkStreamServoOutputRaw : public MavlinkStream
{
public:
MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
{
@ -641,7 +711,8 @@ 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,
@ -659,7 +730,8 @@ protected:
};
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);
@ -768,7 +841,8 @@ protected:
};
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;
@ -959,7 +1042,8 @@ protected:
};
class MavlinkStreamManualControl : public MavlinkStream {
class MavlinkStreamManualControl : public MavlinkStream
{
public:
const char *get_name()
{
@ -982,7 +1066,8 @@ 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,

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;

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;
/* 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
@ -627,9 +628,9 @@ 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;
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;
@ -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;