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

Fixed code style for mavlink app

This commit is contained in:
Lorenz Meier 2013-01-11 07:48:09 +01:00
parent fdf1c712b1
commit b52402dbe2
12 changed files with 334 additions and 261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
apps/mavlink/mavlink.c
View File

@ -156,12 +156,15 @@ set_hil_on_off(bool hil_enabled)
if (baudrate < 19200) { if (baudrate < 19200) {
/* 10 Hz */ /* 10 Hz */
hil_rate_interval = 100; hil_rate_interval = 100;
} else if (baudrate < 38400) { } else if (baudrate < 38400) {
/* 10 Hz */ /* 10 Hz */
hil_rate_interval = 100; hil_rate_interval = 100;
} else if (baudrate < 115200) { } else if (baudrate < 115200) {
/* 20 Hz */ /* 20 Hz */
hil_rate_interval = 50; hil_rate_interval = 50;
} else { } else {
/* 200 Hz */ /* 200 Hz */
hil_rate_interval = 5; hil_rate_interval = 5;
@ -202,13 +205,13 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
/* attitude or rate control */ /* attitude or rate control */
if (v_status.flag_control_attitude_enabled || if (v_status.flag_control_attitude_enabled ||
v_status.flag_control_rates_enabled) { v_status.flag_control_rates_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
} }
/* vector control */ /* vector control */
if (v_status.flag_control_velocity_enabled || if (v_status.flag_control_velocity_enabled ||
v_status.flag_control_position_enabled) { v_status.flag_control_position_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
} }
@ -220,6 +223,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
/* set arming state */ /* set arming state */
if (armed.armed) { if (armed.armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else { } else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
} }
@ -230,9 +234,11 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
v_status.flag_preflight_mag_calibration || v_status.flag_preflight_mag_calibration ||
v_status.flag_preflight_accel_calibration) { v_status.flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING; *mavlink_state = MAV_STATE_CALIBRATING;
} else { } else {
*mavlink_state = MAV_STATE_UNINIT; *mavlink_state = MAV_STATE_UNINIT;
} }
break; break;
case SYSTEM_STATE_STANDBY: case SYSTEM_STATE_STANDBY:
@ -284,41 +290,48 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
int ret = OK; int ret = OK;
switch (mavlink_msg_id) { switch (mavlink_msg_id) {
case MAVLINK_MSG_ID_SCALED_IMU: case MAVLINK_MSG_ID_SCALED_IMU:
/* sensor sub triggers scaled IMU */ /* sensor sub triggers scaled IMU */
orb_set_interval(subs->sensor_sub, min_interval); orb_set_interval(subs->sensor_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_HIGHRES_IMU:
/* sensor sub triggers highres IMU */ case MAVLINK_MSG_ID_HIGHRES_IMU:
orb_set_interval(subs->sensor_sub, min_interval); /* sensor sub triggers highres IMU */
break; orb_set_interval(subs->sensor_sub, min_interval);
case MAVLINK_MSG_ID_RAW_IMU: break;
/* sensor sub triggers RAW IMU */
orb_set_interval(subs->sensor_sub, min_interval); case MAVLINK_MSG_ID_RAW_IMU:
break; /* sensor sub triggers RAW IMU */
case MAVLINK_MSG_ID_ATTITUDE: orb_set_interval(subs->sensor_sub, min_interval);
/* attitude sub triggers attitude */ break;
orb_set_interval(subs->att_sub, min_interval);
break; case MAVLINK_MSG_ID_ATTITUDE:
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: /* attitude sub triggers attitude */
/* actuator_outputs triggers this message */ orb_set_interval(subs->att_sub, min_interval);
orb_set_interval(subs->act_0_sub, min_interval); break;
orb_set_interval(subs->act_1_sub, min_interval);
orb_set_interval(subs->act_2_sub, min_interval); case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
orb_set_interval(subs->act_3_sub, min_interval); /* actuator_outputs triggers this message */
orb_set_interval(subs->actuators_sub, min_interval); orb_set_interval(subs->act_0_sub, min_interval);
break; orb_set_interval(subs->act_1_sub, min_interval);
case MAVLINK_MSG_ID_MANUAL_CONTROL: orb_set_interval(subs->act_2_sub, min_interval);
/* manual_control_setpoint triggers this message */ orb_set_interval(subs->act_3_sub, min_interval);
orb_set_interval(subs->man_control_sp_sub, min_interval); orb_set_interval(subs->actuators_sub, min_interval);
break; break;
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
orb_set_interval(subs->debug_key_value, min_interval); case MAVLINK_MSG_ID_MANUAL_CONTROL:
break; /* manual_control_setpoint triggers this message */
default: orb_set_interval(subs->man_control_sp_sub, min_interval);
/* not found */ break;
ret = ERROR;
break; case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
orb_set_interval(subs->debug_key_value, min_interval);
break;
default:
/* not found */
ret = ERROR;
break;
} }
return ret; return ret;
@ -460,7 +473,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
return uart; return uart;
} }
void void
mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
{ {
write(uart, ch, (size_t)(sizeof(uint8_t) * length)); write(uart, ch, (size_t)(sizeof(uint8_t) * length));
@ -469,7 +482,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
/* /*
* Internal function to give access to the channel status for each channel * Internal function to give access to the channel status for each channel
*/ */
mavlink_status_t* mavlink_get_channel_status(uint8_t channel) mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
{ {
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[channel]; return &m_mavlink_status[channel];
@ -478,7 +491,7 @@ mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
/* /*
* Internal function to give access to the channel buffer for each channel * Internal function to give access to the channel buffer for each channel
*/ */
mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
{ {
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_buffer[channel]; return &m_mavlink_buffer[channel];
@ -501,18 +514,21 @@ void mavlink_update_system(void)
/* update system and component id */ /* update system and component id */
int32_t system_id; int32_t system_id;
param_get(param_system_id, &system_id); param_get(param_system_id, &system_id);
if (system_id > 0 && system_id < 255) { if (system_id > 0 && system_id < 255) {
mavlink_system.sysid = system_id; mavlink_system.sysid = system_id;
} }
int32_t component_id; int32_t component_id;
param_get(param_component_id, &component_id); param_get(param_component_id, &component_id);
if (component_id > 0 && component_id < 255) { if (component_id > 0 && component_id < 255) {
mavlink_system.compid = component_id; mavlink_system.compid = component_id;
} }
int32_t system_type; int32_t system_type;
param_get(param_system_type, &system_type); param_get(param_system_type, &system_type);
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
mavlink_system.type = system_type; mavlink_system.type = system_type;
} }
@ -538,8 +554,10 @@ int mavlink_thread_main(int argc, char *argv[])
switch (ch) { switch (ch) {
case 'b': case 'b':
baudrate = strtoul(optarg, NULL, 10); baudrate = strtoul(optarg, NULL, 10);
if (baudrate == 0) if (baudrate == 0)
errx(1, "invalid baud rate '%s'", optarg); errx(1, "invalid baud rate '%s'", optarg);
break; break;
case 'd': case 'd':
@ -560,6 +578,7 @@ int mavlink_thread_main(int argc, char *argv[])
} }
struct termios uart_config_original; struct termios uart_config_original;
bool usb_uart; bool usb_uart;
/* print welcome text */ /* print welcome text */
@ -573,6 +592,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* default values for arguments */ /* default values for arguments */
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
if (uart < 0) if (uart < 0)
err(1, "could not open %s", device_name); err(1, "could not open %s", device_name);
@ -603,6 +623,7 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
/* 2 Hz */ /* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) { } else if (baudrate >= 115200) {
/* 20 Hz / 50 ms */ /* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
@ -613,6 +634,7 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 2 Hz */ /* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 57600) { } else if (baudrate >= 57600) {
/* 10 Hz / 100 ms */ /* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
@ -625,6 +647,7 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
/* 2 Hz */ /* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else { } else {
/* very low baud rate, limit to 1 Hz / 1000 ms */ /* very low baud rate, limit to 1 Hz / 1000 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
@ -677,6 +700,7 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.errors_count4); v_status.errors_count4);
lowspeed_counter = 0; lowspeed_counter = 0;
} }
lowspeed_counter++; lowspeed_counter++;
/* sleep quarter the time */ /* sleep quarter the time */
@ -693,6 +717,7 @@ int mavlink_thread_main(int argc, char *argv[])
/* sleep quarter the time */ /* sleep quarter the time */
usleep(25000); usleep(25000);
if (baudrate > 57600) { if (baudrate > 57600) {
mavlink_pm_queued_send(); mavlink_pm_queued_send();
} }
@ -704,6 +729,7 @@ int mavlink_thread_main(int argc, char *argv[])
if (!mavlink_logbuffer_is_empty(&lb)) { if (!mavlink_logbuffer_is_empty(&lb)) {
struct mavlink_logmessage msg; struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&lb, &msg); int lb_ret = mavlink_logbuffer_read(&lb, &msg);
if (lb_ret == OK) { if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text); mavlink_missionlib_send_gcs_string(msg.text);
} }
@ -730,8 +756,8 @@ static void
usage() usage()
{ {
fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
" mavlink stop\n" " mavlink stop\n"
" mavlink status\n"); " mavlink status\n");
exit(1); exit(1);
} }
@ -755,16 +781,18 @@ int mavlink_main(int argc, char *argv[])
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
2048, 2048,
mavlink_thread_main, mavlink_thread_main,
(const char**)argv); (const char **)argv);
exit(0); exit(0);
} }
if (!strcmp(argv[1], "stop")) { if (!strcmp(argv[1], "stop")) {
thread_should_exit = true; thread_should_exit = true;
while (thread_running) { while (thread_running) {
usleep(200000); usleep(200000);
printf("."); printf(".");
} }
warnx("terminated."); warnx("terminated.");
exit(0); exit(0);
} }
@ -772,6 +800,7 @@ int mavlink_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
if (thread_running) { if (thread_running) {
errx(0, "running"); errx(0, "running");
} else { } else {
errx(1, "not running"); errx(1, "not running");
} }

4
apps/mavlink/mavlink_bridge_header.h
View File

@ -75,7 +75,7 @@ extern mavlink_system_t mavlink_system;
*/ */
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
mavlink_status_t* mavlink_get_channel_status(uint8_t chan); mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
#endif /* MAVLINK_BRIDGE_HEADER_H */ #endif /* MAVLINK_BRIDGE_HEADER_H */

2
apps/mavlink/mavlink_hil.h
View File

@ -50,7 +50,7 @@ extern orb_advert_t pub_hil_attitude;
* Enable / disable Hardware in the Loop simulation mode. * Enable / disable Hardware in the Loop simulation mode.
* *
* @param hil_enabled The new HIL enable/disable state. * @param hil_enabled The new HIL enable/disable state.
* @return OK if the HIL state changed, ERROR if the * @return OK if the HIL state changed, ERROR if the
* requested change could not be made or was * requested change could not be made or was
* redundant. * redundant.
*/ */

68
apps/mavlink/mavlink_log.c
View File

@ -43,39 +43,47 @@
#include <stdlib.h> #include <stdlib.h>
#include "mavlink_log.h" #include "mavlink_log.h"
void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
lb->size = size; {
lb->start = 0; lb->size = size;
lb->count = 0; lb->start = 0;
lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); lb->count = 0;
lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
} }
int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
{
return lb->count == (int)lb->size; return lb->count == (int)lb->size;
} }
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
return lb->count == 0; {
return lb->count == 0;
} }
void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
int end = (lb->start + lb->count) % lb->size; {
memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); int end = (lb->start + lb->count) % lb->size;
if (mavlink_logbuffer_is_full(lb)) { memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
} else { if (mavlink_logbuffer_is_full(lb)) {
++lb->count; lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
}
} else {
++lb->count;
}
} }
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
if (!mavlink_logbuffer_is_empty(lb)) { {
memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); if (!mavlink_logbuffer_is_empty(lb)) {
lb->start = (lb->start + 1) % lb->size; memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
--lb->count; lb->start = (lb->start + 1) % lb->size;
return 0; --lb->count;
} else { return 0;
return 1;
} } else {
return 1;
}
} }

22
apps/mavlink/mavlink_log.h
View File

@ -82,26 +82,26 @@
#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
struct mavlink_logmessage { struct mavlink_logmessage {
char text[51]; char text[51];
unsigned char severity; unsigned char severity;
}; };
struct mavlink_logbuffer { struct mavlink_logbuffer {
unsigned int start; unsigned int start;
// unsigned int end; // unsigned int end;
unsigned int size; unsigned int size;
int count; int count;
struct mavlink_logmessage *elems; struct mavlink_logmessage *elems;
}; };
void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb); int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb);
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb); int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb);
void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem); void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem);
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
#endif #endif

49
apps/mavlink/mavlink_parameters.c
View File

@ -75,7 +75,7 @@ void mavlink_pm_callback(void *arg, param_t param);
void mavlink_pm_callback(void *arg, param_t param) void mavlink_pm_callback(void *arg, param_t param)
{ {
mavlink_pm_send_param(param); mavlink_pm_send_param(param);
usleep(*(unsigned int*)arg); usleep(*(unsigned int *)arg);
} }
void mavlink_pm_send_all_params(unsigned int delay) void mavlink_pm_send_all_params(unsigned int delay)
@ -90,6 +90,7 @@ int mavlink_pm_queued_send()
mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
mavlink_param_queue_index++; mavlink_param_queue_index++;
return 0; return 0;
} else { } else {
return 1; return 1;
} }
@ -105,7 +106,7 @@ int mavlink_pm_send_param_for_index(uint16_t index)
return mavlink_pm_send_param(param_for_index(index)); return mavlink_pm_send_param(param_for_index(index));
} }
int mavlink_pm_send_param_for_name(const char* name) int mavlink_pm_send_param_for_name(const char *name)
{ {
return mavlink_pm_send_param(param_find(name)); return mavlink_pm_send_param(param_find(name));
} }
@ -123,16 +124,19 @@ int mavlink_pm_send_param(param_t param)
param_type_t type = param_type(param); param_type_t type = param_type(param);
/* copy parameter name */ /* copy parameter name */
strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* /*
* Map onboard parameter type to MAVLink type, * Map onboard parameter type to MAVLink type,
* endianess matches (both little endian) * endianess matches (both little endian)
*/ */
uint8_t mavlink_type; uint8_t mavlink_type;
if (type == PARAM_TYPE_INT32) { if (type == PARAM_TYPE_INT32) {
mavlink_type = MAVLINK_TYPE_INT32_T; mavlink_type = MAVLINK_TYPE_INT32_T;
} else if (type == PARAM_TYPE_FLOAT) { } else if (type == PARAM_TYPE_FLOAT) {
mavlink_type = MAVLINK_TYPE_FLOAT; mavlink_type = MAVLINK_TYPE_FLOAT;
} else { } else {
mavlink_type = MAVLINK_TYPE_FLOAT; mavlink_type = MAVLINK_TYPE_FLOAT;
} }
@ -143,6 +147,7 @@ int mavlink_pm_send_param(param_t param)
*/ */
int ret; int ret;
if ((ret = param_get(param, &val_buf)) != OK) { if ((ret = param_get(param, &val_buf)) != OK) {
return ret; return ret;
} }
@ -163,13 +168,13 @@ int mavlink_pm_send_param(param_t param)
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{ {
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */ /* Start sending parameters */
mavlink_pm_start_queued_send(); mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
} break; } break;
case MAVLINK_MSG_ID_PARAM_SET: { case MAVLINK_MSG_ID_PARAM_SET: {
/* Handle parameter setting */ /* Handle parameter setting */
@ -179,7 +184,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
/* local name buffer to enforce null-terminated string */ /* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1]; char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */ /* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
@ -190,6 +195,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[mavlink pm] unknown: %s", name); sprintf(buf, "[mavlink pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(buf); mavlink_missionlib_send_gcs_string(buf);
} else { } else {
/* set and send parameter */ /* set and send parameter */
param_set(param, &(mavlink_param_set.param_value)); param_set(param, &(mavlink_param_set.param_value));
@ -199,25 +205,26 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
} }
} break; } break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
mavlink_param_request_read_t mavlink_param_request_read; mavlink_param_request_read_t mavlink_param_request_read;
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
/* when no index is given, loop through string ids and compare them */ /* when no index is given, loop through string ids and compare them */
if (mavlink_param_request_read.param_index == -1) { if (mavlink_param_request_read.param_index == -1) {
/* local name buffer to enforce null-terminated string */ /* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1]; char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */ /* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */ /* attempt to find parameter and send it */
mavlink_pm_send_param_for_name(name); mavlink_pm_send_param_for_name(name);
} else {
/* when index is >= 0, send this parameter again */ } else {
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); /* when index is >= 0, send this parameter again */
} mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
} }
}
} break; } break;
} }

6
apps/mavlink/mavlink_parameters.h
View File

@ -48,7 +48,7 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
/** /**
* Handle parameter related messages. * Handle parameter related messages.
*/ */
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
@ -84,14 +84,14 @@ int mavlink_pm_send_param_for_index(uint16_t index);
* @param name The index of the parameter to send. * @param name The index of the parameter to send.
* @return zero on success, nonzero else. * @return zero on success, nonzero else.
*/ */
int mavlink_pm_send_param_for_name(const char* name); int mavlink_pm_send_param_for_name(const char *name);
/** /**
* Send a queue of parameters, one parameter per function call. * Send a queue of parameters, one parameter per function call.
* *
* @return zero on success, nonzero on failure * @return zero on success, nonzero on failure
*/ */
int mavlink_pm_queued_send(void); int mavlink_pm_queued_send(void);
/** /**
* Start sending the parameter queue. * Start sending the parameter queue.

65
apps/mavlink/mavlink_receiver.c
View File

@ -106,7 +106,7 @@ handle_message(mavlink_message_t *msg)
mavlink_msg_command_long_decode(msg, &cmd_mavlink); mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command //check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */ /* This is the link shutdown command, terminate mavlink */
@ -138,6 +138,7 @@ handle_message(mavlink_message_t *msg)
if (cmd_pub <= 0) { if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} }
/* publish */ /* publish */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
} }
@ -162,6 +163,7 @@ handle_message(mavlink_message_t *msg)
/* check if topic is advertised */ /* check if topic is advertised */
if (flow_pub <= 0) { if (flow_pub <= 0) {
flow_pub = orb_advertise(ORB_ID(optical_flow), &f); flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else { } else {
/* publish */ /* publish */
orb_publish(ORB_ID(optical_flow), flow_pub, &f); orb_publish(ORB_ID(optical_flow), flow_pub, &f);
@ -191,6 +193,7 @@ handle_message(mavlink_message_t *msg)
/* check if topic is advertised */ /* check if topic is advertised */
if (cmd_pub <= 0) { if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else { } else {
/* create command */ /* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
@ -214,6 +217,7 @@ handle_message(mavlink_message_t *msg)
if (vicon_position_pub <= 0) { if (vicon_position_pub <= 0) {
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else { } else {
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
} }
@ -230,7 +234,7 @@ handle_message(mavlink_message_t *msg)
/* switch to a receiving link mode */ /* switch to a receiving link mode */
gcs_link = false; gcs_link = false;
/* /*
* rate control mode - defined by MAVLink * rate control mode - defined by MAVLink
*/ */
@ -238,33 +242,37 @@ handle_message(mavlink_message_t *msg)
bool ml_armed = false; bool ml_armed = false;
switch (quad_motors_setpoint.mode) { switch (quad_motors_setpoint.mode) {
case 0: case 0:
ml_armed = false; ml_armed = false;
break; break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
ml_armed = true;
break; case 1:
case 2: ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; ml_armed = true;
ml_armed = true;
break; break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; case 2:
break; ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
case 4: ml_armed = true;
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
break; break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
case 4:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
break;
} }
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_MAX; offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
ml_armed = false; ml_armed = false;
} }
@ -276,6 +284,7 @@ handle_message(mavlink_message_t *msg)
/* check if topic has to be advertised */ /* check if topic has to be advertised */
if (offboard_control_sp_pub <= 0) { if (offboard_control_sp_pub <= 0) {
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else { } else {
/* Publish */ /* Publish */
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
@ -378,12 +387,14 @@ handle_message(mavlink_message_t *msg)
if (rc_pub == 0) { if (rc_pub == 0) {
rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
} else { } else {
orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
} }
if (mc_pub == 0) { if (mc_pub == 0) {
mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
} else { } else {
orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
} }
@ -398,7 +409,7 @@ handle_message(mavlink_message_t *msg)
static void * static void *
receive_thread(void *arg) receive_thread(void *arg)
{ {
int uart_fd = *((int*)arg); int uart_fd = *((int *)arg);
const int timeout = 1000; const int timeout = 1000;
uint8_t ch; uint8_t ch;
@ -442,8 +453,8 @@ receive_start(int uart)
pthread_attr_init(&receiveloop_attr); pthread_attr_init(&receiveloop_attr);
struct sched_param param; struct sched_param param;
param.sched_priority = SCHED_PRIORITY_MAX - 40; param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param); (void)pthread_attr_setschedparam(&receiveloop_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048); pthread_attr_setstacksize(&receiveloop_attr, 2048);

12
apps/mavlink/missionlib.c
View File

@ -106,6 +106,7 @@ mavlink_missionlib_send_gcs_string(const char *string)
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
return mavlink_missionlib_send_message(&msg); return mavlink_missionlib_send_message(&msg);
} else { } else {
return 1; return 1;
} }
@ -144,12 +145,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.altitude = param7_alt_z; sp.altitude = param7_alt_z;
sp.altitude_is_relative = false; sp.altitude_is_relative = false;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
/* Initialize publication if necessary */ /* Initialize publication if necessary */
if (global_position_setpoint_pub < 0) { if (global_position_setpoint_pub < 0) {
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
} else { } else {
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
} }
sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
@ -160,12 +164,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.altitude = param7_alt_z; sp.altitude = param7_alt_z;
sp.altitude_is_relative = true; sp.altitude_is_relative = true;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
/* Initialize publication if necessary */ /* Initialize publication if necessary */
if (global_position_setpoint_pub < 0) { if (global_position_setpoint_pub < 0) {
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
} else { } else {
orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
} }
sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
@ -175,15 +182,18 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.y = param6_lon_y; sp.y = param6_lon_y;
sp.z = param7_alt_z; sp.z = param7_alt_z;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
/* Initialize publication if necessary */ /* Initialize publication if necessary */
if (local_position_setpoint_pub < 0) { if (local_position_setpoint_pub < 0) {
local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
} else { } else {
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp);
} }
sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
} }
mavlink_missionlib_send_gcs_string(buf); mavlink_missionlib_send_gcs_string(buf);
printf("%s\n", buf); printf("%s\n", buf);
//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);

4
apps/mavlink/missionlib.h
View File

@ -48,5 +48,5 @@ extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
extern int mavlink_missionlib_send_gcs_string(const char *string); extern int mavlink_missionlib_send_gcs_string(const char *string);
extern uint64_t mavlink_missionlib_get_system_timestamp(void); extern uint64_t mavlink_missionlib_get_system_timestamp(void);
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param2, float param3, float param4, float param5_lat_x, float param2, float param3, float param4, float param5_lat_x,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);

245
apps/mavlink/orb_listener.c
View File

@ -90,9 +90,8 @@ static uint64_t last_sensor_timestamp;
static void *uorb_receive_thread(void *arg); static void *uorb_receive_thread(void *arg);
struct listener struct listener {
{ void (*callback)(struct listener *l);
void (*callback)(struct listener *l);
int *subp; int *subp;
uintptr_t arg; uintptr_t arg;
}; };
@ -185,14 +184,14 @@ l_sensor_combined(struct listener *l)
if (gcs_link) if (gcs_link)
mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
raw.gyro_rad_s[1], raw.gyro_rad_s[2], raw.gyro_rad_s[1], raw.gyro_rad_s[2],
raw.magnetometer_ga[0], raw.magnetometer_ga[0],
raw.magnetometer_ga[1],raw.magnetometer_ga[2], raw.magnetometer_ga[1], raw.magnetometer_ga[2],
raw.baro_pres_mbar, 0 /* no diff pressure yet */, raw.baro_pres_mbar, 0 /* no diff pressure yet */,
raw.baro_alt_meter, raw.baro_temp_celcius, raw.baro_alt_meter, raw.baro_temp_celcius,
fields_updated); fields_updated);
sensors_raw_counter++; sensors_raw_counter++;
} }
@ -209,13 +208,13 @@ l_vehicle_attitude(struct listener *l)
if (gcs_link) if (gcs_link)
/* send sensor values */ /* send sensor values */
mavlink_msg_attitude_send(MAVLINK_COMM_0, mavlink_msg_attitude_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000, last_sensor_timestamp / 1000,
att.roll, att.roll,
att.pitch, att.pitch,
att.yaw, att.yaw,
att.rollspeed, att.rollspeed,
att.pitchspeed, att.pitchspeed,
att.yawspeed); att.yawspeed);
attitude_counter++; attitude_counter++;
} }
@ -291,21 +290,21 @@ l_input_rc(struct listener *l)
{ {
/* copy rc channels into local buffer */ /* copy rc channels into local buffer */
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
if (gcs_link) if (gcs_link)
/* Channels are sent in MAVLink main loop at a fixed interval */ /* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan, mavlink_msg_rc_channels_raw_send(chan,
rc_raw.timestamp / 1000, rc_raw.timestamp / 1000,
0, 0,
(rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
(rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
(rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
(rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
(rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
(rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
(rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
(rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
255); 255);
} }
void void
@ -317,7 +316,7 @@ l_global_position(struct listener *l)
uint64_t timestamp = global_pos.timestamp; uint64_t timestamp = global_pos.timestamp;
int32_t lat = global_pos.lat; int32_t lat = global_pos.lat;
int32_t lon = global_pos.lon; int32_t lon = global_pos.lon;
int32_t alt = (int32_t)(global_pos.alt*1000); int32_t alt = (int32_t)(global_pos.alt * 1000);
int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
int16_t vx = (int16_t)(global_pos.vx * 100.0f); int16_t vx = (int16_t)(global_pos.vx * 100.0f);
int16_t vy = (int16_t)(global_pos.vy * 100.0f); int16_t vy = (int16_t)(global_pos.vy * 100.0f);
@ -343,16 +342,16 @@ l_local_position(struct listener *l)
{ {
/* copy local position data into local buffer */ /* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
if (gcs_link) if (gcs_link)
mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
local_pos.timestamp / 1000, local_pos.timestamp / 1000,
local_pos.x, local_pos.x,
local_pos.y, local_pos.y,
local_pos.z, local_pos.z,
local_pos.vx, local_pos.vx,
local_pos.vy, local_pos.vy,
local_pos.vz); local_pos.vz);
} }
void void
@ -364,16 +363,17 @@ l_global_position_setpoint(struct listener *l)
orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp); orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
uint8_t coordinate_frame = MAV_FRAME_GLOBAL; uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
if (global_sp.altitude_is_relative) if (global_sp.altitude_is_relative)
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
if (gcs_link) if (gcs_link)
mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
coordinate_frame, coordinate_frame,
global_sp.lat, global_sp.lat,
global_sp.lon, global_sp.lon,
global_sp.altitude, global_sp.altitude,
global_sp.yaw); global_sp.yaw);
} }
void void
@ -386,11 +386,11 @@ l_local_position_setpoint(struct listener *l)
if (gcs_link) if (gcs_link)
mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
MAV_FRAME_LOCAL_NED, MAV_FRAME_LOCAL_NED,
local_sp.x, local_sp.x,
local_sp.y, local_sp.y,
local_sp.z, local_sp.z,
local_sp.yaw); local_sp.yaw);
} }
void void
@ -403,11 +403,11 @@ l_attitude_setpoint(struct listener *l)
if (gcs_link) if (gcs_link)
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
att_sp.timestamp/1000, att_sp.timestamp / 1000,
att_sp.roll_body, att_sp.roll_body,
att_sp.pitch_body, att_sp.pitch_body,
att_sp.yaw_body, att_sp.yaw_body,
att_sp.thrust); att_sp.thrust);
} }
void void
@ -420,11 +420,11 @@ l_vehicle_rates_setpoint(struct listener *l)
if (gcs_link) if (gcs_link)
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
rates_sp.timestamp/1000, rates_sp.timestamp / 1000,
rates_sp.roll, rates_sp.roll,
rates_sp.pitch, rates_sp.pitch,
rates_sp.yaw, rates_sp.yaw,
rates_sp.thrust); rates_sp.thrust);
} }
void void
@ -444,15 +444,15 @@ l_actuator_outputs(struct listener *l)
if (gcs_link) { if (gcs_link) {
mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
l->arg /* port number */, l->arg /* port number */,
act_outputs.output[0], act_outputs.output[0],
act_outputs.output[1], act_outputs.output[1],
act_outputs.output[2], act_outputs.output[2],
act_outputs.output[3], act_outputs.output[3],
act_outputs.output[4], act_outputs.output[4],
act_outputs.output[5], act_outputs.output[5],
act_outputs.output[6], act_outputs.output[6],
act_outputs.output[7]); act_outputs.output[7]);
/* only send in HIL mode */ /* only send in HIL mode */
if (mavlink_hil_enabled && armed.armed) { if (mavlink_hil_enabled && armed.armed) {
@ -468,43 +468,46 @@ l_actuator_outputs(struct listener *l)
if (mavlink_system.type == MAV_TYPE_QUADROTOR) { if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
mavlink_msg_hil_controls_send(chan, mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(), hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-1, -1,
-1, -1,
-1, -1,
-1, -1,
mavlink_mode, mavlink_mode,
0); 0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
mavlink_msg_hil_controls_send(chan, mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(), hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1, -1,
-1, -1,
mavlink_mode, mavlink_mode,
0); 0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
mavlink_msg_hil_controls_send(chan, mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(), hrt_absolute_time(),
((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_mode, mavlink_mode,
0); 0);
} else { } else {
/* /*
@ -516,23 +519,24 @@ l_actuator_outputs(struct listener *l)
if (act_outputs.noutputs < 4) { if (act_outputs.noutputs < 4) {
rudder = 0.0f; rudder = 0.0f;
throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; throttle = (act_outputs.output[2] - 900.0f) / 1200.0f;
} else { } else {
rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; throttle = (act_outputs.output[3] - 900.0f) / 1200.0f;
} }
mavlink_msg_hil_controls_send(chan, mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(), hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[0] - 1500.0f) / 600.0f,
(act_outputs.output[1] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f,
rudder, rudder,
throttle, throttle,
(act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[4] - 1500.0f) / 600.0f,
(act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f,
(act_outputs.output[6] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f,
(act_outputs.output[7] - 1500.0f) / 600.0f, (act_outputs.output[7] - 1500.0f) / 600.0f,
mavlink_mode, mavlink_mode,
0); 0);
} }
} }
} }
@ -554,12 +558,12 @@ l_manual_control_setpoint(struct listener *l)
if (gcs_link) if (gcs_link)
mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_msg_manual_control_send(MAVLINK_COMM_0,
mavlink_system.sysid, mavlink_system.sysid,
man_control.roll * 1000, man_control.roll * 1000,
man_control.pitch * 1000, man_control.pitch * 1000,
man_control.yaw * 1000, man_control.yaw * 1000,
man_control.throttle * 1000, man_control.throttle * 1000,
0); 0);
} }
void void
@ -614,7 +618,7 @@ l_optical_flow(struct listener *l)
orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
} }
static void * static void *
@ -635,6 +639,7 @@ uorb_receive_thread(void *arg)
* Might want to invoke each listener once to set initial state. * Might want to invoke each listener once to set initial state.
*/ */
struct pollfd fds[n_listeners]; struct pollfd fds[n_listeners];
for (unsigned i = 0; i < n_listeners; i++) { for (unsigned i = 0; i < n_listeners; i++) {
fds[i].fd = *listeners[i].subp; fds[i].fd = *listeners[i].subp;
fds[i].events = POLLIN; fds[i].events = POLLIN;
@ -650,8 +655,10 @@ uorb_receive_thread(void *arg)
/* handle the poll result */ /* handle the poll result */
if (poll_ret == 0) { if (poll_ret == 0) {
mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
} else if (poll_ret < 0) { } else if (poll_ret < 0) {
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
} else { } else {
for (unsigned i = 0; i < n_listeners; i++) { for (unsigned i = 0; i < n_listeners; i++) {

1
apps/mavlink/waypoints.c
View File

@ -379,6 +379,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
if (counter % 100 == 0) if (counter % 100 == 0)
printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
} }
// else // else
// { // {
// if(counter % 100 == 0) // if(counter % 100 == 0)