Check if the IMU is 'good' and periodically re-initialise if not.

master
Michael Hope 9 years ago
parent 3cab0d0e71
commit 0a335c01ae
  1. 10
      roverif/imu.cc
  2. 2
      roverif/imu.h
  3. 4
      roverif/main.cc

@ -27,7 +27,7 @@ enum Codes {
};
IMU::IMU()
: mpu_(MPU6050_ADDRESS_AD0_HIGH) {
: mpu_(MPU6050_ADDRESS_AD0_HIGH), good_(false) {
}
void IMU::init() {
@ -50,6 +50,7 @@ uint8_t IMU::wait() {
bool IMU::read(Protocol::IMU* pinto) {
if (!mpu_.testConnection()) {
good_ = false;
return false;
}
@ -59,9 +60,16 @@ bool IMU::read(Protocol::IMU* pinto) {
pinto->gyros + 0,
pinto->gyros + 1,
pinto->gyros + 2);
good_ = true;
return true;
}
void IMU::check() {
if (!good_) {
init();
}
}
#define ERRORX(x) { dbg("err " #x "\r\n"); return -x; }
#define ERROR(x) { return -x; }
#define DBG(x)

@ -10,6 +10,7 @@ public:
void init();
bool read(Protocol::IMU* pinto);
void check();
private:
friend class I2Cdev;
@ -21,5 +22,6 @@ private:
static int8_t set_address(uint8_t device, uint8_t address);
MPU6050 mpu_;
bool good_;
};

@ -19,6 +19,7 @@ uint8_t RoverIf::pilot_supplied_;
uint8_t RoverIf::demands_;
Timer imu_timer;
Timer check_timer;
Timer blinker_timer;
Timer heartbeat_timer;
Timer pwmin_limiter;
@ -202,6 +203,9 @@ void RoverIf::tick() {
defer(Pending::Heartbeat);
defer(Pending::Counters);
}
if (check_timer.tick(500)) {
imu.check();
}
}
#define DISPATCH_REQUEST(_name) \

Loading…
Cancel
Save