aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink.c6
-rw-r--r--src/modules/mavlink/mavlink_main.cpp14
-rw-r--r--src/modules/mavlink/mavlink_main.h4
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp66
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp12
5 files changed, 78 insertions, 24 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 6b4edff78..fb9f65cf5 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,
- 2900,
+ 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 cccb698bf..978aee118 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 bc092c7e9..ca00d1a67 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -538,12 +538,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();
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-01 10:20:48 +0000