Add uavcannode accel and gyro publisher (#19885)

* Add uavcannode accel and gyro publisher.

* Add missing Kconfig

* always publish rawimu

boards/ark/can-flow/default.px4board

@@ -15,6 +15,7 @@ CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
 CONFIG_UAVCANNODE_FLOW_MEASUREMENT=y
 CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/ark/can-gps/default.px4board

@@ -20,6 +20,7 @@ CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y

boards/ark/can-rtk-gps/default.px4board

@@ -21,6 +21,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_SENSORS=y

boards/ark/cannode/default.px4board

@@ -36,6 +36,7 @@ CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
 CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
 CONFIG_UAVCANNODE_RANGE_SENSOR_MEASUREMENT=y
 CONFIG_UAVCANNODE_RAW_AIR_DATA=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_UAVCANNODE_RTK_DATA=y
 CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y

boards/ark/septentrio-gps/default.px4board

@@ -17,6 +17,7 @@ CONFIG_UAVCANNODE_GNSS_FIX=y
 CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
 CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
 CONFIG_UAVCANNODE_RTK_DATA=y
+CONFIG_UAVCANNODE_RAW_IMU=y
 CONFIG_UAVCANNODE_SAFETY_BUTTON=y
 CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y

src/drivers/uavcannode/Kconfig

@@ -58,6 +58,10 @@ if DRIVERS_UAVCANNODE
         bool "Include raw air data"
         default n
 
+    config UAVCANNODE_RAW_IMU
+        bool "Include raw IMU"
+	default n
+
     config UAVCANNODE_RTK_DATA
         bool "Include RTK data"
         default n

src/drivers/uavcannode/Publishers/RawIMU.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "UavcanPublisherBase.hpp"
+
+#include <uavcan/equipment/ahrs/RawIMU.hpp>
+
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/vehicle_imu.h>
+
+namespace uavcannode
+{
+
+class RawIMU :
+	public UavcanPublisherBase,
+	public uORB::SubscriptionCallbackWorkItem,
+	private uavcan::Publisher<uavcan::equipment::ahrs::RawIMU>
+{
+public:
+	RawIMU(px4::WorkItem *work_item, uavcan::INode &node) :
+		UavcanPublisherBase(uavcan::equipment::ahrs::RawIMU::DefaultDataTypeID),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_imu)),
+		uavcan::Publisher<uavcan::equipment::ahrs::RawIMU>(node)
+	{
+		this->setPriority(uavcan::TransferPriority::MiddleLower);
+	}
+
+	void PrintInfo() override
+	{
+		if (uORB::SubscriptionCallbackWorkItem::advertised()) {
+			printf("\t%s -> %s:%d\n",
+			       uORB::SubscriptionCallbackWorkItem::get_topic()->o_name,
+			       uavcan::equipment::ahrs::RawIMU::getDataTypeFullName(),
+			       uavcan::equipment::ahrs::RawIMU::DefaultDataTypeID);
+		}
+	}
+
+	void BroadcastAnyUpdates() override
+	{
+		// vehicle_imu -> uavcan::equipment::ahrs::RawIMU
+		vehicle_imu_s vehicle_imu;
+
+		if (uORB::SubscriptionCallbackWorkItem::update(&vehicle_imu)) {
+
+			uavcan::equipment::ahrs::RawIMU raw_imu{};
+
+			raw_imu.timestamp.usec = getNode().getUtcTime().toUSec() - (hrt_absolute_time() -
+						 vehicle_imu.timestamp_sample);
+
+			raw_imu.integration_interval = vehicle_imu.delta_angle_dt;
+			// raw_imu.integration_interval = vehicle_imu.delta_velocity_dt;
+
+			raw_imu.rate_gyro_latest[0] = (vehicle_imu.delta_angle[0] / vehicle_imu.delta_angle_dt) * 1000000;
+			raw_imu.rate_gyro_latest[1] = (vehicle_imu.delta_angle[1] / vehicle_imu.delta_angle_dt) * 1000000;
+			raw_imu.rate_gyro_latest[2] = (vehicle_imu.delta_angle[2] / vehicle_imu.delta_angle_dt) * 1000000;
+
+			raw_imu.rate_gyro_integral[0] = vehicle_imu.delta_angle[0];
+			raw_imu.rate_gyro_integral[1] = vehicle_imu.delta_angle[1];
+			raw_imu.rate_gyro_integral[2] = vehicle_imu.delta_angle[2];
+
+			raw_imu.accelerometer_latest[0] = (vehicle_imu.delta_velocity[0] / vehicle_imu.delta_velocity_dt) * 1000000;
+			raw_imu.accelerometer_latest[1] = (vehicle_imu.delta_velocity[1] / vehicle_imu.delta_velocity_dt) * 1000000;
+			raw_imu.accelerometer_latest[2] = (vehicle_imu.delta_velocity[2] / vehicle_imu.delta_velocity_dt) * 1000000;
+
+			raw_imu.accelerometer_integral[0] = vehicle_imu.delta_velocity[0];
+			raw_imu.accelerometer_integral[1] = vehicle_imu.delta_velocity[1];
+			raw_imu.accelerometer_integral[2] = vehicle_imu.delta_velocity[2];
+
+			uavcan::Publisher<uavcan::equipment::ahrs::RawIMU>::broadcast(raw_imu);
+
+			// ensure callback is registered
+			uORB::SubscriptionCallbackWorkItem::registerCallback();
+		}
+	}
+};
+} // namespace uavcannode

src/drivers/uavcannode/UavcanNode.cpp

@@ -77,6 +77,10 @@
 #include "Publishers/RawAirData.hpp"
 #endif // CONFIG_UAVCANNODE_RAW_AIR_DATA
 
+#if defined(CONFIG_UAVCANNODE_RAW_IMU)
+#include "Publishers/RawIMU.hpp"
+#endif // CONFIG_UAVCANNODE_RAW_IMU
+
 #if defined(CONFIG_UAVCANNODE_SAFETY_BUTTON)
 #include "Publishers/SafetyButton.hpp"
 #endif // CONFIG_UAVCANNODE_SAFETY_BUTTON
@@ -391,6 +395,16 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_publisher_list.add(new RawAirData(this, _node));
 #endif // CONFIG_UAVCANNODE_RAW_AIR_DATA
 
+#if defined(CONFIG_UAVCANNODE_RAW_IMU)
+	int32_t cannode_pub_raw_imu = 0;
+	param_get(param_find("CANNODE_PUB_IMU"), &cannode_pub_raw_imu);
+
+	if (cannode_pub_raw_imu == 1) {
+		_publisher_list.add(new RawIMU(this, _node));
+	}
+
+#endif // CONFIG_UAVCANNODE_RAW_IMU
+
 #if defined(CONFIG_UAVCANNODE_RTK_DATA)
 	_publisher_list.add(new RelPosHeadingPub(this, _node));
 
@@ -688,6 +702,13 @@ void UavcanNode::PrintInfo()
 
 	printf("\n");
 
+	// UAVCAN Time
+	printf("UAVCAN Time:\n");
+	printf("\tMonotonic time: %llu\n", _node.getMonotonicTime().toUSec());
+	printf("\tUtc time:       %llu\n", _node.getUtcTime().toUSec());
+
+	printf("\n");
+
 	// CAN driver status
 	for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
 		printf("CAN%u status:\n", unsigned(i + 1));

src/drivers/uavcannode/uavcannode_params.c

@@ -76,3 +76,12 @@ PARAM_DEFINE_INT32(CANNODE_SUB_RTCM, 0);
  * @group UAVCAN
  */
 PARAM_DEFINE_INT32(CANNODE_PUB_MBD, 0);
+
+/**
+ * Enable RawIMU pub
+ *
+ * @boolean
+ * @max 1
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(CANNODE_PUB_IMU, 0);