diff options
| -rw-r--r-- | src/modules/commander/px4_custom_mode.h | 2 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 534 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_main.h | 102 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 724 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_messages.h | 15 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_orb_listener.cpp | 829 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_orb_listener.h | 163 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_orb_subscription.cpp | 54 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_orb_subscription.h | 34 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 2 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_stream.cpp | 51 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_stream.h | 42 | ||||
| -rw-r--r-- | src/modules/mavlink/module.mk | 4 |
13 files changed, 1107 insertions, 1449 deletions
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..2144d3460 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,6 +8,8 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ +#include <stdint.h> + enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_SEATBELT, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ce467a7b..33d81729c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -79,11 +79,9 @@ #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include <commander/px4_custom_mode.h> - #include "mavlink_bridge_header.h" #include "mavlink_main.h" -#include "mavlink_orb_listener.h" +#include "mavlink_messages.h" #include "mavlink_receiver.h" /* oddly, ERROR is not defined for c++ */ @@ -92,6 +90,8 @@ #endif static const int ERROR = -1; +#define MAIN_LOOP_DELAY 10000 // 100 Hz + static Mavlink* _head = nullptr; /* TODO: if this is a class member it crashes */ @@ -152,12 +152,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -namespace mavlink -{ - - Mavlink *g_mavlink; -} - Mavlink::Mavlink() : device_name("/dev/ttyS1"), _task_should_exit(false), @@ -166,13 +160,13 @@ Mavlink::Mavlink() : thread_running(false), _mavlink_task(-1), _mavlink_incoming_fd(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _mavlink_hil_enabled(false), - // _params_sub(-1) + _subscriptions(nullptr), + _streams(nullptr), + mission_pub(-1), - mission_pub(-1) +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { wpm = &wpm_s; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; @@ -203,12 +197,14 @@ Mavlink::~Mavlink() } } -void Mavlink::set_mode(enum MAVLINK_MODE mode) +void +Mavlink::set_mode(enum MAVLINK_MODE mode) { _mode = mode; } -int Mavlink::instance_count() +int +Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ Mavlink* inst = ::_head; @@ -221,10 +217,11 @@ int Mavlink::instance_count() return inst_index; } -Mavlink* Mavlink::new_instance() +Mavlink * +Mavlink::new_instance() { - Mavlink* inst = new Mavlink(); - Mavlink* next = ::_head; + Mavlink *inst = new Mavlink(); + Mavlink *next = ::_head; /* create the first instance at _head */ if (::_head == nullptr) { @@ -240,9 +237,10 @@ Mavlink* Mavlink::new_instance() return inst; } -Mavlink* Mavlink::get_instance(unsigned instance) +Mavlink * +Mavlink::get_instance(unsigned instance) { - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; unsigned inst_index = 0; while (inst->_next != nullptr && inst_index < instance) { inst = inst->_next; @@ -256,7 +254,8 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } -int Mavlink::destroy_all_instances() +int +Mavlink::destroy_all_instances() { /* start deleting from the end */ Mavlink *inst_to_del = nullptr; @@ -293,7 +292,8 @@ int Mavlink::destroy_all_instances() return OK; } -bool Mavlink::instance_exists(const char *device_name, Mavlink *self) +bool +Mavlink::instance_exists(const char *device_name, Mavlink *self) { Mavlink* inst = ::_head; while (inst != nullptr) { @@ -306,7 +306,8 @@ bool Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } -int Mavlink::get_uart_fd(unsigned index) +int +Mavlink::get_uart_fd(unsigned index) { Mavlink* inst = get_instance(index); if (inst) @@ -315,25 +316,16 @@ int Mavlink::get_uart_fd(unsigned index) return -1; } -int Mavlink::get_uart_fd() +int +Mavlink::get_uart_fd() { return _uart; } -int Mavlink::get_channel() +mavlink_channel_t +Mavlink::get_channel() { - return (int)_chan; -} - -void -Mavlink::parameters_update() -{ - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - // param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); - + return _chan; } /**************************************************************************** @@ -532,155 +524,16 @@ Mavlink::set_hil_enabled(bool hil_enabled) hil_rate_interval = 5; } - orb_set_interval(subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); +// orb_set_interval(subs.spa_sub, hil_rate_interval); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && _mavlink_hil_enabled) { _mavlink_hil_enabled = false; - orb_set_interval(subs.spa_sub, 200); - - } else { - ret = ERROR; - } - - return ret; -} - -void -Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_base_mode = 0; - *mavlink_custom_mode = 0; +// orb_set_interval(subs.spa_sub, 200); - /** - * Set mode flags - **/ - - /* HIL */ - if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* arming state */ - if (v_status.arming_state == ARMING_STATE_ARMED - || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } - - /* main state */ - *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet.nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } - } - *mavlink_custom_mode = custom_mode.data; - - /** - * Set mavlink state - **/ - - /* set calibration state */ - if (v_status.arming_state == ARMING_STATE_INIT - || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE - || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - *mavlink_state = MAV_STATE_UNINIT; - } else if (v_status.arming_state == ARMING_STATE_ARMED) { - *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_state = MAV_STATE_CRITICAL; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { - *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_REBOOT) { - *mavlink_state = MAV_STATE_POWEROFF; } else { - warnx("Unknown mavlink state"); - *mavlink_state = MAV_STATE_CRITICAL; - } -} - - -int Mavlink::set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) -{ - 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); - orb_set_interval(subs.actuators_effective_sub, min_interval); - orb_set_interval(subs.spa_sub, min_interval); - orb_set_interval(subs.rates_setpoint_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; @@ -688,18 +541,6 @@ int Mavlink::set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) extern mavlink_system_t mavlink_system; -void Mavlink::mavlink_pm_callback(void *arg, param_t param) -{ - //mavlink_pm_send_param(param); - usleep(*(unsigned int *)arg); -} - -void Mavlink::mavlink_pm_send_all_params(unsigned int delay) -{ - unsigned int dbuf = delay; - param_foreach(&mavlink_pm_callback, &dbuf, false); -} - int Mavlink::mavlink_pm_queued_send() { if (mavlink_param_queue_index < param_count()) { @@ -1519,11 +1360,49 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size) +{ + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; + + LL_FOREACH(_subscriptions, sub) { + if (sub->get_topic() == topic) { + /* already subscribed */ + return sub; + } + } + + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; +} + +int +Mavlink::add_stream(const char *stream_name, const float rate) +{ + /* search for stream with specified name */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create stream copy for each mavlink instance */ + MavlinkStream *stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(1000.0f / rate); + stream->subscribe(this); + LL_APPEND(_streams, stream); + return OK; + } + } + return ERROR; +} + int Mavlink::task_main(int argc, char *argv[]) { /* inform about start */ - warnx("Initializing.."); + warnx("start"); fflush(stdout); /* initialize mavlink text message buffering */ @@ -1579,9 +1458,6 @@ Mavlink::task_main(int argc, char *argv[]) bool usb_uart; - /* print welcome text */ - warnx("MAVLink v1.0 serial interface starting..."); - /* inform about mode */ switch (_mode) { case MODE_TX_HEARTBEAT_ONLY: @@ -1626,81 +1502,22 @@ Mavlink::task_main(int argc, char *argv[]) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + if (instance_count() == 1) { + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + } /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* Initialize system properties */ mavlink_update_system(); /* start the MAVLink receiver */ receive_thread = MavlinkReceiver::receive_start(this); - /* start the ORB receiver */ - uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); - /* initialize waypoint manager */ mavlink_wpm_init(wpm); - /* all subscriptions are now active, set up initial guess about rate limits */ - if (_baudrate >= 230400) { - /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 10 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); - /* 10 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); - - } else if (_baudrate >= 115200) { - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - - } else if (_baudrate >= 57600) { - /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); - /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); - - } else { - /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); - /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); - /* 0.5 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); - /* 0.1 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); - } - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); @@ -1709,72 +1526,58 @@ Mavlink::task_main(int argc, char *argv[]) unsigned lowspeed_counter = 0; - /* wakeup source(s) */ - struct pollfd fds[1]; - - _params_sub = orb_subscribe(ORB_ID(parameter_update)); + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); - /* Setup of loop */ - fds[0].fd = _params_sub; - fds[0].events = POLLIN; + struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); - while (!_task_should_exit) { + warnx("started"); + mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + /* add default streams, intervals depend on baud rate */ +// if (_baudrate >= 230400) { +// } else if (_baudrate >= 115200) { +// } else if (_baudrate >= 57600) { +// } + + add_stream("HEARTBEAT", 1.0f); + add_stream("SYS_STATUS", 1.0f); + add_stream("HIGHRES_IMU", 20.0f); +// add_stream("RAW_IMU", 10.0f); + add_stream("ATTITUDE", 20.0f); +// add_stream("NAMED_VALUE_FLOAT", 5.0f); +// add_stream("SERVO_OUTPUT_RAW", 2.0f); +// add_stream("GPS_RAW_INT", 2.0f); +// add_stream("MANUAL_CONTROL", 2.0f); - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } + while (!_task_should_exit) { + /* main loop */ + usleep(MAIN_LOOP_DELAY); perf_begin(_loop_perf); - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); - } + hrt_abstime t = hrt_absolute_time(); - /* 1 Hz */ - if (lowspeed_counter % 10 == 0) { + if (param_sub->update(t)) { + /* parameters updated */ mavlink_update_system(); + } - /* translate the current system state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); - + if (status_sub->update(t)) { /* switch HIL mode if required */ - if (v_status.hil_state == HIL_STATE_ON) + if (status->hil_state == HIL_STATE_ON) set_hil_enabled(true); - else if (v_status.hil_state == HIL_STATE_OFF) + else if (status->hil_state == HIL_STATE_OFF) set_hil_enabled(false); + } - /* 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 * 1000.0f, - v_status.battery_voltage * 1000.0f, - v_status.battery_current * 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); + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); } /* 0.5 Hz */ - if (lowspeed_counter % 20 == 0) { - + if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) { mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } @@ -1795,38 +1598,31 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(hrt_absolute_time()); - + if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { + /* send parameters at 20 Hz (if queued for sending) */ + mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(hrt_absolute_time()); - /* send parameters at 20 Hz (if queued for sending) */ - mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); + if (_baudrate > 57600) { + mavlink_pm_queued_send(); + } - mavlink_waypoint_eventloop(hrt_absolute_time()); + /* send one string at 20 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); - if (_baudrate > 57600) { - mavlink_pm_queued_send(); + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } } - /* send one string at 10 Hz */ - 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); - } - } - perf_end(_loop_perf); } /* wait for threads to complete */ pthread_join(receive_thread, NULL); - pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ tcsetattr(_uart, TCSANOW, &uart_config_original); @@ -1844,15 +1640,50 @@ Mavlink::task_main(int argc, char *argv[]) int Mavlink::start_helper(int argc, char *argv[]) { - // Create the instance in task context + /* create the instance in task context */ Mavlink *instance = Mavlink::new_instance(); - // This will actually only return once MAVLink exits + /* this will actually only return once MAVLink exits */ return instance->task_main(argc, argv); } int -Mavlink::start() +Mavlink::start(int argc, char *argv[]) { + // Instantiate thread + char buf[32]; + sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + // while (!this->is_running()) { + // usleep(200); + // } + + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } + + // if (mavlink::g_mavlink != nullptr) { + // errx(1, "already running"); + // } + + // mavlink::g_mavlink = new Mavlink; + + // if (mavlink::g_mavlink == nullptr) { + // errx(1, "alloc failed"); + // } + + // if (OK != mavlink::g_mavlink->start()) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + // err(1, "start failed"); + // } return OK; } @@ -1875,44 +1706,7 @@ int mavlink_main(int argc, char *argv[]) } if (!strcmp(argv[1], "start")) { - - // Instantiate thread - char buf[32]; - sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - - // if (mavlink::g_mavlink != nullptr) { - // errx(1, "already running"); - // } - - // mavlink::g_mavlink = new Mavlink; - - // if (mavlink::g_mavlink == nullptr) { - // errx(1, "alloc failed"); - // } - - // if (OK != mavlink::g_mavlink->start()) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; - // err(1, "start failed"); - // } - - return 0; + return Mavlink::start(argc, argv); } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9461a51f8..e7f3486da 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -41,6 +41,12 @@ #pragma once #include <stdbool.h> +#include <nuttx/fs/fs.h> +#include <drivers/drv_rc_input.h> +#include <systemlib/param/param.h> +#include <systemlib/perf_counter.h> +#include <pthread.h> +#include <mavlink/mavlink_log.h> #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> @@ -68,12 +74,12 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> -#include <drivers/drv_rc_input.h> #include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/mission.h> -#include <systemlib/param/param.h> -#include <nuttx/fs/fs.h> +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -136,7 +142,7 @@ public: * * @return OK on success. */ - static int start(); + static int start(int argc, char *argv[]); /** * Display the mavlink status. @@ -157,8 +163,6 @@ public: int get_uart_fd(); - int get_channel(); - const char *device_name; enum MAVLINK_MODE { @@ -185,7 +189,7 @@ public: */ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); /** * Enable / disable Hardware in the Loop simulation mode. @@ -197,65 +201,13 @@ public: */ int set_hil_enabled(bool hil_enabled); - 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 safety_sub; - int actuators_sub; - int armed_sub; - int actuators_effective_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int triplet_sub; - int debug_key_value; - int input_rc_sub; - int optical_flow; - int rates_setpoint_sub; - int get_sub; - int airspeed_sub; - int navigation_capabilities_sub; - int position_setpoint_triplet_sub; - int rc_sub; - int status_sub; - int home_sub; - }; + MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); - struct mavlink_subscriptions subs; - - struct mavlink_subscriptions* get_subs() { return &subs; } - mavlink_channel_t get_chan() { return _chan; } - - /** Global position */ - struct vehicle_global_position_s global_pos; - /** Local position */ - struct vehicle_local_position_s local_pos; - /** navigation capabilities */ - struct navigation_capabilities_s nav_cap; - /** Vehicle status */ - struct vehicle_status_s v_status; - /** RC channels */ - struct rc_channels_s rc; - /** Actuator armed state */ - struct actuator_armed_s armed; - /** Position setpoint triplet */ - struct position_setpoint_triplet_s pos_sp_triplet; + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - /** - * Pointer to the default cdev file operations table; useful for - * registering clone devices etc. - */ - Mavlink* _next; private: @@ -270,11 +222,12 @@ private: /* states */ bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ - int _params_sub; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; orb_advert_t mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER + uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; @@ -287,7 +240,6 @@ private: unsigned int total_counter; pthread_t receive_thread; - pthread_t uorb_receive_thread; /* Allocate storage space for waypoints */ mavlink_wpm_storage wpm_s; @@ -307,21 +259,6 @@ private: bool mavlink_link_termination_allowed; /** - * Update our local parameter cache. - */ - void parameters_update(); - - /** - * Send all parameters at once. - * - * This function blocks until all parameters have been sent. - * it delays each parameter by the passed amount of microseconds. - * - * @param delay The delay in us between sending all parameters. - */ - void mavlink_pm_send_all_params(unsigned int delay); - - /** * Send one parameter. * * @param param The parameter id to send. @@ -381,12 +318,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval); - - /** - * Callback for param interface. - */ - static void mavlink_pm_callback(void *arg, param_t param); + int add_stream(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp new file mode 100644 index 000000000..df73581f0 --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -0,0 +1,724 @@ +/* + * mavlink_messages.cpp + * + * Created on: 25.02.2014 + * Author: ton + */ + +#include <commander/px4_custom_mode.h> + +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/position_setpoint_triplet.h> + +#include "mavlink_messages.h" + +class MavlinkStreamHeartbeat : public MavlinkStream { +public: + const char *get_name() + { + return "HEARTBEAT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHeartbeat(); + } + +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; + + struct vehicle_status_s *status; + struct position_setpoint_triplet_s *pos_sp_triplet; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data(); + } + + void send(const hrt_abstime t) { + status_sub->update(t); + pos_sp_triplet_sub->update(t); + + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } + } + mavlink_custom_mode = custom_mode.data; + + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { + mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { + mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { + mavlink_state = MAV_STATE_POWEROFF; + } else { + mavlink_state = MAV_STATE_CRITICAL; + } + + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); + + } +}; + + +class MavlinkStreamSysStatus : public MavlinkStream { +public: + const char *get_name() + { + return "SYS_STATUS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamSysStatus(); + } + +private: + MavlinkOrbSubscription *status_sub; + + struct vehicle_status_s *status; + +protected: + + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + } + + void send(const hrt_abstime t) { + status_sub->update(t); + + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); + } +}; + + +class MavlinkStreamHighresIMU : public MavlinkStream { +public: + const char *get_name() + { + return "HIGHRES_IMU"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHighresIMU(); + } + +private: + MavlinkOrbSubscription *sensor_sub; + + struct sensor_combined_s *sensor; + + uint32_t accel_counter = 0; + uint32_t gyro_counter = 0; + uint32_t mag_counter = 0; + uint32_t baro_counter = 0; + +protected: + + void subscribe(Mavlink *mavlink) + { + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); + sensor = (struct sensor_combined_s *)sensor_sub->get_data(); + } + + void send(const hrt_abstime t) { + sensor_sub->update(t); + + uint16_t fields_updated = 0; + + if (accel_counter != sensor->accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = sensor->accelerometer_counter; + } + + if (gyro_counter != sensor->gyro_counter) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = sensor->gyro_counter; + } + + if (mag_counter != sensor->magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = sensor->magnetometer_counter; + } + + if (baro_counter != sensor->baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = sensor->baro_counter; + } + + mavlink_msg_highres_imu_send(_channel, + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); + } +}; + + +class MavlinkStreamAttitude : public MavlinkStream { +public: + const char *get_name() + { + return "ATTITUDE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitude(); + } + +private: + MavlinkOrbSubscription *att_sub; + + struct vehicle_attitude_s *att; + +protected: + + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } +}; + + +MavlinkStream *streams_list[] = { + new MavlinkStreamHeartbeat(), + new MavlinkStreamSysStatus(), + new MavlinkStreamHighresIMU(), + new MavlinkStreamAttitude(), + nullptr +}; + + + + + + + + + +//void +//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) +//{ +// /* copy attitude data into local buffer */ +// orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send sensor values */ +// mavlink_msg_attitude_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.roll, +// l->listener->att.pitch, +// l->listener->att.yaw, +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// +// /* limit VFR message rate to 10Hz */ +// hrt_abstime t = hrt_absolute_time(); +// if (t >= l->listener->last_sent_vfr + 100000) { +// l->listener->last_sent_vfr = t; +// float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); +// uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; +// float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; +// mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); +// } +// +// /* send quaternion values if it exists */ +// if(l->listener->att.q_valid) { +// mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.q[0], +// l->listener->att.q[1], +// l->listener->att.q[2], +// l->listener->att.q[3], +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// } +// } +// +// l->listener->attitude_counter++; +//} +// +//void +//MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) +//{ +// struct vehicle_gps_position_s gps; +// +// /* copy gps data into local buffer */ +// orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); +// +// /* GPS COG is 0..2PI in degrees * 1e2 */ +// float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; +// +// /* GPS position */ +// mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), +// gps.timestamp_position, +// gps.fix_type, +// gps.lat, +// gps.lon, +// gps.alt, +// cm_uint16_from_m_float(gps.eph_m), +// cm_uint16_from_m_float(gps.epv_m), +// gps.vel_m_s * 1e2f, // from m/s to cm/s +// cog_deg * 1e2f, // from deg to deg * 100 +// gps.satellites_visible); +// +// /* update SAT info every 10 seconds */ +// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { +// mavlink_msg_gps_status_send(l->mavlink->get_chan(), +// gps.satellites_visible, +// gps.satellite_prn, +// gps.satellite_used, +// gps.satellite_elevation, +// gps.satellite_azimuth, +// gps.satellite_snr); +// } +// +// l->listener->gps_counter++; +//} +// +// +//void +//MavlinkOrbListener::l_rc_channels(const struct listener *l) +//{ +// /* copy rc channels into local buffer */ +// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); +// // XXX Add RC channels scaled message here +//} +// +//void +//MavlinkOrbListener::l_input_rc(const struct listener *l) +//{ +// /* copy rc _mavlink->get_chan()nels into local buffer */ +// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// +// const unsigned port_width = 8; +// +// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), +// l->listener->rc_raw.timestamp_publication / 1000, +// i, +// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, +// l->listener->rc_raw.rssi); +// } +// } +//} +// +//void +//MavlinkOrbListener::l_global_position(const struct listener *l) +//{ +// /* copy global position data into local buffer */ +// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); +// +// mavlink_msg_global_position_int_send(l->mavlink->get_chan(), +// l->listener->global_pos.timestamp / 1000, +// l->listener->global_pos.lat * 1e7, +// l->listener->global_pos.lon * 1e7, +// l->listener->global_pos.alt * 1000.0f, +// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, +// l->listener->global_pos.vel_n * 100.0f, +// l->listener->global_pos.vel_e * 100.0f, +// l->listener->global_pos.vel_d * 100.0f, +// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); +//} +// +//void +//MavlinkOrbListener::l_local_position(const struct listener *l) +//{ +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), +// l->listener->local_pos.timestamp / 1000, +// l->listener->local_pos.x, +// l->listener->local_pos.y, +// l->listener->local_pos.z, +// l->listener->local_pos.vx, +// l->listener->local_pos.vy, +// l->listener->local_pos.vz); +//} +// +//void +//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) +//{ +// struct position_setpoint_triplet_s triplet; +// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); +// +// if (!triplet.current.valid) +// return; +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), +// MAV_FRAME_GLOBAL, +// (int32_t)(triplet.current.lat * 1e7d), +// (int32_t)(triplet.current.lon * 1e7d), +// (int32_t)(triplet.current.alt * 1e3f), +// (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); +//} +// +//void +//MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) +//{ +// struct vehicle_local_position_setpoint_s local_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), +// MAV_FRAME_LOCAL_NED, +// local_sp.x, +// local_sp.y, +// local_sp.z, +// local_sp.yaw); +//} +// +//void +//MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) +//{ +// struct vehicle_attitude_setpoint_s att_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), +// att_sp.timestamp / 1000, +// att_sp.roll_body, +// att_sp.pitch_body, +// att_sp.yaw_body, +// att_sp.thrust); +//} +// +//void +//MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) +//{ +// struct vehicle_rates_setpoint_s rates_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), +// rates_sp.timestamp / 1000, +// rates_sp.roll, +// rates_sp.pitch, +// rates_sp.yaw, +// rates_sp.thrust); +//} +// +//void +//MavlinkOrbListener::l_actuator_outputs(const struct listener *l) +//{ +// struct actuator_outputs_s act_outputs; +// +// orb_id_t ids[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; +// +// /* copy actuator data into local buffer */ +// orb_copy(ids[l->arg], *l->subp, &act_outputs); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, +// l->arg /* port number - needs GCS support */, +// /* QGC has port number support already */ +// 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 and only send first group for HIL */ +// if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { +// +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state = 0; +// uint8_t mavlink_base_mode = 0; +// uint32_t mavlink_custom_mode = 0; +// l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); +// +// /* HIL message as per MAVLink spec */ +// +// /* scale / assign outputs depending on system type */ +// +// if (mavlink_system.type == MAV_TYPE_QUADROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_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_base_mode, +// 0); +// +// } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_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_base_mode, +// 0); +// +// } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_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_base_mode, +// 0); +// +// } else { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// (act_outputs.output[0] - 1500.0f) / 500.0f, +// (act_outputs.output[1] - 1500.0f) / 500.0f, +// (act_outputs.output[2] - 1500.0f) / 500.0f, +// (act_outputs.output[3] - 1000.0f) / 1000.0f, +// (act_outputs.output[4] - 1500.0f) / 500.0f, +// (act_outputs.output[5] - 1500.0f) / 500.0f, +// (act_outputs.output[6] - 1500.0f) / 500.0f, +// (act_outputs.output[7] - 1500.0f) / 500.0f, +// mavlink_base_mode, +// 0); +// } +// } +// } +//} +// +//void +//MavlinkOrbListener::l_actuator_armed(const struct listener *l) +//{ +// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); +//} +// +//void +//MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) +//{ +// struct manual_control_setpoint_s man_control; +// +// /* copy manual control data into local buffer */ +// orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_manual_control_send(l->mavlink->get_chan(), +// mavlink_system.sysid, +// man_control.roll * 1000, +// man_control.pitch * 1000, +// man_control.yaw * 1000, +// man_control.throttle * 1000, +// 0); +//} +// +//void +//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) +//{ +// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl0 ", +// l->listener->actuators_0.control[0]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl1 ", +// l->listener->actuators_0.control[1]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl2 ", +// l->listener->actuators_0.control[2]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl3 ", +// l->listener->actuators_0.control[3]); +// } +//} +// +//void +//MavlinkOrbListener::l_debug_key_value(const struct listener *l) +//{ +// struct debug_key_value_s debug; +// +// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); +// +// /* Enforce null termination */ +// debug.key[sizeof(debug.key) - 1] = '\0'; +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// debug.key, +// debug.value); +//} +// +//void +//MavlinkOrbListener::l_optical_flow(const struct listener *l) +//{ +// struct optical_flow_s flow; +// +// orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); +// +// mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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); +//} +// +//void +//MavlinkOrbListener::l_home(const struct listener *l) +//{ +// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); +// +// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); +//} +// +//void +//MavlinkOrbListener::l_airspeed(const struct listener *l) +//{ +// orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); +//} +// +//void +//MavlinkOrbListener::l_nav_cap(const struct listener *l) +//{ +// +// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// hrt_absolute_time() / 1000, +// "turn dist", +// l->listener->nav_cap.turn_distance); +// +//} diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h new file mode 100644 index 000000000..3dc6cb699 --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.h @@ -0,0 +1,15 @@ +/* + * mavlink_messages.h + * + * Created on: 25.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_MESSAGES_H_ +#define MAVLINK_MESSAGES_H_ + +#include "mavlink_stream.h" + +extern MavlinkStream *streams_list[]; + +#endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp deleted file mode 100644 index fdc196371..000000000 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ /dev/null @@ -1,829 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file orb_listener.cpp - * Monitors ORB topics and sends update messages as appropriate. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - */ - -// XXX trim includes -#include <nuttx/config.h> -#include <stdio.h> -#include <math.h> -#include <stdbool.h> -#include <fcntl.h> -#include <string.h> -#include "mavlink_bridge_header.h" -#include <drivers/drv_hrt.h> -#include <time.h> -#include <float.h> -#include <unistd.h> -#include <sys/prctl.h> -#include <stdlib.h> -#include <poll.h> -#include <lib/geo/geo.h> -#include <systemlib/err.h> - - -#include <mavlink/mavlink_log.h> - -#include "mavlink_orb_listener.h" -#include "mavlink_main.h" - - - -uint16_t cm_uint16_from_m_float(float m); - - -uint16_t -cm_uint16_from_m_float(float m) -{ - if (m < 0.0f) { - return 0; - - } else if (m > 655.35f) { - return 65535; - } - - return (uint16_t)(m * 100.0f); -} - -MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), - _mavlink(parent), - _listeners(nullptr), - _n_listeners(0) -{ - add_listener(MavlinkOrbListener::l_sensor_combined, &(_mavlink->get_subs()->sensor_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude, &(_mavlink->get_subs()->att_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0); - add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0); - add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0); - add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0); - add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0); - add_listener(MavlinkOrbListener::l_local_position, &(_mavlink->get_subs()->local_pos_sub), 0); - add_listener(MavlinkOrbListener::l_global_position_setpoint, &(_mavlink->get_subs()->triplet_sub), 0); - add_listener(MavlinkOrbListener::l_local_position_setpoint, &(_mavlink->get_subs()->spl_sub), 0); - add_listener(MavlinkOrbListener::l_attitude_setpoint, &(_mavlink->get_subs()->spa_sub), 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_0_sub), 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_1_sub), 1); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_2_sub), 2); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_3_sub), 3); - add_listener(MavlinkOrbListener::l_actuator_armed, &(_mavlink->get_subs()->armed_sub), 0); - add_listener(MavlinkOrbListener::l_manual_control_setpoint, &(_mavlink->get_subs()->man_control_sp_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &(_mavlink->get_subs()->actuators_sub), 0); - add_listener(MavlinkOrbListener::l_debug_key_value, &(_mavlink->get_subs()->debug_key_value), 0); - add_listener(MavlinkOrbListener::l_optical_flow, &(_mavlink->get_subs()->optical_flow), 0); - add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &(_mavlink->get_subs()->rates_setpoint_sub), 0); - add_listener(MavlinkOrbListener::l_airspeed, &(_mavlink->get_subs()->airspeed_sub), 0); - add_listener(MavlinkOrbListener::l_nav_cap, &(_mavlink->get_subs()->navigation_capabilities_sub), 0); - -} - -void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg) -{ - struct listener *nl = new listener; - - nl->callback = callback; - nl->subp = subp; - nl->arg = arg; - nl->next = nullptr; - nl->mavlink = _mavlink; - nl->listener = this; - - if (_listeners == nullptr) { - _listeners = nl; - } else { - struct listener *next = _listeners; - while (next->next != nullptr) { - next = next->next; - } - next->next = nl; - } - _n_listeners++; -} - -void -MavlinkOrbListener::l_sensor_combined(const struct listener *l) -{ - struct sensor_combined_s raw; - - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); - - /* mark individual fields as _mavlink->get_chan()ged */ - uint16_t fields_updated = 0; - - // if (accel_counter != raw.accelerometer_counter) { - // /* mark first three dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - // accel_counter = raw.accelerometer_counter; - // } - - // if (gyro_counter != raw.gyro_counter) { - // /* mark second group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - // gyro_counter = raw.gyro_counter; - // } - - // if (mag_counter != raw.magnetometer_counter) { - // /* mark third group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - // mag_counter = raw.magnetometer_counter; - // } - - // if (baro_counter != raw.baro_counter) { - // /* mark last group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - // baro_counter = raw.baro_counter; - // } - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->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, raw.differential_pressure_pa, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); - - l->listener->sensors_raw_counter++; -} - -void -MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) -{ - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - /* send sensor values */ - mavlink_msg_attitude_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - l->listener->att.roll, - l->listener->att.pitch, - l->listener->att.yaw, - l->listener->att.rollspeed, - l->listener->att.pitchspeed, - l->listener->att.yawspeed); - - /* limit VFR message rate to 10Hz */ - hrt_abstime t = hrt_absolute_time(); - if (t >= l->listener->last_sent_vfr + 100000) { - l->listener->last_sent_vfr = t; - float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); - uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; - float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); - } - - /* send quaternion values if it exists */ - if(l->listener->att.q_valid) { - mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - l->listener->att.q[0], - l->listener->att.q[1], - l->listener->att.q[2], - l->listener->att.q[3], - l->listener->att.rollspeed, - l->listener->att.pitchspeed, - l->listener->att.yawspeed); - } - } - - l->listener->attitude_counter++; -} - -void -MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) -{ - struct vehicle_gps_position_s gps; - - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); - - /* GPS COG is 0..2PI in degrees * 1e2 */ - float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; - - /* GPS position */ - mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph_m), - cm_uint16_from_m_float(gps.epv_m), - gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from deg to deg * 100 - gps.satellites_visible); - - /* update SAT info every 10 seconds */ - if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(l->mavlink->get_chan(), - gps.satellites_visible, - gps.satellite_prn, - gps.satellite_used, - gps.satellite_elevation, - gps.satellite_azimuth, - gps.satellite_snr); - } - - l->listener->gps_counter++; -} - -void -MavlinkOrbListener::l_vehicle_status(const struct listener *l) -{ - /* immediately communicate state _mavlink->get_chan()ges back to user */ - orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status)); - orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); - orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->position_setpoint_triplet_sub, &(l->mavlink->pos_sp_triplet)); - - /* enable or disable HIL */ - if (l->listener->v_status.hil_state == HIL_STATE_ON) - l->mavlink->set_hil_enabled(true); - else if (l->listener->v_status.hil_state == HIL_STATE_OFF) - l->mavlink->set_hil_enabled(false); - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(l->mavlink->get_chan(), - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - -void -MavlinkOrbListener::l_rc_channels(const struct listener *l) -{ - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); - // XXX Add RC channels scaled message here -} - -void -MavlinkOrbListener::l_input_rc(const struct listener *l) -{ - /* copy rc _mavlink->get_chan()nels into local buffer */ - orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), - l->listener->rc_raw.timestamp_publication / 1000, - i, - (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, - l->listener->rc_raw.rssi); - } - } -} - -void -MavlinkOrbListener::l_global_position(const struct listener *l) -{ - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); - - mavlink_msg_global_position_int_send(l->mavlink->get_chan(), - l->listener->global_pos.timestamp / 1000, - l->listener->global_pos.lat * 1e7, - l->listener->global_pos.lon * 1e7, - l->listener->global_pos.alt * 1000.0f, - (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, - l->listener->global_pos.vel_n * 100.0f, - l->listener->global_pos.vel_e * 100.0f, - l->listener->global_pos.vel_d * 100.0f, - _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -} - -void -MavlinkOrbListener::l_local_position(const struct listener *l) -{ - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), - l->listener->local_pos.timestamp / 1000, - l->listener->local_pos.x, - l->listener->local_pos.y, - l->listener->local_pos.z, - l->listener->local_pos.vx, - l->listener->local_pos.vy, - l->listener->local_pos.vz); -} - -void -MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -{ - struct position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); - - if (!triplet.current.valid) - return; - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), - MAV_FRAME_GLOBAL, - (int32_t)(triplet.current.lat * 1e7d), - (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.alt * 1e3f), - (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); -} - -void -MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) -{ - struct vehicle_local_position_setpoint_s local_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), - MAV_FRAME_LOCAL_NED, - local_sp.x, - local_sp.y, - local_sp.z, - local_sp.yaw); -} - -void -MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) -{ - struct vehicle_attitude_setpoint_s att_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); -} - -void -MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) -{ - struct vehicle_rates_setpoint_s rates_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), - rates_sp.timestamp / 1000, - rates_sp.roll, - rates_sp.pitch, - rates_sp.yaw, - rates_sp.thrust); -} - -void -MavlinkOrbListener::l_actuator_outputs(const struct listener *l) -{ - struct actuator_outputs_s act_outputs; - - orb_id_t ids[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - /* copy actuator data into local buffer */ - orb_copy(ids[l->arg], *l->subp, &act_outputs); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, - l->arg /* port number - needs GCS support */, - /* QGC has port number support already */ - 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 and only send first group for HIL */ - if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* HIL message as per MAVLink spec */ - - /* scale / assign outputs depending on system type */ - - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(l->mavlink->get_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_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(l->mavlink->get_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_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(l->mavlink->get_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_base_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(l->mavlink->get_chan(), - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 500.0f, - (act_outputs.output[1] - 1500.0f) / 500.0f, - (act_outputs.output[2] - 1500.0f) / 500.0f, - (act_outputs.output[3] - 1000.0f) / 1000.0f, - (act_outputs.output[4] - 1500.0f) / 500.0f, - (act_outputs.output[5] - 1500.0f) / 500.0f, - (act_outputs.output[6] - 1500.0f) / 500.0f, - (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); - } - } - } -} - -void -MavlinkOrbListener::l_actuator_armed(const struct listener *l) -{ - orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); -} - -void -MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) -{ - struct manual_control_setpoint_s man_control; - - /* copy manual control data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_manual_control_send(l->mavlink->get_chan(), - mavlink_system.sysid, - man_control.roll * 1000, - man_control.pitch * 1000, - man_control.yaw * 1000, - man_control.throttle * 1000, - 0); -} - -void -MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) -{ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl0 ", - l->listener->actuators_0.control[0]); - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl1 ", - l->listener->actuators_0.control[1]); - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl2 ", - l->listener->actuators_0.control[2]); - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl3 ", - l->listener->actuators_0.control[3]); - } -} - -void -MavlinkOrbListener::l_debug_key_value(const struct listener *l) -{ - struct debug_key_value_s debug; - - orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); - - /* Enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - debug.key, - debug.value); -} - -void -MavlinkOrbListener::l_optical_flow(const struct listener *l) -{ - struct optical_flow_s flow; - - orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); - - mavlink_msg_optical_flow_send(l->mavlink->get_chan(), 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); -} - -void -MavlinkOrbListener::l_home(const struct listener *l) -{ - orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); - - mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); -} - -void -MavlinkOrbListener::l_airspeed(const struct listener *l) -{ - orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); -} - -void -MavlinkOrbListener::l_nav_cap(const struct listener *l) -{ - - orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); - - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - hrt_absolute_time() / 1000, - "turn dist", - l->listener->nav_cap.turn_distance); - -} - -void * -MavlinkOrbListener::uorb_receive_thread(void *arg) -{ - /* Set thread name */ - char thread_name[18]; - sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); - prctl(PR_SET_NAME, thread_name, getpid()); - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; - - /* - * Initialise listener array. - * - * Might want to invoke each listener once to set initial state. - */ - struct pollfd fds[_max_listeners]; - - struct listener* next = _listeners; - unsigned i = 0; - while (next != nullptr) { - - fds[i].fd = *next->subp; - fds[i].events = POLLIN; - - next = next->next; - i++; - } - /* Invoke callback to set initial state */ - //listeners[i].callback(&listener[i]); - - while (!_mavlink->_task_should_exit) { - - int poll_ret = poll(fds, _n_listeners, timeout); - - /* handle the poll result */ - if (poll_ret == 0) { - /* silent */ - - } else if (poll_ret < 0) { - //mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); - - } else { - - i = 0; - struct listener* cb = _listeners; - while (cb != nullptr) { - - if (fds[i].revents & POLLIN) - cb->callback(cb); - - cb = cb->next; - i++; - } - } - } - - return NULL; -} - -void * MavlinkOrbListener::uorb_start_helper(void *context) -{ - MavlinkOrbListener* urcv = new MavlinkOrbListener(((Mavlink *)context)); - return urcv->uorb_receive_thread(NULL); -} - -pthread_t -MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) -{ - - /* --- SENSORS RAW VALUE --- */ - mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->sensor_sub, 200); /* 5Hz updates */ - - /* --- ATTITUDE VALUE --- */ - mavlink->get_subs()->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->att_sub, 200); /* 5Hz updates */ - - /* --- GPS VALUE --- */ - mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink->get_subs()->gps_sub, 200); /* 5Hz updates */ - - /* --- HOME POSITION --- */ - mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position)); - orb_set_interval(mavlink->get_subs()->home_sub, 1000); /* 1Hz updates */ - - /* --- SYSTEM STATE --- */ - mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */ - - /* --- POSITION SETPOINT TRIPLET --- */ - mavlink->get_subs()->position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink->get_subs()->position_setpoint_triplet_sub, 0); /* not polled, don't limit */ - - /* --- RC CHANNELS VALUE --- */ - mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(mavlink->get_subs()->rc_sub, 100); /* 10Hz updates */ - - /* --- RC RAW VALUE --- */ - mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc)); - orb_set_interval(mavlink->get_subs()->input_rc_sub, 100); - - /* --- GLOBAL POS VALUE --- */ - mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink->get_subs()->global_pos_sub, 100); /* 10 Hz active updates */ - - /* --- LOCAL POS VALUE --- */ - mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000); /* 1Hz active updates */ - - /* --- GLOBAL SETPOINT VALUE --- */ - mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink->get_subs()->triplet_sub, 2000); /* 0.5 Hz updates */ - - /* --- LOCAL SETPOINT VALUE --- */ - mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(mavlink->get_subs()->spl_sub, 2000); /* 0.5 Hz updates */ - - /* --- ATTITUDE SETPOINT VALUE --- */ - mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(mavlink->get_subs()->spa_sub, 2000); /* 0.5 Hz updates */ - - /* --- RATES SETPOINT VALUE --- */ - mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - orb_set_interval(mavlink->get_subs()->rates_setpoint_sub, 2000); /* 0.5 Hz updates */ - - /* --- ACTUATOR OUTPUTS --- */ - mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - mavlink->get_subs()->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->act_0_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_3_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR ARMED VALUE --- */ - mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */ - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink->get_subs()->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->man_control_sp_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR CONTROL VALUE --- */ - mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - orb_set_interval(mavlink->get_subs()->actuators_sub, 100); /* 10Hz updates */ - - /* --- DEBUG VALUE OUTPUT --- */ - mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - orb_set_interval(mavlink->get_subs()->debug_key_value, 100); /* 10Hz updates */ - - /* --- FLOW SENSOR --- */ - mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow)); - orb_set_interval(mavlink->get_subs()->optical_flow, 200); /* 5Hz updates */ - - /* --- AIRSPEED --- */ - mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); /* 5Hz updates */ - - /* --- NAVIGATION CAPABILITIES --- */ - mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */ - - /* start the listener loop */ - pthread_attr_t uorb_attr; - pthread_attr_init(&uorb_attr); - - /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink); - - pthread_attr_destroy(&uorb_attr); - return thread; -} diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h deleted file mode 100644 index 26a2e832f..000000000 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ /dev/null @@ -1,163 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_orb_listener.h - * MAVLink 1.0 protocol interface definition. - * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - */ - -#include <systemlib/perf_counter.h> - -#pragma once - -#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_local_position.h> -#include <uORB/topics/home_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_attitude_setpoint.h> -#include <uORB/topics/vehicle_rates_setpoint.h> -#include <uORB/topics/optical_flow.h> -#include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/actuator_controls_effective.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/actuator_armed.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/telemetry_status.h> -#include <uORB/topics/debug_key_value.h> -#include <uORB/topics/airspeed.h> -#include <uORB/topics/battery_status.h> -#include <uORB/topics/position_setpoint_triplet.h> -#include <drivers/drv_rc_input.h> -#include <uORB/topics/navigation_capabilities.h> -#include <uORB/topics/mission.h> - -class Mavlink; - -class MavlinkOrbListener -{ -public: - /** - * Constructor - */ - MavlinkOrbListener(Mavlink* parent); - - /** - * Destructor, also kills the mavlinks task. - */ - ~MavlinkOrbListener(); - - static pthread_t uorb_receive_start(Mavlink *mavlink); - - struct listener { - void (*callback)(const struct listener *l); - int *subp; - uintptr_t arg; - struct listener *next; - Mavlink *mavlink; - MavlinkOrbListener* listener; - }; - - void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg); - static void * uorb_start_helper(void *context); - -private: - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Mavlink* _mavlink; - - struct listener *_listeners; - unsigned _n_listeners; - static const unsigned _max_listeners = 32; - - void *uorb_receive_thread(void *arg); - - static void l_sensor_combined(const struct listener *l); - static void l_vehicle_attitude(const struct listener *l); - static void l_vehicle_gps_position(const struct listener *l); - static void l_vehicle_status(const struct listener *l); - static void l_rc_channels(const struct listener *l); - static void l_input_rc(const struct listener *l); - static void l_global_position(const struct listener *l); - static void l_local_position(const struct listener *l); - static void l_global_position_setpoint(const struct listener *l); - static void l_local_position_setpoint(const struct listener *l); - static void l_attitude_setpoint(const struct listener *l); - static void l_actuator_outputs(const struct listener *l); - static void l_actuator_armed(const struct listener *l); - static void l_manual_control_setpoint(const struct listener *l); - static void l_vehicle_attitude_controls(const struct listener *l); - static void l_debug_key_value(const struct listener *l); - static void l_optical_flow(const struct listener *l); - static void l_vehicle_rates_setpoint(const struct listener *l); - static void l_home(const struct listener *l); - static void l_airspeed(const struct listener *l); - static void l_nav_cap(const struct listener *l); - - struct vehicle_global_position_s global_pos; - struct vehicle_local_position_s local_pos; - struct navigation_capabilities_s nav_cap; - struct vehicle_status_s v_status; - struct rc_channels_s rc; - struct rc_input_values rc_raw; - struct actuator_armed_s armed; - struct actuator_controls_s actuators_0; - struct vehicle_attitude_s att; - struct airspeed_s airspeed; - struct home_position_s home; - - unsigned int sensors_raw_counter; - unsigned int attitude_counter; - unsigned int gps_counter; - - /* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ - uint64_t last_sensor_timestamp; - - hrt_abstime last_sent_vfr; - -}; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp new file mode 100644 index 000000000..b504b6955 --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -0,0 +1,54 @@ +/* + * mavlink_orb_subscription.cpp + * + * Created on: 23.02.2014 + * Author: ton + */ + + +#include <unistd.h> +#include <stdlib.h> +#include <string.h> +#include <uORB/uORB.h> + +#include "mavlink_orb_subscription.h" + +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) +{ + _data = malloc(size); + memset(_data, 0, size); + _fd = orb_subscribe(_topic); +} + +MavlinkOrbSubscription::~MavlinkOrbSubscription() +{ + close(_fd); + free(_data); +} + +const struct orb_metadata * +MavlinkOrbSubscription::get_topic() +{ + return _topic; +} + +void * +MavlinkOrbSubscription::get_data() +{ + return _data; +} + +bool +MavlinkOrbSubscription::update(const hrt_abstime t) +{ + if (_last_check != t) { + _last_check = t; + bool updated; + orb_check(_fd, &updated); + if (updated) { + orb_copy(_topic, _fd, _data); + return true; + } + } + return false; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h new file mode 100644 index 000000000..c38a9cc43 --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -0,0 +1,34 @@ +/* + * mavlink_orb_subscription.h + * + * Created on: 23.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_ORB_SUBSCRIPTION_H_ +#define MAVLINK_ORB_SUBSCRIPTION_H_ + +#include <systemlib/uthash/utlist.h> +#include <drivers/drv_hrt.h> + + +class MavlinkOrbSubscription { +public: + MavlinkOrbSubscription *next; + + MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size); + ~MavlinkOrbSubscription(); + + bool update(const hrt_abstime t); + void *get_data(); + const struct orb_metadata *get_topic(); + +private: + const struct orb_metadata *_topic; + int _fd; + void *_data; + hrt_abstime _last_check; +}; + + +#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d07de0f22..b828420e6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -818,7 +818,7 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp new file mode 100644 index 000000000..16407366e --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -0,0 +1,51 @@ +/* + * mavlink_stream.cpp + * + * Created on: 24.02.2014 + * Author: ton + */ + +#include <stdlib.h> + +#include "mavlink_stream.h" +#include "mavlink_main.h" + +MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +{ +} + +MavlinkStream::~MavlinkStream() +{ +} + +/** + * Set messages interval in ms + */ +void +MavlinkStream::set_interval(const unsigned int interval) +{ + _interval = interval * 1000; +} + +/** + * Set mavlink channel + */ +void +MavlinkStream::set_channel(mavlink_channel_t channel) +{ + _channel = channel; +} + +/** + * Update subscriptions and send message if necessary + */ +int +MavlinkStream::update(const hrt_abstime t) +{ + uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { + /* interval expired, send message */ + send(t); + _last_sent = (t / _interval) * _interval; + } +} diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h new file mode 100644 index 000000000..9f175adbe --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.h @@ -0,0 +1,42 @@ +/* + * mavlink_stream.h + * + * Created on: 24.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_STREAM_H_ +#define MAVLINK_STREAM_H_ + +#include <drivers/drv_hrt.h> + +class Mavlink; +class MavlinkStream; + +#include "mavlink_main.h" + +class MavlinkStream { +private: + hrt_abstime _last_sent; + +protected: + mavlink_channel_t _channel; + unsigned int _interval; + + virtual void send(const hrt_abstime t) = 0; + +public: + MavlinkStream *next; + + MavlinkStream(); + ~MavlinkStream(); + void set_interval(const unsigned int interval); + void set_channel(mavlink_channel_t channel); + int update(const hrt_abstime t); + virtual MavlinkStream *new_instance() = 0; + virtual void subscribe(Mavlink *mavlink) = 0; + virtual const char *get_name() = 0; +}; + + +#endif /* MAVLINK_STREAM_H_ */ diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 76798eb12..5ea6c0f46 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,8 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp + mavlink_orb_subscription.cpp \ + mavlink_messages.cpp \ + mavlink_stream.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink |