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