From b52402dbe2d3447b25a46070bdd771c12fd4c55a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jan 2013 07:48:09 +0100 Subject: [PATCH] Fixed code style for mavlink app --- apps/mavlink/mavlink.c | 117 ++++++++----- apps/mavlink/mavlink_bridge_header.h | 4 +- apps/mavlink/mavlink_hil.h | 2 +- apps/mavlink/mavlink_log.c | 68 ++++---- apps/mavlink/mavlink_log.h | 22 +-- apps/mavlink/mavlink_parameters.c | 49 +++--- apps/mavlink/mavlink_parameters.h | 6 +- apps/mavlink/mavlink_receiver.c | 65 ++++--- apps/mavlink/missionlib.c | 12 +- apps/mavlink/missionlib.h | 4 +- apps/mavlink/orb_listener.c | 245 ++++++++++++++------------- apps/mavlink/waypoints.c | 1 + 12 files changed, 334 insertions(+), 261 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ccc9d6d7d6..2ec459ddc7 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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; @@ -202,13 +205,13 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* attitude or rate control */ 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; } /* vector control */ 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; } @@ -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: @@ -284,41 +290,48 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m int ret = OK; switch (mavlink_msg_id) { - case MAVLINK_MSG_ID_SCALED_IMU: - /* 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); - orb_set_interval(subs->act_1_sub, min_interval); - orb_set_interval(subs->act_2_sub, min_interval); - 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; - break; + case MAVLINK_MSG_ID_SCALED_IMU: + /* 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); + orb_set_interval(subs->act_1_sub, min_interval); + orb_set_interval(subs->act_2_sub, min_interval); + 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; + break; } return ret; @@ -460,7 +473,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } -void +void mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int 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 */ -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); } @@ -730,8 +756,8 @@ static void usage() { fprintf(stderr, "usage: mavlink start [-d ] [-b ]\n" - " mavlink stop\n" - " mavlink status\n"); + " mavlink stop\n" + " mavlink status\n"); exit(1); } @@ -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"); } diff --git a/apps/mavlink/mavlink_bridge_header.h b/apps/mavlink/mavlink_bridge_header.h index bf7c5354cc..270ec17274 100644 --- a/apps/mavlink/mavlink_bridge_header.h +++ b/apps/mavlink/mavlink_bridge_header.h @@ -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 */ diff --git a/apps/mavlink/mavlink_hil.h b/apps/mavlink/mavlink_hil.h index 95790db934..1eb8c32e9e 100644 --- a/apps/mavlink/mavlink_hil.h +++ b/apps/mavlink/mavlink_hil.h @@ -50,7 +50,7 @@ extern orb_advert_t pub_hil_attitude; * Enable / disable Hardware in the Loop simulation mode. * * @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 * redundant. */ diff --git a/apps/mavlink/mavlink_log.c b/apps/mavlink/mavlink_log.c index 73d59e76fc..ed65fb883c 100644 --- a/apps/mavlink/mavlink_log.c +++ b/apps/mavlink/mavlink_log.c @@ -43,39 +43,47 @@ #include #include "mavlink_log.h" - -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)); + +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) { - return lb->count == 0; + +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) { - 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; - } + +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) { - 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; - } + +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; + } } diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index cb6fd9d98c..62e6f7ca07 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -82,26 +82,26 @@ #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); struct mavlink_logmessage { - char text[51]; - unsigned char severity; + char text[51]; + unsigned char severity; }; struct mavlink_logbuffer { - unsigned int start; - // unsigned int end; - unsigned int size; - int count; - struct mavlink_logmessage *elems; + unsigned int start; + // unsigned int end; + unsigned int size; + int count; + struct mavlink_logmessage *elems; }; - + void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); - + int mavlink_logbuffer_is_full(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); - + int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); #endif diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index f7638550db..819f3441bb 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -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)); } @@ -123,16 +124,19 @@ int mavlink_pm_send_param(param_t param) param_type_t type = param_type(param); /* copy parameter name */ strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - + /* * Map onboard parameter type to MAVLink type, * 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; } @@ -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) { switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { /* Start sending parameters */ mavlink_pm_start_queued_send(); mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); } break; - case MAVLINK_MSG_ID_PARAM_SET: { + case MAVLINK_MSG_ID_PARAM_SET: { /* 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))) { /* 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)); @@ -199,25 +205,26 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } 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_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))) { - /* 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]; - 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); - } + 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 */ + 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]; + 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); } + } } break; } diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h index 146e9e15c4..b1e38bcc85 100644 --- a/apps/mavlink/mavlink_parameters.h +++ b/apps/mavlink/mavlink_parameters.h @@ -48,7 +48,7 @@ #include /** - * Handle parameter related messages. + * Handle parameter related messages. */ 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. * @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. diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 58761e89c6..79452f515e 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -106,7 +106,7 @@ handle_message(mavlink_message_t *msg) mavlink_msg_command_long_decode(msg, &cmd_mavlink); 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 if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ @@ -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); } @@ -230,7 +234,7 @@ handle_message(mavlink_message_t *msg) /* switch to a receiving link mode */ gcs_link = false; - /* + /* * rate control mode - defined by MAVLink */ @@ -238,33 +242,37 @@ handle_message(mavlink_message_t *msg) bool ml_armed = false; switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; + case 0: + ml_armed = false; + break; - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + 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; @@ -442,8 +453,8 @@ receive_start(int uart) pthread_attr_init(&receiveloop_attr); struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); diff --git a/apps/mavlink/missionlib.c b/apps/mavlink/missionlib.c index 50282a9e30..8064febc47 100644 --- a/apps/mavlink/missionlib.c +++ b/apps/mavlink/missionlib.c @@ -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,15 +182,18 @@ 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); } - + mavlink_missionlib_send_gcs_string(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); diff --git a/apps/mavlink/missionlib.h b/apps/mavlink/missionlib.h index 3439c185dd..c2ca735b3b 100644 --- a/apps/mavlink/missionlib.h +++ b/apps/mavlink/missionlib.h @@ -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 uint64_t mavlink_missionlib_get_system_timestamp(void); extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - 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 param2, float param3, float param4, float param5_lat_x, + float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 40e52a5005..f5253ea358 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -90,9 +90,8 @@ static uint64_t last_sensor_timestamp; static void *uorb_receive_thread(void *arg); -struct listener -{ - void (*callback)(struct listener *l); +struct listener { + void (*callback)(struct listener *l); int *subp; uintptr_t arg; }; @@ -185,14 +184,14 @@ l_sensor_combined(struct listener *l) if (gcs_link) 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[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.baro_pres_mbar, 0 /* no diff pressure yet */, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); + raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], + 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.baro_pres_mbar, 0 /* no diff pressure yet */, + raw.baro_alt_meter, raw.baro_temp_celcius, + fields_updated); sensors_raw_counter++; } @@ -209,13 +208,13 @@ l_vehicle_attitude(struct listener *l) if (gcs_link) /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, - last_sensor_timestamp / 1000, - att.roll, - att.pitch, - att.yaw, - att.rollspeed, - att.pitchspeed, - att.yawspeed); + last_sensor_timestamp / 1000, + att.roll, + att.pitch, + att.yaw, + att.rollspeed, + att.pitchspeed, + att.yawspeed); attitude_counter++; } @@ -291,21 +290,21 @@ l_input_rc(struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); - + if (gcs_link) /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, - 0, - (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 > 2) ? rc_raw.values[2] : 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 > 5) ? rc_raw.values[5] : UINT16_MAX, - (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, - (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, - 255); + rc_raw.timestamp / 1000, + 0, + (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 > 2) ? rc_raw.values[2] : 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 > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); } void @@ -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); @@ -343,16 +342,16 @@ l_local_position(struct listener *l) { /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - + if (gcs_link) mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, - local_pos.timestamp / 1000, - local_pos.x, - local_pos.y, - local_pos.z, - local_pos.vx, - local_pos.vy, - local_pos.vz); + local_pos.timestamp / 1000, + local_pos.x, + local_pos.y, + local_pos.z, + local_pos.vx, + local_pos.vy, + local_pos.vz); } 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); uint8_t coordinate_frame = MAV_FRAME_GLOBAL; + if (global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; if (gcs_link) mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, - coordinate_frame, - global_sp.lat, - global_sp.lon, - global_sp.altitude, - global_sp.yaw); + coordinate_frame, + global_sp.lat, + global_sp.lon, + global_sp.altitude, + global_sp.yaw); } void @@ -386,11 +386,11 @@ l_local_position_setpoint(struct listener *l) if (gcs_link) mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, - MAV_FRAME_LOCAL_NED, - local_sp.x, - local_sp.y, - local_sp.z, - local_sp.yaw); + MAV_FRAME_LOCAL_NED, + local_sp.x, + local_sp.y, + local_sp.z, + local_sp.yaw); } void @@ -403,11 +403,11 @@ 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.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); + att_sp.timestamp / 1000, + att_sp.roll_body, + att_sp.pitch_body, + att_sp.yaw_body, + att_sp.thrust); } void @@ -420,11 +420,11 @@ 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.roll, - rates_sp.pitch, - rates_sp.yaw, - rates_sp.thrust); + rates_sp.timestamp / 1000, + rates_sp.roll, + rates_sp.pitch, + rates_sp.yaw, + rates_sp.thrust); } void @@ -444,15 +444,15 @@ l_actuator_outputs(struct listener *l) if (gcs_link) { mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, - l->arg /* port number */, - act_outputs.output[0], - act_outputs.output[1], - act_outputs.output[2], - act_outputs.output[3], - act_outputs.output[4], - act_outputs.output[5], - act_outputs.output[6], - act_outputs.output[7]); + l->arg /* port number */, + act_outputs.output[0], + act_outputs.output[1], + act_outputs.output[2], + act_outputs.output[3], + act_outputs.output[4], + act_outputs.output[5], + act_outputs.output[6], + act_outputs.output[7]); /* only send in HIL mode */ if (mavlink_hil_enabled && armed.armed) { @@ -468,43 +468,46 @@ l_actuator_outputs(struct listener *l) if (mavlink_system.type == MAV_TYPE_QUADROTOR) { mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 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[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_mode, - 0); + hrt_absolute_time(), + ((act_outputs.output[0] - 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[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 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[3] - 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, - -1, - -1, - mavlink_mode, - 0); + hrt_absolute_time(), + ((act_outputs.output[0] - 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[3] - 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, + -1, + -1, + mavlink_mode, + 0); + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - ((act_outputs.output[0] - 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[3] - 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[6] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, - 0); + hrt_absolute_time(), + ((act_outputs.output[0] - 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[3] - 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[6] - 900.0f) / 600.0f) / 2.0f, + ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_mode, + 0); + } else { /* @@ -516,23 +519,24 @@ 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; } mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 600.0f, - (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, - (act_outputs.output[4] - 1500.0f) / 600.0f, - (act_outputs.output[5] - 1500.0f) / 600.0f, - (act_outputs.output[6] - 1500.0f) / 600.0f, - (act_outputs.output[7] - 1500.0f) / 600.0f, - mavlink_mode, - 0); + hrt_absolute_time(), + (act_outputs.output[0] - 1500.0f) / 600.0f, + (act_outputs.output[1] - 1500.0f) / 600.0f, + rudder, + throttle, + (act_outputs.output[4] - 1500.0f) / 600.0f, + (act_outputs.output[5] - 1500.0f) / 600.0f, + (act_outputs.output[6] - 1500.0f) / 600.0f, + (act_outputs.output[7] - 1500.0f) / 600.0f, + mavlink_mode, + 0); } } } @@ -554,12 +558,12 @@ l_manual_control_setpoint(struct listener *l) if (gcs_link) mavlink_msg_manual_control_send(MAVLINK_COMM_0, - mavlink_system.sysid, - man_control.roll * 1000, - man_control.pitch * 1000, - man_control.yaw * 1000, - man_control.throttle * 1000, - 0); + mavlink_system.sysid, + man_control.roll * 1000, + man_control.pitch * 1000, + man_control.yaw * 1000, + man_control.throttle * 1000, + 0); } void @@ -614,7 +618,7 @@ l_optical_flow(struct listener *l) 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, - 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 * @@ -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++) { diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 16759237e9..3832ebe708 100644 --- a/apps/mavlink/waypoints.c +++ b/apps/mavlink/waypoints.c @@ -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)