forked from Archive/PX4-Autopilot
Fixed code style for mavlink app
This commit is contained in:
Lorenz Meier
parent
fdf1c712b1
commit
b52402dbe2
|
|
@ -156,12 +156,15 @@ set_hil_on_off(bool hil_enabled)
|
|||
if (baudrate < 19200) {
|
||||
/* 10 Hz */
|
||||
hil_rate_interval = 100;
|
||||
|
||||
} else if (baudrate < 38400) {
|
||||
/* 10 Hz */
|
||||
hil_rate_interval = 100;
|
||||
|
||||
} else if (baudrate < 115200) {
|
||||
/* 20 Hz */
|
||||
hil_rate_interval = 50;
|
||||
|
||||
} else {
|
||||
/* 200 Hz */
|
||||
hil_rate_interval = 5;
|
||||
|
|
@ -220,6 +223,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
|
|||
/* set arming state */
|
||||
if (armed.armed) {
|
||||
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
|
||||
} else {
|
||||
*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_accel_calibration) {
|
||||
*mavlink_state = MAV_STATE_CALIBRATING;
|
||||
|
||||
} else {
|
||||
*mavlink_state = MAV_STATE_UNINIT;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case SYSTEM_STATE_STANDBY:
|
||||
|
|
@ -288,18 +294,22 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
|
|||
/* sensor sub triggers scaled IMU */
|
||||
orb_set_interval(subs->sensor_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIGHRES_IMU:
|
||||
/* sensor sub triggers highres IMU */
|
||||
orb_set_interval(subs->sensor_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_RAW_IMU:
|
||||
/* sensor sub triggers RAW IMU */
|
||||
orb_set_interval(subs->sensor_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ATTITUDE:
|
||||
/* attitude sub triggers attitude */
|
||||
orb_set_interval(subs->att_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
|
||||
/* actuator_outputs triggers this message */
|
||||
orb_set_interval(subs->act_0_sub, min_interval);
|
||||
|
|
@ -308,13 +318,16 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
|
|||
orb_set_interval(subs->act_3_sub, min_interval);
|
||||
orb_set_interval(subs->actuators_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
||||
/* manual_control_setpoint triggers this message */
|
||||
orb_set_interval(subs->man_control_sp_sub, min_interval);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
|
||||
orb_set_interval(subs->debug_key_value, min_interval);
|
||||
break;
|
||||
|
||||
default:
|
||||
/* not found */
|
||||
ret = ERROR;
|
||||
|
|
@ -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
|
||||
*/
|
||||
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];
|
||||
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
|
||||
*/
|
||||
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];
|
||||
return &m_mavlink_buffer[channel];
|
||||
|
|
@ -501,18 +514,21 @@ void mavlink_update_system(void)
|
|||
/* update system and component id */
|
||||
int32_t system_id;
|
||||
param_get(param_system_id, &system_id);
|
||||
|
||||
if (system_id > 0 && system_id < 255) {
|
||||
mavlink_system.sysid = system_id;
|
||||
}
|
||||
|
||||
int32_t component_id;
|
||||
param_get(param_component_id, &component_id);
|
||||
|
||||
if (component_id > 0 && component_id < 255) {
|
||||
mavlink_system.compid = component_id;
|
||||
}
|
||||
|
||||
int32_t system_type;
|
||||
param_get(param_system_type, &system_type);
|
||||
|
||||
if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
|
||||
mavlink_system.type = system_type;
|
||||
}
|
||||
|
|
@ -538,8 +554,10 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
switch (ch) {
|
||||
case 'b':
|
||||
baudrate = strtoul(optarg, NULL, 10);
|
||||
|
||||
if (baudrate == 0)
|
||||
errx(1, "invalid baud rate '%s'", optarg);
|
||||
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
|
|
@ -560,6 +578,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
struct termios uart_config_original;
|
||||
|
||||
bool usb_uart;
|
||||
|
||||
/* print welcome text */
|
||||
|
|
@ -573,6 +592,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
|
||||
/* default values for arguments */
|
||||
uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
|
||||
|
||||
if (uart < 0)
|
||||
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);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
|
||||
|
||||
} else if (baudrate >= 115200) {
|
||||
/* 20 Hz / 50 ms */
|
||||
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);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
|
||||
} else if (baudrate >= 57600) {
|
||||
/* 10 Hz / 100 ms */
|
||||
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);
|
||||
/* 2 Hz */
|
||||
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
|
||||
|
||||
} else {
|
||||
/* very low baud rate, limit to 1 Hz / 1000 ms */
|
||||
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);
|
||||
lowspeed_counter = 0;
|
||||
}
|
||||
|
||||
lowspeed_counter++;
|
||||
|
||||
/* sleep quarter the time */
|
||||
|
|
@ -693,6 +717,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
|
||||
/* sleep quarter the time */
|
||||
usleep(25000);
|
||||
|
||||
if (baudrate > 57600) {
|
||||
mavlink_pm_queued_send();
|
||||
}
|
||||
|
|
@ -704,6 +729,7 @@ int mavlink_thread_main(int argc, char *argv[])
|
|||
if (!mavlink_logbuffer_is_empty(&lb)) {
|
||||
struct mavlink_logmessage msg;
|
||||
int lb_ret = mavlink_logbuffer_read(&lb, &msg);
|
||||
|
||||
if (lb_ret == OK) {
|
||||
mavlink_missionlib_send_gcs_string(msg.text);
|
||||
}
|
||||
|
|
@ -755,16 +781,18 @@ int mavlink_main(int argc, char *argv[])
|
|||
SCHED_PRIORITY_DEFAULT,
|
||||
2048,
|
||||
mavlink_thread_main,
|
||||
(const char**)argv);
|
||||
(const char **)argv);
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
|
||||
while (thread_running) {
|
||||
usleep(200000);
|
||||
printf(".");
|
||||
}
|
||||
|
||||
warnx("terminated.");
|
||||
exit(0);
|
||||
}
|
||||
|
|
@ -772,6 +800,7 @@ int mavlink_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
errx(0, "running");
|
||||
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
|
||||
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
|
||||
mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
|
||||
mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
|
||||
|
||||
#endif /* MAVLINK_BRIDGE_HEADER_H */
|
||||
|
|
|
|||
|
|
@ -44,37 +44,45 @@
|
|||
|
||||
#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->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;
|
||||
}
|
||||
|
||||
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) {
|
||||
int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
|
||||
{
|
||||
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));
|
||||
|
||||
if (mavlink_logbuffer_is_full(lb)) {
|
||||
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));
|
||||
lb->start = (lb->start + 1) % lb->size;
|
||||
--lb->count;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,7 +75,7 @@ void mavlink_pm_callback(void *arg, param_t param);
|
|||
void mavlink_pm_callback(void *arg, param_t param)
|
||||
{
|
||||
mavlink_pm_send_param(param);
|
||||
usleep(*(unsigned int*)arg);
|
||||
usleep(*(unsigned int *)arg);
|
||||
}
|
||||
|
||||
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_param_queue_index++;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
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));
|
||||
}
|
||||
|
||||
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));
|
||||
}
|
||||
|
|
@ -129,10 +130,13 @@ int mavlink_pm_send_param(param_t param)
|
|||
* endianess matches (both little endian)
|
||||
*/
|
||||
uint8_t mavlink_type;
|
||||
|
||||
if (type == PARAM_TYPE_INT32) {
|
||||
mavlink_type = MAVLINK_TYPE_INT32_T;
|
||||
|
||||
} else if (type == PARAM_TYPE_FLOAT) {
|
||||
mavlink_type = MAVLINK_TYPE_FLOAT;
|
||||
|
||||
} else {
|
||||
mavlink_type = MAVLINK_TYPE_FLOAT;
|
||||
}
|
||||
|
|
@ -143,6 +147,7 @@ int mavlink_pm_send_param(param_t param)
|
|||
*/
|
||||
|
||||
int ret;
|
||||
|
||||
if ((ret = param_get(param, &val_buf)) != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -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))) {
|
||||
/* 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);
|
||||
/* enforce null termination */
|
||||
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];
|
||||
sprintf(buf, "[mavlink pm] unknown: %s", name);
|
||||
mavlink_missionlib_send_gcs_string(buf);
|
||||
|
||||
} else {
|
||||
/* set and send parameter */
|
||||
param_set(param, &(mavlink_param_set.param_value));
|
||||
|
|
@ -207,12 +213,13 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
|
|||
/* when no index is given, loop through string ids and compare them */
|
||||
if (mavlink_param_request_read.param_index == -1) {
|
||||
/* 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);
|
||||
/* enforce null termination */
|
||||
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
|
||||
/* attempt to find parameter and send it */
|
||||
mavlink_pm_send_param_for_name(name);
|
||||
|
||||
} else {
|
||||
/* when index is >= 0, send this parameter again */
|
||||
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
|
||||
|
|
|
|||
|
|
@ -84,14 +84,14 @@ int mavlink_pm_send_param_for_index(uint16_t index);
|
|||
* @param name The index of the parameter to send.
|
||||
* @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.
|
||||
*
|
||||
* @return zero on success, nonzero on failure
|
||||
*/
|
||||
int mavlink_pm_queued_send(void);
|
||||
int mavlink_pm_queued_send(void);
|
||||
|
||||
/**
|
||||
* Start sending the parameter queue.
|
||||
|
|
|
|||
|
|
@ -138,6 +138,7 @@ handle_message(mavlink_message_t *msg)
|
|||
if (cmd_pub <= 0) {
|
||||
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
}
|
||||
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
|
||||
}
|
||||
|
|
@ -162,6 +163,7 @@ handle_message(mavlink_message_t *msg)
|
|||
/* check if topic is advertised */
|
||||
if (flow_pub <= 0) {
|
||||
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
|
||||
|
||||
} else {
|
||||
/* publish */
|
||||
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
|
||||
|
|
@ -191,6 +193,7 @@ handle_message(mavlink_message_t *msg)
|
|||
/* check if topic is advertised */
|
||||
if (cmd_pub <= 0) {
|
||||
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
|
||||
} else {
|
||||
/* create command */
|
||||
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
|
||||
|
|
@ -214,6 +217,7 @@ handle_message(mavlink_message_t *msg)
|
|||
|
||||
if (vicon_position_pub <= 0) {
|
||||
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
|
||||
}
|
||||
|
|
@ -241,30 +245,34 @@ handle_message(mavlink_message_t *msg)
|
|||
case 0:
|
||||
ml_armed = false;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
|
||||
ml_armed = true;
|
||||
|
||||
break;
|
||||
|
||||
case 2:
|
||||
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
|
||||
ml_armed = true;
|
||||
|
||||
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.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.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_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.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;
|
||||
|
||||
if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
|
||||
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
|
||||
ml_armed = false;
|
||||
}
|
||||
|
||||
|
|
@ -276,6 +284,7 @@ handle_message(mavlink_message_t *msg)
|
|||
/* check if topic has to be advertised */
|
||||
if (offboard_control_sp_pub <= 0) {
|
||||
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
|
||||
} else {
|
||||
/* Publish */
|
||||
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) {
|
||||
rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
|
||||
}
|
||||
|
||||
if (mc_pub == 0) {
|
||||
mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
|
||||
}
|
||||
|
|
@ -398,7 +409,7 @@ handle_message(mavlink_message_t *msg)
|
|||
static void *
|
||||
receive_thread(void *arg)
|
||||
{
|
||||
int uart_fd = *((int*)arg);
|
||||
int uart_fd = *((int *)arg);
|
||||
|
||||
const int timeout = 1000;
|
||||
uint8_t ch;
|
||||
|
|
|
|||
|
|
@ -106,6 +106,7 @@ mavlink_missionlib_send_gcs_string(const char *string)
|
|||
|
||||
mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
|
||||
return mavlink_missionlib_send_message(&msg);
|
||||
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
|
|
@ -144,12 +145,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
|||
sp.altitude = param7_alt_z;
|
||||
sp.altitude_is_relative = false;
|
||||
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (global_position_setpoint_pub < 0) {
|
||||
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
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);
|
||||
|
||||
} 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_is_relative = true;
|
||||
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (global_position_setpoint_pub < 0) {
|
||||
global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
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);
|
||||
|
||||
} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
|
||||
|
|
@ -175,12 +182,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
|
|||
sp.y = param6_lon_y;
|
||||
sp.z = param7_alt_z;
|
||||
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
|
||||
|
||||
/* Initialize publication if necessary */
|
||||
if (local_position_setpoint_pub < 0) {
|
||||
local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp);
|
||||
|
||||
} else {
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -90,8 +90,7 @@ static uint64_t last_sensor_timestamp;
|
|||
|
||||
static void *uorb_receive_thread(void *arg);
|
||||
|
||||
struct listener
|
||||
{
|
||||
struct listener {
|
||||
void (*callback)(struct listener *l);
|
||||
int *subp;
|
||||
uintptr_t arg;
|
||||
|
|
@ -189,7 +188,7 @@ l_sensor_combined(struct listener *l)
|
|||
raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
|
||||
raw.gyro_rad_s[1], raw.gyro_rad_s[2],
|
||||
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_alt_meter, raw.baro_temp_celcius,
|
||||
fields_updated);
|
||||
|
|
@ -317,7 +316,7 @@ l_global_position(struct listener *l)
|
|||
uint64_t timestamp = global_pos.timestamp;
|
||||
int32_t lat = global_pos.lat;
|
||||
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);
|
||||
int16_t vx = (int16_t)(global_pos.vx * 100.0f);
|
||||
int16_t vy = (int16_t)(global_pos.vy * 100.0f);
|
||||
|
|
@ -364,6 +363,7 @@ l_global_position_setpoint(struct listener *l)
|
|||
orb_copy(ORB_ID(vehicle_global_position_setpoint), mavlink_subs.spg_sub, &global_sp);
|
||||
|
||||
uint8_t coordinate_frame = MAV_FRAME_GLOBAL;
|
||||
|
||||
if (global_sp.altitude_is_relative)
|
||||
coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
|
||||
|
||||
|
|
@ -403,7 +403,7 @@ l_attitude_setpoint(struct listener *l)
|
|||
|
||||
if (gcs_link)
|
||||
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.pitch_body,
|
||||
att_sp.yaw_body,
|
||||
|
|
@ -420,7 +420,7 @@ l_vehicle_rates_setpoint(struct listener *l)
|
|||
|
||||
if (gcs_link)
|
||||
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.pitch,
|
||||
rates_sp.yaw,
|
||||
|
|
@ -479,6 +479,7 @@ l_actuator_outputs(struct listener *l)
|
|||
-1,
|
||||
mavlink_mode,
|
||||
0);
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
|
|
@ -492,6 +493,7 @@ l_actuator_outputs(struct listener *l)
|
|||
-1,
|
||||
mavlink_mode,
|
||||
0);
|
||||
|
||||
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
|
||||
mavlink_msg_hil_controls_send(chan,
|
||||
hrt_absolute_time(),
|
||||
|
|
@ -505,6 +507,7 @@ l_actuator_outputs(struct listener *l)
|
|||
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
|
||||
mavlink_mode,
|
||||
0);
|
||||
|
||||
} else {
|
||||
|
||||
/*
|
||||
|
|
@ -516,6 +519,7 @@ l_actuator_outputs(struct listener *l)
|
|||
if (act_outputs.noutputs < 4) {
|
||||
rudder = 0.0f;
|
||||
throttle = (act_outputs.output[2] - 900.0f) / 1200.0f;
|
||||
|
||||
} else {
|
||||
rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
|
||||
throttle = (act_outputs.output[3] - 900.0f) / 1200.0f;
|
||||
|
|
@ -635,6 +639,7 @@ uorb_receive_thread(void *arg)
|
|||
* Might want to invoke each listener once to set initial state.
|
||||
*/
|
||||
struct pollfd fds[n_listeners];
|
||||
|
||||
for (unsigned i = 0; i < n_listeners; i++) {
|
||||
fds[i].fd = *listeners[i].subp;
|
||||
fds[i].events = POLLIN;
|
||||
|
|
@ -650,8 +655,10 @@ uorb_receive_thread(void *arg)
|
|||
/* handle the poll result */
|
||||
if (poll_ret == 0) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
|
||||
|
||||
} else if (poll_ret < 0) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
|
||||
|
||||
} else {
|
||||
|
||||
for (unsigned i = 0; i < n_listeners; i++) {
|
||||
|
|
|
|||
|
|
@ -379,6 +379,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
|
|||
if (counter % 100 == 0)
|
||||
printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
|
||||
}
|
||||
|
||||
// else
|
||||
// {
|
||||
// if(counter % 100 == 0)
|
||||
|
|
|
|||
Loading…
Reference in New Issue