Browse Source

fixed deltat in quaternion

master
Andrew Tridgell 13 years ago
parent
commit
c8a459ab0d
  1. 5
      libraries/AP_Quaternion/AP_Quaternion.cpp
  2. 2
      libraries/AP_Quaternion/AP_Quaternion.h
  3. 4
      libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde

5
libraries/AP_Quaternion/AP_Quaternion.cpp

@ -242,12 +242,15 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V @@ -242,12 +242,15 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
// Function to compute one quaternion iteration
void AP_Quaternion::update(float deltat)
void AP_Quaternion::update(void)
{
Vector3f gyro, accel;
float deltat;
_imu->update();
delta_t = _imu->get_delta_time();
// get current IMU state
gyro = _imu->get_gyro();
gyro.x = -gyro.x;

2
libraries/AP_Quaternion/AP_Quaternion.h

@ -43,7 +43,7 @@ public: @@ -43,7 +43,7 @@ public:
void set_compass(Compass *compass);
// Methods
void update(float deltat);
void update(void);
// Euler angles (radians)
float roll;

4
libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde

@ -111,7 +111,6 @@ void loop(void) @@ -111,7 +111,6 @@ void loop(void)
static uint16_t counter;
static uint32_t last_t, last_print, last_compass;
uint32_t now = micros();
float deltat;
static uint32_t compass_reads;
static uint32_t compass_time;
@ -119,7 +118,6 @@ void loop(void) @@ -119,7 +118,6 @@ void loop(void)
last_t = now;
return;
}
deltat = (now - last_t) * 1.0e-6;
last_t = now;
if (now - last_compass > 100*1000UL) {
@ -128,7 +126,7 @@ void loop(void) @@ -128,7 +126,7 @@ void loop(void)
last_compass = now;
}
quaternion.update(deltat);
quaternion.update();
counter++;
if (now - last_print >= 0.5e6) {

Loading…
Cancel
Save