From c182a46ba68610ca6ac69ca76e8e16357e5a004d Mon Sep 17 00:00:00 2001 From: neo Date: Tue, 28 May 2024 19:41:24 +0200 Subject: [PATCH] fts module dev --- CMakeLists.txt | 1 + boards/px4/fts-proto/default.px4board | 1 + boards/px4/fts-proto/init/rc.board_sensors | 1 + build_mfts.sh | 2 +- cmake/kconfig.cmake | 1 - msg/CMakeLists.txt | 2 +- msg/{FtsStatus.msg => FtsState.msg} | 3 + src/drivers/fts/fts.cpp | 6 +- src/drivers/fts/fts.h | 4 +- src/modules/fts/main_state_machine.cpp | 89 ------------------- .../{fts => fts_control}/CMakeLists.txt | 4 +- src/modules/fts_control/Kconfig | 5 ++ .../{fts => fts_control}/fts_controller.cpp | 22 ++--- .../{fts => fts_control}/fts_controller.hpp | 21 +---- .../fts_controller_params.c | 0 .../fts_control/main_state_machine.cpp | 63 +++++++++++++ .../main_state_machine.hpp | 24 ++--- src/modules/mavlink/mavlink | 2 +- 18 files changed, 109 insertions(+), 142 deletions(-) rename msg/{FtsStatus.msg => FtsState.msg} (52%) delete mode 100644 src/modules/fts/main_state_machine.cpp rename src/modules/{fts => fts_control}/CMakeLists.txt (66%) create mode 100644 src/modules/fts_control/Kconfig rename src/modules/{fts => fts_control}/fts_controller.cpp (84%) rename src/modules/{fts => fts_control}/fts_controller.hpp (65%) rename src/modules/{fts => fts_control}/fts_controller_params.c (100%) create mode 100644 src/modules/fts_control/main_state_machine.cpp rename src/modules/{fts => fts_control}/main_state_machine.hpp (58%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 741e8e5..be7f52c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -426,6 +426,7 @@ if(EXISTS "${PX4_BOARD_DIR}/CMakeLists.txt") endif() foreach(module ${config_module_list}) + message(STATUS "Adding module ${module}") add_subdirectory(src/${module}) endforeach() diff --git a/boards/px4/fts-proto/default.px4board b/boards/px4/fts-proto/default.px4board index d988e28..f0240dc 100644 --- a/boards/px4/fts-proto/default.px4board +++ b/boards/px4/fts-proto/default.px4board @@ -10,6 +10,7 @@ CONFIG_COMMON_TELEMETRY=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_FTS_CONTROL=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAVLINK=y diff --git a/boards/px4/fts-proto/init/rc.board_sensors b/boards/px4/fts-proto/init/rc.board_sensors index f15cefa..2935cad 100644 --- a/boards/px4/fts-proto/init/rc.board_sensors +++ b/boards/px4/fts-proto/init/rc.board_sensors @@ -3,3 +3,4 @@ # PX4 FTS-CAN specific board sensors init #------------------------------------------------------------------------------ board_adc start +fts_control start diff --git a/build_mfts.sh b/build_mfts.sh index 6b3d8c7..078f5ec 100755 --- a/build_mfts.sh +++ b/build_mfts.sh @@ -5,4 +5,4 @@ rm -rf build/ git submodule update --recursive #make -j$(nproc) px4_fts-can #make -j$(nproc) px4_fts-proto -make px4_fts-proto \ No newline at end of file +make px4_fts-proto_default \ No newline at end of file diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index ea5e488..6dfae4d 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -159,7 +159,6 @@ if(EXISTS ${BOARD_DEFCONFIG}) # Find variable name string(REGEX MATCH "^CONFIG_MODULES[^=]+" Modules ${NameAndValue}) - if(Modules) # Find the value string(REPLACE "${Name}=" "" Value ${NameAndValue}) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index d7ff2e0..268be8c 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -88,7 +88,7 @@ set(msg_files FollowTarget.msg FollowTargetEstimator.msg FollowTargetStatus.msg - FtsStatus.msg + FtsState.msg GeneratorStatus.msg GeofenceResult.msg GimbalDeviceAttitudeStatus.msg diff --git a/msg/FtsStatus.msg b/msg/FtsState.msg similarity index 52% rename from msg/FtsStatus.msg rename to msg/FtsState.msg index f4e8662..a1afe23 100644 --- a/msg/FtsStatus.msg +++ b/msg/FtsState.msg @@ -1,5 +1,8 @@ uint64 timestamp # time since system start (microseconds) bool fts_on +uint8 ground_fts_state +uint8 auto_fts_state +uint8 main_fts_state diff --git a/src/drivers/fts/fts.cpp b/src/drivers/fts/fts.cpp index e7afb8b..788e532 100644 --- a/src/drivers/fts/fts.cpp +++ b/src/drivers/fts/fts.cpp @@ -33,7 +33,7 @@ FTS::FTS() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { - _fts_status_pub.advertise(); + _fts_state_pub.advertise(); } FTS::~FTS() @@ -107,10 +107,10 @@ void FTS::Run() void FTS::publish_status() { - fts_status_s status{}; + fts_state_s status{}; status.fts_on = _fts_on; status.timestamp = hrt_absolute_time(); - _fts_status_pub.publish(status); + _fts_state_pub.publish(status); } int FTS::start() diff --git a/src/drivers/fts/fts.h b/src/drivers/fts/fts.h index b94ab64..9cf82f8 100644 --- a/src/drivers/fts/fts.h +++ b/src/drivers/fts/fts.h @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include @@ -95,7 +95,7 @@ private: bool _fts_initialized = false; bool _fts_on = false; - uORB::Publication _fts_status_pub{ORB_ID(fts_status)}; + uORB::Publication _fts_state_pub{ORB_ID(fts_state)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/fts/main_state_machine.cpp b/src/modules/fts/main_state_machine.cpp deleted file mode 100644 index f3d4e88..0000000 --- a/src/modules/fts/main_state_machine.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include "main_state_machine.hpp" - -MainStateMachine::MainStateMachine() -{ -} - -uint8_t MainStateMachine::get_mainstatemachine_state() -{ -} - -void MainStateMachine::init_state_machine(uORB::Publication *main_fts_state_pub, - MainStateMachine::fts_state_s *FTS_state) -{ -} - -void MainStateMachine::send_cmd_ack(uint8_t cmd_result, uint8_t refuse_reason) -{ - if (!_ground_cmd_reply_pub) { - return; - } - - 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) -{ - if (!_fts_cmd_pub) { - PX4_ERR("FTS command publisher null"); - return false; - } - - // Limit the command send rate - if (hrt_elapsed_time(&_cmd_sent_time) < FTS_STATE_SEND_INTERVAL) { - return false; - } - - fts_control_cmd_s msg{}; - msg.timestamp = hrt_absolute_time(); - msg.command = cmd; - _cmd_sent_time = msg.timestamp; - _fts_cmd_pub->publish(msg); - - 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; -} - -bool MainStateMachine::check_fts_error() -{ - if (_fts_state->fts_state == fts_state_s::MAIN_STATE_ERROR) { - // FTS in an error state. Notice the server. - send_cmd_ack(fts_server_cmd_ack_s::SERVER_CMD_REFUSED, fts_server_cmd_ack_s::REFUSE_REASON_FTS_ERROR); - go_to_error_state(); - return true; - } - - return false; -} - -void MainStateMachine::run_state_machine() -{ - switch (MAIN_STATE_t(_current_node->node_index)) { - case IDLE: - break; - - case ERROR_STATE: - break; - - case END_OF_STATE: - break; - } -} diff --git a/src/modules/fts/CMakeLists.txt b/src/modules/fts_control/CMakeLists.txt similarity index 66% rename from src/modules/fts/CMakeLists.txt rename to src/modules/fts_control/CMakeLists.txt index 5974b00..c64a28a 100644 --- a/src/modules/fts/CMakeLists.txt +++ b/src/modules/fts_control/CMakeLists.txt @@ -1,9 +1,7 @@ px4_add_module( - MODULE modules__fts + MODULE modules__fts_control MAIN fts_controller SRCS fts_controller.cpp main_state_machine.cpp - DEPENDS - px4_work_queue ) diff --git a/src/modules/fts_control/Kconfig b/src/modules/fts_control/Kconfig new file mode 100644 index 0000000..a64a2eb --- /dev/null +++ b/src/modules/fts_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_FTS_CONTROL + bool "fts_control" + default n + ---help--- + Enable support for fts_control diff --git a/src/modules/fts/fts_controller.cpp b/src/modules/fts_control/fts_controller.cpp similarity index 84% rename from src/modules/fts/fts_controller.cpp rename to src/modules/fts_control/fts_controller.cpp index fe0f43a..4219cd5 100644 --- a/src/modules/fts/fts_controller.cpp +++ b/src/modules/fts_control/fts_controller.cpp @@ -31,7 +31,7 @@ FTSController::init() return false; } - _main_state_machine.init_state_machine(&_fts_ground_cmd_reply_pub, &_fts_control_cmd_pub, &_fts_state); + //_main_state_machine.init_state_machine(&_fts_ground_cmd_reply_pub, &_fts_control_cmd_pub, &_fts_state); // run every 50 ms (20 Hz) ScheduleOnInterval(50_ms); return true; @@ -55,17 +55,14 @@ FTSController::Run() // Update FTS comm status update_status(); - // Update server command - update_server_cmd(); - // Excecute the state machine _main_state_machine.run_state_machine(); // Publish FTS controller state - _fts_controller_state.timestamp = hrt_absolute_time(); - _fts_controller_state.fts_comm_timeout = _fts_state.fts_comm_timeout; - _fts_controller_state.fts_state = _fts_state.fts_state; - _fts_controller_state_pub.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); } @@ -74,7 +71,6 @@ void FTSController::update_status() { // Update FTS state - _fts_state.fts_state = _fts_status_sub.get().main_state; // Update GROUND-FTS communication state _fts_state.gfts_comm_timeout = hrt_elapsed_time(&_last_fts_state_update_time) >= _fts_comm_timeout_threshold_us; @@ -83,7 +79,7 @@ void FTSController::update_status() } -void FTSController::send_fts_status() +void FTSController::send_fts_state() { } @@ -91,8 +87,8 @@ void FTSController::send_fts_status() void FTSController::update_topics() { - if (_fts_status_sub.update()) { - _last_fts_state_update_time = _fts_status_sub.get().timestamp; + if (_fts_state_sub.update()) { + _last_fts_state_update_time = _fts_state_sub.get().timestamp; } } @@ -176,7 +172,7 @@ FTSController::print_status() 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.fts_state); + PX4_INFO("Current FTS state:%u", _fts_state.main_fts_state); return 0; } diff --git a/src/modules/fts/fts_controller.hpp b/src/modules/fts_control/fts_controller.hpp similarity index 65% rename from src/modules/fts/fts_controller.hpp rename to src/modules/fts_control/fts_controller.hpp index 3ccc2b6..6d473ce 100644 --- a/src/modules/fts/fts_controller.hpp +++ b/src/modules/fts_control/fts_controller.hpp @@ -14,13 +14,9 @@ #include #include #include -#include -#include #include -#include #include #include -#include #include "main_state_machine.hpp" @@ -49,35 +45,26 @@ public: private: void Run() override; - void update_server_cmd(); + void send_fts_state(); void update_topics(); void update_status(); bool update_param(); + void fire(); + void reset(); perf_counter_t _cycle_perf; perf_counter_t _interval_perf; uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::SubscriptionData _fts_status_sub{ORB_ID(fts_state)}; - uORB::SubscriptionData _fts_server_cmd_sub{ORB_ID(fts_server_cmd)}; + uORB::SubscriptionData _fts_state_sub{ORB_ID(fts_state)}; uORB::SubscriptionData _home_position_sub{ORB_ID(home_position)}; uORB::SubscriptionData _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::SubscriptionData _battery_status_sub{ORB_ID(battery_status)}; - uORB::Publication _fts_ground_cmd_reply_pub{ORB_ID(fts_server_cmd_ack)}; - uORB::Publication _fts_control_cmd_pub{ORB_ID(fts_control_cmd)}; - uORB::Publication _fts_controller_state_pub{ORB_ID(fts_controller_state)}; - fts_controller_state_s _fts_controller_state{0}; hrt_abstime _last_fts_state_update_time{0}; MainStateMachine _main_state_machine; MainStateMachine::fts_state_s _fts_state{}; - bool _new_server_cmd_arrive{false}; - - float _minimum_batt_capacity{.5f}; - float _minimum_vehicle_altitude_before_close_box{10.0f}; - uint64_t _fts_comm_timeout_threshold_us{10_s}; // Currently the box emits state at 3 Hz }; diff --git a/src/modules/fts/fts_controller_params.c b/src/modules/fts_control/fts_controller_params.c similarity index 100% rename from src/modules/fts/fts_controller_params.c rename to src/modules/fts_control/fts_controller_params.c diff --git a/src/modules/fts_control/main_state_machine.cpp b/src/modules/fts_control/main_state_machine.cpp new file mode 100644 index 0000000..00a8afa --- /dev/null +++ b/src/modules/fts_control/main_state_machine.cpp @@ -0,0 +1,63 @@ +#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/main_state_machine.hpp b/src/modules/fts_control/main_state_machine.hpp similarity index 58% rename from src/modules/fts/main_state_machine.hpp rename to src/modules/fts_control/main_state_machine.hpp index 57346f3..f755e51 100644 --- a/src/modules/fts/main_state_machine.hpp +++ b/src/modules/fts_control/main_state_machine.hpp @@ -1,37 +1,39 @@ -// -// Created by tianfang on 06/05/22. -// #ifndef PX4_MAIN_STATE_MACHINE_HPP #define PX4_MAIN_STATE_MACHINE_HPP +#define FTS_PING_TIMEOUT 1000 +#include #include #include -#include #include using namespace time_literals; -class MainStateMachine : public StateMachineHelper +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{ground_fts_state_s::MAIN_STATE_UNKNOWN}; - uint8_t auto_fts_state{auto_fts_state_s::MAIN_STATE_UNKNOWN}; - uint8_t main_fts_state{main_fts_state_s::MAIN_STATE_IDLE}; + 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 *main_fts_state_pub, + void init_state_machine(uORB::Publication *fts_state_pub, MainStateMachine::fts_state_s *FTS_state); - void run_state_machine() override; + 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}; diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 18955a0..4474134 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 18955a04c7c7467e00ea42b704addb4a9c12b53a +Subproject commit 4474134e1f9f2f8bc228b6751268294ee3aa84ed