diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index ea174b2ef6..3733dbbe57 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -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 diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 41d719212f..3caee04cbd 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -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 diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 235791456c..f7e4dd70d5 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -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() {} diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 31ce8dbdb0..e5980382d4 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -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);