在小角度内摆动可以较好的反应，但是大角度摆动延迟比较严重，而且很难回去，这样可以吗？
一般如果小角度下的动态跟好，这算解析完成了吗？
图像大概是这样：
以下是用arduino101做的：
[mw_shl_code=cpp,true]#include "CurieTimerOne.h"
#include <CurieIMU.h>
#include "MadgwickAHRS.h"
#include"struct_all.h"
//const unsigned int oneSecInUsec = 1000000; // A second in mirco second unit.
#define task_timeunit 1000; // 1000000 is one second in mirco second unit.所以1000为1ms
unsigned int cnt_1ms = 0;
unsigned int cnt_2ms = 0;
unsigned int cnt_4ms = 0;
uint8_t Rc_Lock = 1; //1 lock 0 not lock
int calibrateOffsets = 1; // int to determine whether calibration takes place or not
Madgwick filter; // initialise Madgwick object
//int factor = 800; // variable by which to divide gyroscope values, used to control sensitivity
// note that an increased baud rate requires an increase in value of factor
float convertRawAcceleration(int aRaw) {
// since we are using 2G range
// -2g maps to a raw value of -32768
// +2g maps to a raw value of 32767
float a = (aRaw * 16.0) / 32768.0;
return a;
}
float convertRawGyro(int gRaw) {
// since we are using 250 degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * 250.0) / 32768.0;
return g;
}
//timer interrupt
void timedBlinkIsr()
{
cnt_1ms++;
cnt_2ms++;
cnt_4ms++;
}
void setup() {
// initialize Serial communication
Serial.begin(9600);
// initialize device
CurieIMU.begin();
CurieIMU.setGyroRate(1000);
CurieIMU.setAccelerometerRate(1000);
filter.begin(1000);
// Set the accelerometer range to 2G
CurieIMU.setAccelerometerRange(2);
// Set the gyroscope range to 250 degrees/second
CurieIMU.setGyroRange(250);
// use the code below to calibrate accel/gyro offset values
if (calibrateOffsets == 1) {
Serial.println("Internal sensor offsets BEFORE calibration...");
Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getGyroOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getGyroOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getGyroOffset(Z_AXIS)); Serial.print("\t");
Serial.println("");
//IMU device must be resting in a horizontal position for the following calibration procedure to work correctly!
Serial.print("Starting Gyroscope calibration...");
CurieIMU.autoCalibrateGyroOffset();
Serial.println(" Done");
Serial.print("Starting Acceleration calibration...");
CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
Serial.println(" Done");
Serial.println("Internal sensor offsets AFTER calibration...");
Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(X_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Y_AXIS)); Serial.print("\t");
Serial.print(CurieIMU.getAccelerometerOffset(Z_AXIS)); Serial.print("\t");
Serial.println("");
}
unsigned int time = task_timeunit;
CurieTimerOne.start(time, &timedBlinkIsr);
// initialize digital pin 13 as an output.
pinMode(13, OUTPUT);
pinMode(12, OUTPUT);
pinMode(11, OUTPUT);
pinMode(10, OUTPUT);
}
void loop() {
if (cnt_1ms >= 1)
{
digitalWrite(10, HIGH);
cnt_1ms = 0;
Task_1000Hz();
digitalWrite(10, LOW);
}
if (cnt_2ms >= 2)
{
digitalWrite(11, HIGH);
cnt_2ms = 0;
Task_500Hz();
digitalWrite(11, LOW);
}
if (cnt_4ms >= 4)
{
digitalWrite(12, HIGH);
cnt_4ms = 0;
Task_250Hz();
digitalWrite(12, LOW);
}
}
void Task_1000Hz(void)
{
// read raw accel/gyro measurements from device
CurieIMU.readMotionSensor(acc.x, acc.y, acc.z, gyro.x, gyro.y, gyro.z);
// convert from raw data to gravity and degrees/second units
SI_acc.x = convertRawAcceleration(acc.x);
SI_acc.y = convertRawAcceleration(acc.y);
SI_acc.z = convertRawAcceleration(acc.z);
SI_gyro.x = convertRawGyro(gyro.x);
SI_gyro.y = convertRawGyro(gyro.y);
SI_gyro.z = convertRawGyro(gyro.z);
// Serial.print(SI_acc.x);
// Serial.print(",");
// Serial.print(SI_gyro.x);
// Serial.print("\n");
// use function from MagdwickAHRS.h to return quaternions
filter.updateIMU(SI_gyro.x, SI_gyro.y, SI_gyro.z, SI_acc.x, SI_acc.y, SI_acc.z);
}
void Task_500Hz(void)
{
//Control_Gyro(&SI_gyro, &Rc, Rc_Lock);
}
void Task_250Hz(void)
{
// functions to find yaw roll and pitch from quaternions
angle.yaw = filter.getYaw();
angle.roll = filter.getRoll();
angle.pitch = filter.getPitch();
// Serial.print(angle.yaw);
// Serial.print(",");
Serial.print(angle.roll);
Serial.print(",");
Serial.print(angle.pitch);
Serial.print("\n");
// Control_Angle(&out_angle, &Rc);
}[/mw_shl_code]
|
发表于 2016-5-15 00:43
