diff options
Diffstat (limited to 'apps/mavlink/mavlink.c')
-rw-r--r-- | apps/mavlink/mavlink.c | 1345 |
1 files changed, 91 insertions, 1254 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 0592d0377..3a1365c66 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -58,30 +58,18 @@ #include <errno.h> #include <stdlib.h> #include <poll.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/rc_channels.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_vicon_position.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> -#include <uORB/topics/vehicle_attitude_setpoint.h> -#include <uORB/topics/optical_flow.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/debug_key_value.h> #include <systemlib/param/param.h> #include <systemlib/systemlib.h> #include "waypoints.h" #include "mavlink_log.h" +#include "orb_topics.h" +#include "missionlib.h" +#include "mavlink_hil.h" +#include "util.h" +#include "waypoints.h" +#include "mavlink_parameters.h" /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); @@ -89,63 +77,47 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_QUADROTOR); __EXPORT int mavlink_main(int argc, char *argv[]); -int mavlink_thread_main(int argc, char *argv[]); - -static bool thread_should_exit = false; -static bool thread_running = false; -static int mavlink_task; -/* terminate MAVLink on user request - disabled by default */ -static bool mavlink_link_termination_allowed = false; +static int mavlink_thread_main(int argc, char *argv[]); -mavlink_system_t mavlink_system = {100, 50, MAV_TYPE_QUADROTOR, 0, 0, 0}; // System ID, 1-255, Component/Subsystem ID, 1-255 -static uint8_t chan = MAVLINK_COMM_0; -static mavlink_status_t status; +/* thread state */ +volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int mavlink_task; /* pthreads */ static pthread_t receive_thread; static pthread_t uorb_receive_thread; -/* Allocate storage space for waypoints */ -mavlink_wpm_storage wpm_s; - -/** Global position */ -static struct vehicle_global_position_s global_pos; - -/** Local position */ -static struct vehicle_local_position_s local_pos; - -/** Vehicle status */ -static struct vehicle_status_s v_status; - -/** RC channels */ -static struct rc_channels_s rc; - -/* HIL publishers */ -static orb_advert_t pub_hil_attitude = -1; - -/** HIL attitude */ -static struct vehicle_attitude_s hil_attitude; - -static struct vehicle_global_position_s hil_global_pos; +/* terminate MAVLink on user request - disabled by default */ +static bool mavlink_link_termination_allowed = false; -static struct offboard_control_setpoint_s offboard_control_sp; +mavlink_system_t mavlink_system = { + 100, + 50, + MAV_TYPE_QUADROTOR, + 0, + 0, + 0 +}; // System ID, 1-255, Component/Subsystem ID, 1-255 -static struct vehicle_command_s vcmd; +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; -static struct actuator_armed_s armed; +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); -static struct vehicle_vicon_position_s vicon_position; +/* Allocate storage space for waypoints */ +static mavlink_wpm_storage wpm_s; +mavlink_wpm_storage *wpm = &wpm_s; -static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; -static orb_advert_t global_position_setpoint_pub = -1; -static orb_advert_t local_position_setpoint_pub = -1; -static bool mavlink_hil_enabled = false; +bool mavlink_hil_enabled = false; +/* buffer for message strings */ static char mavlink_message_string[51] = {0}; +/* protocol interface */ +static int uart; static int baudrate = 57600; /* interface mode */ @@ -154,207 +126,19 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; -static struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int armed_sub; - int actuators_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int spg_sub; - int debug_key_value; - bool initialized; -} mavlink_subs = { - .sensor_sub = 0, - .att_sub = 0, - .global_pos_sub = 0, - .act_0_sub = 0, - .act_1_sub = 0, - .act_2_sub = 0, - .act_3_sub = 0, - .gps_sub = 0, - .man_control_sp_sub = 0, - .armed_sub = 0, - .actuators_sub = 0, - .local_pos_sub = 0, - .spa_sub = 0, - .spl_sub = 0, - .spg_sub = 0, - .debug_key_value = 0, - .initialized = false -}; - -static struct mavlink_publications { - orb_advert_t offboard_control_sp_pub; - orb_advert_t vicon_position_pub; -} mavlink_pubs = { - .offboard_control_sp_pub = -1, - .vicon_position_pub = -1 -}; - - -/* 3: Define waypoint helper functions */ -void mavlink_wpm_send_message(mavlink_message_t *msg); -void mavlink_wpm_send_gcs_string(const char *string); -uint64_t mavlink_wpm_get_system_timestamp(void); -int mavlink_missionlib_send_message(mavlink_message_t *msg); -int mavlink_missionlib_send_gcs_string(const char *string); -uint64_t mavlink_missionlib_get_system_timestamp(void); - -void handleMessage(mavlink_message_t *msg); static void mavlink_update_system(void); - -/** - * Enable / disable Hardware in the Loop simulation mode. - */ -int set_hil_on_off(bool hil_enabled); - -/** - * Translate the custom state into standard mavlink modes and state. - */ -void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const struct actuator_armed_s *actuator, uint8_t *mavlink_state, uint8_t *mavlink_mode); - -int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - -/* 4: Include waypoint protocol */ -#include "waypoints.h" -mavlink_wpm_storage *wpm; +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -#include "mavlink_parameters.h" - /** * Print the usage */ static void usage(const char *reason); -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -int mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - int writelen = write(uart, missionlib_msg_buf, len); - if (writelen != len) { - return 1; - } else { - return 0; - } -} - -int mavlink_missionlib_send_gcs_string(const char *string) -{ - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') - break; - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - return mavlink_missionlib_send_message(&msg); - } else { - return 1; - } -} - -/** - * Get system time since boot in microseconds - * - * @return the system time since boot in microseconds - */ -uint64_t mavlink_missionlib_get_system_timestamp() -{ - return hrt_absolute_time(); -} - -/** - * This callback is executed each time a waypoint changes. - * - * It publishes the vehicle_global_position_setpoint_s or the - * vehicle_local_position_setpoint_s topic, depending on the type of waypoint - */ -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) -{ - char buf[50] = {0}; - - /* Update controller setpoints */ - if (frame == (int)MAV_FRAME_GLOBAL) { - /* global, absolute waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - 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) { - /* global, relative alt (in relation to HOME) waypoint */ - struct vehicle_global_position_setpoint_s sp; - sp.lat = param5_lat_x * 1e7f; - sp.lon = param6_lon_y * 1e7f; - 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) { - /* local, absolute waypoint */ - struct vehicle_local_position_setpoint_s sp; - sp.x = param5_lat_x; - 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); -} - -int set_hil_on_off(bool hil_enabled) +int +set_hil_on_off(bool hil_enabled) { int ret = OK; @@ -363,9 +147,6 @@ int set_hil_on_off(bool hil_enabled) //printf("\n HIL ON \n"); - (void)close(pub_hil_attitude); - (void)close(pub_hil_global_pos); - /* Advertise topics */ pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); @@ -401,8 +182,6 @@ int set_hil_on_off(bool hil_enabled) if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; orb_set_interval(mavlink_subs.spa_sub, 200); - (void)close(pub_hil_attitude); - (void)close(pub_hil_global_pos); } else { ret = ERROR; @@ -411,32 +190,33 @@ int set_hil_on_off(bool hil_enabled) return ret; } -void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const struct actuator_armed_s *actuator, uint8_t *mavlink_state, uint8_t *mavlink_mode) +void +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (c_status->flag_control_manual_enabled) { + if (v_status.flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (c_status->flag_hil_enabled) { + if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (actuator->armed) { + if (armed.armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (c_status->state_machine) { + switch (v_status.state_machine) { case SYSTEM_STATE_PREFLIGHT: - if (c_status->flag_preflight_gyro_calibration || - c_status->flag_preflight_mag_calibration || - c_status->flag_preflight_accel_calibration) { + if (v_status.flag_preflight_gyro_calibration || + v_status.flag_preflight_mag_calibration || + v_status.flag_preflight_accel_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; } else { *mavlink_state = MAV_STATE_UNINIT; @@ -489,47 +269,6 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, const s } -/** - * Receive data from UART. - */ -static void *receiveloop(void *arg) -{ - int uart_fd = *((int*)arg); - - const int timeout = 1000; - uint8_t ch; - - mavlink_message_t msg; - - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); - - while (!thread_should_exit) { - - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - - if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; - - do { - nread = read(uart_fd, &ch, 1); - - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char - /* handle generic messages and commands */ - handleMessage(&msg); - - /* Handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - - /* Handle packet with parameter component */ - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); - } - } while (nread > 0); - } - } - - return NULL; -} static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { @@ -537,35 +276,35 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma switch (mavlink_msg_id) { case MAVLINK_MSG_ID_SCALED_IMU: - /* senser sub triggers scaled IMU */ - if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); + /* sensor sub triggers scaled IMU */ + orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_HIGHRES_IMU: - /* senser sub triggers highres IMU */ - if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); + /* sensor sub triggers highres IMU */ + orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_RAW_IMU: - /* senser sub triggers RAW IMU */ - if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); + /* sensor sub triggers RAW IMU */ + orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_ATTITUDE: /* attitude sub triggers attitude */ - if (subs->att_sub) orb_set_interval(subs->att_sub, min_interval); + orb_set_interval(subs->att_sub, min_interval); break; case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: /* actuator_outputs triggers this message */ - if (subs->act_0_sub) orb_set_interval(subs->act_0_sub, min_interval); - if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval); - if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval); - if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval); - if (subs->actuators_sub) orb_set_interval(subs->actuators_sub, min_interval); + 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 */ - if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval); + orb_set_interval(subs->man_control_sp_sub, min_interval); break; case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - if (subs->debug_key_value) orb_set_interval(subs->debug_key_value, min_interval); + orb_set_interval(subs->debug_key_value, min_interval); break; default: /* not found */ @@ -576,546 +315,6 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma return ret; } -/** - * Listen for uORB topics and send via MAVLink. - * - * This pthread performs a blocking wait on selected - * uORB topics and sends them via MAVLink to other - * vehicles or a ground control station. - */ -static void *uorb_receiveloop(void *arg) -{ - /* obtain reference to task's subscriptions */ - struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg; - - /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); - - - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 25; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - - - union { - struct sensor_combined_s raw; - struct vehicle_attitude_s att; - struct vehicle_gps_position_s gps; - struct vehicle_local_position_setpoint_s local_sp; - struct vehicle_global_position_setpoint_s global_sp; - struct vehicle_attitude_setpoint_s att_sp; - struct actuator_outputs_s act_outputs; - struct manual_control_setpoint_s man_control; - struct actuator_controls_s actuators; - struct debug_key_value_s debug; - } buf; - - /* --- SENSORS RAW VALUE --- */ - /* subscribe to ORB for sensors raw */ - subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = subs->sensor_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE VALUE --- */ - /* subscribe to ORB for attitude */ - subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(subs->att_sub, 100); - fds[fdsc_count].fd = subs->att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS VALUE --- */ - /* subscribe to ORB for attitude */ - subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(subs->gps_sub, 1000); /* 1Hz updates */ - fds[fdsc_count].fd = subs->gps_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- SYSTEM STATE --- */ - /* struct already globally allocated */ - /* subscribe to topic */ - int status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ - fds[fdsc_count].fd = status_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- RC CHANNELS VALUE --- */ - /* struct already globally allocated */ - /* subscribe to ORB for global position */ - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(rc_sub, 100); /* 10Hz updates */ - fds[fdsc_count].fd = rc_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POS VALUE --- */ - /* struct already globally allocated and topic already subscribed */ - fds[fdsc_count].fd = subs->global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POS VALUE --- */ - /* struct and topic already globally subscribed */ - fds[fdsc_count].fd = subs->local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL SETPOINT VALUE --- */ - /* subscribe to ORB for local setpoint */ - /* struct already allocated */ - subs->spg_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - orb_set_interval(subs->spg_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spg_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL SETPOINT VALUE --- */ - /* subscribe to ORB for local setpoint */ - /* struct already allocated */ - subs->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(subs->spl_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spl_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT VALUE --- */ - /* subscribe to ORB for attitude setpoint */ - /* struct already allocated */ - subs->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(subs->spa_sub, 2000); /* 0.5 Hz updates */ - fds[fdsc_count].fd = subs->spa_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- ACTUATOR OUTPUTS --- */ - subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - fds[fdsc_count].fd = subs->act_0_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - fds[fdsc_count].fd = subs->act_1_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - fds[fdsc_count].fd = subs->act_2_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); - fds[fdsc_count].fd = subs->act_3_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /** --- MAPPED MANUAL CONTROL INPUTS --- */ - subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - fds[fdsc_count].fd = subs->man_control_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR ARMED VALUE --- */ - /* subscribe to ORB for actuator armed */ - subs->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - fds[fdsc_count].fd = subs->armed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL VALUE --- */ - /* subscribe to ORB for actuator control */ - subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs->actuators_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- DEBUG VALUE OUTPUT --- */ - /* subscribe to ORB for debug value outputs */ - subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - fds[fdsc_count].fd = subs->debug_key_value; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* all subscriptions initialized, return success */ - subs->initialized = true; - - unsigned int sensors_raw_counter = 0; - unsigned int attitude_counter = 0; - unsigned int gps_counter = 0; - - /* WARNING: If you get the error message below, - * then the number of registered messages (fdsc) - * differs from the number of messages in the above list. - */ - if (fdsc_count > fdsc) { - fprintf(stderr, "[mavlink] WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; - - /* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ - uint64_t last_sensor_timestamp = 0; - - while (!thread_should_exit) { - - int poll_ret = poll(fds, fdsc_count, timeout); - - /* 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 { - - int ifds = 0; - - /* --- SENSORS RAW VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw); - - last_sensor_timestamp = buf.raw.timestamp; - - /* send raw imu data */ - // mavlink_msg_raw_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, buf.raw.accelerometer_raw[0], - // buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], - // buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], - // buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); - - /* mark individual fields as changed */ - uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; - - if (accel_counter != buf.raw.accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = buf.raw.accelerometer_counter; - } - - if (gyro_counter != buf.raw.gyro_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = buf.raw.gyro_counter; - } - - if (mag_counter != buf.raw.magnetometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = buf.raw.magnetometer_counter; - } - - if (baro_counter != buf.raw.baro_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = buf.raw.baro_counter; - } - - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, - buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], - buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0], - buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2], - buf.raw.magnetometer_ga[0], - buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2], - buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */, - buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius, - fields_updated); - /* send pressure */ - //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100); - - sensors_raw_counter++; - } - - /* --- ATTITUDE VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att); - - /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); - - attitude_counter++; - } - - /* --- GPS VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps); - /* GPS position */ - mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible); - - if (buf.gps.satellite_info_available && (gps_counter % 4 == 0)) { - mavlink_msg_gps_status_send(MAVLINK_COMM_0, buf.gps.satellites_visible, buf.gps.satellite_prn, buf.gps.satellite_used, buf.gps.satellite_elevation, buf.gps.satellite_azimuth, buf.gps.satellite_snr); - } - - gps_counter++; - } - - /* --- SYSTEM STATUS --- */ - if (fds[ifds++].revents & POLLIN) { - /* immediately communicate state changes back to user */ - orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); - /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); - } - - /* --- RC CHANNELS --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw, - rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi); - } - - /* --- VEHICLE GLOBAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos); - 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 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); - int16_t vz = (int16_t)(global_pos.vz * 100.0f); - /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); - - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, - relative_alt, vx, vy, vz, hdg); - } - - /* --- VEHICLE LOCAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), subs->local_pos_sub, &local_pos); - 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); - } - - /* --- VEHICLE GLOBAL SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position_setpoint), subs->spg_sub, &buf.global_sp); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - if (buf.global_sp.altitude_is_relative) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, coordinate_frame, buf.global_sp.lat, - buf.global_sp.lon, buf.global_sp.altitude, buf.global_sp.yaw); - } - - /* --- VEHICLE LOCAL SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), subs->spl_sub, &buf.local_sp); - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, - buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw); - } - - /* --- VEHICLE ATTITUDE SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs->spa_sub, &buf.att_sp); - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, - buf.att_sp.roll_body, buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust); - } - - /* --- ACTUATOR OUTPUTS 0 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 0 /* port 0 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - - // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), - // 1 /* port 1 */, - // buf.act_outputs.output[ 8], - // buf.act_outputs.output[ 9], - // buf.act_outputs.output[10], - // buf.act_outputs.output[11], - // buf.act_outputs.output[12], - // buf.act_outputs.output[13], - // buf.act_outputs.output[14], - // buf.act_outputs.output[15]); - // } - } - - /* --- ACTUATOR OUTPUTS 1 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 3 /* port 3 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- ACTUATOR OUTPUTS 2 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 5 /* port 5 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- ACTUATOR OUTPUTS 3 --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy actuator data into local buffer */ - orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */, - buf.act_outputs.output[0], - buf.act_outputs.output[1], - buf.act_outputs.output[2], - buf.act_outputs.output[3], - buf.act_outputs.output[4], - buf.act_outputs.output[5], - buf.act_outputs.output[6], - buf.act_outputs.output[7]); - if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp/1000, - 7 /* port 7 */, - buf.act_outputs.output[ 8], - buf.act_outputs.output[ 9], - buf.act_outputs.output[10], - buf.act_outputs.output[11], - buf.act_outputs.output[12], - buf.act_outputs.output[13], - buf.act_outputs.output[14], - buf.act_outputs.output[15]); - } - } - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - if (fds[ifds++].revents & POLLIN) { - /* copy local position data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000, - buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0); - } - - /* --- ACTUATOR ARMED --- */ - if (fds[ifds++].revents & POLLIN) { - } - - /* --- ACTUATOR CONTROL --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators); - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl0 ", buf.actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl1 ", buf.actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl2 ", buf.actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "ctrl3 ", buf.actuators.control[3]); - - /* Only send in HIL mode */ - if (mavlink_hil_enabled) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - - /* HIL message as per MAVLink spec */ - mavlink_msg_hil_controls_send(chan, - hrt_absolute_time(), - buf.actuators.control[0], - buf.actuators.control[1], - buf.actuators.control[2], - buf.actuators.control[3], - 0, - 0, - 0, - 0, - mavlink_mode, - 0); - } - } - - /* --- DEBUG KEY/VALUE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug); - /* Enforce null termination */ - buf.debug.key[sizeof(buf.debug.key) - 1] = '\0'; - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, buf.debug.key, buf.debug.value); - } - } - } - - return NULL; -} /**************************************************************************** * MAVLink text message logger @@ -1152,349 +351,6 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) /**************************************************************************** * Public Functions ****************************************************************************/ -void handleMessage(mavlink_message_t *msg) -{ - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { - - mavlink_command_long_t 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) - || (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 */ - printf("[mavlink] Terminating .. \n"); - fflush(stdout); - usleep(50000); - - /* terminate other threads and this thread */ - thread_should_exit = true; - - } else { - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - vcmd.command = cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; - - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - } - /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } - } - - if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); - - struct optical_flow_s f; - - f.timestamp = flow.time_usec; - f.flow_raw_x = flow.flow_x; - f.flow_raw_y = flow.flow_y; - f.flow_comp_x_m = flow.flow_comp_m_x; - f.flow_comp_y_m = flow.flow_comp_m_y; - f.ground_distance_m = flow.ground_distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - - /* 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); - } - } - - if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { - /* Set mode on request */ - mavlink_set_mode_t new_mode; - mavlink_msg_set_mode_decode(msg, &new_mode); - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = MAV_CMD_DO_SET_MODE; - vcmd.target_system = new_mode.target_system; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - /* 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); - } - } - - /* Handle Vicon position estimates */ - if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); - - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - - if (mavlink_pubs.vicon_position_pub <= 0) { - mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - } else { - orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position); - } - } - - /* Handle quadrotor motor setpoints */ - - if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - //printf("got message\n"); - //printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode); - - if (mavlink_system.sysid < 4) { - /* - * rate control mode - defined by MAVLink - */ - - uint8_t ml_mode = 0; - bool ml_armed = false; - -// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) { -// ml_armed = true; -// } - - switch (quad_motors_setpoint.mode) { - 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 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX; - //offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ; - - if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){ - ml_armed = false; - - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - /* check if topic has to be advertised */ - if (mavlink_pubs.offboard_control_sp_pub <= 0) { - mavlink_pubs.offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - } else { - /* Publish */ - orb_publish(ORB_ID(offboard_control_setpoint), mavlink_pubs.offboard_control_sp_pub, &offboard_control_sp); - } - - // /* change armed status if required */ - // bool cmd_armed = (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED); - - // bool cmd_generated = false; - - // if (v_status.flag_control_offboard_enabled != cmd_armed) { - // vcmd.param1 = cmd_armed; - // vcmd.param2 = 0; - // vcmd.param3 = 0; - // vcmd.param4 = 0; - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = MAV_CMD_COMPONENT_ARM_DISARM; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // /* check if input has to be enabled */ - // if ((v_status.flag_control_rates_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES)) || - // (v_status.flag_control_attitude_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE)) || - // (v_status.flag_control_velocity_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY)) || - // (v_status.flag_control_position_enabled != (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION))) { - // vcmd.param1 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_RATES); - // vcmd.param2 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_ATTITUDE); - // vcmd.param3 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_VELOCITY); - // vcmd.param4 = (quad_motors_setpoint.mode == MAVLINK_OFFBOARD_CONTROL_MODE_POSITION); - // vcmd.param5 = 0; - // vcmd.param6 = 0; - // vcmd.param7 = 0; - // vcmd.command = PX4_CMD_CONTROLLER_SELECTION; - // vcmd.target_system = mavlink_system.sysid; - // vcmd.target_component = MAV_COMP_ID_ALL; - // vcmd.source_system = msg->sysid; - // vcmd.source_component = msg->compid; - // vcmd.confirmation = 1; - - // cmd_generated = true; - // } - - // if (cmd_generated) { - // /* 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); - // } - // } - } - } - - /* - * Only decode hil messages in HIL mode. - * - * The HIL mode is enabled by the HIL bit flag - * in the system mode. Either send a set mode - * COMMAND_LONG message or a SET_MODE message - */ - - // printf("\n HIL ENABLED?: %s \n",(mavlink_hil_enabled)?"true":"false"); - - if (mavlink_hil_enabled) { - - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) { - - mavlink_hil_state_t hil_state; - mavlink_msg_hil_state_decode(msg, &hil_state); - - // printf("\n HILSTATE : \n LAT: %i \n LON: %i \n ALT: %i \n " - // "ROLL %i \n PITCH %i \n YAW %i \n" - // "ROLLSPEED: %i \n PITCHSPEED: %i \n, YAWSPEED: %i \n", - // hil_state.lat/1000000, // 1e7 - // hil_state.lon/1000000, // 1e7 - // hil_state.alt/1000, // mm - // hil_state.roll, // float rad - // hil_state.pitch, // float rad - // hil_state.yaw, // float rad - // hil_state.rollspeed, // float rad/s - // hil_state.pitchspeed, // float rad/s - // hil_state.yawspeed); // float rad/s - - - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; - - /* set timestamp and notify processes (broadcast) */ - hil_global_pos.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); - - hil_attitude.roll = hil_state.roll; - hil_attitude.pitch = hil_state.pitch; - hil_attitude.yaw = hil_state.yaw; - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - - /* set timestamp and notify processes (broadcast) */ - hil_attitude.counter++; - hil_attitude.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); - } - - if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - struct rc_channels_s rc_hil; - memset(&rc_hil, 0, sizeof(rc_hil)); - static orb_advert_t rc_pub = 0; - - rc_hil.timestamp = hrt_absolute_time(); - rc_hil.chan_count = 4; - rc_hil.chan[0].raw = 1500 + man.x / 2; - rc_hil.chan[1].raw = 1500 + man.y / 2; - rc_hil.chan[2].raw = 1500 + man.r / 2; - rc_hil.chan[3].raw = 1500 + man.z / 2; - - rc_hil.chan[0].scaled = man.x / 1000.0f; - rc_hil.chan[1].scaled = man.y / 1000.0f; - rc_hil.chan[2].scaled = man.r / 1000.0f; - rc_hil.chan[3].scaled = man.z / 1000.0f; - - struct manual_control_setpoint_s mc; - static orb_advert_t mc_pub = 0; - - mc.timestamp = rc_hil.timestamp; - mc.roll = man.x / 1000.0f; - mc.pitch = man.y / 1000.0f; - mc.yaw = man.r / 1000.0f; - mc.throttle = man.z / 1000.0f; - - /* fake RC channels with manual control input from simulator */ - - - 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); - } - } - } -} int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { @@ -1593,6 +449,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf return uart; } +void +mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) +{ + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); +} + void mavlink_update_system(void) { static bool initialized = false; @@ -1631,22 +493,10 @@ void mavlink_update_system(void) */ int mavlink_thread_main(int argc, char *argv[]) { - wpm = &wpm_s; - - /* initialize global data structs */ - memset(&global_pos, 0, sizeof(global_pos)); - memset(&local_pos, 0, sizeof(local_pos)); - memset(&v_status, 0, sizeof(v_status)); - memset(&rc, 0, sizeof(rc)); - memset(&hil_attitude, 0, sizeof(hil_attitude)); - memset(&hil_global_pos, 0, sizeof(hil_global_pos)); - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - memset(&vcmd, 0, sizeof(vcmd)); - /* print welcome text */ printf("[mavlink] MAVLink v1.0 serial interface starting..\n"); - /* reate the device node that's used for sending text log messages, etc. */ + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); /* default values for arguments */ @@ -1704,40 +554,19 @@ int mavlink_thread_main(int argc, char *argv[]) /* Initialize system properties */ mavlink_update_system(); - /* topics to subscribe globally */ - /* subscribe to ORB for global position */ - mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ - /* subscribe to ORB for local position */ - mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ - + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - pthread_attr_setstacksize(&receiveloop_attr, 2048); - pthread_create(&receive_thread, &receiveloop_attr, receiveloop, &uart); - - pthread_attr_t uorb_attr; - pthread_attr_init(&uorb_attr); - /* Set stack size, needs more than 8000 bytes */ - pthread_attr_setstacksize(&uorb_attr, 8192); - pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, &mavlink_subs); + /* start the ORB receiver */ + uorb_receive_thread = uorb_receive_start(); /* initialize waypoint manager */ mavlink_wpm_init(wpm); uint16_t counter = 0; - /* make sure all threads have registered their subscriptions */ - while (!mavlink_subs.initialized) { - usleep(500); - } - /* all subscriptions are now active, set up initial guess about rate limits */ - if (baudrate >= 460800) { - /* 200 Hz / 5 ms */ - } else if (baudrate >= 230400) { + if (baudrate >= 230400) { /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); @@ -1791,19 +620,14 @@ int mavlink_thread_main(int argc, char *argv[]) while (!thread_should_exit) { - /* get local and global position */ - orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); - /* 1 Hz */ if (lowspeed_counter == 10) { mavlink_update_system(); - /* translate the current syste state to mavlink state and mode */ + /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); @@ -1812,10 +636,20 @@ int mavlink_thread_main(int argc, char *argv[]) set_hil_on_off(v_status.flag_hil_enabled); /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, - v_status.onboard_control_sensors_health, v_status.load, v_status.voltage_battery * 1000.f, v_status.current_battery * 1000.f, - v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, - v_status.errors_count1, v_status.errors_count2, v_status.errors_count3, v_status.errors_count4); + mavlink_msg_sys_status_send(chan, + v_status.onboard_control_sensors_present, + v_status.onboard_control_sensors_enabled, + v_status.onboard_control_sensors_health, + v_status.load, + v_status.voltage_battery * 1000.0f, + v_status.current_battery * 1000.0f, + v_status.battery_remaining, + v_status.drop_rate_comm, + v_status.errors_comm, + v_status.errors_count1, + v_status.errors_count2, + v_status.errors_count3, + v_status.errors_count4); lowspeed_counter = 0; } lowspeed_counter++; @@ -1831,6 +665,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); + /* sleep quarter the time */ usleep(25000); if (baudrate > 57600) { @@ -1841,8 +676,10 @@ int mavlink_thread_main(int argc, char *argv[]) usleep(10000); /* send one string at 10 Hz */ - mavlink_missionlib_send_gcs_string(mavlink_message_string); - mavlink_message_string[0] = '\0'; + if (mavlink_message_string[0] != '\0') { + mavlink_missionlib_send_gcs_string(mavlink_message_string); + mavlink_message_string[0] = '\0'; + } counter++; /* sleep 15 ms */ |