fts-can/src/modules/fts_control/fts_control.cpp

#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[0];
    fts_id.uidBytes[1]=guid[1];
    fts_id.uidBytes[2]=guid[2];
    fts_id.uidBytes[3]=guid[3];

	// run every 1s
	ScheduleOnInterval(1000_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 AUTO-FTS status
    if (_auto_fts_message_sub.updated()) {
        _auto_fts_message_sub.copy(&_auto_fts_message);

        if (_auto_fts_message.gps_lock != 0) {
            arm();

            if (_auto_fts_message.geofence_violated != 0) {
                fire();
            } else {
                reset();
            }

        } else {
            disarm();
            reset();
        }
    }

    // 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) {

                _ground_fts_response.message = PONG;
                _ground_fts_response.fts_uid = fts_id.uid;
                _ground_fts_response_pub.publish(_ground_fts_response);

            }
        } 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);
        }
    }

    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
                fire();
            }
        }

        // Publish FTS controller state
        _fts_state.timestamp = hrt_absolute_time();

        perf_end(_cycle_perf);
    }
}

void
FTSController::update_status()
{

	// Update FTS state

}

void FTSController::send_fts_state()
{
}


void
FTSController::update_topics()
{
	_fts_state_pub.publish(_fts_state);

}

bool FTSController::update_param()
{

	float fts_comm_timeout_s = 5.0f;


	_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()
{
    fts_drv.fts_on();
}

void
FTSController::reset()
{
    fts_drv.fts_off();
}

void
FTSController::arm()
{
    fts_drv.fts_arm();
}

void
FTSController::disarm()
{
    fts_drv.fts_disarm();
}

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("Current FTS state:%u", _fts_state.main_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);
}