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

AP_Periph: add Servo OUT support

This commit is contained in:
Tom Pittenger 2020-12-12 01:08:45 -08:00 committed by Tom Pittenger
parent c01726b07d
commit 07aeab5c44
8 changed files with 218 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
Tools/AP_Periph/AP_Periph.cpp
View File

@ -125,11 +125,18 @@ void AP_Periph_FW::init()
battery.lib.init();
#endif
#ifdef HAL_PERIPH_NEOPIXEL_COUNT
#if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_RC_OUT)
hal.rcout->init();
#endif
#ifdef HAL_PERIPH_NEOPIXEL_COUNT
hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, AP_HAL::RCOutput::MODE_NEOPIXEL);
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
rcout_init();
#endif
#ifdef HAL_PERIPH_ENABLE_ADSB
adsb_init();
#endif
@ -272,6 +279,10 @@ void AP_Periph_FW::update()
#ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
check_for_serial_reboot_cmd(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT);
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
SRV_Channels::enable_aux_servos();
#endif
}
static uint32_t last_error_ms;

11
Tools/AP_Periph/AP_Periph.h
View File

@ -5,6 +5,7 @@
#include <AP_GPS/AP_GPS.h>
#include <AP_Compass/AP_Compass.h>
#include <AP_Baro/AP_Baro.h>
#include "SRV_Channel/SRV_Channel.h"
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Airspeed/AP_Airspeed.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
@ -128,6 +129,16 @@ public:
void hwesc_telem_update();
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
SRV_Channels servo_channels;
void rcout_init();
void rcout_esc(int16_t *rc, uint8_t num_channels);
void rcout_srv(const uint8_t actuator_id, const float command_value);
void rcout_update();
void rcout_handle_safety_state(uint8_t safety_state);
#endif
// setup the var_info table
AP_Param param_loader{var_info};

7
Tools/AP_Periph/Parameters.cpp
View File

@ -113,6 +113,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
GSCALAR(esc_number, "ESC_NUMBER", 0),
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
// Servo driver
// @Group: OUT
// @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
GOBJECT(servo_channels, "OUT", SRV_Channels),
#endif
AP_VAREND
};

1
Tools/AP_Periph/Parameters.h
View File

@ -32,6 +32,7 @@ public:
k_param_debug,
k_param_serial_number,
k_param_adsb_port,
k_param_servo_channels,
};
AP_Int16 format_version;

1
Tools/AP_Periph/README.md
View File

@ -34,6 +34,7 @@ UAVCAN sensor types. Support is included for:
- LEDs (GPIO, I2C or WS2812 serial)
- Safety LED and Safety Switch
- Buzzer (tonealarm or simple GPIO)
- RC Output (All standard RCOutput protocols)
An AP_Periph UAVCAN firmware supports these UAVCAN features:

88
Tools/AP_Periph/can.cpp
View File

@ -44,6 +44,9 @@
#include <uavcan/equipment/gnss/RTCMStream.h>
#include <uavcan/equipment/power/BatteryInfo.h>
#include <uavcan/protocol/debug/LogMessage.h>
#include <uavcan/equipment/esc/RawCommand.h>
#include <uavcan/equipment/actuator/ArrayCommand.h>
#include <uavcan/equipment/actuator/Command.h>
#include <stdio.h>
#include <drivers/stm32/canard_stm32.h>
#include <AP_HAL/I2CDevice.h>
@ -495,7 +498,7 @@ static void can_buzzer_update(void)
}
#endif // HAL_PERIPH_ENABLE_BUZZER
#ifdef HAL_GPIO_PIN_SAFE_LED
#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
static uint8_t safety_state;
/*
@ -508,6 +511,9 @@ static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer)
return;
}
safety_state = req.status;
#ifdef HAL_PERIPH_ENABLE_RC_OUT
periph.rcout_handle_safety_state(safety_state);
#endif
}
#endif // HAL_GPIO_PIN_SAFE_LED
@ -604,6 +610,59 @@ static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer
}
#endif // AP_PERIPH_HAVE_LED
#ifdef HAL_PERIPH_ENABLE_RC_OUT
static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfer)
{
uavcan_equipment_esc_RawCommand cmd;
uint8_t arraybuf[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
uint8_t *arraybuf_ptr = arraybuf;
if (uavcan_equipment_esc_RawCommand_decode(transfer, transfer->payload_len, &cmd, &arraybuf_ptr) < 0) {
return;
}
periph.rcout_esc(cmd.cmd.data, cmd.cmd.len);
}
static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer)
{
// manual decoding due to TAO bug in libcanard generated code
if (transfer->payload_len < 1 || transfer->payload_len > UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE+1) {
return;
}
const uint8_t data_count = (transfer->payload_len / UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_MAX_SIZE);
uavcan_equipment_actuator_Command data[data_count] {};
uint32_t offset = 0;
for (uint8_t i=0; i<data_count; i++) {
canardDecodeScalar(transfer, offset, 8, false, (void*)&data[i].actuator_id);
offset += 8;
canardDecodeScalar(transfer, offset, 8, false, (void*)&data[i].command_type);
offset += 8;
#ifndef CANARD_USE_FLOAT16_CAST
uint16_t tmp_float = 0;
#else
CANARD_USE_FLOAT16_CAST tmp_float = 0;
#endif
canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
offset += 16;
#ifndef CANARD_USE_FLOAT16_CAST
data[i].command_value = canardConvertFloat16ToNativeFloat(tmp_float);
#else
data[i].command_value = (float)tmp_float;
#endif
}
for (uint8_t i=0; i < data_count; i++) {
if (data[i].command_type != UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS) {
// this is the only type we support
continue;
}
periph.rcout_srv(data[i].actuator_id, data[i].command_value);
}
}
#endif // HAL_PERIPH_ENABLE_RC_OUT
#ifdef HAL_GPIO_PIN_SAFE_LED
/*
update safety LED
@ -731,7 +790,7 @@ static void onTransferReceived(CanardInstance* ins,
break;
#endif
#ifdef HAL_GPIO_PIN_SAFE_LED
#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
handle_safety_state(ins, transfer);
break;
@ -748,6 +807,16 @@ static void onTransferReceived(CanardInstance* ins,
handle_lightscommand(ins, transfer);
break;
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
handle_esc_rawcommand(ins, transfer);
break;
case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID:
handle_act_command(ins, transfer);
break;
#endif
}
}
@ -802,7 +871,7 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
*out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_SIGNATURE;
return true;
#endif
#ifdef HAL_GPIO_PIN_SAFE_LED
#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
*out_data_type_signature = ARDUPILOT_INDICATION_SAFETYSTATE_SIGNATURE;
return true;
@ -816,6 +885,15 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE;
return true;
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
return true;
case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID:
*out_data_type_signature = UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE;
return true;
#endif
default:
break;
@ -1229,7 +1307,9 @@ void AP_Periph_FW::can_update()
#ifdef HAL_PERIPH_ENABLE_MSP
msp_sensor_update();
#endif
#ifdef HAL_PERIPH_ENABLE_RC_OUT
rcout_update();
#endif
processTx();
processRx();
}

100
Tools/AP_Periph/rc_out.cpp Normal file
View File

@ -0,0 +1,100 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#ifdef HAL_PERIPH_ENABLE_RC_OUT
#include "AP_Periph.h"
// magic value from UAVCAN driver packet
// dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan
// Raw ESC command normalized into [-8192, 8191]
#define UAVCAN_ESC_MAX_VALUE 8191
#define SERVO_OUT_RCIN_MAX 16 // SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16
#define SERVO_OUT_MOTOR_MAX 12 // SRV_Channel::k_motor1 ... SRV_Channel::k_motor8, SRV_Channel::k_motor9 ... SRV_Channel::k_motor12
#if HAL_PWM_COUNT == 0
#error "You must define a PWM output in your hwdef.dat"
#endif
static bool has_new_data_to_update;
extern const AP_HAL::HAL &hal;
void AP_Periph_FW::rcout_init()
{
for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
servo_channels.set_default_function(i, SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + i));
}
for (uint8_t i=0; i<SERVO_OUT_RCIN_MAX; i++) {
SRV_Channels::set_angle(SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + i), 1000);
}
for (uint8_t i=0; i<SERVO_OUT_MOTOR_MAX; i++) {
SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), UAVCAN_ESC_MAX_VALUE);
}
}
void AP_Periph_FW::rcout_esc(int16_t *rc, uint8_t num_channels)
{
if (rc == nullptr) {
return;
}
const uint8_t channel_count = MIN(num_channels, SERVO_OUT_MOTOR_MAX);
for (uint8_t i=0; i<channel_count; i++) {
SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), rc[i]);
}
has_new_data_to_update = true;
}
void AP_Periph_FW::rcout_srv(uint8_t actuator_id, const float command_value)
{
if ((actuator_id == 0) || (actuator_id > HAL_PWM_COUNT)) {
// not supported or out of range
return;
}
const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1);
SRV_Channels::set_output_norm(function, command_value);
has_new_data_to_update = true;
}
void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state)
{
if (safety_state == 255) {
hal.rcout->force_safety_off();
} else {
hal.rcout->force_safety_on();
}
has_new_data_to_update = true;
}
void AP_Periph_FW::rcout_update()
{
if (!has_new_data_to_update) {
return;
}
has_new_data_to_update = false;
SRV_Channels::calc_pwm();
SRV_Channels::cork();
SRV_Channels::output_ch_all();
SRV_Channels::push();
}
#endif // HAL_PERIPH_ENABLE_RC_OUT

1
Tools/AP_Periph/wscript
View File

@ -37,6 +37,7 @@ def build(bld):
'AP_RangeFinder',
'AP_ROMFS',
'AP_MSP',
'SRV_Channel',
],
exclude_src=[
'libraries/AP_HAL_ChibiOS/Storage.cpp'