Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Relay/AP_Relay.cpp

692 lines
19 KiB
C++
Raw Blame History

/*
* AP_Relay.cpp
*
* Created on: Oct 2, 2011
* Author: Amilcar Lucas
*/
#include "AP_Relay_config.h"
#if AP_RELAY_ENABLED
#include "AP_Relay.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_ICEngine/AP_ICEngine.h>
#include <AP_Parachute/AP_Parachute.h>
#include <AP_Camera/AP_Camera.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#if APM_BUILD_TYPE(APM_BUILD_Rover)
#include <AR_Motors/AP_MotorsUGV.h>
#endif
#if AP_RELAY_DRONECAN_ENABLED
#include <AP_DroneCAN/AP_DroneCAN.h>
#include <AP_CANManager/AP_CANManager.h>
#endif
#if AP_SIM_ENABLED
#include <SITL/SITL.h>
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define RELAY1_PIN_DEFAULT 13
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE
#define RELAY1_PIN_DEFAULT 57
#define RELAY2_PIN_DEFAULT 49
#define RELAY3_PIN_DEFAULT 116
#define RELAY4_PIN_DEFAULT 113
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
#define RELAY1_PIN_DEFAULT 27
#define RELAY2_PIN_DEFAULT 65
#define RELAY3_PIN_DEFAULT 22
#define RELAY4_PIN_DEFAULT 81
#define RELAY5_PIN_DEFAULT 23
#define RELAY6_PIN_DEFAULT 26
#endif
#endif
#ifndef RELAY1_PIN_DEFAULT
#define RELAY1_PIN_DEFAULT -1
#endif
#ifndef RELAY2_PIN_DEFAULT
#define RELAY2_PIN_DEFAULT -1
#endif
#ifndef RELAY3_PIN_DEFAULT
#define RELAY3_PIN_DEFAULT -1
#endif
#ifndef RELAY4_PIN_DEFAULT
#define RELAY4_PIN_DEFAULT -1
#endif
#ifndef RELAY5_PIN_DEFAULT
#define RELAY5_PIN_DEFAULT -1
#endif
#ifndef RELAY6_PIN_DEFAULT
#define RELAY6_PIN_DEFAULT -1
#endif
const AP_Param::GroupInfo AP_Relay::var_info[] = {
// 0 was PIN
// 1 was PIN2
// 2 was PIN3
// 3 was PIN4
// 4 was DEFAULT
// 5 was PIN5
// 6 was PIN6
// @Group: 1_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[0], "1_", 7, AP_Relay, AP_Relay_Params),
#if AP_RELAY_NUM_RELAYS > 1
// @Group: 2_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[1], "2_", 8, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 2
// @Group: 3_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[2], "3_", 9, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 3
// @Group: 4_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[3], "4_", 10, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 4
// @Group: 5_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[4], "5_", 11, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 5
// @Group: 6_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[5], "6_", 12, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 6
// @Group: 7_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[6], "7_", 13, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 7
// @Group: 8_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[7], "8_", 14, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 8
// @Group: 9_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[8], "9_", 15, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 9
// @Group: 10_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[9], "10_", 16, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 10
// @Group: 11_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[10], "11_", 17, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 11
// @Group: 12_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[11], "12_", 18, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 12
// @Group: 13_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[12], "13_", 19, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 13
// @Group: 14_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[13], "14_", 20, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 14
// @Group: 15_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[14], "15_", 21, AP_Relay, AP_Relay_Params),
#endif
#if AP_RELAY_NUM_RELAYS > 15
// @Group: 16_
// @Path: AP_Relay_Params.cpp
AP_SUBGROUPINFO(_params[15], "16_", 22, AP_Relay, AP_Relay_Params),
#endif
AP_GROUPEND
};
AP_Relay *AP_Relay::singleton;
extern const AP_HAL::HAL& hal;
AP_Relay::AP_Relay(void)
{
AP_Param::setup_object_defaults(this, var_info);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (singleton != nullptr) {
AP_HAL::panic("AP_Relay must be singleton");
}
#endif
singleton = this;
}
void AP_Relay::convert_params()
{
// PARAMETER_CONVERSION - Added: Dec-2023
#ifndef HAL_BUILD_AP_PERIPH
// Dont need this conversion on periph as relay support is more recent
// Before converting local params we must find any relays being used by index from external libs
int8_t ice_relay = -1;
#if AP_ICENGINE_ENABLED
AP_ICEngine *ice = AP::ice();
int8_t ice_relay_index;
if (ice != nullptr && ice->get_legacy_ignition_relay_index(ice_relay_index)) {
ice_relay = ice_relay_index;
}
#endif
int8_t chute_relay = -1;
#if HAL_PARACHUTE_ENABLED
AP_Parachute *parachute = AP::parachute();
int8_t parachute_relay_index;
if (parachute != nullptr && parachute->get_legacy_relay_index(parachute_relay_index)) {
chute_relay = parachute_relay_index;
}
#endif
int8_t cam_relay = -1;
#if AP_CAMERA_ENABLED
AP_Camera *camera = AP::camera();
int8_t camera_relay_index;
if ((camera != nullptr) && (camera->get_legacy_relay_index(camera_relay_index))) {
cam_relay = camera_relay_index;
}
#endif
#if APM_BUILD_TYPE(APM_BUILD_Rover)
int8_t rover_relay[] = { -1, -1, -1, -1 };
AP_MotorsUGV *motors = AP::motors_ugv();
if (motors != nullptr) {
motors->get_legacy_relay_index(rover_relay[0], rover_relay[1], rover_relay[2], rover_relay[3]);
}
#endif
// Find old default param
int8_t default_state = 0; // off was the old behaviour
const bool have_default = AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &default_state);
// grab the old values if they were set
for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), 6U); i++) {
if (_params[i].function.configured()) {
// Conversion already done, or user has configured manually
continue;
}
uint8_t param_index = i;
if (i > 3) {
// Skip over the old DEFAULT parameter at index 4
param_index += 1;
}
int8_t pin = 0;
if (!_params[i].pin.configured() && AP_Param::get_param_by_index(this, param_index, AP_PARAM_INT8, &pin) && (pin >= 0)) {
// Copy old pin parameter if valid
_params[i].pin.set_and_save(pin);
}
// Work out what function this relay should be
AP_Relay_Params::FUNCTION new_fun;
if (i == chute_relay) {
new_fun = AP_Relay_Params::FUNCTION::PARACHUTE;
} else if (i == ice_relay) {
new_fun = AP_Relay_Params::FUNCTION::IGNITION;
} else if (i == cam_relay) {
new_fun = AP_Relay_Params::FUNCTION::CAMERA;
#if APM_BUILD_TYPE(APM_BUILD_Rover)
} else if (i == rover_relay[0]) {
new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1;
} else if (i == rover_relay[1]) {
new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2;
} else if (i == rover_relay[2]) {
new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3;
} else if (i == rover_relay[3]) {
new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4;
#endif
} else {
if (_params[i].pin < 0) {
// Don't enable as numbered relay if pin is invalid
// Other functions should be enabled with a invalid pin
// This will result in a pre-arm promoting the user to resolve
continue;
}
new_fun = AP_Relay_Params::FUNCTION::RELAY;
}
_params[i].function.set_and_save(int8_t(new_fun));
// Set the default state
if (have_default) {
_params[i].default_state.set_and_save(default_state);
}
}
#endif // HAL_BUILD_AP_PERIPH
}
void AP_Relay::set_defaults() {
const int8_t pins[] = { RELAY1_PIN_DEFAULT,
RELAY2_PIN_DEFAULT,
RELAY3_PIN_DEFAULT,
RELAY4_PIN_DEFAULT,
RELAY5_PIN_DEFAULT,
RELAY6_PIN_DEFAULT };
for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), ARRAY_SIZE(pins)); i++) {
// set the default
if (pins[i] != -1) {
_params[i].pin.set_default(pins[i]);
}
}
}
// Return true is function is valid
bool AP_Relay::function_valid(AP_Relay_Params::FUNCTION function) const
{
return (function > AP_Relay_Params::FUNCTION::NONE) && (function < AP_Relay_Params::FUNCTION::NUM_FUNCTIONS);
}
void AP_Relay::init()
{
set_defaults();
convert_params();
// setup the actual default values of all the pins
for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) {
const int16_t pin = _params[instance].pin;
if (pin == -1) {
// no valid pin to set it on, skip it
continue;
}
const AP_Relay_Params::FUNCTION function = _params[instance].function;
if (!function_valid(function)) {
// invalid function, skip it
continue;
}
if (function == AP_Relay_Params::FUNCTION::RELAY) {
// relay by instance number, set the state to match our output
const AP_Relay_Params::DefaultState default_state = _params[instance].default_state;
if ((default_state == AP_Relay_Params::DefaultState::OFF) ||
(default_state == AP_Relay_Params::DefaultState::ON)) {
set_pin_by_instance(instance, (bool)default_state);
}
} else {
// all functions are supposed to be off by default
// this will need revisiting when we support inversion
set_pin_by_instance(instance, false);
}
// Make sure any DroneCAN pin is enabled for streaming
#if AP_RELAY_DRONECAN_ENABLED
dronecan.enable_pin(pin);
#endif
}
}
void AP_Relay::set(const AP_Relay_Params::FUNCTION function, const bool value) {
if (!function_valid(function)) {
// invalid function
return;
}
for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) {
if (function != _params[instance].function) {
continue;
}
set_pin_by_instance(instance, value);
}
}
// set a pins output state by instance and log if required
// this is an internal helper, instance must have already been validated to be in range
void AP_Relay::set_pin_by_instance(uint8_t instance, bool value)
{
const int16_t pin = _params[instance].pin;
if (pin == -1) {
// no valid pin to set it on, skip it
return;
}
#if AP_SIM_ENABLED
if (!(AP::sitl()->on_hardware_relay_enable_mask & (1U << instance))) {
return;
}
#endif
const bool initial_value = get_pin(pin);
if (initial_value != value) {
set_pin(pin, value);
#if HAL_LOGGING_ENABLED
AP::logger().Write("RELY", "TimeUS,Instance,State", "s#-", "F--", "QBB",
AP_HAL::micros64(),
instance,
value);
#endif
}
}
void AP_Relay::set(const uint8_t instance, const bool value)
{
if (instance >= ARRAY_SIZE(_params)) {
return;
}
if (_params[instance].function != AP_Relay_Params::FUNCTION::RELAY) {
return;
}
set_pin_by_instance(instance, value);
}
void AP_Relay::toggle(uint8_t instance)
{
if (instance < ARRAY_SIZE(_params)) {
set(instance, !get(instance));
}
}
// check settings are valid
bool AP_Relay::arming_checks(size_t buflen, char *buffer) const
{
for (uint8_t i=0; i<ARRAY_SIZE(_params); i++) {
if (!function_valid(_params[i].function)) {
// Relay disabled
continue;
}
const int16_t pin = _params[i].pin.get();
if (pin == -1) {
// Pin disabled, may want to pre-arm this in the future as function enabled with invalid pin
// User should set function to none to disable
continue;
}
#if AP_RELAY_DRONECAN_ENABLED
const bool DroneCAN_pin = dronecan.valid_pin(pin);
#else
const bool DroneCAN_pin = false;
#endif
if (!DroneCAN_pin && !hal.gpio->valid_pin(pin)) {
// Check GPIO pin is valid
char param_name_buf[14];
hal.util->snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY%u_PIN", unsigned(i+1));
uint8_t servo_ch;
if (hal.gpio->pin_to_servo_channel(pin, servo_ch)) {
hal.util->snprintf(buffer, buflen, "%s=%d, set SERVO%u_FUNCTION=-1", param_name_buf, int(pin), unsigned(servo_ch+1));
} else {
hal.util->snprintf(buffer, buflen, "%s=%d invalid", param_name_buf, int(pin));
}
return false;
}
// Each pin can only be used by a single relay
for (uint8_t j=i+1; j<ARRAY_SIZE(_params); j++) {
if (!function_valid((AP_Relay_Params::FUNCTION)_params[j].function.get())) {
// Relay disabled
continue;
}
if (pin == _params[j].pin.get()) {
hal.util->snprintf(buffer, buflen, "pin conflict RELAY%u_PIN = RELAY%u_PIN", int(i+1), int(j+1));
return false;
}
}
}
return true;
}
bool AP_Relay::get(uint8_t instance) const
{
if (instance >= ARRAY_SIZE(_params)) {
// invalid instance
return false;
}
return get_pin(_params[instance].pin.get());
}
// Get relay state from pin number
bool AP_Relay::get_pin(const int16_t pin) const
{
if (pin < 0) {
// invalid pin
return false;
}
#if AP_RELAY_DRONECAN_ENABLED
if (dronecan.valid_pin(pin)) {
// Virtual DroneCAN pin
return dronecan.get_pin(pin);
}
#endif
// Real GPIO pin
hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
return (bool)hal.gpio->read(pin);
}
// Set relay state from pin number
void AP_Relay::set_pin(const int16_t pin, const bool value)
{
if (pin < 0) {
// invalid pin
return;
}
#if AP_RELAY_DRONECAN_ENABLED
if (dronecan.valid_pin(pin)) {
// Virtual DroneCAN pin
dronecan.set_pin(pin, value);
return;
}
#endif
// Real GPIO pin
hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT);
hal.gpio->write(pin, value);
}
// see if the relay is enabled
bool AP_Relay::enabled(uint8_t instance) const
{
// Must be a valid instance with function relay and pin set
return (instance < ARRAY_SIZE(_params)) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::FUNCTION::RELAY);
}
// see if the relay is enabled
bool AP_Relay::enabled(AP_Relay_Params::FUNCTION function) const
{
for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) {
if ((_params[instance].function == function) && (_params[instance].pin != -1)) {
return true;
}
}
return false;
}
#if AP_RELAY_DRONECAN_ENABLED
// Return true if pin number is a virtual DroneCAN pin
bool AP_Relay::DroneCAN::valid_pin(int16_t pin) const
{
switch(pin) {
case (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0 ... (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_15:
return true;
default:
return false;
}
}
// Enable streaming of pin number
void AP_Relay::DroneCAN::enable_pin(int16_t pin)
{
if (!valid_pin(pin)) {
return;
}
const uint8_t index = hardpoint_index(pin);
state[index].enabled = true;
}
// Get the hardpoint index of given pin number
uint8_t AP_Relay::DroneCAN::hardpoint_index(const int16_t pin) const
{
return pin - (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0;
}
// Set DroneCAN relay state from pin number
void AP_Relay::DroneCAN::set_pin(const int16_t pin, const bool value)
{
const uint8_t index = hardpoint_index(pin);
// Set pin and ensure enabled for streaming
state[index].enabled = true;
state[index].value = value;
// Broadcast msg on all channels
// Just a single send, rely on streaming to fill in any lost packet
uavcan_equipment_hardpoint_Command msg {};
msg.hardpoint_id = index;
msg.command = state[index].value;
uint8_t can_num_drivers = AP::can().get_num_drivers();
for (uint8_t i = 0; i < can_num_drivers; i++) {
auto *ap_dronecan = AP_DroneCAN::get_dronecan(i);
if (ap_dronecan != nullptr) {
ap_dronecan->relay_hardpoint.broadcast(msg);
}
}
}
// Get relay state from pin number, this relies on a cached value, assume remote pin is in sync
bool AP_Relay::DroneCAN::get_pin(const int16_t pin) const
{
const uint8_t index = hardpoint_index(pin);
return state[index].value;
}
// Populate message and update index with the sent command
// This will allow the caller to cycle through each enabled pin
bool AP_Relay::DroneCAN::populate_next_command(uint8_t &index, uavcan_equipment_hardpoint_Command &msg) const
{
// Find the next enabled index
for (uint8_t i = 0; i < ARRAY_SIZE(state); i++) {
// Look for next index, wrapping back to 0 as needed
const uint8_t new_index = (index + 1 + i) % ARRAY_SIZE(state);
if (!state[new_index].enabled) {
// This index is not being used
continue;
}
// Update command and index then return
msg.hardpoint_id = new_index;
msg.command = state[new_index].value;
index = new_index;
return true;
}
return false;
}
#endif // AP_RELAY_DRONECAN_ENABLED
#if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
// this method may only return false if there is no space in the
// supplied link for the message.
bool AP_Relay::send_relay_status(const GCS_MAVLINK &link) const
{
static_assert(AP_RELAY_NUM_RELAYS <= 16, "Too many relays for MAVLink status reporting to work.");
if (!HAVE_PAYLOAD_SPACE(link.get_chan(), RELAY_STATUS)) {
return false;
}
uint16_t present_mask = 0;
uint16_t on_mask = 0;
for (uint8_t i=0; i<ARRAY_SIZE(_params); i++) {
if (!function_valid(_params[i].function)) {
continue;
}
const uint16_t relay_bit_mask = 1U << i;
present_mask |= relay_bit_mask;
if (get(i)) {
on_mask |= relay_bit_mask;
}
}
mavlink_msg_relay_status_send(
link.get_chan(),
AP_HAL::millis(),
on_mask,
present_mask
);
return true;
}
#endif // AP_MAVLINK_MSG_RELAY_STATUS_ENABLED
namespace AP {
AP_Relay *relay()
{
return AP_Relay::get_singleton();
}
}
#endif // AP_RELAY_ENABLED
Reference in New Issue View Git Blame Copy Permalink