aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink/mavlink_messages.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp106
1 files changed, 79 insertions, 27 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index cccb698bf..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);
}
@@ -1786,33 +1834,32 @@ protected:
}
};
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
+class MavlinkStreamOpticalFlowRad : public MavlinkStream
{
public:
const char *get_name() const
{
- return MavlinkStreamOpticalFlow::get_name_static();
+ return MavlinkStreamOpticalFlowRad::get_name_static();
}
static const char *get_name_static()
{
- return "OPTICAL_FLOW";
+ return "OPTICAL_FLOW_RAD";
}
uint8_t get_id()
{
- return MAVLINK_MSG_ID_OPTICAL_FLOW;
+ return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamOpticalFlow(mavlink);
+ return new MavlinkStreamOpticalFlowRad(mavlink);
}
unsigned get_size()
{
- return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+ return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
@@ -1820,11 +1867,11 @@ private:
uint64_t _flow_time;
/* do not allow top copying this class */
- MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
- MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
+ MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
protected:
- explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+ explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
_flow_time(0)
{}
@@ -1834,18 +1881,23 @@ protected:
struct optical_flow_s flow;
if (_flow_sub->update(&_flow_time, &flow)) {
- mavlink_optical_flow_t msg;
+ mavlink_optical_flow_rad_t msg;
msg.time_usec = flow.timestamp;
msg.sensor_id = flow.sensor_id;
- msg.flow_x = flow.flow_raw_x;
- msg.flow_y = flow.flow_raw_y;
- msg.flow_comp_m_x = flow.flow_comp_x_m;
- msg.flow_comp_m_y = flow.flow_comp_y_m;
+ msg.integrated_x = flow.pixel_flow_x_integral;
+ msg.integrated_y = flow.pixel_flow_y_integral;
+ msg.integrated_xgyro = flow.gyro_x_rate_integral;
+ msg.integrated_ygyro = flow.gyro_y_rate_integral;
+ msg.integrated_zgyro = flow.gyro_z_rate_integral;
+ msg.distance = flow.ground_distance_m;
msg.quality = flow.quality;
- msg.ground_distance = flow.ground_distance_m;
+ msg.integration_time_us = flow.integration_timespan;
+ msg.sensor_id = flow.sensor_id;
+ msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+ msg.temperature = flow.gyro_temperature;
- _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
}
}
};
@@ -2151,7 +2203,7 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
- new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+ new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 10:00:17 +0000