diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 0cbf5f419e..a3694e122c 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -568,6 +568,15 @@ public: uint16_t pool_peak_percent(); void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue); +#if AP_SIM_ENABLED + // update simulation of servos + void sim_update_actuator(uint8_t actuator_id); + struct { + uint32_t mask; + uint32_t last_send_ms; + } sim_actuator; +#endif + struct dronecan_protocol_t { CanardInstance canard; uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)]; diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp index 8feb3e0805..2d8df1079a 100644 --- a/Tools/AP_Periph/rc_out.cpp +++ b/Tools/AP_Periph/rc_out.cpp @@ -15,6 +15,9 @@ #include #ifdef HAL_PERIPH_ENABLE_RC_OUT #include "AP_Periph.h" +#if AP_SIM_ENABLED +#include +#endif // magic value from UAVCAN driver packet // dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan @@ -112,6 +115,9 @@ void AP_Periph_FW::rcout_srv_unitless(uint8_t actuator_id, const float command_v SRV_Channels::set_output_norm(function, command_value); rcout_has_new_data_to_update = true; +#if AP_SIM_ENABLED + sim_update_actuator(actuator_id); +#endif #endif } @@ -122,6 +128,9 @@ void AP_Periph_FW::rcout_srv_PWM(uint8_t actuator_id, const float command_value) SRV_Channels::set_output_pwm(function, uint16_t(command_value+0.5)); rcout_has_new_data_to_update = true; +#if AP_SIM_ENABLED + sim_update_actuator(actuator_id); +#endif #endif } @@ -172,4 +181,46 @@ void AP_Periph_FW::rcout_update() #endif } +#if AP_SIM_ENABLED +/* + update simulation of servos, sending actuator status +*/ +void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id) +{ + sim_actuator.mask |= 1U << actuator_id; + + // send status at 10Hz + const uint32_t period_ms = 100; + const uint32_t now_ms = AP_HAL::millis(); + + if (now_ms - sim_actuator.last_send_ms < period_ms) { + return; + } + sim_actuator.last_send_ms = now_ms; + + for (uint8_t i=0; i