AP_Periph: added HARDPOINT_RATE

and send highest pwm from last sample

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
 

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

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() {}

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);