2011-10-01 19:55:41 -03:00
|
|
|
/*
|
|
|
|
* AP_Relay.h
|
|
|
|
*
|
|
|
|
* Created on: Oct 2, 2011
|
|
|
|
* Author: Amilcar Lucas
|
|
|
|
*/
|
|
|
|
|
|
|
|
/// @file AP_Relay.h
|
|
|
|
/// @brief APM relay control class
|
2016-02-17 21:25:46 -04:00
|
|
|
#pragma once
|
2011-10-01 19:55:41 -03:00
|
|
|
|
2023-06-06 05:05:06 -03:00
|
|
|
#include "AP_Relay_config.h"
|
|
|
|
|
|
|
|
#if AP_RELAY_ENABLED
|
|
|
|
|
2015-08-11 03:28:45 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
2023-09-26 18:58:26 -03:00
|
|
|
#include <AP_Relay/AP_Relay_Params.h>
|
|
|
|
#include <AP_Common/Bitmask.h>
|
2013-06-24 09:39:18 -03:00
|
|
|
|
2024-01-03 15:36:16 -04:00
|
|
|
#ifndef AP_RELAY_NUM_RELAYS
|
|
|
|
#define AP_RELAY_NUM_RELAYS 6
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#if AP_RELAY_NUM_RELAYS < 1
|
|
|
|
#error There must be at least one relay instance if using AP_Relay
|
|
|
|
#endif
|
2014-01-19 21:59:21 -04:00
|
|
|
|
2024-01-01 16:54:24 -04:00
|
|
|
#if AP_RELAY_DRONECAN_ENABLED
|
|
|
|
#include <AP_DroneCAN/AP_DroneCAN.h>
|
|
|
|
#endif
|
|
|
|
|
2011-10-01 19:55:41 -03:00
|
|
|
/// @class AP_Relay
|
2017-08-29 15:08:41 -03:00
|
|
|
/// @brief Class to manage the ArduPilot relay
|
2012-08-17 03:21:08 -03:00
|
|
|
class AP_Relay {
|
2024-01-01 16:54:24 -04:00
|
|
|
#if AP_RELAY_DRONECAN_ENABLED
|
|
|
|
// Allow DroneCAN to directly access private DroneCAN state
|
|
|
|
friend class AP_DroneCAN;
|
|
|
|
#endif
|
2011-10-01 19:55:41 -03:00
|
|
|
public:
|
2017-12-12 21:06:14 -04:00
|
|
|
AP_Relay();
|
2017-08-29 15:08:41 -03:00
|
|
|
|
|
|
|
/* Do not allow copies */
|
2022-09-30 06:50:43 -03:00
|
|
|
CLASS_NO_COPY(AP_Relay);
|
2013-06-24 09:39:18 -03:00
|
|
|
|
2012-12-04 19:26:30 -04:00
|
|
|
// setup the relay pin
|
|
|
|
void init();
|
|
|
|
|
2012-08-17 03:21:08 -03:00
|
|
|
// activate the relay
|
2019-09-26 21:33:48 -03:00
|
|
|
void on(uint8_t instance) { set(instance, true); }
|
2011-10-01 19:55:41 -03:00
|
|
|
|
2012-08-17 03:21:08 -03:00
|
|
|
// de-activate the relay
|
2019-09-26 21:33:48 -03:00
|
|
|
void off(uint8_t instance) { set(instance, false); }
|
2011-10-01 19:55:41 -03:00
|
|
|
|
2022-10-09 20:47:42 -03:00
|
|
|
// get state of relay
|
2023-09-26 18:58:26 -03:00
|
|
|
bool get(uint8_t instance) const;
|
2022-10-09 20:47:42 -03:00
|
|
|
|
2014-01-20 00:35:38 -04:00
|
|
|
// see if the relay is enabled
|
2023-09-26 18:58:26 -03:00
|
|
|
bool enabled(uint8_t instance) const;
|
2014-01-20 00:35:38 -04:00
|
|
|
|
2012-08-17 03:21:08 -03:00
|
|
|
// toggle the relay status
|
2019-09-26 21:33:48 -03:00
|
|
|
void toggle(uint8_t instance);
|
2011-10-01 19:55:41 -03:00
|
|
|
|
2021-07-20 01:26:33 -03:00
|
|
|
// check settings are valid
|
|
|
|
bool arming_checks(size_t buflen, char *buffer) const;
|
|
|
|
|
2019-04-22 20:12:56 -03:00
|
|
|
static AP_Relay *get_singleton(void) {return singleton; }
|
|
|
|
|
2013-06-24 09:39:18 -03:00
|
|
|
static const struct AP_Param::GroupInfo var_info[];
|
2011-10-01 19:55:41 -03:00
|
|
|
|
2023-08-03 00:31:12 -03:00
|
|
|
bool send_relay_status(const class GCS_MAVLINK &link) const;
|
|
|
|
|
2023-12-12 10:02:14 -04:00
|
|
|
void set(AP_Relay_Params::FUNCTION function, bool value);
|
2023-09-26 18:58:26 -03:00
|
|
|
|
2023-12-08 00:36:03 -04:00
|
|
|
// see if the relay is enabled
|
2023-12-12 10:02:14 -04:00
|
|
|
bool enabled(AP_Relay_Params::FUNCTION function) const;
|
2023-12-08 00:36:03 -04:00
|
|
|
|
2013-06-24 09:39:18 -03:00
|
|
|
private:
|
2019-04-22 20:12:56 -03:00
|
|
|
static AP_Relay *singleton;
|
|
|
|
|
2023-09-26 18:58:26 -03:00
|
|
|
AP_Relay_Params _params[AP_RELAY_NUM_RELAYS];
|
|
|
|
|
2024-01-01 10:23:00 -04:00
|
|
|
// Return true is function is valid
|
|
|
|
bool function_valid(AP_Relay_Params::FUNCTION function) const;
|
|
|
|
|
2023-09-26 18:58:26 -03:00
|
|
|
void set(uint8_t instance, bool value);
|
|
|
|
|
|
|
|
void set_defaults();
|
|
|
|
void convert_params();
|
|
|
|
|
|
|
|
void set_pin_by_instance(uint8_t instance, bool value);
|
2024-01-01 16:54:24 -04:00
|
|
|
|
|
|
|
// Set relay state from pin number
|
|
|
|
void set_pin(const int16_t pin, const bool value);
|
|
|
|
|
|
|
|
// Get relay state from pin number
|
|
|
|
bool get_pin(const int16_t pin) const;
|
|
|
|
|
|
|
|
#if HAL_ENABLE_DRONECAN_DRIVERS
|
|
|
|
// Virtual DroneCAN pins
|
|
|
|
class DroneCAN {
|
|
|
|
public:
|
|
|
|
// Return true if pin number is a virtual DroneCAN pin
|
|
|
|
bool valid_pin(int16_t pin) const;
|
|
|
|
|
|
|
|
// Enable streaming of pin number
|
|
|
|
void enable_pin(int16_t pin);
|
|
|
|
|
|
|
|
// Populate message and update index with the sent command
|
|
|
|
bool populate_next_command(uint8_t &index, uavcan_equipment_hardpoint_Command &msg) const;
|
|
|
|
|
|
|
|
// Set DroneCAN relay state from pin number
|
|
|
|
void set_pin(const int16_t pin, const bool value);
|
|
|
|
|
|
|
|
// Get relay state from pin number
|
|
|
|
bool get_pin(const int16_t pin) const;
|
|
|
|
|
|
|
|
private:
|
|
|
|
|
|
|
|
// Get the hardpoint index of given pin number
|
|
|
|
uint8_t hardpoint_index(const int16_t pin) const;
|
|
|
|
|
|
|
|
// Send DroneCAN hardpoint message for given index on all interfaces
|
|
|
|
void send_index(const uint8_t index);
|
|
|
|
|
|
|
|
static constexpr uint8_t num_pins = (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_15 - (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0;
|
|
|
|
|
|
|
|
struct {
|
|
|
|
bool value;
|
|
|
|
bool enabled;
|
|
|
|
} state[num_pins];
|
|
|
|
|
|
|
|
} dronecan;
|
|
|
|
#endif // HAL_ENABLE_DRONECAN_DRIVERS
|
|
|
|
|
2011-10-01 19:55:41 -03:00
|
|
|
};
|
2019-06-27 04:22:21 -03:00
|
|
|
|
|
|
|
namespace AP {
|
|
|
|
AP_Relay *relay();
|
|
|
|
};
|
2023-06-06 05:05:06 -03:00
|
|
|
|
|
|
|
#endif // AP_RELAY_ENABLED
|