|
|
|
@ -536,8 +536,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
@@ -536,8 +536,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
|
|
|
|
_ts_opt_recovery = new TailsitterRecovery(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* initialize thermal corrections as we might not immediately get a topic update */ |
|
|
|
|
memset(&_sensor_correction, 0, sizeof(_sensor_correction)); |
|
|
|
|
/* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */ |
|
|
|
|
for (unsigned i = 0; i < 3; i++) { |
|
|
|
|
// used scale factors to unity
|
|
|
|
|
_sensor_correction.gyro_scale_0[i] = 1.0f; |
|
|
|
@ -1007,6 +1006,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
@@ -1007,6 +1006,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
|
|
|
|
|
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0]; |
|
|
|
|
rates(1) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; |
|
|
|
|
rates(2) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
rates(0) = _sensor_gyro.x; |
|
|
|
|
rates(1) = _sensor_gyro.y; |
|
|
|
|