Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Periph: support a PWM -> UAVCAN Hardpoint adapter

Browse Source 
capture PWM input and map onto a hardpoint command
This commit is contained in:
Andrew Tridgell 2019-12-19 17:21:25 +11:00
parent f732a482fe
commit 9ed2f1d5e2
5 changed files with 77 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/AP_Periph/AP_Periph.cpp
View File

@ -125,6 +125,10 @@ void AP_Periph_FW::init()
serial_manager.set_protocol_and_baud(sernum, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud);
rangefinder.init(ROTATION_NONE);
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
pwm_hardpoint_init();
#endif
start_ms = AP_HAL::millis();
}

14
Tools/AP_Periph/AP_Periph.h
View File

@ -69,7 +69,19 @@ public:
#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
RangeFinder rangefinder;
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
void pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp);
void pwm_hardpoint_init();
void pwm_hardpoint_update();
struct {
uint8_t last_state;
uint32_t last_ts_us;
uint32_t last_send_ms;
uint16_t pwm_value;
} pwm_hardpoint;
#endif
// setup the var_info table
AP_Param param_loader{var_info};

4
Tools/AP_Periph/Parameters.cpp
View File

@ -84,6 +84,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
#ifdef HAL_PERIPH_ENABLE_ADSB
GSCALAR(adsb_baudrate, "ADSB_BAUDRATE", 57600),
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
GSCALAR(hardpoint_id, "HARDPOINT_ID", HAL_PWM_HARDPOINT_ID_DEFAULT),
#endif
AP_VAREND
};

5
Tools/AP_Periph/Parameters.h
View File

@ -24,6 +24,7 @@ public:
k_param_flash_bootloader,
k_param_rangefinder_baud,
k_param_adsb_baudrate,
k_param_hardpoint_id,
};
AP_Int16 format_version;
@ -48,6 +49,10 @@ public:
AP_Int32 adsb_baudrate;
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
AP_Int16 hardpoint_id;
#endif
Parameters() {}
};

51
Tools/AP_Periph/can.cpp
View File

@ -37,6 +37,7 @@
#include <uavcan/equipment/indication/BeepCommand.h>
#include <uavcan/equipment/indication/LightsCommand.h>
#include <uavcan/equipment/range_sensor/Measurement.h>
#include <uavcan/equipment/hardpoint/Command.h>
#include <ardupilot/indication/SafetyState.h>
#include <ardupilot/indication/Button.h>
#include <ardupilot/equipment/trafficmonitor/TrafficReport.h>
@ -1036,6 +1037,53 @@ void AP_Periph_FW::can_start()
can_wait_node_id();
}
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
void AP_Periph_FW::pwm_hardpoint_init()
{
hal.gpio->attach_interrupt(
PWM_HARDPOINT_PIN,
FUNCTOR_BIND_MEMBER(&AP_Periph_FW::pwm_irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH);
}
/*
called on PWM pin transition
*/
void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
{
if (pin_state == 0 && pwm_hardpoint.last_state == 1 && pwm_hardpoint.last_ts_us != 0) {
uint32_t width = timestamp - pwm_hardpoint.last_ts_us;
if (width > 500 && width < 2500) {
pwm_hardpoint.pwm_value = width;
}
}
pwm_hardpoint.last_state = pin_state;
pwm_hardpoint.last_ts_us = timestamp;
}
void AP_Periph_FW::pwm_hardpoint_update()
{
uint32_t now = AP_HAL::millis();
// send at 10Hz
if (now - pwm_hardpoint.last_send_ms >= 100) {
pwm_hardpoint.last_send_ms = now;
uavcan_equipment_hardpoint_Command cmd {};
cmd.hardpoint_id = g.hardpoint_id;
cmd.command = pwm_hardpoint.pwm_value;
uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE];
uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer);
canardBroadcast(&canard,
UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE,
UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID,
&transfer_id,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
}
#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT
void AP_Periph_FW::can_update()
{
@ -1058,6 +1106,9 @@ void AP_Periph_FW::can_update()
#endif
#ifdef HAL_GPIO_PIN_SAFE_BUTTON
can_safety_button_update();
#endif
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
pwm_hardpoint_update();
#endif
processTx();
processRx();