uavcan: implement OpenDroneID Location

src/drivers/uavcan/remoteid.cpp

@@ -33,12 +33,17 @@
 
 #include "remoteid.hpp"
 #include <lib/open_drone_id/open_drone_id_translations.hpp>
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
 
 UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) :
 	ModuleParams(nullptr),
 	_timer(node),
 	_node(node),
-	_uavcan_pub_remoteid_basicid(node)
+	_uavcan_pub_remoteid_basicid(node),
+	_uavcan_pub_remoteid_location(node)
 {
 }
 
@@ -52,15 +57,19 @@ int UavcanRemoteIDController::init()
 
 void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &)
 {
-	if (!_vehicle_status_sub.update()) {
-		return;
-	}
+	_vehicle_status.update();
 
+	send_basic_id();
+	send_location();
+}
+
+void UavcanRemoteIDController::send_basic_id()
+{
 	dronecan::remoteid::BasicID basic_id {};
 	// basic_id.id_or_mac // supposedly only used for drone ID data from other UAs
 	basic_id.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER;
 	basic_id.ua_type = static_cast<uint8_t>(open_drone_id_translations::odidTypeForMavType(
-			_vehicle_status_sub.get().system_type));
+			_vehicle_status.get().system_type));
 
 	// uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type
 	// TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format
@@ -71,3 +80,141 @@ void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &)
 
 	_uavcan_pub_remoteid_basicid.broadcast(basic_id);
 }
+
+void UavcanRemoteIDController::send_location()
+{
+	dronecan::remoteid::Location msg {};
+
+	// initialize all fields to unknown
+	msg.status = MAV_ODID_STATUS_UNDECLARED;
+	msg.direction = 36100; // If unknown: 36100 centi-degrees
+	msg.speed_horizontal = 25500; // If unknown: 25500 cm/s
+	msg.speed_vertical = 6300; // If unknown: 6300 cm/s
+	msg.latitude = 0; // If unknown: 0
+	msg.longitude = 0; // If unknown: 0
+	msg.altitude_geodetic = -1000; // If unknown: -1000 m
+	msg.altitude_geodetic = -1000; // If unknown: -1000 m
+	msg.height = -1000; // If unknown: -1000 m
+	msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
+	msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+	msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+	msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
+	msg.timestamp = 0xFFFF; // If unknown: 0xFFFF
+	msg.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN;
+
+	bool updated = false;
+
+	if (_vehicle_land_detected_sub.advertised()) {
+		vehicle_land_detected_s vehicle_land_detected{};
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
+		    && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 10_s)) {
+			if (vehicle_land_detected.landed) {
+				msg.status = MAV_ODID_STATUS_GROUND;
+
+			} else {
+				msg.status = MAV_ODID_STATUS_AIRBORNE;
+			}
+
+			updated = true;
+		}
+	}
+
+	if (hrt_elapsed_time(&_vehicle_status.get().timestamp) < 10_s) {
+		if (_vehicle_status.get().failsafe && (_vehicle_status.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+			msg.status = MAV_ODID_STATUS_EMERGENCY;
+			updated = true;
+		}
+	}
+
+	if (_vehicle_gps_position_sub.advertised()) {
+		sensor_gps_s vehicle_gps_position{};
+
+		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)
+		    && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 10_s)) {
+
+			if (vehicle_gps_position.vel_ned_valid) {
+				const matrix::Vector3f vel_ned{vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s};
+
+				// direction: calculate GPS course over ground angle
+				const float course = atan2f(vel_ned(1), vel_ned(0));
+				const int course_deg = roundf(math::degrees(matrix::wrap_2pi(course)));
+				msg.direction = math::constrain(100 * course_deg, 0, 35999); // 0 - 35999 centi-degrees
+
+				// speed_horizontal: If speed is larger than 25425 cm/s, use 25425 cm/s.
+				const int speed_horizontal_cm_s = matrix::Vector2f(vel_ned).length() * 100.f;
+				msg.speed_horizontal = math::constrain(speed_horizontal_cm_s, 0, 25425);
+
+				// speed_vertical: Up is positive, If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.
+				const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f);
+				msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200);
+
+				msg.speed_accuracy = open_drone_id_translations::odidSpeedAccForVariance(vehicle_gps_position.s_variance_m_s);
+
+				updated = true;
+			}
+
+			if (vehicle_gps_position.fix_type >= 2) {
+				msg.latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));
+				msg.longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));
+
+				// altitude_geodetic
+				if (vehicle_gps_position.fix_type >= 3) {
+					msg.altitude_geodetic = static_cast<float>(round(vehicle_gps_position.altitude_msl_m)); // [m]
+				}
+
+				msg.horizontal_accuracy = open_drone_id_translations::odidHorAccForEph(vehicle_gps_position.eph);
+
+				msg.vertical_accuracy = open_drone_id_translations::odidVerAccForEpv(vehicle_gps_position.epv);
+
+				updated = true;
+			}
+
+			if (vehicle_gps_position.time_utc_usec != 0) {
+				// timestamp: UTC then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000
+				uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000;
+				msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000;
+
+				msg.timestamp_accuracy = open_drone_id_translations::odidTimeForElapsed(hrt_elapsed_time(
+								 &vehicle_gps_position.timestamp));
+
+				updated = true;
+			}
+		}
+	}
+
+	// altitude_barometric: The altitude calculated from the barometric pressue
+	if (_vehicle_air_data_sub.advertised()) {
+		vehicle_air_data_s vehicle_air_data{};
+
+		if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) {
+			msg.altitude_barometric = vehicle_air_data.baro_alt_meter;
+			msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; // We just don't without calibration.
+			updated = true;
+		}
+	}
+
+	// height: The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference
+	if (_home_position_sub.advertised() && _vehicle_local_position_sub.updated()) {
+		home_position_s home_position{};
+		vehicle_local_position_s vehicle_local_position{};
+
+		if (_home_position_sub.copy(&home_position)
+		    && _vehicle_local_position_sub.copy(&vehicle_local_position)
+		    && (hrt_elapsed_time(&vehicle_local_position.timestamp) < 1_s)
+		   ) {
+
+			if (home_position.valid_alt && vehicle_local_position.z_valid && vehicle_local_position.z_global) {
+				float altitude = (-vehicle_local_position.z + vehicle_local_position.ref_alt);
+
+				msg.height = altitude - home_position.alt;
+				msg.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF;
+				updated = true;
+			}
+		}
+	}
+
+	if (updated) {
+		_uavcan_pub_remoteid_location.broadcast(msg);
+	}
+}

src/drivers/uavcan/remoteid.hpp

@@ -33,11 +33,17 @@
 
 #pragma once
 
+#include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/home_position.h>
 #include <uORB/Subscription.hpp>
 
 #include <uavcan/uavcan.hpp>
 #include <dronecan/remoteid/BasicID.hpp>
+#include <dronecan/remoteid/Location.hpp>
 
 #include <px4_platform_common/module_params.h>
 
@@ -58,16 +64,18 @@ class UavcanRemoteIDController : public ModuleParams
 
 	void periodic_update(const uavcan::TimerEvent &);
 
+	void send_basic_id();
+	void send_location();
+
 	uavcan::INode &_node;
 
-	uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 
 	uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;
-
-	//DEFINE_PARAMETERS(
-	//	(ParamInt<px4::params::UAVCAN_LGT_ANTCL>) _param_mode_anti_col,
-	//	(ParamInt<px4::params::UAVCAN_LGT_STROB>) _param_mode_strobe,
-	//	(ParamInt<px4::params::UAVCAN_LGT_NAV>) _param_mode_nav,
-	//	(ParamInt<px4::params::UAVCAN_LGT_LAND>) _param_mode_land
-	//)
+	uavcan::Publisher<dronecan::remoteid::Location> _uavcan_pub_remoteid_location;
 };