最近想在Library中加入測試SSE指令集支援程度，然後看到這篇有cpuid這指令可用，功能選擇跟中斷很像根據eax決定。
2012年12月25日 星期二
2012年12月18日 星期二
wxWidgets Connect Arduino（Windows）
跟之前的wxWidgets Connect Arduino（Linux）一樣，只不過變成Windows版，因為只用Serial Port所以Enumeration就直接一個一個Create，如果改天要用其他如USB的Device寫成Driver會比較好處理，最後非同步有開/Od要處理WriteFile的ERROR_IO_PENDING而開/O2不用，另外我是在Windows 7 x64測試。
2012年12月7日 星期五
Arduino - SD Card
趁這兩天工作又被隊友delay又跑去研究其他姿態穩定的濾波演算法，Arduino IDE和Library好像沒有把數據存成文字檔的功能，也不想自己另外弄個receive程式，所以還是直接用SD Card去存，然後將文字資料丟到Google Doc就能製作成曲線圖方便觀察濾波效果，不過SPI我很少用所以常常忘記接法，所以還是記錄上來。
Arduino SD Card
Digital 4 (XCK) ----> CS
Digital 11(MOSI) ----> MOSI（DI）
Digital 12(MISO) ----> MISO（DO）
Digital 13(SCK) ----> SCLK（CK）
VCC 3.3V ----> VCC
GND ----> GND
存Heartbeat Sensor的值製成曲線圖。
#include <SD.h>
int sensor_pin = 0;
int n = 0;
int n_1 = 0;
int diff = 0;
int count = 0;
int elapse_up = 0;
int elapse_down = 0;
int integral_plus = 0;
int integral_minus = 0;
void setup()
{
Serial.begin(9600);
pinMode(10,OUTPUT);
if(!SD.begin(4)){
Serial.println("SD Card Error!");
return ;
}
}
void loop()
{
File file = SD.open("data.txt",FILE_WRITE);
n = analogRead(sensor_pin); //取得現在得到的值
diff = (n - n_1); //將現在的值減去上次得到的值(N -(N-1))
n_1 = n; //更新上一次的值
file.println(n / 5);
//如果差值趨近零，則表示到達波谷又要進到新的一個PWM中。
//並且在確定上個PWM的爬升（integral_plus）與下降（integral_minus）
//是否有到達積分值來確定是否有PWM。
//兩者成立則更新所有狀態。
if(diff < 10 && diff > -10 &&
integral_plus > 160 && integral_minus < -200){
if(elapse_up > 50 && elapse_up < 400 &&
elapse_down > 96 && elapse_down < 800){
++count;
Serial.print(count);
Serial.println(" Heartbeat");
}
elapse_up = 0;
elapse_down = 0;
integral_plus = 0;
integral_minus = 0;
}
else if(diff > 20 && diff < 280){
//PWM上升
integral_plus += diff;
elapse_up += 20;
}
else if(diff < -20 && diff > -200){
//PWM下降
integral_minus += diff;
elapse_down += 20;
}
file.close();
//每20ms取樣，即一秒取樣50次。
delay(20);
}
Kalman Filter
基本上所有參數跟成員的意思跟Simple Kalman Filter都差不多（參考預測與更新七個步驟），初始化r_measure是觀測誤差q_angle與q_bias是在加速度計與陀螺儀的預測選擇較相信誰arg_angle是初始角度，而member function 的 Update沒有空行的code表示是同一組算式，第一組計算當前陀螺儀的角速度減去前次計算出的偏移，接著將角度加上這個差的角度，第二組P是一個2x2 Matrix，內容是推導後的結果用來計算預測估計誤差，第三組量測現在加速度計角度與上次角度的差，第四組計算預測與觀測的Covariance，第五組計算最佳卡爾曼增益，第六組由卡爾曼增益乘上前次與這次角度差決定角度與偏移要更新多少，第七組則是用卡爾曼增益乘上當前預測估計誤差然後更新下個預測估計誤差，這七個步驟的前兩個是計算預測，後五個才是真正的更新，順序都與上面的wiki參考預測與更新七個步驟一樣，然後這整個系統的觀測時間統計是個隱馬爾可夫模型。
Kalman Filter：
Used：
下面初始參數適用幾乎所有的IMU除非有特別要自己設定。
Acc只有加速度計，CF用互補濾波加上陀螺儀修正，KF用卡爾曼濾波加上陀螺儀修正。
Kalman Filter：
#ifndef __KALMAN_FILTER__
#define __KALMAN_FILTER__
class KalmanFilter
{
public:
KalmanFilter(double r_measure,double q_angle,double q_bias,double arg_angle)
{
Q_angle = q_angle;
Q_bias = q_bias;
R_measure = r_measure;
angle = arg_angle;
bias = 0;
P[0][0] = 0;
P[0][1] = 0;
P[1][0] = 0;
P[1][1] = 0;
}
/* Accelerometer Angle , Gyro Rate , dt */
double Update(double acc,double gyro,double dt)
{
rate = gyro - bias;
angle += rate * dt;
P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
angle_diff = acc - angle;
S = P[0][0] + R_measure;
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
angle += K[0] * angle_diff;
bias += K[1] * angle_diff;
P[0][0] -= K[0] * P[0][0];
P[0][1] -= K[0] * P[0][1];
P[1][0] -= K[1] * P[0][0];
P[1][1] -= K[1] * P[0][1];
return angle;
}
private:
double R_measure;
double Q_angle;
double Q_bias;
double angle;
double bias;
double rate;
double angle_diff;
double S;
double P[2][2];
double K[2];
};
#endif
Used：
下面初始參數適用幾乎所有的IMU除非有特別要自己設定。
KalmanFilter KFx(0.03f,0.001f,0.003f,0.0f);
KalmanFilter KFy(0.03f,0.001f,0.003f,0.0f);
unsigned long last_KF_computing_time; //us
void loop()
{
double KF_x_angle = KFx.Update(AccAngleX,GyroRateYaw,
(double)(micros() - last_KF_computing_time) / 1000000.0f);
double KF_y_angle = KFx.Update(AccAngleY,GyroRatePitch,
(double)(micros() - last_KF_computing_time) / 1000000.0f);
last_KF_computing_time = micros();
}
2012年12月6日 星期四
Complementary Filter
互補濾波是另一種比較簡單取得平滑的採樣值的方法，計算起來比卡爾曼濾波簡單，要將兩個感測器的值互補只要對一方做高通濾波（陀螺儀瞬時動態較靈敏所以高頻的值較有效）一方做低通濾波（加速度計長時間靜態的值較準確所以低頻的值較有效）然後相加，但兩者乘上的係數相加要等於一，這篇文章有比較詳細的說明我是參考他的，將陀螺儀的角速度乘上時間微分dt然後連續積分就可以得到該時間累積的角度，再與加速度計的角度做互補濾波即可，另外WMP與Wii Nunchuk使用延伸模式時對I2C操作跟單純使用單一個不一樣（參考Activated Wii Motion Plus in Nunchuck passthrough mode），使用這個模式兩者好像會有3ms左右的採樣延遲（MultiWii Source Code － IMU.ino內的computeIMU注解說的，我沒計算過驗證）。
方向
Acc只有加速度計Noise突出很多，AccGyro有用陀螺儀做互補所以較平滑。
Complementary Filter.ino
#include <Wire.h>
uint8_t buffer[6];
int xID;
double AccAngleX = 0,AccAngleY = 0;
int GyroLastYaw = 0,GyroLastPitch = 0,GyroLastRoll = 0;
double GyroAngleYaw = 0,GyroAnglePitch = 0,GyroAngleRoll = 0;
double HighPass = 0.93f,LowPass = 0.07f;
double AngleX = 0,AngleY = 0;
unsigned long last_loop_time; //ms
unsigned long last_gyro_computing_time; //us
void setup()
{
Serial.begin(115200);
Wire.begin();
Init6DOF();
last_loop_time = millis();
last_gyro_computing_time = micros();
}
void loop()
{
if(millis() > (last_loop_time + 10)){
IICRead(buffer,0x52,6);
if((buffer[5] & 0x03) == 0x00){
ComputingNunchuk(buffer);
}
else if((buffer[5] & 0x03) == 0x02){
ComputingWMP(buffer);
}
ComplementaryFilter();
IICWrite(0x52,0x00);
last_loop_time = millis();
}
}
void Init6DOF()
{
delay(100);
IICWrite(0x53,0xFE,0x05);
Serial.println("Passthrough Mode Ok!");
delay(100);
IICWrite(0x53,0xF0,0x55);
Serial.println("Init WMP Ok!");
delay(100);
IICWrite(0x52,0xFA);
Serial.println("Set Reading Address 0xFA Ok!");
delay(100);
IICRead(buffer,0x52,6);
xID = buffer[0] + buffer[1] + buffer[2] +
buffer[3] + buffer[4] + buffer[5];
Serial.print("Extension Controller xID = 0x");
Serial.println(xID,HEX);
if(xID == 0xCB){
Serial.println("WMP Connected But Not Avtivared!");
}
if(xID == 0xCE){
Serial.println("WMP Connected And Avtivared!");
}
if(xID == 0x00){
Serial.println("WMP Not Connected!");
}
delay(100);
IICWrite(0x52,0x8);
Serial.println("Set Reading Address 0x08 Ok!");
delay(100);
IICWrite(0x52,0x00);
}
double Map(double value,double Input_Min,double Input_Max,double Output_Min,double Output_Max)
{
double rValue = (value - Input_Min) * (Output_Max - Output_Min) / (Input_Max - Input_Min) + Output_Min;
double rMin,rMax;
if(Output_Min < Output_Max){
rMin = Output_Min;
rMax = Output_Max;
}
else{
rMin = Output_Max;
rMax = Output_Min;
}
if(rValue < rMin){
return rMin;
}
if(rValue > rMax){
return rMax;
}
return rValue;
}
void ComputingNunchuk(uint8_t *buf)
{
int accel_x_axis = (buf[2] << 2) + ((buf[5] >> 3) & 2);
int accel_y_axis = (buf[3] << 2) + ((buf[5] >> 4) & 2);
int accel_z_axis = (buf[4] << 2) + ((buf[5] >> 5) & 6);
int mapX = Map(accel_x_axis,300.0f,700.0f,-90.0f,90.0f);
int mapY = Map(accel_y_axis,300.0f,700.0f,-90.0f,90.0f);
int mapZ = Map(accel_z_axis,360.0f,760.0f,-90.0f,90.0f);
AccAngleX = atan2(mapX,mapZ) / 3.14159 * 180.0f;
AccAngleY = atan2(mapY,mapZ) / 3.14159 * 180.0f;
/*
Serial.print(accel_x_axis);
Serial.print(" ");
Serial.print(accel_y_axis);
Serial.print(" ");
Serial.println(accel_z_axis);
*/
}
void ComputingWMP(uint8_t *buf)
{
int yaw = (((buf[5] & 0xFC) << 6) + buf[0]);
int pitch = (((buf[4] & 0xFC) << 6) + buf[1]);
int roll = (((buf[3] & 0xFC) << 6) + buf[2]);
double GyroDiffYaw = (yaw - GyroLastYaw) / 14.375f;
double GyroDiffPitch = (pitch - GyroLastPitch) / 14.375f;
double GyroDiffRoll = (roll - GyroLastRoll) / 14.375f;
GyroAngleYaw = GyroDiffYaw *
(double)(micros() - last_gyro_computing_time) / 1000000.0f;
GyroAnglePitch = GyroDiffPitch *
(double)(micros() - last_gyro_computing_time) / 1000000.0f;
GyroAngleRoll = GyroDiffRoll *
(double)(micros() - last_gyro_computing_time) / 1000000.0f;
last_gyro_computing_time = micros();
GyroLastYaw = yaw;
GyroLastPitch = pitch;
GyroLastRoll = roll;
/*
Serial.print(yaw);
Serial.print(" ");
Serial.print(pitch);
Serial.print(" ");
Serial.println(roll);
*/
/*
Serial.print(GyroAngleYaw);
Serial.print(" ");
Serial.print(GyroAnglePitch);
Serial.print(" ");
Serial.println(GyroAngleRoll);
*/
}
void ComplementaryFilter()
{
AngleX = (HighPass * (AngleX + GyroAngleYaw)) + (LowPass * AccAngleX);
AngleY = (HighPass * (AngleY + GyroAnglePitch)) + (LowPass * AccAngleY);
Serial.print(AngleX);
Serial.print(" ");
Serial.println(AngleY);
}
void IICWrite(uint8_t address,uint8_t register_address)
{
Wire.beginTransmission(address);
Wire.write(register_address);
Wire.endTransmission();
}
void IICWrite(uint8_t address,uint8_t register_address,uint8_t data)
{
Wire.beginTransmission(address);
Wire.write(register_address);
Wire.write(data);
Wire.endTransmission();
}
void IICRead(uint8_t *buf,uint8_t address,uint8_t length)
{
Wire.requestFrom(address,length);
for(int i = 0;Wire.available();++i){
buf[i] = Wire.read();
}
}
2012年12月2日 星期日
Windows Driver Loader
這個月有個工作是要用隊友給的driver去對某個設備做I/O操作控制，不過這幾天發現有時DriverMonitor掛驅動有的路徑會有問題，看call CreateService建在 HKEY_LOCAL_MACHINE\SYSTEM\CurrentControlSet\Services的登錄檔ImagePath可能因為編碼問題有點錯誤，也懶得寫INF而且我不太會用工具產生XD，還有個問題我不知道是什麼原因，用看系統service運作的工具理應掛載的測試driver應該不存在了，可是似乎無法重新用DriverMonitor啟動，非得重新開機才可以，前天為了這幾個問題還花幾小時去re DriverMonitor研究幾個function，不過看不出來流程有什麼差異或問題，但還是自己寫了一個常規Loader去處理...話說隊友給的driver還是一直有問題對I/O寫資料給設備有時還是會BSoD.........Orz
訂閱：
文章 (Atom)