365 lines
9.6 KiB
C++
365 lines
9.6 KiB
C++
|
|
#include "fts_control.hpp"
|
|
#include "px4_platform_common/board_common.h"
|
|
|
|
|
|
|
|
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_control failed to get necessary parameters.");
|
|
return false;
|
|
}
|
|
uint8_t guid[PX4_GUID_BYTE_LENGTH];
|
|
board_get_px4_guid(guid);
|
|
fts_id.uidBytes[0]=guid[14];
|
|
fts_id.uidBytes[1]=guid[15];
|
|
fts_id.uidBytes[2]=guid[16];
|
|
fts_id.uidBytes[3]=guid[17];
|
|
|
|
get_instance()->reset();
|
|
|
|
// Task run frequency
|
|
ScheduleOnInterval(500_ms);
|
|
return true;
|
|
}
|
|
|
|
void
|
|
FTSController::Run() {
|
|
if (should_exit()) {
|
|
ScheduleClear();
|
|
exit_and_cleanup();
|
|
return;
|
|
}
|
|
|
|
perf_begin(_cycle_perf);
|
|
perf_count(_interval_perf);
|
|
|
|
// Update AUTO-FTS status
|
|
//TODO manage comm timeout
|
|
if (_auto_fts_message_sub.updated()) {
|
|
_auto_fts_message_sub.copy(&_auto_fts_message);
|
|
_auto_fts_timestamp_last = _auto_fts_message.timestamp;
|
|
//ARMING CONDITIONS ARE MET
|
|
switch (_auto_fts_state){
|
|
case AUTO_FTS_STATE_ARMED:{
|
|
//AFTS IS CURRENTLY ARMED
|
|
//TODO ADD GEOFENCE LOADED CHECK ETC
|
|
//if (_auto_fts_message.config_status != 0 && _auto_fts_message.geofence_loaded != 0 && _auto_fts_message.gps_lock != 0) {
|
|
if (_auto_fts_message.gps_lock != 0) {
|
|
// FIRING CONTITIONS ARE MET
|
|
if (_auto_fts_message.geofence_violated != 0) {
|
|
//GEOFENCE VIOLATED, FIRE
|
|
_auto_fts_state = AUTO_FTS_STATE_FIRE;
|
|
} else {
|
|
//GEOFENCE NOT VIOLATED, REMAIN ARMED AND READY
|
|
}
|
|
} else {
|
|
//FIRING CONDITIONS ARE LOST, DISARM AUTO-FTS
|
|
_auto_fts_state = AUTO_FTS_STATE_DISARMED;
|
|
}
|
|
}break;
|
|
|
|
case AUTO_FTS_STATE_DISARMED:
|
|
{
|
|
//AFTS IS NOT CURRENTLY ARMED
|
|
//TODO ADD GEOFENCE LOADED CHECK ETC
|
|
//if (_auto_fts_message.config_status != 0 && _auto_fts_message.geofence_loaded != 0 && _auto_fts_message.gps_lock != 0) {
|
|
if (_auto_fts_message.gps_lock != 0) {
|
|
if (_auto_fts_message.geofence_violated != 0) {
|
|
// FAILSAFE: CANNOT ARM AUTO-FTS WHEN GEOFENCE IS ALREADY VIOLATED
|
|
_auto_fts_state = AUTO_FTS_STATE_ERROR;
|
|
} else {
|
|
// ARM AUTO-FTS
|
|
_auto_fts_state = AUTO_FTS_STATE_ARMED;
|
|
}
|
|
}else {
|
|
// REMAIN DISARMED
|
|
}
|
|
}break;
|
|
|
|
case AUTO_FTS_STATE_FIRE:
|
|
case AUTO_FTS_STATE_ERROR:
|
|
default: {
|
|
//DO NOTHING
|
|
}break;
|
|
}
|
|
} else{
|
|
if (hrt_elapsed_time(&_auto_fts_timestamp_last) > AUTO_FTS_COMM_TIMEOUT){
|
|
_auto_fts_state = AUTO_FTS_STATE_DISARMED;
|
|
}
|
|
}
|
|
|
|
|
|
// Manage communication with GROUND-FTS module and update GROUND-FTS status
|
|
if (_ground_fts_message_sub.updated()) {
|
|
_ground_fts_message_sub.copy(&_ground_fts_message);
|
|
|
|
if (_ground_fts_message.message == PING) {
|
|
//PING COMMAND
|
|
if (_ground_fts_message.fts_uid == fts_id.uid) {
|
|
//PING RECEIVED, GROUND-FTS IS ARMED
|
|
_ground_fts_state = GROUND_FTS_STATE_ARMED;
|
|
|
|
_ground_fts_response.message = PONG;
|
|
_ground_fts_response.fts_uid = fts_id.uid;
|
|
_ground_fts_response_pub.publish(_ground_fts_response);
|
|
|
|
_ground_fts_timestamp_last = _ground_fts_message.timestamp;
|
|
}
|
|
} else if (_ground_fts_message.message == PAIR) {
|
|
//PAIR COMMAND
|
|
_ground_fts_response.message = PAIR;
|
|
_ground_fts_response.fts_uid = fts_id.uid;
|
|
_ground_fts_response_pub.publish(_ground_fts_response);
|
|
int32_t param_val = 0;
|
|
if (param_get(param_find("GFTS_PAIRED"), ¶m_val)==0){
|
|
param_val = 1;
|
|
param_set(param_find("GFTS_PAIRED"), ¶m_val);
|
|
}
|
|
}
|
|
|
|
} else {
|
|
if (hrt_elapsed_time(&_ground_fts_timestamp_last) > GROUND_FTS_COMM_TIMEOUT){
|
|
_ground_fts_state = GROUND_FTS_STATE_DISARMED;
|
|
}
|
|
}
|
|
|
|
if (_emergency_command_sub.updated()) {
|
|
|
|
_emergency_command_sub.copy(&_emergency_command);
|
|
|
|
if (_emergency_command.param == fts_id.uid) {
|
|
if (_emergency_command.emergency_command == ACTIVATE_FTS) {
|
|
//TERMINATE COMMAND RECEIVED
|
|
|
|
if(_ground_fts_state == GROUND_FTS_STATE_ARMED) {
|
|
_ground_fts_state = GROUND_FTS_STATE_FIRE;
|
|
}else{
|
|
// FAILSAFE: CANNOT FIRE GROUND-FTS IF CURRENTLY DISARMED
|
|
_ground_fts_state = GROUND_FTS_STATE_ERROR;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
switch (_ground_fts_state) {
|
|
case GROUND_FTS_STATE_DISARMED:{
|
|
|
|
}break;
|
|
|
|
case GROUND_FTS_STATE_ARMED:{
|
|
|
|
}break;
|
|
|
|
case GROUND_FTS_STATE_FIRE:
|
|
case GROUND_FTS_STATE_ERROR:
|
|
default:
|
|
{
|
|
//DO NOTHING
|
|
}break;
|
|
}
|
|
|
|
switch (_main_fts_state) {
|
|
case MAIN_FTS_STATE_DISARMED:{
|
|
|
|
if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){
|
|
_main_fts_state = MAIN_FTS_STATE_ERROR;
|
|
}
|
|
|
|
else if (_ground_fts_state == GROUND_FTS_STATE_ARMED || _auto_fts_state ==AUTO_FTS_STATE_ARMED){
|
|
get_instance()->arm();
|
|
}
|
|
|
|
}break;
|
|
case MAIN_FTS_STATE_ARMED:{
|
|
if (_ground_fts_state == GROUND_FTS_STATE_FIRE || _auto_fts_state ==AUTO_FTS_STATE_FIRE){
|
|
get_instance()->fire();
|
|
} else if (_ground_fts_state == GROUND_FTS_STATE_DISARMED && _auto_fts_state ==AUTO_FTS_STATE_DISARMED){
|
|
get_instance()->disarm();
|
|
}
|
|
}break;
|
|
case MAIN_FTS_STATE_FIRE:
|
|
case MAIN_FTS_STATE_ERROR:
|
|
default:
|
|
{
|
|
//DO NOTHING
|
|
}break;
|
|
}
|
|
|
|
|
|
// Publish new status
|
|
update_topics();
|
|
|
|
perf_end(_cycle_perf);
|
|
|
|
}
|
|
|
|
void
|
|
FTSController::update_topics()
|
|
{
|
|
// Publish FTS controller state
|
|
_main_fts_status_msg.timestamp = hrt_absolute_time();
|
|
_main_fts_status_msg.main_fts_state = _main_fts_state;
|
|
_main_fts_status_msg.ground_fts_state = _ground_fts_state;
|
|
_main_fts_status_msg.auto_fts_state = _auto_fts_state;
|
|
_main_fts_status_pub.publish(_main_fts_status_msg);
|
|
}
|
|
|
|
bool FTSController::update_param()
|
|
{
|
|
param_get(param_find("GFTS_PAIRED"), &_ground_fts_paired);
|
|
return 1;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
uint8_t
|
|
FTSController::fire()
|
|
{
|
|
if (_main_fts_state == MAIN_FTS_STATE_ARMED) {
|
|
fts_drv.fts_on();
|
|
_main_fts_state = MAIN_FTS_STATE_FIRE;
|
|
return 0;
|
|
}else{
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
uint8_t
|
|
FTSController::reset()
|
|
{
|
|
_main_fts_state = MAIN_FTS_STATE_DISARMED;
|
|
_ground_fts_state = GROUND_FTS_STATE_DISARMED;
|
|
_auto_fts_state = AUTO_FTS_STATE_DISARMED;
|
|
fts_drv.fts_off();
|
|
fts_drv.fts_disarm();
|
|
return 0;
|
|
}
|
|
|
|
uint8_t
|
|
FTSController::arm()
|
|
{
|
|
fts_drv.fts_arm();
|
|
_main_fts_state = MAIN_FTS_STATE_ARMED;
|
|
return 0;
|
|
}
|
|
|
|
uint8_t
|
|
FTSController::disarm()
|
|
{
|
|
fts_drv.fts_disarm();
|
|
_main_fts_state = MAIN_FTS_STATE_DISARMED;
|
|
return 0;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
if (!strcmp(argv[0], "arm")) {
|
|
get_instance()->arm();
|
|
return 0;
|
|
}
|
|
|
|
if (!strcmp(argv[0], "disarm")) {
|
|
get_instance()->disarm();
|
|
return 0;
|
|
}
|
|
|
|
return print_usage("unknown command");
|
|
}
|
|
|
|
int
|
|
FTSController::print_status()
|
|
{
|
|
perf_print_counter(_cycle_perf);
|
|
perf_print_counter(_interval_perf);
|
|
|
|
PX4_INFO("Main FTS module UID:%li", fts_id.uid);
|
|
PX4_INFO("Main FTS state:%s", mfts_status_str(_main_fts_state));
|
|
PX4_INFO("Ground module state:%s", gfts_status_str(_ground_fts_state));
|
|
PX4_INFO("Auto module state:%s", afts_status_str(_auto_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("arm");
|
|
PRINT_MODULE_USAGE_COMMAND("disarm");
|
|
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);
|
|
}
|