Compass: sanity check instance in set_and_save_offsets

This commit is contained in:
Randy Mackay 2014-07-10 14:37:27 +09:00
parent 76369d153f
commit 16d4af8346
1 changed files with 5 additions and 2 deletions

View File

@ -162,8 +162,11 @@ Compass::init()
void
Compass::set_and_save_offsets(uint8_t i, const Vector3f &offsets)
{
_offset[i].set(offsets);
save_offsets(i);
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_offset[i].set(offsets);
save_offsets(i);
}
}
void