diff --git a/boards/ark/can-flow/default.px4board b/boards/ark/can-flow/default.px4board index a120df6638fa..54d5438de667 100644 --- a/boards/ark/can-flow/default.px4board +++ b/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 diff --git a/boards/ark/can-gps/default.px4board b/boards/ark/can-gps/default.px4board index 86af45774c2c..bc2492575cd2 100644 --- a/boards/ark/can-gps/default.px4board +++ b/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 diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board index b74e74a63078..44ada635996e 100644 --- a/boards/ark/can-rtk-gps/default.px4board +++ b/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 diff --git a/boards/ark/cannode/default.px4board b/boards/ark/cannode/default.px4board index d875f6b8211f..29b964b237c4 100644 --- a/boards/ark/cannode/default.px4board +++ b/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 diff --git a/boards/ark/septentrio-gps/default.px4board b/boards/ark/septentrio-gps/default.px4board index 7600200577e1..c6802bef56b0 100644 --- a/boards/ark/septentrio-gps/default.px4board +++ b/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 diff --git a/src/drivers/uavcannode/Kconfig b/src/drivers/uavcannode/Kconfig index 9aa7073a5b0f..532fe3229d31 100644 --- a/src/drivers/uavcannode/Kconfig +++ b/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 diff --git a/src/drivers/uavcannode/Publishers/RawIMU.hpp b/src/drivers/uavcannode/Publishers/RawIMU.hpp new file mode 100644 index 000000000000..6cd015e573d6 --- /dev/null +++ b/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 + +#include +#include + +namespace uavcannode +{ + +class RawIMU : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +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(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::broadcast(raw_imu); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index c67f48e4dbae..a79485dbf6da 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/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)); diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 5b6a27e44ebb..854698995463 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/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);