From 981936ab3f6b2dabeac13584d173efe86341872b Mon Sep 17 00:00:00 2001 From: neo Date: Sat, 8 Jun 2024 18:58:04 +0200 Subject: [PATCH] mfts + afts basic geofence demo working ok --- .../init.d/airframes/0001_main_fts | 3 +- .../init.d/airframes/0002_auto_fts | 7 +++ boards/px4/fts-can/nuttx-config/nsh/defconfig | 3 - boards/px4/fts-can/src/spi.cpp | 16 ----- src/drivers/fts/fts.cpp | 43 +++++++------ src/drivers/fts/fts.h | 10 +-- src/modules/fts_control/CMakeLists.txt | 1 - src/modules/fts_control/fts_control.cpp | 29 ++++----- src/modules/fts_control/fts_control.hpp | 15 ++--- .../fts_control/main_state_machine.cpp | 63 ------------------- .../fts_control/main_state_machine.hpp | 47 -------------- .../mavlink/streams/AUTO_FTS_MESSAGE.hpp | 12 ++-- src/modules/navigator/navigator_main.cpp | 25 +++++--- 13 files changed, 79 insertions(+), 195 deletions(-) delete mode 100644 src/modules/fts_control/main_state_machine.cpp delete mode 100644 src/modules/fts_control/main_state_machine.hpp diff --git a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts index f65ef20..ffbc93f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts +++ b/ROMFS/px4fmu_common/init.d/airframes/0001_main_fts @@ -23,4 +23,5 @@ param set-default FTS_SIM_MODE 0 param set-default MAV_TYPE 37 -fts start \ No newline at end of file +fts start +fts_controller start \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts b/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts index bdc996f..a5eefae 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts +++ b/ROMFS/px4fmu_common/init.d/airframes/0002_auto_fts @@ -22,3 +22,10 @@ # param set-default FTS_SIM_MODE 0 param set-default MAV_TYPE 37 +param set-default GF_ACTION 0 +param set-default GF_ALTMODE 0 +param set-default GF_COUNT -1 +param set-default GF_MAX_HOR_DIST 0.0000 +param set-default GF_MAX_VER_DIST 0.0000 +param set-default GF_PREDICT 0 +param set-default GF_SOURCE 1 diff --git a/boards/px4/fts-can/nuttx-config/nsh/defconfig b/boards/px4/fts-can/nuttx-config/nsh/defconfig index e2601c0..2892f6d 100644 --- a/boards/px4/fts-can/nuttx-config/nsh/defconfig +++ b/boards/px4/fts-can/nuttx-config/nsh/defconfig @@ -204,9 +204,6 @@ CONFIG_STM32H7_SAVE_CRASHDUMP=y CONFIG_STM32H7_SDMMC2=y CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y -CONFIG_STM32H7_SPI1=y -CONFIG_STM32H7_SPI1_DMA=y -CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 CONFIG_STM32H7_SPI2=y CONFIG_STM32H7_SPI2_DMA=y CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 diff --git a/boards/px4/fts-can/src/spi.cpp b/boards/px4/fts-can/src/spi.cpp index 1cf3ece..2b5876e 100644 --- a/boards/px4/fts-can/src/spi.cpp +++ b/boards/px4/fts-can/src/spi.cpp @@ -32,36 +32,20 @@ ****************************************************************************/ #include -#include #include constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { initSPIHWVersion(V6C00, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), - }, {GPIO::PortB, GPIO::Pin2}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) }), }), initSPIHWVersion(V6C01, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), - }, {GPIO::PortB, GPIO::Pin2}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) }), }), initSPIHWVersion(V6C21, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), - initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), - }, {GPIO::PortB, GPIO::Pin2}), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) }), diff --git a/src/drivers/fts/fts.cpp b/src/drivers/fts/fts.cpp index 1b07188..c550d09 100644 --- a/src/drivers/fts/fts.cpp +++ b/src/drivers/fts/fts.cpp @@ -7,6 +7,7 @@ #include #include #include +#include @@ -73,26 +74,6 @@ px4_arch_configgpio(GPIO_FTS_CAN_OUTPUT); px4_arch_configgpio(GPIO_FTS_CHUTE_OUTPUT); } -void FTS::fts_arm() -{ - FTS_ARM_NEGATED(false); -} - -void FTS::fts_disarm() -{ - FTS_ARM_NEGATED(true); -} - -void FTS::fts_off() -{ - FTS_TERMINATE_CAN(false); -} - -void FTS::fts_on() -{ - FTS_TERMINATE_CAN(true); -} - bool FTS::initialize_topics() { return true; @@ -171,3 +152,25 @@ extern "C" __EXPORT int fts_main(int argc, char *argv[]) { return FTS::main(argc, argv); } + +void FTS::fts_arm() +{ + FTS_ARM_NEGATED(false); +} + +void FTS::fts_disarm() +{ + FTS_ARM_NEGATED(true); +} + +void FTS::fts_off() +{ + FTS_TERMINATE_CAN(false); + FTS_TRIGGER_CHUTE(false); +} + +void FTS::fts_on() +{ + FTS_TERMINATE_CAN(true); + FTS_TRIGGER_CHUTE(true); +} diff --git a/src/drivers/fts/fts.h b/src/drivers/fts/fts.h index f0b0268..bbbc59b 100644 --- a/src/drivers/fts/fts.h +++ b/src/drivers/fts/fts.h @@ -58,16 +58,16 @@ public: */ int start(); + void fts_on(); + void fts_off(); + void fts_arm(); + void fts_disarm(); + private: /** Disables the fts */ void disable_fts(); - void fts_on(); - void fts_off(); - void fts_arm(); - void fts_disarm(); - void initialize(); /** Enables / configures the fts GPIO */ diff --git a/src/modules/fts_control/CMakeLists.txt b/src/modules/fts_control/CMakeLists.txt index a720bf5..6539304 100644 --- a/src/modules/fts_control/CMakeLists.txt +++ b/src/modules/fts_control/CMakeLists.txt @@ -3,5 +3,4 @@ px4_add_module( MAIN fts_controller SRCS fts_control.cpp - main_state_machine.cpp ) diff --git a/src/modules/fts_control/fts_control.cpp b/src/modules/fts_control/fts_control.cpp index 8ab24b5..8ebb243 100644 --- a/src/modules/fts_control/fts_control.cpp +++ b/src/modules/fts_control/fts_control.cpp @@ -31,7 +31,6 @@ FTSController::init() return false; } - //_main_state_machine.init_state_machine(&_fts_ground_cmd_reply_pub, &_fts_control_cmd_pub, &_fts_state); // run every 1s ScheduleOnInterval(1000_ms); return true; @@ -55,14 +54,18 @@ FTSController::Run() // Update FTS comm status update_status(); - // Excecute the state machine - _main_state_machine.run_state_machine(); + // Update AUTO-FTS status + if(_auto_fts_message_sub.updated()){ + _auto_fts_message_sub.copy(&_auto_fts_message); + if (_auto_fts_message.geofence_violated!=0){ + fire(); + }else{ + reset(); + } + } // Publish FTS controller state _fts_state.timestamp = hrt_absolute_time(); - _fts_state.afts_comm_timeout = 0; - _fts_state.main_fts_state = 0; - //_fts_state_pub.publish(_fts_state); perf_end(_cycle_perf); } @@ -72,11 +75,6 @@ void FTSController::update_status() // Update FTS state - // Update GROUND-FTS communication state - _fts_state.gfts_comm_timeout = hrt_elapsed_time(&_last_fts_state_update_time) >= _fts_comm_timeout_threshold_us; - // Update AUTO-FTS communication state - _fts_state.afts_comm_timeout = hrt_elapsed_time(&_last_fts_state_update_time) >= _fts_comm_timeout_threshold_us; - } void FTSController::send_fts_state() @@ -87,9 +85,7 @@ void FTSController::send_fts_state() void FTSController::update_topics() { - if (_fts_state_sub.update()) { - _last_fts_state_update_time = _fts_state_sub.get().timestamp; - } + _fts_state_pub.publish(_fts_state); } @@ -135,13 +131,13 @@ FTSController::task_spawn(int argc, char *argv[]) void FTSController::fire() { - px4_arch_gpiowrite(GPIO_OUTPUT|5, true); + fts_drv.fts_on(); } void FTSController::reset() { - px4_arch_gpiowrite(GPIO_OUTPUT|5, false); + fts_drv.fts_off(); } int @@ -166,7 +162,6 @@ FTSController::print_status() perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - PX4_INFO("Current state machine state:%u", _main_state_machine.get_mainstatemachine_state()); PX4_INFO("Current FTS state:%u", _fts_state.main_fts_state); return 0; diff --git a/src/modules/fts_control/fts_control.hpp b/src/modules/fts_control/fts_control.hpp index 6d473ce..577ab0d 100644 --- a/src/modules/fts_control/fts_control.hpp +++ b/src/modules/fts_control/fts_control.hpp @@ -14,12 +14,11 @@ #include #include #include +#include #include #include #include - -#include "main_state_machine.hpp" - +#include using namespace time_literals; class FTSController : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -56,15 +55,17 @@ private: perf_counter_t _interval_perf; uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::SubscriptionData _fts_state_sub{ORB_ID(fts_state)}; + uORB::SubscriptionData _auto_fts_message_sub{ORB_ID(auto_fts_message)}; uORB::SubscriptionData _home_position_sub{ORB_ID(home_position)}; uORB::SubscriptionData _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Publication _fts_state_pub{ORB_ID(fts_state)}; - hrt_abstime _last_fts_state_update_time{0}; + FTS fts_drv; + hrt_abstime _last_fts_state_update_time{0}; - MainStateMachine _main_state_machine; - MainStateMachine::fts_state_s _fts_state{}; + fts_state_s _fts_state{}; + auto_fts_message_s _auto_fts_message{}; uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz }; diff --git a/src/modules/fts_control/main_state_machine.cpp b/src/modules/fts_control/main_state_machine.cpp deleted file mode 100644 index 00a8afa..0000000 --- a/src/modules/fts_control/main_state_machine.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "main_state_machine.hpp" - -MainStateMachine::MainStateMachine() -{ -} - -uint8_t MainStateMachine::get_mainstatemachine_state() -{ - return 0; -} - -void MainStateMachine::init_state_machine(uORB::Publication *fts_state_pub, - MainStateMachine::fts_state_s *FTS_state) -{ -} - -void MainStateMachine::send_cmd_ack(uint8_t cmd_result) -{ - - //ground_fts_reply_s msg{}; - //msg.timestamp = hrt_absolute_time(); - //msg.cmd_result = cmd_result; - - //_ground_cmd_reply_pub->publish(msg); -} - -bool MainStateMachine::send_fts_cmd(uint8_t cmd) -{ - - - - return true; -} - -bool MainStateMachine::check_ground_fts_timeout() -{ - if (hrt_elapsed_time(&_last_mfts_ping_time) > FTS_PING_TIMEOUT) { - return true; - } - return false; -} -bool MainStateMachine::check_auto_fts_timeout() -{ - if (hrt_elapsed_time(&_last_afts_ping_time) > FTS_PING_TIMEOUT) { - return true; - } - return false; -} - - -void MainStateMachine::run_state_machine() -{ - switch (0) { - case 0: - break; - - case 1: - break; - - case 2: - break; - } -} diff --git a/src/modules/fts_control/main_state_machine.hpp b/src/modules/fts_control/main_state_machine.hpp deleted file mode 100644 index f755e51..0000000 --- a/src/modules/fts_control/main_state_machine.hpp +++ /dev/null @@ -1,47 +0,0 @@ - -#ifndef PX4_MAIN_STATE_MACHINE_HPP -#define PX4_MAIN_STATE_MACHINE_HPP - -#define FTS_PING_TIMEOUT 1000 -#include -#include -#include -#include - -using namespace time_literals; - -class MainStateMachine : public ModuleBase -{ - -public: - - struct fts_state_s { - uint64_t timestamp{0}; - bool gfts_comm_timeout{false}; - bool afts_comm_timeout{false}; - uint8_t ground_fts_state{0}; - uint8_t auto_fts_state{0}; - uint8_t main_fts_state{0}; - - }; - - MainStateMachine(); - void init_state_machine(uORB::Publication *fts_state_pub, - MainStateMachine::fts_state_s *FTS_state); - void run_state_machine(); - uint8_t get_mainstatemachine_state(); - void send_cmd_ack(uint8_t cmd_result); - bool send_fts_cmd(uint8_t cmd); - bool check_ground_fts_timeout(); - bool check_auto_fts_timeout(); -private: - - hrt_abstime _last_mfts_ping_time{0}; - hrt_abstime _last_afts_ping_time{0}; - uORB::Publication *_fts_state_pub{nullptr}; - - static constexpr uint64_t FTS_STATE_SEND_INTERVAL = 200_ms; // Limit the publication of the FTS state at 5 Hz -}; - - -#endif //PX4_MAIN_STATE_MACHINE_HPP diff --git a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp index 37cc991..b5e44d2 100644 --- a/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp +++ b/src/modules/mavlink/streams/AUTO_FTS_MESSAGE.hpp @@ -37,7 +37,12 @@ public: const char *get_name() const override { return get_name_static(); } uint16_t get_id() override { return get_id_static(); } - unsigned get_size() override + uORB::Subscription _geofence_sub{ORB_ID(geofence_result)}; + + hrt_abstime _last_reset_time{0}; + hrt_abstime _last_update_time{0}; + + unsigned get_size() override { return MAVLINK_MSG_ID_AUTO_FTS_MESSAGE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -96,11 +101,6 @@ private: msg.geofence_violated = 0; } - uORB::Subscription _geofence_sub{ORB_ID(geofence_result)}; - - hrt_abstime _last_reset_time{0}; - hrt_abstime _last_update_time{0}; - }; #endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cbd0bd8..e1fd791 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -923,27 +923,27 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) matrix::Vector2 fence_violation_test_point; geofence_violation_type_u gf_violation_type{}; - float test_point_bearing; - float test_point_distance; + float test_point_bearing = 0; + float test_point_distance = 0; float vertical_test_point_distance; if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); + //test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); _gf_breach_avoidance.setHorizontalVelocity(velocity_hor_abs); _gf_breach_avoidance.setClimbRate(-_local_pos.vz); - test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor(); + //test_point_distance = _gf_breach_avoidance.computeBrakingDistanceMultirotor(); vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor(); } else { - test_point_distance = 2.0f * get_loiter_radius(); + //test_point_distance = 2.0f * get_loiter_radius(); vertical_test_point_distance = 5.0f; if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) { - test_point_bearing = pos_ctrl_status.nav_bearing; + //test_point_bearing = pos_ctrl_status.nav_bearing; } else { - test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); + //test_point_bearing = atan2f(_local_pos.vy, _local_pos.vx); } } @@ -962,8 +962,15 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data) fence_violation_test_point = _gf_breach_avoidance.getFenceViolationTestPoint(); } else { - fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon); - vertical_test_point_distance = 0; + //fence_violation_test_point = matrix::Vector2d(_global_pos.lat, _global_pos.lon); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + fence_violation_test_point = matrix::Vector2d(_gps_pos.lat, _gps_pos.lon); + + } else { + fence_violation_test_point = matrix::Vector2d(_gps_pos.lat * 1.0e-7, _gps_pos.lon * 1.0e-7); + } + + vertical_test_point_distance = 0; } gf_violation_type.flags.dist_to_home_exceeded = !_geofence.isCloserThanMaxDistToHome(fence_violation_test_point(0),