Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 09:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed rpm indexing for vtol motors

Browse Source 
quadplane vtol motors start at 1
This commit is contained in:
Andrew Tridgell 2020-10-21 15:42:58 +11:00
parent a0bc9d3148
commit e8f21d3458
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -100,7 +100,7 @@ void AP_InertialSensor_SITL::generate_accel()
if (!is_zero(sitl->vibe_motor) && motors_on) {
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
float &phase = accel_motor_phase[i];
float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation);
float motor_freq = calculate_noise(sitl->state.rpm[sitl->state.vtol_motor_start+i] / 60.0f, freq_variation);
float phase_incr = motor_freq * 2 * M_PI / (accel_sample_hz * nsamples);
phase += phase_incr;
if (phase_incr > M_PI) {
@ -206,7 +206,7 @@ void AP_InertialSensor_SITL::generate_gyro()
// VIB_MOT_MAX is a rpm-scaled vibration applied to each axis
if (!is_zero(sitl->vibe_motor) && motors_on) {
for (uint8_t i = 0; i < sitl->state.num_motors; i++) {
float motor_freq = calculate_noise(sitl->state.rpm[i] / 60.0f, freq_variation);
float motor_freq = calculate_noise(sitl->state.rpm[sitl->state.vtol_motor_start+i] / 60.0f, freq_variation);
float phase_incr = motor_freq * 2 * M_PI / (gyro_sample_hz * nsamples);
float &phase = gyro_motor_phase[i];
phase += phase_incr;