Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: use motor mask for noise checking for motors

This commit is contained in:
Andrew Tridgell 2022-08-18 17:22:01 +10:00 committed by Randy Mackay
parent d062688b30
commit e584a07610
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_HAL_SITL/SITL_State.cpp
View File

@ -808,8 +808,12 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
// do a little quadplane dance // do a little quadplane dance
float hover_throttle = 0.0f; float hover_throttle = 0.0f;
uint8_t running_motors = 0; uint8_t running_motors = 0;
for (uint8_t i=0; i < sitl_model->get_num_motors() - 1; i++) { uint32_t mask = _sitl->state.motor_mask;
float motor_throttle = constrain_float((input.servos[sitl_model->get_motors_offset() + i] - 1000) / 1000.0f, 0.0f, 1.0f); uint8_t bit;
while ((bit = __builtin_ffs(mask)) != 0) {
uint8_t motor = bit-1;
mask &= ~(1U<<motor);
float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
// update motor_on flag // update motor_on flag
if (!is_zero(motor_throttle)) { if (!is_zero(motor_throttle)) {
hover_throttle += motor_throttle; hover_throttle += motor_throttle;
@ -831,8 +835,12 @@ void SITL_State::_simulator_servos(struct sitl_input &input)
} else { } else {
// run checks on each motor // run checks on each motor
uint8_t running_motors = 0; uint8_t running_motors = 0;
for (uint8_t i=0; i < sitl_model->get_num_motors(); i++) { uint32_t mask = _sitl->state.motor_mask;
float motor_throttle = constrain_float((input.servos[i] - 1000) / 1000.0f, 0.0f, 1.0f); uint8_t bit;
while ((bit = __builtin_ffs(mask)) != 0) {
const uint8_t motor = bit-1;
mask &= ~(1U<<motor);
float motor_throttle = constrain_float((input.servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
// update motor_on flag // update motor_on flag
if (!is_zero(motor_throttle)) { if (!is_zero(motor_throttle)) {
throttle += motor_throttle; throttle += motor_throttle;