diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 87f1c97167..ca46800a92 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -749,10 +749,9 @@ void SITL_State::_simulator_servos(struct sitl_input &input) /* this maps the registers used for PWM outputs. The RC * driver updates these whenever it wants the channel output * to change */ - uint8_t i; if (last_update_usec == 0 || !output_ready) { - for (i=0; iwind_turbulance:0; input.wind.dir_z = wind_dir_z; - for (i=0; iget_num_motors() - 1; i++) { + for (uint8_t i=0; i < sitl_model->get_num_motors() - 1; i++) { float motor_throttle = constrain_float((input.servos[sitl_model->get_motors_offset() + i] - 1000) / 1000.0f, 0.0f, 1.0f); // update motor_on flag if (!is_zero(motor_throttle)) { @@ -857,7 +856,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input) } else { // run checks on each motor uint8_t running_motors = 0; - for (i=0; i < sitl_model->get_num_motors(); i++) { + for (uint8_t i=0; i < sitl_model->get_num_motors(); i++) { float motor_throttle = constrain_float((input.servos[i] - 1000) / 1000.0f, 0.0f, 1.0f); // update motor_on flag if (!is_zero(motor_throttle)) { @@ -880,7 +879,7 @@ void SITL_State::_simulator_servos(struct sitl_input &input) if (_sitl->state.battery_voltage <= 0) { if (_vehicle == ArduSub) { voltage = _sitl->batt_voltage; - for (i = 0; i < 6; i++) { + for (uint8_t i=0; i<6; i++) { float pwm = input.servos[i]; //printf("i: %d, pwm: %.2f\n", i, pwm); float fraction = fabsf((pwm - 1500) / 500.0f);