AP_AHRS: make it possible to setup board orientation at runtime

This commit is contained in:
Andrew Tridgell 2013-06-03 16:52:28 +10:00
parent 4c9bc30f58
commit aaaa5247e3
1 changed files with 10 additions and 1 deletions

View File

@ -46,7 +46,7 @@ public:
// init sets up INS board orientation
virtual void init() {
_ins->set_board_orientation((enum Rotation)_board_orientation.get());
set_orientation();
};
// Accessors
@ -60,10 +60,19 @@ public:
void set_compass(Compass *compass) {
_compass = compass;
set_orientation();
}
// allow for runtime change of orientation
// this makes initial config easier
void set_orientation() {
_ins->set_board_orientation((enum Rotation)_board_orientation.get());
if (_compass != NULL) {
_compass->set_board_orientation((enum Rotation)_board_orientation.get());
}
}
void set_airspeed(AP_Airspeed *airspeed) {
_airspeed = airspeed;
}