diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml index 407897bc17..e212cbe236 100644 --- a/conf/airframes/examples/cube_orange.xml +++ b/conf/airframes/examples/cube_orange.xml @@ -24,10 +24,8 @@ - - - - + + diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 822cd9c31b..58d9fea926 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -7,7 +7,7 @@ telemetry="telemetry/highspeed_rotorcraft.xml" flight_plan="flight_plans/rotorcraft_basic_geofence.xml" settings="settings/rotorcraft_basic.xml" - settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml" + settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml" gui_color="blue" /> - Airspeed sensor over the uavcan protocol. Currently only subscribes to the + Airspeed sensor over the uavcan protocol and optionally publishes over ABI. - + - uavcan airspeed -
- - diff --git a/conf/modules/range_sensor_uavcan.xml b/conf/modules/range_sensor_uavcan.xml new file mode 100644 index 0000000000..6e286ffb59 --- /dev/null +++ b/conf/modules/range_sensor_uavcan.xml @@ -0,0 +1,20 @@ + + + + + + Range sensor over the uavcan protocol which publishes over ABI as AGL + + + + uavcan + sonar + +
+ +
+ + + + + diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index d561924f60..770e08f887 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -197,6 +197,10 @@ #define AGL_LIDAR_MATEKSYS_3901_L0X_ID 15 #endif +#ifndef AGL_UAVCAN_ID +#define AGL_UAVCAN_ID 16 +#endif + /* * IDs of magnetometer sensors (including IMUs with mag) */ diff --git a/sw/airborne/modules/sensors/airspeed_uavcan.c b/sw/airborne/modules/sensors/airspeed_uavcan.c index 59ad87681b..eb1512e2af 100644 --- a/sw/airborne/modules/sensors/airspeed_uavcan.c +++ b/sw/airborne/modules/sensors/airspeed_uavcan.c @@ -26,15 +26,16 @@ #include "airspeed_uavcan.h" #include "uavcan/uavcan.h" #include "core/abi.h" -#include "filters/low_pass_filter.h" -#include "pprzlink/messages.h" -#include "modules/datalink/downlink.h" - -#if PERIODIC_TELEMETRY -#include "modules/datalink/telemetry.h" +/* Enable ABI sending */ +#ifndef AIRSPEED_UAVCAN_SEND_ABI +#define AIRSPEED_UAVCAN_SEND_ABI true #endif +/* Airspeed lowpass filter*/ +#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER +#include "filters/low_pass_filter.h" + #ifndef AIRSPEED_UAVCAN_LOWPASS_TAU #define AIRSPEED_UAVCAN_LOWPASS_TAU 0.15 #endif @@ -43,21 +44,26 @@ #define AIRSPEED_UAVCAN_LOWPASS_PERIOD 0.1 #endif -#ifndef AIRSPEED_UAVCAN_SEND_ABI -#define AIRSPEED_UAVCAN_SEND_ABI 1 -#endif - -#ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER static Butterworth2LowPass airspeed_filter; -#endif -static uavcan_event airspeed_uavcan_ev; +#endif /* USE_AIRSPEED_UAVCAN_LOWPASS_FILTER */ /* uavcan EQUIPMENT_ESC_STATUS message definition */ #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID 1027 #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE (0xC77DF38BA122F5DAULL) #define UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE ((397 + 7)/8) -struct airspeed_uavcan_s airspeed_uavcan; +/* Local structure */ +struct airspeed_uavcan_t { + float diff_p; ///< Differential pressure + float temperature; ///< Temperature in Celsius +}; + +/* Local variables */ +static struct airspeed_uavcan_t airspeed_uavcan = {0}; +static uavcan_event airspeed_uavcan_ev; + +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" static void airspeed_uavcan_downlink(struct transport_tx *trans, struct link_device *dev) { @@ -77,6 +83,7 @@ static void airspeed_uavcan_downlink(struct transport_tx *trans, struct link_dev &airspeed_uavcan.temperature, &airspeed); } +#endif /* PERIODIC_TELEMETRY */ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) { uint16_t tmp_float = 0; @@ -118,18 +125,15 @@ static void airspeed_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unuse void airspeed_uavcan_init(void) { - // Setup low pass filter with time constant and 100Hz sampling freq + // Setup the low pass filter #ifdef USE_AIRSPEED_UAVCAN_LOWPASS_FILTER init_butterworth_2_low_pass(&airspeed_filter, AIRSPEED_UAVCAN_LOWPASS_TAU, AIRSPEED_UAVCAN_LOWPASS_PERIOD, 0); #endif - airspeed_uavcan.diff_p = 0; - airspeed_uavcan.temperature = 0; - // Bind uavcan RAWAIRDATA message from EQUIPMENT.AIR_DATA uavcan_bind(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, &airspeed_uavcan_ev, &airspeed_uavcan_cb); - #if PERIODIC_TELEMETRY - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_uavcan_downlink); - #endif +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AIRSPEED_RAW, airspeed_uavcan_downlink); +#endif } diff --git a/sw/airborne/modules/sensors/airspeed_uavcan.h b/sw/airborne/modules/sensors/airspeed_uavcan.h index 2b10a74409..e45454ad36 100644 --- a/sw/airborne/modules/sensors/airspeed_uavcan.h +++ b/sw/airborne/modules/sensors/airspeed_uavcan.h @@ -26,15 +26,7 @@ #ifndef AIRSPEED_UAVCAN_H #define AIRSPEED_UAVCAN_H -/* External variables */ -extern struct airspeed_uavcan_s { - float diff_p; - float temperature; -} airspeed_uavcan; - /* External functions */ extern void airspeed_uavcan_init(void); - - #endif /* AIRSPEED_UAVCAN_H */ \ No newline at end of file diff --git a/sw/airborne/modules/sensors/range_sensor_uavcan.c b/sw/airborne/modules/sensors/range_sensor_uavcan.c new file mode 100644 index 0000000000..c3a33f6938 --- /dev/null +++ b/sw/airborne/modules/sensors/range_sensor_uavcan.c @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2023 Freek van Tienen + * + * This file is part of paparazzi + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file modules/sensors/range_sensor_uavcan.c + * Range sensor on the uavcan bus + */ + +#include "range_sensor_uavcan.h" +#include "uavcan/uavcan.h" +#include "core/abi.h" + + +/* uavcan EQUIPMENT_RANGE_SENSOR_MEASUREMENT message definition */ +#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID 1050 +#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE (0x68FFFE70FC771952ULL) +#define UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE ((120 + 7)/8) + +/* Local structure */ +struct range_sensor_uavcan_t { + float range; + uint8_t reading_type; +}; + +/* Local variables */ +static struct range_sensor_uavcan_t range_sensor_uavcan = {0}; +static uavcan_event range_sensor_uavcan_ev; + +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" + +static void range_sensor_uavcan_send_lidar(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t nul = 0; + pprz_msg_send_LIDAR(trans, dev, AC_ID, + &range_sensor_uavcan.range, + &range_sensor_uavcan.reading_type, + &nul); +} + +#endif + +static void range_sensor_uavcan_cb(struct uavcan_iface_t *iface __attribute__((unused)), CanardRxTransfer *transfer) { + uint16_t tmp_float = 0; + + /* Decode the message */ + //canardDecodeScalar(transfer, (uint32_t)0, 56, false, (void*)&dest->usec); + //canardDecodeScalar(transfer, (uint32_t)56, 8, false, (void*)&dest->sensor_id); + //canardDecodeScalar(transfer, (uint32_t)64, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 0)); + //canardDecodeScalar(transfer, (uint32_t)69, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 1)); + //canardDecodeScalar(transfer, (uint32_t)74, 5, true, (void*)(dest->fixed_axis_roll_pitch_yaw + 2)); + //canardDecodeScalar(transfer, (uint32_t)79, 1, false, (void*)&dest->orientation_defined); + //canardDecodeScalar(transfer, (uint32_t)80, 16, false, (void*)&tmp_float); + //float fov = canardConvertFloat16ToNativeFloat(tmp_float); + //canardDecodeScalar(transfer, (uint32_t)96, 5, false, (void*)&dest->sensor_type); + canardDecodeScalar(transfer, (uint32_t)101, 3, false, (void*)&range_sensor_uavcan.reading_type); + canardDecodeScalar(transfer, (uint32_t)104, 16, false, (void*)&tmp_float); + range_sensor_uavcan.range = canardConvertFloat16ToNativeFloat(tmp_float); + + // Send the range over ABI + if(!isnan(range_sensor_uavcan.range)) { + uint32_t now_ts = get_sys_time_usec(); + AbiSendMsgAGL(AGL_UAVCAN_ID, now_ts, range_sensor_uavcan.range); + } +} + +void range_sensor_uavcan_init(void) +{ + // Bind uavcan MEASUREMENT message from EQUIPMENT.RANGE_SENSOR + uavcan_bind(UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, &range_sensor_uavcan_ev, &range_sensor_uavcan_cb); + +#if PERIODIC_TELEMETRY + register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_LIDAR, range_sensor_uavcan_send_lidar); +#endif +} diff --git a/sw/airborne/modules/sensors/range_sensor_uavcan.h b/sw/airborne/modules/sensors/range_sensor_uavcan.h new file mode 100644 index 0000000000..c90153fd8c --- /dev/null +++ b/sw/airborne/modules/sensors/range_sensor_uavcan.h @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2023 Freek van Tienen + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** @file modules/sensors/range_sensor_uavcan.h + * Range sensor sensor on the uavcan bus + */ + +#ifndef RANGE_SENSOR_UAVCAN_H +#define RANGE_SENSOR_UAVCAN_H + +/* External functions */ +extern void range_sensor_uavcan_init(void); + +#endif /* RANGE_SENSOR_UAVCAN_H */ \ No newline at end of file