fmu: add CRSF RC and Telemetry support

- Telemetry is only enabled on omnibus, since on Pixhawk it seems we cannot
  write to the RC UART due to how the board is wired
- For the Telemetry the UART needs to be opened RW

msg/input_rc.msg

@@ -12,6 +12,7 @@ uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10
 uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11
 uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12
 uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13
+uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14
 
 uint8 RC_INPUT_MAX_CHANNELS = 18 	# Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
 

src/drivers/px4fmu/CMakeLists.txt

@@ -37,6 +37,7 @@ px4_add_module(
 	COMPILE_FLAGS
 	SRCS
 		fmu.cpp
+		crsf_telemetry.cpp
 	DEPENDS
 		circuit_breaker
 		mixer

src/drivers/px4fmu/crsf_telemetry.cpp

@@ -0,0 +1,207 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+#include "crsf_telemetry.h"
+#include <lib/rc/crsf.h>
+
+CRSFTelemetry::CRSFTelemetry(int uart_fd)
+	: _vehicle_gps_position_sub(orb_subscribe(ORB_ID(vehicle_gps_position))),
+	  _battery_status_sub(orb_subscribe(ORB_ID(battery_status))),
+	  _vehicle_attitude_sub(orb_subscribe(ORB_ID(vehicle_attitude))),
+	  _vehicle_status_sub(orb_subscribe(ORB_ID(vehicle_status))),
+	  _uart_fd(uart_fd)
+{
+}
+
+CRSFTelemetry::~CRSFTelemetry()
+{
+	orb_unsubscribe(_vehicle_gps_position_sub);
+	orb_unsubscribe(_battery_status_sub);
+	orb_unsubscribe(_vehicle_attitude_sub);
+	orb_unsubscribe(_vehicle_status_sub);
+}
+
+bool CRSFTelemetry::update(const hrt_abstime &now)
+{
+	const int update_rate_hz = 10;
+
+	if (now - _last_update <= 1_s / (update_rate_hz * num_data_types)) {
+		return false;
+	}
+
+	bool sent = false;
+
+	switch (_next_type) {
+	case 0:
+		sent = send_battery();
+		break;
+
+	case 1:
+		sent = send_gps();
+		break;
+
+	case 2:
+		sent = send_attitude();
+		break;
+
+	case 3:
+		sent = send_flight_mode();
+		break;
+	}
+
+	_last_update = now;
+	_next_type = (_next_type + 1) % num_data_types;
+
+	return sent;
+}
+
+bool CRSFTelemetry::send_battery()
+{
+	battery_status_s battery_status;
+
+	if (orb_copy(ORB_ID(battery_status), _battery_status_sub, &battery_status) != 0) {
+		return false;
+	}
+
+	uint16_t voltage = battery_status.voltage_filtered_v * 10;
+	uint16_t current = battery_status.current_filtered_a * 10;
+	int fuel = battery_status.discharged_mah;
+	uint8_t remaining = battery_status.remaining * 100;
+	return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);
+}
+
+bool CRSFTelemetry::send_gps()
+{
+	vehicle_gps_position_s vehicle_gps_position;
+
+	if (orb_copy(ORB_ID(vehicle_gps_position), _vehicle_gps_position_sub, &vehicle_gps_position) != 0) {
+		return false;
+	}
+
+	int32_t latitude = vehicle_gps_position.lat;
+	int32_t longitude = vehicle_gps_position.lon;
+	uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f;
+	uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f;
+	uint16_t altitude = vehicle_gps_position.alt + 1000;
+	uint8_t num_satellites = vehicle_gps_position.satellites_used;
+
+	return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed,
+				       gps_heading, altitude, num_satellites);
+}
+
+bool CRSFTelemetry::send_attitude()
+{
+	vehicle_attitude_s vehicle_attitude;
+
+	if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude) != 0) {
+		return false;
+	}
+
+	matrix::Eulerf attitude = matrix::Quatf(vehicle_attitude.q);
+	int16_t pitch = attitude(1) * 1e4f;
+	int16_t roll = attitude(0) * 1e4f;
+	int16_t yaw = attitude(2) * 1e4f;
+	return crsf_send_telemetry_attitude(_uart_fd, pitch, roll, yaw);
+}
+
+bool CRSFTelemetry::send_flight_mode()
+{
+	vehicle_status_s vehicle_status;
+
+	if (orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status) != 0) {
+		return false;
+	}
+
+	const char *flight_mode = "(unknown)";
+
+	switch (vehicle_status.nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
+		flight_mode = "Manual";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
+		flight_mode = "Altitude";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL:
+		flight_mode = "Position";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		flight_mode = "Return";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+		flight_mode = "Mission";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
+	case vehicle_status_s::NAVIGATION_STATE_DESCEND:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
+		flight_mode = "Auto";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+		flight_mode = "Failure";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ACRO:
+		flight_mode = "Acro";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
+		flight_mode = "Terminate";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
+		flight_mode = "Offboard";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_STAB:
+		flight_mode = "Stabilized";
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
+		flight_mode = "Rattitude";
+		break;
+	}
+
+	return crsf_send_telemetry_flight_mode(_uart_fd, flight_mode);
+}
+

src/drivers/px4fmu/crsf_telemetry.h

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *	Copyright (c) 2018 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 crsf_telemetry.h
+ *
+ * @author Beat Küng <[email protected]>
+ */
+
+#pragma once
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <drivers/drv_hrt.h>
+
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+
+using namespace time_literals;
+
+/**
+ * High-level class that handles sending of CRSF telemetry data
+ */
+class CRSFTelemetry
+{
+public:
+	/**
+	 * @param uart_fd file descriptor for the UART to use. It is expected to be configured
+	 * already.
+	 */
+	CRSFTelemetry(int uart_fd);
+
+	~CRSFTelemetry();
+
+	/**
+	 * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically
+	 * limit the sending rate.
+	 * @return true if new data sent
+	 */
+	bool update(const hrt_abstime &now);
+
+private:
+	bool send_battery();
+	bool send_gps();
+	bool send_attitude();
+	bool send_flight_mode();
+
+	int _vehicle_gps_position_sub;
+	int _battery_status_sub;
+	int _vehicle_attitude_sub;
+	int _vehicle_status_sub;
+
+	hrt_abstime _last_update{0};
+
+	static constexpr int num_data_types{4}; ///< number of different telemetry data types
+	int _next_type{0};
+
+	int _uart_fd;
+};