84 lines
2.6 KiB
C++
84 lines
2.6 KiB
C++
|
/**
|
||
|
|
*
|
||
|
|
* Controller of Nimbus Charging FTS
|
||
|
|
*
|
||
|
|
* @author Flavio Tonelli <f.tonelli@nimbus.aero>
|
||
|
|
*/
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <lib/parameters/param.h>
|
||
|
|
#include <px4_platform_common/module.h>
|
||
|
|
#include <px4_platform_common/module_params.h>
|
||
|
|
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||
|
|
#include <uORB/Publication.hpp>
|
||
|
|
#include <uORB/Subscription.hpp>
|
||
|
|
#include <uORB/topics/fts_state.h>
|
||
|
|
#include <uORB/topics/fts_server_cmd_ack.h>
|
||
|
|
#include <uORB/topics/fts_control_cmd.h>
|
||
|
|
#include <uORB/topics/vehicle_status.h>
|
||
|
|
#include <uORB/topics/fts_server_cmd.h>
|
||
|
|
#include <uORB/topics/home_position.h>
|
||
|
|
#include <uORB/topics/vehicle_local_position.h>
|
||
|
|
#include <uORB/topics/fts_controller_state.h>
|
||
|
|
|
||
|
|
#include "main_state_machine.hpp"
|
||
|
|
|
||
|
|
using namespace time_literals;
|
||
|
|
|
||
|
|
class FTSController : public ModuleBase<FTSController>, 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_s> _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||
|
|
uORB::SubscriptionData<fts_state_s> _fts_status_sub{ORB_ID(fts_state)};
|
||
|
|
uORB::SubscriptionData<fts_server_cmd_s> _fts_server_cmd_sub{ORB_ID(fts_server_cmd)};
|
||
|
|
uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
|
||
|
|
uORB::SubscriptionData<vehicle_local_position_s> _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||
|
|
uORB::SubscriptionData<battery_status_s> _battery_status_sub{ORB_ID(battery_status)};
|
||
|
|
|
||
|
|
uORB::Publication<fts_server_cmd_ack_s> _fts_ground_cmd_reply_pub{ORB_ID(fts_server_cmd_ack)};
|
||
|
|
uORB::Publication<fts_control_cmd_s> _fts_control_cmd_pub{ORB_ID(fts_control_cmd)};
|
||
|
|
uORB::Publication<fts_controller_state_s> _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
|
||
|
|
};
|