Browse Source

AP_Compass: make sure SITL rotation gets initialized like all the others

don't remove a custom rotation that has already been set
zr-v5.1
Andy Piper 5 years ago committed by Andrew Tridgell
parent
commit
f5320e8816
  1. 4
      libraries/AP_Compass/AP_Compass.h
  2. 1
      libraries/AP_Compass/AP_Compass_SITL.cpp

4
libraries/AP_Compass/AP_Compass.h

@ -224,7 +224,9 @@ public: @@ -224,7 +224,9 @@ public:
// set overall board orientation
void set_board_orientation(enum Rotation orientation, Matrix3f* custom_rotation = nullptr) {
_board_orientation = orientation;
_custom_rotation = custom_rotation;
if (custom_rotation) {
_custom_rotation = custom_rotation;
}
}
/// Set the motor compensation type

1
libraries/AP_Compass/AP_Compass_SITL.cpp

@ -24,6 +24,7 @@ AP_Compass_SITL::AP_Compass_SITL() @@ -24,6 +24,7 @@ AP_Compass_SITL::AP_Compass_SITL()
// save so the compass always comes up configured in SITL
save_dev_id(_compass_instance[_num_compass]);
set_rotation(instance, ROTATION_NONE);
_num_compass++;
}
}

Loading…
Cancel
Save