Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

IMU: abstract the setting of the LEDs in IMU initialisation

Browse Source 
we need this to cope with different LED pin assignments on APM1/APM2
This commit is contained in:
Andrew Tridgell 2011-12-13 18:19:12 +11:00
parent 888d6226b2
commit a819c1a3dc
5 changed files with 44 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_IMU/AP_IMU_INS.cpp
View File

@ -21,15 +21,11 @@
#include "AP_IMU_INS.h"
// XXX secret knowledge about the APM/oilpan wiring
//
#define A_LED_PIN 37
#define C_LED_PIN 35
void
AP_IMU_INS::init( Start_style style,
void (*delay_cb)(unsigned long t),
AP_PeriodicProcess * scheduler )
void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on),
AP_PeriodicProcess * scheduler )
{
_ins->init(scheduler);
// if we are warm-starting, load the calibration data from EEPROM and go
@ -39,7 +35,7 @@ AP_IMU_INS::init( Start_style style,
} else {
// do cold-start calibration for both accel and gyro
_init_gyro(delay_cb);
_init_gyro(delay_cb, flash_leds_cb);
// save calibration
_sensor_cal.save();
@ -49,21 +45,23 @@ AP_IMU_INS::init( Start_style style,
/**************************************************/
void
AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t))
AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
_init_gyro(delay_cb);
_init_gyro(delay_cb, flash_leds_cb);
_sensor_cal.save();
}
#define FLASH_LEDS(on) do { if (flash_leds_cb != NULL) flash_leds_cb(on); } while (0)
void
AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t))
AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
int flashcount = 0;
float adc_in;
float prev[3] = {0,0,0};
float total_change;
float max_offset;
float ins_gyro[6];
float ins_gyro[6];
// cold start
delay_cb(500);
@ -72,15 +70,13 @@ AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t))
for(int c = 0; c < 25; c++){
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, HIGH);
FLASH_LEDS(true);
delay_cb(20);
_ins->update();
_ins->get_gyros(ins_gyro);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(C_LED_PIN, LOW);
FLASH_LEDS(false);
delay_cb(20);
}
@ -112,14 +108,12 @@ AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t))
delay_cb(20);
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, HIGH);
FLASH_LEDS(true);
}
if(flashcount >= 10) {
flashcount = 0;
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
FLASH_LEDS(false);
}
flashcount++;
}
@ -138,21 +132,21 @@ AP_IMU_INS::save()
}
void
AP_IMU_INS::init_accel(void (*delay_cb)(unsigned long t))
AP_IMU_INS::init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
_init_accel(delay_cb);
_init_accel(delay_cb, flash_leds_cb);
_sensor_cal.save();
}
void
AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t))
AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
int flashcount = 0;
float adc_in;
float prev[6] = {0,0,0};
float total_change;
float max_offset;
float ins_accel[3];
float ins_accel[3];
// cold start
@ -185,14 +179,12 @@ AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t))
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, HIGH);
FLASH_LEDS(true);
}
if(flashcount >= 10) {
flashcount = 0;
digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH);
FLASH_LEDS(false);
}
flashcount++;
}

13
libraries/AP_IMU/AP_IMU_INS.h
View File

@ -43,11 +43,14 @@ public:
///
virtual void init( Start_style style = COLD_START,
void (*delay_cb)(unsigned long t) = delay,
void (*flash_leds_cb)(bool on) = NULL,
AP_PeriodicProcess *scheduler = NULL );
virtual void save();
virtual void init_accel(void (*delay_cb)(unsigned long t) = delay);
virtual void init_gyro(void (*delay_cb)(unsigned long t) = delay);
virtual void init_accel(void (*delay_cb)(unsigned long t) = delay,
void (*flash_leds_cb)(bool on) = NULL);
virtual void init_gyro(void (*delay_cb)(unsigned long t) = delay,
void (*flash_leds_cb)(bool on) = NULL);
virtual bool update(void);
// for jason
@ -67,8 +70,10 @@ private:
AP_InertialSensor *_ins; ///< INS provides an axis and unit correct sensor source.
AP_VarA<float,6> _sensor_cal; ///< Calibrated sensor offsets
virtual void _init_accel(void (*delay_cb)(unsigned long t)); ///< no-save implementation
virtual void _init_gyro(void (*delay_cb)(unsigned long t)); ///< no-save implementation
virtual void _init_accel(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on) = NULL); ///< no-save implementation
virtual void _init_gyro(void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on) = NULL); ///< no-save implementation
float _calibrated(uint8_t channel, float ins_value);

10
libraries/AP_IMU/AP_IMU_Shim.h
View File

@ -15,10 +15,14 @@ public:
/// @name IMU protocol
//@{
virtual void init(Start_style style = COLD_START, void (*callback)(unsigned long t) = delay,
virtual void init(Start_style style = COLD_START,
void (*callback)(unsigned long t) = delay,
void (*flash_leds_cb)(bool on) = NULL,
AP_PeriodicProcess *scheduler = NULL) {};
virtual void init_accel(void (*callback)(unsigned long t) = delay) {};
virtual void init_gyro(void (*callback)(unsigned long t) = delay) {};
virtual void init_accel(void (*callback)(unsigned long t) = delay,
void (*flash_leds_cb)(bool on) = NULL) {};
virtual void init_gyro(void (*callback)(unsigned long t) = delay,
void (*flash_leds_cb)(bool on) = NULL) {};
virtual bool update(void) {
bool updated = _updated;
_updated = false;

9
libraries/AP_IMU/IMU.cpp
View File

@ -11,14 +11,15 @@ IMU::IMU () {}
void IMU::init( Start_style style,
void (*delay_cb)(unsigned long t),
AP_PeriodicProcess * scheduler )
void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on),
AP_PeriodicProcess * scheduler )
{ }
void IMU::init_accel(void (*delay_cb)(unsigned long t))
void IMU::init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{ }
void IMU::init_gyro(void (*delay_cb)(unsigned long t))
void IMU::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{ }
bool IMU::update(void) { return false; }

5
libraries/AP_IMU/IMU.h
View File

@ -38,6 +38,7 @@ public:
///
virtual void init( Start_style style,
void (*delay_cb)(unsigned long t),
void (*flash_leds_cb)(bool on),
AP_PeriodicProcess * scheduler );
/// Perform cold startup initialisation for just the accelerometers.
@ -45,14 +46,14 @@ public:
/// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work.
///
virtual void init_accel(void (*callback)(unsigned long t));
virtual void init_accel(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on));
/// Perform cold-start initialisation for just the gyros.
///
/// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work
///
virtual void init_gyro(void (*callback)(unsigned long t));
virtual void init_gyro(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on));
/// Give the IMU some cycles to perform/fetch an update from its
/// sensors.