diff options
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink.c | 6 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 14 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_main.h | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 66 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 14 |
5 files changed, 79 insertions, 25 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index bec706bad..9afe74af1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, - 50, - MAV_TYPE_FIXED_WING, - 0, - 0, - 0 + 50 }; // System ID, 1-255, Component/Subsystem ID, 1-255 /* diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3c1d360c2..29b7ec7b7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,8 +167,10 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), - _param_system_type(0), + _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void) param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; + _system_type = system_type; } int32_t use_hil_gps; @@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1f96e586b..ad5e5001b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -265,6 +265,8 @@ public: struct mavlink_logbuffer *get_logbuffer() { return &_logbuffer; } + unsigned get_system_type() { return _system_type; } + protected: Mavlink *next; @@ -354,6 +356,8 @@ private: param_t _param_use_hil_gps; param_t _param_forward_externalsp; + unsigned _system_type; + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d955b5f76..a8f956ad0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -302,7 +302,7 @@ protected: msg.base_mode = 0; msg.custom_mode = 0; get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode); - msg.type = mavlink_system.type; + msg.type = _mavlink->get_system_type(); msg.autopilot = MAV_AUTOPILOT_PX4; msg.mavlink_version = 3; @@ -382,11 +382,11 @@ protected: clock_gettime(CLOCK_REALTIME, &ts); /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm t; - gmtime_r(&gps_time_sec, &t); + struct tm tt; + gmtime_r(&gps_time_sec, &tt); // XXX we do not want to interfere here with the SD log app - strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t); + strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); } @@ -1353,15 +1353,17 @@ protected: const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + unsigned system_type = _mavlink->get_system_type(); + /* scale outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR || - mavlink_system.type == MAV_TYPE_HEXAROTOR || - mavlink_system.type == MAV_TYPE_OCTOROTOR) { + if (system_type == MAV_TYPE_QUADROTOR || + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; - switch (mavlink_system.type) { + switch (system_type) { case MAV_TYPE_QUADROTOR: n = 4; break; @@ -1717,7 +1719,53 @@ protected: msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX; msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX; msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX; - msg.rssi = rc.rssi; + + /* RSSI has a max value of 100, and when Spektrum or S.BUS are + * available, the RSSI field is invalid, as they do not provide + * an RSSI measurement. Use an out of band magic value to signal + * these digital ports. XXX revise MAVLink spec to address this. + * One option would be to use the top bit to toggle between RSSI + * and input source mode. + * + * Full RSSI field: 0b 1 111 1111 + * + * ^ If bit is set, RSSI encodes type + RSSI + * + * ^ These three bits encode a total of 8 + * digital RC input types. + * 0: PPM, 1: SBUS, 2: Spektrum, 2: ST24 + * ^ These four bits encode a total of + * 16 RSSI levels. 15 = full, 0 = no signal + * + */ + + /* Initialize RSSI with the special mode level flag */ + msg.rssi = (1 << 7); + + /* Set RSSI */ + msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; + + switch (rc.input_source) { + case RC_INPUT_SOURCE_PX4FMU_PPM: + /* fallthrough */ + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; + } + + if (rc.rc_lost) { + /* RSSI is by definition zero */ + msg.rssi = 0; + } _mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 75b0c6e17..e98d72afe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -546,12 +546,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << - OFB_IGN_BIT_YAW; + if (set_position_target_local_ned.type_mask & (1 << 10)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + } + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << - OFB_IGN_BIT_YAWRATE; + if (set_position_target_local_ned.type_mask & (1 << 11)) { + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + } offboard_control_sp.timestamp = hrt_absolute_time(); @@ -1404,7 +1408,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); - param.sched_priority = SCHED_PRIORITY_MAX - 40; + param.sched_priority = SCHED_PRIORITY_MAX - 80; (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); |