From c46fa01195b2b3b2223767f430bf6553447a0bc2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 14:56:49 -0400 Subject: [PATCH 01/19] sensors: add kconfig mechanism to optionally exclude sensor types --- src/modules/sensors/CMakeLists.txt | 33 ++++- src/modules/sensors/Kconfig | 15 ++ src/modules/sensors/sensors.cpp | 215 +++++++++++++++++++++-------- 3 files changed, 195 insertions(+), 68 deletions(-) diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0eb8a7e0a9..428e5d8fde 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2022 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 @@ -34,12 +34,22 @@ add_subdirectory(data_validator) include_directories(${CMAKE_CURRENT_SOURCE_DIR}) + add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) -add_subdirectory(vehicle_air_data) -add_subdirectory(vehicle_gps_position) add_subdirectory(vehicle_imu) -add_subdirectory(vehicle_magnetometer) + +if(CONFIG_SENSORS_VEHICLE_AIR_DATA) + add_subdirectory(vehicle_air_data) +endif() + +if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + add_subdirectory(vehicle_gps_position) +endif() + +if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + add_subdirectory(vehicle_magnetometer) +endif() px4_add_module( MODULE modules__sensors @@ -58,8 +68,17 @@ px4_add_module( sensor_calibration vehicle_acceleration vehicle_angular_velocity - vehicle_air_data - vehicle_gps_position vehicle_imu - vehicle_magnetometer ) + +if(CONFIG_SENSORS_VEHICLE_AIR_DATA) + target_link_libraries(modules__sensors PRIVATE vehicle_air_data) +endif() + +if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + target_link_libraries(modules__sensors PRIVATE vehicle_gps_position) +endif() + +if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + target_link_libraries(modules__sensors PRIVATE vehicle_magnetometer) +endif() diff --git a/src/modules/sensors/Kconfig b/src/modules/sensors/Kconfig index d7edc7cf74..cc697a3938 100644 --- a/src/modules/sensors/Kconfig +++ b/src/modules/sensors/Kconfig @@ -10,3 +10,18 @@ menuconfig USER_SENSORS depends on BOARD_PROTECTED && MODULES_SENSORS ---help--- Put sensors in userspace memory + +if MODULES_SENSORS + config SENSORS_VEHICLE_AIR_DATA + bool "Include vehicle air data" + default y + + config SENSORS_VEHICLE_GPS_POSITION + bool "Include vehicle gps position" + default y + + config SENSORS_VEHICLE_MAGNETOMETER + bool "Include vehicle magnetometer" + default y + +endif #MODULES_SENSORS diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 30ba6aeaac..6706fa3de2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 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 @@ -48,6 +48,7 @@ #include #include #include + #include #include #include @@ -61,7 +62,6 @@ #include #include #include -#include #include #include #include @@ -74,10 +74,23 @@ #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" -#include "vehicle_air_data/VehicleAirData.hpp" -#include "vehicle_gps_position/VehicleGPSPosition.hpp" + + #include "vehicle_imu/VehicleIMU.hpp" -#include "vehicle_magnetometer/VehicleMagnetometer.hpp" + +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +# include "vehicle_air_data/VehicleAirData.hpp" +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) +# include "vehicle_gps_position/VehicleGPSPosition.hpp" +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) +# include "vehicle_magnetometer/VehicleMagnetometer.hpp" +# include +# include +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER using namespace sensors; using namespace time_literals; @@ -178,17 +191,26 @@ private: VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; - VehicleAirData *_vehicle_air_data{nullptr}; - VehicleMagnetometer *_vehicle_magnetometer{nullptr}; - VehicleGPSPosition *_vehicle_gps_position{nullptr}; - VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + VehicleAirData *_vehicle_air_data {nullptr}; + uint8_t _n_baro{0}; +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + VehicleMagnetometer *_vehicle_magnetometer {nullptr}; + uint8_t _n_mag{0}; +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + VehicleGPSPosition *_vehicle_gps_position {nullptr}; + uint8_t _n_gps{0}; +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + + VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; uint8_t _n_accel{0}; - uint8_t _n_baro{0}; - uint8_t _n_gps{0}; uint8_t _n_gyro{0}; - uint8_t _n_mag{0}; /** * Update our local parameter cache. @@ -265,10 +287,7 @@ Sensors::Sensors(bool hil_enabled) : parameters_update(); - InitializeVehicleAirData(); - InitializeVehicleGPSPosition(); InitializeVehicleIMU(); - InitializeVehicleMagnetometer(); } Sensors::~Sensors() @@ -281,21 +300,33 @@ Sensors::~Sensors() _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + if (_vehicle_air_data) { _vehicle_air_data->Stop(); delete _vehicle_air_data; } +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + if (_vehicle_gps_position) { _vehicle_gps_position->Stop(); delete _vehicle_gps_position; } +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + if (_vehicle_magnetometer) { _vehicle_magnetometer->Stop(); delete _vehicle_magnetometer; } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + for (auto &vehicle_imu : _vehicle_imu_list) { if (vehicle_imu) { vehicle_imu->Stop(); @@ -331,61 +362,78 @@ int Sensors::parameters_update() _voted_sensors_update.parametersUpdate(); - // mark all existing sensor calibrations active even if sensor is missing - // this preserves the calibration in the event of a parameter export while the sensor is missing - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); - uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); - uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); - - if (device_id_accel != 0) { - calibration::Accelerometer accel_cal(device_id_accel); - } - - if (device_id_gyro != 0) { - calibration::Gyroscope gyro_cal(device_id_gyro); - } - - if (device_id_mag != 0) { - calibration::Magnetometer mag_cal(device_id_mag); - } - } - - // ensure calibration slots are active for the number of sensors currently available - // this to done to eliminate differences in the active set of parameters before and after sensor calibration + // 1. mark all existing sensor calibrations active even if sensor is missing + // this preserves the calibration in the event of a parameter export while the sensor is missing + // 2. ensure calibration slots are active for the number of sensors currently available + // this to done to eliminate differences in the active set of parameters before and after sensor calibration for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - // sensor_accel - uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; + { + const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); - if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { - calibration::Accelerometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); + if (device_id_accel != 0) { + calibration::Accelerometer accel_cal(device_id_accel); + } + + uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; + + if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { + calibration::Accelerometer cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } } + // sensor_gyro - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + { + const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); - if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { - calibration::Gyroscope cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); + if (device_id_gyro != 0) { + calibration::Gyroscope gyro_cal(device_id_gyro); + } + + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { + calibration::Gyroscope cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } } + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) // sensor_mag - uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; + { + uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); - if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { - calibration::Magnetometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); + if (device_id_mag != 0) { + calibration::Magnetometer mag_cal(device_id_mag); + } + + uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; + + if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { + calibration::Magnetometer cal; + cal.set_calibration_index(i); + cal.ParametersLoad(); + } } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER } +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) InitializeVehicleAirData(); +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) InitializeVehicleGPSPosition(); +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) InitializeVehicleMagnetometer(); +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER return PX4_OK; } @@ -563,6 +611,7 @@ void Sensors::adc_poll() #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ } +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) void Sensors::InitializeVehicleAirData() { if (_param_sys_has_baro.get()) { @@ -575,7 +624,9 @@ void Sensors::InitializeVehicleAirData() } } } +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) void Sensors::InitializeVehicleGPSPosition() { if (_param_sys_has_gps.get()) { @@ -588,6 +639,7 @@ void Sensors::InitializeVehicleGPSPosition() } } } +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION void Sensors::InitializeVehicleIMU() { @@ -624,6 +676,7 @@ void Sensors::InitializeVehicleIMU() } } +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) void Sensors::InitializeVehicleMagnetometer() { if (_param_sys_has_mag.get()) { @@ -636,6 +689,7 @@ void Sensors::InitializeVehicleMagnetometer() } } } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER void Sensors::Run() { @@ -664,18 +718,45 @@ void Sensors::Run() // when not adding sensors poll for param updates if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) { - const int n_accel = orb_group_count(ORB_ID(sensor_accel)); - const int n_baro = orb_group_count(ORB_ID(sensor_baro)); - const int n_gps = orb_group_count(ORB_ID(sensor_gps)); - const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); - const int n_mag = orb_group_count(ORB_ID(sensor_mag)); + bool updated = false; - if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) { - _n_accel = n_accel; +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + const int n_baro = orb_group_count(ORB_ID(sensor_baro)); + + if (n_baro != _n_baro) { _n_baro = n_baro; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + const int n_gps = orb_group_count(ORB_ID(sensor_gps)); + + if (n_gps != _n_gps) { _n_gps = n_gps; - _n_gyro = n_gyro; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + const int n_mag = orb_group_count(ORB_ID(sensor_mag)); + + if (n_mag != _n_mag) { _n_mag = n_mag; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + + + const int n_accel = orb_group_count(ORB_ID(sensor_accel)); + const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); + + if ((n_accel != _n_accel) || (n_gyro != _n_gyro) || updated) { + _n_accel = n_accel; + _n_gyro = n_gyro; parameters_update(); } @@ -766,16 +847,24 @@ int Sensors::print_status() { _voted_sensors_update.printStatus(); +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) + if (_vehicle_magnetometer) { PX4_INFO_RAW("\n"); _vehicle_magnetometer->PrintStatus(); } +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER + +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) + if (_vehicle_air_data) { PX4_INFO_RAW("\n"); _vehicle_air_data->PrintStatus(); } +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA + PX4_INFO_RAW("\n"); PX4_INFO_RAW("Airspeed status:\n"); _airspeed_validator.print(); @@ -786,11 +875,15 @@ int Sensors::print_status() PX4_INFO_RAW("\n"); _vehicle_angular_velocity.PrintStatus(); +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) + if (_vehicle_gps_position) { PX4_INFO_RAW("\n"); _vehicle_gps_position->PrintStatus(); } +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION + PX4_INFO_RAW("\n"); for (auto &i : _vehicle_imu_list) { From db4e09d52992cc3a5c49272bbb3d93b596c9a7a4 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 15:09:42 -0400 Subject: [PATCH 02/19] Tools/kconfig: delete old migration helpers - people are updating this without really understanding the purpose, and it's generally no longer useful --- Tools/kconfig/board_cmake_to_kconfig.py | 72 --------- Tools/kconfig/cmake_kconfig_lut.txt | 204 ------------------------ 2 files changed, 276 deletions(-) delete mode 100644 Tools/kconfig/board_cmake_to_kconfig.py delete mode 100644 Tools/kconfig/cmake_kconfig_lut.txt diff --git a/Tools/kconfig/board_cmake_to_kconfig.py b/Tools/kconfig/board_cmake_to_kconfig.py deleted file mode 100644 index 86e9c817e3..0000000000 --- a/Tools/kconfig/board_cmake_to_kconfig.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/bin/python3 - -import parse_cmake.parsing as cmp -import glob -import pprint -import re -import os - -__location__ = os.path.realpath( - os.path.join(os.getcwd(), os.path.dirname(__file__))) - -serial_regex = r"(\D\D\D\d):(/dev/ttyS\d+)" -io_regex = r"IO (.*)" -romfs_regex = r"ROMFSROOT (.*)" -arch_regex = r"ARCHITECTURE (.*)" -toolchain_regex = r"TOOLCHAIN (.*)" - - - -def stripComments(code): - code = str(code) - return re.sub(r'(?m) *#.*\n?', '', code) - -lut = {} -with open(os.path.join(__location__, "cmake_kconfig_lut.txt"),'r') as lookup: - for line in lookup: - if ',' in line: - key, value = line.strip().split(',') - lut[key] = value - -#for name in glob.glob('boards/*/*/*.cmake'): -px4_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../../')) - -for name in glob.glob(px4_dir + '/boards/*/*/*.cmake'): - print(name) - with open(name, 'r') as f: - romfs_set = False - w = open(name.replace(".cmake",".px4board"), "w") - for line in f: - clean_line = stripComments(line.strip()) - value = lut.get(clean_line) - if value is not None: - print(value, file=w) - print(value) - else: - matches = re.finditer(serial_regex, clean_line, re.MULTILINE) - for matchNum, match in enumerate(matches, start=1): - print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"") - print("CONFIG_BOARD_SERIAL_" + match.groups()[0] + "=\"" + match.groups()[1] + "\"", file=w) - matches = re.finditer(io_regex, clean_line, re.MULTILINE) - for matchNum, match in enumerate(matches, start=1): - print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"") - print("CONFIG_BOARD_IO=\"" + match.groups()[0] + "\"", file=w) - matches = re.finditer(romfs_regex, clean_line, re.MULTILINE) - for matchNum, match in enumerate(matches, start=1): - print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"") - print("CONFIG_BOARD_ROMFSROOT=\"" + match.groups()[0] + "\"", file=w) - romfs_set = True - matches = re.finditer(arch_regex, clean_line, re.MULTILINE) - for matchNum, match in enumerate(matches, start=1): - print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"") - print("CONFIG_BOARD_ARCHITECTURE=\"" + match.groups()[0] + "\"", file=w) - matches = re.finditer(toolchain_regex, clean_line, re.MULTILINE) - for matchNum, match in enumerate(matches, start=1): - print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"") - print("CONFIG_BOARD_TOOLCHAIN=\"" + match.groups()[0] + "\"", file=w) - - if(romfs_set == False): - print("CONFIG_BOARD_ROMFSROOT=\"\"", file=w) - - - w.close() diff --git a/Tools/kconfig/cmake_kconfig_lut.txt b/Tools/kconfig/cmake_kconfig_lut.txt deleted file mode 100644 index 4183da71ac..0000000000 --- a/Tools/kconfig/cmake_kconfig_lut.txt +++ /dev/null @@ -1,204 +0,0 @@ -PLATFORM nuttx,CONFIG_PLATFORM_NUTTX=y -PLATFORM posix,CONFIG_PLATFORM_POSIX=y -CONSTRAINED_MEMORY,CONFIG_BOARD_CONSTRAINED_MEMORY=y -CONSTRAINED_FLASH,CONFIG_BOARD_CONSTRAINED_FLASH=y -NO_HELP,CONFIG_BOARD_NO_HELP=y -EXTERNAL_METADATA,CONFIG_BOARD_EXTERNAL_METADATA=y -BUILD_BOOTLOADER,CONFIG_BOARD_BUILD_BOOTLOADER=y -UAVCAN_INTERFACES 2,CONFIG_BOARD_UAVCAN_INTERFACES=2 -UAVCAN_INTERFACES 1,CONFIG_BOARD_UAVCAN_INTERFACES=1 -UAVCAN_TIMER_OVERRIDE 2,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 -UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=1 -UAVCAN_TIMER_OVERRIDE 1,CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=0 -TESTING,CONFIG_BOARD_TESTING=y -ETHERNET,CONFIG_BOARD_ETHERNET=y -adc/ads1115,CONFIG_DRIVERS_ADC_ADS1115=y -adc/board_adc,CONFIG_DRIVERS_ADC_BOARD_ADC=y -barometer,CONFIG_COMMON_BAROMETERS=y -barometer/bmp280,CONFIG_DRIVERS_BAROMETER_BMP280=y -barometer/bmp388,CONFIG_DRIVERS_BAROMETER_BMP388=y -barometer/dps310,CONFIG_DRIVERS_BAROMETER_DPS310=y -barometer/lps22hb,CONFIG_DRIVERS_BAROMETER_LPS22HB=y -barometer/lps25h,CONFIG_DRIVERS_BAROMETER_LPS25H=y -barometer/lps33hw,CONFIG_DRIVERS_BAROMETER_LPS33HW=y -barometer/mpl3115a2,CONFIG_DRIVERS_BAROMETER_MPL3115A2=y -barometer/ms5611,CONFIG_DRIVERS_BAROMETER_MS5611=y -barometer/tcbp001ta,CONFIG_DRIVERS_BAROMETER_TCBP001TA=y -batt_smbus,CONFIG_DRIVERS_BATT_SMBUS=y -bootloaders,CONFIG_DRIVERS_BOOTLOADERS=y -camera_capture,CONFIG_DRIVERS_CAMERA_CAPTURE=y -camera_trigger,CONFIG_DRIVERS_CAMERA_TRIGGER=y -differential_pressure,CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE=y -distance_sensor,CONFIG_COMMON_DISTANCE_SENSOR=y -distance_sensor/ll40ls,CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y -distance_sensor/lightware_laser_serial,CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y -distance_sensor/broadcom/afbrs50,CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y -distance_sensor/vl53l0x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y -distance_sensor/vl53l1x,CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y -distance_sensor/srf05,CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y -dshot,CONFIG_DRIVERS_DSHOT=y -gps,CONFIG_DRIVERS_GPS=y -heater,CONFIG_DRIVERS_HEATER=y -imu,CONFIG_COMMON_IMU=y -imu/adis16477,CONFIG_DRIVERS_IMU_ADIS16477=y -imu/adis16497,CONFIG_DRIVERS_IMU_ADIS16497=y -imu/analog_devices/adis16448,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y -imu/analog_devices/adis16470,CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y -imu/bosch/bmi055,CONFIG_DRIVERS_IMU_BOSCH_BMI055=y -imu/bosch/bmi088,CONFIG_DRIVERS_IMU_BOSCH_BMI088=y -imu/fxas21002c,CONFIG_DRIVERS_IMU_FXAS21002C=y -imu/fxos8701cq,CONFIG_DRIVERS_IMU_FXOS8701CQ=y -imu/invensense/icm20602,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y -imu/invensense/icm20608g,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20608G=y -imu/invensense/icm20649,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y -imu/invensense/icm20689,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y -imu/invensense/icm20948,CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y -imu/invensense/icm40609d,CONFIG_DRIVERS_IMU_INVENSENSE_ICM40609D=y -imu/invensense/icm42605,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y -imu/invensense/icm42688p,CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y -imu/invensense/mpu6000,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y -imu/invensense/mpu6500,CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y -imu/invensense/mpu9250,CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y -imu/l3gd20,CONFIG_DRIVERS_IMU_L3GD20=y -imu/lsm303d,CONFIG_DRIVERS_IMU_LSM303D=y -imu/st,CONFIG_DRIVERS_IMU_ST=y -irlock,CONFIG_DRIVERS_IRLOCK=y -lights,CONFIG_COMMON_LIGHT=y -lights/neopixel,CONFIG_DRIVERS_LIGHTS_NEOPIXEL=y -lights/rgbled,CONFIG_DRIVERS_LIGHTS_RGBLED=y -lights/rgbled_ncp5623c,CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y -lights/rgbled_pwm,CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y -magnetometer,CONFIG_COMMON_MAGNETOMETER=y -magnetometer/akm/ak09916,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK09916=y -magnetometer/akm/ak8963,CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y -magnetometer/bosch/bmm150,CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y -magnetometer/hmc5883,CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y -magnetometer/isentek/ist8308,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y -magnetometer/isentek/ist8310,CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y -magnetometer/lis2mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS2MDL=y -magnetometer/lis3mdl,CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y -magnetometer/lsm303agr,CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y -magnetometer/lsm9ds1_mag,CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y -magnetometer/qmc5883l,CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y -magnetometer/rm3100,CONFIG_DRIVERS_MAGNETOMETER_RM3100=y -magnetometer/vtrantech/vcm1193l,CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y -optical_flow,CONFIG_COMMON_OPTICAL_FLOW=y -optical_flow/paw3902,CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y -optical_flow/paw3901,CONFIG_DRIVERS_OPTICAL_FLOW_PMW3901=y -optical_flow/px4flow,CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW=y -optical_flow/thoneflow,CONFIG_DRIVERS_OPTICAL_FLOW_THONEFLOW=y -osd,CONFIG_DRIVERS_OSD=y -pca9685,CONFIG_DRIVERS_PCA9685=y -pca9685_pwm_out,CONFIG_DRIVERS_PCA9685_PWM_OUT=y -power_monitor/ina226,CONFIG_DRIVERS_POWER_MONITOR_INA226=y -power_monitor/voxlpm,CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y -pps_capture,CONFIG_DRIVERS_PPS_CAPTURE=y -protocol_splitter,CONFIG_DRIVERS_PROTOCOL_SPLITTER=y -pwm_input,CONFIG_DRIVERS_PWM_INPUT=y -pwm_out_sim,CONFIG_DRIVERS_PWM_OUT_SIM=y -pwm_out,CONFIG_DRIVERS_PWM_OUT=y -px4io,CONFIG_DRIVERS_PX4IO=y -rc_input,CONFIG_DRIVERS_RC_INPUT=y -roboclaw,CONFIG_DRIVERS_ROBOCLAW=y -rpi_rc_in,CONFIG_DRIVERS_RPI_RC_IN=y -rpm,CONFIG_DRIVERS_RPM=y -safety_button,CONFIG_DRIVERS_SAFETY_BUTTON=y -smart_battery/batmon,CONFIG_DRIVERS_SMART_BATTERY_BATMON=y -spektrum_rc,CONFIG_DRIVERS_SPEKTRUM_RC=y -telemetry,CONFIG_DRIVERS_TELEMETRY=y -test_ppm,CONFIG_DRIVERS_TEST_PPM=y -tone_alarm,CONFIG_DRIVERS_TONE_ALARM=y -uavcan,CONFIG_DRIVERS_UAVCAN=y -uavcannode,CONFIG_DRIVERS_UAVCANNODE=y -uavcannode_gps_demo,CONFIG_DRIVERS_UAVCANNODE_GPS_DEMO=y -airship_att_control,CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y -airspeed_selector,CONFIG_MODULES_AIRSPEED_SELECTOR=y -velocity_controller,CONFIG_MODULES_ANGULAR_VELOCITY_CONTROLLER=y -attitude_estimator_q,CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y -battery_status,CONFIG_MODULES_BATTERY_STATUS=y -camera_feedback,CONFIG_MODULES_CAMERA_FEEDBACK=y -commander,CONFIG_MODULES_COMMANDER=y -control_allocator,CONFIG_MODULES_CONTROL_ALLOCATOR=y -dataman,CONFIG_MODULES_DATAMAN=y -ekf2,CONFIG_MODULES_EKF2=y -esc_battery,CONFIG_MODULES_ESC_BATTERY=y -events,CONFIG_MODULES_EVENTS=y -flight_mode_manager,CONFIG_MODULES_FLIGHT_MODE_MANAGER=y -fw_att_control,CONFIG_MODULES_FW_ATT_CONTROL=y -fw_pos_control_l1,CONFIG_MODULES_FW_POS_CONTROL_L1=y -gyro_calibration,CONFIG_MODULES_GYRO_CALIBRATION=y -gyro_fft,CONFIG_MODULES_GYRO_FFT=y -land_detector,CONFIG_MODULES_LAND_DETECTOR=y -landing_target_estimator,CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y -load_mon,CONFIG_MODULES_LOAD_MON=y -local_position_estimator,CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y -logger,CONFIG_MODULES_LOGGER=y -mavlink,CONFIG_MODULES_MAVLINK=y -mc_att_control,CONFIG_MODULES_MC_ATT_CONTROL=y -mc_hover_thrust_estimator,CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y -mc_pos_control,CONFIG_MODULES_MC_POS_CONTROL=y -mc_rate_control,CONFIG_MODULES_MC_RATE_CONTROL=y -micrortps_bridge,CONFIG_MODULES_MICRORTPS_BRIDGE=y -microdds_client,CONFIG_MODULES_MICRODDS_CLIENT=y -navigator,CONFIG_MODULES_NAVIGATOR=y -px4iofirmware,CONFIG_MODULES_PX4IOFIRMWARE=y -rc_update,CONFIG_MODULES_RC_UPDATE=y -replay,CONFIG_MODULES_REPLAY=y -rover_pos_control,CONFIG_MODULES_ROVER_POS_CONTROL=y -sensors,CONFIG_MODULES_SENSORS=y -sih,CONFIG_MODULES_SIH=y -simulator,CONFIG_MODULES_SIMULATOR=y -temperature_compensation,CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -uuv_att_control,CONFIG_MODULES_UUV_ATT_CONTROL=y -uuv_pos_control,CONFIG_MODULES_UUV_POS_CONTROL=y -gimbal,CONFIG_MODULES_GIMBAL=y -vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y -bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y -dmesg,CONFIG_SYSTEMCMDS_DMESG=y -dumpfile,CONFIG_SYSTEMCMDS_DUMPFILE=y -dyn,CONFIG_SYSTEMCMDS_DYN=y -failure,CONFIG_SYSTEMCMDS_FAILURE=y -gpio,CONFIG_SYSTEMCMDS_GPIO=y -hardfault_log,CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y -i2cdetect,CONFIG_SYSTEMCMDS_I2CDETECT=y -led_control,CONFIG_SYSTEMCMDS_LED_CONTROL=y -mft,CONFIG_SYSTEMCMDS_MFT=y -microbench,CONFIG_SYSTEMCMDS_MICROBENCH=y -mixer,CONFIG_SYSTEMCMDS_MIXER=y -motor_test,CONFIG_SYSTEMCMDS_MOTOR_TEST=y -mtd,CONFIG_SYSTEMCMDS_MTD=y -netman,CONFIG_SYSTEMCMDS_NETMAN=y -nshterm,CONFIG_SYSTEMCMDS_NSHTERM=y -param,CONFIG_SYSTEMCMDS_PARAM=y -perf,CONFIG_SYSTEMCMDS_PERF=y -pwm,CONFIG_SYSTEMCMDS_PWM=y -reboot,CONFIG_SYSTEMCMDS_REBOOT=y -reflect,CONFIG_SYSTEMCMDS_REFLECT=y -sd_bench,CONFIG_SYSTEMCMDS_SD_BENCH=y -serial_tet,CONFIG_SYSTEMCMDS_SERIAL_TEST=y -shutdown,CONFIG_SYSTEMCMDS_SHUTDOWN=y -system_time,CONFIG_SYSTEMCMDS_SYSTEM_TIME=y -tests,CONFIG_SYSTEMCMDS_TESTS=y -top,CONFIG_SYSTEMCMDS_TOP=y -topic_listener,CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y -tune_control,CONFIG_SYSTEMCMDS_TUNE_CONTROL=y -uorb,CONFIG_SYSTEMCMDS_UORB=y -usb_connected,CONFIG_SYSTEMCMDS_USB_CONNECTED=y -ver,CONFIG_SYSTEMCMDS_VER=y -work_queue,CONFIG_SYSTEMCMDS_WORK_QUEUE=y -dyn_hello,CONFIG_EXAMPLES_DYN_HELLO=y -fake_gps,CONFIG_EXAMPLES_FAKE_GPS=y -fake_gyro,CONFIG_EXAMPLES_FAKE_GYRO=y -fake_imu,CONFIG_EXAMPLES_FAKE_IMU=y -fake_magnetometer,CONFIG_EXAMPLES_FAKE_MAGNETOMETER=y -fixedwing_control,CONFIG_EXAMPLES_FIXEDWING_CONTROL=y -hello,CONFIG_EXAMPLES_HELLO=y -hwtest,CONFIG_EXAMPLES_HWTEST=y -matlab_csv_serial,CONFIG_EXAMPLES_MATLAB_CSV_SERIAL=y -px4_mavlink_debug,CONFIG_EXAMPLES_PX4_MAVLINK_DEBUG=y -px4_simple_app,CONFIG_EXAMPLES_PX4_SIMPLE_APP=y -rover_steering_control,CONFIG_EXAMPLES_ROVER_STEERING_CONTROL=y -uuv_example_app,CONFIG_EXAMPLES_UUV_EXAMPLE_APP=y -work_item,CONFIG_EXAMPLES_WORK_ITEM=y -add_compile_options(-Wno-narrowing),CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing" --D__PX4_LINUX,CONFIG_BOARD_LINUX=y From 10deb7019e1930833ed01d8de4a839f6ebe577fa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 15:12:27 -0400 Subject: [PATCH 03/19] enable gyro_calibration for CAN nodes - allow saving initial gyro cal if very close to 0 --- ROMFS/cannode/init.d/rcS | 9 ++++++++- boards/ark/can-flow/default.px4board | 8 ++++++++ boards/ark/can-flow/init/rc.board_sensors | 2 ++ src/lib/sensor_calibration/Gyroscope.cpp | 2 +- src/modules/gyro_calibration/CMakeLists.txt | 3 ++- src/modules/gyro_calibration/GyroCalibration.cpp | 4 ++-- src/modules/gyro_calibration/GyroCalibration.hpp | 2 +- 7 files changed, 24 insertions(+), 6 deletions(-) diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index 6e53056245..e2fd2817b6 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -86,10 +86,17 @@ unset BOARD_RC_SENSORS . ${R}etc/init.d/rc.serial # Check for flow sensor -if param compare SENS_EN_PX4FLOW 1 +if param compare -s SENS_EN_PX4FLOW 1 then px4flow start -X & fi +if param compare -s IMU_GYRO_CAL_EN 1 +then + gyro_calibration start +fi + +sensors start + uavcannode start unset R diff --git a/boards/ark/can-flow/default.px4board b/boards/ark/can-flow/default.px4board index 3036ca5a87..6df227e138 100644 --- a/boards/ark/can-flow/default.px4board +++ b/boards/ark/can-flow/default.px4board @@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y @@ -10,7 +11,14 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set +# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/can-flow/init/rc.board_sensors b/boards/ark/can-flow/init/rc.board_sensors index 38d38e9288..b7502f98fd 100644 --- a/boards/ark/can-flow/init/rc.board_sensors +++ b/boards/ark/can-flow/init/rc.board_sensors @@ -3,6 +3,8 @@ # board sensors init #------------------------------------------------------------------------------ +param set-default IMU_GYRO_RATEMAX 1000 + # Internal SPI paw3902 -s start -Y 180 diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index c1adf1ca2a..c953e98592 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -110,7 +110,7 @@ void Gyroscope::SensorCorrectionsUpdate(bool force) bool Gyroscope::set_offset(const Vector3f &offset) { - if (Vector3f(_offset - offset).longerThan(0.01f)) { + if (Vector3f(_offset - offset).longerThan(0.01f) || (_calibration_count == 0)) { if (PX4_ISFINITE(offset(0)) && PX4_ISFINITE(offset(1)) && PX4_ISFINITE(offset(2))) { _offset = offset; _calibration_count++; diff --git a/src/modules/gyro_calibration/CMakeLists.txt b/src/modules/gyro_calibration/CMakeLists.txt index f8f73f1df0..cc5381d57b 100644 --- a/src/modules/gyro_calibration/CMakeLists.txt +++ b/src/modules/gyro_calibration/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2021-2022 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 @@ -41,4 +41,5 @@ px4_add_module( GyroCalibration.hpp DEPENDS px4_work_queue + sensor_calibration ) diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp index 51cb743628..a832b3bfba 100644 --- a/src/modules/gyro_calibration/GyroCalibration.cpp +++ b/src/modules/gyro_calibration/GyroCalibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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 @@ -245,7 +245,7 @@ void GyroCalibration::Run() const Vector3f old_offset{_gyro_calibration[gyro].offset()}; - if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) { + if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean()) || !_gyro_calibration[gyro].calibrated()) { calibration_updated = true; diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp index 9d1047d6b3..c12d24f8bc 100644 --- a/src/modules/gyro_calibration/GyroCalibration.hpp +++ b/src/modules/gyro_calibration/GyroCalibration.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-2022 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 From c7cec4252cf2d1865da89698b7e546184a274c5e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 15:39:41 -0400 Subject: [PATCH 04/19] sensors: add CONFIG_SENSORS_VEHICLE_AIRSPEED for airspeed/differential pressure - disable CONFIG_SENSORS_VEHICLE_AIRSPEED on boards only used for multicopter --- boards/ark/can-flow/default.px4board | 2 + boards/ark/can-gps/default.px4board | 9 +- boards/ark/can-rtk-gps/default.px4board | 9 +- boards/bitcraze/crazyflie/default.px4board | 1 + boards/bitcraze/crazyflie21/default.px4board | 1 + boards/holybro/kakutef7/default.px4board | 1 + boards/holybro/kakuteh7/default.px4board | 9 +- boards/omnibus/f4sd/default.px4board | 1 + boards/spracing/h7extreme/default.px4board | 1 + src/modules/sensors/CMakeLists.txt | 5 +- src/modules/sensors/Kconfig | 4 + src/modules/sensors/sensors.cpp | 94 ++++++++++--------- .../sensors/vehicle_air_data/CMakeLists.txt | 1 + .../vehicle_magnetometer/CMakeLists.txt | 1 + 14 files changed, 89 insertions(+), 50 deletions(-) diff --git a/boards/ark/can-flow/default.px4board b/boards/ark/can-flow/default.px4board index 6df227e138..48b3132cc3 100644 --- a/boards/ark/can-flow/default.px4board +++ b/boards/ark/can-flow/default.px4board @@ -13,6 +13,7 @@ CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set # CONFIG_SENSORS_VEHICLE_AIR_DATA is not set # CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set # CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set @@ -21,4 +22,5 @@ CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/can-gps/default.px4board b/boards/ark/can-gps/default.px4board index ad4ec22ad8..151aeda7bc 100644 --- a/boards/ark/can-gps/default.px4board +++ b/boards/ark/can-gps/default.px4board @@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y -CONFIG_BOARD_EXTERNAL_METADATA=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y @@ -14,5 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board index ad4ec22ad8..151aeda7bc 100644 --- a/boards/ark/can-rtk-gps/default.px4board +++ b/boards/ark/can-rtk-gps/default.px4board @@ -4,7 +4,6 @@ CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y -CONFIG_BOARD_EXTERNAL_METADATA=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y @@ -14,5 +13,13 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 80437d3812..41509df016 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -30,6 +30,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MFT=y diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 61b075eb0c..5f06c0fef9 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -30,6 +30,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MIXER=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index f2c412b7c4..d82f8bb1da 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_MIXER=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PWM=y diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 095afea53d..f124262a0b 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -15,10 +15,10 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y -CONFIG_DRIVERS_OSD=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PWM_OUT_SIM=y @@ -28,8 +28,8 @@ CONFIG_DRIVERS_RPM=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y @@ -42,6 +42,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y @@ -60,12 +61,12 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_MODULES_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y -CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_VTOL_ATT_CONTROL=y -CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 942328ce95..05506d3c2d 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MIXER=y CONFIG_SYSTEMCMDS_PARAM=y diff --git a/boards/spracing/h7extreme/default.px4board b/boards/spracing/h7extreme/default.px4board index 5e50b09833..d1463c40f6 100644 --- a/boards/spracing/h7extreme/default.px4board +++ b/boards/spracing/h7extreme/default.px4board @@ -41,6 +41,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 428e5d8fde..120c478212 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -61,7 +61,6 @@ px4_add_module( MODULE_CONFIG module.yaml DEPENDS - airspeed conversion data_validator mathlib @@ -71,6 +70,10 @@ px4_add_module( vehicle_imu ) +if(CONFIG_SENSORS_VEHICLE_AIRSPEED) + target_link_libraries(modules__sensors PRIVATE airspeed) +endif() + if(CONFIG_SENSORS_VEHICLE_AIR_DATA) target_link_libraries(modules__sensors PRIVATE vehicle_air_data) endif() diff --git a/src/modules/sensors/Kconfig b/src/modules/sensors/Kconfig index cc697a3938..67863ec53f 100644 --- a/src/modules/sensors/Kconfig +++ b/src/modules/sensors/Kconfig @@ -12,6 +12,10 @@ menuconfig USER_SENSORS Put sensors in userspace memory if MODULES_SENSORS + config SENSORS_VEHICLE_AIRSPEED + bool "Include vehicle airspeed" + default y + config SENSORS_VEHICLE_AIR_DATA bool "Include vehicle air data" default y diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6706fa3de2..a9d1840fb4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -41,10 +41,7 @@ * @author Beat Küng */ -#include #include -#include -#include #include #include #include @@ -62,23 +59,27 @@ #include #include #include -#include -#include #include -#include #include -#include #include #include #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" - - #include "vehicle_imu/VehicleIMU.hpp" +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +# include +# include +# include +# include +# include +# include +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) +# include # include "vehicle_air_data/VehicleAirData.hpp" #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA @@ -140,16 +141,18 @@ private: }; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::Subscription _diff_pres_sub{ORB_ID(differential_pressure)}; uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; perf_counter_t _loop_perf; /**< loop performance counter */ +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + + uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; + DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ uint64_t _airspeed_last_publish{0}; @@ -159,11 +162,10 @@ private: float _baro_pressure_sum{0.f}; int _diff_pres_count{0}; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL +# ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL uORB::Subscription _adc_report_sub {ORB_ID(adc_report)}; uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - +# endif // ADC_AIRSPEED_VOLTAGE_CHANNEL struct Parameters { float diff_pres_offset_pa; @@ -186,6 +188,7 @@ private: param_t air_tube_length; param_t air_tube_diameter_mm; } _parameter_handles{}; /**< handles for interesting parameters */ +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED VotedSensorsUpdate _voted_sensors_update; @@ -217,6 +220,7 @@ private: */ int parameters_update(); +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /** * Poll the differential pressure sensor for updated data. * @@ -225,11 +229,6 @@ private: */ void diff_pres_poll(); - /** - * Check for changes in parameters. - */ - void parameter_update_poll(bool forced = false); - /** * Poll the ADC and update readings to suit. * @@ -237,6 +236,7 @@ private: * data should be returned. */ void adc_poll(); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED void InitializeVehicleAirData(); void InitializeVehicleGPSPosition(); @@ -244,9 +244,15 @@ private: void InitializeVehicleMagnetometer(); DEFINE_PARAMETERS( +#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) (ParamBool) _param_sys_has_baro, +#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) (ParamBool) _param_sys_has_gps, +#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) (ParamBool) _param_sys_has_mag, +#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER (ParamBool) _param_sens_imu_mode ) }; @@ -263,6 +269,7 @@ Sensors::Sensors(bool hil_enabled) : _vehicle_angular_velocity.Start(); _vehicle_acceleration.Start(); +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -273,6 +280,10 @@ Sensors::Sensors(bool hil_enabled) : _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); + _airspeed_validator.set_timeout(300000); + _airspeed_validator.set_equal_value_threshold(100); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + param_find("SYS_FAC_CAL_MODE"); // Parameters controlling the on-board sensor thermal calibrator @@ -282,9 +293,6 @@ Sensors::Sensors(bool hil_enabled) : _sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - _airspeed_validator.set_timeout(300000); - _airspeed_validator.set_equal_value_threshold(100); - parameters_update(); InitializeVehicleIMU(); @@ -350,6 +358,7 @@ int Sensors::parameters_update() return 0; } +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -359,6 +368,7 @@ int Sensors::parameters_update() param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel); param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED _voted_sensors_update.parametersUpdate(); @@ -438,6 +448,7 @@ int Sensors::parameters_update() return PX4_OK; } +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) void Sensors::diff_pres_poll() { differential_pressure_s diff_pres{}; @@ -545,21 +556,6 @@ void Sensors::diff_pres_poll() } } -void -Sensors::parameter_update_poll(bool forced) -{ - // check for parameter updates - if (_parameter_update_sub.updated() || forced) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - parameters_update(); - updateParams(); - } -} - void Sensors::adc_poll() { /* only read if not in HIL mode */ @@ -608,8 +604,9 @@ void Sensors::adc_poll() } } -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ +#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL } +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED) #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) void Sensors::InitializeVehicleAirData() @@ -768,8 +765,16 @@ void Sensors::Run() _last_config_update = hrt_absolute_time(); } else { - // check parameters for updates - parameter_update_poll(); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + parameters_update(); + updateParams(); + } } _voted_sensors_update.sensorsPoll(_sensor_combined); @@ -781,10 +786,11 @@ void Sensors::Run() _sensor_combined_prev_timestamp = _sensor_combined.timestamp; } +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) // check analog airspeed adc_poll(); - diff_pres_poll(); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED // backup schedule as a watchdog timeout ScheduleDelayed(10_ms); @@ -865,9 +871,11 @@ int Sensors::print_status() #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) PX4_INFO_RAW("\n"); PX4_INFO_RAW("Airspeed status:\n"); _airspeed_validator.print(); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED PX4_INFO_RAW("\n"); _vehicle_acceleration.PrintStatus(); diff --git a/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/vehicle_air_data/CMakeLists.txt index 55ece1fcac..c8457cc2df 100644 --- a/src/modules/sensors/vehicle_air_data/CMakeLists.txt +++ b/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -37,6 +37,7 @@ px4_add_library(vehicle_air_data ) target_link_libraries(vehicle_air_data PRIVATE + data_validator px4_work_queue sensor_calibration ) diff --git a/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt b/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt index a864b955e2..0fad202f50 100644 --- a/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt +++ b/src/modules/sensors/vehicle_magnetometer/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_library(vehicle_magnetometer target_link_libraries(vehicle_magnetometer PRIVATE + data_validator px4_work_queue sensor_calibration ) From b81a5b3efad6ba6a15dcc269e7537c16a6572c22 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 8 Mar 2022 11:13:13 -0500 Subject: [PATCH 05/19] ekf2: request mag yaw reset after calibration or sensor change --- src/modules/ekf2/EKF/ekf.h | 3 ++- src/modules/ekf2/EKF/ekf_helper.cpp | 8 +++++++- src/modules/ekf2/EKF2.cpp | 4 ++-- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a0d39576b2..b74517f6bb 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -193,7 +193,8 @@ public: void resetAccelBias(); // Reset all magnetometer bias states and covariances to initial alignment values. - void resetMagBias(); + // Requests full mag yaw reset (if using mag) + void resetMagBiasAndYaw(); Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a731bc8047..5c54dc36c8 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -907,7 +907,7 @@ void Ekf::resetAccelBias() _prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag(); } -void Ekf::resetMagBias() +void Ekf::resetMagBiasAndYaw() { // Zero the magnetometer bias states _state.mag_B.zero(); @@ -918,6 +918,12 @@ void Ekf::resetMagBias() // reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion _saved_mag_bf_variance.zero(); + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + _mag_yaw_reset_req = true; + } + + _control_status.flags.mag_fault = false; } // get EKF innovation consistency check status information comprising of: diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e8f72196d1..9b3b40e3d2 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1818,14 +1818,14 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) reset = true; - } else if (magnetometer.calibration_count > _mag_calibration_count) { + } else if (magnetometer.calibration_count != _mag_calibration_count) { // existing calibration has changed, reset saved mag bias PX4_DEBUG("%d - mag %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_mag); reset = true; } if (reset) { - _ekf.resetMagBias(); + _ekf.resetMagBiasAndYaw(); _device_id_mag = magnetometer.device_id; _mag_calibration_count = magnetometer.calibration_count; From 902b789292fd18d5af67054b3108948d1c5db6f9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Jun 2022 21:29:09 -0400 Subject: [PATCH 06/19] boards: px4_fmu-v5_protected disable gyro_fft module to save flash --- boards/px4/fmu-v5/protected.px4board | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index 5937929c45..d51a7314d5 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -1,11 +1,9 @@ -CONFIG_BOARD_PROTECTED=y +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set CONFIG_COMMON_BAROMETERS=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_OPTICAL_FLOW=n CONFIG_COMMON_TELEMETRY=n -CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y CONFIG_DRIVERS_OSD=n CONFIG_DRIVERS_ROBOCLAW=n CONFIG_DRIVERS_RPM=n @@ -17,8 +15,8 @@ CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_FW_POS_CONTROL=n CONFIG_MODULES_FW_POS_CONTROL_L1=n +CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n @@ -29,11 +27,9 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n CONFIG_SYSTEMCMDS_BL_UPDATE=n CONFIG_SYSTEMCMDS_DMESG=n CONFIG_SYSTEMCMDS_DUMPFILE=n -CONFIG_SYSTEMCMDS_ESC_CALIB=n CONFIG_SYSTEMCMDS_GPIO=n CONFIG_SYSTEMCMDS_I2CDETECT=n CONFIG_SYSTEMCMDS_LED_CONTROL=n -CONFIG_SYSTEMCMDS_MOTOR_RAMP=n CONFIG_SYSTEMCMDS_MOTOR_TEST=n CONFIG_SYSTEMCMDS_MTD=n CONFIG_SYSTEMCMDS_NSHTERM=n @@ -43,3 +39,6 @@ CONFIG_SYSTEMCMDS_SD_STRESS=n CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_SYSTEMCMDS_SYSTEM_TIME=n CONFIG_SYSTEMCMDS_USB_CONNECTED=n +CONFIG_BOARD_PROTECTED=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y From 73f45fee6e775fffe0320f5945f257b51b5b84fe Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 Jun 2022 21:29:24 -0400 Subject: [PATCH 07/19] boards: px4_fmu-v5_test disable fake_gps module to save flash --- boards/px4/fmu-v5/test.px4board | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board index 983aaf05cf..d60d76b54b 100644 --- a/boards/px4/fmu-v5/test.px4board +++ b/boards/px4/fmu-v5/test.px4board @@ -4,6 +4,7 @@ CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_RPM=n +CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n From 4cc3e785588a878ef91ac3293de1c9fa54e2675d Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Fri, 17 Jun 2022 18:36:55 -0600 Subject: [PATCH 08/19] Add mag bias estimator to CAN GPS units with IMUs - run `sensors` hub to process sensor_mag and publish vehicle_magnetometer - update uavcannode to use vehicle_magnetometer --- ROMFS/cannode/init.d/rcS | 11 +++++++++++ boards/ark/can-gps/default.px4board | 5 +++++ boards/ark/can-gps/init/rc.board_defaults | 1 + boards/ark/can-rtk-gps/default.px4board | 5 +++++ boards/ark/can-rtk-gps/init/rc.board_defaults | 1 + boards/cuav/can-gps-v1/default.px4board | 13 +++++++++++++ boards/freefly/can-rtk-gps/default.px4board | 12 ++++++++++-- boards/holybro/can-gps-v1/default.px4board | 13 +++++++++++++ boards/matek/gnss-m9n-f4/default.px4board | 9 +++++++++ .../Publishers/MagneticFieldStrength2.hpp | 18 +++++++++--------- src/modules/sensors/sensor_params_mag.c | 2 +- .../VehicleMagnetometer.cpp | 9 +++++++++ 12 files changed, 87 insertions(+), 12 deletions(-) diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index e2fd2817b6..519f9db889 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -96,6 +96,17 @@ then gyro_calibration start fi + +if param compare -s MBE_ENABLE 1 +then + # conservative mag bias estimation + param set-default MBE_LEARN_GAIN 5 + param set-default IMU_GYRO_CUTOFF 20 + mag_bias_estimator start +fi + +param set-default SENS_MAG_RATE 100 + sensors start uavcannode start diff --git a/boards/ark/can-gps/default.px4board b/boards/ark/can-gps/default.px4board index 151aeda7bc..8f69c01983 100644 --- a/boards/ark/can-gps/default.px4board +++ b/boards/ark/can-gps/default.px4board @@ -14,10 +14,15 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y diff --git a/boards/ark/can-gps/init/rc.board_defaults b/boards/ark/can-gps/init/rc.board_defaults index 7f8139aceb..480d2c92c5 100644 --- a/boards/ark/can-gps/init/rc.board_defaults +++ b/boards/ark/can-gps/init/rc.board_defaults @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ param set-default CBRK_IO_SAFETY 0 +param set-default MBE_ENABLE 1 safety_button start tone_alarm start diff --git a/boards/ark/can-rtk-gps/default.px4board b/boards/ark/can-rtk-gps/default.px4board index 151aeda7bc..8f69c01983 100644 --- a/boards/ark/can-rtk-gps/default.px4board +++ b/boards/ark/can-rtk-gps/default.px4board @@ -14,10 +14,15 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index 4e172c2c3c..25bc347662 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -5,6 +5,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_GPS_RTCM 1 +param set-default MBE_ENABLE 1 safety_button start tone_alarm start diff --git a/boards/cuav/can-gps-v1/default.px4board b/boards/cuav/can-gps-v1/default.px4board index 6a12a5eb40..3a838e1cbe 100644 --- a/boards/cuav/can-gps-v1/default.px4board +++ b/boards/cuav/can-gps-v1/default.px4board @@ -1,6 +1,8 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_COMPILE_DEFINITIONS="-DUSE_S_RGB_LED_DMA" CONFIG_DRIVERS_BAROMETER_MS5611=y @@ -12,5 +14,16 @@ CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/freefly/can-rtk-gps/default.px4board b/boards/freefly/can-rtk-gps/default.px4board index 83619d320a..85b1558597 100644 --- a/boards/freefly/can-rtk-gps/default.px4board +++ b/boards/freefly/can-rtk-gps/default.px4board @@ -1,16 +1,22 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m7" CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y +CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y -CONFIG_DRIVERS_IMU_ST=y CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y -CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_LED_CONTROL=y CONFIG_SYSTEMCMDS_MFT=y CONFIG_SYSTEMCMDS_MTD=y @@ -19,5 +25,7 @@ CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/holybro/can-gps-v1/default.px4board b/boards/holybro/can-gps-v1/default.px4board index 09929ddfa4..60ae33341f 100644 --- a/boards/holybro/can-gps-v1/default.px4board +++ b/boards/holybro/can-gps-v1/default.px4board @@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y @@ -12,6 +13,18 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/matek/gnss-m9n-f4/default.px4board b/boards/matek/gnss-m9n-f4/default.px4board index 55a22c564a..230f1e85a5 100644 --- a/boards/matek/gnss-m9n-f4/default.px4board +++ b/boards/matek/gnss-m9n-f4/default.px4board @@ -1,6 +1,8 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" +CONFIG_BOARD_CONSTRAINED_FLASH=y +CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" CONFIG_DRIVERS_BAROMETER_DPS310=y @@ -10,13 +12,20 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_MAGNETOMETER_RM3100=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_DRIVERS_UAVCANNODE=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y +# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set +# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y CONFIG_SERIAL_PASSTHRU_UBLOX=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y CONFIG_SYSTEMCMDS_UORB=y diff --git a/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp index 6b48fc099c..6a0e9adc63 100644 --- a/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp +++ b/src/drivers/uavcannode/Publishers/MagneticFieldStrength2.hpp @@ -38,7 +38,7 @@ #include #include -#include +#include namespace uavcannode { @@ -51,7 +51,7 @@ class MagneticFieldStrength2 : public: MagneticFieldStrength2(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(uavcan::equipment::ahrs::MagneticFieldStrength2::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_mag)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_magnetometer)), uavcan::Publisher(node) { this->setPriority(uavcan::TransferPriority::Default); @@ -69,15 +69,15 @@ public: void BroadcastAnyUpdates() override { - // sensor_mag -> uavcan::equipment::ahrs::MagneticFieldStrength2 - sensor_mag_s mag; + // vehicle_magnetometer -> uavcan::equipment::ahrs::MagneticFieldStrength2 + vehicle_magnetometer_s vehicle_magnetometer; - if (uORB::SubscriptionCallbackWorkItem::update(&mag)) { + if (uORB::SubscriptionCallbackWorkItem::update(&vehicle_magnetometer)) { uavcan::equipment::ahrs::MagneticFieldStrength2 magnetic_field{}; - magnetic_field.sensor_id = mag.device_id; - magnetic_field.magnetic_field_ga[0] = mag.x; - magnetic_field.magnetic_field_ga[1] = mag.y; - magnetic_field.magnetic_field_ga[2] = mag.z; + magnetic_field.sensor_id = uORB::SubscriptionCallbackWorkItem::get_instance(); + magnetic_field.magnetic_field_ga[0] = vehicle_magnetometer.magnetometer_ga[0]; + magnetic_field.magnetic_field_ga[1] = vehicle_magnetometer.magnetometer_ga[1]; + magnetic_field.magnetic_field_ga[2] = vehicle_magnetometer.magnetometer_ga[2]; uavcan::Publisher::broadcast(magnetic_field); // ensure callback is registered diff --git a/src/modules/sensors/sensor_params_mag.c b/src/modules/sensors/sensor_params_mag.c index 2b373800ac..6a904137ac 100644 --- a/src/modules/sensors/sensor_params_mag.c +++ b/src/modules/sensors/sensor_params_mag.c @@ -116,4 +116,4 @@ PARAM_DEFINE_INT32(SENS_MAG_MODE, 1); * @category system * @group Sensors */ -PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 0); +PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 1); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 48c50365c4..157413c4d5 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -205,6 +205,8 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index] && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) { + const Vector3f old_offset = _calibration[mag_index].offset(); + // set initial mag calibration const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); @@ -212,6 +214,13 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() // save parameters with preferred calibration slot to current sensor index _calibration[mag_index].ParametersSave(mag_index); + PX4_INFO("mag %d (%" PRIu32 ") setting offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f]", + mag_index, _calibration[mag_index].device_id(), + (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), + (double)_calibration[mag_index].offset()(0), + (double)_calibration[mag_index].offset()(1), + (double)_calibration[mag_index].offset()(2)); + _calibration_estimator_bias[mag_index].zero(); parameters_notify = true; From 714234ca90f116ead5ac4b8d2941d7d96a53b003 Mon Sep 17 00:00:00 2001 From: Bruce Meagher Date: Mon, 20 Jun 2022 02:51:47 -0700 Subject: [PATCH 09/19] posix: add mavlink shell for posix targets (#19800) --- .../posix/src/px4/common/px4_daemon/pxh.cpp | 282 +++++++++++++++++- .../posix/src/px4/common/px4_daemon/pxh.h | 12 +- .../src/px4/common/px4_daemon/server.cpp | 8 +- src/modules/mavlink/mavlink_shell.cpp | 50 +++- 4 files changed, 328 insertions(+), 24 deletions(-) diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp index 82d8021a3a..c17bbda267 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015-2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2015-2022 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 @@ -43,28 +43,32 @@ #include #include #include +#include #include +#include +#include +#include #include "pxh.h" namespace px4_daemon { -Pxh *Pxh::_instance = nullptr; - apps_map_type Pxh::_apps = {}; - +Pxh *Pxh::_instance = nullptr; Pxh::Pxh() { _history.try_to_add("commander takeoff"); // for convenience _history.reset_to_end(); - _instance = this; } Pxh::~Pxh() { - _instance = nullptr; + if (_local_terminal) { + tcsetattr(0, TCSANOW, &_orig_term); + _instance = nullptr; + } } int Pxh::process_line(const std::string &line, bool silently_fail) @@ -133,11 +137,167 @@ int Pxh::process_line(const std::string &line, bool silently_fail) } } +void Pxh::_check_remote_uorb_command(std::string &line) +{ + + if (line.empty()) { + return; + } + + std::stringstream line_stream(line); + std::string word; + + line_stream >> word; + + if (word == "uorb") { + line += " -1"; // Run uorb command only once + } +} + +void Pxh::run_remote_pxh(int remote_in_fd, int remote_out_fd) +{ + std::string mystr; + int p1[2], pipe_stdout; + int p2[2], pipe_stderr; + int backup_stdout_fd = dup(STDOUT_FILENO); + int backup_stderr_fd = dup(STDERR_FILENO); + + if (pipe(p1) != 0) { + perror("Remote shell pipe creation failed"); + return; + } + + if (pipe(p2) != 0) { + perror("Remote shell pipe 2 creation failed"); + close(p1[0]); + close(p1[1]); + return; + } + + // Create pipe to receive stdout and stderr + dup2(p1[1], STDOUT_FILENO); + close(p1[1]); + + dup2(p2[1], STDERR_FILENO); + close(p2[1]); + + pipe_stdout = p1[0]; + pipe_stderr = p2[0]; + + // Set fds for non-blocking operation + fcntl(pipe_stdout, F_SETFL, fcntl(pipe_stdout, F_GETFL) | O_NONBLOCK); + fcntl(pipe_stderr, F_SETFL, fcntl(pipe_stderr, F_GETFL) | O_NONBLOCK); + fcntl(remote_in_fd, F_SETFL, fcntl(remote_in_fd, F_GETFL) | O_NONBLOCK); + + // Check for input on any pipe (i.e. stdout, stderr, or remote_in_fd + // stdout and stderr will be sent to the local terminal and a copy of the data + // will be sent over to the mavlink shell through the remote_out_fd. + // + // Any data from remote_in_fd will be process as shell commands when an '\n' is received + while (!_should_exit) { + + struct pollfd fds[3] { {pipe_stderr, POLLIN}, {pipe_stdout, POLLIN}, {remote_in_fd, POLLIN}}; + + if (poll(fds, 3, -1) == -1) { + perror("Mavlink Shell Poll Error"); + break; + } + + if (fds[0].revents & POLLIN) { + + uint8_t buffer[512]; + size_t len; + + if ((len = read(pipe_stderr, buffer, sizeof(buffer))) <= 0) { + break; //EOF or ERROR + } + + // Send all the stderr data to the local terminal as well as the remote shell + if (write(backup_stderr_fd, buffer, len) <= 0) { + perror("Remote shell write stdout"); + break; + } + + if (write(remote_out_fd, buffer, len) <= 0) { + perror("Remote shell write"); + break; + } + + // Process all the stderr data first + continue; + } + + if (fds[1].revents & POLLIN) { + + uint8_t buffer[512]; + size_t len; + + if ((len = read(pipe_stdout, buffer, sizeof(buffer))) <= 0) { + break; //EOF or ERROR + } + + // Send all the stdout data to the local terminal as well as the remote shell + if (write(backup_stdout_fd, buffer, len) <= 0) { + perror("Remote shell write stdout"); + break; + } + + if (write(remote_out_fd, buffer, len) <= 0) { + perror("Remote shell write"); + break; + } + } + + if (fds[2].revents & POLLIN) { + + char c; + + if (read(remote_in_fd, &c, 1) <= 0) { + break; // EOF or ERROR + } + + switch (c) { + + case '\n': // user hit enter + printf("\n"); + _check_remote_uorb_command(mystr); + process_line(mystr, false); + // reset string + mystr = ""; + + _print_prompt(); + + break; + + default: // any other input + if (c > 3) { + fprintf(stdout, "%c", c); + fflush(stdout); + mystr += (char)c; + } + + break; + } + } + } + + // Restore stdout and stderr + dup2(backup_stdout_fd, STDOUT_FILENO); + dup2(backup_stderr_fd, STDERR_FILENO); + close(backup_stdout_fd); + close(backup_stderr_fd); + + close(pipe_stdout); + close(pipe_stderr); + close(remote_in_fd); + close(remote_out_fd); +} void Pxh::run_pxh() { - _should_exit = false; - + // Only the local_terminal needed for static calls + _instance = this; + _local_terminal = true; _setup_term(); std::string mystr; @@ -154,7 +314,10 @@ void Pxh::run_pxh() switch (c) { case EOF: - _should_exit = true; + break; + + case '\t': + _tab_completion(mystr); break; case 127: // backslash @@ -230,7 +393,6 @@ void Pxh::run_pxh() break; } - if (update_prompt) { // reprint prompt with mystr mystr.insert(mystr.length() - cursor_position, add_string); @@ -243,14 +405,11 @@ void Pxh::run_pxh() _move_cursor(cursor_position); } } - } } void Pxh::stop() { - _restore_term(); - if (_instance) { _instance->_should_exit = true; } @@ -294,4 +453,101 @@ void Pxh::_move_cursor(int position) printf("\033[%dD", position); } +void Pxh::_tab_completion(std::string &mystr) +{ + // parse line and get command + std::stringstream line(mystr); + std::string cmd; + line >> cmd; + + // cmd is empty or white space send a list of available commands + if (cmd.size() == 0) { + + printf("\n"); + + for (auto it = _apps.begin(); it != _apps.end(); ++it) { + printf("%s ", it->first.c_str()); + } + + printf("\n"); + mystr = ""; + + } else { + + // find tab completion matches + std::vector matches; + + for (auto it = _apps.begin(); it != _apps.end(); ++it) { + if (it->first.compare(0, cmd.size(), cmd) == 0) { + matches.push_back(it->first); + } + } + + if (matches.size() >= 1) { + // if more than one match print all matches + if (matches.size() != 1) { + printf("\n"); + + for (const auto &item : matches) { + printf("%s ", item.c_str()); + } + + printf("\n"); + } + + // find minimum size match + size_t min_size = 0; + + for (const auto &item : matches) { + if (min_size == 0) { + min_size = item.size(); + + } else if (item.size() < min_size) { + min_size = item.size(); + } + } + + // parse through elements to find longest match + std::string longest_match; + bool done = false; + + for (int i = 0; i < (int)min_size ; ++i) { + bool first_time = true; + + for (const auto &item : matches) { + if (first_time) { + longest_match += item[i]; + first_time = false; + + } else if (longest_match[i] != item[i]) { + done = true; + longest_match.pop_back(); + break; + } + } + + if (done) { break; } + + mystr = longest_match; + } + } + + std::string flags; + + while (line >> cmd) { + flags += " " + cmd; + } + + // add flags back in when there is a command match + if (matches.size() == 1) { + if (flags.empty()) { + mystr += " "; + + } else { + mystr += flags; + } + } + } +} + } // namespace px4_daemon diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.h b/platforms/posix/src/px4/common/px4_daemon/pxh.h index 2139277530..45bd4b8633 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.h +++ b/platforms/posix/src/px4/common/px4_daemon/pxh.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2016 PX4 Development Team. All rights reserved. + * Copyright (C) 2016-2022 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 @@ -72,7 +72,12 @@ public: void run_pxh(); /** - * Can be called to stop pxh. + * Run the remote mavlink pxh shell. + */ + void run_remote_pxh(int remote_in_fd, int remote_out_fd); + + /** + * Can be called to stop all pxh shells. */ static void stop(); @@ -80,11 +85,14 @@ private: void _print_prompt(); void _move_cursor(int position); void _clear_line(); + void _tab_completion(std::string &prefix); + void _check_remote_uorb_command(std::string &line); void _setup_term(); static void _restore_term(); bool _should_exit{false}; + bool _local_terminal{false}; History _history; struct termios _orig_term {}; diff --git a/platforms/posix/src/px4/common/px4_daemon/server.cpp b/platforms/posix/src/px4/common/px4_daemon/server.cpp index 1f1762bdf3..dab591cb3a 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/server.cpp @@ -150,8 +150,12 @@ Server::_server_main() int n_ready = poll(poll_fds.data(), poll_fds.size(), -1); if (n_ready < 0) { - PX4_ERR("poll() failed: %s", strerror(errno)); - return; + // Reboot command causes System Interrupt to stop poll(). This is not an error + if (errno != EINTR) { + PX4_ERR("poll() failed: %s", strerror(errno)); + } + + break; } _lock(); diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp index f0feafa623..38bd12932e 100644 --- a/src/modules/mavlink/mavlink_shell.cpp +++ b/src/modules/mavlink/mavlink_shell.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2022 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 @@ -45,11 +45,14 @@ #include #include - #ifdef __PX4_NUTTX #include #endif /* __PX4_NUTTX */ +#ifdef __PX4_POSIX +#include "../../../platforms/posix/src/px4/common/px4_daemon/pxh.h" +#endif /* __PX4_POSIX */ + #ifdef __PX4_CYGWIN #include #endif @@ -69,11 +72,10 @@ MavlinkShell::~MavlinkShell() int MavlinkShell::start() { - //this currently only works for NuttX -#ifndef __PX4_NUTTX + //this currently only works for NuttX & POSIX +#if !defined(__PX4_NUTTX) && !defined(__PX4_POSIX) return -1; -#endif /* __PX4_NUTTX */ - +#endif PX4_INFO("Starting mavlink shell"); @@ -113,6 +115,17 @@ int MavlinkShell::start() fflush(stdout); fflush(stderr); +#ifdef __PX4_POSIX + int remote_in_fd = dup(_shell_fds[0]); // Input file descriptor for the remote shell + int remote_out_fd = dup(_shell_fds[1]); // Output file descriptor for the remote shell + + char r_in[32]; + char r_out[32]; + sprintf(r_in, "%d", remote_in_fd); + sprintf(r_out, "%d", remote_out_fd); + char *const argv[3] = {r_in, r_out, nullptr}; + +#else int fd_backups[2]; //we don't touch stderr, we will redirect it to stdout in the startup of the shell task for (int i = 0; i < 2; ++i) { @@ -125,6 +138,7 @@ int MavlinkShell::start() dup2(_shell_fds[0], 0); dup2(_shell_fds[1], 1); +#endif if (ret == 0) { _task = px4_task_spawn_cmd("mavlink_shell", @@ -132,13 +146,19 @@ int MavlinkShell::start() SCHED_PRIORITY_DEFAULT, 2048, &MavlinkShell::shell_start_thread, +#ifdef __PX4_POSIX + argv); +#else nullptr); +#endif if (_task < 0) { ret = -1; } } +#if !defined(__PX4_POSIX) + //restore fd's for (int i = 0; i < 2; ++i) { if (dup2(fd_backups[i], i) == -1) { @@ -148,6 +168,8 @@ int MavlinkShell::start() close(fd_backups[i]); } +#endif + //close unused pipe fd's close(_shell_fds[0]); close(_shell_fds[1]); @@ -161,12 +183,26 @@ int MavlinkShell::start() int MavlinkShell::shell_start_thread(int argc, char *argv[]) { +#ifdef __PX4_NUTTX dup2(1, 2); //redirect stderror to stdout -#ifdef __PX4_NUTTX nsh_consolemain(0, NULL); #endif /* __PX4_NUTTX */ +#ifdef __PX4_POSIX + + if (argc != 3) { + PX4_ERR("Mavlink shell bug"); + return -1; + } + + int remote_in_fd = atoi(argv[1]); + int remote_out_fd = atoi(argv[2]); + + px4_daemon::Pxh pxh; + pxh.run_remote_pxh(remote_in_fd, remote_out_fd); +#endif + return 0; } From 8d2e8ef422c60690b208a9ef9e42189e3c046b8b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jun 2022 12:59:31 -0400 Subject: [PATCH 10/19] boards: px4_fmu-v5_test disable gyro_fft module to save flash --- boards/px4/fmu-v5/test.px4board | 1 + 1 file changed, 1 insertion(+) diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board index d60d76b54b..5279a489dc 100644 --- a/boards/px4/fmu-v5/test.px4board +++ b/boards/px4/fmu-v5/test.px4board @@ -6,6 +6,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_RPM=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n +CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_BOARD_TESTING=y From 450fcca8b8487239e4db0304a7922fccf784723d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jun 2022 13:04:03 -0400 Subject: [PATCH 11/19] drivers/differential_pressure/Kconfig: remove ets and ms4515 from common sensors to save flash --- src/drivers/differential_pressure/Kconfig | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/differential_pressure/Kconfig b/src/drivers/differential_pressure/Kconfig index a34dc5a40e..f044be565f 100644 --- a/src/drivers/differential_pressure/Kconfig +++ b/src/drivers/differential_pressure/Kconfig @@ -2,8 +2,6 @@ menu "Differential pressure" menuconfig COMMON_DIFFERENTIAL_PRESSURE bool "Common differential pressure module's" default n - select DRIVERS_DIFFERENTIAL_PRESSURE_ETS - select DRIVERS_DIFFERENTIAL_PRESSURE_MS4515 select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO select DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X From 9d486b1ccde12512a581e0bb9461133fdf7681ac Mon Sep 17 00:00:00 2001 From: Claudio Micheli Date: Mon, 20 Jun 2022 18:14:13 -0400 Subject: [PATCH 12/19] esc_battery: account for online ESCs when averaging voltage Signed-off-by: Claudio Micheli --- src/modules/esc_battery/EscBattery.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 798c17aa99..6a6a21f02a 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -33,6 +33,8 @@ #include "EscBattery.hpp" +#include + using namespace time_literals; EscBattery::EscBattery() : @@ -82,22 +84,22 @@ EscBattery::Run() if (_esc_status_sub.copy(&esc_status)) { - int online_bitmask = (1 << esc_status.esc_count) - 1; - - if (online_bitmask != esc_status.esc_online_flags || esc_status.esc_count == 0 || - esc_status.esc_count > esc_status_s::CONNECTED_ESC_MAX) { + if (esc_status.esc_count == 0 || esc_status.esc_count > esc_status_s::CONNECTED_ESC_MAX) { return; } + const uint8_t online_esc_count = math::countSetBits(esc_status.esc_online_flags); float average_voltage_v = 0.0f; float total_current_a = 0.0f; for (unsigned i = 0; i < esc_status.esc_count; ++i) { - average_voltage_v += esc_status.esc[i].esc_voltage; - total_current_a += esc_status.esc[i].esc_current; + if ((1 << i) & esc_status.esc_online_flags) { + average_voltage_v += esc_status.esc[i].esc_voltage; + total_current_a += esc_status.esc[i].esc_current; + } } - average_voltage_v /= esc_status.esc_count; + average_voltage_v /= online_esc_count; _battery.setConnected(true); _battery.updateVoltage(average_voltage_v); From 32544452f0f91e8063197a369506d147dd459143 Mon Sep 17 00:00:00 2001 From: Chuck <31755523+faberc@users.noreply.github.com> Date: Mon, 20 Jun 2022 15:16:07 -0700 Subject: [PATCH 13/19] drivers: Sagetech MXS transponder support Co-authored-by: Megan McCormick Co-authored-by: Chuck Faber --- Tools/px4moduledoc/srcparser.py | 2 +- boards/cuav/x7pro/default.px4board | 1 + boards/cubepilot/cubeorange/default.px4board | 4 +- msg/templates/module.yaml | 6 + src/drivers/drv_sensor.h | 2 + src/drivers/transponder/CMakeLists.txt | 34 + src/drivers/transponder/Kconfig | 10 + .../transponder/sagetech_mxs/CMakeLists.txt | 82 + src/drivers/transponder/sagetech_mxs/Kconfig | 5 + .../transponder/sagetech_mxs/SagetechMXS.cpp | 1380 +++++++++++++++++ .../transponder/sagetech_mxs/SagetechMXS.hpp | 279 ++++ .../transponder/sagetech_mxs/module.yaml | 8 + .../transponder/sagetech_mxs/parameters.c | 269 ++++ .../transponder/sagetech_mxs/sg_sdk/LICENSE | 201 +++ .../transponder/sagetech_mxs/sg_sdk/README.md | 6 + .../sagetech_mxs/sg_sdk/appendChecksum.c | 19 + .../sagetech_mxs/sg_sdk/calcChecksum.c | 26 + .../sagetech_mxs/sg_sdk/charArray2Buf.c | 22 + .../sagetech_mxs/sg_sdk/float2Buf.c | 30 + .../sagetech_mxs/sg_sdk/icao2Buf.c | 21 + .../sagetech_mxs/sg_sdk/sagetech_mxs.h | 13 + .../transponder/sagetech_mxs/sg_sdk/sg.h | 899 +++++++++++ .../sagetech_mxs/sg_sdk/sgDecodeAck.c | 72 + .../sagetech_mxs/sg_sdk/sgDecodeFlightId.c | 41 + .../sagetech_mxs/sg_sdk/sgDecodeInstall.c | 84 + .../sagetech_mxs/sg_sdk/sgDecodeMSR.c | 183 +++ .../sagetech_mxs/sg_sdk/sgDecodeSVR.c | 209 +++ .../sagetech_mxs/sg_sdk/sgEncodeDataReq.c | 51 + .../sagetech_mxs/sg_sdk/sgEncodeFlightId.c | 47 + .../sagetech_mxs/sg_sdk/sgEncodeGPS.c | 92 ++ .../sagetech_mxs/sg_sdk/sgEncodeInstall.c | 132 ++ .../sagetech_mxs/sg_sdk/sgEncodeOperating.c | 107 ++ .../sagetech_mxs/sg_sdk/sgEncodeTargetReq.c | 63 + .../transponder/sagetech_mxs/sg_sdk/sgUtil.h | 325 ++++ .../transponder/sagetech_mxs/sg_sdk/target.c | 127 ++ .../transponder/sagetech_mxs/sg_sdk/target.h | 128 ++ .../transponder/sagetech_mxs/sg_sdk/toAlt.c | 24 + .../transponder/sagetech_mxs/sg_sdk/toGS.c | 50 + .../sagetech_mxs/sg_sdk/toHeading.c | 24 + .../sagetech_mxs/sg_sdk/toHeading2.c | 33 + .../transponder/sagetech_mxs/sg_sdk/toIcao.c | 21 + .../transponder/sagetech_mxs/sg_sdk/toInt16.c | 21 + .../transponder/sagetech_mxs/sg_sdk/toInt32.c | 22 + .../sagetech_mxs/sg_sdk/toLatLon.c | 24 + .../transponder/sagetech_mxs/sg_sdk/toTOA.c | 20 + .../sagetech_mxs/sg_sdk/toUint16.c | 21 + .../sagetech_mxs/sg_sdk/toUint32.c | 22 + .../transponder/sagetech_mxs/sg_sdk/toVel.c | 24 + .../sagetech_mxs/sg_sdk/uint162Buf.c | 20 + .../sagetech_mxs/sg_sdk/uint322Buf.c | 22 + 50 files changed, 5325 insertions(+), 3 deletions(-) create mode 100644 msg/templates/module.yaml create mode 100644 src/drivers/transponder/CMakeLists.txt create mode 100644 src/drivers/transponder/Kconfig create mode 100644 src/drivers/transponder/sagetech_mxs/CMakeLists.txt create mode 100644 src/drivers/transponder/sagetech_mxs/Kconfig create mode 100644 src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp create mode 100644 src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp create mode 100644 src/drivers/transponder/sagetech_mxs/module.yaml create mode 100644 src/drivers/transponder/sagetech_mxs/parameters.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/README.md create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/target.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/target.h create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c create mode 100644 src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py index 258ba0bfa4..b843a235f1 100644 --- a/Tools/px4moduledoc/srcparser.py +++ b/Tools/px4moduledoc/srcparser.py @@ -16,7 +16,7 @@ class ModuleDocumentation(object): valid_categories = ['driver', 'estimator', 'controller', 'system', 'communication', 'command', 'template', 'simulation', 'autotune'] valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor', - 'magnetometer', 'baro', 'optical_flow', 'rpm_sensor'] + 'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder'] max_line_length = 80 # wrap lines that are longer than this diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index a262ced94e..38d2db27ce 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -106,3 +106,4 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_EXAMPLES_FAKE_GPS=y +#CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index f3dd85dbb3..c8b1e35ed4 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -24,14 +24,13 @@ CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_DRIVERS_OSD=y -CONFIG_DRIVERS_PCA9685=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_INPUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_DRIVERS_PX4IO=y -CONFIG_DRIVERS_ROBOCLAW=y +#CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_RPM=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y @@ -101,3 +100,4 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_DRIVERS_TRANSPONDER_SAGETECH_MXS=y diff --git a/msg/templates/module.yaml b/msg/templates/module.yaml new file mode 100644 index 0000000000..099976c11e --- /dev/null +++ b/msg/templates/module.yaml @@ -0,0 +1,6 @@ +module_name: Sagtech MXS +serial_config: + - command: mxs start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} + port_config_param: + name: TRANS_MXS_CFG + group: Transponders diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 360c5b9913..4c9b1ab90d 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -208,6 +208,8 @@ #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1 +#define DRV_TRNS_DEVTYPE_MXS 0xB2 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/transponder/CMakeLists.txt b/src/drivers/transponder/CMakeLists.txt new file mode 100644 index 0000000000..571c65554a --- /dev/null +++ b/src/drivers/transponder/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +add_subdirectory(sagetech_mxs) diff --git a/src/drivers/transponder/Kconfig b/src/drivers/transponder/Kconfig new file mode 100644 index 0000000000..d1c2f9fbb8 --- /dev/null +++ b/src/drivers/transponder/Kconfig @@ -0,0 +1,10 @@ +menu "Transponder" + menuconfig TRANSPONDER + bool "Transponder" + default n + select DRIVERS_TRANSPONDER_SAGETECH_MXS + ---help--- + Enable default set of transponder drivers + + rsource "*/Kconfig" +endmenu #transponder diff --git a/src/drivers/transponder/sagetech_mxs/CMakeLists.txt b/src/drivers/transponder/sagetech_mxs/CMakeLists.txt new file mode 100644 index 0000000000..56d7049218 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/CMakeLists.txt @@ -0,0 +1,82 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__transponder__sagetech_mxs + MAIN sagetech_mxs + COMPILE_FLAGS + #-DDEBUG_BUILD # uncomment for PX4_DEBUG output + #-O0 # uncomment when debugging + SRCS + sg_sdk/appendChecksum.c + sg_sdk/float2Buf.c + sg_sdk/sgDecodeFlightId.c + sg_sdk/sgDecodeSVR.c + sg_sdk/sgEncodeGPS.c + sg_sdk/sgEncodeTargetReq.c + sg_sdk/target.c + sg_sdk/toGS.c + sg_sdk/toIcao.c + sg_sdk/toLatLon.c + sg_sdk/toUint32.c + sg_sdk/uint322Buf.c + sg_sdk/calcChecksum.c + sg_sdk/icao2Buf.c + sg_sdk/sgDecodeInstall.c + sg_sdk/sgEncodeDataReq.c + sg_sdk/sgEncodeInstall.c + sg_sdk/toHeading2.c + sg_sdk/toInt16.c + sg_sdk/toTOA.c + sg_sdk/toVel.c + sg_sdk/charArray2Buf.c + sg_sdk/sgDecodeAck.c + sg_sdk/sgDecodeMSR.c + sg_sdk/sgEncodeFlightId.c + sg_sdk/sgEncodeOperating.c + sg_sdk/toAlt.c + sg_sdk/toHeading.c + sg_sdk/toInt32.c + sg_sdk/toUint16.c + sg_sdk/uint162Buf.c + sg_sdk/sagetech_mxs.h + sg_sdk/sg.h + sg_sdk/target.h + sg_sdk/sgUtil.h + SagetechMXS.cpp + SagetechMXS.hpp + DEPENDS + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/transponder/sagetech_mxs/Kconfig b/src/drivers/transponder/sagetech_mxs/Kconfig new file mode 100644 index 0000000000..eeb3a8fed7 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_TRANSPONDER_SAGETECH_MXS + bool "sagetech_mxs" + default n + ---help--- + Enable support for my_work_item diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp new file mode 100644 index 0000000000..d355098235 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -0,0 +1,1380 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "SagetechMXS.hpp" + +/*************************************** + * Workqueue Functions + * *************************************/ + +extern "C" __EXPORT int sagetech_mxs_main(int argc, char *argv[]) +{ + return SagetechMXS::main(argc, argv); +} + +SagetechMXS::SagetechMXS(const char *port) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) +{ + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; +} + +SagetechMXS::~SagetechMXS() +{ + free((char *)_port); + + if (!(_fd < 0)) { + close(_fd); + } + + perf_free(_loop_elapsed_perf); + perf_free(_loop_count_perf); + perf_free(_loop_interval_perf); + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int SagetechMXS::task_spawn(int argc, char *argv[]) +{ + const char *device_path = nullptr; + int ch = '\0'; + int myoptind = 1; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_path = myoptarg; + break; + + default: + PX4_WARN("unrecognized flag"); + return PX4_ERROR; + break; + } + } + + SagetechMXS *instance = new SagetechMXS(device_path); + + if (instance == nullptr) { + PX4_ERR("Allocation Failed."); + } + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; +} + + + +bool SagetechMXS::init() +{ + ScheduleOnInterval(UPDATE_INTERVAL_US); // 50Hz + mxs_state.initialized = false; + mxs_state.init_failed = false; + + if (vehicle_list == nullptr) { + if (_adsb_list_max.get() > MAX_VEHICLES_LIMIT) { // Safety Check + _adsb_list_max.set(MAX_VEHICLES_LIMIT); + _adsb_list_max.commit(); + list_size_allocated = MAX_VEHICLES_LIMIT; + } + + vehicle_list = new transponder_report_s[_adsb_list_max.get()]; + + if (vehicle_list == nullptr) { + mxs_state.init_failed = true; + PX4_ERR("Unable to initialize vehicle list."); + return false; + } + + list_size_allocated = _adsb_list_max.get(); + } + + return true; +} + +int SagetechMXS::custom_command(int argc, char *argv[]) +{ + const char *verb = argv[0]; + + if (!is_running()) { + int ret = SagetechMXS::task_spawn(argc, argv); + return ret; + + + } + + if (!strcmp(verb, "flightid")) { + const char *fid = argv[1]; + + if (fid == nullptr) { + print_usage("Missing Flight ID"); + return PX4_ERROR; + } + + return get_instance()->handle_fid(fid); + } + + if (!strcmp(verb, "ident")) { + get_instance()->_adsb_ident.set(1); + return get_instance()->_adsb_ident.commit(); + } + + if (!strcmp(verb, "opmode")) { + const char *opmode = argv[1]; + + if (opmode == nullptr) { + print_usage("Missing Op Mode"); + return PX4_ERROR; + + } else if (!strcmp(opmode, "off") || !strcmp(opmode, "0")) { + get_instance()->_mxs_op_mode.set(0); + return get_instance()->_mxs_op_mode.commit(); + + } else if (!strcmp(opmode, "on") || !strcmp(opmode, "1")) { + get_instance()->_mxs_op_mode.set(1); + return get_instance()->_mxs_op_mode.commit(); + + } else if (!strcmp(opmode, "stby") || !strcmp(opmode, "2")) { + get_instance()->_mxs_op_mode.set(2); + return get_instance()->_mxs_op_mode.commit(); + + } else if (!strcmp(opmode, "alt") || !strcmp(opmode, "3")) { + get_instance()->_mxs_op_mode.set(3); + return get_instance()->_mxs_op_mode.commit(); + + } else { + print_usage("Invalid Op Mode"); + return PX4_ERROR; + } + } + + if (!strcmp(verb, "squawk")) { + const char *squawk = argv[1]; + int sqk = 0; + + if (squawk == nullptr) { + print_usage("Missing Squawk Code"); + return PX4_ERROR; + } + + sqk = atoi(squawk); + + if (!get_instance()->check_valid_squawk(sqk)) { + print_usage("Invalid Squawk"); + return PX4_ERROR; + + } else { + get_instance()->_adsb_squawk.set(sqk); + return get_instance()->_adsb_squawk.commit(); + } + } + + + print_usage("Unknown Command"); + return PX4_ERROR; +} + +int SagetechMXS::print_usage(const char *reason) +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( + ### Description + + This driver integrates the Sagetech MXS Certified Transponder to send and receive ADSB messages and traffic. + + ### Examples + + Attempt to start driver on a specified serial device. + $ sagetech_mxs start -d /dev/ttyS1 + Stop driver + $ sagetech_mxs stop + Set Flight ID (8 char max) + $ sagetech_mxs flight_id MXS12345 + Set MXS Operating Mode + $ sagetech_mxs opmode off/on/stby/alt + $ sagetech_mxs opmode 0/1/2/3 + Set the Squawk Code + $ sagetech_mxs squawk 1200 + )DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sagetech_mxs", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("transponder"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("flightid", "Set Flight ID (8 char max)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("ident", "Set the IDENT bit in ADSB-Out messages"); + PRINT_MODULE_USAGE_COMMAND_DESCR("opmode", + "Set the MXS operating mode. ('off', 'on', 'stby', 'alt', or numerical [0-3])"); + PRINT_MODULE_USAGE_COMMAND_DESCR("squawk", "Set the Squawk Code. [0-7777] Octal (no digit larger than 7)"); + return PX4_OK; +} + +int SagetechMXS::print_status() +{ + perf_print_counter(_loop_count_perf); + perf_print_counter(_loop_elapsed_perf); + perf_print_counter(_loop_interval_perf); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + return 0; +} + + + + +/****************************** + * Main Run Thread + * ****************************/ + +void SagetechMXS::Run() +{ + // Thread Stop + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_elapsed_perf); + perf_count(_loop_interval_perf); + + // Count the number of times the loop has executed + perf_count(_loop_count_perf); + _loop_count = perf_event_count(_loop_count_perf); + + /************************* + * 50 Hz Timer + * ***********************/ + open_serial_port(); + + // Initialization + if (!mxs_state.initialized) { + if ((_loop_count > MXS_INIT_TIMEOUT_COUNT) && !mxs_state.init_failed) { // Initialization timeout + PX4_ERR("Timeout: Failed to Initialize."); + mxs_state.init_failed = true; + } + + if (!_mxs_ext_cfg.get()) { + // Auto configuration + auto_config_operating(); + auto_config_installation(); + auto_config_flightid(); + _mxs_op_mode.set(sg_op_mode_t::modeStby); + _mxs_op_mode.commit(); + send_targetreq_msg(); + mxs_state.initialized = true; + } + } + + // Parse Bytes + int bytes_available {}; + int ret = -1; + ret = ioctl(_fd, FIONREAD, (unsigned long)&bytes_available); + (void) ret; + + // PX4_INFO("Bytes in buffer: %d. Ret code: %d", bytes_available, ret); + if (bytes_available > 0) { + while (bytes_available > 0) { + uint16_t data = 0; + // tcflush(_fd, TCOFLUSH); + ret = read(_fd, &data, 1); + + if (ret < 0) { + PX4_ERR("Read Err."); + perf_count(_comms_errors); + continue; + } + + // PX4_INFO("GOT BYTE: %02x", (uint8_t)data); + parse_byte((uint8_t)data); + bytes_available -= 1; + } + } + + /****************** + * 5 Hz Timer + * ****************/ + if (!(_loop_count % FIVE_HZ_MOD)) { // 5Hz Timer (GPS Flight) + // PX4_INFO("5 Hz callback"); + + // Check if vehicle is landed or in air + if (_vehicle_land_detected_sub.updated()) { + _vehicle_land_detected_sub.copy(&_landed); + } + + // Check GPS for updates at 5Hz + if (_sensor_gps_sub.updated()) { + _sensor_gps_sub.copy(&_gps); + } + + // If Vehicle is in air send GPS messages at 5Hz + if (!_landed.landed) { + send_gps_msg(); + } + + } + + /*********************** + * 1 Hz Timer + * *********************/ + + if (!(_loop_count % ONE_HZ_MOD)) { // 1Hz Timer (Operating Message/GPS Ground) + // PX4_INFO("1 Hz callback"); + + if (!mxs_state.initialized && _mxs_ext_cfg.get()) { + // External Configuration + send_data_req(dataInstall); + } + + // If Vehicle is grounded send GPS message at 1 Hz + if (_landed.landed) { + send_gps_msg(); + } + + // Send 1Hz Operating Message + send_operating_msg(); + + // Update Parameters + if (_parameter_update_sub.updated()) { + parameter_update_s param_update{}; + _parameter_update_sub.copy(¶m_update); + ModuleParams::updateParams(); + + if (mxs_state.treq.transmitPort != (sg_transmitport_t)_mxs_targ_port.get()) { + send_targetreq_msg(); + } + } + } + + /************************ + * 8.2 Second Timer + * **********************/ + + if (!(_loop_count % EIGHT_TWO_SEC_MOD)) { // 8.2 second timer (Flight ID) + // PX4_INFO("8.2 second callback"); + send_flight_id_msg(); + } + + perf_end(_loop_elapsed_perf); +} + + +/*************************** + * ADSB Vehicle List Functions +****************************/ + +bool SagetechMXS::get_vehicle_by_ICAO(const uint32_t icao, transponder_report_s &vehicle) const +{ + transponder_report_s temp_vehicle{}; + temp_vehicle.icao_address = icao; + + uint16_t i = 0; + + if (find_index(temp_vehicle, &i)) { + memcpy(&vehicle, &vehicle_list[i], sizeof(transponder_report_s)); + return true; + + } else { + return false; + } +} + +bool SagetechMXS::find_index(const transponder_report_s &vehicle, uint16_t *index) const +{ + for (uint16_t i = 0; i < vehicle_count; i++) { + if (vehicle_list[i].icao_address == vehicle.icao_address) { + *index = i; + return true; + } + } + + return false; +} + +void SagetechMXS::set_vehicle(const uint16_t index, const transponder_report_s &vehicle) +{ + if (index >= list_size_allocated) { + return; // out of range + } + + vehicle_list[index] = vehicle; +} + +void SagetechMXS::determine_furthest_aircraft() +{ + float max_distance = 0; + uint16_t max_distance_index = 0; + + for (uint16_t index = 0; index < vehicle_count; index++) { + if (is_special_vehicle(vehicle_list[index].icao_address)) { + continue; + } + + const float distance = get_distance_to_next_waypoint(_gps.lat, _gps.lon, vehicle_list[index].lat, + vehicle_list[index].lon); + + if ((max_distance < distance) || (index == 0)) { + max_distance = distance; + max_distance_index = index; + } + } + + furthest_vehicle_index = max_distance_index; + furthest_vehicle_distance = max_distance; +} + +void SagetechMXS::delete_vehicle(const uint16_t index) +{ + if (index >= vehicle_count) { + return; + } + + if (index == furthest_vehicle_index && furthest_vehicle_distance > 0) { + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; + } + + if (index != vehicle_count - 1) { + vehicle_list[index] = vehicle_list[vehicle_count - 1]; + } + + vehicle_count--; +} + +void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle) +{ + // needs to handle updating the vehicle list, keeping track of which vehicles to drop + // and which to keep, allocating new vehicles, and publishing to the transponder_report topic + uint16_t index = list_size_allocated + 1; // Make invalid to start with. + const bool my_loc_is_zero = (_gps.lat == 0) && (_gps.lon == 0); + const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat, _gps.lon, vehicle.lat, vehicle.lon); + const bool is_tracked_in_list = find_index(vehicle, &index); + // const bool is_special = is_special_vehicle(vehicle.icao_address); + const uint16_t required_flags_position = transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE | + transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; + + if (!(vehicle.flags & required_flags_position)) { + if (is_tracked_in_list) { + delete_vehicle(index); // If the vehicle is tracked in our list but doesn't have the right flags remove it + } + + return; + + } else if (is_tracked_in_list) { // If the vehicle is in the list update it with the index found + set_vehicle(index, vehicle); + + } else if (vehicle_count < + list_size_allocated) { // If the vehicle is not in the list, and the vehicle count is less than the max count + // then add it to the vehicle_count index (after the last vehicle) and increment vehicle_count + set_vehicle(vehicle_count, vehicle); + vehicle_count++; + + } else { // Buffer is full. If new vehicle is closer, replace furthest with new vehicle + if (my_loc_is_zero) { // Invalid GPS + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; + + } else { + if (furthest_vehicle_distance <= 0) { + determine_furthest_aircraft(); + } + + if (my_loc_distance_to_vehicle < furthest_vehicle_distance) { + set_vehicle(furthest_vehicle_index, vehicle); + furthest_vehicle_distance = 0; + furthest_vehicle_index = 0; + } + } + } + + const uint16_t required_flags_avoidance = required_flags_position | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | + transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + + if (vehicle.flags & required_flags_avoidance) { + _transponder_report_pub.publish(vehicle); + } +} + +/************************************* + * Handlers for Received Messages + * ***********************************/ + +void SagetechMXS::handle_ack(const sg_ack_t ack) +{ + if ((ack.ackId != last.msg.id) || (ack.ackType != last.msg.type)) { + // The message id doesn't match the last message sent. + PX4_ERR("Message Id %d of type %d not Acked by MXS", last.msg.id, last.msg.type); + } + + // System health + if (ack.failXpdr && !last.failXpdr) { + mavlink_log_info(&_mavlink_log_pub, "Transponder Failure\t"); + events::send(events::ID("mxs_xpdr_fail"), events::Log::Critical, "Transponder Failure"); + } + + if (ack.failSystem && !last.failSystem) { + mavlink_log_info(&_mavlink_log_pub, "Transponder System Failure\t"); + events::send(events::ID("mxs_system_fail"), events::Log::Critical, "Transponder System Failure"); + } + + last.failXpdr = ack.failXpdr; + last.failSystem = ack.failSystem; +} + +void SagetechMXS::handle_svr(sg_svr_t svr) +{ + if (svr.addrType != svrAdrIcaoUnknown && svr.addrType != svrAdrIcao && svr.addrType != svrAdrIcaoSurface) { + return; // invalid icao + } + + transponder_report_s t{}; + + // Check if vehicle already exist in buffer + if (!get_vehicle_by_ICAO(svr.addr, t)) { + memset(&t, 0, sizeof(t)); + t.icao_address = svr.addr; + } + + t.timestamp = hrt_absolute_time(); + t.flags &= ~transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE; + + //Set data from svr message + if (svr.validity.position) { + t.lat = (double) svr.lat; + t.lon = (double) svr.lon; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; + } + + if (svr.validity.geoAlt) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; + t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; + t.altitude = (svr.airborne.geoAlt * SAGETECH_SCALE_FEET_TO_M); //Convert from Feet to Meters + + } else if (svr.validity.baroAlt) { + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; + t.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; + t.altitude = (svr.airborne.baroAlt * SAGETECH_SCALE_FEET_TO_M); //Convert from Feet to Meters + } + + if (svr.validity.baroVRate || svr.validity.geoVRate) { + t.ver_velocity = svr.airborne.vrate * SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC; //Convert from feet/min to meters/second + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + + if (svr.type == svrSurface) { + if (svr.validity.surfSpeed) { + t.hor_velocity = svr.surface.speed * SAGETECH_SCALE_KNOTS_TO_M_PER_SEC; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + + if (svr.validity.surfHeading) { + t.heading = matrix::wrap_pi((float)svr.surface.heading * (M_PI_F / 180.0f) + M_PI_F); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; + } + } + + if (svr.type == svrAirborne) { + if (svr.validity.airSpeed) { + t.hor_velocity = (svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_M_PER_SEC); //Convert from knots to meters/second + t.heading = matrix::wrap_pi((float)svr.airborne.heading * (M_PI_F / 180.0f) + M_PI_F); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; + } + } + + // PX4_INFO("SVR ICAO %x, SVR Air Heading %f", (int) t.icao_address, (double) t.heading); + handle_vehicle(t); +} + +void SagetechMXS::handle_msr(sg_msr_t msr) +{ + + transponder_report_s t{}; + + if (!get_vehicle_by_ICAO(msr.addr, t)) { + // new vehicle creation isn't allowed here since position isn't provided + return; + } + + t.timestamp = hrt_absolute_time(); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE; + + if (strlen(msr.callsign)) { + snprintf(t.callsign, sizeof(t.callsign), "%-8s", msr.callsign); + t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; + + } else { + t.flags &= ~transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; + } + + // PX4_INFO("Got MSR for ICAO: %x with callsign: %s", (int) t.icao_address, t.callsign); + + handle_vehicle(t); +} + +/************************************** + * Message Sending Functions + **************************************/ + +int SagetechMXS::msg_write(const uint8_t *data, const uint16_t len) const +{ + int ret = 0; + + if (_fd >= 0) { + ret = write(_fd, data, len); + + } else { + return PX4_ERROR; + } + + if (ret != len) { + perf_count(_comms_errors); + return PX4_ERROR; + } + + return PX4_OK; +} + +void SagetechMXS::send_data_req(const sg_datatype_t dataReqType) +{ + sg_datareq_t dataReq {}; + dataReq.reqType = dataReqType; + last.msg.type = SG_MSG_TYPE_HOST_DATAREQ; + uint8_t txComBuffer[SG_MSG_LEN_DATAREQ] {}; + sgEncodeDataReq(txComBuffer, &dataReq, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_DATAREQ); +} + +void SagetechMXS::send_install_msg() +{ + // MXS must be in OFF mode to change ICAO or Registration + if (mxs_state.op.opMode != modeOff) { + // gcs().send_text(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: unable to send installation data while not in OFF mode."); + return; + } + + mxs_state.inst.icao = _adsb_icao.get(); + mxs_state.inst.emitter = convert_emitter_type_to_sg(_adsb_emit_type.get()); + mxs_state.inst.size = (sg_size_t)_adsb_len_width.get(); + mxs_state.inst.maxSpeed = (sg_airspeed_t)_adsb_max_speed.get(); + mxs_state.inst.antenna = sg_antenna_t::antBottom; + + last.msg.type = SG_MSG_TYPE_HOST_INSTALL; + uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {}; + sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_INSTALL); +} + +void SagetechMXS::send_flight_id_msg() +{ + last.msg.type = SG_MSG_TYPE_HOST_FLIGHT; + uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {}; + sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_FLIGHT); +} + +void SagetechMXS::send_operating_msg() +{ + + mxs_state.op.opMode = (sg_op_mode_t)_mxs_op_mode.get(); + + if (check_valid_squawk(_adsb_squawk.get())) { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, _adsb_squawk.get()); + + } else { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, INVALID_SQUAWK); + } + + mxs_state.op.savePowerUp = true; // Save power-up state in non-volatile + mxs_state.op.enableSqt = true; // Enable extended squitters + mxs_state.op.enableXBit = false; // Enable the x-bit + mxs_state.op.milEmergency = false; // Broadcast a military emergency + mxs_state.op.emergcType = sg_emergc_t::emergcNone; // Enumerated civilian emergency type + + mxs_state.op.altUseIntrnl = + true; // True = Report altitude from internal pressure sensor (will ignore other bits in the field) + mxs_state.op.altHostAvlbl = false; + mxs_state.op.altRes25 = + !mxs_state.inst.altRes100; // Host Altitude Resolution from install + + mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + + mxs_state.op.identOn = _adsb_ident.get(); + + if (mxs_state.op.identOn) { + _adsb_ident.set(0); + _adsb_ident.commit(); + } + + if (_gps.vel_ned_valid) { + mxs_state.op.climbValid = true; + mxs_state.op.climbRate = _gps.vel_d_m_s * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN; + mxs_state.op.airspdValid = true; + mxs_state.op.headingValid = true; + + } else { + // PX4_WARN("send_operating_msg: Invalid NED"); + mxs_state.op.climbValid = false; + mxs_state.op.climbRate = -CLIMB_RATE_LIMIT; + mxs_state.op.airspdValid = false; + mxs_state.op.headingValid = false; + } + + const uint16_t speed_knots = _gps.vel_m_s * SAGETECH_SCALE_M_PER_SEC_TO_KNOTS; + double heading = (double) math::degrees(matrix::wrap_2pi(_gps.cog_rad)); + mxs_state.op.airspd = speed_knots; + mxs_state.op.heading = heading; + + last.msg.type = SG_MSG_TYPE_HOST_OPMSG; + uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {}; + sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_OPMSG); +} + + +void SagetechMXS::send_gps_msg() +{ + sg_gps_t gps {}; + + gps.hpl = SAGETECH_HPL_UNKNOWN; // HPL over 37,040m means unknown + gps.hfom = _gps.eph >= 0 ? _gps.eph : 0; + gps.vfom = _gps.epv >= 0 ? _gps.epv : 0; + gps.nacv = sg_nacv_t::nacvUnknown; + + if (_gps.s_variance_m_s >= (float)10.0 || _gps.s_variance_m_s < 0) { + gps.nacv = sg_nacv_t::nacvUnknown; + + } else if (_gps.s_variance_m_s >= (float)3.0) { + gps.nacv = sg_nacv_t::nacv10dot0; + + } else if (_gps.s_variance_m_s >= (float)1.0) { + gps.nacv = sg_nacv_t::nacv3dot0; + + } else if (_gps.s_variance_m_s >= (float)0.3) { + gps.nacv = sg_nacv_t::nacv1dot0; + + } else { //if (_gps.s_variance_m_s >= 0.0) + gps.nacv = sg_nacv_t::nacv0dot3; + } + + // Get Vehicle Longitude and Latitude and Convert to string + const int32_t longitude = _gps.lon; + const int32_t latitude = _gps.lat; + const double lon_deg = longitude * 1.0E-7 * (longitude < 0 ? -1 : 1); + const double lon_minutes = (lon_deg - int(lon_deg)) * 60; + snprintf((char *)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, + unsigned((lon_minutes - (int)lon_minutes) * 1.0E5)); + + const double lat_deg = latitude * 1.0E-7 * (latitude < 0 ? -1 : 1); + const double lat_minutes = (lat_deg - int(lat_deg)) * 60; + snprintf((char *)&gps.latitude, 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, + unsigned((lat_minutes - (int)lat_minutes) * 1.0E5)); + + const float speed_knots = _gps.vel_m_s * SAGETECH_SCALE_M_PER_SEC_TO_KNOTS; + snprintf((char *)&gps.grdSpeed, 7, "%03u.%02u", (unsigned)speed_knots, + unsigned((speed_knots - (int)speed_knots) * (float)1.0E2)); + + // const float heading = math::degrees(matrix::wrap_2pi(_gps.cog_rad)); + const float heading = matrix::wrap_2pi(_gps.cog_rad) * (180.0f / M_PI_F); + // PX4_INFO("CoG: %f. Heading: %f", (double) _gps.cog_rad, (double) _gps.heading); + // PX4_INFO("Heading: %f", (double)heading); + + snprintf((char *)&gps.grdTrack, 9, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * (float)1.0E4)); + + gps.latNorth = latitude >= 0; + gps.lngEast = longitude >= 0; + + gps.gpsValid = !(_gps.fix_type < 2); // If the status is not OK, gpsValid is false. + + const time_t time_sec = _gps.time_utc_usec * 1E-6; + struct tm *tm = gmtime(&time_sec); + snprintf((char *)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, + tm->tm_sec + (_gps.time_utc_usec % 1000000) * 1.0e-6); + // PX4_INFO("send_gps_msg: ToF %s, Longitude %s, Latitude %s, Grd Speed %s, Grd Track %s", gps.timeOfFix, gps.longitude, gps.latitude, gps.grdSpeed, gps.grdTrack); + + gps.height = _gps.alt_ellipsoid * 1E-3; + + // checkGPSInputs(&gps); + last.msg.type = SG_MSG_TYPE_HOST_GPS; + uint8_t txComBuffer[SG_MSG_LEN_GPS] {}; + sgEncodeGPS(txComBuffer, &gps, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_GPS); +} + + + +void SagetechMXS::send_targetreq_msg() +{ + mxs_state.treq.reqType = sg_reporttype_t::reportAuto; + mxs_state.treq.transmitPort = (sg_transmitport_t)_mxs_targ_port.get(); + mxs_state.treq.maxTargets = list_size_allocated; + mxs_state.treq.icao = _adsb_icao_specl.get(); + mxs_state.treq.stateVector = true; + mxs_state.treq.modeStatus = true; + mxs_state.treq.targetState = false; + mxs_state.treq.airRefVel = false; + mxs_state.treq.tisb = false; + mxs_state.treq.military = false; + mxs_state.treq.commA = false; + mxs_state.treq.ownship = true; + + last.msg.type = SG_MSG_TYPE_HOST_TARGETREQ; + uint8_t txComBuffer[SG_MSG_LEN_TARGETREQ] {}; + sgEncodeTargetReq(txComBuffer, &mxs_state.treq, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_TARGETREQ); +} + +/*********************************************** + * Byte Parsing and Packet Handling + * *********************************************/ + +void SagetechMXS::handle_packet(const Packet &msg) +{ + switch (msg.type) { + case MsgType::ACK: + if (sgDecodeAck((uint8_t *) &msg, &mxs_state.ack)) { + handle_ack(mxs_state.ack); + } + + break; + + case MsgType::Installation_Response: + if (!mxs_state.initialized && sgDecodeInstall((uint8_t *)&msg, &mxs_state.inst)) { + store_inst_resp(); + mxs_state.initialized = true; + } + + break; + + case MsgType::FlightID_Response: { + sg_flightid_t fid{}; + + if (sgDecodeFlightId((uint8_t *) &msg, &fid)) { + } + + break; + } + + case MsgType::ADSB_StateVector_Report: { + sg_svr_t svr{}; + + if (sgDecodeSVR((uint8_t *) &msg, &svr)) { + handle_svr(svr); + } + + break; + } + + case MsgType::ADSB_ModeStatus_Report: { + sg_msr_t msr{}; + + if (sgDecodeMSR((uint8_t *) &msg, &msr)) { + handle_msr(msr); + } + + break; + } + + case MsgType::FlightID: + case MsgType::Installation: + case MsgType::Operating: + case MsgType::GPS_Data: + case MsgType::Data_Request: + case MsgType::Target_Request: + case MsgType::Mode: + case MsgType::Status_Response: + case MsgType::RESERVED_0x84: + case MsgType::RESERVED_0x85: + case MsgType::Mode_Settings: + case MsgType::RESERVED_0x8D: + case MsgType::Version_Response: + case MsgType::Serial_Number_Response: + case MsgType::Target_Summary_Report: + case MsgType::ADSB_Target_State_Report: + case MsgType::ADSB_Air_Ref_Vel_Report: + // Not handling the rest of these. + break; + } +} + +bool SagetechMXS::parse_byte(const uint8_t data) +{ + switch (_message_in.state) { + default: + case ParseState::WaitingFor_Start: + if (data == SG_MSG_START_BYTE) { + _message_in.checksum = data; // initialize checksum here + _message_in.state = ParseState::WaitingFor_MsgType; + } + + break; + + case ParseState::WaitingFor_MsgType: + _message_in.checksum += data; + _message_in.packet.type = static_cast(data); + _message_in.state = ParseState::WaitingFor_MsgId; + break; + + case ParseState::WaitingFor_MsgId: + _message_in.checksum += data; + _message_in.packet.id = data; + _message_in.state = ParseState::WaitingFor_PayloadLen; + break; + + case ParseState::WaitingFor_PayloadLen: + _message_in.checksum += data; + _message_in.packet.payload_length = data; + // PX4_INFO("Packet Payload Length: %d", _message_in.packet.payload_length); + // maybe useful to add a length check here. Very few errors are due to length though... + _message_in.index = 0; + _message_in.state = (data == 0) ? ParseState::WaitingFor_Checksum : ParseState::WaitingFor_PayloadContents; + break; + + case ParseState::WaitingFor_PayloadContents: + _message_in.checksum += data; + _message_in.packet.payload[_message_in.index++] = data; + + if (_message_in.index >= + _message_in.packet.payload_length) { // Note: yeah, this is right since it will increment index after writing the last byte and it will equal the payload length + _message_in.state = ParseState::WaitingFor_Checksum; + } + + break; + + case ParseState::WaitingFor_Checksum: + // PX4_INFO("Payload Bytes Got: %d", _message_in.index); + _message_in.state = ParseState::WaitingFor_Start; + + if (_message_in.checksum == data) { + // append the checksum to the payload and zero out the payload index + _message_in.packet.payload[_message_in.index] = data; + handle_packet(_message_in.packet); + + } else if (data == SG_MSG_START_BYTE) { + PX4_INFO("ERROR: Byte Lost. Catching new packet."); + _message_in.state = ParseState::WaitingFor_MsgType; + _message_in.checksum = data; + + } else { + PX4_INFO("ERROR: Checksum Mismatch. Expected %02x. Received %02x.", _message_in.checksum, data); + } + + break; + } + + return false; +} + +/****************************** + * Helper Functions + * ***************************/ + +uint32_t SagetechMXS::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber) +{ + // Our only sensible input bases are 16 and 8 + if (baseIn != BASE_OCTAL && baseIn != BASE_HEX) { + return inputNumber; + } + + uint32_t outputNumber = 0; + + for (uint8_t i = 0; i < BASE_DEC; i++) { + outputNumber += (inputNumber % BASE_DEC) * powf(baseIn, i); + inputNumber /= BASE_DEC; + + if (inputNumber == 0) { break; } + } + + return outputNumber; +} + +int SagetechMXS::open_serial_port() +{ + + if (_fd < 0) { // Open port if not open + _fd = open(_port, (O_RDWR | O_NOCTTY | O_NONBLOCK)); + + if (_fd < 0) { + PX4_ERR("Opening port %s failed %i", _port, errno); + return PX4_ERROR; + } + + } else { + return PX4_OK; + } + + // UART Configuration + termios uart_config {}; + int termios_state = -1; + + if (tcgetattr(_fd, &uart_config)) { + PX4_ERR("Unable to get UART Configuration"); + close(_fd); + _fd = -1; + return PX4_ERROR; + } + + /***************************** + * UART Control Options + * ***************************/ + + // Enable Receiver and Set Local Mode + uart_config.c_cflag |= (CREAD | CLOCAL); + + // Send 8N1 No Parity + uart_config.c_cflag &= ~(CSIZE | CSTOPB | PARENB); + uart_config.c_cflag |= CS8; + + // Disable hardware flow control + uart_config.c_cflag &= ~CRTSCTS; + + // Set Baud Rate + unsigned baud = convert_to_px4_baud(_ser_mxs_baud.get()); + + if ((cfsetispeed(&uart_config, baud) < 0) || (cfsetospeed(&uart_config, baud) < 0)) { + PX4_ERR("ERR SET BAUD %s: %d\n", _port, termios_state); + close(_fd); + return PX4_ERROR; + } + + /***************************** + * UART Local Options + * ***************************/ + + // Set Raw Input + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN); + + /***************************** + * UART Output Options + * ***************************/ + + // Set raw output and map NL to CR-LF + uart_config.c_oflag &= ~ONLCR; + + // Flush the buffer + tcflush(_fd, TCIOFLUSH); + + + /********************************* + * Apply Modified Port Attributes + * *******************************/ + if (tcsetattr(_fd, TCSANOW, &uart_config) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + return PX4_ERROR; + } + + PX4_INFO("Opened port %s", _port); + return PX4_OK; +} + +unsigned SagetechMXS::convert_to_px4_baud(int baudType) +{ + switch (baudType) { + case 0: return B38400; + + case 1: return B600; + + case 2: return B4800; + + case 3: return B9600; + + // case 4: return B0; + + case 5: return B57600; + + case 6: return B115200; + + case 7: return B230400; + + case 8: return B19200; + + case 9: return BAUD_460800; + + case 10: return BAUD_921600; + + default: return B57600; + } +} + +sg_emitter_t SagetechMXS::convert_emitter_type_to_sg(int emitType) +{ + switch (emitType) { + case 0: return sg_emitter_t::aUnknown; + + case 1: return sg_emitter_t::aLight; + + case 2: return sg_emitter_t::aSmall; + + case 3: return sg_emitter_t::aLarge; + + case 4: return sg_emitter_t::aHighVortex; + + case 5: return sg_emitter_t::aHeavy; + + case 6: return sg_emitter_t::aPerformance; + + case 7: return sg_emitter_t::aRotorCraft; + + case 8: return sg_emitter_t::bUnknown; + + case 9: return sg_emitter_t::bGlider; + + case 10: return sg_emitter_t::bAir; + + case 11: return sg_emitter_t::bParachutist; + + case 12: return sg_emitter_t::bUltralight; + + case 13: return sg_emitter_t::bUnknown; + + case 14: return sg_emitter_t::bUAV; + + case 15: return sg_emitter_t::bSpace; + + case 16: return sg_emitter_t::cUnknown; + + case 17: return sg_emitter_t::cEmergency; + + case 18: return sg_emitter_t::cService; + + case 19: return sg_emitter_t::cPoint; + + default: return sg_emitter_t::dUnknown; + } +} + +int SagetechMXS::convert_sg_to_emitter_type(sg_emitter_t sg_emit) +{ + switch (sg_emit) { + case sg_emitter_t::aUnknown: return 0; + + case sg_emitter_t::aLight: return 1; + + case sg_emitter_t::aSmall: return 2; + + case sg_emitter_t::aLarge: return 3; + + case sg_emitter_t::aHighVortex: return 4; + + case sg_emitter_t::aHeavy: return 5; + + case sg_emitter_t::aPerformance: return 6; + + case sg_emitter_t::aRotorCraft: return 7; + + case sg_emitter_t::bUnknown: return 8; + + case sg_emitter_t::bGlider: return 9; + + case sg_emitter_t::bAir: return 10; + + case sg_emitter_t::bParachutist: return 11; + + case sg_emitter_t::bUltralight: return 12; + + case sg_emitter_t::bUAV: return 14; + + case sg_emitter_t::bSpace: return 15; + + case sg_emitter_t::cUnknown: return 16; + + case sg_emitter_t::cEmergency: return 17; + + case sg_emitter_t::cService: return 18; + + case sg_emitter_t::cPoint: return 19; + + default: return 20; + } +} + +int SagetechMXS::handle_fid(const char *fid) +{ + if (snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", fid) != 8) { + PX4_ERR("Failed to write Flight ID"); + return PX4_ERROR; + } + + PX4_INFO("Changed Flight ID to %s", mxs_state.fid.flightId); + return PX4_OK; +} + +int SagetechMXS::store_inst_resp() +{ + _mxs_op_mode.set(mxs_state.ack.opMode); + _mxs_op_mode.commit(); + _adsb_icao.set(mxs_state.inst.icao); + _adsb_icao.commit(); + _adsb_len_width.set(mxs_state.inst.size); + _adsb_len_width.commit(); + _adsb_emit_type.set(convert_sg_to_emitter_type(mxs_state.inst.emitter)); + _adsb_emit_type.commit(); + return PX4_OK; +} + + +void SagetechMXS::auto_config_operating() +{ + mxs_state.op.opMode = sg_op_mode_t::modeOff; + _mxs_op_mode.set(sg_op_mode_t::modeOff); + _mxs_op_mode.commit(); + + if (check_valid_squawk(_adsb_squawk.get())) { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, _adsb_squawk.get()); + + } else { + mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, INVALID_SQUAWK); + } + + mxs_state.op.savePowerUp = true; // Save power-up state in non-volatile + mxs_state.op.enableSqt = true; // Enable extended squitters + mxs_state.op.enableXBit = false; // Enable the x-bit + mxs_state.op.milEmergency = false; // Broadcast a military emergency + mxs_state.op.emergcType = (sg_emergc_t) + _adsb_emergc.get(); // Enumerated civilian emergency type + + mxs_state.op.altUseIntrnl = + true; // True = Report altitude from internal pressure sensor (will ignore other bits in the field) + mxs_state.op.altHostAvlbl = false; + mxs_state.op.altRes25 = true; // Host Altitude Resolution from install + + mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + + mxs_state.op.identOn = false; + + if (_gps.vel_ned_valid) { + mxs_state.op.climbValid = true; + mxs_state.op.climbRate = _gps.vel_d_m_s * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN; + mxs_state.op.airspdValid = true; + mxs_state.op.headingValid = true; + + } else { + // PX4_WARN("send_operating_msg: Invalid NED"); + mxs_state.op.climbValid = false; + mxs_state.op.climbRate = -CLIMB_RATE_LIMIT; + mxs_state.op.airspdValid = false; + mxs_state.op.headingValid = false; + } + + const uint16_t speed_knots = _gps.vel_m_s * SAGETECH_SCALE_M_PER_SEC_TO_KNOTS; + double heading = (double) math::degrees(matrix::wrap_2pi(_gps.cog_rad)); + mxs_state.op.airspd = speed_knots; + mxs_state.op.heading = heading; + + last.msg.type = SG_MSG_TYPE_HOST_OPMSG; + uint8_t txComBuffer[SG_MSG_LEN_OPMSG] {}; + sgEncodeOperating(txComBuffer, &mxs_state.op, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_OPMSG); +} + +void SagetechMXS::auto_config_installation() +{ + if (mxs_state.ack.opMode != modeOff) { + PX4_ERR("MXS not put in OFF Mode before installation."); + return; + } + + mxs_state.inst.icao = (uint32_t) _adsb_icao.get(); + snprintf(mxs_state.inst.reg, 8, "%-7s", "PX4TEST"); + + mxs_state.inst.com0 = sg_baud_t::baud230400; + mxs_state.inst.com1 = sg_baud_t::baud57600; + + mxs_state.inst.eth.ipAddress = 0; + mxs_state.inst.eth.subnetMask = 0; + mxs_state.inst.eth.portNumber = 0; + + mxs_state.inst.sil = sg_sil_t::silUnknown; + mxs_state.inst.sda = sg_sda_t::sdaUnknown; + mxs_state.inst.emitter = convert_emitter_type_to_sg(_adsb_emit_type.get()); + mxs_state.inst.size = (sg_size_t)_adsb_len_width.get(); + mxs_state.inst.maxSpeed = (sg_airspeed_t) _adsb_max_speed.get(); + mxs_state.inst.altOffset = 0; // Alt encoder offset is legacy field that should always be 0. + mxs_state.inst.antenna = sg_antenna_t::antBottom; + + mxs_state.inst.altRes100 = true; + mxs_state.inst.hdgTrueNorth = false; + mxs_state.inst.airspeedTrue = false; + mxs_state.inst.heater = true; // Heater should always be connected. + mxs_state.inst.wowConnected = true; + + last.msg.type = SG_MSG_TYPE_HOST_INSTALL; + + uint8_t txComBuffer[SG_MSG_LEN_INSTALL] {}; + sgEncodeInstall(txComBuffer, &mxs_state.inst, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_INSTALL); +} + +void SagetechMXS::auto_config_flightid() +{ + snprintf(mxs_state.fid.flightId, sizeof(mxs_state.fid.flightId), "%-8s", mxs_state.inst.reg); + + last.msg.type = SG_MSG_TYPE_HOST_FLIGHT; + + uint8_t txComBuffer[SG_MSG_LEN_FLIGHT] {}; + sgEncodeFlightId(txComBuffer, &mxs_state.fid, ++last.msg.id); + msg_write(txComBuffer, SG_MSG_LEN_FLIGHT); +} + +bool SagetechMXS::check_valid_squawk(int squawk) +{ + if (squawk > INVALID_SQUAWK) { + return false; + } + + for (int i = 4; i > 0; i--) { + squawk = squawk - ((int)(squawk / powf(10, i)) * (int)powf(10, i)); + + if ((int)(squawk / powf(10, i - 1)) > 7) { + return false; + } + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp new file mode 100644 index 0000000000..0c19d33f62 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "sg_sdk/sagetech_mxs.h" + +using namespace time_literals; + +class SagetechMXS : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + SagetechMXS(const char *port); + ~SagetechMXS() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void print_info(); + + bool init(); + + int print_status() override; + +private: + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _adsb_squawk, + (ParamInt) _adsb_ident, + (ParamInt) _adsb_list_max, + (ParamInt) _adsb_icao, + (ParamInt) _adsb_len_width, + (ParamInt) _adsb_emit_type, + (ParamInt) _adsb_max_speed, + (ParamInt) _adsb_icao_specl, + (ParamInt) _adsb_emergc, + (ParamInt) _mxs_op_mode, + (ParamInt) _mxs_targ_port, + // (ParamInt) _mxs_com0_baud, + // (ParamInt) _mxs_com1_baud, + (ParamInt) _mxs_ext_cfg, + (ParamInt) _ser_mxs_baud + ); + + // Serial Port Variables + char _port[20] {}; + int _fd{-1}; + + // Publications + uORB::Publication _transponder_report_pub{ORB_ID(transponder_report)}; + orb_advert_t _mavlink_log_pub{nullptr}; + + + // Subscriptions + uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data + uORB::Subscription _transponder_report_sub{ORB_ID(transponder_report)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + + + // Performance (perf) counters + perf_counter_t _loop_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": run_loop")}; + perf_counter_t _loop_elapsed_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; + + // Constants + static constexpr uint32_t UPDATE_INTERVAL_US{1000000 / 50}; // 20ms = 50 Hz + static constexpr uint8_t FIVE_HZ_MOD{10}; // 0.2s = 5 Hz + static constexpr uint8_t TWO_HZ_MOD{25}; // 0.5s = 2 Hz + static constexpr uint8_t ONE_HZ_MOD{50}; // 1 Hz + static constexpr uint16_t EIGHT_TWO_SEC_MOD{410}; // 8.2 seconds + // static constexpr uint8_t SG_MSG_START_BYTE{0xAA}; + static constexpr uint32_t PAYLOAD_MXS_MAX_SIZE{255}; + static constexpr float SAGETECH_SCALE_FEET_TO_M{0.3048F}; + static constexpr float SAGETECH_SCALE_KNOTS_TO_M_PER_SEC{0.514444F}; + static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_KNOTS{1.94384F}; + static constexpr float SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC{0.00508F}; + static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F}; + static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN{196.85F}; + static constexpr uint8_t ADSB_ALTITUDE_TYPE_PRESSURE_QNH{0}; + static constexpr uint8_t ADSB_ALTITUDE_TYPE_GEOMETRIC{1}; + static constexpr uint16_t MAX_VEHICLES_LIMIT{50}; + static constexpr float SAGETECH_HPL_UNKNOWN{38000.0F}; + static constexpr float CLIMB_RATE_LIMIT{16448}; + static constexpr uint16_t MXS_INIT_TIMEOUT_COUNT{1000}; // 1000 loop cycles = 20 seconds + static constexpr uint8_t BASE_OCTAL{8}; + static constexpr uint8_t BASE_HEX{16}; + static constexpr uint8_t BASE_DEC{10}; + static constexpr uint16_t INVALID_SQUAWK{7777}; + static constexpr unsigned BAUD_460800{0010004}; // B460800 not defined in MacOS termios + static constexpr unsigned BAUD_921600{0010007}; // B921600 not defined in MacOS termios + + // Stored variables + uint64_t _loop_count; + + // Cached Subscription Data + sensor_gps_s _gps; + vehicle_land_detected_s _landed; + + enum class MsgType : uint8_t { + Installation = SG_MSG_TYPE_HOST_INSTALL, + FlightID = SG_MSG_TYPE_HOST_FLIGHT, + Operating = SG_MSG_TYPE_HOST_OPMSG, + GPS_Data = SG_MSG_TYPE_HOST_GPS, + Data_Request = SG_MSG_TYPE_HOST_DATAREQ, + // RESERVED 0x06 - 0x0A + Target_Request = SG_MSG_TYPE_HOST_TARGETREQ, + Mode = SG_MSG_TYPE_HOST_MODE, + // RESERVED 0x0D - 0xC1 + ACK = SG_MSG_TYPE_XPNDR_ACK, + Installation_Response = SG_MSG_TYPE_XPNDR_INSTALL, + FlightID_Response = SG_MSG_TYPE_XPNDR_FLIGHT, + Status_Response = SG_MSG_TYPE_XPNDR_STATUS, + RESERVED_0x84 = 0x84, + RESERVED_0x85 = 0x85, + Mode_Settings = SG_MSG_TYPE_XPNDR_MODE, + RESERVED_0x8D = 0x8D, + Version_Response = SG_MSG_TYPE_XPNDR_VERSION, + Serial_Number_Response = SG_MSG_TYPE_XPNDR_SERIALNUM, + Target_Summary_Report = SG_MSG_TYPE_ADSB_TSUMMARY, + + ADSB_StateVector_Report = SG_MSG_TYPE_ADSB_SVR, + ADSB_ModeStatus_Report = SG_MSG_TYPE_ADSB_MSR, + ADSB_Target_State_Report = SG_MSG_TYPE_ADSB_TSTATE, + ADSB_Air_Ref_Vel_Report = SG_MSG_TYPE_ADSB_ARVR, + }; + + enum class ParseState { + WaitingFor_Start, + WaitingFor_MsgType, + WaitingFor_MsgId, + WaitingFor_PayloadLen, + WaitingFor_PayloadContents, + WaitingFor_Checksum, + }; + + struct __attribute__((packed)) Packet { + const uint8_t start = SG_MSG_START_BYTE; + MsgType type; + uint8_t id; + uint8_t payload_length; + uint8_t payload[PAYLOAD_MXS_MAX_SIZE]; + }; + + struct { + ParseState state; + uint8_t index; + Packet packet; + uint8_t checksum; + } _message_in; + + struct { + bool initialized; + bool init_failed; + sg_operating_t op; + sg_install_t inst; + // sg_gps_t gps; + sg_targetreq_t treq; + sg_flightid_t fid; + sg_ack_t ack; + } mxs_state; + + struct { + // cached variables to compare against params so we can send msg on param change. + bool failXpdr; + bool failSystem; + struct { + uint8_t id; + uint8_t type; + } msg; + } last; + + // External Vehicle List + transponder_report_s *vehicle_list; + uint16_t list_size_allocated; + uint16_t vehicle_count = 0; + uint16_t furthest_vehicle_index = 0; + float furthest_vehicle_distance = 0; + + // Functions + void Run() override; + void handle_packet(const Packet &msg); + int msg_write(const uint8_t *data, const uint16_t len) const; + bool parse_byte(const uint8_t data); + void handle_ack(const sg_ack_t ack); + void handle_svr(const sg_svr_t svr); + void handle_msr(sg_msr_t msr); + bool get_vehicle_by_ICAO(const uint32_t icao, transponder_report_s &vehicle) const; + bool find_index(const transponder_report_s &vehicle, uint16_t *index) const; + void set_vehicle(const uint16_t index, const transponder_report_s &vehicle); + void delete_vehicle(const uint16_t index); + bool is_special_vehicle(uint32_t icao) const {return _adsb_icao_specl.get() != 0 && (_adsb_icao_specl.get() == (int32_t) icao);} + void handle_vehicle(const transponder_report_s &vehicle); + void determine_furthest_aircraft(); + void send_data_req(const sg_datatype_t dataReqType); + void send_install_msg(); + void send_flight_id_msg(); + void send_operating_msg(); + void send_gps_msg(); + void send_targetreq_msg(); + uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber); + int open_serial_port(); + sg_emitter_t convert_emitter_type_to_sg(int emitType); + int convert_sg_to_emitter_type(sg_emitter_t sg_emit); + int handle_fid(const char *fid); + int store_inst_resp(); + void auto_config_operating(); + void auto_config_installation(); + void auto_config_flightid(); + unsigned convert_to_px4_baud(int baudType); + bool check_valid_squawk(int squawk); +}; diff --git a/src/drivers/transponder/sagetech_mxs/module.yaml b/src/drivers/transponder/sagetech_mxs/module.yaml new file mode 100644 index 0000000000..7af6176bca --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/module.yaml @@ -0,0 +1,8 @@ +module_name: Sagetech MXS +serial_config: + - command: sagetech_mxs start -d ${SERIAL_DEV} + port_config_param: + name: MXS_SER_CFG + group: Transponder + default: TEL2 + label: Sagetech MXS Serial Port diff --git a/src/drivers/transponder/sagetech_mxs/parameters.c b/src/drivers/transponder/sagetech_mxs/parameters.c new file mode 100644 index 0000000000..0d31d1a445 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/parameters.c @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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. + * + ****************************************************************************/ + +/* + * parameters.c + * + * Sagetech MXS transponder custom parameters + * @author Megan McCormick megan.mccormick@sagetech.com + * @author Check Faber chuck.faber@sagetech.com + */ + +/** + * ADSB-Out squawk code configuration + * + * This parameter defines the squawk code. Value should be between 0000 and 7777. + * + * @reboot_required false + * @min 0 + * @max 7777 + * @group Transponder + * + */ +PARAM_DEFINE_INT32(ADSB_SQUAWK, 1200); + +/** + * ADSB-Out Ident Configuration + * + * Enable Identification of Position feature + * + * @boolean + * @reboot_required false + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_IDENT, 0); + +/** + * ADSB-In Vehicle List Size + * + * Change number of targets to track + * + * @min 0 + * @max 50 + * @reboot_required true + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_LIST_MAX, 25); + +/** + * ADSB-Out ICAO configuration + * + * Defines the ICAO ID of the vehicle + * + * @reboot_required true + * @min -1 + * @max 16777215 + * @group Transponder + * + */ +PARAM_DEFINE_INT32(ADSB_ICAO_ID, 1194684); + +/** + * ADSB-Out Vehicle Size Configuration + * + * Report the length and width of the vehicle in meters. In most cases, use '1' for the smallest vehicle size. + * + * @reboot_required true + * @min 0 + * @max 15 + * @group Transponder + * + * @value 0 SizeUnknown + * @value 1 Len15_Wid23 + * @value 2 Len25_Wid28 + * @value 3 Len25_Wid34 + * @value 4 Len35_Wid33 + * @value 5 Len35_Wid38 + * @value 6 Len45_Wid39 + * @value 7 Len45_Wid45 + * @value 8 Len55_Wid45 + * @value 9 Len55_Wid52 + * @value 10 Len65_Wid59 + * @value 11 Len65_Wid67 + * @value 12 Len75_Wid72 + * @value 13 Len75_Wid80 + * @value 14 Len85_Wid80 + * @value 15 Len85_Wid90 + */ +PARAM_DEFINE_INT32(ADSB_LEN_WIDTH, 1); + +/** + * ADSB-Out Vehicle Emitter Type + * + * Configure the emitter type of the vehicle. + * + * @reboot_required true + * @min 0 + * @max 15 + * @group Transponder + * + * @value 0 Unknown + * @value 1 Light + * @value 2 Small + * @value 3 Large + * @value 4 HighVortex + * @value 5 Heavy + * @value 6 Performance + * @value 7 Rotorcraft + * @value 8 RESERVED + * @value 9 Glider + * @value 10 LightAir + * @value 11 Parachute + * @value 12 UltraLight + * @value 13 RESERVED + * @value 14 UAV + * @value 15 Space + * @value 16 RESERVED + * @value 17 EmergencySurf + * @value 18 ServiceSurf + * @value 19 PointObstacle + */ +PARAM_DEFINE_INT32(ADSB_EMIT_TYPE, 14); + +/** + * ADSB-Out Vehicle Max Speed + * + * Informs ADSB vehicles of this vehicle's max speed capability + * + * @reboot_required true + * @min 0 + * @max 6 + * @value 0 UnknownMaxSpeed + * @value 1 75Kts + * @value 2 150Kts + * @value 3 300Kts + * @value 4 600Kts + * @value 5 1200Kts + * @value 6 Over1200Kts + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_MAX_SPEED, 0); + +/** + * ADSB-In Special ICAO configuration + * + * This vehicle is always tracked. Use 0 to disable. + * + * @reboot_required false + * @min 0 + * @max 16777215 + * @group Transponder + * + */ +PARAM_DEFINE_INT32(ADSB_ICAO_SPECL, 0); + +/** + * ADSB-Out Emergency State + * + * Sets the vehicle emergency state + * + * @reboot_required false + * @min 0 + * @max 6 + * @value 0 NoEmergency + * @value 1 General + * @value 2 Medical + * @value 3 LowFuel + * @value 4 NoCommunications + * @value 5 Interference + * @value 6 Downed + * @group Transponder + */ +PARAM_DEFINE_INT32(ADSB_EMERGC, 0); + +/** + * Sagetech MXS mode configuration + * + * This parameter defines the operating mode of the MXS + * + * @reboot_required false + * @min 0 + * @max 3 + * @group Transponder + * + * @value 0 Off + * @value 1 On + * @value 2 Standby + * @value 3 Alt + */ +PARAM_DEFINE_INT32(MXS_OP_MODE, 0); + +/** + * Sagetech MXS Participant Configuration + * + * The MXS communication port to receive Target data from + * + * @min 0 + * @max 2 + * @reboot_required false + * @group Transponder + * + * @value 0 Auto + * @value 1 COM0 + * @value 2 COM1 + */ +PARAM_DEFINE_INT32(MXS_TARG_PORT, 1); + +/** + * Sagetech External Configuration Mode + * + * Disables auto-configuration mode enabling MXS config through external software. + * + * @reboot_required true + * @boolean + * @group Transponder + */ +PARAM_DEFINE_INT32(MXS_EXT_CFG, 0); + +/** + * MXS Serial Communication Baud rate + * + * Baudrate for the Serial Port connected to the MXS Transponder + * + * @reboot_required true + * @min 0 + * @max 10 + * @value 0 38400 + * @value 1 600 + * @value 2 4800 + * @value 3 9600 + * @value 4 RESERVED + * @value 5 57600 + * @value 6 115200 + * @value 7 230400 + * @value 8 19200 + * @value 9 460800 + * @value 10 921600 + * @group Serial + */ +PARAM_DEFINE_INT32(SER_MXS_BAUD, 5); diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE b/src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE new file mode 100644 index 0000000000..f49a4e16e6 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. \ No newline at end of file diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/README.md b/src/drivers/transponder/sagetech_mxs/sg_sdk/README.md new file mode 100644 index 0000000000..c0a769e11e --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/README.md @@ -0,0 +1,6 @@ +# Sagetech SDK +The contents of this folder includes selected functions from the full Sagetech SDK which can be found here: +https://github.com/Sagetech-Avionics/sagetech-mxs-sdk + +## Links +[Host Interface Control Document for MXS](https://github.com/Sagetech-Avionics/sagetech-mxs-sdk/blob/main/doc/pdf/ICD02373_MXS_Host_ICD.pdf) diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c new file mode 100644 index 0000000000..1a4311ca39 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/appendChecksum.c @@ -0,0 +1,19 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file appendChecksum.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file + */ +void appendChecksum(uint8_t *buffer, uint8_t len) +{ + buffer[len - 1] = calcChecksum(buffer, len); +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c new file mode 100644 index 0000000000..6a54c44fc0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/calcChecksum.c @@ -0,0 +1,26 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file calcChecksum.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file + */ +uint8_t calcChecksum(uint8_t *buffer, uint8_t len) +{ + uint8_t sum = 0x00; + + // Add all bytes excluding checksum + for (uint8_t i = 0; i < len - 1; ++i) { + sum += buffer[i]; + } + + return sum; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c new file mode 100644 index 0000000000..80aa4e6b7d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/charArray2Buf.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file charArray2Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" +#include + +/* + * Documented in the header file. + */ +void charArray2Buf(uint8_t *bufferPos, char arr[], uint8_t len) +{ + for (uint8_t i = 0; i < len; ++i) { + bufferPos[i] = toupper(arr[i]); + } +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c new file mode 100644 index 0000000000..e51d43a22f --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/float2Buf.c @@ -0,0 +1,30 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file float2Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void float2Buf(uint8_t *bufferPos, float value) +{ + const uint16_t FLOAT_SIZE = 4; + + union { + float val; + unsigned char bytes[FLOAT_SIZE]; + } conversion; + + conversion.val = value; + + for (int i = 0; i < FLOAT_SIZE; ++i) { + bufferPos[i] = conversion.bytes[i]; + } +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c new file mode 100644 index 0000000000..63ee41b748 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/icao2Buf.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file icao2Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void icao2Buf(uint8_t *bufferPos, uint32_t icao) +{ + bufferPos[0] = (icao & 0x00FF0000) >> 16; + bufferPos[1] = (icao & 0x0000FF00) >> 8; + bufferPos[2] = (icao & 0x000000FF); +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h new file mode 100644 index 0000000000..587b1dec39 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sagetech_mxs.h @@ -0,0 +1,13 @@ +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "sg.h" +#include "sgUtil.h" + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h new file mode 100644 index 0000000000..b5e3ce6ced --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sg.h @@ -0,0 +1,899 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sg.h + * @author jimb + * + * @date Feb 10, 2021 + * + * Sagetech protocol for host message building and parsing. + * + * This module performs both the following: + * 1. Parses raw Sagetech host messages defined in the SDIM and + * returns a populated struct dataset of the message type. + * 2. Receives a populated struct dataset of the desired host message + * and returns the corresponding raw message data buffer. + */ + +#ifndef SG_H +#define SG_H + +#include +#include + +/// Host Message Lengths (bytes) +#define SG_MSG_LEN_INSTALL 41 +#define SG_MSG_LEN_FLIGHT 17 +#define SG_MSG_LEN_OPMSG 17 +#define SG_MSG_LEN_GPS 68 +#define SG_MSG_LEN_DATAREQ 9 +#define SG_MSG_LEN_TARGETREQ 12 +#define SG_MSG_LEN_MODE 10 + +/// Host Message Types +#define SG_MSG_TYPE_HOST_INSTALL 0x01 +#define SG_MSG_TYPE_HOST_FLIGHT 0x02 +#define SG_MSG_TYPE_HOST_OPMSG 0x03 +#define SG_MSG_TYPE_HOST_GPS 0x04 +#define SG_MSG_TYPE_HOST_DATAREQ 0x05 +#define SG_MSG_TYPE_HOST_TARGETREQ 0x0B +#define SG_MSG_TYPE_HOST_MODE 0x0C + +/// XPNDR Message Types +#define SG_MSG_TYPE_XPNDR_ACK 0x80 +#define SG_MSG_TYPE_XPNDR_INSTALL 0x81 +#define SG_MSG_TYPE_XPNDR_FLIGHT 0x82 +#define SG_MSG_TYPE_XPNDR_STATUS 0x83 +#define SG_MSG_TYPE_XPNDR_COMMA 0x85 +#define SG_MSG_TYPE_XPNDR_MODE 0x8C +#define SG_MSG_TYPE_XPNDR_VERSION 0x8E +#define SG_MSG_TYPE_XPNDR_SERIALNUM 0x8F + +/// ADS-B Message Types +#define SG_MSG_TYPE_ADSB_TSUMMARY 0x90 +#define SG_MSG_TYPE_ADSB_SVR 0x91 +#define SG_MSG_TYPE_ADSB_MSR 0x92 +#define SG_MSG_TYPE_ADSB_TSTATE 0x97 +#define SG_MSG_TYPE_ADSB_ARVR 0x98 + +/// Start byte for all host messages +#define SG_MSG_START_BYTE 0xAA + +/// Emitter category set byte values +#define SG_EMIT_GROUP_A 0x00 +#define SG_EMIT_GROUP_B 0x01 +#define SG_EMIT_GROUP_C 0x02 +#define SG_EMIT_GROUP_D 0x03 + +/// Emitter category enumeration offsets +#define SG_EMIT_OFFSET_A 0x00 +#define SG_EMIT_OFFSET_B 0x10 +#define SG_EMIT_OFFSET_C 0x20 +#define SG_EMIT_OFFSET_D 0x30 + +/** + * Available COM port baud rates. + */ +typedef enum { + baud38400 = 0, + baud600, + baud4800, + baud9600, + baud28800, + baud57600, + baud115200, + baud230400, + baud19200, + baud460800, + baud921600 +} sg_baud_t; + +/** + * Transponder ethernet configuration + */ +typedef struct { + uint32_t ipAddress; /// The transponder ip address + uint32_t subnetMask; /// The transponder subnet mask + uint16_t portNumber; /// The transponder port number +} sg_ethernet_t; + +/** + * Available GPS integrity SIL values + */ +typedef enum { + silUnknown = 0, + silLow, + silMedium, + silHigh +} sg_sil_t; + +/** + * Available GPS integrity SDA values + */ +typedef enum { + sdaUnknown = 0, + sdaMinor, + sdaMajor, + sdaHazardous +} sg_sda_t; + +/** + * Available emitter types + */ +typedef enum { + aUnknown = SG_EMIT_OFFSET_A, + aLight, + aSmall, + aLarge, + aHighVortex, + aHeavy, + aPerformance, + aRotorCraft, + bUnknown = SG_EMIT_OFFSET_B, + bGlider, + bAir, + bParachutist, + bUltralight, + bUAV = SG_EMIT_OFFSET_B + 6, + bSpace, + cUnknown = SG_EMIT_OFFSET_C, + cEmergency, + cService, + cPoint, + cCluster, + cLine, + dUnknown = SG_EMIT_OFFSET_D +} sg_emitter_t; + +/** + * Available aircraft sizes in meters + */ +typedef enum { + sizeUnknown = 0, /// Dimensions unknown + sizeL15W23, /// Length <= 15m & Width <= 23m + sizeL25W28, /// Length <= 25m & Width <= 28.5m + sizeL25W34, /// Length <= 25m & Width <= 34m + sizeL35W33, /// Length <= 35m & Width <= 33m + sizeL35W38, /// Length <= 35m & Width <= 38m + sizeL45W39, /// Length <= 45m & Width <= 39.5m + sizeL45W45, /// Length <= 45m & Width <= 45m + sizeL55W45, /// Length <= 55m & Width <= 45m + sizeL55W52, /// Length <= 55m & Width <= 52m + sizeL65W59, /// Length <= 65m & Width <= 59.5m + sizeL65W67, /// Length <= 65m & Width <= 67m + sizeL75W72, /// Length <= 75m & Width <= 72.5m + sizeL75W80, /// Length <= 75m & Width <= 80m + sizeL85W80, /// Length <= 85m & Width <= 80m + sizeL85W90 /// Length <= 85m & Width <= 90m +} sg_size_t; + +/** + * Available aircraft maximum airspeeds + */ +typedef enum { + speedUnknown = 0, /// Max speed unknown + speed75kt, /// 0 knots < Max speed < 75 knots + speed150kt, /// 75 knots < Max speed < 150 knots + speed300kt, /// 150 knots < Max speed < 300 knots + speed600kt, /// 300 knots < Max speed < 600 knots + speed1200kt, /// 600 knots < Max speed < 1200 knots + speedGreater /// 1200 knots < Max speed +} sg_airspeed_t; + +/** + * Available antenna configurations + */ +typedef enum { + antBottom = 1, /// bottom antenna only + antBoth = 3 /// both top and bottom antennae +} sg_antenna_t; + +/** + * The XPNDR Installation Message. + * Host --> XPNDR. + * XPNDR --> Host. + * Use 'strcpy(install.reg, "REGVAL1")' to assign the registration. + */ +typedef struct { + uint32_t icao; /// The aircraft's ICAO address + char reg[8]; /// The aircraft's registration (left-justified alphanumeric characters padded with spaces) + sg_baud_t com0; /// The baud rate for COM Port 0 + sg_baud_t com1; /// The baud rate for COM Port 1 + sg_ethernet_t eth; /// The ethernet configuration + sg_sil_t sil; /// The gps integrity SIL parameter + sg_sda_t sda; /// The gps integrity SDA parameter + sg_emitter_t emitter; /// The platform's emitter type + sg_size_t size; /// The platform's dimensions + sg_airspeed_t maxSpeed; /// The platform's maximum airspeed + int16_t altOffset; /// The altitude encoder offset is a legacy field that should always = 0 + sg_antenna_t antenna; /// The antenna configuration + bool altRes100; /// Altitude resolution. true = 100 foot, false = 25 foot + bool hdgTrueNorth; /// Heading type. true = true north, false = magnetic north + bool airspeedTrue; /// Airspeed type. true = true speed, false = indicated speed + bool heater; /// true = heater enabled, false = heater disabled + bool wowConnected; /// Weight on Wheels sensor. true = connected, false = not connected +} sg_install_t; + +/** + * The XPNDR Flight ID Message. + * Host --> XPNDR. + * XPNDR --> Host. + * * Use 'strcpy(id.flightID, "FLIGHTNO")' to assign the flight identification. + */ +typedef struct { + char flightId[9]; /// The flight identification (left-justified alphanumeric characters padded with spaces) +} sg_flightid_t; + +/** + * Available transponder operating modes. The enumerated values are + * offset from the host message protocol values. + */ +typedef enum { + modeOff = 0, /// 'Off' Mode: Xpdr will not transmit + modeOn, /// 'On' Mode: Full functionality with Altitude = Invalid + modeStby, /// 'Standby' Mode: Reply to lethal interrogations, only + modeAlt /// 'Alt' Mode: Full functionality +} sg_op_mode_t; + +/** + * Available emergency status codes. + */ +typedef enum { + emergcNone = 0, /// No Emergency + emergcGeneral, /// General Emergency + emergcMed, /// Lifeguard/Medical Emergency + emergcFuel, /// Minimum Fuel + emergcComm, /// No Communications + emergcIntrfrc, /// Unlawful Interference + emergcDowned /// Downed Aircraft +} sg_emergc_t; + +/** + * The XPNDR Operating Message. + * Host --> XPNDR. + */ +typedef struct { + uint16_t squawk; /// 4-digit octal Mode A code + sg_op_mode_t opMode; /// Operational mode + bool savePowerUp; /// Save power-up state in non-volatile + bool enableSqt; /// Enable extended squitters + bool enableXBit; /// Enable the x-bit + bool milEmergency; /// Broadcast a military emergency + sg_emergc_t emergcType; /// Enumerated civilian emergency type + bool identOn; /// Set the identification switch = On + bool altUseIntrnl; /// True = Report altitude from internal pressure sensor (will ignore other bits in the field) + bool altHostAvlbl; /// True = Host Altitude is being provided + bool altRes25; /// Host Altitude Resolution from install message, True = 25 ft, False = 100 ft + int32_t altitude; /// Sea-level altitude in feet. Field is ignored when internal altitude is selected. + bool climbValid; /// Climb rate is provided; + int16_t climbRate; /// Climb rate in ft/min. Limits are +/- 16,448 ft/min. + bool headingValid; /// Heading is valid. + double heading; /// Heading in degrees + bool airspdValid; /// Airspeed is valid. + uint16_t airspd; /// Airspeed in knots. +} sg_operating_t; + +/** + * Avaiable NACp values. + */ +typedef enum { + nacpUnknown, /// >= 18.52 km ( 10 nmile) + nacp10dot0, /// < 18.52 km ( 10 nmile) + nacp4dot0, /// < 7.408 km ( 4 nmile) + nacp2dot0, /// < 3.704 km ( 2 nmile) + nacp1dot0, /// < 1.852 km ( 1 nmile) + nacp0dot5, /// < 0.926 km (0.5 nmile) + nacp0dot3, /// < 0.556 km (0.3 nmile) + nacp0dot1, /// < 0.185 km (0.1 nmile) + nacp0dot05, /// < 92.6 m (0.05 nmile) + nacp30, /// < 30.0 m + nacp10, /// < 10.0 m + nacp3 /// < 3.0 m +} sg_nacp_t; + +/** + * Available NACv values (m/s) + */ +typedef enum { + nacvUnknown = 0, /// 10 <= NACv (or NACv is unknown) + nacv10dot0, /// 3 <= NACv < 10 + nacv3dot0, /// 1 <= NACv < 3 + nacv1dot0, /// 0.3 <= NACv < 1 + nacv0dot3 /// 0.0 <= NACv < 0.3 +} sg_nacv_t; + +/** + * The XPNDR Simulated GPS Message. + * Host --> XPNDR. + */ +typedef struct { + char longitude[12]; /// The absolute value of longitude (degree and decimal minute) + char latitude[11]; /// The absolute value of latitude (degree and decimal minute) + char grdSpeed[7]; /// The GPS over-ground speed (knots) + char grdTrack[9]; /// The GPS track referenced from True North (degrees, clockwise) + bool latNorth; /// The aircraft is in the northern hemisphere + bool lngEast; /// The aircraft is in the eastern hemisphere + bool fdeFail; /// True = A satellite error has occurred + bool gpsValid; /// True = GPS data is valid + char timeOfFix[11]; /// Time, relative to midnight UTC (can optionally be filled spaces) + float height; /// The height above the WGS-84 ellipsoid (meters) + float hpl; /// The Horizontal Protection Limit (meters) + float hfom; /// The Horizontal Figure of Merit (meters) + float vfom; /// The Vertical Figure of Merit (meters) + sg_nacv_t nacv; /// Navigation Accuracy for Velocity (meters/second) +} sg_gps_t; + +/** + * Available data request types + */ +typedef enum { + dataInstall = 0x81, /// Installation data + dataFlightID = 0x82, /// Flight Identification data + dataStatus = 0x83, /// Status Response data + dataMode = 0x8C, /// Mode Settings data + dataHealth = 0x8D, /// Health Monitor data + dataVersion = 0x8E, /// Version data + dataSerialNum = 0x8F, /// Serial Number data + dataTOD = 0xD2, /// Time of Day data + dataMode5 = 0xD3, /// Mode 5 Indication data + dataCrypto = 0xD4, /// Crypto Status data + dataMilSettings = 0xD7 /// Military Settings data +} sg_datatype_t; + +/** + * The Data Request message. + * Host --> XPDR. + */ +typedef struct { + sg_datatype_t reqType; /// The desired data response + uint8_t resv[3]; +} sg_datareq_t; + +/** + * Available target request types + */ +typedef enum { + reportAuto = 0, /// Enable auto output of all target reports + reportSummary, /// Report list of all tracked targets (disables auto-output) + reportIcao, /// Generate reports for specific target, only (disables auto-output) + reportNone /// Disable all target reports +} sg_reporttype_t; + +/** + * Available target report transmission ports + */ +typedef enum { + transmitSource = 0, /// Transmit reports on channel where target request was received + transmitCom0, /// Transmit reports on Com0 + transmitCom1, /// Transmit reports on Com1 + transmitEth /// Transmit reports on Ethernet +} sg_transmitport_t; + +/** + * The Target Request message for ADS-B 'in' data. + * Host --> XPDR. + */ +typedef struct { + sg_reporttype_t reqType; /// The desired report mode + sg_transmitport_t transmitPort; /// The communication port used for report transmission + uint16_t maxTargets; /// The maximum number of targets to track (max value: 404) + uint32_t icao; /// The desired target's ID, if applicable + bool stateVector; /// Transmit state vector reports + bool modeStatus; /// Transmit mode status reports + bool targetState; /// Transmit target state reports + bool airRefVel; /// Transmit air referenced velocity reports + bool tisb; /// Transmit raw TIS-B message reports (requires auto-output) + bool military; /// Enable tracking of military aircraft + bool commA; /// Transmit Comm-A Reports (requires auto-output) + bool ownship; /// Transmit reports about own aircraft +} sg_targetreq_t; + +/** + * The Mode message. + * Host --> XPDR. + */ +typedef struct { + bool reboot; /// Reboot the MX +} sg_mode_t; + +/** + * The XPNDR Acknowledge Message following all host messages. + * XPNDR --> Host. + */ +typedef struct { + uint8_t ackType; /// Message type being acknowledged + uint8_t ackId; /// Message ID being acknowledged + bool failXpdr; /// Built-in-test failure + bool failSystem; /// Required system input missing + bool failCrypto; /// Crypto status failure + bool wow; /// Weight-on-wheels indicates aircraft is on-ground + bool maint; /// Maintenance mode enabled + bool isHostAlt; /// False = Pressure sensor altitude, True = Host provided value + sg_op_mode_t opMode; /// Operational mode + int32_t alt; /// Altitude (feet) + bool altValid; /// Altitude is valid +} sg_ack_t; + +/** + * The XPNDR Status Response Message following a Data Request for Status. + * XPNDR --> Host. + */ +typedef struct { + uint8_t versionSW; /// SW Version # installed on the XPNDR + uint8_t versionFW; /// FW Version # installed on the XPNDR + uint32_t crc; /// CRC Checksum for the installed XPNDR SW/FW versions + + bool powerUp : 1; /// Integrity of CPU and Non-Volatile data at power-up + bool continuous : 1; /// Set by any other B.I.T. failures during operation + bool processor : 1; /// One-time processor instruction set test at power-up + bool crcValid : 1; /// Calculate then verifies the CRC against the stored value + bool memory : 1; /// Processor RAM is functional + bool calibrated : 1; /// Transponder is calibrated + bool receiver : 1; /// RF signals travel through hardware correctly + bool power53v : 1; /// Voltage at the 53V power supply is correct + bool adc : 1; /// Analog-to-Digital Converter is functional + bool pressure : 1; /// Internal pressure transducer is functional + bool fpga : 1; /// FPGA I/O operations are functional + bool rxLock : 1; /// Rx oscillator reporting PLL Lock at reference frequency + bool txLock : 1; /// Tx oscillator reporting PLL Lock at reference frequency + bool mtSuppress : 1; /// Mutual suppression is operating correctly + bool temp : 1; /// Internal temperature is within range (< 110 C) + bool sqMonitor : 1; /// Squitters are transmitting at their nominal rates + bool txRate : 1; /// Transmission duty cycle is in the safe range + bool sysLatency : 1; /// Systems events occurred within expected time limits + bool txPower : 1; /// Transmission power is in-range + bool voltageIn : 1; /// Input voltage is in-range (10V-32V) + bool icao : 1; /// ICAO Address is valid (fail at '000000' or 'FFFFFF') + bool gps : 1; /// Valid GPS data is received at 1Hz, minimum +} sg_status_t; + +/** + * The XPNDR Health Monitor Response Message. + * XPNDR --> Host. + */ +typedef struct { + int8_t socTemp; /// System on a Chip temperature + int8_t rfTemp; /// RF Board temperature + int8_t ptTemp; /// Pressure Transducer temperature +} sg_healthmonitor_t; + +/** + * The XPNDR Version Response Message. + * XPNDR --> Host. + */ +typedef struct { + uint8_t swVersion; /// The SW Version major revision number + uint8_t fwVersion; /// The FW Version major revision number + uint16_t swSvnRevision; /// The SW Repository version number + uint16_t fwSvnRevision; /// The FW Repository version number +} sg_version_t; + +/** + * The XPNDR Serial Number Response Message. + * XPNDR --> Host. + */ +typedef struct { + char ifSN[33]; /// The Interface Board serial number + char rfSN[33]; /// The RF Board serial number + char xpndrSN[33]; /// The Transponder serial number +} sg_serialnumber_t; + +/// The state vector report type. +typedef enum { + svrAirborne = 1, /// Airborne state vector report type. + svrSurface /// Surface state vector report type. +} sg_svr_type_t; + +/// The state vector report participant address type. +typedef enum { + svrAdrIcaoUnknown, /// ICAO address unknown emitter category. + svrAdrNonIcaoUnknown, /// Non-ICAO address unknown emitter category. + svrAdrIcao, /// ICAO address aircraft. + svrAdrNonIcao, /// Non-ICAO address aircraft. + svrAdrIcaoSurface, /// ICAO address surface vehicle, fixed ground, tethered obstruction. + svrAdrNonIcaoSurface, /// Non-ICAO address surface vehicle, fixed ground, tethered obstruction. + svrAdrDup, /// Duplicate target of another ICAO address. + svrAdrAdsr /// ADS-R target. +} sg_addr_type_t; + +/// The surface part of a state vector report. +typedef struct { + int16_t speed; /// Surface speed. + int16_t heading; /// Surface heading. +} sg_svr_surface_t; + +/// The airborne part of a state vector report. +typedef struct { + int16_t velNS; /// The NS speed vector component. [knots] + int16_t velEW; /// The EW speed vector component. [knots] + int16_t speed; /// Speed from N/S and E/W velocity. [knots] + int16_t heading; /// Heading from N/S and E/W velocity. [deg from N] + int32_t geoAlt; /// Geometric altitude. [ft] + int32_t baroAlt; /// Barometric altitude. [ft] + int16_t vrate; /// Vertical rate. [ft/min] + float estLat; /// Estimated latitude. [deg N] + float estLon; /// Estimated longitude. [deg E] +} sg_svr_airborne_t; + +typedef struct { + bool baroVRate : 1; /// Barometric vertical rate valid. + bool geoVRate : 1; /// Geometric vertical rate valid. + bool baroAlt : 1; /// Barometric altitude valid. + bool surfHeading : 1; /// Surface heading valid. + bool surfSpeed : 1; /// Surface speed valid. + bool airSpeed : 1; /// Airborne speed and heading valid. + bool geoAlt : 1; /// Geometric altitude valid. + bool position : 1; /// Lat and lon data valid. +} sg_svr_validity_t; + +typedef struct { + uint8_t reserved : 6; /// Reserved. + bool estSpeed : 1; /// Estimated N/S and E/W velocity. + bool estPosition : 1; /// Estimated lat/lon position. +} sg_svr_est_validity_t; + +/** + * The XPDR ADS-B state vector report Message. + * Host --> XPDR. + * + * @note The time of applicability values are based on the MX system clock that starts + * at 0 on power up. The time is the floating point number that is the seconds since + * power up. The time number rolls over at 512.0. + */ +typedef struct { + sg_svr_type_t type; /// Report type. + union { + uint8_t flags; + sg_svr_validity_t validity; /// Field validity flags. + }; + union { + uint8_t eflags; + sg_svr_est_validity_t evalidity; /// Estimated field validity flags. + }; + uint32_t addr; /// Participant address. + sg_addr_type_t addrType; /// Participant address type. + float toaEst; /// Report estimated position and speed time of applicability. + float toaPosition; /// Report position time of applicability. + float toaSpeed; /// Report speed time of applicability. + uint8_t survStatus; /// Surveillance status. + uint8_t mode; /// Report mode. + uint8_t nic; /// Navigation integrity category. + float lat; /// Latitude. + float lon; /// Longitude. + union { + sg_svr_surface_t surface; /// Surface SVR data. + sg_svr_airborne_t airborne; /// Airborne SVR data. + }; +} sg_svr_t; + +typedef enum { + msrTypeV0, + msrTypeV1Airborne, + msrTypeV1Surface, + msrTypeV2Airborne, + msrTypeV2Surface +} sg_msr_type_t; + +typedef struct { + uint8_t reserved : 2; + bool priority : 1; + bool sil : 1; + bool nacv : 1; + bool nacp : 1; + bool opmode : 1; + bool capcodes : 1; +} sg_msr_validity_t; + +typedef enum { + adsbVerDO260, + adsbVerDO260A, + adsbVerDO260B +} sg_adsb_version_t; + +typedef enum { + adsbUnknown, + adsbLight, + adsbSmall = 0x3, + adsbLarge = 0x5, + adsbHighVortex, + adsbHeavy, + adsbPerformance, + adsbRotorcraft = 0x0A, + adsbGlider, + adsbAir, + adsbUnmaned, + adsbSpace, + adsbUltralight, + adsbParachutist, + adsbVehicle_emg = 0x14, + adsbVehicle_serv, + adsbObsticlePoint, + adsbObsticleCluster, + adsbObsticleLinear +} sg_adsb_emitter_t; + +typedef enum { + priNone, + priGeneral, + priMedical, + priFuel, + priComm, + priUnlawful, + priDowned +} sg_priority_t; + +typedef enum { + tcrNone, + tcrSingle, + tcrMultiple +} sg_tcr_t; + +typedef struct { + bool b2low : 1; + bool uat : 1; + bool arv : 1; + bool tsr : 1; + bool adsb : 1; + bool tcas : 1; + sg_tcr_t tcr; +} sg_capability_t; + +typedef enum { + gpsLonNodata, + gpsLonSensorSupplied, + gpsLon2m, + gpsLon4m, + gpsLon6m, + gpsLon8m, + gpsLon10m, + gpsLon12m, + gpsLon14m, + gpsLon16m, + gpsLon18m, + gpsLon20m, + gpsLon22m, + gpsLon24m, + gpsLon26m, + gpsLon28m, + gpsLon30m, + gpsLon32m, + gpsLon34m, + gpsLon36m, + gpsLon38m, + gpsLon40m, + gpsLon42m, + gpsLon44m, + gpsLon46m, + gpsLon48m, + gpsLon50m, + gpsLon52m, + gpsLon54m, + gpsLon56m, + gpsLon58m, + gpsLon60m +} sg_gps_lonofs_t; + +typedef enum { + gpslatNodata, + gpslatLeft2m, + gpslatLeft4m, + gpslatLeft6m, + gpslatRight0m, + gpslatRight2m, + gpslatRight4m, + gpslatRight6m, +} sg_gps_latofs_t; + +typedef struct { + bool gpsLatFmt; + sg_gps_latofs_t gpsLatOfs; + bool gpsLonFmt; + sg_gps_lonofs_t gpsLonOfs; + bool tcasRA : 1; + bool ident : 1; + bool singleAnt : 1; +} sg_adsb_opmode_t; + +typedef enum { + gvaUnknown, + gvaLT150m, + gvaLT45m +} sg_gva_t; + +typedef enum { + nicGolham, + nicNonGilham +} sg_nicbaro_t; + +typedef enum { + svsilUnknown, + svsilPow3, + svsilPow5, + svsilPow7 +} sg_svsil_t; + +typedef struct { + sg_nacp_t nacp; + sg_nacv_t nacv; + sg_sda_t sda; + bool silSupp; + sg_svsil_t sil; + sg_gva_t gva; + sg_nicbaro_t nicBaro; +} sg_sv_qual_t; + +typedef enum { + trackTrueNorth, + trackMagNorth, + headingTrueNorth, + headingMagNorth +} sg_trackheading_t; + +typedef enum { + vrateBaroAlt, + vrateGeoAlt +} sg_vratetype_t; + +/** + * The XPDR ADS-B mode status report Message. + * Host --> XPDR. + * + * @note The time of applicability values are based on the MX system clock that starts + * at 0 on power up. The time is the floating point number that is the seconds since + * power up. The time number rolls over at 512.0. + */ +typedef struct { + sg_msr_type_t type; /// Report type. + + union { + uint8_t flags; + sg_msr_validity_t validity; /// Field validity flags. + }; + + uint32_t addr; /// Participant address. + sg_addr_type_t addrType; /// Participant address type. + + float toa; + sg_adsb_version_t version; + char callsign[9]; + sg_adsb_emitter_t emitter; + sg_size_t size; + sg_priority_t priority; + sg_capability_t capability; + sg_adsb_opmode_t opMode; + sg_sv_qual_t svQuality; + sg_trackheading_t trackHeading; + sg_vratetype_t vrateType; +} sg_msr_t; + +/** + * Convert install message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw install message. + * @param[in] stl The install message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in stl parameter must be pre-validated. + */ +bool sgEncodeInstall(uint8_t *buffer, sg_install_t *stl, uint8_t msgId); + +/** + * Convert flight identification struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw flight identification message. + * @param[in] id The flight id struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in id parameter must be pre-validated. + */ +bool sgEncodeFlightId(uint8_t *buffer, sg_flightid_t *id, uint8_t msgId); + +/** + * Convert operating message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw operating message. + * @param[in] op The operating message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in op parameter must be pre-validated. + */ +bool sgEncodeOperating(uint8_t *buffer, sg_operating_t *op, uint8_t msgId); + +/* TODO: Create GPS helper functions to convert other data types --> char buffers */ + +/** + * Convert GPS message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw GPS message. + * @param[in] gps The GPS message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in gps parameter must be pre-validated. + */ +bool sgEncodeGPS(uint8_t *buffer, sg_gps_t *gps, uint8_t msgId); + +/** + * Convert data request message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw target request message. + * @param[in] data The data request message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in data parameter must be pre-validated. + */ +bool sgEncodeDataReq(uint8_t *buffer, sg_datareq_t *data, uint8_t msgId); + +/** + * Convert target request message struct to the raw buffer format. + * + * @param[out] buffer An empty buffer to contain the raw target request message. + * @param[in] tgt The target request message struct with fields populated. + * @param[in] msgId The sequence number for the message. + * + * @return true if successful or false on failure. + * + * @warning data in tgt parameter must be pre-validated. + */ +bool sgEncodeTargetReq(uint8_t *buffer, sg_targetreq_t *tgt, uint8_t msgId); + +/** + * Process the ACK message response from the transponder. + * + * @param[in] buffer The raw ACK message buffer. + * @param[out] ack The parsed message results. + * + * @return true if successful or false on failure. + */ +bool sgDecodeAck(uint8_t *buffer, sg_ack_t *ack); + +/** + * Process the Install message response from the transponder. + * + * @param[in] buffer The raw Install message buffer. + * @param[out] stl The parsed message results. + * + * @return true if successful or false on failure. + */ +bool sgDecodeInstall(uint8_t *buffer, sg_install_t *stl); + +/** + * Process the Flight ID message response from the transponder. + * + * @param[in] buffer The raw Flight ID message buffer. + * @param[out] id The parsed message results. + * + * @return true if successful or false on failure. + */ +bool sgDecodeFlightId(uint8_t *buffer, sg_flightid_t *id); + +/** + * Process the state vector report message. + * + * @param[in] buffer The raw SVR message buffer. + * @param[out] svr The parsed SVR message. + * + * @return true if successful or false on failure. + */ +bool sgDecodeSVR(uint8_t *buffer, sg_svr_t *svr); + +/** + * Process the mode status report message. + * + * @param buffer The raw MSR message buffer. + * @param msr The parsed MSR message. + * + * @return true if successful or false on failure. + */ +bool sgDecodeMSR(uint8_t *buffer, sg_msr_t *msr); + +#endif /* SG_H */ diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c new file mode 100644 index 0000000000..c1a2afe5e0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeAck.c @@ -0,0 +1,72 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeAck.c + * @author jimb + * + * @date Feb 10, 2021 + * + * This file receives a raw Acknowledge message buffer and + * parses the payload into a data struct. + */ + +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_ACK_XPNDR_FAIL 0x01 +#define SG_ACK_SYSTEM_FAIL 0x02 +#define SG_ACK_CRYPTO_FAIL 0x04 +#define SG_ACK_WOW 0x08 +#define SG_ACK_MAINT 0x10 +#define SG_ACK_ALT_SOURCE 0x20 +#define SG_ACK_OP_MODE 0xC0 + +typedef struct __attribute__((packed)) +{ + uint8_t start; + uint8_t type; + uint8_t id; + uint8_t payloadLen; + uint8_t ackType; + uint8_t ackId; + uint8_t state; + uint8_t alt[3]; + uint8_t checksum; +} ack_t; + +/* + * Documented in the header file. + */ +bool sgDecodeAck(uint8_t *buffer, sg_ack_t *ack) +{ + ack_t sgAck; + memcpy(&sgAck, buffer, sizeof(ack_t)); + + ack->ackType = sgAck.ackType; + ack->ackId = sgAck.ackId; + ack->failXpdr = (sgAck.state & SG_ACK_XPNDR_FAIL) > 0; + ack->failSystem = (sgAck.state & SG_ACK_SYSTEM_FAIL) > 0; + ack->failCrypto = (sgAck.state & SG_ACK_CRYPTO_FAIL) > 0; + ack->wow = (sgAck.state & SG_ACK_WOW) > 0; + ack->maint = (sgAck.state & SG_ACK_MAINT) > 0; + ack->isHostAlt = (sgAck.state & SG_ACK_ALT_SOURCE) > 0; + ack->opMode = (sgAck.state & SG_ACK_OP_MODE) >> 6; + + int32_t int24 = toInt32(sgAck.alt); + + // Bitmask altitude field to determine if alt = invalid + if ((int24 & 0x00FFFFFF) == 0x00800000) { + ack->alt = 0; + ack->altValid = false; + + } else { + ack->alt = int24; + ack->altValid = true; + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c new file mode 100644 index 0000000000..e9cb01ff0f --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeFlightId.c @@ -0,0 +1,41 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeFlightId.c + * @author Jacob.Garrison + * + * @date Mar 10, 2021 + * + */ + +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_ID_LEN 8 // The number of bytes in the flight id field + +typedef struct __attribute__((packed)) +{ + uint8_t start; + uint8_t type; + uint8_t id; + uint8_t payloadLen; + char flightId[SG_ID_LEN]; + uint8_t rsvd[4]; + uint8_t checksum; +} flightid_t; + +/* + * Documented in the header file. + */ +bool sgDecodeFlightId(uint8_t *buffer, sg_flightid_t *id) +{ + flightid_t sgId; + memcpy(&sgId, buffer, sizeof(flightid_t)); + + strcpy(id->flightId, sgId.flightId); + memset(&id->flightId[SG_ID_LEN], '\0', 1); // Ensure flight id is null-terminated + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c new file mode 100644 index 0000000000..c969509f1f --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeInstall.c @@ -0,0 +1,84 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeInstall.c + * @author Jacob.Garrison + * + * @date Mar 9, 2021 + * + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_REG_LEN 7 // The number of bytes in the registration field + +#define SG_STL_ANTENNA 0x03 +#define SG_STL_ALT_RES 0x08 +#define SG_STL_HDG_TYPE 0x10 +#define SG_STL_SPD_TYPE 0x20 +#define SG_STL_HEATER 0x40 +#define SG_STL_WOW 0x80 + +typedef struct __attribute__((packed)) +{ + uint8_t start; + uint8_t type; + uint8_t id; + uint8_t payloadLen; + uint8_t icao[3]; + char registration[SG_REG_LEN]; + uint8_t rsvd1[2]; + uint8_t com0; + uint8_t com1; + uint8_t ipAddress[4]; + uint8_t subnetMask[4]; + uint8_t port[2]; + uint8_t gpsIntegrity; + uint8_t emitterSet; + uint8_t emitterType; + uint8_t size; + uint8_t maxSpeed; + uint8_t altOffset[2]; + uint8_t rsvd2[2]; + uint8_t config; + uint8_t rsvd3[2]; + uint8_t checksum; +} stl_t; + +/* + * Documented in the header file. + */ +bool sgDecodeInstall(uint8_t *buffer, sg_install_t *stl) +{ + memset(&stl->reg[0], '\0', sizeof(stl->reg)); // Ensure registration is null-terminated + + stl_t sgStl; + memcpy(&sgStl, buffer, sizeof(stl_t)); + + stl->icao = toIcao(sgStl.icao); + strcpy(stl->reg, sgStl.registration); + memset(&stl->reg[SG_REG_LEN], 0, 1); // Ensure registration is null-terminated + stl->com0 = (sg_baud_t)(sgStl.com0); + stl->com1 = (sg_baud_t)(sgStl.com1); + stl->eth.ipAddress = toUint32(sgStl.ipAddress); + stl->eth.subnetMask = toUint32(sgStl.subnetMask); + stl->eth.portNumber = toUint16(sgStl.port); + stl->sil = (sg_sil_t)(sgStl.gpsIntegrity >> 4); + stl->sda = (sg_sda_t)(sgStl.gpsIntegrity & 0x0F); + stl->emitter = (sg_emitter_t)(0x10 * sgStl.emitterSet + sgStl.emitterType); + stl->size = (sg_size_t)sgStl.size; + stl->maxSpeed = (sg_airspeed_t)sgStl.maxSpeed; + stl->altOffset = toUint16(sgStl.altOffset); + stl->antenna = (sg_antenna_t)sgStl.config & SG_STL_ANTENNA; + stl->altRes100 = sgStl.config & SG_STL_ALT_RES; + stl->hdgTrueNorth = sgStl.config & SG_STL_HDG_TYPE; + stl->airspeedTrue = sgStl.config & SG_STL_SPD_TYPE; + stl->heater = sgStl.config & SG_STL_HEATER; + stl->wowConnected = sgStl.config & SG_STL_WOW; + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c new file mode 100644 index 0000000000..522b1b58d0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeMSR.c @@ -0,0 +1,183 @@ +/** + * @copyright Copyright (c) 2020 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeMSR.c + * @author jim.billmeyer + * + * @date Mar 17, 2021 + */ + +#include "sg.h" +#include +#include +#include +#include "sgUtil.h" + +#define MS_PARAM_TOA (1 << 3) +#define MS_PARAM_ADSBVER (1 << 2) +#define MS_PARAM_CALLSIGN (1 << 1) +#define MS_PARAM_CATEMITTER (1 << 0) + +#define MS_PARAM_AVLEN (1 << 7) +#define MS_PARAM_PRIORITY (1 << 6) +#define MS_PARAM_CAPCODES (1 << 5) +#define MS_PARAM_OPMODE (1 << 4) +#define MS_PARAM_NACP (1 << 3) +#define MS_PARAM_NACV (1 << 2) +#define MS_PARAM_SDA (1 << 1) +#define MS_PARAM_GVA (1 << 0) + +#define MS_PARAM_NIC (1 << 7) +#define MS_PARAM_HEADING (1 << 6) +#define MS_PARAM_VRATE (1 << 5) + +#define MS_CAP_B2LOW (1 << 3) +#define MS_CAP_UAT (1 << 1) +#define MS_CAP_TCR ((1 << 2) | (1 << 3)) +#define MS_CAP_TSR (1 << 4) +#define MS_CAP_ARV (1 << 5) +#define MS_CAP_ADSB (1 << 6) +#define MS_CAP_TCAS (1 << 7) + +#define MS_OP_GPS_LATFMT (1 << 7) +#define MS_OP_GPS_LONFMT (1 << 6) +#define MS_OP_TCAS_RA (1 << 5) +#define MS_OP_IDENT (1 << 4) +#define MS_OP_SINGLE_ANT (1 << 2) + +/// the payload offset. +#define PBASE 4 + +/* + * Documented in the header file. + */ +bool sgDecodeMSR(uint8_t *buffer, sg_msr_t *msr) +{ + memset(msr, 0, sizeof(sg_msr_t)); + + uint8_t fields[3]; + memcpy(fields, &buffer[PBASE + 0], 3); + + if (buffer[PBASE + 1] == 0x6E && buffer[PBASE + 2] == 0x60) { + msr->type = msrTypeV0; + + } else if (buffer[PBASE + 1] == 0x7E && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV1Airborne; + + } else if (buffer[PBASE + 1] == 0xFE && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV1Surface; + + } else if (buffer[PBASE + 1] == 0x7F && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV2Airborne; + + } else if (buffer[PBASE + 1] == 0xFF && buffer[PBASE + 2] == 0xE0) { + msr->type = msrTypeV2Surface; + } + + msr->flags = buffer[PBASE + 3]; + msr->addr = toInt32(&buffer[PBASE + 4]) & 0xFFFFFF; + msr->addrType = buffer[PBASE + 7] & 0xFF; + + uint8_t ofs = 8; + + if (fields[0] & MS_PARAM_TOA) { + msr->toa = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & MS_PARAM_ADSBVER) { + msr->version = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[0] & MS_PARAM_CALLSIGN) { + memset(msr->callsign, 0, 9); + memcpy(msr->callsign, &buffer[PBASE + ofs], 8); + ofs += 8; + } + + if (fields[0] & MS_PARAM_CATEMITTER) { + msr->emitter = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_AVLEN) { + msr->size = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_PRIORITY) { + msr->priority = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_CAPCODES) { + uint8_t cap = buffer[PBASE + ofs + 0]; + msr->capability.b2low = cap & MS_CAP_B2LOW; + + cap = buffer[PBASE + ofs + 1]; + msr->capability.uat = cap & MS_CAP_UAT; + msr->capability.tcr = (cap & MS_CAP_TCR) >> 2; + msr->capability.tsr = cap & MS_CAP_TSR; + msr->capability.arv = cap & MS_CAP_ARV; + msr->capability.adsb = cap & MS_CAP_ADSB; + msr->capability.tcas = cap & MS_CAP_TCAS; + + ofs += 3; + } + + if (fields[1] & MS_PARAM_OPMODE) { + uint8_t op = buffer[PBASE + ofs + 0]; + msr->opMode.gpsLatFmt = (op & MS_OP_GPS_LATFMT) == 0; + msr->opMode.gpsLonFmt = (op & MS_OP_GPS_LONFMT) == 0; + msr->opMode.tcasRA = op & MS_OP_TCAS_RA; + msr->opMode.ident = op & MS_OP_IDENT; + msr->opMode.singleAnt = op & MS_OP_SINGLE_ANT; + + op = buffer[PBASE + ofs + 1]; + msr->opMode.gpsLatOfs = op >> 5; + msr->opMode.gpsLonOfs = op & 0x17; + + ofs += 2; + } + + if (fields[1] & MS_PARAM_NACP) { + msr->svQuality.nacp = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_NACV) { + msr->svQuality.nacv = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[1] & MS_PARAM_SDA) { + uint8_t sda = buffer[PBASE + ofs]; + msr->svQuality.sda = (sda & 0x18) >> 3; + msr->svQuality.silSupp = (sda & 0x04); + msr->svQuality.sil = (sda & 0x03); + ofs++; + } + + if (fields[1] & MS_PARAM_GVA) { + msr->svQuality.gva = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[2] & MS_PARAM_NIC) { + msr->svQuality.nicBaro = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[2] & MS_PARAM_HEADING) { + msr->trackHeading = buffer[PBASE + ofs]; + ofs++; + } + + if (fields[2] & MS_PARAM_VRATE) { + msr->vrateType = buffer[PBASE + ofs]; + ofs++; + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c new file mode 100644 index 0000000000..3d24a7729d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgDecodeSVR.c @@ -0,0 +1,209 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgDecodeSVR.c + * @author jimb + * + * @date Feb 10, 2021 + * + * This file receives a raw ADS-B target state vector report message buffer and + * parses the payload into a data struct. + */ + +#include +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" +#include "target.h" + +// airborne surface +// -------- -------- +#define SV_PARAM_TOA_EPOS (1 << 3) // x +#define SV_PARAM_TOA_POS (1 << 2) // x x +#define SV_PARAM_TOA_VEL (1 << 1) // x x +#define SV_PARAM_LATLON (1 << 0) // x x + +#define SV_PARAM_GEOALT (1 << 7) // x +#define SV_PARAM_VEL (1 << 6) // x +#define SV_PARAM_SURF_GS (1 << 5) // x +#define SV_PARAM_SURF_HEAD (1 << 4) // x +#define SV_PARAM_BAROALT (1 << 3) // x +#define SV_PARAM_VRATE (1 << 2) // x +#define SV_PARAM_NIC (1 << 1) // x x +#define SV_PARAM_ESTLAT (1 << 0) // x + +#define SV_PARAM_ESTLON (1 << 7) // x +#define SV_PARAM_ESTNVEL (1 << 6) +#define SV_PARAM_ESTEVAL (1 << 5) +#define SV_PARAM_SURV (1 << 4) // x x +#define SV_PARAM_REPORT (1 << 3) // x x + +/// the payload offset. +#define PBASE 4 + +/* + * Documented in the header file. + */ +bool sgDecodeSVR(uint8_t *buffer, sg_svr_t *svr) +{ + // memset(svr, 0, sizeof(sg_svr_t)); + + uint8_t fields[3]; + memcpy(&fields, &buffer[PBASE + 0], 3); + + svr->type = buffer[PBASE + 0] == 0x1F ? svrAirborne : svrSurface; + svr->flags = buffer[PBASE + 3]; + svr->eflags = buffer[PBASE + 4]; + svr->addr = toInt32(&buffer[PBASE + 5]) & 0xFFFFFF; + svr->addrType = buffer[PBASE + 8] & 0xFF; + + uint8_t ofs = 9; + + if (fields[0] & SV_PARAM_TOA_EPOS) { + svr->toaEst = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & SV_PARAM_TOA_POS) { + svr->toaPosition = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & SV_PARAM_TOA_VEL) { + svr->toaSpeed = toTOA(&buffer[PBASE + ofs]); + ofs += 2; + } + + if (fields[0] & SV_PARAM_LATLON) { + if (svr->validity.position) { + svr->lat = toLatLon(&buffer[PBASE + ofs + 0]); + svr->lon = toLatLon(&buffer[PBASE + ofs + 3]); + + } else { + svr->lat = 0.0; + svr->lon = 0.0; + } + + ofs += 6; + } + + if (svr->type == svrAirborne) { + if (fields[1] & SV_PARAM_GEOALT) { + if (svr->validity.geoAlt) { + svr->airborne.geoAlt = toAlt(&buffer[PBASE + ofs]); + + } else { + svr->airborne.geoAlt = 0; + } + + ofs += 3; + } + + if (fields[1] & SV_PARAM_VEL) { + if (svr->validity.airSpeed) { + int16_t nvel = toVel(&buffer[PBASE + ofs + 0]); + int16_t evel = toVel(&buffer[PBASE + ofs + 2]); + + svr->airborne.heading = toHeading2((double)nvel, (double)evel); + svr->airborne.speed = sqrt(nvel * nvel + evel * evel); + svr->airborne.velEW = evel; + svr->airborne.velNS = nvel; + + } else { + svr->airborne.heading = 0; + svr->airborne.speed = 0; + svr->airborne.velEW = 0; + svr->airborne.velNS = 0; + } + + ofs += 4; + } + + if (fields[1] & SV_PARAM_BAROALT) { + if (svr->validity.baroAlt) { + svr->airborne.baroAlt = toAlt(&buffer[PBASE + ofs]); + + } else { + svr->airborne.baroAlt = 0; + } + + ofs += 3; + } + + if (fields[1] & SV_PARAM_VRATE) { + if (svr->validity.baroVRate || svr->validity.geoVRate) { + svr->airborne.vrate = toInt16(&buffer[PBASE + ofs]); + + } else { + svr->airborne.vrate = 0; + } + + ofs += 2; + } + + } else { + if (fields[1] & SV_PARAM_SURF_GS) { + if (svr->validity.surfSpeed) { + svr->surface.speed = toGS(&buffer[PBASE + ofs]); + + } else { + svr->surface.speed = 0; + } + + ofs += 1; + } + + if (fields[1] & SV_PARAM_SURF_HEAD) { + if (svr->validity.surfHeading) { + svr->surface.heading = toHeading(&buffer[PBASE + ofs]); + + } else { + svr->surface.heading = 0; + } + + ofs += 1; + } + } + + if (fields[1] & SV_PARAM_NIC) { + svr->nic = buffer[PBASE + ofs]; + + ofs += 1; + } + + if (fields[1] & SV_PARAM_ESTLAT) { + if (svr->evalidity.estPosition) { + svr->airborne.estLat = toLatLon(&buffer[PBASE + ofs]); + + } else { + svr->airborne.estLat = 0; + } + + ofs += 3; + } + + if (fields[2] & SV_PARAM_ESTLON) { + if (svr->evalidity.estPosition) { + svr->airborne.estLon = toLatLon(&buffer[PBASE + ofs]); + + } else { + svr->airborne.estLon = 0; + } + + ofs += 3; + } + + if (fields[2] & SV_PARAM_SURV) { + svr->survStatus = buffer[PBASE + ofs]; + ofs += 1; + } + + if (fields[2] & SV_PARAM_REPORT) { + svr->mode = buffer[PBASE + ofs]; + } + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c new file mode 100644 index 0000000000..4a048548eb --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeDataReq.c @@ -0,0 +1,51 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeDataReq.c + * @author Jacob.Garrison + * + * @date Feb 23, 2021 + * + * This file receives a populated data request struct and + * converts it into a data request message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_DATAREQ SG_MSG_LEN_DATAREQ - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. + +#define OFFSET_REQ_TYPE 0 /// the requested response message type +#define OFFSET_RSVD_1 1 /// a reserved field +#define OFFSET_RSVD_2 2 /// a reserved field +#define OFFSET_RSVD_3 3 /// a reserved field + +/* + * Documented in the header file. + */ +bool sgEncodeDataReq(uint8_t *buffer, sg_datareq_t *data, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_DATAREQ; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_DATAREQ; + + // populate Request Type + buffer[PBASE + OFFSET_REQ_TYPE] = data->reqType; + + // populate Reserved fields + buffer[PBASE + OFFSET_RSVD_1] = 0; + buffer[PBASE + OFFSET_RSVD_2] = 0; + buffer[PBASE + OFFSET_RSVD_3] = 0; + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_DATAREQ); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c new file mode 100644 index 0000000000..a4466b7a07 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeFlightId.c @@ -0,0 +1,47 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeFlightId.c + * @author Jacob.Garrison + * + * @date Feb 25, 2021 + * + */ + +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_FLIGHT SG_MSG_LEN_FLIGHT - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. +#define OFFSET_ID 0 /// the flight id offset in the payload. +#define OFFSET_RSVD 8 /// the reserved field offset in the payload. + +#define ID_LEN 8 /// the length of the flight identification field. + +/* + * Documented in the header file. + */ +bool sgEncodeFlightId(uint8_t *buffer, sg_flightid_t *id, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_FLIGHT; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_FLIGHT; + + // populate flight identification + charArray2Buf(&buffer[PBASE + OFFSET_ID], id->flightId, ID_LEN); + + // populate reserved field + uint322Buf(&buffer[PBASE + OFFSET_RSVD], 0); + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_FLIGHT); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c new file mode 100644 index 0000000000..4820e0424b --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeGPS.c @@ -0,0 +1,92 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeGPS.c + * @author Jacob.Garrison + * + * @date Mar 1, 2021 + * + * This file receives a populated GPS message struct and + * converts it into a GPS message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_GPS SG_MSG_LEN_GPS - 5 /// the payload length. +#define _UNUSED(x) ((void)(x)) + +#define PBASE 4 /// the payload offset. +#define OFFSET_LONGITUDE 0 /// the longitude offset in the payload. +#define OFFSET_LATITUDE 11 /// the latitude offset in the payload. +#define OFFSET_SPEED 21 /// the ground speed offset in the payload. +#define OFFSET_TRACK 27 /// the ground track offset in the payload. +#define OFFSET_STATUS 35 /// the hemisphere/data status offset in the payload. +#define OFFSET_TIME 36 /// the time of fix offset in the payload. +#define OFFSET_HEIGHT 46 /// the GNSS height offset in the payload. +#define OFFSET_HPL 50 /// the horizontal protection limit offset in the payload. +#define OFFSET_HFOM 54 /// the horizontal figure of merit offset in the payload. +#define OFFSET_VFOM 58 /// the vertical figure of merit offset in the payload. +#define OFFSET_NACV 62 /// the navigation accuracy for velocity offset in the payload. + +#define LEN_LNG 11 /// bytes in the longitude field +#define LEN_LAT 10 /// bytes in the latitude field +#define LEN_SPD 6 /// bytes in the speed over ground field +#define LEN_TRK 8 /// bytes in the ground track field +#define LEN_TIME 10 /// bytes in the time of fix field + +/* + * Documented in the header file. + */ +bool sgEncodeGPS(uint8_t *buffer, sg_gps_t *gps, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_GPS; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_GPS; + + // populate longitude + charArray2Buf(&buffer[PBASE + OFFSET_LONGITUDE], gps->longitude, LEN_LNG); + + // populate latitude + charArray2Buf(&buffer[PBASE + OFFSET_LATITUDE], gps->latitude, LEN_LAT); + + // populate ground speed + charArray2Buf(&buffer[PBASE + OFFSET_SPEED], gps->grdSpeed, LEN_SPD); + + // populate ground track + charArray2Buf(&buffer[PBASE + OFFSET_TRACK], gps->grdTrack, LEN_TRK); + + // populate hemisphere/data status + buffer[PBASE + OFFSET_STATUS] = !gps->gpsValid << 7 | + gps->fdeFail << 6 | + gps->lngEast << 1 | + gps->latNorth; + + // populate time of fix + charArray2Buf(&buffer[PBASE + OFFSET_TIME], gps->timeOfFix, LEN_TIME); + + // populate gnss height + float2Buf(&buffer[PBASE + OFFSET_HEIGHT], gps->height); + + // populate HPL + float2Buf(&buffer[PBASE + OFFSET_HPL], gps->hpl); + + // populate HFOM + float2Buf(&buffer[PBASE + OFFSET_HFOM], gps->hfom); + + // populate VFOM + float2Buf(&buffer[PBASE + OFFSET_VFOM], gps->vfom); + + // populate NACv + buffer[PBASE + OFFSET_NACV] = gps->nacv << 4; + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_GPS); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c new file mode 100644 index 0000000000..c7fb0876cd --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeInstall.c @@ -0,0 +1,132 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeInstall.c + * @author Jacob.Garrison + * + * @date Feb 23, 2021 + * + * This file receives a populated installation message struct and + * converts it into a installation message buffer. + */ + +#include +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_INSTALL SG_MSG_LEN_INSTALL - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. +#define OFFSET_ICAO 0 /// the icao address offset in the payload. +#define OFFSET_REG 3 /// the registration offset in the payload. +#define OFFSET_RSVD1 10 /// the first reserved field offset in the payload. +#define OFFSET_COM0 12 /// the COM port 0 offset in the payload. +#define OFFSET_COM1 13 /// the COM port 1 offset in the payload. +#define OFFSET_IP 14 /// the IP address offset in the payload. +#define OFFSET_MASK 18 /// the net mask offset in the payload. +#define OFFSET_PORT 22 /// the port number offset in the payload. +#define OFFSET_GPS 24 /// the GPS integrity offset in the payload. +#define OFFSET_EMIT_SET 25 /// the emitter category offset in the payload. +#define OFFSET_EMIT_TYPE 26 /// the emitter type offset in the payload. +#define OFFSET_SIZE 27 /// the aircraft size offset in the payload. +#define OFFSET_SPEED 28 /// the maximum airspeed offset in the payload. +#define OFFSET_ENCODER 29 /// the altitude-encoder-offset offset in the payload. +#define OFFSET_RSVD2 31 /// the second reserved field offset in the payload. +#define OFFSET_CONFIG 33 /// the configuration offset in the payload. +#define OFFSET_RSVD3 34 /// the third reserved field offset in the payload. + +#define REG_LEN 7 /// the registration field length. +/* + * Documented in the header file. + */ +bool sgEncodeInstall(uint8_t *buffer, sg_install_t *stl, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_INSTALL; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_INSTALL; + + // populate icao address + icao2Buf(&buffer[PBASE + OFFSET_ICAO], stl->icao); + + // populate aircraft registration + charArray2Buf(&buffer[PBASE + OFFSET_REG], stl->reg, REG_LEN); + + // populate reserved fields + uint162Buf(&buffer[PBASE + OFFSET_RSVD1], 0); + + // populate COM port 0, correct enum offset + buffer[PBASE + OFFSET_COM0] = stl->com0; + + // populate COM port 1, correct enum offset + buffer[PBASE + OFFSET_COM1] = stl->com1; + + // populate IP address + uint322Buf(&buffer[PBASE + OFFSET_IP], stl->eth.ipAddress); + + // populate net mask + uint322Buf(&buffer[PBASE + OFFSET_MASK], stl->eth.subnetMask); + + // populate port number + uint162Buf(&buffer[PBASE + OFFSET_PORT], stl->eth.portNumber); + + // populate gps integrity + buffer[PBASE + OFFSET_GPS] = stl->sil << 4 | + stl->sda; + + // populate emitter category set and type + uint8_t emitSet; + uint8_t emitType; + + if (stl->emitter < SG_EMIT_OFFSET_B) { // group A + emitSet = SG_EMIT_GROUP_A; + emitType = stl->emitter - SG_EMIT_OFFSET_A; + + } else if (stl->emitter < SG_EMIT_OFFSET_C) { // group B + emitSet = SG_EMIT_GROUP_B; + emitType = stl->emitter - SG_EMIT_OFFSET_B; + + } else if (stl->emitter < SG_EMIT_OFFSET_D) { // group C + emitSet = SG_EMIT_GROUP_C; + emitType = stl->emitter - SG_EMIT_OFFSET_C; + + } else { // group D + emitSet = SG_EMIT_GROUP_D; + emitType = stl->emitter - SG_EMIT_OFFSET_D; + } + + buffer[PBASE + OFFSET_EMIT_SET] = emitSet; + buffer[PBASE + OFFSET_EMIT_TYPE] = emitType; + + // populate aircraft size + buffer[PBASE + OFFSET_SIZE] = stl->size; + + // populate max airspeed + buffer[PBASE + OFFSET_SPEED] = stl->maxSpeed; + + // populate altitude encoder offset + uint162Buf(&buffer[PBASE + OFFSET_ENCODER], stl->altOffset); + + // populate reserved fields + uint162Buf(&buffer[PBASE + OFFSET_RSVD2], 0); + + // populate install configuration + buffer[PBASE + OFFSET_CONFIG] = stl->wowConnected << 7 | + stl->heater << 6 | + stl->airspeedTrue << 5 | + stl->hdgTrueNorth << 4 | + stl->altRes100 << 3 | + stl->antenna; + + // populate reserved fields + uint162Buf(&buffer[PBASE + OFFSET_RSVD3], 0); + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_INSTALL); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c new file mode 100644 index 0000000000..70de6c7b85 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeOperating.c @@ -0,0 +1,107 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeOperating.c + * @author Jacob.Garrison + * + * @date Feb 15, 2021 + * + * This file receives a populated operating message struct and + * converts it into a operating message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_OPMSG SG_MSG_LEN_OPMSG - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. +#define OFFSET_SQUAWK 0 /// the squawk code offset in the payload. +#define OFFSET_CONFIG 2 /// the mode/config offset in the payload. +#define OFFSET_EMRG_ID 3 /// the emergency flag offset in the payload. +#define OFFSET_ALT 4 /// the altitude offset in the payload. +#define OFFSET_RATE 6 /// the climb rate offset in the payload. +#define OFFSET_HEADING 8 /// the heading offset in the payload. +#define OFFSET_AIRSPEED 10 /// the airspeed offset in the payload. + +/* + * Documented in the header file. + */ +bool sgEncodeOperating(uint8_t *buffer, sg_operating_t *op, uint8_t msgId) +{ + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_OPMSG; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_OPMSG; + + // populate Squawk code + uint162Buf(&buffer[PBASE + OFFSET_SQUAWK], op->squawk); + + // populate Mode/Config + buffer[PBASE + OFFSET_CONFIG] = op->milEmergency << 5 | + op->enableXBit << 4 | + op->enableSqt << 3 | + op->savePowerUp << 2 | + op->opMode; + + // populate Emergency/Ident + buffer[PBASE + OFFSET_EMRG_ID] = op->identOn << 3 | + op->emergcType; + + // populate Altitude + uint16_t altCode = 0; + + if (op->altUseIntrnl) { + altCode = 0x8000; + + } else if (op->altHostAvlbl) { + // 100 foot encoding conversion + altCode = (op->altitude + 1200) / 100; + + if (op->altRes25) { + altCode *= 4; + } + + // 'Host altitude available' flag + altCode += 0x4000; + } + + uint162Buf(&buffer[PBASE + OFFSET_ALT], altCode); + + // populate Altitude Rate + int16_t rate = op->climbRate / 64; + + if (!op->climbValid) { + rate = 0x8000; + } + + uint162Buf(&buffer[PBASE + OFFSET_RATE], rate); + + // populate Heading + // conversion: heading * ( pow(2, 15) / 360 ) + uint16_t heading = op->heading * 32768 / 360; + + if (op->headingValid) { + heading += 0x8000; + } + + uint162Buf(&buffer[PBASE + OFFSET_HEADING], heading); + + // populate Airspeed + uint16_t airspeed = op->airspd; + + if (op->airspdValid) { + airspeed += 0x8000; + } + + uint162Buf(&buffer[PBASE + OFFSET_AIRSPEED], airspeed); + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_OPMSG); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c new file mode 100644 index 0000000000..1cd918c1b0 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgEncodeTargetReq.c @@ -0,0 +1,63 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgEncodeTargetReq.c + * @author Jacob.Garrison + * + * @date Feb 19, 2021 + * + * This file receives a populated target request struct and + * converts it into a target request message buffer. + */ + +#include +#include + +#include "sg.h" +#include "sgUtil.h" + +#define SG_PAYLOAD_LEN_TARGETREQ SG_MSG_LEN_TARGETREQ - 5 /// the payload length. + +#define PBASE 4 /// the payload offset. + +#define OFFSET_REQ_TYPE 0 /// the adsb reporting type and transmit port offset +#define OFFSET_MAX_TARGETS 1 /// the maximum number of targets offset +#define OFFSET_ICAO 3 /// the requested target icao offset +#define OFFSET_REPORTS 6 /// the requested report type offset +/* + * Documented in the header file. + */ +bool sgEncodeTargetReq(uint8_t *buffer, sg_targetreq_t *tgt, uint8_t msgId) +{ + + // populate header + buffer[0] = SG_MSG_START_BYTE; + buffer[1] = SG_MSG_TYPE_HOST_TARGETREQ; + buffer[2] = msgId; + buffer[3] = SG_PAYLOAD_LEN_TARGETREQ; + + // populate Request Type + buffer[PBASE + OFFSET_REQ_TYPE] = tgt->transmitPort << 6 | + tgt->reqType; + + // populate Max Targets + uint162Buf(&buffer[PBASE + OFFSET_MAX_TARGETS], tgt->maxTargets); + + // populate Requested ICAO + icao2Buf(&buffer[PBASE + OFFSET_ICAO], tgt->icao); + + // populated Requested Reports + buffer[PBASE + OFFSET_REPORTS] = tgt->ownship << 7 | + tgt->commA << 6 | + tgt->military << 5 | + tgt->tisb << 4 | + tgt->airRefVel << 3 | + tgt->targetState << 2 | + tgt->modeStatus << 1 | + tgt->stateVector; + + // populate checksum + appendChecksum(buffer, SG_MSG_LEN_TARGETREQ); + + return true; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h new file mode 100644 index 0000000000..1901305446 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/sgUtil.h @@ -0,0 +1,325 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file sgUtil.h + * @author jimb + * + * @date Feb 2, 2021 + */ + +#ifndef UTIL_H +#define UTIL_H + +#include + +#ifndef swap16 +#define swap16(data) \ + ((((data) >> 8) & 0x00FF) | (((data) << 8) & 0xFF00)) +#endif + +#ifndef swap24 +#define swap24(data) \ + (((data) >> 16) | ((data)&0x00FF00) | (((data) << 16) & 0xFF0000)) +#endif + +#ifndef swap32 +#define swap32(data) \ + (((data) >> 24) | (((data)&0x00FF0000) >> 8) | (((data)&0x0000FF00) << 8) | ((data) << 24)) +#endif + +#ifndef swap64 +#define swap64(data) \ + (swap32((data & 0x00000000ffffffffULL))) << 32 | swap32(data >> 32)) +#endif + +#ifndef PI +#define PI 3.14159265359 +#endif + +#ifndef toRad +#define toRad(deg) \ + ((deg)*PI / 180.0) +#endif + +#ifndef toDeg +#define toDeg(rad) \ + ((rad)*180 / PI) +#endif + +#ifndef toMeter +#define toMeter(feet) \ + ((feet)*0.3048) +#endif + +#ifndef toFeet +#define toFeet(meter) \ + ((meter)*3.2808) +#endif + +/** + * Converts an array of bytes to a 16 bit integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 16 bit integer. + */ +int16_t toInt16(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 32 bit integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 32 bit integer. + */ +int32_t toInt32(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 16 unsigned bit integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 16 bit integer. + */ +uint16_t toUint16(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 24 bit unsigned integer with leading 0s. + * + * @param bytes The array of bytes to convert. + * + * @return The 24 bit unsigned integer with leading 0s. + */ +uint32_t toUint24(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a 32 bit unsigned integer. + * + * @param bytes The array of bytes to convert. + * + * @return The 32 bit unsigned integer. + */ +uint32_t toUint32(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a distance. + * + * @param bytes The array of bytes to convert. + * + * @return The distance value. + */ +double toDist(const uint8_t *bytes); + +/** + * Converts an array of bytes to a 24 bit unsigned integer with leading 0's. + * + * @param bytes The array of bytes to convert. + * + * @return The 32 bit unsigned integer. + */ +uint32_t toIcao(const uint8_t bytes[]); + +/** + * Converts an array of bytes to a lat/lon floating point number. + * + * @param bytes The array of bytes to convert. + * + * @return The lat/lon value. + */ +double toLatLon(const uint8_t bytes[]); + +/** + * Convert an array to an altitude. + * + * @param bytes The bytes to get the altitude from. + * + * @return The converted altitude. + */ +double toAlt(const uint8_t bytes[]); + +/** + * Converts an array of bytes to an airborne velocity. + * + * @param bytes The bytes to extract the velocity. + * + * @return The converted velocity. + */ +double toVel(const uint8_t bytes[]); + +/** + * Converts the array of bytes to the surface ground speed. + * + * @param bytes The bytes to extract the ground speed. + * + * @return The converted ground speed. + */ +uint8_t toGS(const uint8_t bytes[]); + +/** + * Converts the bytes into the heading value. + * + * @param bytes The bytes holding the heading value. + * + * @return The heading. + */ +double toHeading(const uint8_t bytes[]); + +/** + * Determine heading from y and x speed vectors. + * + * @param y The y speed vector. + * @param x The x speed vector. + * + * @return The resulting heading. + */ +int16_t toHeading2(double y, double x); + +/** + * Convert the array of bytes to a time of applicability (TOA). + * + * @param bytes The bytes to convert to a TOA. + * + * @return The converted TOA. + */ +float toTOA(const uint8_t bytes[]); + +/** + * Convert an array of bytes to a float + * + * @param bufferPos the address of the field's first corresponding buffer byte. + * + * @return The converted float value. + */ + +float toFloat(const uint8_t *bufferPos); + +/** + * Convert an array of bytes to a double + * + * @param bufferPos the address of the field's first corresponding buffer byte. + * + * @return The converted double value. + */ + +double toDouble(const uint8_t *bufferPos); + +/** + * Converts a uint16_t into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The uint16_t to be converted. + * + * no return value, two buffer bytes are filled by reference + */ +void uint162Buf(uint8_t *bufferPos, uint16_t value); + +/** + * Converts a int16_t into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The int32_t to be converted. + * + * no return value, two buffer bytes are filled by reference + */ +void int242Buf(uint8_t *bufferPos, int32_t value); + +/** + * Converts a uint32_t into a 24 bit host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The int32_t to be converted. + * + * no return value, three buffer bytes are filled by reference + */ +void uint242Buf(uint8_t *bufferPos, uint32_t value); + +/** + * Converts a uint32_t into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The uint32_t to be converted. + * + * no return value, two buffer bytes are filled by reference + */ +void uint322Buf(uint8_t *bufferPos, uint32_t value); + +/** + * Converts a uint32_t containing an ICAO into its 24-bit host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param icao The uint32_t ICAO to be converted. + * + * no return value, three buffer bytes are filled by reference + * + * @warning icao parameter must be between 0x000000 and 0xFFFFFF + */ +void icao2Buf(uint8_t *bufferPos, uint32_t icao); + +/** + * Converts an array of characters into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param arr[] The array of characters. + * @param len The number of characters in the array. + * + * no return value, the specified quantity of buffer bytes are filled by reference + */ +void charArray2Buf(uint8_t *bufferPos, char arr[], uint8_t len); + +/** + * Converts a float into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte. + * @param value The float to be converted. + * + * no return value, four buffer bytes are filled by reference + * + * @warning The output of this function depends on the machine's endianness. It is designed + * for Little-Endian machines, only. + */ +void float2Buf(uint8_t *bufferPos, float value); + +/** + * Converts a double into its host message buffer format + * + * @param bufferPos The address of the field's first corresponding buffer byte + * @param value The double to be converted + * + * no return value, eight buffer bytes are filled by reference + * + * @warning The output of this function depends on the machine's endianness. It is designed + * for Little-Endian machines, only + */ +void double2Buf(uint8_t *bufferPos, double value); + +/** + * Converts a double into an encoded lat/lon buffer format. + * + * @param bytes address of the field's first corresponding buffer byte + * @param value the double to be converted. + * + * no return value, 3 buffer bytes are filled by reference. + */ +void latLon2Buf(uint8_t bytes[], double value); + +/** + * Calculate checksum for a host message. + * + * @param buffer The raw message buffer. + * @param len The total quantity of bytes in the buffer + * + * @return The resulting checksum. + */ +uint8_t calcChecksum(uint8_t *buffer, uint8_t len); + +/** + * Add the checksum to a host message. + * + * @param buffer The raw message buffer. + * @param len The total quantity of bytes in the buffer + * + * no return value, final buffer byte is set to the checksum value. + */ +void appendChecksum(uint8_t *buffer, uint8_t len); + +#endif /* UTIL_H */ diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/target.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.c new file mode 100644 index 0000000000..d1d51ad65d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.c @@ -0,0 +1,127 @@ +/** + * @copyright Copyright (c) 2020 SoftSolutions, Inc. All rights reserved. + * + * @File: target.c + * @Author: jim billmeyer + * + * @date December 11, 2020, 12:50 AM + */ + +#include +#include "target.h" + +static ownship_t ownship; +static target_t targets[XPNDR_ADSB_TARGETS] = { + { + 0, + }, +}; + +/* + * Documented in the header file. + */ +target_t *targetList(void) +{ + return targets; +} + +/* + * Documented in the header file. + */ +ownship_t *targetOwnship(void) +{ + return &ownship; +} + +/* + * Documented in the header file. + */ +target_t *targetFind(uint32_t icao) +{ + for (uint16_t i = 0; i < XPNDR_ADSB_TARGETS; i++) { + if (icao == targets[i].icao) { + // clear strike counter and set find flag while preserving the used bit. + targets[i].flag = TARGET_FLAG_FOUND | TARGET_FLAG_USED; + return &targets[i]; + } + } + + return 0; +} + +/* + * Documented in the header file. + */ +void targetPurge(void) +{ + for (uint16_t i = 0; i < XPNDR_ADSB_TARGETS; i++) { + // the the found flag was not set increment the strike counter. + if ((targets[i].flag & TARGET_FLAG_USED) && ((targets[i].flag & TARGET_FLAG_FOUND) == 0)) { + uint8_t strikes = (targets[i].flag & 0xFE) >> 2; + strikes++; + + if (strikes > 5) { + memset(&targets[i], 0, sizeof(target_t)); + + } else { + // set the strike counter and clear the found flag. + targets[i].flag = strikes << 2 | TARGET_FLAG_USED; + } + + } else { + // clear the found flag so the target find function can set it + // to signal that the icao address is still in range. + targets[i].flag = targets[i].flag & (TARGET_FLAG_STRIKE_MASK | TARGET_FLAG_USED); + } + } +} + +/* + * Documented in the header file. + */ +void targetAdd(target_t *target) +{ + for (uint16_t i = 0; i < XPNDR_ADSB_TARGETS; i++) { + if ((targets[i].flag & TARGET_FLAG_USED) == 0x0) { + memcpy(&targets[i], target, sizeof(target_t)); + targets[i].flag = TARGET_FLAG_USED; + break; + } + } +} + +/* + * Documented in the header file. + */ +targetclimb_t targetClimb(int16_t vrate) +{ + if (abs(vrate) < 500) { + return trafLevel; + + } else if (vrate > 0) { + return trafClimb; + + } else { + return trafDescend; + } +} + +/* + * Documented in the header file. + */ +targetalert_t targetAlert(double dist, + uint16_t alt, + int16_t nvel, + int16_t evel) +{ + if (alt <= 3000) { + if (dist <= 3.0) { + return trafResolution; + + } else if (dist <= 6.0) { + return trafAdvisory; + } + } + + return trafTraffic; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/target.h b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.h new file mode 100644 index 0000000000..a06961f421 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/target.h @@ -0,0 +1,128 @@ +/** + * @copyright Copyright (c) 2020 Sagetech, Inc. All rights reserved. + * + * @File: target.h + * @Author: jim billmeyer + * + * @date December 11, 2020, 12:49 AM + */ + +#ifndef TARGET_H +#define TARGET_H + +#include +#include +#include + +#define XPNDR_ADSB_TARGETS 400 // change this to the max number of +// target supported in the system. + +typedef enum { + trafLevel, + trafClimb, + trafDescend +} targetclimb_t; + +typedef enum { + trafTraffic, + trafAdvisory, + trafResolution +} targetalert_t; + +// bit 0 - target found flag. +// bit 1 - target slot in use. +// bits 2-7 - the strike counter. +#define TARGET_FLAG_FOUND 0x01 +#define TARGET_FLAG_USED 0x02 +#define TARGET_FLAG_STRIKE_MASK 0xFC + +typedef struct __attribute__((packed)) +{ + uint32_t icao; + bool airborne; + float bearing; + uint8_t distance; + int8_t altDiff; + int16_t nvel; + int16_t evel; + targetclimb_t climb; + targetalert_t alert; +#ifdef TARGET_SVR + msg_svr_t svr; +#endif + uint8_t flag; // used internally to purge stale targets. +} target_t; + +typedef struct { + uint32_t icao; + bool airborne; + float lat; + float lon; + int32_t alt; + int16_t heading; + uint16_t speed; +} ownship_t; + +/** + * Gets the target list. + * + * @return The array of traffic targets. + */ +target_t *targetList(void); + +/** + * Gets the ownship target information. + * + * @return The ownship target info. + */ +ownship_t *targetOwnship(void); + +/** + * Find a target based on its icao number. + * + * @param icao The target's icao number + * + * @return A pointer to the target element or null if not found. + */ +target_t *targetFind(uint32_t icao); + +/** + * Purge the traffic target list of stale traffic. + * + * The traffic gets purged if a find has not been done based + * on a strike counter. + */ +void targetPurge(void); + +/** + * Adds a target to the traffic target list. + * + * @param target The target to add. + */ +void targetAdd(target_t *target); + +/** + * Gets the target climb flag based on the vertical rate. + * + * @param vrate The current vertical rate of climb for the target. + * + * @return The level, climb or descend flag. + */ +targetclimb_t targetClimb(int16_t vrate); + +/** + * Gets the traffic alert flag. + * + * @param dist The distance of the target to the ownship. + * @param alt The altitude difference between the target and ownship. + * @param nvel The NS speed vector of the target. + * @param evel The EW speed vector of the target. + * + * @return The traffic flag based on the parameters. + */ +targetalert_t targetAlert(double dist, + uint16_t alt, + int16_t nvel, + int16_t evel); + +#endif /* TARGET_H */ diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c new file mode 100644 index 0000000000..188dfae4bb --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toAlt.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toAlt.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_ALT 0.015625 + +/* + * Documented in the header file. + */ +double toAlt(const uint8_t bytes[]) +{ + double value = toInt32(bytes); + value *= SV_RES_ALT; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c new file mode 100644 index 0000000000..66c57b94e4 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toGS.c @@ -0,0 +1,50 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toGS.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint8_t toGS(const uint8_t bytes[]) +{ + uint8_t code = bytes[0]; + float gs = 0.0f; + + if (code <= 0x01) { + gs = 0.0f; + + } else if (code <= 0x08) { + gs = 1.0f; + + } else if (code <= 0x0C) { + gs = 1.0f + (code - 0x09) * 0.25f; + + } else if (code <= 0x26) { + gs = 2.0f + (code - 0x0D) * 0.5f; + + } else if (code <= 0x5D) { + gs = 15.0f + (code - 0x27) * 1.0f; + + } else if (code <= 0x6C) { + gs = 70.0f + (code - 0x5E) * 2.0f; + + } else if (code <= 0x7B) { + gs = 100.0f + (code - 0x6D) * 5.0f; + + } else { + gs = 176.0f; + } + + // first converting to an 16 bit integer is necessary + // to keep the floating point conversion from + // truncating to 0. + return (uint8_t)((int16_t)gs & 0xFF); +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c new file mode 100644 index 0000000000..a89623b512 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toHeading.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_HEAD 1.40625 + +/* + * Documented in the header file. + */ +double toHeading(const uint8_t bytes[]) +{ + double value = bytes[0]; + value *= SV_RES_HEAD; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c new file mode 100644 index 0000000000..4c65f873e7 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toHeading2.c @@ -0,0 +1,33 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toHeading2.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +int16_t toHeading2(double y, double x) +{ + int16_t heading = (toDeg(atan2(y, x)) + 0.5); + heading = 360 - heading + 90; // atan is ccw 0 degrees at x = 1 and y = 0. + + if (heading > 360) { + heading -= 360; + + } else { + while (heading < 0) { + heading += 360; + } + } + + return heading; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c new file mode 100644 index 0000000000..cc683e19cf --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toIcao.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toIcao.c + * @author Jacob.Garrison + * + * @date Mar 9, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint32_t toIcao(const uint8_t bytes[]) +{ + uint32_t icao = (0 << 24) | ((uint32_t)bytes[0] << 16) | ((uint32_t)bytes[1] << 8) | ((uint32_t)bytes[2]); + + return icao; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c new file mode 100644 index 0000000000..0bf4842db8 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt16.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toInt16.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +int16_t toInt16(const uint8_t bytes[]) +{ + int16_t int16 = ((int16_t)bytes[0] << 8) | bytes[1]; + + return int16; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c new file mode 100644 index 0000000000..555844cc10 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toInt32.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toInt32.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +int32_t toInt32(const uint8_t bytes[]) +{ + int32_t int32 = ((int32_t)bytes[0] << 24) | ((int32_t)bytes[1] << 16) | ((int32_t)bytes[2] << 8); + int32 = int32 >> 8; + + return int32; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c new file mode 100644 index 0000000000..bc09e9ee49 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toLatLon.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toLatLon.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_LATLON 180.0 / 8388608.0 // 180 degrees / 2^23 + +/* + * Documented in the header file. + */ +double toLatLon(const uint8_t bytes[]) +{ + double value = toInt32(bytes); + value *= SV_RES_LATLON; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c new file mode 100644 index 0000000000..557fe31774 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toTOA.c @@ -0,0 +1,20 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toTOA.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +float toTOA(const uint8_t bytes[]) +{ + float toa = toInt16(bytes) & 0xFFFF; + return toa / (float) 128.0; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c new file mode 100644 index 0000000000..9b627bd0fd --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint16.c @@ -0,0 +1,21 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toUint16.c + * @author Jacob.Garrison + * + * @date Mar 9, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint16_t toUint16(const uint8_t bytes[]) +{ + uint16_t uint16 = ((uint16_t) bytes[0] << 8 | (uint16_t) bytes[1]); + + return uint16; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c new file mode 100644 index 0000000000..0f869ea755 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toUint32.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toUint32.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +uint32_t toUint32(const uint8_t bytes[]) +{ + uint32_t uint32 = ((uint32_t) bytes[0] << 24) | ((uint32_t) bytes[1] << 16) | ((uint32_t) bytes[2] << 8) | (( + uint32_t) bytes[3]); + + return uint32; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c new file mode 100644 index 0000000000..c21fe14272 --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/toVel.c @@ -0,0 +1,24 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file toVel.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +#define SV_RES_VEL 0.125 + +/* + * Documented in the header file. + */ +double toVel(const uint8_t bytes[]) +{ + double value = toInt16(bytes); + value *= SV_RES_VEL; + + return value; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c new file mode 100644 index 0000000000..775d76057c --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint162Buf.c @@ -0,0 +1,20 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file uint162Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void uint162Buf(uint8_t *bufferPos, uint16_t value) +{ + bufferPos[0] = value >> 8; + bufferPos[1] = value & 0xFF; +} diff --git a/src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c new file mode 100644 index 0000000000..f2e0986e4d --- /dev/null +++ b/src/drivers/transponder/sagetech_mxs/sg_sdk/uint322Buf.c @@ -0,0 +1,22 @@ +/** + * @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved. + * + * @file uint322Buf.c + * @author Jacob.Garrison + * + * @date Mar 2, 2021 + * + */ + +#include "sgUtil.h" + +/* + * Documented in the header file. + */ +void uint322Buf(uint8_t *bufferPos, uint32_t value) +{ + bufferPos[0] = (value & 0xFF000000) >> 24; + bufferPos[1] = (value & 0x00FF0000) >> 16; + bufferPos[2] = (value & 0x0000FF00) >> 8; + bufferPos[3] = (value & 0x000000FF); +} From d5839e2dd52ca81e7d15051212f17b7eacce39be Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 16:02:09 -0400 Subject: [PATCH 14/19] optical flow sensor pipeline overhaul - all sources of optical flow publish sensor_optical_flow - sensor_optical_flow is aggregated by the sensors module, aligned with integrated gyro, and published as vehicle_optical_flow Co-authored-by: alexklimaj --- .../init.d-posix/airframes/1010_iris_opt_flow | 4 + .../airframes/1017_iris_opt_flow_mockup | 1 - .../init.d/airframes/4016_holybro_px4vision | 1 - .../init.d/airframes/4061_atl_mantis_edu | 2 +- msg/CMakeLists.txt | 27 +- msg/estimator_optical_flow_vel.msg | 8 - msg/optical_flow.msg | 29 -- msg/sensor_optical_flow.msg | 30 ++ msg/tools/urtps_bridge_topics.yaml | 2 +- msg/vehicle_optical_flow.msg | 21 + msg/vehicle_optical_flow_vel.msg | 13 + src/drivers/drv_sensor.h | 13 +- src/drivers/optical_flow/paa3905/PAA3905.cpp | 27 +- src/drivers/optical_flow/paa3905/PAA3905.hpp | 4 +- src/drivers/optical_flow/paw3902/PAW3902.cpp | 27 +- src/drivers/optical_flow/paw3902/PAW3902.hpp | 4 +- src/drivers/optical_flow/pmw3901/PMW3901.cpp | 37 +- src/drivers/optical_flow/pmw3901/PMW3901.hpp | 7 +- src/drivers/optical_flow/px4flow/px4flow.cpp | 159 ++---- .../optical_flow/thoneflow/thoneflow.cpp | 127 ++--- .../thoneflow/thoneflow_parser.cpp | 6 +- .../optical_flow/thoneflow/thoneflow_parser.h | 4 +- src/drivers/uavcan/sensors/flow.cpp | 44 +- src/drivers/uavcan/sensors/flow.hpp | 2 +- .../uavcannode/Publishers/FlowMeasurement.hpp | 27 +- src/drivers/uavcannode/uavcannode_params.c | 21 - src/modules/ekf2/EKF/ekf.h | 5 +- src/modules/ekf2/EKF2.cpp | 44 +- src/modules/ekf2/EKF2.hpp | 12 +- .../BlockLocalPositionEstimator.hpp | 4 +- .../local_position_estimator/sensors/flow.cpp | 12 +- src/modules/logger/logged_topics.cpp | 5 +- src/modules/mavlink/mavlink_receiver.cpp | 183 +++---- src/modules/mavlink/mavlink_receiver.h | 19 +- .../mavlink/streams/OPTICAL_FLOW_RAD.hpp | 40 +- src/modules/replay/ReplayEkf2.cpp | 6 +- src/modules/sensors/CMakeLists.txt | 11 + .../sensors/{vehicle_imu => }/Integrator.hpp | 11 +- src/modules/sensors/Kconfig | 4 + src/modules/sensors/sensor_params_flow.c | 35 +- src/modules/sensors/sensors.cpp | 59 +++ .../sensors/vehicle_imu/CMakeLists.txt | 1 - .../sensors/vehicle_imu/VehicleIMU.hpp | 6 +- .../vehicle_optical_flow}/CMakeLists.txt | 11 +- .../vehicle_optical_flow/RingBuffer.hpp | 217 ++++++++ .../VehicleOpticalFlow.cpp | 481 ++++++++++++++++++ .../VehicleOpticalFlow.hpp | 147 ++++++ src/modules/simulator/simulator.h | 5 +- src/modules/simulator/simulator_mavlink.cpp | 84 ++- 49 files changed, 1415 insertions(+), 634 deletions(-) delete mode 100644 msg/estimator_optical_flow_vel.msg delete mode 100644 msg/optical_flow.msg create mode 100644 msg/sensor_optical_flow.msg create mode 100644 msg/vehicle_optical_flow.msg create mode 100644 msg/vehicle_optical_flow_vel.msg rename src/modules/sensors/{vehicle_imu => }/Integrator.hpp (96%) rename src/{drivers/optical_flow => modules/sensors/vehicle_optical_flow}/CMakeLists.txt (88%) create mode 100644 src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp create mode 100644 src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp create mode 100644 src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow index f3fe604155..6003d04d16 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_iris_opt_flow @@ -18,3 +18,7 @@ param set-default LPE_FAKE_ORIGIN 1 param set-default MPC_ALT_MODE 2 +param set-default SENS_FLOW_ROT 6 +param set-default SENS_FLOW_MINHGT 0.7 +param set-default SENS_FLOW_MAXHGT 3.0 +param set-default SENS_FLOW_MAXR 2.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup b/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup index 01246495cb..d2c0dcf4cb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1017_iris_opt_flow_mockup @@ -9,7 +9,6 @@ # EKF2 param set-default EKF2_AID_MASK 2 -param set-default SENS_FLOW_ROT 0 # LPE: Flow-only mode param set-default LPE_FUSION 242 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index cf15f0cbca..feb06fc88a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -104,7 +104,6 @@ param set-default SDLOG_PROFILE 131 param set-default SENS_CM8JL65_CFG 104 param set-default SENS_FLOW_MAXHGT 25 param set-default SENS_FLOW_MINHGT 0.5 -param set-default SENS_FLOW_ROT 0 param set-default IMU_GYRO_CUTOFF 100 param set-default SENS_EN_PMW3901 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index 56812f15d0..2dacd54acb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -174,7 +174,7 @@ param set-default RC1_TRIM 1000 param set-default SENS_FLOW_MAXR 7.4 param set-default SENS_FLOW_MINHGT 0.15 param set-default SENS_FLOW_MAXHGT 5.0 -param set-default SENS_FLOW_ROT 0 +param set-default SENS_FLOW_ROT 0 # ignore the SD card errors and use normal startup sound set STARTUP_TUNE "1" diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2ae9a92335..fcd004f85f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -65,26 +65,25 @@ set(msg_files differential_pressure.msg distance_sensor.msg ekf2_timestamps.msg - estimator_gps_status.msg esc_report.msg esc_status.msg + estimator_aid_source_1d.msg + estimator_aid_source_2d.msg + estimator_aid_source_3d.msg estimator_baro_bias.msg estimator_event_flags.msg + estimator_gps_status.msg estimator_innovations.msg - estimator_optical_flow_vel.msg estimator_selector_status.msg estimator_sensor_bias.msg estimator_states.msg estimator_status.msg estimator_status_flags.msg - estimator_aid_source_1d.msg - estimator_aid_source_2d.msg - estimator_aid_source_3d.msg event.msg - follow_target_status.msg + failure_detector_status.msg follow_target.msg follow_target_estimator.msg - failure_detector_status.msg + follow_target_status.msg generator_status.msg geofence_result.msg gimbal_device_attitude_status.msg @@ -99,8 +98,8 @@ set(msg_files heater_status.msg home_position.msg hover_thrust_estimate.msg - internal_combustion_engine_status.msg input_rc.msg + internal_combustion_engine_status.msg iridiumsbd_status.msg irlock_report.msg landing_gear.msg @@ -123,17 +122,16 @@ set(msg_files obstacle_distance.msg offboard_control_mode.msg onboard_computer_status.msg - optical_flow.msg orbit_status.msg parameter_update.msg ping.msg - pps_capture.msg position_controller_landing_status.msg position_controller_status.msg position_setpoint.msg position_setpoint_triplet.msg power_button_state.msg power_monitor.msg + pps_capture.msg pwm_input.msg px4io_status.msg radio_status.msg @@ -148,17 +146,18 @@ set(msg_files sensor_baro.msg sensor_combined.msg sensor_correction.msg - sensor_gps.msg sensor_gnss_relative.msg + sensor_gps.msg sensor_gyro.msg sensor_gyro_fft.msg sensor_gyro_fifo.msg sensor_hygrometer.msg sensor_mag.msg + sensor_optical_flow.msg sensor_preflight_mag.msg sensor_selection.msg - sensors_status_imu.msg sensors_status.msg + sensors_status_imu.msg system_power.msg takeoff_status.msg task_stack_info.msg @@ -176,8 +175,8 @@ set(msg_files uavcan_parameter_value.msg ulog_stream.msg ulog_stream_ack.msg - uwb_grid.msg uwb_distance.msg + uwb_grid.msg vehicle_acceleration.msg vehicle_air_data.msg vehicle_angular_acceleration.msg @@ -198,6 +197,8 @@ set(msg_files vehicle_local_position_setpoint.msg vehicle_magnetometer.msg vehicle_odometry.msg + vehicle_optical_flow.msg + vehicle_optical_flow_vel.msg vehicle_rates_setpoint.msg vehicle_roi.msg vehicle_status.msg diff --git a/msg/estimator_optical_flow_vel.msg b/msg/estimator_optical_flow_vel.msg deleted file mode 100644 index 38ee0d7f68..0000000000 --- a/msg/estimator_optical_flow_vel.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) - -float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) -float32[2] vel_ne # same as vel_body but in local frame (m/s) -float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad) -float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad) -float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg deleted file mode 100644 index 4b3796ff9f..0000000000 --- a/msg/optical_flow.msg +++ /dev/null @@ -1,29 +0,0 @@ -# Optical flow in XYZ body frame in SI units. -# http://en.wikipedia.org/wiki/International_System_of_Units - -uint64 timestamp # time since system start (microseconds) - -uint8 sensor_id # id of the sensor emitting the flow value -float32 pixel_flow_x_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the X body axis -float32 pixel_flow_y_integral # accumulated optical flow in radians where a positive value is produced by a RH rotation about the Y body axis -float32 gyro_x_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the X body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 gyro_y_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Y body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 gyro_z_rate_integral # accumulated gyro value in radians where a positive value is produced by a RH rotation about the Z body axis. Set to NaN if flow sensor does not have 3-axis gyro data. -float32 ground_distance_m # Altitude / distance to ground in meters -uint32 integration_timespan # accumulation timespan in microseconds -uint32 time_since_last_sonar_update # time since last sonar update in microseconds -uint16 frame_count_since_last_readout # number of accumulated frames in timespan -int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius -uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality - -float32 max_flow_rate # Magnitude of maximum angular which the optical flow sensor can measure reliably -float32 min_ground_distance # Minimum distance from ground at which the optical flow sensor operates reliably -float32 max_ground_distance # Maximum distance from ground at which the optical flow sensor operates reliably - - -uint8 MODE_UNKNOWN = 0 -uint8 MODE_BRIGHT = 1 -uint8 MODE_LOWLIGHT = 2 -uint8 MODE_SUPER_LOWLIGHT = 3 - -uint8 mode diff --git a/msg/sensor_optical_flow.msg b/msg/sensor_optical_flow.msg new file mode 100644 index 0000000000..ce7e8bf08c --- /dev/null +++ b/msg/sensor_optical_flow.msg @@ -0,0 +1,30 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data. +bool delta_angle_available + +float32 distance_m # (meters) Distance to the center of the flow field +bool distance_available + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # quality, 0: bad quality, 255: maximum quality + +uint32 error_count + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably + +uint8 MODE_UNKNOWN = 0 +uint8 MODE_BRIGHT = 1 +uint8 MODE_LOWLIGHT = 2 +uint8 MODE_SUPER_LOWLIGHT = 3 + +uint8 mode diff --git a/msg/tools/urtps_bridge_topics.yaml b/msg/tools/urtps_bridge_topics.yaml index 6f6a538a7c..529811ce5b 100644 --- a/msg/tools/urtps_bridge_topics.yaml +++ b/msg/tools/urtps_bridge_topics.yaml @@ -40,7 +40,7 @@ rtps: receive: true - msg: offboard_control_mode receive: true - - msg: optical_flow + - msg: sensor_optical_flow receive: true - msg: position_setpoint receive: true diff --git a/msg/vehicle_optical_flow.msg b/msg/vehicle_optical_flow.msg new file mode 100644 index 0000000000..13bdb57bbf --- /dev/null +++ b/msg/vehicle_optical_flow.msg @@ -0,0 +1,21 @@ +# Optical flow in XYZ body frame in SI units. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable) + +float32 distance_m # (meters) Distance to the center of the flow field (NAN if unavailable) + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably diff --git a/msg/vehicle_optical_flow_vel.msg b/msg/vehicle_optical_flow_vel.msg new file mode 100644 index 0000000000..c89207f4ab --- /dev/null +++ b/msg/vehicle_optical_flow_vel.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) +float32[2] vel_ne # same as vel_body but in local frame (m/s) + +float32[2] flow_uncompensated_integral # integrated optical flow measurement (rad) +float32[2] flow_compensated_integral # integrated optical flow measurement compensated for angular motion (rad) + +float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) +float32[3] gyro_rate_integral # gyro measurement integrated to flow rate and synchronized with flow measurements (rad) + +# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 4c9b1ab90d..4547f28cec 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -68,6 +68,7 @@ #define DRV_IMU_DEVTYPE_SIM 0x14 #define DRV_DIFF_PRESS_DEVTYPE_SIM 0x15 +#define DRV_FLOW_DEVTYPE_SIM 0x16 #define DRV_IMU_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 @@ -136,10 +137,6 @@ #define DRV_PWM_DEVTYPE_PCA9685 0x69 #define DRV_ACC_DEVTYPE_BMI088 0x6a #define DRV_OSD_DEVTYPE_ATXXXX 0x6b -#define DRV_FLOW_DEVTYPE_PMW3901 0x6c -#define DRV_FLOW_DEVTYPE_PAW3902 0x6d -#define DRV_FLOW_DEVTYPE_PX4FLOW 0x6e -#define DRV_FLOW_DEVTYPE_PAA3905 0x6f #define DRV_DIST_DEVTYPE_LL40LS 0x70 @@ -206,9 +203,15 @@ #define DRV_GPS_DEVTYPE_SIM 0xAF +#define DRV_TRNS_DEVTYPE_MXS 0xB0 + #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1 -#define DRV_TRNS_DEVTYPE_MXS 0xB2 +#define DRV_FLOW_DEVTYPE_MAVLINK 0xB2 +#define DRV_FLOW_DEVTYPE_PMW3901 0xB3 +#define DRV_FLOW_DEVTYPE_PAW3902 0xB4 +#define DRV_FLOW_DEVTYPE_PX4FLOW 0xB5 +#define DRV_FLOW_DEVTYPE_PAA3905 0xB6 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index a9a306d803..b6457946a0 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -535,27 +535,26 @@ void PAA3905::RunImpl() return; } - optical_flow_s report{}; - report.timestamp = timestamp_sample; - //report.device_id = get_device_id(); + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow_x_integral = pixel_flow_rotated(0); - report.pixel_flow_y_integral = pixel_flow_rotated(1); + report.pixel_flow[0] = pixel_flow_rotated(0); + report.pixel_flow[1] = pixel_flow_rotated(1); - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + report.integration_timespan_us = _flow_dt_sum_usec; // microseconds report.quality = _flow_quality_sum / _flow_sample_counter; // No gyro on this board - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + report.delta_angle[0] = NAN; + report.delta_angle[1] = NAN; + report.delta_angle[2] = NAN; // set (conservative) specs according to datasheet report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s @@ -565,20 +564,20 @@ void PAA3905::RunImpl() switch (_mode) { case Mode::Bright: - report.mode = optical_flow_s::MODE_BRIGHT; + report.mode = sensor_optical_flow_s::MODE_BRIGHT; break; case Mode::LowLight: - report.mode = optical_flow_s::MODE_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; break; case Mode::SuperLowLight: - report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; break; } report.timestamp = hrt_absolute_time(); - _optical_flow_pub.publish(report); + _sensor_optical_flow_pub.publish(report); if (report.quality > 10) { _last_good_publish = report.timestamp; diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index dd15f548aa..8235e923d3 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include using namespace time_literals; using namespace PixArt_PAA3905; @@ -99,7 +99,7 @@ private: void ResetAccumulatedData(); - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index ea3b66d7ae..3df138a14c 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -788,27 +788,26 @@ void PAW3902::RunImpl() return; } - optical_flow_s report{}; - report.timestamp = timestamp_sample; - //report.device_id = get_device_id(); + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow_x_integral = pixel_flow_rotated(0); - report.pixel_flow_y_integral = pixel_flow_rotated(1); + report.pixel_flow[0] = pixel_flow_rotated(0); + report.pixel_flow[1] = pixel_flow_rotated(1); - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + report.integration_timespan_us = _flow_dt_sum_usec; // microseconds report.quality = _flow_quality_sum / _flow_sample_counter; // No gyro on this board - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + report.delta_angle[0] = NAN; + report.delta_angle[1] = NAN; + report.delta_angle[2] = NAN; // set (conservative) specs according to datasheet report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s @@ -818,20 +817,20 @@ void PAW3902::RunImpl() switch (_mode) { case Mode::Bright: - report.mode = optical_flow_s::MODE_BRIGHT; + report.mode = sensor_optical_flow_s::MODE_BRIGHT; break; case Mode::LowLight: - report.mode = optical_flow_s::MODE_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; break; case Mode::SuperLowLight: - report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT; + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; break; } report.timestamp = hrt_absolute_time(); - _optical_flow_pub.publish(report); + _sensor_optical_flow_pub.publish(report); if (report.quality > 10) { _last_good_publish = report.timestamp; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index ab8d574f3d..d6ea4ff436 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include using namespace time_literals; using namespace PixArt_PAW3902JF; @@ -97,7 +97,7 @@ private: void ResetAccumulatedData(); - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp index 78809cf554..7c5506c81d 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp @@ -207,16 +207,6 @@ PMW3901::sensorInit() int PMW3901::init() { - // get yaw rotation from sensor frame to body frame - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 0; - param_get(rot, &val); - - _yaw_rotation = (enum Rotation)val; - } - /* For devices competing with NuttX SPI drivers on a bus (Crazyflie SD Card expansion board) */ SPI::set_lockmode(LOCK_THREADS); @@ -326,28 +316,24 @@ PMW3901::RunImpl() delta_x = (float)_flow_sum_x / 385.0f; // proportional factor + convert from pixels to radians delta_y = (float)_flow_sum_y / 385.0f; // proportional factor + convert from pixels to radians - optical_flow_s report{}; - report.timestamp = timestamp; + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp; - report.pixel_flow_x_integral = static_cast(delta_x); - report.pixel_flow_y_integral = static_cast(delta_y); + report.pixel_flow[0] = static_cast(delta_x); + report.pixel_flow[1] = static_cast(delta_y); - // rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT + // rotate measurements in yaw from sensor frame to body frame float zeroval = 0.0f; - rotate_3f(_yaw_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); - rotate_3f(_yaw_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); + rotate_3f(_yaw_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval); - report.frame_count_since_last_readout = _flow_sample_counter; // number of frames - report.integration_timespan = _flow_dt_sum_usec; // microseconds + report.integration_timespan_us = _flow_dt_sum_usec; // microseconds - report.sensor_id = 0; report.quality = _flow_sample_counter > 0 ? _flow_quality_sum / _flow_sample_counter : 0; - /* No gyro on this board */ - report.gyro_x_rate_integral = NAN; - report.gyro_y_rate_integral = NAN; - report.gyro_z_rate_integral = NAN; + report.delta_angle[0] = NAN; + report.delta_angle[1] = NAN; + report.delta_angle[2] = NAN; // set (conservative) specs according to datasheet report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s @@ -360,7 +346,8 @@ PMW3901::RunImpl() _flow_sample_counter = 0; _flow_quality_sum = 0; - _optical_flow_pub.publish(report); + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); perf_end(_sample_perf); } diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp index 13d850a66b..2c845fbdcb 100644 --- a/src/drivers/optical_flow/pmw3901/PMW3901.hpp +++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp @@ -45,10 +45,9 @@ #include #include #include -#include #include #include -#include +#include #include /* Configuration Constants */ @@ -84,14 +83,14 @@ private: const uint64_t _collect_time{15000}; // usecs, ensures flow data is published every second iteration of Run() (100Hz -> 50Hz) - uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; uint64_t _previous_collect_timestamp{0}; - enum Rotation _yaw_rotation; + enum Rotation _yaw_rotation {}; int _flow_sum_x{0}; int _flow_sum_y{0}; diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index c1c59ed7a9..5002a5106d 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/src/drivers/optical_flow/px4flow/px4flow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 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 @@ -41,8 +41,6 @@ #include #include -#include -#include #include #include #include @@ -50,8 +48,7 @@ #include #include #include -#include -#include +#include /* Configuration Constants */ #define I2C_FLOW_ADDRESS_DEFAULT 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 @@ -89,28 +86,19 @@ public: * and start a new one. */ void RunImpl(); -protected: - int probe() override; private: + int probe() override; - uint8_t _sonar_rotation; bool _sensor_ok{false}; bool _collect_phase{false}; - uORB::PublicationMulti _px4flow_topic{ORB_ID(optical_flow)}; - uORB::PublicationMulti _distance_sensor_topic{ORB_ID(distance_sensor)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _comms_errors; - enum Rotation _sensor_rotation; - float _sensor_min_range{0.0f}; - float _sensor_max_range{0.0f}; - float _sensor_max_flow_rate{0.0f}; - i2c_frame _frame; - i2c_integral_frame _frame_integral; /** * Test whether the device supported by the driver is present at a @@ -134,15 +122,11 @@ private: }; -extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); - PX4FLOW::PX4FLOW(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), - _sonar_rotation(config.rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")), - _sensor_rotation(Rotation::ROTATION_NONE) + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")) { } @@ -166,44 +150,6 @@ PX4FLOW::init() /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; - /* get yaw rotation from sensor frame to body frame */ - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 6; // the recommended installation for the flow sensor is with the Y sensor axis forward - param_get(rot, &val); - - _sensor_rotation = (enum Rotation)val; - } - - /* get operational limits of the sensor */ - param_t hmin = param_find("SENS_FLOW_MINHGT"); - - if (hmin != PARAM_INVALID) { - float val = 0.7; - param_get(hmin, &val); - - _sensor_min_range = val; - } - - param_t hmax = param_find("SENS_FLOW_MAXHGT"); - - if (hmax != PARAM_INVALID) { - float val = 3.0; - param_get(hmax, &val); - - _sensor_max_range = val; - } - - param_t ratemax = param_find("SENS_FLOW_MAXR"); - - if (ratemax != PARAM_INVALID) { - float val = 2.5; - param_get(ratemax, &val); - - _sensor_max_flow_rate = val; - } - start(); return ret; @@ -269,6 +215,8 @@ PX4FLOW::collect() return ret; } + i2c_integral_frame _frame_integral{}; + if (PX4FLOW_REG == 0) { memcpy(&_frame, val, I2C_FRAME_SIZE); memcpy(&_frame_integral, &(val[I2C_FRAME_SIZE]), I2C_INTEGRAL_FRAME_SIZE); @@ -279,52 +227,37 @@ PX4FLOW::collect() } - optical_flow_s report{}; + DeviceId device_id; + device_id.devid = get_device_id(); + device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW; + device_id.devid_s.address = get_i2c_address(); + + sensor_optical_flow_s report{}; + + report.timestamp_sample = hrt_absolute_time(); + report.device_id = device_id.devid; + + report.pixel_flow[0] = static_cast(_frame_integral.pixel_flow_x_integral) / 10000.f; //convert to radians + report.pixel_flow[1] = static_cast(_frame_integral.pixel_flow_y_integral) / 10000.f; //convert to radians + + report.delta_angle_available = true; + report.delta_angle[0] = static_cast(_frame_integral.gyro_x_rate_integral) / 10000.0f; // convert to radians + report.delta_angle[1] = static_cast(_frame_integral.gyro_y_rate_integral) / 10000.0f; // convert to radians + report.delta_angle[2] = static_cast(_frame_integral.gyro_z_rate_integral) / 10000.0f; // convert to radians + + report.distance_m = static_cast(_frame_integral.ground_distance) / 1000.f; // convert to meters + report.distance_available = true; + + report.integration_timespan_us = _frame_integral.integration_timespan; // microseconds + + report.quality = _frame_integral.qual; // 0:bad ; 255 max quality + + report.max_flow_rate = 2.5f; + report.min_ground_distance = PX4FLOW_MIN_DISTANCE; + report.max_ground_distance = PX4FLOW_MAX_DISTANCE; report.timestamp = hrt_absolute_time(); - report.pixel_flow_x_integral = static_cast(_frame_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians - report.pixel_flow_y_integral = static_cast(_frame_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians - report.frame_count_since_last_readout = _frame_integral.frame_count_since_last_readout; - report.ground_distance_m = static_cast(_frame_integral.ground_distance) / 1000.0f;//convert to meters - report.quality = _frame_integral.qual; //0:bad ; 255 max quality - report.gyro_x_rate_integral = static_cast(_frame_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians - report.gyro_y_rate_integral = static_cast(_frame_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians - report.gyro_z_rate_integral = static_cast(_frame_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians - report.integration_timespan = _frame_integral.integration_timespan; //microseconds - report.time_since_last_sonar_update = _frame_integral.sonar_timestamp;//microseconds - report.gyro_temperature = _frame_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius - report.sensor_id = 0; - report.max_flow_rate = _sensor_max_flow_rate; - report.min_ground_distance = _sensor_min_range; - report.max_ground_distance = _sensor_max_range; - - /* rotate measurements in yaw from sensor frame to body frame according to parameter SENS_FLOW_ROT */ - float zeroval = 0.0f; - - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); - rotate_3f(_sensor_rotation, report.gyro_x_rate_integral, report.gyro_y_rate_integral, report.gyro_z_rate_integral); - - _px4flow_topic.publish(report); - - /* publish to the distance_sensor topic as well */ - if (_distance_sensor_topic.get_instance() == 0) { - distance_sensor_s distance_report{}; - DeviceId device_id; - device_id.devid = get_device_id(); - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW; - - distance_report.timestamp = report.timestamp; - distance_report.min_distance = PX4FLOW_MIN_DISTANCE; - distance_report.max_distance = PX4FLOW_MAX_DISTANCE; - distance_report.current_distance = report.ground_distance_m; - distance_report.variance = 0.0f; - distance_report.signal_quality = -1; - distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - distance_report.device_id = device_id.devid; - distance_report.orientation = _sonar_rotation; - - _distance_sensor_topic.publish(distance_report); - } + _sensor_optical_flow_pub.publish(report); perf_end(_sample_perf); @@ -374,28 +307,16 @@ PX4FLOW::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x42); - PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 35, "Rotation (default=downwards)", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -px4flow_main(int argc, char *argv[]) +extern "C" __EXPORT int px4flow_main(int argc, char *argv[]) { - int ch; using ThisDriver = PX4FLOW; BusCLIArguments cli{true, false}; cli.default_i2c_frequency = PX4FLOW_I2C_MAX_BUS_SPEED; - cli.rotation = (Rotation)distance_sensor_s::ROTATION_DOWNWARD_FACING; cli.i2c_address = I2C_FLOW_ADDRESS_DEFAULT; - while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { - switch (ch) { - case 'R': - cli.rotation = (Rotation)atoi(cli.optArg()); - break; - } - } - const char *verb = cli.optArg(); if (!verb) { @@ -415,13 +336,11 @@ px4flow_main(int argc, char *argv[]) } return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } diff --git a/src/drivers/optical_flow/thoneflow/thoneflow.cpp b/src/drivers/optical_flow/thoneflow/thoneflow.cpp index 743e809baf..91734a26a3 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 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 @@ -50,10 +50,10 @@ #include #include #include -#include -#include -#include -#include +#include + +#include +#include #include "thoneflow_parser.h" @@ -71,21 +71,20 @@ public: void print_info(); private: - char _port[20]; - Rotation _rotation; - int _cycle_interval; - int _fd; - char _linebuf[5]; - unsigned _linebuf_index; - THONEFLOW_PARSE_STATE _parse_state; + char _port[20] {}; + Rotation _rotation{ROTATION_NONE}; + int _cycle_interval{10526}; + int _fd{-1}; + char _linebuf[5] {}; + unsigned _linebuf_index{0}; + THONEFLOW_PARSE_STATE _parse_state{THONEFLOW_PARSE_STATE0_UNSYNC}; - hrt_abstime _last_read; + hrt_abstime _last_read{0}; - optical_flow_s _report; - orb_advert_t _optical_flow_pub; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; /** * Initialise the automatic measurement state machine and start it. @@ -106,23 +105,8 @@ private: }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]); - Thoneflow::Thoneflow(const char *port) : - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), - _rotation(Rotation(0)), - _cycle_interval(10526), - _fd(-1), - _linebuf_index(0), - _parse_state(THONEFLOW_PARSE_STATE0_UNSYNC), - _last_read(0), - _report(), - _optical_flow_pub(nullptr), - _sample_perf(perf_alloc(PC_ELAPSED, "thoneflow_read")), - _comms_errors(perf_alloc(PC_COUNT, "thoneflow_com_err")) + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) { /* store port name */ strncpy(_port, port, sizeof(_port) - 1); @@ -206,41 +190,6 @@ Thoneflow::init() ret = PX4_ERROR; break; } - - /* get yaw rotation from sensor frame to body frame */ - param_t rot = param_find("SENS_FLOW_ROT"); - - if (rot != PARAM_INVALID) { - int32_t val = 0; - param_get(rot, &val); - - _rotation = Rotation(val); - } - - /* Initialise report structure */ - /* No gyro on this board */ - _report.gyro_x_rate_integral = NAN; - _report.gyro_y_rate_integral = NAN; - _report.gyro_z_rate_integral = NAN; - - /* Conservative specs according to datasheet */ - _report.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s - _report.min_ground_distance = 0.1f; // Datasheet: 80mm - _report.max_ground_distance = 30.0f; // Datasheet: infinity - - /* Integrated flow is sent at 66fps */ - _report.frame_count_since_last_readout = 1; - _report.integration_timespan = 10526; // microseconds - - /* Get a publish handle on the optical flow topic */ - _optical_flow_pub = orb_advertise(ORB_ID(optical_flow), &_report); - - if (_optical_flow_pub == nullptr) { - PX4_ERR("Failed to create optical_flow object"); - ret = PX4_ERROR; - break; - } - } while (0); /* Close the fd */ @@ -299,20 +248,33 @@ Thoneflow::collect() _last_read = hrt_absolute_time(); + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = hrt_absolute_time(); + /* Parse each byte of read buffer */ for (int i = 0; i < ret; i++) { - valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &_report); + valid |= thoneflow_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &report); } /* Publish most recent valid measurement */ if (valid) { - _report.timestamp = hrt_absolute_time(); + + report.device_id = 0; // TODO get_device_id(); + report.integration_timespan_us = 10526; // microseconds /* Rotate measurements from sensor frame to body frame */ float zeroval = 0.0f; - rotate_3f(_rotation, _report.pixel_flow_x_integral, _report.pixel_flow_y_integral, zeroval); + rotate_3f(_rotation, report.pixel_flow[0], report.pixel_flow[1], zeroval); - orb_publish(ORB_ID(optical_flow), _optical_flow_pub, &_report); + // Conservative specs according to datasheet + report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = INFINITY; // Datasheet: infinity + + report.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(report); } /* Bytes left to parse */ @@ -478,16 +440,15 @@ $ thoneflow stop PRINT_MODULE_USAGE_NAME("thoneflow", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("optical_flow"); - PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); - PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); - PRINT_MODULE_USAGE_COMMAND_DESCR("info","Print driver information"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("info", "Print driver information"); } } // namespace -int -thoneflow_main(int argc, char *argv[]) +extern "C" __EXPORT int thoneflow_main(int argc, char *argv[]) { int ch; const char *device_path = ""; @@ -522,19 +483,11 @@ thoneflow_main(int argc, char *argv[]) thoneflow::usage(); return -1; } - } - /* - * Stop the driver - */ - if (!strcmp(argv[myoptind], "stop")) { + } else if (!strcmp(argv[myoptind], "stop")) { return thoneflow::stop(); - } - /* - * Print driver information. - */ - if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { + } else if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) { thoneflow::info(); return 0; } diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp index 28cc5a5693..3534af002a 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.cpp @@ -61,7 +61,7 @@ const char *parser_state[] = { #endif bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *flow) + sensor_optical_flow_s *flow) { bool parsed_packet = false; @@ -133,8 +133,8 @@ bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum TH // Checksum valid, populate sensor report int16_t delta_x = uint16_t(parserbuf[1]) << 8 | parserbuf[0]; int16_t delta_y = uint16_t(parserbuf[3]) << 8 | parserbuf[2]; - flow->pixel_flow_x_integral = static_cast(delta_x) * (3.52e-3f); - flow->pixel_flow_y_integral = static_cast(delta_y) * (3.52e-3f); + flow->pixel_flow[0] = static_cast(delta_x) * (3.52e-3f); + flow->pixel_flow[1] = static_cast(delta_y) * (3.52e-3f); *state = THONEFLOW_PARSE_STATE7_CHECKSUM; } else { diff --git a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h index fd27b8c355..d63906fdc6 100644 --- a/src/drivers/optical_flow/thoneflow/thoneflow_parser.h +++ b/src/drivers/optical_flow/thoneflow/thoneflow_parser.h @@ -33,7 +33,7 @@ #pragma once -#include +#include // Data Format for ThoneFlow 3901U // =============================== @@ -62,4 +62,4 @@ enum THONEFLOW_PARSE_STATE { }; bool thoneflow_parse(char c, char *parserbuf, unsigned *parserbuf_index, enum THONEFLOW_PARSE_STATE *state, - optical_flow_s *report); + sensor_optical_flow_s *report); diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index f9cef4c551..099de615f0 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -38,7 +38,7 @@ const char *const UavcanFlowBridge::NAME = "flow"; UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node) : - UavcanSensorBridgeBase("uavcan_flow", ORB_ID(optical_flow)), + UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow)), _sub_flow(node) { } @@ -58,23 +58,41 @@ UavcanFlowBridge::init() void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure &msg) { - optical_flow_s flow{}; + sensor_optical_flow_s flow{}; + flow.timestamp_sample = hrt_absolute_time(); // TODO - // We're only given an 8 bit field for sensor ID; just use the UAVCAN node ID - flow.sensor_id = msg.getSrcNodeID().get(); + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN; + device_id.devid_s.bus = 0; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_UAVCAN; + device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; - flow.timestamp = hrt_absolute_time(); - flow.integration_timespan = 1.e6f * msg.integration_interval; // s -> micros - flow.pixel_flow_x_integral = msg.flow_integral[0]; - flow.pixel_flow_y_integral = msg.flow_integral[1]; + flow.device_id = device_id.devid; - flow.gyro_x_rate_integral = msg.rate_gyro_integral[0]; - flow.gyro_y_rate_integral = msg.rate_gyro_integral[1]; + flow.pixel_flow[0] = msg.flow_integral[0]; + flow.pixel_flow[1] = msg.flow_integral[1]; + + flow.integration_timespan_us = 1.e6f * msg.integration_interval; // s -> us flow.quality = msg.quality; - flow.max_flow_rate = 5.0f; // Datasheet: 7.4 rad/s - flow.min_ground_distance = 0.1f; // Datasheet: 80mm - flow.max_ground_distance = 30.0f; // Datasheet: infinity + + if (PX4_ISFINITE(msg.rate_gyro_integral[0]) && PX4_ISFINITE(msg.rate_gyro_integral[1])) { + flow.delta_angle[0] = msg.rate_gyro_integral[0]; + flow.delta_angle[1] = msg.rate_gyro_integral[1]; + flow.delta_angle[2] = 0.f; + flow.delta_angle_available = true; + + } else { + flow.delta_angle[0] = NAN; + flow.delta_angle[1] = NAN; + flow.delta_angle[2] = NAN; + } + + flow.max_flow_rate = NAN; + flow.min_ground_distance = NAN; + flow.max_ground_distance = NAN; + + flow.timestamp = hrt_absolute_time(); publish(msg.getSrcNodeID().get(), &flow); } diff --git a/src/drivers/uavcan/sensors/flow.hpp b/src/drivers/uavcan/sensors/flow.hpp index 94aec78a0e..3a5484269e 100644 --- a/src/drivers/uavcan/sensors/flow.hpp +++ b/src/drivers/uavcan/sensors/flow.hpp @@ -37,7 +37,7 @@ #include -#include +#include #include diff --git a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp index 7780bd85f0..de1b550afb 100644 --- a/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/FlowMeasurement.hpp @@ -39,7 +39,7 @@ #include #include -#include +#include namespace uavcannode { @@ -52,18 +52,9 @@ class FlowMeasurement : public: FlowMeasurement(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(com::hex::equipment::flow::Measurement::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(optical_flow)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(vehicle_optical_flow)), uavcan::Publisher(node) { - _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, 0.f}}; - - param_t rot = param_find("CANNODE_FLOW_ROT"); - int32_t val = 0; - - if (param_get(rot, &val) == PX4_OK) { - _rotation = get_rot_matrix((enum Rotation)val); - } - this->setPriority(uavcan::TransferPriority::Default); } @@ -80,20 +71,18 @@ public: void BroadcastAnyUpdates() override { // optical_flow -> com::hex::equipment::flow::Measurement - optical_flow_s optical_flow; + vehicle_optical_flow_s optical_flow; if (uORB::SubscriptionCallbackWorkItem::update(&optical_flow)) { com::hex::equipment::flow::Measurement measurement{}; - measurement.integration_interval = optical_flow.integration_timespan * 1e-6f; // us -> s + measurement.integration_interval = optical_flow.integration_timespan_us * 1e-6f; // us -> s // rotate measurements in yaw from sensor frame to body frame - const matrix::Vector3f gyro_flow_rotated = _rotation * matrix::Vector3f{optical_flow.gyro_x_rate_integral, optical_flow.gyro_y_rate_integral, 0.f}; - const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{optical_flow.pixel_flow_x_integral, optical_flow.pixel_flow_y_integral, 0.f}; + measurement.rate_gyro_integral[0] = optical_flow.delta_angle[0]; + measurement.rate_gyro_integral[1] = optical_flow.delta_angle[1]; - measurement.rate_gyro_integral[0] = gyro_flow_rotated(0); - measurement.rate_gyro_integral[1] = gyro_flow_rotated(1); - measurement.flow_integral[0] = pixel_flow_rotated(0); - measurement.flow_integral[1] = pixel_flow_rotated(1); + measurement.flow_integral[0] = optical_flow.pixel_flow[0]; + measurement.flow_integral[1] = optical_flow.pixel_flow[1]; measurement.quality = optical_flow.quality; diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 709e333ee8..8e9a10e833 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/src/drivers/uavcannode/uavcannode_params.c @@ -60,27 +60,6 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000); */ PARAM_DEFINE_INT32(CANNODE_TERM, 0); -/** - * Cannode flow board rotation - * - * This parameter defines the yaw rotation of the Cannode flow board relative to the vehicle body frame. - * Zero rotation is defined as X on flow board pointing towards front of vehicle. - * - * @value 0 No rotation - * @value 1 Yaw 45° - * @value 2 Yaw 90° - * @value 3 Yaw 135° - * @value 4 Yaw 180° - * @value 5 Yaw 225° - * @value 6 Yaw 270° - * @value 7 Yaw 315° - * - * @reboot_required true - * - * @group UAVCAN - */ -PARAM_DEFINE_INT32(CANNODE_FLOW_ROT, 0); - /** * Enable RTCM pub/sub * diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b74517f6bb..66399646a3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -104,9 +104,12 @@ public: const Vector2f &getFlowVelBody() const { return _flow_vel_body; } const Vector2f &getFlowVelNE() const { return _flow_vel_ne; } + const Vector2f &getFlowCompensated() const { return _flow_compensated_XY_rad; } const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_xy_rad; } - const Vector3f &getFlowGyro() const { return _flow_sample_delayed.gyro_xyz; } + + const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } + const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } void getHeadingInnov(float &heading_innov) const { heading_innov = _heading_innov; } void getHeadingInnovVar(float &heading_innov_var) const { heading_innov_var = _heading_innov_var; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 9b3b40e3d2..349b71c5aa 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1426,14 +1426,18 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) { if (_ekf.getFlowCompensated().longerThan(0.f)) { - estimator_optical_flow_vel_s flow_vel{}; + vehicle_optical_flow_vel_s flow_vel{}; flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; _ekf.getFlowVelBody().copyTo(flow_vel.vel_body); _ekf.getFlowVelNE().copyTo(flow_vel.vel_ne); + _ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral); _ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral); - _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral); + + _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate); + _ekf.getFlowGyroIntegral().copyTo(flow_vel.gyro_rate_integral); + flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); @@ -1711,29 +1715,29 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF flow sample bool new_optical_flow = false; - const unsigned last_generation = _optical_flow_sub.get_last_generation(); - optical_flow_s optical_flow; + const unsigned last_generation = _vehicle_optical_flow_sub.get_last_generation(); + vehicle_optical_flow_s optical_flow; - if (_optical_flow_sub.update(&optical_flow)) { + if (_vehicle_optical_flow_sub.update(&optical_flow)) { if (_msg_missed_optical_flow_perf == nullptr) { _msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed"); - } else if (_optical_flow_sub.get_last_generation() != last_generation + 1) { + } else if (_vehicle_optical_flow_sub.get_last_generation() != last_generation + 1) { perf_count(_msg_missed_optical_flow_perf); } flowSample flow { - .time_us = optical_flow.timestamp, + .time_us = optical_flow.timestamp_sample, // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate // is produced by a RH rotation of the image about the sensor axis. - .flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral}, - .gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral}, - .dt = 1e-6f * (float)optical_flow.integration_timespan, + .flow_xy_rad = Vector2f{-optical_flow.pixel_flow[0], -optical_flow.pixel_flow[1]}, + .gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]}, + .dt = 1e-6f * (float)optical_flow.integration_timespan_us, .quality = optical_flow.quality, }; - if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) && - PX4_ISFINITE(optical_flow.pixel_flow_x_integral) && + if (PX4_ISFINITE(optical_flow.pixel_flow[0]) && + PX4_ISFINITE(optical_flow.pixel_flow[1]) && flow.dt < 1) { // Save sensor limits reported by the optical flow sensor @@ -1745,6 +1749,22 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) new_optical_flow = true; } + // use optical_flow distance as range sample if distance_sensor unavailable + if (PX4_ISFINITE(optical_flow.distance_m) && (hrt_elapsed_time(&_last_range_sensor_update) > 1_s)) { + + int8_t quality = static_cast(optical_flow.quality) / static_cast(UINT8_MAX) * 100.f; + + rangeSample range_sample { + .time_us = optical_flow.timestamp_sample, + .rng = optical_flow.distance_m, + .quality = quality, + }; + _ekf.setRangeData(range_sample); + + // set sensor limits + _ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance); + } + ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 9dff6dcf40..e279ff0b56 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -71,13 +71,11 @@ #include #include #include -#include #include #include #include #include #include -#include #include #include #include @@ -91,11 +89,12 @@ #include #include #include +#include +#include #include #include #include - extern pthread_mutex_t ekf2_module_mutex; class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem @@ -276,17 +275,18 @@ private: uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; - uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + hrt_abstime _last_range_sensor_update{0}; int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations unsigned _distance_sensor_last_generation{0}; @@ -295,8 +295,6 @@ private: hrt_abstime _last_event_flags_publish{0}; hrt_abstime _last_status_flags_publish{0}; - hrt_abstime _last_range_sensor_update{0}; - uint32_t _filter_control_status{0}; uint32_t _filter_fault_status{0}; uint32_t _innov_check_fail_status{0}; @@ -314,12 +312,12 @@ private: uORB::PublicationMulti _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::PublicationMulti _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::PublicationMulti _estimator_innovations_pub{ORB_ID(estimator_innovations)}; - uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _estimator_sensor_bias_pub{ORB_ID(estimator_sensor_bias)}; uORB::PublicationMulti _estimator_states_pub{ORB_ID(estimator_states)}; uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; + uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub{ORB_ID(estimator_aid_src_baro_hgt)}; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 196d6bcebb..12f169360a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -270,7 +270,7 @@ private: uORB::SubscriptionData _sub_land{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionData _sub_att{ORB_ID(vehicle_attitude)}; uORB::SubscriptionData _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)}; - uORB::SubscriptionData _sub_flow{ORB_ID(optical_flow)}; + uORB::SubscriptionData _sub_flow{ORB_ID(vehicle_optical_flow)}; uORB::SubscriptionData _sub_gps{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionData _sub_visual_odom{ORB_ID(vehicle_visual_odometry)}; uORB::SubscriptionData _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)}; diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index ff32dc5310..4213ecfd09 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -61,9 +61,9 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) // optical flow in x, y axis // TODO consider making flow scale a states of the kalman filter - float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _param_lpe_flw_scale.get(); - float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _param_lpe_flw_scale.get(); - float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f; + float flow_x_rad = _sub_flow.get().pixel_flow[0] * _param_lpe_flw_scale.get(); + float flow_y_rad = _sub_flow.get().pixel_flow[1] * _param_lpe_flw_scale.get(); + float dt_flow = _sub_flow.get().integration_timespan_us / 1.0e6f; if (dt_flow > 0.5f || dt_flow < 1.0e-6f) { return -1; @@ -74,10 +74,8 @@ int BlockLocalPositionEstimator::flowMeasure(Vector &y) float gyro_y_rad = 0; if (_param_lpe_fusion.get() & FUSE_FLOW_GYRO_COMP) { - gyro_x_rad = _flow_gyro_x_high_pass.update( - _sub_flow.get().gyro_x_rate_integral); - gyro_y_rad = _flow_gyro_y_high_pass.update( - _sub_flow.get().gyro_y_rate_integral); + gyro_x_rad = _flow_gyro_x_high_pass.update(_sub_flow.get().delta_angle[0]); + gyro_y_rad = _flow_gyro_y_high_pass.update(_sub_flow.get().delta_angle[1]); } //warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f", diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e827a43bbd..1d0d80d927 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -174,7 +174,6 @@ void LoggedTopics::add_default_topics() add_topic_multi("battery_status", 200, 2); add_topic_multi("differential_pressure", 1000, 2); add_topic_multi("distance_sensor", 1000, 2); - add_topic_multi("optical_flow", 1000, 1); add_optional_topic_multi("sensor_accel", 1000, 4); add_optional_topic_multi("sensor_baro", 1000, 4); add_topic_multi("sensor_gps", 1000, 2); @@ -182,9 +181,13 @@ void LoggedTopics::add_default_topics() add_optional_topic("pps_capture", 1000); add_optional_topic_multi("sensor_gyro", 1000, 4); add_optional_topic_multi("sensor_mag", 1000, 4); + add_optional_topic_multi("sensor_optical_flow", 1000, 2); + add_topic_multi("vehicle_imu", 500, 4); add_topic_multi("vehicle_imu_status", 1000, 4); add_optional_topic_multi("vehicle_magnetometer", 500, 4); + add_optional_topic("vehicle_optical_flow", 500); + //add_optional_topic("vehicle_optical_flow_vel", 100); // SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled int32_t sys_ctrl_alloc = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8fa244e799..196d991a6c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -89,12 +89,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _parameters_manager(parent), _mavlink_timesync(parent) { - _handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT"); - _handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR"); - _handle_sens_flow_minhgt = param_find("SENS_FLOW_MINHGT"); - _handle_sens_flow_rot = param_find("SENS_FLOW_ROT"); - _handle_ekf2_min_rng = param_find("EKF2_MIN_RNG"); - _handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX"); } void @@ -795,105 +789,93 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) void MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) { - /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); - optical_flow_s f{}; + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_MAVLINK; - f.timestamp = hrt_absolute_time(); - f.time_since_last_sonar_update = flow.time_delta_distance_us; - f.integration_timespan = flow.integration_time_us; - f.pixel_flow_x_integral = flow.integrated_x; - f.pixel_flow_y_integral = flow.integrated_y; - f.gyro_x_rate_integral = flow.integrated_xgyro; - f.gyro_y_rate_integral = flow.integrated_ygyro; - f.gyro_z_rate_integral = flow.integrated_zgyro; - f.gyro_temperature = flow.temperature; - f.ground_distance_m = flow.distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - f.max_flow_rate = _param_sens_flow_maxr; - f.min_ground_distance = _param_sens_flow_minhgt; - f.max_ground_distance = _param_sens_flow_maxhgt; + sensor_optical_flow_s sensor_optical_flow{}; - /* read flow sensor parameters */ - const Rotation flow_rot = (Rotation)_param_sens_flow_rot; + sensor_optical_flow.timestamp_sample = hrt_absolute_time(); + sensor_optical_flow.device_id = device_id.devid; - /* rotate measurements according to parameter */ - float zero_val = 0.0f; - rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zero_val); - rotate_3f(flow_rot, f.gyro_x_rate_integral, f.gyro_y_rate_integral, f.gyro_z_rate_integral); + sensor_optical_flow.pixel_flow[0] = flow.integrated_x; + sensor_optical_flow.pixel_flow[1] = flow.integrated_y; - _flow_pub.publish(f); + sensor_optical_flow.integration_timespan_us = flow.integration_time_us; + sensor_optical_flow.quality = flow.quality; - /* Use distance value for distance sensor topic */ - if (flow.distance > 0.0f) { // negative values signal invalid data - - distance_sensor_s d{}; - - device::Device::DeviceId device_id; - device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; - device_id.devid_s.address = msg->sysid; - - d.timestamp = f.timestamp; - d.min_distance = _param_ekf2_min_rng; - d.max_distance = _param_ekf2_rng_a_hmax; - d.current_distance = flow.distance; /* both are in m */ - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; - d.device_id = device_id.devid; - d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - d.variance = 0.01; - d.signal_quality = -1; - - _flow_distance_sensor_pub.publish(d); + if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { + sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; + sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; + sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + sensor_optical_flow.delta_angle_available = true; } + + sensor_optical_flow.max_flow_rate = NAN; + sensor_optical_flow.min_ground_distance = NAN; + sensor_optical_flow.max_ground_distance = NAN; + + // Use distance value for distance sensor topic + if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) { + // Positive value (including zero): distance known. Negative value: Unknown distance. + sensor_optical_flow.distance_m = flow.distance; + sensor_optical_flow.distance_available = true; + } + + sensor_optical_flow.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(sensor_optical_flow); } void MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) { - /* optical flow */ mavlink_hil_optical_flow_t flow; mavlink_msg_hil_optical_flow_decode(msg, &flow); - optical_flow_s f{}; - - f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec; - f.integration_timespan = flow.integration_time_us; - f.pixel_flow_x_integral = flow.integrated_x; - f.pixel_flow_y_integral = flow.integrated_y; - f.gyro_x_rate_integral = flow.integrated_xgyro; - f.gyro_y_rate_integral = flow.integrated_ygyro; - f.gyro_z_rate_integral = flow.integrated_zgyro; - f.time_since_last_sonar_update = flow.time_delta_distance_us; - f.ground_distance_m = flow.distance; - f.quality = flow.quality; - f.sensor_id = flow.sensor_id; - f.gyro_temperature = flow.temperature; - - _flow_pub.publish(f); - - /* Use distance value for distance sensor topic */ - distance_sensor_s d{}; - device::Device::DeviceId device_id; - device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; - d.timestamp = hrt_absolute_time(); - d.min_distance = 0.3f; - d.max_distance = 5.0f; - d.current_distance = flow.distance; /* both are in m */ - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - d.device_id = device_id.devid; - d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - d.variance = 0.01; - d.signal_quality = -1; + sensor_optical_flow_s sensor_optical_flow{}; - _flow_distance_sensor_pub.publish(d); + sensor_optical_flow.timestamp_sample = hrt_absolute_time(); + sensor_optical_flow.device_id = device_id.devid; + + sensor_optical_flow.pixel_flow[0] = flow.integrated_x; + sensor_optical_flow.pixel_flow[1] = flow.integrated_y; + + sensor_optical_flow.integration_timespan_us = flow.integration_time_us; + sensor_optical_flow.quality = flow.quality; + + if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { + sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; + sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; + sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + sensor_optical_flow.delta_angle_available = true; + } + + sensor_optical_flow.max_flow_rate = NAN; + sensor_optical_flow.min_ground_distance = NAN; + sensor_optical_flow.max_ground_distance = NAN; + + // Use distance value for distance sensor topic + if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) { + // Positive value (including zero): distance known. Negative value: Unknown distance. + sensor_optical_flow.distance_m = flow.distance; + sensor_optical_flow.distance_available = true; + } + + sensor_optical_flow.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(sensor_optical_flow); } void @@ -934,9 +916,10 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) distance_sensor_s ds{}; device::Device::DeviceId device_id; - device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; - device_id.devid_s.address = dist_sensor.id; ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */ ds.min_distance = static_cast(dist_sensor.min_distance) * 1e-2f; /* cm to m */ @@ -2338,10 +2321,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) sensor_gps_s gps{}; - device::Device::DeviceId device_id{}; + device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = _mavlink->get_instance_id(); device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + gps.device_id = device_id.devid; gps.lat = hil_gps.lat; @@ -3475,30 +3460,6 @@ MavlinkReceiver::updateParams() { // update parameters from storage ModuleParams::updateParams(); - - if (_handle_sens_flow_maxhgt != PARAM_INVALID) { - param_get(_handle_sens_flow_maxhgt, &_param_sens_flow_maxhgt); - } - - if (_handle_sens_flow_maxr != PARAM_INVALID) { - param_get(_handle_sens_flow_maxr, &_param_sens_flow_maxr); - } - - if (_handle_sens_flow_minhgt != PARAM_INVALID) { - param_get(_handle_sens_flow_minhgt, &_param_sens_flow_minhgt); - } - - if (_handle_sens_flow_rot != PARAM_INVALID) { - param_get(_handle_sens_flow_rot, &_param_sens_flow_rot); - } - - if (_handle_ekf2_min_rng != PARAM_INVALID) { - param_get(_handle_ekf2_min_rng, &_param_ekf2_min_rng); - } - - if (_handle_ekf2_rng_a_hmax != PARAM_INVALID) { - param_get(_handle_ekf2_rng_a_hmax, &_param_ekf2_rng_a_hmax); - } } void *MavlinkReceiver::start_trampoline(void *context) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 38cb1c5fa4..93723fea62 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -86,13 +86,13 @@ #include #include #include -#include #include #include #include #include #include #include +#include #include #include #include @@ -308,7 +308,6 @@ private: uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _generator_status_pub{ORB_ID(generator_status)}; - uORB::Publication _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)}; @@ -331,13 +330,13 @@ private: // ORB publications (multi) uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; - uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; // ORB publications (queue length > 1) uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; @@ -396,20 +395,6 @@ private: hrt_abstime _heartbeat_component_udp_bridge{0}; hrt_abstime _heartbeat_component_uart_bridge{0}; - param_t _handle_sens_flow_maxhgt{PARAM_INVALID}; - param_t _handle_sens_flow_maxr{PARAM_INVALID}; - param_t _handle_sens_flow_minhgt{PARAM_INVALID}; - param_t _handle_sens_flow_rot{PARAM_INVALID}; - param_t _handle_ekf2_min_rng{PARAM_INVALID}; - param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID}; - - float _param_sens_flow_maxhgt{-1.0f}; - float _param_sens_flow_maxr{-1.0f}; - float _param_sens_flow_minhgt{-1.0f}; - int32_t _param_sens_flow_rot{0}; - float _param_ekf2_min_rng{NAN}; - float _param_ekf2_rng_a_hmax{NAN}; - DEFINE_PARAMETERS( (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, diff --git a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp index ebbe35ea1f..02f368b4c3 100644 --- a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp +++ b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -34,7 +34,7 @@ #ifndef OPTICAL_FLOW_RAD_HPP #define OPTICAL_FLOW_RAD_HPP -#include +#include class MavlinkStreamOpticalFlowRad : public MavlinkStream { @@ -49,34 +49,40 @@ public: unsigned get_size() override { - return _optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _vehicle_optical_flow_sub.advertised() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : + 0; } private: explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)}; + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; bool send() override { - optical_flow_s flow; + vehicle_optical_flow_s flow; - if (_optical_flow_sub.update(&flow)) { + if (_vehicle_optical_flow_sub.update(&flow)) { mavlink_optical_flow_rad_t msg{}; - msg.time_usec = flow.timestamp; - msg.sensor_id = flow.sensor_id; - msg.integrated_x = flow.pixel_flow_x_integral; - msg.integrated_y = flow.pixel_flow_y_integral; - msg.integrated_xgyro = flow.gyro_x_rate_integral; - msg.integrated_ygyro = flow.gyro_y_rate_integral; - msg.integrated_zgyro = flow.gyro_z_rate_integral; - msg.distance = flow.ground_distance_m; + msg.time_usec = flow.timestamp_sample; + msg.sensor_id = _vehicle_optical_flow_sub.get_instance(); + msg.integration_time_us = flow.integration_timespan_us; + msg.integrated_x = flow.pixel_flow[0]; + msg.integrated_y = flow.pixel_flow[1]; + msg.integrated_xgyro = flow.delta_angle[0]; + msg.integrated_ygyro = flow.delta_angle[1]; + msg.integrated_zgyro = flow.delta_angle[2]; + // msg.temperature = 0; msg.quality = flow.quality; - msg.integration_time_us = flow.integration_timespan; - msg.sensor_id = flow.sensor_id; - msg.time_delta_distance_us = flow.time_since_last_sonar_update; - msg.temperature = flow.gyro_temperature; + // msg.time_delta_distance_us = 0; + + if (PX4_ISFINITE(flow.distance_m)) { + msg.distance = flow.distance_m; + + } else { + msg.distance = -1; + } mavlink_msg_optical_flow_rad_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 941718323b..c096da6ab5 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include #include #include @@ -47,6 +46,7 @@ #include #include #include +#include #include #include @@ -91,7 +91,7 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(distance_sensor)) { _distance_sensor_msg_id = msg_id; - } else if (sub.orb_meta == ORB_ID(optical_flow)) { + } else if (sub.orb_meta == ORB_ID(vehicle_optical_flow)) { _optical_flow_msg_id = msg_id; } else if (sub.orb_meta == ORB_ID(vehicle_air_data)) { @@ -204,7 +204,7 @@ ReplayEkf2::onExitMainLoop() print_sensor_statistics(_airspeed_msg_id, "airspeed"); print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); - print_sensor_statistics(_optical_flow_msg_id, "optical_flow"); + print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow"); print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); print_sensor_statistics(_vehicle_air_data_msg_id, "vehicle_air_data"); print_sensor_statistics(_vehicle_magnetometer_msg_id, "vehicle_magnetometer"); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 120c478212..9af0ab04cb 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -51,13 +51,20 @@ if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) add_subdirectory(vehicle_magnetometer) endif() +if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + add_subdirectory(vehicle_optical_flow) +endif() + px4_add_module( MODULE modules__sensors MAIN sensors + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR} SRCS sensors.cpp voted_sensors_update.cpp voted_sensors_update.h + Integrator.hpp MODULE_CONFIG module.yaml DEPENDS @@ -85,3 +92,7 @@ endif() if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) target_link_libraries(modules__sensors PRIVATE vehicle_magnetometer) endif() + +if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + target_link_libraries(modules__sensors PRIVATE vehicle_optical_flow) +endif() diff --git a/src/modules/sensors/vehicle_imu/Integrator.hpp b/src/modules/sensors/Integrator.hpp similarity index 96% rename from src/modules/sensors/vehicle_imu/Integrator.hpp rename to src/modules/sensors/Integrator.hpp index 80cd6f46e9..b6c26f331e 100644 --- a/src/modules/sensors/vehicle_imu/Integrator.hpp +++ b/src/modules/sensors/Integrator.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2015-2022 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 @@ -45,6 +45,9 @@ #include #include +namespace sensors +{ + class Integrator { public: @@ -94,6 +97,8 @@ public: */ inline bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); } + float integral_dt() const { return _integral_dt; } + void reset() { _alpha.zero(); @@ -137,7 +142,7 @@ protected: matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */ float _integral_dt{0}; - float _reset_interval_min{0.005f}; /**< the interval after which the content will be published and the integrator reset */ + float _reset_interval_min{0.001f}; /**< the interval after which the content will be published and the integrator reset */ uint8_t _reset_samples_min{1}; uint8_t _integrated_samples{0}; @@ -214,3 +219,5 @@ private: matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */ }; + +}; // namespace sensors diff --git a/src/modules/sensors/Kconfig b/src/modules/sensors/Kconfig index 67863ec53f..163ac29506 100644 --- a/src/modules/sensors/Kconfig +++ b/src/modules/sensors/Kconfig @@ -28,4 +28,8 @@ if MODULES_SENSORS bool "Include vehicle magnetometer" default y + config SENSORS_VEHICLE_OPTICAL_FLOW + bool "Include vehicle optical flow" + default y + endif #MODULES_SENSORS diff --git a/src/modules/sensors/sensor_params_flow.c b/src/modules/sensors/sensor_params_flow.c index b8a2b7574e..7d3de7ff7a 100644 --- a/src/modules/sensors/sensor_params_flow.c +++ b/src/modules/sensors/sensor_params_flow.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2022 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 @@ -32,11 +32,10 @@ ****************************************************************************/ /** - * PX4Flow board rotation + * Optical flow rotation * - * This parameter defines the yaw rotation of the PX4FLOW board relative to the vehicle body frame. + * This parameter defines the yaw rotation of the optical flow relative to the vehicle body frame. * Zero rotation is defined as X on flow board pointing towards front of vehicle. - * The recommneded installation default for the PX4FLOW board is with the Y axis forward (270 deg yaw). * * @value 0 No rotation * @value 1 Yaw 45° @@ -47,11 +46,9 @@ * @value 6 Yaw 270° * @value 7 Yaw 315° * - * @reboot_required true - * * @group Sensors */ -PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6); +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); /** * Minimum height above ground when reliant on optical flow. @@ -66,7 +63,7 @@ PARAM_DEFINE_INT32(SENS_FLOW_ROT, 6); * @decimal 1 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f); +PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.08f); /** * Maximum height above ground when reliant on optical flow. @@ -78,12 +75,12 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.7f); * * @unit m * @min 1.0 - * @max 25.0 + * @max 100.0 * @increment 0.1 * @decimal 1 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f); +PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 100.f); /** * Magnitude of maximum angular flow rate reliably measurable by the optical flow sensor. @@ -97,4 +94,20 @@ PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 3.0f); * @decimal 2 * @group Sensor Calibration */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 2.5f); +PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 8.f); + +/** + * Optical flow max rate. + * + * Optical flow data maximum publication rate. This is an upper bound, + * actual optical flow data rate is still dependant on the sensor. + * + * @min 1 + * @max 200 + * @group Sensors + * @unit Hz + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_FLOW_RATE, 70.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a9d1840fb4..840d722597 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -93,6 +93,10 @@ # include #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +# include "vehicle_optical_flow/VehicleOpticalFlow.hpp" +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + using namespace sensors; using namespace time_literals; @@ -210,6 +214,11 @@ private: uint8_t _n_gps{0}; #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + VehicleOpticalFlow *_vehicle_optical_flow {nullptr}; + uint8_t _n_optical_flow{0}; +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; uint8_t _n_accel{0}; @@ -242,6 +251,7 @@ private: void InitializeVehicleGPSPosition(); void InitializeVehicleIMU(); void InitializeVehicleMagnetometer(); + void InitializeVehicleOpticalFlow(); DEFINE_PARAMETERS( #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) @@ -335,6 +345,15 @@ Sensors::~Sensors() #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + + if (_vehicle_optical_flow) { + _vehicle_optical_flow->Stop(); + delete _vehicle_optical_flow; + } + +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + for (auto &vehicle_imu : _vehicle_imu_list) { if (vehicle_imu) { vehicle_imu->Stop(); @@ -445,6 +464,10 @@ int Sensors::parameters_update() InitializeVehicleMagnetometer(); #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + InitializeVehicleOpticalFlow(); +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + return PX4_OK; } @@ -688,6 +711,23 @@ void Sensors::InitializeVehicleMagnetometer() } #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) +void Sensors::InitializeVehicleOpticalFlow() +{ + if (_vehicle_optical_flow == nullptr) { + uORB::Subscription sensor_optical_flow_sub{ORB_ID(sensor_optical_flow)}; + + if (sensor_optical_flow_sub.advertised()) { + _vehicle_optical_flow = new VehicleOpticalFlow(); + + if (_vehicle_optical_flow) { + _vehicle_optical_flow->Start(); + } + } + } +} +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + void Sensors::Run() { if (should_exit()) { @@ -747,6 +787,16 @@ void Sensors::Run() #endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow)); + + if (n_optical_flow != _n_optical_flow) { + _n_optical_flow = n_optical_flow; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + const int n_accel = orb_group_count(ORB_ID(sensor_accel)); const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); @@ -877,6 +927,15 @@ int Sensors::print_status() _airspeed_validator.print(); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED +#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) + + if (_vehicle_optical_flow) { + PX4_INFO_RAW("\n"); + _vehicle_optical_flow->PrintStatus(); + } + +#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW + PX4_INFO_RAW("\n"); _vehicle_acceleration.PrintStatus(); diff --git a/src/modules/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/vehicle_imu/CMakeLists.txt index 8176327f4d..02139c4a7f 100644 --- a/src/modules/sensors/vehicle_imu/CMakeLists.txt +++ b/src/modules/sensors/vehicle_imu/CMakeLists.txt @@ -32,7 +32,6 @@ ############################################################################ px4_add_library(vehicle_imu - Integrator.hpp VehicleIMU.cpp VehicleIMU.hpp ) diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 6420fbb13b..f4f5f520c1 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -33,7 +33,7 @@ #pragma once -#include "Integrator.hpp" +#include #include #include @@ -108,8 +108,8 @@ private: calibration::Accelerometer _accel_calibration{}; calibration::Gyroscope _gyro_calibration{}; - Integrator _accel_integrator{}; - IntegratorConing _gyro_integrator{}; + sensors::Integrator _accel_integrator{}; + sensors::IntegratorConing _gyro_integrator{}; uint32_t _imu_integration_interval_us{5000}; diff --git a/src/drivers/optical_flow/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt similarity index 88% rename from src/drivers/optical_flow/CMakeLists.txt rename to src/modules/sensors/vehicle_optical_flow/CMakeLists.txt index ab6c477290..ba8761aa00 100644 --- a/src/drivers/optical_flow/CMakeLists.txt +++ b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2020 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 @@ -31,7 +31,8 @@ # ############################################################################ -add_subdirectory(paw3902) -add_subdirectory(pmw3901) -add_subdirectory(px4flow) -add_subdirectory(thoneflow) +px4_add_library(vehicle_optical_flow + VehicleOpticalFlow.cpp + VehicleOpticalFlow.hpp +) +target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp b/src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp new file mode 100644 index 0000000000..591088bc30 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/RingBuffer.hpp @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (C) 2022 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 RingBuffer.hpp + * Template RingBuffer + */ + +#include +#include +#include + +template +class RingBuffer +{ +public: + void push(const T &sample) + { + uint8_t head_new = _head; + + if (!_first_write) { + head_new = (_head + 1) % SIZE; + } + + _buffer[head_new] = sample; + _head = head_new; + + // move tail if we overwrite it + if (_head == _tail && !_first_write) { + _tail = (_tail + 1) % SIZE; + + } else { + _first_write = false; + } + } + + uint8_t get_length() const { return SIZE; } + + T &operator[](const uint8_t index) { return _buffer[index]; } + + const T &get_newest() const { return _buffer[_head]; } + const T &get_oldest() const { return _buffer[_tail]; } + + uint8_t get_oldest_index() const { return _tail; } + + bool peak_first_older_than(const uint64_t ×tamp, T *sample) + { + // start looking from newest observation data + for (uint8_t i = 0; i < SIZE; i++) { + int index = (_head - i); + index = index < 0 ? SIZE + index : index; + + if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) { + *sample = _buffer[index]; + return true; + } + + if (index == _tail) { + // we have reached the tail and haven't got a + // match + return false; + } + } + + return false; + } + + bool pop_first_older_than(const uint64_t ×tamp, T *sample) + { + // start looking from newest observation data + for (uint8_t i = 0; i < SIZE; i++) { + int index = (_head - i); + index = index < 0 ? SIZE + index : index; + + if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) { + *sample = _buffer[index]; + + // Now we can set the tail to the item which + // comes after the one we removed since we don't + // want to have any older data in the buffer + if (index == _head) { + _tail = _head; + _first_write = true; + + } else { + _tail = (index + 1) % SIZE; + } + + _buffer[index].time_us = 0; + + return true; + } + + if (index == _tail) { + // we have reached the tail and haven't got a + // match + return false; + } + } + + return false; + } + + bool pop_oldest(const uint64_t ×tamp_oldest, const uint64_t ×tamp_newest, T *sample) + { + if (timestamp_oldest >= timestamp_newest) { + return false; + } + + for (uint8_t i = 0; i < SIZE; i++) { + + uint8_t index = (_tail + i) % SIZE; + + if (_buffer[index].time_us >= timestamp_oldest && _buffer[index].time_us <= timestamp_newest) { + *sample = _buffer[index]; + + // Now we can set the tail to the item which + // comes after the one we removed since we don't + // want to have any older data in the buffer + if (index == _head) { + _tail = _head; + _first_write = true; + + } else { + _tail = (index + 1) % SIZE; + } + + _buffer[index] = {}; + + return true; + } + } + + return false; + } + + bool pop_oldest(T *sample) + { + if (_tail == _head) { + return false; + } + + *sample = _buffer[_tail]; + _buffer[_tail].time_us = 0; + + _tail = (_tail + 1) % SIZE; + + return true; + } + + bool pop_newest(T *sample) + { + if (_tail == _head) { + return false; + } + + *sample = _buffer[_head]; + _buffer[_head].time_us = 0; + + _head = (_head - 1) % SIZE; + + return true; + } + + int get_total_size() const { return sizeof(*this) + sizeof(T) * SIZE; } + + uint8_t entries() const + { + int count = 0; + + for (uint8_t i = 0; i < SIZE; i++) { + if (_buffer[i].time_us != 0) { + count++; + } + } + + return count; + } + +private: + T _buffer[SIZE] {}; + + uint8_t _head{0}; + uint8_t _tail{0}; + + bool _first_write{true}; +}; diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp new file mode 100644 index 0000000000..11d43ee0b9 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -0,0 +1,481 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "VehicleOpticalFlow.hpp" + +#include + +namespace sensors +{ + +using namespace matrix; +using namespace time_literals; + +static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; + +VehicleOpticalFlow::VehicleOpticalFlow() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ + _vehicle_optical_flow_pub.advertise(); + + _gyro_integrator.set_reset_samples(1); +} + +VehicleOpticalFlow::~VehicleOpticalFlow() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleOpticalFlow::Start() +{ + _sensor_flow_sub.registerCallback(); + + _sensor_gyro_sub.registerCallback(); + _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); + + _sensor_selection_sub.registerCallback(); + + ScheduleNow(); + return true; +} + +void VehicleOpticalFlow::Stop() +{ + Deinit(); + + // clear all registered callbacks + _sensor_flow_sub.unregisterCallback(); + _sensor_gyro_sub.unregisterCallback(); + _sensor_selection_sub.unregisterCallback(); +} + +void VehicleOpticalFlow::ParametersUpdate() +{ + // Check if parameters have changed + if (_params_sub.updated()) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + + _flow_rotation = get_rot_matrix((enum Rotation)_param_sens_flow_rot.get()); + } +} + +void VehicleOpticalFlow::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + UpdateDistanceSensor(); + UpdateSensorGyro(); + + sensor_optical_flow_s sensor_optical_flow; + + if (_sensor_flow_sub.update(&sensor_optical_flow)) { + + // clear data accumulation if there's a gap in data + if (((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last) + > sensor_optical_flow.integration_timespan_us * 1.5f) + || (_accumulated_count > 0 && _quality_sum == 0)) { + ClearAccumulatedData(); + } + + + const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf( + sensor_optical_flow.integration_timespan_us); + const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp; + + // delta angle + // - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available + if (sensor_optical_flow.delta_angle_available + && PX4_ISFINITE(sensor_optical_flow.delta_angle[0]) + && PX4_ISFINITE(sensor_optical_flow.delta_angle[1]) + && PX4_ISFINITE(sensor_optical_flow.delta_angle[2]) + ) { + // passthrough integrated gyro if available + _delta_angle += _flow_rotation * Vector3f{sensor_optical_flow.delta_angle}; + + } else { + // integrate synchronized gyro + gyroSample gyro_sample; + + while (_gyro_buffer.pop_oldest(timestamp_oldest, timestamp_newest, &gyro_sample)) { + + _gyro_integrator.put(gyro_sample.data, gyro_sample.dt); + + float min_interval_s = (sensor_optical_flow.integration_timespan_us * 1e-6f) * 0.99f; + + if (_gyro_integrator.integral_dt() > min_interval_s) { + //PX4_INFO("integral dt: %.6f, min interval: %.6f", (double)_gyro_integrator.integral_dt(),(double) min_interval_s); + break; + } + } + + Vector3f delta_angle{NAN, NAN, NAN}; + uint16_t delta_angle_dt; + + if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) { + _delta_angle += delta_angle; + + } else { + // force integrator reset + _gyro_integrator.reset(); + } + } + + // distance + // - from sensor_optical_flow if available, otherwise use downward distance_sensor if available + if (sensor_optical_flow.distance_available && PX4_ISFINITE(sensor_optical_flow.distance_m)) { + if (!PX4_ISFINITE(_distance_sum)) { + _distance_sum = sensor_optical_flow.distance_m; + _distance_sum_count = 1; + + } else { + _distance_sum += sensor_optical_flow.distance_m; + _distance_sum_count += 1; + } + + } else { + // otherwise use buffered downward facing distance_sensor if available + rangeSample range_sample; + + if (_range_buffer.peak_first_older_than(sensor_optical_flow.timestamp_sample, &range_sample)) { + if (!PX4_ISFINITE(_distance_sum)) { + _distance_sum = range_sample.data; + _distance_sum_count = 1; + + } else { + _distance_sum += range_sample.data; + _distance_sum_count += 1; + } + } + } + + _flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample; + _flow_integral(0) += sensor_optical_flow.pixel_flow[0]; + _flow_integral(1) += sensor_optical_flow.pixel_flow[1]; + + _integration_timespan_us += sensor_optical_flow.integration_timespan_us; + + _quality_sum += sensor_optical_flow.quality; + _accumulated_count++; + + bool publish = true; + + if (_param_sens_flow_rate.get() > 0) { + const float interval_us = 1e6f / _param_sens_flow_rate.get(); + + // don't allow publishing faster than SENS_FLOW_RATE + if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) { + publish = false; + } + + // integrate for full interval unless we haven't published recently + if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) { + publish = false; + } + } + + if (publish) { + vehicle_optical_flow_s vehicle_optical_flow{}; + + vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample; + vehicle_optical_flow.device_id = sensor_optical_flow.device_id; + + _flow_integral.copyTo(vehicle_optical_flow.pixel_flow); + _delta_angle.copyTo(vehicle_optical_flow.delta_angle); + + vehicle_optical_flow.integration_timespan_us = _integration_timespan_us; + + vehicle_optical_flow.quality = _quality_sum / _accumulated_count; + + if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { + vehicle_optical_flow.distance_m = _distance_sum / _distance_sum_count; + + } else { + vehicle_optical_flow.distance_m = NAN; + } + + // SENS_FLOW_MAXR + if (PX4_ISFINITE(sensor_optical_flow.max_flow_rate) + && (sensor_optical_flow.max_flow_rate <= _param_sens_flow_maxr.get())) { + + vehicle_optical_flow.max_flow_rate = sensor_optical_flow.max_flow_rate; + + } else { + vehicle_optical_flow.max_flow_rate = _param_sens_flow_maxr.get(); + } + + // SENS_FLOW_MINHGT + if (PX4_ISFINITE(sensor_optical_flow.min_ground_distance) + && (sensor_optical_flow.min_ground_distance >= _param_sens_flow_minhgt.get())) { + + vehicle_optical_flow.min_ground_distance = sensor_optical_flow.min_ground_distance; + + } else { + vehicle_optical_flow.min_ground_distance = _param_sens_flow_minhgt.get(); + } + + // SENS_FLOW_MAXHGT + if (PX4_ISFINITE(sensor_optical_flow.max_ground_distance) + && (sensor_optical_flow.max_ground_distance <= _param_sens_flow_maxhgt.get())) { + + vehicle_optical_flow.max_ground_distance = sensor_optical_flow.max_ground_distance; + + } else { + vehicle_optical_flow.max_ground_distance = _param_sens_flow_maxhgt.get(); + } + + + // rotate (SENS_FLOW_ROT) + float zeroval = 0.f; + rotate_3f((enum Rotation)_param_sens_flow_rot.get(), vehicle_optical_flow.pixel_flow[0], + vehicle_optical_flow.pixel_flow[1], zeroval); + + vehicle_optical_flow.timestamp = hrt_absolute_time(); + _vehicle_optical_flow_pub.publish(vehicle_optical_flow); + _last_publication_timestamp = vehicle_optical_flow.timestamp_sample; + + + // vehicle_optical_flow_vel if distance is available (for logging) + if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { + const float range = _distance_sum / _distance_sum_count; + + vehicle_optical_flow_vel_s flow_vel{}; + + flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample; + + // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate + // is produced by a RH rotation of the image about the sensor axis. + const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]}; + const Vector3f gyro_xyz{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]}; + + const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us; + + // compensate for body motion to give a LOS rate + const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_xyz.xy(); + + Vector3f vel_optflow_body; + vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt; + vel_optflow_body(1) = range * flow_compensated_XY_rad(0) / flow_dt; + vel_optflow_body(2) = 0.f; + + // vel_body + flow_vel.vel_body[0] = vel_optflow_body(0); + flow_vel.vel_body[1] = vel_optflow_body(1); + + // vel_ne + flow_vel.vel_ne[0] = NAN; + flow_vel.vel_ne[1] = NAN; + + vehicle_attitude_s vehicle_attitude{}; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + const matrix::Dcmf R_to_earth = matrix::Quatf(vehicle_attitude.q); + const Vector3f flow_vel_ne = R_to_earth * vel_optflow_body; + + flow_vel.vel_ne[0] = flow_vel_ne(0); + flow_vel.vel_ne[1] = flow_vel_ne(1); + } + + // flow_uncompensated_integral + flow_xy_rad.copyTo(flow_vel.flow_uncompensated_integral); + + // flow_compensated_integral + flow_compensated_XY_rad.copyTo(flow_vel.flow_compensated_integral); + + const Vector3f measured_body_rate(gyro_xyz * (1.f / flow_dt)); + + // gyro_rate + flow_vel.gyro_rate[0] = measured_body_rate(0); + flow_vel.gyro_rate[1] = measured_body_rate(1); + + // gyro_rate_integral + flow_vel.gyro_rate_integral[0] = gyro_xyz(0); + flow_vel.gyro_rate_integral[1] = gyro_xyz(1); + + flow_vel.timestamp = hrt_absolute_time(); + + _vehicle_optical_flow_vel_pub.publish(flow_vel); + } + + ClearAccumulatedData(); + } + } + + // reschedule backup + ScheduleDelayed(10_ms); + + perf_end(_cycle_perf); +} + +void VehicleOpticalFlow::UpdateDistanceSensor() +{ + // update range finder buffer + distance_sensor_s distance_sensor; + + if ((_distance_sensor_selected < 0) && _distance_sensor_subs.advertised()) { + for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { + + if (_distance_sensor_subs[i].update(&distance_sensor)) { + // only use the first instace which has the correct orientation + if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) + && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { + + int ndist = orb_group_count(ORB_ID(distance_sensor)); + + if (ndist > 1) { + PX4_INFO("selected distance_sensor:%d (%d advertised)", i, ndist); + } + + _distance_sensor_selected = i; + _last_range_sensor_update = distance_sensor.timestamp; + break; + } + } + } + } + + if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) { + // range sample + if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { + + if ((distance_sensor.current_distance >= distance_sensor.min_distance) + && (distance_sensor.current_distance <= distance_sensor.max_distance)) { + + rangeSample sample; + sample.time_us = distance_sensor.timestamp; + sample.data = distance_sensor.current_distance; + + _range_buffer.push(sample); + + _last_range_sensor_update = distance_sensor.timestamp; + + return; + } + + } else { + _distance_sensor_selected = -1; + } + } + + if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) { + _distance_sensor_selected = -1; + } +} + +void VehicleOpticalFlow::UpdateSensorGyro() +{ + if (_sensor_selection_sub.updated()) { + sensor_selection_s sensor_selection{}; + _sensor_selection_sub.copy(&sensor_selection); + + for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { + uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; + + if (sensor_gyro_sub.advertised() + && (sensor_gyro_sub.get().timestamp != 0) + && (sensor_gyro_sub.get().device_id != 0) + && (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) { + + if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) { + if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) { + + _gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id); + PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); + break; + + } else { + PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); + } + } + } + } + } + + // buffer + while (_sensor_gyro_sub.updated()) { + const unsigned last_generation = _sensor_gyro_sub.get_last_generation(); + + sensor_gyro_s sensor_gyro; + + if (_sensor_gyro_sub.copy(&sensor_gyro)) { + + if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation()); + } + + _gyro_calibration.set_device_id(sensor_gyro.device_id); + _gyro_calibration.SensorCorrectionsUpdate(); + + const float dt_s = (sensor_gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; + _gyro_timestamp_sample_last = sensor_gyro.timestamp_sample; + + gyroSample gyro_sample; + gyro_sample.time_us = sensor_gyro.timestamp_sample; + gyro_sample.data = _gyro_calibration.Correct(Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}); + gyro_sample.dt = dt_s; + + _gyro_buffer.push(gyro_sample); + } + } +} + +void VehicleOpticalFlow::ClearAccumulatedData() +{ + // clear accumulated data + _flow_integral.zero(); + _integration_timespan_us = 0; + + _delta_angle.zero(); + + _distance_sum = NAN; + _distance_sum_count = 0; + + _quality_sum = 0; + _accumulated_count = 0; + + _gyro_integrator.reset(); +} + +void VehicleOpticalFlow::PrintStatus() +{ + +} + +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp new file mode 100644 index 0000000000..611cf7c2e8 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 "data_validator/DataValidatorGroup.hpp" +#include "RingBuffer.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sensors +{ + +class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + VehicleOpticalFlow(); + ~VehicleOpticalFlow() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void ClearAccumulatedData(); + void UpdateDistanceSensor(); + void UpdateSensorGyro(); + + void Run() override; + + void ParametersUpdate(); + void SensorCorrectionsUpdate(bool force = false); + + static constexpr int MAX_SENSOR_COUNT = 3; + + uORB::Publication _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)}; + uORB::Publication _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; + + uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + + uORB::SubscriptionCallbackWorkItem _sensor_flow_sub{this, ORB_ID(sensor_optical_flow)}; + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)}; + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; + + sensors::IntegratorConing _gyro_integrator{}; + + hrt_abstime _gyro_timestamp_sample_last{0}; + + calibration::Gyroscope _gyro_calibration{}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + matrix::Dcmf _flow_rotation{matrix::eye()}; + + hrt_abstime _flow_timestamp_sample_last{0}; + matrix::Vector2f _flow_integral{}; + matrix::Vector3f _delta_angle{}; + uint32_t _integration_timespan_us{}; + float _distance_sum{NAN}; + uint8_t _distance_sum_count{0}; + uint16_t _quality_sum{0}; + uint8_t _accumulated_count{0}; + + hrt_abstime _last_publication_timestamp{0}; + + int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations + hrt_abstime _last_range_sensor_update{0}; + + struct gyroSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + matrix::Vector3f data{}; + float dt{0.f}; + }; + + struct rangeSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + float data{}; + }; + + RingBuffer _gyro_buffer{}; + RingBuffer _range_buffer{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_flow_rot, + (ParamFloat) _param_sens_flow_minhgt, + (ParamFloat) _param_sens_flow_maxhgt, + (ParamFloat) _param_sens_flow_maxr, + (ParamFloat) _param_sens_flow_rate + ) +}; +}; // namespace sensors diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 0dd34d2fc4..4c0964f1c3 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -64,10 +64,10 @@ #include #include #include -#include #include #include #include +#include #include #include #include @@ -158,7 +158,6 @@ private: void check_failure_injections(); - int publish_flow_topic(const mavlink_hil_optical_flow_t *flow); int publish_odometry_topic(const mavlink_message_t *odom_mavlink); int publish_distance_topic(const mavlink_distance_sensor_t *dist); @@ -191,7 +190,7 @@ private: // uORB publisher handlers uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; - uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 09b8925092..981c8efb18 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -665,7 +665,45 @@ void Simulator::handle_message_optical_flow(const mavlink_message_t *msg) { mavlink_hil_optical_flow_t flow; mavlink_msg_hil_optical_flow_decode(msg, &flow); - publish_flow_topic(&flow); + + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; + device_id.devid_s.bus = 0; + device_id.devid_s.address = msg->sysid; + device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; + + sensor_optical_flow_s sensor_optical_flow{}; + + sensor_optical_flow.timestamp_sample = hrt_absolute_time(); + sensor_optical_flow.device_id = device_id.devid; + + sensor_optical_flow.pixel_flow[0] = flow.integrated_x; + sensor_optical_flow.pixel_flow[1] = flow.integrated_y; + + sensor_optical_flow.integration_timespan_us = flow.integration_time_us; + sensor_optical_flow.quality = flow.quality; + + if (PX4_ISFINITE(flow.integrated_xgyro) && PX4_ISFINITE(flow.integrated_ygyro) && PX4_ISFINITE(flow.integrated_zgyro)) { + sensor_optical_flow.delta_angle[0] = flow.integrated_xgyro; + sensor_optical_flow.delta_angle[1] = flow.integrated_ygyro; + sensor_optical_flow.delta_angle[2] = flow.integrated_zgyro; + sensor_optical_flow.delta_angle_available = true; + } + + sensor_optical_flow.max_flow_rate = NAN; + sensor_optical_flow.min_ground_distance = NAN; + sensor_optical_flow.max_ground_distance = NAN; + + // Use distance value for distance sensor topic + if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) { + // Positive value (including zero): distance known. Negative value: Unknown distance. + sensor_optical_flow.distance_m = flow.distance; + sensor_optical_flow.distance_available = true; + } + + sensor_optical_flow.timestamp = hrt_absolute_time(); + + _sensor_optical_flow_pub.publish(sensor_optical_flow); } void Simulator::handle_message_rc_channels(const mavlink_message_t *msg) @@ -1309,50 +1347,6 @@ void Simulator::check_failure_injections() } } -int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink) -{ - optical_flow_s flow = {}; - flow.sensor_id = flow_mavlink->sensor_id; - flow.timestamp = hrt_absolute_time(); - flow.time_since_last_sonar_update = 0; - flow.frame_count_since_last_readout = 0; // ? - flow.integration_timespan = flow_mavlink->integration_time_us; - - flow.ground_distance_m = flow_mavlink->distance; - flow.gyro_temperature = flow_mavlink->temperature; - flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro; - flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro; - flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro; - flow.pixel_flow_x_integral = flow_mavlink->integrated_x; - flow.pixel_flow_y_integral = flow_mavlink->integrated_y; - flow.quality = flow_mavlink->quality; - - /* fill in sensor limits */ - float flow_rate_max; - param_get(param_find("SENS_FLOW_MAXR"), &flow_rate_max); - float flow_min_hgt; - param_get(param_find("SENS_FLOW_MINHGT"), &flow_min_hgt); - float flow_max_hgt; - param_get(param_find("SENS_FLOW_MAXHGT"), &flow_max_hgt); - - flow.max_flow_rate = flow_rate_max; - flow.min_ground_distance = flow_min_hgt; - flow.max_ground_distance = flow_max_hgt; - - /* rotate measurements according to parameter */ - int32_t flow_rot_int; - param_get(param_find("SENS_FLOW_ROT"), &flow_rot_int); - const enum Rotation flow_rot = (Rotation)flow_rot_int; - - float zeroval = 0.0f; - rotate_3f(flow_rot, flow.pixel_flow_x_integral, flow.pixel_flow_y_integral, zeroval); - rotate_3f(flow_rot, flow.gyro_x_rate_integral, flow.gyro_y_rate_integral, flow.gyro_z_rate_integral); - - _flow_pub.publish(flow); - - return PX4_OK; -} - int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) { uint64_t timestamp = hrt_absolute_time(); From 1fbe3c4ab338f440b7bc6e8b408f5ff3a02c3841 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 16:16:58 -0400 Subject: [PATCH 15/19] drivers/optical_flow/paw3902: cleanup/overhaul - remove internal accumulation and publish every valid raw sample synchronized with sensor - store timestamp_sample from motion interrupt - improve timing requirements from datasheet (minimum delays after register read/write) --- .../optical_flow/paw3902/CMakeLists.txt | 4 +- src/drivers/optical_flow/paw3902/PAW3902.cpp | 981 +++++++++--------- src/drivers/optical_flow/paw3902/PAW3902.hpp | 75 +- ...sters.hpp => PixArt_PAW3902_Registers.hpp} | 38 +- src/drivers/optical_flow/paw3902/parameters.c | 4 +- .../optical_flow/paw3902/paw3902_main.cpp | 8 +- 6 files changed, 553 insertions(+), 557 deletions(-) rename src/drivers/optical_flow/paw3902/{PixArt_PAW3902JF_Registers.hpp => PixArt_PAW3902_Registers.hpp} (71%) diff --git a/src/drivers/optical_flow/paw3902/CMakeLists.txt b/src/drivers/optical_flow/paw3902/CMakeLists.txt index 3626379ed2..2bd4380bf9 100644 --- a/src/drivers/optical_flow/paw3902/CMakeLists.txt +++ b/src/drivers/optical_flow/paw3902/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2022 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 @@ -38,7 +38,7 @@ px4_add_module( paw3902_main.cpp PAW3902.cpp PAW3902.hpp - PixArt_PAW3902JF_Registers.hpp + PixArt_PAW3902_Registers.hpp DEPENDS conversion drivers__device diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index 3df138a14c..4e41b091aa 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 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 @@ -43,6 +43,10 @@ PAW3902::PAW3902(const I2CSPIDriverConfig &config) : I2CSPIDriver(config), _drdy_gpio(config.drdy_gpio) { + if (_drdy_gpio != 0) { + _no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt"); + } + float yaw_rotation_degrees = (float)config.custom1; if (yaw_rotation_degrees >= 0.f) { @@ -52,30 +56,21 @@ PAW3902::PAW3902(const I2CSPIDriverConfig &config) : _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}}; } else { - // otherwise use the parameter SENS_FLOW_ROT - param_t rot = param_find("SENS_FLOW_ROT"); - int32_t val = 0; - - if (param_get(rot, &val) == PX4_OK) { - _rotation = get_rot_matrix((enum Rotation)val); - - } else { - _rotation.identity(); - } + _rotation.identity(); } } PAW3902::~PAW3902() { // free perf counters - perf_free(_sample_perf); + perf_free(_cycle_perf); perf_free(_interval_perf); - perf_free(_comms_errors); + perf_free(_reset_perf); perf_free(_false_motion_perf); - perf_free(_register_write_fail_perf); perf_free(_mode_change_bright_perf); perf_free(_mode_change_low_light_perf); perf_free(_mode_change_super_low_light_perf); + perf_free(_no_motion_interrupt_perf); } int PAW3902::init() @@ -85,38 +80,37 @@ int PAW3902::init() return PX4_ERROR; } - // force to low light mode (1) initially - ChangeMode(Mode::LowLight, true); - - _previous_collect_timestamp = hrt_absolute_time(); + Configure(); return PX4_OK; } int PAW3902::probe() { - const uint8_t Product_ID = RegisterRead(Register::Product_ID); + for (int retry = 0; retry < 3; retry++) { + const uint8_t Product_ID = RegisterRead(Register::Product_ID); + const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); + const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); - if (Product_ID != PRODUCT_ID) { - PX4_ERR("Product_ID: %X", Product_ID); - return PX4_ERROR; + if (Product_ID != PRODUCT_ID) { + PX4_ERR("Product_ID: %X", Product_ID); + break; + } + + if (Revision_ID != REVISION_ID) { + PX4_ERR("Revision_ID: %X", Revision_ID); + break; + } + + if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { + PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); + break; + } + + return PX4_OK; } - const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); - - if (Revision_ID != REVISION_ID) { - PX4_ERR("Revision_ID: %X", Revision_ID); - return PX4_ERROR; - } - - const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); - - if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { - PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); - return PX4_ERROR; - } - - return PX4_OK; + return PX4_ERROR; } int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -127,12 +121,14 @@ int PAW3902::DataReadyInterruptCallback(int irq, void *context, void *arg) void PAW3902::DataReady() { + _drdy_timestamp_sample.store(hrt_absolute_time()); ScheduleNow(); } bool PAW3902::DataReadyInterruptConfigure() { if (_drdy_gpio == 0) { + _data_ready_interrupt_enabled = false; return false; } @@ -148,12 +144,12 @@ bool PAW3902::DataReadyInterruptConfigure() bool PAW3902::DataReadyInterruptDisable() { + _data_ready_interrupt_enabled = false; + if (_drdy_gpio == 0) { return false; } - _data_ready_interrupt_enabled = false; - return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; } @@ -163,33 +159,62 @@ void PAW3902::exit_and_cleanup() I2CSPIDriverBase::exit_and_cleanup(); } +void PAW3902::Reset() +{ + perf_count(_reset_perf); + + DataReadyInterruptDisable(); + ScheduleClear(); + + // Issue a soft reset + RegisterWrite(Register::Power_Up_Reset, 0x5A); + px4_usleep(1000); + _last_reset = hrt_absolute_time(); + + _discard_reading = 3; + + _bright_to_low_counter = 0; + _low_to_superlow_counter = 0; + _low_to_bright_counter = 0; + _superlow_to_low_counter = 0; + + // Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state. + RegisterRead(0x02); + RegisterRead(0x03); + RegisterRead(0x04); + RegisterRead(0x05); + RegisterRead(0x06); +} + +void PAW3902::Configure() +{ + Reset(); + + ChangeMode(Mode::LowLight, true); +} + bool PAW3902::ChangeMode(Mode newMode, bool force) { if (newMode != _mode || force) { PX4_DEBUG("changing from mode %d -> %d", static_cast(_mode), static_cast(newMode)); - DataReadyInterruptDisable(); - ScheduleClear(); - // Issue a soft reset - RegisterWrite(Register::Power_Up_Reset, 0x5A); - px4_usleep(1000); - _last_reset = hrt_absolute_time(); + Reset(); switch (newMode) { case Mode::Bright: - ModeBright(); + ConfigureModeBright(); _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0; perf_count(_mode_change_bright_perf); break; case Mode::LowLight: - ModeLowLight(); + ConfigureModeLowLight(); _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1; perf_count(_mode_change_low_light_perf); break; case Mode::SuperLowLight: - ModeSuperLowLight(); + ConfigureModeSuperLowLight(); _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2; perf_count(_mode_change_super_low_light_perf); break; @@ -197,362 +222,354 @@ bool PAW3902::ChangeMode(Mode newMode, bool force) EnableLed(); - _discard_reading = 3; - _valid_count = 0; - if (DataReadyInterruptConfigure()) { - // backup schedule as a watchdog timeout - ScheduleDelayed(_scheduled_interval_us * 2); + // backup schedule + ScheduleDelayed(500_ms); } else { - ScheduleOnInterval(_scheduled_interval_us); + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } _mode = newMode; } - _bright_to_low_counter = 0; - _low_to_superlow_counter = 0; - _low_to_bright_counter = 0; - _superlow_to_low_counter = 0; - return true; } -void PAW3902::ModeBright() +void PAW3902::ConfigureModeBright() { // Mode 0: Bright (126 fps) 60 Lux typical // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); - RegisterWriteVerified(0x48, 0x02); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); + RegisterWrite(0x48, 0x02); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5e, 0x34); - RegisterWriteVerified(0x5b, 0x32); - RegisterWriteVerified(0x6d, 0x32); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xe5); - RegisterWriteVerified(0x71, 0xe5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6a, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xFE); - RegisterWriteVerified(0x65, 0x1F); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x47); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x03); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5e, 0x34); + RegisterWrite(0x5b, 0x32); + RegisterWrite(0x6d, 0x32); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xe5); + RegisterWrite(0x71, 0xe5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6a, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xFE); + RegisterWrite(0x65, 0x1F); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x47); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x03); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x00); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x70); - RegisterWriteVerified(0x69, 0x01); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x1f); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x00); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x70); + RegisterWrite(0x69, 0x01); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x1f); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms RegisterWrite(0x73, 0x00); } -void PAW3902::ModeLowLight() +void PAW3902::ConfigureModeLowLight() { // Mode 1: Low Light (126 fps) 30 Lux typical // low light and low speed motion tracking // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); - RegisterWriteVerified(0x48, 0x02); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); + RegisterWrite(0x48, 0x02); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5e, 0x34); - RegisterWriteVerified(0x5b, 0x65); - RegisterWriteVerified(0x6d, 0x65); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xe5); - RegisterWriteVerified(0x71, 0xe5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6a, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xFE); - RegisterWriteVerified(0x65, 0x1F); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x67); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x03); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5e, 0x34); + RegisterWrite(0x5b, 0x65); + RegisterWrite(0x6d, 0x65); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xe5); + RegisterWrite(0x71, 0xe5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6a, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xFE); + RegisterWrite(0x65, 0x1F); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x67); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x03); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x00); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x70); - RegisterWriteVerified(0x69, 0x01); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x1f); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x00); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x70); + RegisterWrite(0x69, 0x01); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x1f); - usleep(10_ms); // delay 10ms + px4_usleep(10'000); // delay 10ms RegisterWrite(0x73, 0x00); } -void PAW3902::ModeSuperLowLight() +void PAW3902::ConfigureModeSuperLowLight() { // Mode 2: Super Low Light (50 fps) 9 Lux typical // super low light and low speed motion tracking // set performance optimization registers - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x55, 0x01); - RegisterWriteVerified(0x50, 0x07); - RegisterWriteVerified(0x7f, 0x0e); - RegisterWriteVerified(0x43, 0x10); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x55, 0x01); + RegisterWrite(0x50, 0x07); + RegisterWrite(0x7f, 0x0e); + RegisterWrite(0x43, 0x10); - RegisterWriteVerified(0x48, 0x04); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0x7b); - RegisterWriteVerified(0x50, 0x00); - RegisterWriteVerified(0x55, 0x00); + RegisterWrite(0x48, 0x04); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0x7b); + RegisterWrite(0x50, 0x00); + RegisterWrite(0x55, 0x00); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x61, 0xAD); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x40, 0x00); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x41, 0xB3); - RegisterWriteVerified(0x43, 0xF1); - RegisterWriteVerified(0x45, 0x14); - RegisterWriteVerified(0x5F, 0x34); - RegisterWriteVerified(0x7B, 0x08); - RegisterWriteVerified(0x5E, 0x34); - RegisterWriteVerified(0x5B, 0x32); - RegisterWriteVerified(0x6D, 0x32); - RegisterWriteVerified(0x45, 0x17); - RegisterWriteVerified(0x70, 0xE5); - RegisterWriteVerified(0x71, 0xE5); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x44, 0x1B); - RegisterWriteVerified(0x40, 0xBF); - RegisterWriteVerified(0x4E, 0x3F); - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x66, 0x44); - RegisterWriteVerified(0x65, 0x20); - RegisterWriteVerified(0x6A, 0x3a); - RegisterWriteVerified(0x61, 0x05); - RegisterWriteVerified(0x62, 0x05); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x4F, 0xAF); - RegisterWriteVerified(0x48, 0x80); - RegisterWriteVerified(0x49, 0x80); - RegisterWriteVerified(0x57, 0x77); - RegisterWriteVerified(0x5F, 0x40); - RegisterWriteVerified(0x60, 0x78); - RegisterWriteVerified(0x61, 0x78); - RegisterWriteVerified(0x62, 0x08); - RegisterWriteVerified(0x63, 0x50); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x45, 0x60); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x4D, 0x11); - RegisterWriteVerified(0x55, 0x80); - RegisterWriteVerified(0x74, 0x21); - RegisterWriteVerified(0x75, 0x1F); - RegisterWriteVerified(0x4A, 0x78); - RegisterWriteVerified(0x4B, 0x78); - RegisterWriteVerified(0x44, 0x08); - RegisterWriteVerified(0x45, 0x50); - RegisterWriteVerified(0x64, 0xCE); - RegisterWriteVerified(0x65, 0x0B); - RegisterWriteVerified(0x72, 0x0A); - RegisterWriteVerified(0x73, 0x00); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x44, 0x84); - RegisterWriteVerified(0x65, 0x67); - RegisterWriteVerified(0x66, 0x18); - RegisterWriteVerified(0x63, 0x70); - RegisterWriteVerified(0x6f, 0x2c); - RegisterWriteVerified(0x7F, 0x15); - RegisterWriteVerified(0x48, 0x48); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x41, 0x0D); - RegisterWriteVerified(0x43, 0x14); - RegisterWriteVerified(0x4B, 0x0E); - RegisterWriteVerified(0x45, 0x0F); - RegisterWriteVerified(0x44, 0x42); - RegisterWriteVerified(0x4C, 0x80); - RegisterWriteVerified(0x7F, 0x10); - RegisterWriteVerified(0x5B, 0x02); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x41); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x61, 0xAD); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x40, 0x00); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x41, 0xB3); + RegisterWrite(0x43, 0xF1); + RegisterWrite(0x45, 0x14); + RegisterWrite(0x5F, 0x34); + RegisterWrite(0x7B, 0x08); + RegisterWrite(0x5E, 0x34); + RegisterWrite(0x5B, 0x32); + RegisterWrite(0x6D, 0x32); + RegisterWrite(0x45, 0x17); + RegisterWrite(0x70, 0xE5); + RegisterWrite(0x71, 0xE5); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x44, 0x1B); + RegisterWrite(0x40, 0xBF); + RegisterWrite(0x4E, 0x3F); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x66, 0x44); + RegisterWrite(0x65, 0x20); + RegisterWrite(0x6A, 0x3a); + RegisterWrite(0x61, 0x05); + RegisterWrite(0x62, 0x05); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x4F, 0xAF); + RegisterWrite(0x48, 0x80); + RegisterWrite(0x49, 0x80); + RegisterWrite(0x57, 0x77); + RegisterWrite(0x5F, 0x40); + RegisterWrite(0x60, 0x78); + RegisterWrite(0x61, 0x78); + RegisterWrite(0x62, 0x08); + RegisterWrite(0x63, 0x50); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x45, 0x60); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x4D, 0x11); + RegisterWrite(0x55, 0x80); + RegisterWrite(0x74, 0x21); + RegisterWrite(0x75, 0x1F); + RegisterWrite(0x4A, 0x78); + RegisterWrite(0x4B, 0x78); + RegisterWrite(0x44, 0x08); + RegisterWrite(0x45, 0x50); + RegisterWrite(0x64, 0xCE); + RegisterWrite(0x65, 0x0B); + RegisterWrite(0x72, 0x0A); + RegisterWrite(0x73, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x44, 0x84); + RegisterWrite(0x65, 0x67); + RegisterWrite(0x66, 0x18); + RegisterWrite(0x63, 0x70); + RegisterWrite(0x6f, 0x2c); + RegisterWrite(0x7F, 0x15); + RegisterWrite(0x48, 0x48); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x41, 0x0D); + RegisterWrite(0x43, 0x14); + RegisterWrite(0x4B, 0x0E); + RegisterWrite(0x45, 0x0F); + RegisterWrite(0x44, 0x42); + RegisterWrite(0x4C, 0x80); + RegisterWrite(0x7F, 0x10); + RegisterWrite(0x5B, 0x02); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x41); - usleep(25_ms); // delay 25ms + px4_usleep(25'000); // delay 25ms - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x32, 0x44); - RegisterWriteVerified(0x7F, 0x07); - RegisterWriteVerified(0x40, 0x40); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x68, 0x40); - RegisterWriteVerified(0x69, 0x02); - RegisterWriteVerified(0x7F, 0x0D); - RegisterWriteVerified(0x48, 0xC0); - RegisterWriteVerified(0x6F, 0xD5); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); - RegisterWriteVerified(0x4E, 0xA8); - RegisterWriteVerified(0x5A, 0x50); - RegisterWriteVerified(0x40, 0x80); - RegisterWriteVerified(0x73, 0x0B); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x32, 0x44); + RegisterWrite(0x7F, 0x07); + RegisterWrite(0x40, 0x40); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x68, 0x40); + RegisterWrite(0x69, 0x02); + RegisterWrite(0x7F, 0x0D); + RegisterWrite(0x48, 0xC0); + RegisterWrite(0x6F, 0xD5); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); + RegisterWrite(0x4E, 0xA8); + RegisterWrite(0x5A, 0x50); + RegisterWrite(0x40, 0x80); + RegisterWrite(0x73, 0x0B); - usleep(25_ms); // delay 25ms + px4_usleep(25'000); // delay 25ms RegisterWrite(0x73, 0x00); } @@ -565,73 +582,78 @@ void PAW3902::EnableLed() RegisterWrite(0x7F, 0x00); } -uint8_t PAW3902::RegisterRead(uint8_t reg, int retries) +uint8_t PAW3902::RegisterRead(uint8_t reg) { - for (int i = 0; i < retries; i++) { - px4_udelay(TIME_us_TSRAD); - uint8_t cmd[2] {reg, 0}; + // tSWR SPI Time Between Write And Read Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); - if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) { - return cmd[1]; - } + if (elapsed_last_write < TIME_TSWR_us) { + px4_udelay(TIME_TSWR_us - elapsed_last_write); } - perf_count(_comms_errors); - return 0; + // tSRW/tSRR SPI Time Between Read And Subsequent Commands + const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); + + if (elapsed_last_write < TIME_TSRW_TSRR_us) { + px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); + } + + uint8_t cmd[2]; + cmd[0] = DIR_READ(reg); + cmd[1] = 0; + transfer(&cmd[0], &cmd[0], sizeof(cmd)); + hrt_store_absolute_time(&_last_read_time); + + return cmd[1]; } void PAW3902::RegisterWrite(uint8_t reg, uint8_t data) { + // tSWW SPI Time Between Write Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWW_us) { + px4_udelay(TIME_TSWW_us - elapsed_last_write); + } + uint8_t cmd[2]; cmd[0] = DIR_WRITE(reg); cmd[1] = data; - - if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) { - perf_count(_comms_errors); - } -} - -bool PAW3902::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries) -{ - for (int i = 0; i < retries; i++) { - uint8_t cmd[2]; - cmd[0] = DIR_WRITE(reg); - cmd[1] = data; - transfer(&cmd[0], nullptr, sizeof(cmd)); - px4_udelay(TIME_us_TSWW); - - // read back to verify - uint8_t data_read = RegisterRead(reg); - - if (data_read == data) { - return true; - } - - PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read); - } - - perf_count(_register_write_fail_perf); - - return false; + transfer(&cmd[0], nullptr, sizeof(cmd)); + hrt_store_absolute_time(&_last_write_time); } void PAW3902::RunImpl() { - // backup schedule - if (_data_ready_interrupt_enabled) { - ScheduleDelayed(_scheduled_interval_us * 2); - } + perf_begin(_cycle_perf); + perf_count(_interval_perf); - // force reset if there hasn't been valid data for an extended period (sensor could be in a bad state) - static constexpr hrt_abstime RESET_TIMEOUT_US = 5_s; + const hrt_abstime now = hrt_absolute_time(); - if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) { - ChangeMode(Mode::LowLight, true); + // force reconfigure if we haven't received valid data for quite some time + if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) { + PX4_ERR("no valid data for %.3f seconds, resetting", 1e-6 * (now - _last_good_data)); + Configure(); + perf_end(_cycle_perf); return; } - perf_begin(_sample_perf); - perf_count(_interval_perf); + hrt_abstime timestamp_sample = now; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if (now < drdy_timestamp_sample + _scheduled_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_no_motion_interrupt_perf); + } + + // push backup schedule back + ScheduleDelayed(500_ms); + } struct TransferBuffer { uint8_t cmd = Register::Motion_Burst; @@ -639,25 +661,16 @@ void PAW3902::RunImpl() } buf{}; static_assert(sizeof(buf) == (12 + 1)); - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); + if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) { + perf_end(_cycle_perf); return; } - perf_end(_sample_perf); - - const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp; - - // update for next iteration - _previous_collect_timestamp = timestamp_sample; + hrt_store_absolute_time(&_last_read_time); if (_discard_reading > 0) { _discard_reading--; - ResetAccumulatedData(); - _valid_count = 0; + perf_end(_cycle_perf); return; } @@ -666,8 +679,17 @@ void PAW3902::RunImpl() // Bright Mode, SQUAL < 0x19, Shutter ≥ 0x1FF0 // Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x1FF0 // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0 - const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower; + // 13-bit Shutter register + const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit4 | Bit3 | Bit2 | Bit1 | Bit0); + const uint8_t Shutter_Lower = buf.data.Shutter_Lower; + + const uint16_t shutter = (Shutter_Upper << 8) | Shutter_Lower; + + // Motion since last report and Surface quality non-zero + const bool motion_detected = buf.data.Motion & Motion_Bit::MOT; + + // Number of Features = SQUAL * 4 bool data_valid = (buf.data.SQUAL > 0); switch (_mode) { @@ -766,99 +788,66 @@ void PAW3902::RunImpl() } if (data_valid) { - const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L); - const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L); + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); - _flow_dt_sum_usec += dt_flow; - _flow_sum_x += delta_x_raw; - _flow_sum_y += delta_y_raw; - _flow_sample_counter++; - _flow_quality_sum += buf.data.SQUAL; + report.integration_timespan_us = _scheduled_interval_us; + report.quality = buf.data.SQUAL; - _valid_count++; + // set specs according to datasheet + report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = INFINITY; // Datasheet: infinity - } else { - _valid_count = 0; - ResetAccumulatedData(); - return; + switch (_mode) { + case Mode::Bright: + report.mode = sensor_optical_flow_s::MODE_BRIGHT; + break; + + case Mode::LowLight: + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + break; + + case Mode::SuperLowLight: + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; + break; + } + + if (motion_detected) { + // only populate flow if data valid (motion and quality > 0) + const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L); + const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L); + + // rotate measurements in yaw from sensor frame to body frame + const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; + + report.pixel_flow[0] = pixel_flow_rotated(0) / 500.0f; // proportional factor + convert from pixels to radians + report.pixel_flow[1] = pixel_flow_rotated(1) / 500.0f; // proportional factor + convert from pixels to radians + } + + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); + + if (report.quality >= 1) { + _last_good_data = report.timestamp_sample; + } } - // returns if the collect time has not been reached - if (_flow_dt_sum_usec < COLLECT_TIME) { - return; - } - - sensor_optical_flow_s report{}; - report.timestamp_sample = timestamp_sample; - report.device_id = get_device_id(); - - float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians - float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians - - // rotate measurements in yaw from sensor frame to body frame - const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow[0] = pixel_flow_rotated(0); - report.pixel_flow[1] = pixel_flow_rotated(1); - - report.integration_timespan_us = _flow_dt_sum_usec; // microseconds - - report.quality = _flow_quality_sum / _flow_sample_counter; - - // No gyro on this board - report.delta_angle[0] = NAN; - report.delta_angle[1] = NAN; - report.delta_angle[2] = NAN; - - // set (conservative) specs according to datasheet - report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s - report.min_ground_distance = 0.08f; // Datasheet: 80mm - report.max_ground_distance = 30.0f; // Datasheet: infinity - - - switch (_mode) { - case Mode::Bright: - report.mode = sensor_optical_flow_s::MODE_BRIGHT; - break; - - case Mode::LowLight: - report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; - break; - - case Mode::SuperLowLight: - report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; - break; - } - - report.timestamp = hrt_absolute_time(); - _sensor_optical_flow_pub.publish(report); - - if (report.quality > 10) { - _last_good_publish = report.timestamp; - } - - ResetAccumulatedData(); -} - -void PAW3902::ResetAccumulatedData() -{ - // reset - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - _flow_sample_counter = 0; - _flow_quality_sum = 0; + perf_end(_cycle_perf); } void PAW3902::print_status() { I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); + perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - perf_print_counter(_comms_errors); + perf_print_counter(_reset_perf); perf_print_counter(_false_motion_perf); - perf_print_counter(_register_write_fail_perf); perf_print_counter(_mode_change_bright_perf); perf_print_counter(_mode_change_low_light_perf); perf_print_counter(_mode_change_super_low_light_perf); + perf_print_counter(_no_motion_interrupt_perf); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index d6ea4ff436..11951be383 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 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 @@ -34,12 +34,12 @@ /** * @file PAW3902.hpp * - * Driver for the Pixart PAW3902 & PAW3903 optical flow sensors connected via SPI. + * Driver for the PAW3902JF-TXQT: Optical Motion Tracking Chip */ #pragma once -#include "PixArt_PAW3902JF_Registers.hpp" +#include "PixArt_PAW3902_Registers.hpp" #include #include @@ -48,16 +48,15 @@ #include #include #include -#include #include #include #include using namespace time_literals; -using namespace PixArt_PAW3902JF; +using namespace PixArt_PAW3902; -#define DIR_WRITE(a) ((a) | (1 << 7)) -#define DIR_READ(a) ((a) & 0x7f) +#define DIR_WRITE(a) ((a) | Bit7) +#define DIR_READ(a) ((a) & 0x7F) class PAW3902 : public device::SPI, public I2CSPIDriver { @@ -78,65 +77,61 @@ private: int probe() override; + void Reset(); + static int DataReadyInterruptCallback(int irq, void *context, void *arg); void DataReady(); bool DataReadyInterruptConfigure(); bool DataReadyInterruptDisable(); - uint8_t RegisterRead(uint8_t reg, int retries = 2); + uint8_t RegisterRead(uint8_t reg); void RegisterWrite(uint8_t reg, uint8_t data); - bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1); - void EnableLed(); - - void ModeBright(); - void ModeLowLight(); - void ModeSuperLowLight(); + void Configure(); bool ChangeMode(Mode newMode, bool force = false); - void ResetAccumulatedData(); + void ConfigureModeBright(); + void ConfigureModeLowLight(); + void ConfigureModeSuperLowLight(); + + void EnableLed(); uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; - perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; - perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; - perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; - perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")}; - perf_counter_t _register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")}; - perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")}; - perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")}; - perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")}; - - static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")}; + perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")}; + perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")}; + perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")}; + perf_counter_t _no_motion_interrupt_perf{nullptr}; const spi_drdy_gpio_t _drdy_gpio; - uint64_t _previous_collect_timestamp{0}; - uint64_t _flow_dt_sum_usec{0}; - uint8_t _flow_sample_counter{0}; - uint16_t _flow_quality_sum{0}; + matrix::Dcmf _rotation; - matrix::Dcmf _rotation; + int _discard_reading{3}; - int _discard_reading{3}; + Mode _mode{Mode::LowLight}; - int _flow_sum_x{0}; - int _flow_sum_y{0}; - - Mode _mode{Mode::LowLight}; - - uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1}; + uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0}; int _bright_to_low_counter{0}; int _low_to_superlow_counter{0}; int _low_to_bright_counter{0}; int _superlow_to_low_counter{0}; - int _valid_count{0}; - + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; - hrt_abstime _last_good_publish{0}; + hrt_abstime _last_write_time{0}; + hrt_abstime _last_read_time{0}; + + // force reset if there hasn't been valid data for an extended period (sensor could be in a bad state) + static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s; + + hrt_abstime _last_good_data{0}; hrt_abstime _last_reset{0}; }; diff --git a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp similarity index 71% rename from src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp rename to src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp index 205f0065d5..eceb490200 100644 --- a/src/drivers/optical_flow/paw3902/PixArt_PAW3902JF_Registers.hpp +++ b/src/drivers/optical_flow/paw3902/PixArt_PAW3902_Registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 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 @@ -33,22 +33,36 @@ #pragma once -namespace PixArt_PAW3902JF +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace PixArt_PAW3902 { -static constexpr uint8_t PRODUCT_ID = 0x49; -static constexpr uint8_t REVISION_ID = 0x01; +static constexpr uint8_t PRODUCT_ID = 0x49; +static constexpr uint8_t REVISION_ID = 0x01; static constexpr uint8_t PRODUCT_ID_INVERSE = 0xB6; -static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps -static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps -static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface -// Various time delay needed for PAW3902 -static constexpr uint32_t TIME_us_TSWW = 11; // actually 10.5us -static constexpr uint32_t TIME_us_TSRAD = 2; +// Various time delays +static constexpr uint32_t TIME_TSWW_us = 11; // SPI Time Between Write Commands (actually 10.5us) +static constexpr uint32_t TIME_TSWR_us = 6; // SPI Time Between Write and Read Commands +static constexpr uint32_t TIME_TSRW_TSRR_us = 2; // SPI Time Between Read And Subsequent Commands (actually 1.5us) +static constexpr uint32_t TIME_TSRAD_us = 2; // SPI Read Address-Data Delay enum Register : uint8_t { Product_ID = 0x00, @@ -75,8 +89,8 @@ enum Register : uint8_t { Inverse_Product_ID = 0x5F, }; -enum Product_ID_Bit : uint8_t { - Reset = 0x5A, +enum Motion_Bit : uint8_t { + MOT = Bit7, // Motion since last report }; enum class Mode { diff --git a/src/drivers/optical_flow/paw3902/parameters.c b/src/drivers/optical_flow/paw3902/parameters.c index 96fa1ce316..e2ce1da533 100644 --- a/src/drivers/optical_flow/paw3902/parameters.c +++ b/src/drivers/optical_flow/paw3902/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 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 @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * PAW3902 & PAW3903 Optical Flow + * PAW3902/PAW3903 Optical Flow * * @reboot_required true * diff --git a/src/drivers/optical_flow/paw3902/paw3902_main.cpp b/src/drivers/optical_flow/paw3902/paw3902_main.cpp index 68a56ba9ff..f8f383f1ee 100644 --- a/src/drivers/optical_flow/paw3902/paw3902_main.cpp +++ b/src/drivers/optical_flow/paw3902/paw3902_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2022 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 @@ -71,13 +71,11 @@ extern "C" __EXPORT int paw3902_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } From e5f081d9aca178546697099f2c881a689d99a1c0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 17 Jun 2022 16:24:40 -0400 Subject: [PATCH 16/19] drivers/optical_flow/paa3905: cleanup/overhaul - remove internal accumulation and publish every valid raw sample synchronized with sensor - store timestamp_sample from motion interrupt - improve timing requirements from datasheet (minimum delays after register read/write) --- src/drivers/optical_flow/paa3905/PAA3905.cpp | 715 +++++++++--------- src/drivers/optical_flow/paa3905/PAA3905.hpp | 77 +- .../paa3905/PixArt_PAA3905_Registers.hpp | 40 +- .../optical_flow/paa3905/paa3905_main.cpp | 2 +- src/drivers/optical_flow/paa3905/parameters.c | 2 +- 5 files changed, 439 insertions(+), 397 deletions(-) diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index b6457946a0..ede8687c8d 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -43,6 +43,10 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) : I2CSPIDriver(config), _drdy_gpio(config.drdy_gpio) { + if (_drdy_gpio != 0) { + _no_motion_interrupt_perf = perf_alloc(PC_COUNT, MODULE_NAME": no motion interrupt"); + } + float yaw_rotation_degrees = (float)config.custom1; if (yaw_rotation_degrees >= 0.f) { @@ -52,27 +56,21 @@ PAA3905::PAA3905(const I2CSPIDriverConfig &config) : _rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}}; } else { - // otherwise use the parameter SENS_FLOW_ROT - param_t rot = param_find("SENS_FLOW_ROT"); - int32_t val = 0; - - if (param_get(rot, &val) == PX4_OK) { - _rotation = get_rot_matrix((enum Rotation)val); - - } else { - _rotation.identity(); - } + _rotation.identity(); } } PAA3905::~PAA3905() { // free perf counters - perf_free(_sample_perf); + perf_free(_cycle_perf); perf_free(_interval_perf); - perf_free(_comms_errors); + perf_free(_reset_perf); perf_free(_false_motion_perf); - perf_free(_register_write_fail_perf); + perf_free(_mode_change_bright_perf); + perf_free(_mode_change_low_light_perf); + perf_free(_mode_change_super_low_light_perf); + perf_free(_no_motion_interrupt_perf); } int PAA3905::init() @@ -84,35 +82,35 @@ int PAA3905::init() Configure(); - _previous_collect_timestamp = hrt_absolute_time(); - return PX4_OK; } int PAA3905::probe() { - const uint8_t Product_ID = RegisterRead(Register::Product_ID); + for (int retry = 0; retry < 3; retry++) { + const uint8_t Product_ID = RegisterRead(Register::Product_ID); + const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); + const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); - if (Product_ID != PRODUCT_ID) { - PX4_ERR("Product_ID: %X", Product_ID); - return PX4_ERROR; + if (Product_ID != PRODUCT_ID) { + PX4_ERR("Product_ID: %X", Product_ID); + break; + } + + if (Revision_ID != REVISION_ID) { + PX4_ERR("Revision_ID: %X", Revision_ID); + break; + } + + if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { + PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); + break; + } + + return PX4_OK; } - const uint8_t Revision_ID = RegisterRead(Register::Revision_ID); - - if (Revision_ID != REVISION_ID) { - PX4_ERR("Revision_ID: %X", Revision_ID); - return PX4_ERROR; - } - - const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID); - - if (Inverse_Product_ID != PRODUCT_ID_INVERSE) { - PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID); - return PX4_ERROR; - } - - return PX4_OK; + return PX4_ERROR; } int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -123,12 +121,14 @@ int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg) void PAA3905::DataReady() { + _drdy_timestamp_sample.store(hrt_absolute_time()); ScheduleNow(); } bool PAA3905::DataReadyInterruptConfigure() { if (_drdy_gpio == 0) { + _data_ready_interrupt_enabled = false; return false; } @@ -144,12 +144,12 @@ bool PAA3905::DataReadyInterruptConfigure() bool PAA3905::DataReadyInterruptDisable() { + _data_ready_interrupt_enabled = false; + if (_drdy_gpio == 0) { return false; } - _data_ready_interrupt_enabled = false; - return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; } @@ -159,8 +159,10 @@ void PAA3905::exit_and_cleanup() I2CSPIDriverBase::exit_and_cleanup(); } -void PAA3905::Configure() +void PAA3905::Reset() { + perf_count(_reset_perf); + DataReadyInterruptDisable(); ScheduleClear(); @@ -169,274 +171,312 @@ void PAA3905::Configure() px4_usleep(1000); _last_reset = hrt_absolute_time(); - StandardDetectionSetting(); - ModeAuto012(); + _discard_reading = 3; - CheckMode(); + // Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion pin state. + RegisterRead(0x02); + RegisterRead(0x03); + RegisterRead(0x04); + RegisterRead(0x05); + RegisterRead(0x06); +} - switch (_mode) { - case Mode::Bright: - _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0; - break; +void PAA3905::Configure() +{ + Reset(); - case Mode::LowLight: - _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1; - break; + ConfigureStandardDetectionSetting(); - case Mode::SuperLowLight: - _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2; - break; - } + ConfigureAutomaticModeSwitching(); EnableLed(); - _discard_reading = 3; - _valid_count = 0; + // Read Register 0x15. Check Bit [7:6] for AMS mode + const uint8_t Observation = RegisterRead(Register::Observation); + UpdateMode(Observation); if (DataReadyInterruptConfigure()) { - // backup schedule as a watchdog timeout - ScheduleDelayed(_scheduled_interval_us * 2); + // backup schedule + ScheduleDelayed(500_ms); } else { - ScheduleOnInterval(_scheduled_interval_us); + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } } -void PAA3905::CheckMode() +void PAA3905::ConfigureStandardDetectionSetting() { - // Read Register 0x15. Check Bit [7:6] for AMS mode and store it into a variable. - const uint8_t Observation = RegisterRead(Register::Observation); + // Standard Detection Setting is recommended for general tracking operations. In this mode, the chip can detect + // when it is operating over striped, checkerboard, and glossy tile surfaces where tracking performance is + // compromised. - // Bit [7:6] AMS mode - const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5; + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0xFF); + RegisterWrite(0x4E, 0x2A); + RegisterWrite(0x66, 0x3E); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x7E, 0x71); + RegisterWrite(0x55, 0x00); + RegisterWrite(0x59, 0x00); + RegisterWrite(0x6F, 0x2C); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x4D, 0xAC); + RegisterWrite(0x4E, 0x32); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x5C, 0xAF); + RegisterWrite(0x5F, 0xAF); + RegisterWrite(0x70, 0x08); + RegisterWrite(0x71, 0x04); + RegisterWrite(0x72, 0x06); + RegisterWrite(0x74, 0x3C); + RegisterWrite(0x75, 0x28); + RegisterWrite(0x76, 0x20); + RegisterWrite(0x4E, 0xBF); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x64, 0x14); + RegisterWrite(0x65, 0x0A); + RegisterWrite(0x66, 0x10); + RegisterWrite(0x55, 0x3C); + RegisterWrite(0x56, 0x28); + RegisterWrite(0x57, 0x20); + RegisterWrite(0x4A, 0x2D); - if (ams_mode == 0x0) { - // Mode 0 - _mode = Mode::SuperLowLight; - - } else if (ams_mode == 0x1) { - // Mode 1 - _mode = Mode::LowLight; - - } else if (ams_mode == 0x2) { - // Mode 2 - _mode = Mode::SuperLowLight; - } + RegisterWrite(0x4B, 0x2D); + RegisterWrite(0x4E, 0x4B); + RegisterWrite(0x69, 0xFA); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x69, 0x1F); + RegisterWrite(0x47, 0x1F); + RegisterWrite(0x48, 0x0C); + RegisterWrite(0x5A, 0x20); + RegisterWrite(0x75, 0x0F); + RegisterWrite(0x4A, 0x0F); + RegisterWrite(0x42, 0x02); + RegisterWrite(0x45, 0x03); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x67, 0x76); + RegisterWrite(0x68, 0x76); + RegisterWrite(0x6A, 0xC5); + RegisterWrite(0x43, 0x00); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x4A, 0x18); + RegisterWrite(0x4B, 0x0C); + RegisterWrite(0x4C, 0x0C); + RegisterWrite(0x4D, 0x0C); + RegisterWrite(0x46, 0x0A); + RegisterWrite(0x59, 0xCD); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x4A, 0x2A); + RegisterWrite(0x48, 0x96); + RegisterWrite(0x52, 0xB4); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); } -void PAA3905::StandardDetectionSetting() +void PAA3905::ConfigureEnhancedDetectionMode() { - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0xFF); - RegisterWriteVerified(0x4E, 0x2A); - RegisterWriteVerified(0x66, 0x3E); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x7E, 0x71); - RegisterWriteVerified(0x55, 0x00); - RegisterWriteVerified(0x59, 0x00); - RegisterWriteVerified(0x6F, 0x2C); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x4D, 0xAC); - RegisterWriteVerified(0x4E, 0x32); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x5C, 0xAF); - RegisterWriteVerified(0x5F, 0xAF); - RegisterWriteVerified(0x70, 0x08); - RegisterWriteVerified(0x71, 0x04); - RegisterWriteVerified(0x72, 0x06); - RegisterWriteVerified(0x74, 0x3C); - RegisterWriteVerified(0x75, 0x28); - RegisterWriteVerified(0x76, 0x20); - RegisterWriteVerified(0x4E, 0xBF); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x64, 0x14); - RegisterWriteVerified(0x65, 0x0A); - RegisterWriteVerified(0x66, 0x10); - RegisterWriteVerified(0x55, 0x3C); - RegisterWriteVerified(0x56, 0x28); - RegisterWriteVerified(0x57, 0x20); - RegisterWriteVerified(0x4A, 0x2D); + // Enhance Detection Setting relatively has better detection sensitivity, it is recommended where yaw motion + // detection is required, and also where more sensitive challenging surface detection is required. The recommended + // operating height must be greater than 15 cm to avoid false detection on challenging surfaces due to increasing of + // sensitivity. - RegisterWriteVerified(0x4B, 0x2D); - RegisterWriteVerified(0x4E, 0x4B); - RegisterWriteVerified(0x69, 0xFA); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x69, 0x1F); - RegisterWriteVerified(0x47, 0x1F); - RegisterWriteVerified(0x48, 0x0C); - RegisterWriteVerified(0x5A, 0x20); - RegisterWriteVerified(0x75, 0x0F); - RegisterWriteVerified(0x4A, 0x0F); - RegisterWriteVerified(0x42, 0x02); - RegisterWriteVerified(0x45, 0x03); - RegisterWriteVerified(0x65, 0x00); - RegisterWriteVerified(0x67, 0x76); - RegisterWriteVerified(0x68, 0x76); - RegisterWriteVerified(0x6A, 0xC5); - RegisterWriteVerified(0x43, 0x00); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x4A, 0x18); - RegisterWriteVerified(0x4B, 0x0C); - RegisterWriteVerified(0x4C, 0x0C); - RegisterWriteVerified(0x4D, 0x0C); - RegisterWriteVerified(0x46, 0x0A); - RegisterWriteVerified(0x59, 0xCD); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x4A, 0x2A); - RegisterWriteVerified(0x48, 0x96); - RegisterWriteVerified(0x52, 0xB4); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x51, 0xFF); + RegisterWrite(0x4E, 0x2A); + RegisterWrite(0x66, 0x26); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x7E, 0x71); + RegisterWrite(0x55, 0x00); + RegisterWrite(0x59, 0x00); + RegisterWrite(0x6F, 0x2C); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x4D, 0xAC); + RegisterWrite(0x4E, 0x65); + RegisterWrite(0x7F, 0x09); + RegisterWrite(0x5C, 0xAF); + RegisterWrite(0x5F, 0xAF); + RegisterWrite(0x70, 0x00); + RegisterWrite(0x71, 0x00); + RegisterWrite(0x72, 0x00); + RegisterWrite(0x74, 0x14); + RegisterWrite(0x75, 0x14); + RegisterWrite(0x76, 0x06); + RegisterWrite(0x4E, 0x8F); + RegisterWrite(0x7F, 0x03); + RegisterWrite(0x64, 0x00); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x66, 0x00); + RegisterWrite(0x55, 0x14); + RegisterWrite(0x56, 0x14); + RegisterWrite(0x57, 0x06); + RegisterWrite(0x4A, 0x20); + + RegisterWrite(0x4B, 0x20); + RegisterWrite(0x4E, 0x32); + RegisterWrite(0x69, 0xFE); + RegisterWrite(0x7F, 0x05); + RegisterWrite(0x69, 0x14); + RegisterWrite(0x47, 0x14); + RegisterWrite(0x48, 0x1C); + RegisterWrite(0x5A, 0x20); + RegisterWrite(0x75, 0xE5); + RegisterWrite(0x4A, 0x05); + RegisterWrite(0x42, 0x04); + RegisterWrite(0x45, 0x03); + RegisterWrite(0x65, 0x00); + RegisterWrite(0x67, 0x50); + RegisterWrite(0x68, 0x50); + RegisterWrite(0x6A, 0xC5); + RegisterWrite(0x43, 0x00); + RegisterWrite(0x7F, 0x06); + RegisterWrite(0x4A, 0x1E); + RegisterWrite(0x4B, 0x1E); + RegisterWrite(0x4C, 0x34); + RegisterWrite(0x4D, 0x34); + RegisterWrite(0x46, 0x32); + RegisterWrite(0x59, 0x0D); + RegisterWrite(0x7F, 0x0A); + RegisterWrite(0x4A, 0x2A); + RegisterWrite(0x48, 0x96); + RegisterWrite(0x52, 0xB4); + RegisterWrite(0x7F, 0x00); + RegisterWrite(0x5B, 0xA0); } -void PAA3905::EnhancedDetectionMode() -{ - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x51, 0xFF); - RegisterWriteVerified(0x4E, 0x2A); - RegisterWriteVerified(0x66, 0x26); - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x7E, 0x71); - RegisterWriteVerified(0x55, 0x00); - RegisterWriteVerified(0x59, 0x00); - RegisterWriteVerified(0x6F, 0x2C); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x4D, 0xAC); - RegisterWriteVerified(0x4E, 0x65); - RegisterWriteVerified(0x7F, 0x09); - RegisterWriteVerified(0x5C, 0xAF); - RegisterWriteVerified(0x5F, 0xAF); - RegisterWriteVerified(0x70, 0x00); - RegisterWriteVerified(0x71, 0x00); - RegisterWriteVerified(0x72, 0x00); - RegisterWriteVerified(0x74, 0x14); - RegisterWriteVerified(0x75, 0x14); - RegisterWriteVerified(0x76, 0x06); - RegisterWriteVerified(0x4E, 0x8F); - RegisterWriteVerified(0x7F, 0x03); - RegisterWriteVerified(0x64, 0x00); - RegisterWriteVerified(0x65, 0x00); - RegisterWriteVerified(0x66, 0x00); - RegisterWriteVerified(0x55, 0x14); - RegisterWriteVerified(0x56, 0x14); - RegisterWriteVerified(0x57, 0x06); - RegisterWriteVerified(0x4A, 0x20); - - RegisterWriteVerified(0x4B, 0x20); - RegisterWriteVerified(0x4E, 0x32); - RegisterWriteVerified(0x69, 0xFE); - RegisterWriteVerified(0x7F, 0x05); - RegisterWriteVerified(0x69, 0x14); - RegisterWriteVerified(0x47, 0x14); - RegisterWriteVerified(0x48, 0x1C); - RegisterWriteVerified(0x5A, 0x20); - RegisterWriteVerified(0x75, 0xE5); - RegisterWriteVerified(0x4A, 0x05); - RegisterWriteVerified(0x42, 0x04); - RegisterWriteVerified(0x45, 0x03); - RegisterWriteVerified(0x65, 0x00); - RegisterWriteVerified(0x67, 0x50); - RegisterWriteVerified(0x68, 0x50); - RegisterWriteVerified(0x6A, 0xC5); - RegisterWriteVerified(0x43, 0x00); - RegisterWriteVerified(0x7F, 0x06); - RegisterWriteVerified(0x4A, 0x1E); - RegisterWriteVerified(0x4B, 0x1E); - RegisterWriteVerified(0x4C, 0x34); - RegisterWriteVerified(0x4D, 0x34); - RegisterWriteVerified(0x46, 0x32); - RegisterWriteVerified(0x59, 0x0D); - RegisterWriteVerified(0x7F, 0x0A); - RegisterWriteVerified(0x4A, 0x2A); - RegisterWriteVerified(0x48, 0x96); - RegisterWriteVerified(0x52, 0xB4); - RegisterWriteVerified(0x7F, 0x00); - RegisterWriteVerified(0x5B, 0xA0); -} - -void PAA3905::ModeAuto012() +void PAA3905::ConfigureAutomaticModeSwitching() { // Automatic switching between Mode 0, 1 and 2: - RegisterWriteVerified(0x7F, 0x08); - RegisterWriteVerified(0x68, 0x02); - RegisterWriteVerified(0x7F, 0x00); + RegisterWrite(0x7F, 0x08); + RegisterWrite(0x68, 0x02); + RegisterWrite(0x7F, 0x00); + + // TODO: for mode 0 and 1 only + // Automatic switching between Mode 0 and 1 only: + // RegisterWrite(0x7F, 0x08); + // RegisterWrite(0x68, 0x01); // different than mode 0,1,2 + // RegisterWrite(0x7F, 0x00); } void PAA3905::EnableLed() { // Enable LED_N controls - RegisterWriteVerified(0x7F, 0x14); - RegisterWriteVerified(0x6F, 0x0C); - RegisterWriteVerified(0x7F, 0x00); + RegisterWrite(0x7F, 0x14); + RegisterWrite(0x6F, 0x0C); + RegisterWrite(0x7F, 0x00); } -uint8_t PAA3905::RegisterRead(uint8_t reg, int retries) +bool PAA3905::UpdateMode(const uint8_t observation) { - for (int i = 0; i < retries; i++) { - px4_udelay(TIME_us_TSRAD); - uint8_t cmd[2] {reg, 0}; + bool mode_changed = false; - if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) { - return cmd[1]; + // Bit [7:6] AMS mode + const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5; + + if (ams_mode == 0x0) { + // Mode 0 (Bright) + if (_mode != Mode::Bright) { + mode_changed = true; + perf_count(_mode_change_bright_perf); } + + _mode = Mode::Bright; + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_0; + + } else if (ams_mode == 0x1) { + // Mode 1 (LowLight) + if (_mode != Mode::LowLight) { + mode_changed = true; + perf_count(_mode_change_low_light_perf); + } + + _mode = Mode::LowLight; + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_1; + + } else if (ams_mode == 0x2) { + // Mode 2 (SuperLowLight) + if (_mode != Mode::SuperLowLight) { + mode_changed = true; + perf_count(_mode_change_super_low_light_perf); + } + + _mode = Mode::SuperLowLight; + _scheduled_interval_us = SAMPLE_INTERVAL_MODE_2; } - perf_count(_comms_errors); - return 0; + return mode_changed; +} + +uint8_t PAA3905::RegisterRead(uint8_t reg) +{ + // tSWR SPI Time Between Write And Read Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWR_us) { + px4_udelay(TIME_TSWR_us - elapsed_last_write); + } + + // tSRW/tSRR SPI Time Between Read And Subsequent Commands + const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); + + if (elapsed_last_write < TIME_TSRW_TSRR_us) { + px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); + } + + uint8_t cmd[2]; + cmd[0] = DIR_READ(reg); + cmd[1] = 0; + transfer(&cmd[0], &cmd[0], sizeof(cmd)); + hrt_store_absolute_time(&_last_read_time); + + return cmd[1]; } void PAA3905::RegisterWrite(uint8_t reg, uint8_t data) { + // tSWW SPI Time Between Write Commands + const hrt_abstime elapsed_last_write = hrt_elapsed_time(&_last_write_time); + + if (elapsed_last_write < TIME_TSWW_us) { + px4_udelay(TIME_TSWW_us - elapsed_last_write); + } + uint8_t cmd[2]; cmd[0] = DIR_WRITE(reg); cmd[1] = data; - - if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) { - perf_count(_comms_errors); - } -} - -bool PAA3905::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries) -{ - for (int i = 0; i < retries; i++) { - uint8_t cmd[2]; - cmd[0] = DIR_WRITE(reg); - cmd[1] = data; - transfer(&cmd[0], nullptr, sizeof(cmd)); - px4_udelay(TIME_us_TSWW); - - // read back to verify - uint8_t data_read = RegisterRead(reg); - - if (data_read == data) { - return true; - } - - PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read); - } - - perf_count(_register_write_fail_perf); - - return false; + transfer(&cmd[0], nullptr, sizeof(cmd)); + hrt_store_absolute_time(&_last_write_time); } void PAA3905::RunImpl() { - // backup schedule - if (_data_ready_interrupt_enabled) { - ScheduleDelayed(_scheduled_interval_us * 2); - } + perf_begin(_cycle_perf); + perf_count(_interval_perf); - // force reset if there hasn't been valid data for an extended period (sensor could be in a bad state) - static constexpr hrt_abstime RESET_TIMEOUT_US = 5_s; + const hrt_abstime now = hrt_absolute_time(); - if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) { + // force reconfigure if we haven't received valid data for quite some time + if ((now > _last_good_data + RESET_TIMEOUT_US) && (now > _last_reset + RESET_TIMEOUT_US)) { Configure(); + perf_end(_cycle_perf); return; } - perf_begin(_sample_perf); - perf_count(_interval_perf); + hrt_abstime timestamp_sample = now; + + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if (now < drdy_timestamp_sample + _scheduled_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_no_motion_interrupt_perf); + } + + // push backup schedule back + ScheduleDelayed(500_ms); + } struct TransferBuffer { uint8_t cmd = Register::Motion_Burst; @@ -444,37 +484,52 @@ void PAA3905::RunImpl() } buf{}; static_assert(sizeof(buf) == (14 + 1)); - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) { - perf_count(_comms_errors); - perf_end(_sample_perf); + if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != 0) { + perf_end(_cycle_perf); return; } - perf_end(_sample_perf); - - const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp; - - // update for next iteration - _previous_collect_timestamp = timestamp_sample; + hrt_store_absolute_time(&_last_read_time); if (_discard_reading > 0) { _discard_reading--; - ResetAccumulatedData(); - _valid_count = 0; + perf_end(_cycle_perf); return; } - CheckMode(); // update _mode variable + // Bit [5:0] check if chip is working correctly + // 0x3F: chip is working correctly + if ((buf.data.Observation & (Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0)) != 0x3F) { + // Other value: recommend to issue a software reset + Configure(); + perf_end(_cycle_perf); + return; + } + + if (UpdateMode(buf.data.Observation)) { + // update scheduling if mode changed + if (!_data_ready_interrupt_enabled) { + ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); + } + } // check SQUAL & Shutter values // To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition // Bright Mode, SQUAL < 0x19, Shutter ≥ 0x00FF80 // Low Light Mode, SQUAL < 0x46, Shutter ≥ 0x00FF80 // Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998 - const uint32_t shutter = (buf.data.Shutter_Upper << 16) | (buf.data.Shutter_Middle << 8) | buf.data.Shutter_Lower; + // 23-bit Shutter register + const uint8_t Shutter_Lower = buf.data.Shutter_Lower; + const uint8_t Shutter_Middle = buf.data.Shutter_Middle; + const uint8_t Shutter_Upper = buf.data.Shutter_Upper & (Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0); + + const uint32_t shutter = (Shutter_Upper << 16) | (Shutter_Middle << 8) | Shutter_Lower; + + // Motion since last report and Surface quality non-zero + const bool motion_detected = buf.data.Motion & Motion_Bit::MotionOccurred; + + // Number of Features = SQUAL * 4 bool data_valid = (buf.data.SQUAL > 0); switch (_mode) { @@ -513,96 +568,66 @@ void PAA3905::RunImpl() } if (data_valid) { - const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L); - const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L); + // publish sensor_optical_flow + sensor_optical_flow_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); - _flow_dt_sum_usec += dt_flow; - _flow_sum_x += delta_x_raw; - _flow_sum_y += delta_y_raw; - _flow_sample_counter++; - _flow_quality_sum += buf.data.SQUAL; + report.integration_timespan_us = _scheduled_interval_us; + report.quality = buf.data.SQUAL; - _valid_count++; + // set specs according to datasheet + report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s + report.min_ground_distance = 0.08f; // Datasheet: 80mm + report.max_ground_distance = INFINITY; // Datasheet: infinity - } else { - _valid_count = 0; - ResetAccumulatedData(); - return; + switch (_mode) { + case Mode::Bright: + report.mode = sensor_optical_flow_s::MODE_BRIGHT; + break; + + case Mode::LowLight: + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + break; + + case Mode::SuperLowLight: + report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; + break; + } + + if (motion_detected) { + // only populate flow if data valid (motion and quality > 0) + const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L); + const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L); + + // rotate measurements in yaw from sensor frame to body frame + const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; + + report.pixel_flow[0] = pixel_flow_rotated(0) / 500.0f; // proportional factor + convert from pixels to radians + report.pixel_flow[1] = pixel_flow_rotated(1) / 500.0f; // proportional factor + convert from pixels to radians + } + + report.timestamp = hrt_absolute_time(); + _sensor_optical_flow_pub.publish(report); + + if (report.quality >= 1) { + _last_good_data = report.timestamp_sample; + } } - // returns if the collect time has not been reached - if (_flow_dt_sum_usec < COLLECT_TIME) { - return; - } - - sensor_optical_flow_s report{}; - report.timestamp_sample = timestamp_sample; - report.device_id = get_device_id(); - - float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f; // proportional factor + convert from pixels to radians - float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f; // proportional factor + convert from pixels to radians - - // rotate measurements in yaw from sensor frame to body frame - const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f}; - report.pixel_flow[0] = pixel_flow_rotated(0); - report.pixel_flow[1] = pixel_flow_rotated(1); - - report.integration_timespan_us = _flow_dt_sum_usec; // microseconds - - report.quality = _flow_quality_sum / _flow_sample_counter; - - // No gyro on this board - report.delta_angle[0] = NAN; - report.delta_angle[1] = NAN; - report.delta_angle[2] = NAN; - - // set (conservative) specs according to datasheet - report.max_flow_rate = 7.4f; // Datasheet: 7.4 rad/s - report.min_ground_distance = 0.08f; // Datasheet: 80mm - report.max_ground_distance = 30.0f; // Datasheet: infinity - - - switch (_mode) { - case Mode::Bright: - report.mode = sensor_optical_flow_s::MODE_BRIGHT; - break; - - case Mode::LowLight: - report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; - break; - - case Mode::SuperLowLight: - report.mode = sensor_optical_flow_s::MODE_SUPER_LOWLIGHT; - break; - } - - report.timestamp = hrt_absolute_time(); - _sensor_optical_flow_pub.publish(report); - - if (report.quality > 10) { - _last_good_publish = report.timestamp; - } - - ResetAccumulatedData(); -} - -void PAA3905::ResetAccumulatedData() -{ - // reset - _flow_dt_sum_usec = 0; - _flow_sum_x = 0; - _flow_sum_y = 0; - _flow_sample_counter = 0; - _flow_quality_sum = 0; + perf_end(_cycle_perf); } void PAA3905::print_status() { I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); + perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - perf_print_counter(_comms_errors); + perf_print_counter(_reset_perf); perf_print_counter(_false_motion_perf); - perf_print_counter(_register_write_fail_perf); + perf_print_counter(_mode_change_bright_perf); + perf_print_counter(_mode_change_low_light_perf); + perf_print_counter(_mode_change_super_low_light_perf); + perf_print_counter(_no_motion_interrupt_perf); } diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index 8235e923d3..337ed19659 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file paa3905.hpp + * @file PAA3905.hpp * - * Driver for the Pixart paa3905 optical flow sensors connected via SPI. + * Driver for the PAA3905E1-Q: Optical Motion Tracking Chip */ #pragma once @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -56,8 +55,8 @@ using namespace time_literals; using namespace PixArt_PAA3905; -#define DIR_WRITE(a) ((a) | (1 << 7)) -#define DIR_READ(a) ((a) & 0x7f) +#define DIR_WRITE(a) ((a) | Bit7) +#define DIR_READ(a) ((a) & 0x7F) class PAA3905 : public device::SPI, public I2CSPIDriver { @@ -78,59 +77,61 @@ private: int probe() override; + void Reset(); + static int DataReadyInterruptCallback(int irq, void *context, void *arg); void DataReady(); bool DataReadyInterruptConfigure(); bool DataReadyInterruptDisable(); - uint8_t RegisterRead(uint8_t reg, int retries = 2); + uint8_t RegisterRead(uint8_t reg); void RegisterWrite(uint8_t reg, uint8_t data); - bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1); - - void EnableLed(); - - void StandardDetectionSetting(); - void EnhancedDetectionMode(); - void ModeAuto012(); - - void CheckMode(); void Configure(); - void ResetAccumulatedData(); + void ConfigureAutomaticModeSwitching(); + + void ConfigureModeBright(); + void ConfigureModeLowLight(); + void ConfigureModeSuperLowLight(); + + void ConfigureStandardDetectionSetting(); + void ConfigureEnhancedDetectionMode(); + + void EnableLed(); + + bool UpdateMode(const uint8_t observation); uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; - perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; - perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; - perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; - perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")}; - perf_counter_t _register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")}; - - static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")}; + perf_counter_t _mode_change_bright_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change bright (0)")}; + perf_counter_t _mode_change_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change low light (1)")}; + perf_counter_t _mode_change_super_low_light_perf{perf_alloc(PC_COUNT, MODULE_NAME": mode change super low light (2)")}; + perf_counter_t _no_motion_interrupt_perf{nullptr}; const spi_drdy_gpio_t _drdy_gpio; - uint64_t _previous_collect_timestamp{0}; - uint64_t _flow_dt_sum_usec{0}; - uint8_t _flow_sample_counter{0}; - uint16_t _flow_quality_sum{0}; + matrix::Dcmf _rotation; - matrix::Dcmf _rotation; + int _discard_reading{3}; - int _discard_reading{3}; + Mode _mode{Mode::LowLight}; - int _flow_sum_x{0}; - int _flow_sum_y{0}; - - Mode _mode{Mode::LowLight}; - - uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1}; - - int _valid_count{0}; + uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; - hrt_abstime _last_good_publish{0}; + hrt_abstime _last_write_time{0}; + hrt_abstime _last_read_time{0}; + + // force reset if there hasn't been valid data for an extended period (sensor could be in a bad state) + static constexpr hrt_abstime RESET_TIMEOUT_US = 3_s; + + hrt_abstime _last_good_data{0}; hrt_abstime _last_reset{0}; }; diff --git a/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp index 8eb5a99602..de67e0cf4e 100644 --- a/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp +++ b/src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp @@ -33,8 +33,8 @@ #pragma once -namespace PixArt_PAA3905 -{ +#include + // TODO: move to a central header static constexpr uint8_t Bit0 = (1 << 0); static constexpr uint8_t Bit1 = (1 << 1); @@ -45,19 +45,24 @@ static constexpr uint8_t Bit5 = (1 << 5); static constexpr uint8_t Bit6 = (1 << 6); static constexpr uint8_t Bit7 = (1 << 7); -static constexpr uint8_t PRODUCT_ID = 0xA2; -static constexpr uint8_t REVISION_ID = 0x00; +namespace PixArt_PAA3905 +{ + +static constexpr uint8_t PRODUCT_ID = 0xA2; +static constexpr uint8_t REVISION_ID = 0x00; static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D; -static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps -static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps -static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126}; // 126 fps +static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50}; // 50 fps static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface -// Various time delay needed for paa3905 -static constexpr uint32_t TIME_us_TSWW = 11; // actually 10.5us -static constexpr uint32_t TIME_us_TSRAD = 2; +// Various time delays +static constexpr uint32_t TIME_TSWW_us = 11; // SPI Time Between Write Commands (actually 10.5us) +static constexpr uint32_t TIME_TSWR_us = 6; // SPI Time Between Write and Read Commands +static constexpr uint32_t TIME_TSRW_TSRR_us = 2; // SPI Time Between Read And Subsequent Commands (actually 1.5us) +static constexpr uint32_t TIME_TSRAD_us = 2; // SPI Read Address-Data Delay enum Register : uint8_t { Product_ID = 0x00, @@ -86,9 +91,20 @@ enum Register : uint8_t { Inverse_Product_ID = 0x5F, }; -// Observation +enum Motion_Bit : uint8_t { + MotionOccurred = Bit7, // Motion since last report + + ChallengingSurface = Bit0, // Challenging surface is detected +}; + enum Observation_Bit : uint8_t { - Reset = 0x5A, + // Bit [7:6] + AMS_mode_0 = 0, + AMS_mode_1 = Bit6, + AMS_mode_2 = Bit7, + + // Bit [5:0] + WorkingCorrectly = 0x3F, }; enum class Mode { diff --git a/src/drivers/optical_flow/paa3905/paa3905_main.cpp b/src/drivers/optical_flow/paa3905/paa3905_main.cpp index 6c5e16b2b3..ca43b70b7b 100644 --- a/src/drivers/optical_flow/paa3905/paa3905_main.cpp +++ b/src/drivers/optical_flow/paa3905/paa3905_main.cpp @@ -49,7 +49,7 @@ extern "C" __EXPORT int paa3905_main(int argc, char *argv[]) using ThisDriver = PAA3905; BusCLIArguments cli{false, true}; cli.custom1 = -1; - cli.spi_mode = SPIDEV_MODE0; + cli.spi_mode = SPIDEV_MODE3; cli.default_spi_frequency = SPI_SPEED; while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) { diff --git a/src/drivers/optical_flow/paa3905/parameters.c b/src/drivers/optical_flow/paa3905/parameters.c index 9aa2da7c63..2646721d94 100644 --- a/src/drivers/optical_flow/paa3905/parameters.c +++ b/src/drivers/optical_flow/paa3905/parameters.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * paa3905 Optical Flow + * PAA3905 Optical Flow * * @reboot_required true * From 5d2dfadb0e2e0c0a8b19b1ef1a3f566da771e934 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jun 2022 21:31:27 -0400 Subject: [PATCH 17/19] boards: px4_fmu-v5_uavcanv0periph disable modules to save flash --- boards/px4/fmu-v5/uavcanv0periph.px4board | 2 ++ 1 file changed, 2 insertions(+) diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index 9371b61b9d..f7565e20c9 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -22,6 +22,7 @@ CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIH=n +CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_SYSTEMCMDS_BL_UPDATE=n @@ -31,6 +32,7 @@ CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_SYSTEMCMDS_SD_BENCH=n CONFIG_SYSTEMCMDS_SD_STRESS=n CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_SYSTEMCMDS_SYSTEM_TIME=n CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n CONFIG_BOARD_UAVCAN_PERIPHERALS="cuav_can-gps-v1_default" CONFIG_DRIVERS_BAROMETER_MS5611=y From 15747239c1a9708d7d3766aa041a779062ad4e15 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jun 2022 18:09:20 -0400 Subject: [PATCH 18/19] mc_pos_control: always respect position estimate vxy_max if set Co-authored-by: Matthias Grob --- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 47e1aecf2d..af69de733a 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -489,8 +489,14 @@ void MulticopterPositionControl::Run() _control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get()); + float max_speed_xy = _param_mpc_xy_vel_max.get(); + + if (PX4_ISFINITE(vehicle_local_position.vxy_max)) { + max_speed_xy = math::min(max_speed_xy, vehicle_local_position.vxy_max); + } + _control.setVelocityLimits( - _param_mpc_xy_vel_max.get(), + max_speed_xy, math::min(speed_up, _param_mpc_z_vel_max_up.get()), // takeoff ramp starts with negative velocity limit math::max(speed_down, 0.f)); From dc8ed978097b6efed46719cdc11bec0c6e914c4e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jun 2022 18:10:02 -0400 Subject: [PATCH 19/19] ekf2: optical flow control limits constrain speed using HAGL max --- src/modules/ekf2/EKF/ekf_helper.cpp | 43 +++++++++++----------- src/modules/ekf2/EKF/estimator_interface.h | 4 +- 2 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5c54dc36c8..a8a7a08a2e 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -838,43 +838,42 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed */ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const { - // Calculate range finder limits - const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); - // Allow use of 75% of rangefinder maximum range to allow for angular motion - const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal(); - - // Calculate optical flow limits - // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f); - const float flow_hagl_min = _flow_min_distance; - const float flow_hagl_max = _flow_max_distance; - - // TODO : calculate visual odometry limits - - const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid; - - const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); - // Do not require limiting by default *vxy_max = NAN; *vz_max = NAN; *hagl_min = NAN; *hagl_max = NAN; + // Calculate range finder limits + const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); + + // Allow use of 75% of rangefinder maximum range to allow for angular motion + const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal(); + + // TODO : calculate visual odometry limits + const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid; + const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); + // Keep within range sensor limit when using rangefinder as primary height source if (relying_on_rangefinder) { - *vxy_max = NAN; - *vz_max = NAN; *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max; } // Keep within flow AND range sensor limits when exclusively using optical flow if (relying_on_optical_flow) { + // Calculate optical flow limits + const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance); + const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance); + + const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max); + + // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion + const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + *vxy_max = flow_vxy_max; - *vz_max = NAN; - *hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min); - *hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max); + *hagl_min = flow_hagl_min; + *hagl_max = flow_hagl_max; } } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 7144ee60fe..6693c2662e 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -303,9 +303,9 @@ protected: float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) // Sensor limitations - float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) + float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m) - float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m) + float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m) // Output Predictor outputSample _output_new{}; // filter output on the non-delayed time horizon