aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Periph: added HARDPOINT_RATE

Browse Source 
and send highest pwm from last sample
This commit is contained in:
Andrew Tridgell 2019-12-19 18:46:03 +11:00
parent 41b49768b8
commit d249f870b9
4 changed files with 15 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/AP_Periph/AP_Periph.h
View File

@ -79,6 +79,7 @@ public:
uint32_t last_ts_us;
uint32_t last_send_ms;
uint16_t pwm_value;
uint16_t highest_pwm;
} pwm_hardpoint;
#endif

1
Tools/AP_Periph/Parameters.cpp
View File

@ -87,6 +87,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
GSCALAR(hardpoint_id, "HARDPOINT_ID", HAL_PWM_HARDPOINT_ID_DEFAULT),
GSCALAR(hardpoint_rate, "HARDPOINT_RATE", 100),
#endif
AP_VAREND

2
Tools/AP_Periph/Parameters.h
View File

@ -25,6 +25,7 @@ public:
k_param_rangefinder_baud,
k_param_adsb_baudrate,
k_param_hardpoint_id,
k_param_hardpoint_rate,
};
AP_Int16 format_version;
@ -51,6 +52,7 @@ public:
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
AP_Int16 hardpoint_id;
AP_Int8 hardpoint_rate;
#endif
Parameters() {}

13
Tools/AP_Periph/can.cpp
View File

@ -1055,6 +1055,9 @@ void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timesta
uint32_t width = timestamp - pwm_hardpoint.last_ts_us;
if (width > 500 && width < 2500) {
pwm_hardpoint.pwm_value = width;
if (width > pwm_hardpoint.highest_pwm) {
pwm_hardpoint.highest_pwm = width;
}
}
}
pwm_hardpoint.last_state = pin_state;
@ -1065,11 +1068,17 @@ void AP_Periph_FW::pwm_hardpoint_update()
{
uint32_t now = AP_HAL::millis();
// send at 10Hz
if (now - pwm_hardpoint.last_send_ms >= 100) {
void *save = hal.scheduler->disable_interrupts_save();
uint16_t value = pwm_hardpoint.highest_pwm;
pwm_hardpoint.highest_pwm = 0;
hal.scheduler->restore_interrupts(save);
float rate = g.hardpoint_rate;
rate = constrain_float(rate, 10, 100);
if (value > 0 && now - pwm_hardpoint.last_send_ms >= 1000U/rate) {
pwm_hardpoint.last_send_ms = now;
uavcan_equipment_hardpoint_Command cmd {};
cmd.hardpoint_id = g.hardpoint_id;
cmd.command = pwm_hardpoint.pwm_value;
cmd.command = value;
uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE];
uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer);