Skip to content

Commit

Permalink
SITL: AP_Periph: sim_update_actuator report IDs that match AP chann…
Browse files Browse the repository at this point in the history
…el indexes
  • Loading branch information
IamPete1 committed Jan 24, 2025
1 parent 6413da9 commit 6236d2d
Showing 1 changed file with 2 additions and 2 deletions.
4 changes: 2 additions & 2 deletions Tools/AP_Periph/rc_out.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -187,7 +187,7 @@ void AP_Periph_FW::rcout_update()
*/
void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id)
{
sim_actuator.mask |= 1U << actuator_id;
sim_actuator.mask |= 1U << (actuator_id - 1);

// send status at 10Hz
const uint32_t period_ms = 100;
Expand All @@ -202,7 +202,7 @@ void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id)
if ((sim_actuator.mask & (1U<<i)) == 0) {
continue;
}
const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1);
const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + i);
uavcan_equipment_actuator_Status pkt {};
pkt.actuator_id = i;
// assume 45 degree angle for simulation
Expand Down

0 comments on commit 6236d2d

Please sign in to comment.