diff --git a/.gitmodules b/.gitmodules index 76c72ed..6ecc529 100644 --- a/.gitmodules +++ b/.gitmodules @@ -62,6 +62,3 @@ path = src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client url = https://github.com/PX4/Micro-XRCE-DDS-Client.git branch = px4 -[submodule "src/modules/mavlink/mavlink_nimbus"] - path = src/modules/mavlink/mavlink_nimbus - url = git@bitbucket.org:nimbussrl/mavlinknimbusc.git diff --git a/boards/px4/fts-proto/src/board_config.h b/boards/px4/fts-proto/src/board_config.h index d69d253..edea3c2 100644 --- a/boards/px4/fts-proto/src/board_config.h +++ b/boards/px4/fts-proto/src/board_config.h @@ -149,6 +149,11 @@ #define V6C11 HW_VER_REV(0x1,0x1) // NO PX4IO, Rev 1 I2C4 Internal I2C2 External #define V6C21 HW_VER_REV(0x2,0x1) // FMUV6CMini, Rev 1 I2C4 Internal I2C2 External +/*FTS*/ +#define GPIO_FTS_CAN_OUTPUT /* PB9 FTS CAN CUT */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_FTS_CHUTE_OUTPUT /* PB9 FTS CHUTE FIRE */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) +#define FTS_TERMINATE_CAN(on_true) px4_arch_gpiowrite(GPIO_FTS_CAN_OUTPUT, (on_true)) +#define FTS_TRIGGER_CHUTE(on_true) px4_arch_gpiowrite(GPIO_FTS_CHUTE_OUTPUT, (on_true)) /* HEATER * PWM in future @@ -266,6 +271,8 @@ GPIO_VDD_5V_HIPOWER_nOC, \ GPIO_VDD_3V3_SENSORS_EN, \ GPIO_TONE_ALARM_IDLE, \ + GPIO_FTS_CAN_OUTPUT, \ + GPIO_FTS_CHUTE_OUTPUT, \ } #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/build_afts.sh b/build_afts.sh index 0b5b6af..4c85a91 100755 --- a/build_afts.sh +++ b/build_afts.sh @@ -5,4 +5,5 @@ git submodule update --recursive #make px4_fmu-v6x_default #make px4_fmu-v6c_default #make px4_fmu-v6c_bootloader -make -j${nproc} px4_fts-can \ No newline at end of file +#make -j$(nproc) px4_fts-can +make px4_fts-can \ No newline at end of file diff --git a/build_mfts.sh b/build_mfts.sh index 312bac6..6b3d8c7 100755 --- a/build_mfts.sh +++ b/build_mfts.sh @@ -3,5 +3,6 @@ rm -rf build/ git submodule update --recursive -#make -j${nproc} px4_fts-can -make -j${nproc} px4_fts-proto \ No newline at end of file +#make -j$(nproc) px4_fts-can +#make -j$(nproc) px4_fts-proto +make px4_fts-proto \ No newline at end of file diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 30e4e3d..7881e11 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -88,6 +88,7 @@ set(msg_files FollowTarget.msg FollowTargetEstimator.msg FollowTargetStatus.msg + FTSStatus.msg GeneratorStatus.msg GeofenceResult.msg GimbalDeviceAttitudeStatus.msg diff --git a/msg/FTSStatus.msg b/msg/FTSStatus.msg new file mode 100644 index 0000000..f4e8662 --- /dev/null +++ b/msg/FTSStatus.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +bool fts_on + + diff --git a/src/drivers/fts/CMakeLists.txt b/src/drivers/fts/CMakeLists.txt new file mode 100644 index 0000000..ed50136 --- /dev/null +++ b/src/drivers/fts/CMakeLists.txt @@ -0,0 +1,7 @@ +px4_add_module( + MODULE drivers__fts + MAIN fts + COMPILE_FLAGS + SRCS + fts.cpp + ) diff --git a/src/drivers/fts/Kconfig b/src/drivers/fts/Kconfig new file mode 100644 index 0000000..f1e36c8 --- /dev/null +++ b/src/drivers/fts/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_FTS + bool "fts" + default y + ---help--- + Enable support for fts \ No newline at end of file diff --git a/src/drivers/fts/fts.cpp b/src/drivers/fts/fts.cpp new file mode 100644 index 0000000..0443d88 --- /dev/null +++ b/src/drivers/fts/fts.cpp @@ -0,0 +1,236 @@ +/** + * @file fts.cpp + */ + +#include "fts.h" + +#include +#include +#include + +#define IO_HEATER_DEVICE_PATH "/dev/px4io" + +#define _IO_HEATER_BASE (0x2e00) + +#define PX4IO_HEATER_CONTROL _PX4_IOC(_IO_HEATER_BASE, 0) + +/* ... to IOX_SET_VALUE + 8 */ + +/* enum passed to IO_HEATER_CONTROL ioctl()*/ +enum IO_HEATER_MODE { + HEATER_MODE_OFF = 0, + HEATER_MODE_ON = 1, + HEATER_MODE_DISABLED = -1 +}; + +// Use direct calls to turn GPIO pin on/off +# if not defined(GPIO_FTS_CAN_OUTPUT) || if not defined(GPIO_FTS_CHUTE_OUTPUT) +# error "To use the fts driver, the board_config.h must define and initialize GPIO_FTS_CAN_OUTPUT and GPIO_FTS_CHUTE_OUTPUT " +# endif + +FTS::FTS() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + + _fts_status_pub.advertise(); +} + +FTS::~FTS() +{ + disable_fts(); +} + +int FTS::custom_command(int argc, char *argv[]) +{ + // Check if the driver is running. + if (!is_running()) { + PX4_INFO("not running"); + return PX4_ERROR; + } + + return print_usage("Unrecognized command."); +} + +void FTS::disable_fts() +{ + // Reset fts to off state. + px4_arch_configgpio(GPIO_FTS_CAN_OUTPUT); + px4_arch_configgpio(GPIO_FTS_CHUTE_OUTPUT); +} + +void FTS::initialize_fts_io() +{ + // Initialize fts to off state. + +px4_arch_configgpio(GPIO_FTS_CAN_OUTPUT); +px4_arch_configgpio(GPIO_FTS_CHUTE_OUTPUT); +} + +void FTS::fts_off() +{ + FTS_TERMINATE_CAN(false); +} + +void FTS::fts_on() +{ + FTS_TERMINATE_CAN(true); +} + +bool FTS::initialize_topics() +{ + for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; + + if (sensor_accel_sub.get().timestamp != 0 && + sensor_accel_sub.get().device_id != 0 && + PX4_ISFINITE(sensor_accel_sub.get().temperature)) { + + // If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance. + if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) { + _sensor_accel_sub.ChangeInstance(i); + _sensor_device_id = sensor_accel_sub.get().device_id; + initialize_fts_io(); + return true; + } + } + } + + return false; +} + +void FTS::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + update_params(); + + if (_sensor_device_id == 0) { + if (!initialize_topics()) { + // if sensor still not found try again in 1 second + ScheduleDelayed(1_s); + return; + } + } + + sensor_accel_s sensor_accel; + float temperature_delta {0.f}; + + if (_fts_on) { + // Turn the fts off. + _fts_on = false; + fts_off(); + ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); + + } else if (_sensor_accel_sub.update(&sensor_accel)) { + + // Update the current IMU sensor temperature if valid. + if (PX4_ISFINITE(sensor_accel.temperature)) { + temperature_delta = _param_sens_imu_temp.get() - sensor_accel.temperature; + _temperature_last = sensor_accel.temperature; + } + + _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); + _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); + + _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); + + _controller_time_on_usec = static_cast((_param_sens_imu_temp_ff.get() + _proportional_value + + _integrator_value) * static_cast(_controller_period_usec)); + + _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); + + if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) { + _temperature_target_met = true; + + } else { + + _temperature_target_met = false; + } + + _fts_on = true; + fts_on(); + ScheduleDelayed(_controller_time_on_usec); + } + + publish_status(); +} + +void FTS::publish_status() +{ + fts_status_s status{}; + status.fts_on = _fts_on; + status.timestamp = hrt_absolute_time(); + _fts_status_pub.publish(status); +} + +int FTS::start() +{ + // Exit the driver if the sensor ID does not match the desired sensor. + if (_param_sens_temp_id.get() == 0) { + PX4_ERR("Valid SENS_TEMP_ID required"); + request_stop(); + return PX4_ERROR; + } + + update_params(true); + ScheduleNow(); + return PX4_OK; +} + +int FTS::task_spawn(int argc, char *argv[]) +{ + FTS *fts = new FTS(); + + if (!fts) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; + } + + _object.store(fts); + _task_id = task_id_is_work_queue; + + fts->start(); + return 0; +} + +void FTS::update_params(const bool force) +{ + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // update parameters from storage + ModuleParams::updateParams(); + } +} + +int FTS::print_usage(const char *reason) +{ + if (reason) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint. + +This task can be started at boot from the startup scripts by setting SENS_EN_THERMAL or via CLI. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fts", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fts_main(int argc, char *argv[]) +{ + return FTS::main(argc, argv); +} diff --git a/src/drivers/fts/fts.h b/src/drivers/fts/fts.h new file mode 100644 index 0000000..1821f9b --- /dev/null +++ b/src/drivers/fts/fts.h @@ -0,0 +1,100 @@ +/** + * @file fts.h + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace time_literals; + +class FTS : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + FTS(); + + virtual ~FTS(); + + /** + * @see ModuleBase::custom_command(). + * @brief main Main entry point to the module that should be + * called directly from the module's main method. + * @param argc The input argument count. + * @param argv Pointer to the input argument array. + * @return Returns 0 iff successful, -1 otherwise. + */ + static int custom_command(int argc, char *argv[]); + + /** + * @see ModuleBase::print_usage(). + * @brief Prints the module usage to the nuttshell console. + * @param reason The requested reason for printing to console. + */ + static int print_usage(const char *reason = nullptr); + + /** + * @see ModuleBase::task_spawn(). + * @brief Initializes the class in the same context as the work queue + * and starts the background listener. + * @param argv Pointer to the input argument array. + * @return Returns 0 iff successful, -1 otherwise. + */ + static int task_spawn(int argc, char *argv[]); + + /** + * @brief Initiates the fts driver work queue, starts a new background task, + * and fails if it is already running. + * @return Returns 1 iff start was successful. + */ + int start(); + +private: + + /** Disables the fts */ + void disable_fts(); + + /** Turns the fts on */ + void fts_on(); + + /** Turns the fts off */ + void fts_off(); + + void initialize(); + + /** Enables / configures the fts GPIO */ + void initialize_fts_io(); + + /** @brief Called once to initialize uORB topics. */ + bool initialize_topics(); + + void publish_status(); + + /** + * @brief Updates and checks for updated uORB parameters. + * @param force Boolean to determine if an update check should be forced. + */ + void update_params(const bool force = false); + + /** Work queue struct for the scheduler. */ + static struct work_s _work; + + /** File descriptor for PX4IO for fts ioctl's */ + + bool _fts_initialized = false; + bool _fts_on = false; + + uORB::Publication _fts_status_pub{ORB_ID(fts_status)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + +}; diff --git a/src/drivers/fts/fts_params.c b/src/drivers/fts/fts_params.c new file mode 100644 index 0000000..ee60651 --- /dev/null +++ b/src/drivers/fts/fts_params.c @@ -0,0 +1,5 @@ +/** + * @file fts_params.c + * FTS parameters. + */ + diff --git a/src/modules/fts/CMakeLists.txt b/src/modules/fts/CMakeLists.txt new file mode 100644 index 0000000..5974b00 --- /dev/null +++ b/src/modules/fts/CMakeLists.txt @@ -0,0 +1,9 @@ +px4_add_module( + MODULE modules__fts + MAIN fts_controller + SRCS + fts_controller.cpp + main_state_machine.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/fts/fts_controller.cpp b/src/modules/fts/fts_controller.cpp new file mode 100644 index 0000000..fe0f43a --- /dev/null +++ b/src/modules/fts/fts_controller.cpp @@ -0,0 +1,211 @@ +/** + * + * Controller of Nimbus Charging FTS + * + * @author Flavio Tonelli + */ + +#include "fts_controller.hpp" + +FTSController::FTSController() : + ModuleParams(nullptr), + ScheduledWorkItem("fts_controller", px4::wq_configurations::lp_default), + _cycle_perf(perf_alloc(PC_ELAPSED, "fts_controller"": cycle")), + _interval_perf(perf_alloc(PC_INTERVAL, "fts_controller"": interval")) +{ +} + +FTSController::~FTSController() +{ + ScheduleClear(); + perf_free(_cycle_perf); + perf_free(_interval_perf); +} + +bool +FTSController::init() +{ + // Update parameters + if (!update_param()) { + PX4_WARN("fts_controller failed to get necessary parameters."); + return false; + } + + _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; +} + +void +FTSController::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + // Update all subscriber + update_topics(); + + // 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); + + perf_end(_cycle_perf); +} + +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; + // 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_status() +{ +} + + +void +FTSController::update_topics() +{ + if (_fts_status_sub.update()) { + _last_fts_state_update_time = _fts_status_sub.get().timestamp; + } + +} + +bool FTSController::update_param() +{ + + // Dummy find all the FTSController params. So the NimbusGUI will read these params on first connection. + param_find("FTS_GEOFENCE_ENABLED"); + param_find("FTS_MANUAL_FTS_ENABLED"); + + + param_t fts_comm_timeout_s_handle = param_find("FTS_COMM_TIMEOUT"); + float fts_comm_timeout_s = 0.0f; + + if (param_get(fts_comm_timeout_s_handle, &fts_comm_timeout_s) != PX4_OK) { + return false; + } + + _fts_comm_timeout_threshold_us = (uint64_t)fts_comm_timeout_s * 1000000; + + return true; +} + +int +FTSController::task_spawn(int argc, char *argv[]) +{ + FTSController *instance = new FTSController(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +void +FTSController::fire() +{ + px4_arch_gpiowrite(GPIO_OUTPUT|5, true); +} + +void +FTSController::reset() +{ + px4_arch_gpiowrite(GPIO_OUTPUT|5, false); +} + +int +FTSController::custom_command(int argc, char *argv[]) +{ + if (!strcmp(argv[0], "fire")) { + get_instance()->fire(); + return 0; + } + + if (!strcmp(argv[0], "reset")) { + get_instance()->reset(); + return 0; + } + + return print_usage("unknown command"); +} + +int +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.fts_state); + + return 0; +} + +int +FTSController::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("fts_controller", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND("status"); + PRINT_MODULE_USAGE_COMMAND("fire"); + PRINT_MODULE_USAGE_COMMAND("reset"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int fts_controller_main(int argc, char *argv[]) +{ + return FTSController::main(argc, argv); +} diff --git a/src/modules/fts/fts_controller.hpp b/src/modules/fts/fts_controller.hpp new file mode 100644 index 0000000..3ccc2b6 --- /dev/null +++ b/src/modules/fts/fts_controller.hpp @@ -0,0 +1,83 @@ +/** + * + * Controller of Nimbus Charging FTS + * + * @author Flavio Tonelli + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "main_state_machine.hpp" + +using namespace time_literals; + +class FTSController : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + FTSController(); + + ~FTSController() 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); + + bool init(); + + int print_status() override; + +private: + + void Run() override; + void update_server_cmd(); + void update_topics(); + void update_status(); + bool update_param(); + + 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 _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/fts_controller_params.c new file mode 100644 index 0000000..c92e8d4 --- /dev/null +++ b/src/modules/fts/fts_controller_params.c @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 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 fts_controller_params.c + * Parameters for FTS controller. + * + * @author Germano Girardi + */ + +/** + * Enable FTS relates funtions. + * + * @boolean + * @group FTS Control + */ +PARAM_DEFINE_INT32(FTS_EN, 0); + +/** + * The timeout for fts_state message + * + * @unit s + * @reboot_required true + * @decimal 1 + * + * @group FTS Control + */ +PARAM_DEFINE_FLOAT(FTS_COMM_TM, 10.0f); diff --git a/src/modules/fts/main_state_machine.cpp b/src/modules/fts/main_state_machine.cpp new file mode 100644 index 0000000..f3d4e88 --- /dev/null +++ b/src/modules/fts/main_state_machine.cpp @@ -0,0 +1,89 @@ +#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/main_state_machine.hpp b/src/modules/fts/main_state_machine.hpp new file mode 100644 index 0000000..57346f3 --- /dev/null +++ b/src/modules/fts/main_state_machine.hpp @@ -0,0 +1,45 @@ +// +// Created by tianfang on 06/05/22. +// + +#ifndef PX4_MAIN_STATE_MACHINE_HPP +#define PX4_MAIN_STATE_MACHINE_HPP + +#include +#include +#include +#include + +using namespace time_literals; + +class MainStateMachine : public StateMachineHelper +{ + +public: + + struct fts_state_s { + 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}; + + }; + + MainStateMachine(); + void init_state_machine(uORB::Publication *main_fts_state_pub, + MainStateMachine::fts_state_s *FTS_state); + void run_state_machine() override; + uint8_t get_mainstatemachine_state(); + +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/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 45f7737..8b006c5 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -33,6 +33,8 @@ set(MAVLINK_GIT_DIR "${CMAKE_CURRENT_LIST_DIR}/mavlink") set(MAVLINK_LIBRARY_DIR "${CMAKE_BINARY_DIR}/mavlink") +#set(MAVLINK_GIT_DIR "${CMAKE_CURRENT_LIST_DIR}/mavlink_nimbus") +#set(MAVLINK_LIBRARY_DIR "${CMAKE_BINARY_DIR}/mavlink_nimbus") file(RELATIVE_PATH MAVLINK_GIT_DIR_RELATIVE ${CMAKE_SOURCE_DIR} ${MAVLINK_GIT_DIR}) px4_add_git_submodule(TARGET git_mavlink_v2 PATH "${MAVLINK_GIT_DIR}") diff --git a/src/modules/mavlink/mavlink_nimbus b/src/modules/mavlink/mavlink_nimbus deleted file mode 160000 index 71ae8c3..0000000 --- a/src/modules/mavlink/mavlink_nimbus +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 71ae8c334f1602db04249216e4d1a5c628889420