Login▼/회원가입BASIC4MCU | 질문게시판 | 아두이노코드 합치기 문의드립니다!
페이지 정보
작성자 Linie 작성일2022-09-13 20:56 조회2,664회 댓글0건첨부파일
본문
안녕하세요!
아두이노 메가보드를 이용해서 코드를 짜고있습니다.
칼만필터를 이용해서 자이로가속도센서(mpu 6050)를 제어하고 gps센서(neo_6M)를 함께 이용하고 있습니다.
가속도 자이로 값이 급격하게 변화할때 gps센서의 값을 이후 블루투스를 이용해서 어플로 전송하고자 합니다.
깃허브와 구글링을 통해서 각각의 코드를 찾아해맨끝에 각각 보드에 구동은 하는데 하나로 합치는데에 어려움을 겪고 있습니다.
이에 도움을 청하고자 문의를 드려봅니다ㅠ
첫번째 코드 (GPS)
#include <TinyGPS.h>
uint8_t _hour, _minute, _second, _year, _month, _day; // GPS로부터 시간값 읽기
#define GPSBAUD 9600
TinyGPS gps;
float latitude, longitude;
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
Serial.println("START...");
Serial1.begin(GPSBAUD);
Serial1.setTimeout(10);
gps = TinyGPS();
}
void loop() {
{
String temp = "";
while(Serial1.available())
{
char c = Serial1.read();
if(gps.encode(c))
{
getgps(gps);
}
}
if(0 < temp.length())
{
}
}
}
void getgps(TinyGPS &gps)
{
gps.f_get_position(&latitude, &longitude);
Serial.print("Lat/Long: ");
Serial.print(latitude,5);
Serial.print(", ");
Serial.println(longitude,5);
int year;
byte month, day, hour, minute, second, hundredths;
gps.crack_datetime(&year,&month,&day,&hour,&minute,&second,&hundredths);
// Print data and time
Serial.print("Date: "); Serial.print(month, DEC); Serial.print("/");
Serial.print(day, DEC); Serial.print("/"); Serial.print(year);
Serial.print(" Time: "); Serial.print(hour+9, DEC); Serial.print(":");
Serial.print(minute, DEC); Serial.print(":"); Serial.print(second, DEC);
Serial.print("."); Serial.println(hundredths, DEC);
_year = year-2000;
_month = month;
_day = day;
_hour = hour;
_minute = minute;
_second = second;
Serial.print("Altitude (meters): "); Serial.println(gps.f_altitude());
Serial.print("Course (degrees): "); Serial.println(gps.f_course());
Serial.print("Speed(kmph): "); Serial.println(gps.f_speed_kmph());
Serial.println();
unsigned long chars;
unsigned short sentences, failed_checksum;
gps.stats(&chars, &sentences, &failed_checksum);
Serial.print("Failed Checksums: ");Serial.print(failed_checksum);
Serial.println(); Serial.println();
delay(10000);
}
두번째코드(칼만필터를 이용한 자이로가속도센서)
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 accelgyro;
unsigned long now, lastTime = 0;
float dt;
int16_t ax, ay, az, gx, gy, gz;
float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0;
long axo = 0, ayo = 0, azo = 0;
long gxo = 0, gyo = 0, gzo = 0;
float pi = 3.1415926;
float AcceRatio = 16384.0;
float GyroRatio = 131.0;
uint8_t n_sample = 8;
float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0};
long aax_sum, aay_sum,aaz_sum;
float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0};
float Px=1, Rx, Kx, Sx, Vx, Qx;
float Py=1, Ry, Ky, Sy, Vy, Qy;
float Pz=1, Rz, Kz, Sz, Vz, Qz;
void setup()
{
Wire.begin();
Serial.begin(115200);
accelgyro.initialize();
unsigned short times = 200;
for(int i=0;i<times;i++)
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
axo += ax; ayo += ay; azo += az;
gxo += gx; gyo += gy; gzo += gz;
}
axo /= times; ayo /= times; azo /= times;
gxo /= times; gyo /= times; gzo /= times;
}
void loop()
{
unsigned long now = millis();
dt = (now - lastTime) / 1000.0;
lastTime = now;
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
float accx = ax / AcceRatio;
float accy = ay / AcceRatio;
float accz = az / AcceRatio;
aax = atan(accy / accz) * (-180) / pi;
aay = atan(accx / accz) * 180 / pi;
aaz = atan(accz / accy) * 180 / pi;
aax_sum = 0;
aay_sum = 0;
aaz_sum = 0;
for(int i=1;i<n_sample;i++)
{
aaxs[i-1] = aaxs[i];
aax_sum += aaxs[i] * i;
aays[i-1] = aays[i];
aay_sum += aays[i] * i;
aazs[i-1] = aazs[i];
aaz_sum += aazs[i] * i;
}
aaxs[n_sample-1] = aax;
aax_sum += aax * n_sample;
aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0;
aays[n_sample-1] = aay;
aay_sum += aay * n_sample;
aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
aazs[n_sample-1] = aaz;
aaz_sum += aaz * n_sample;
aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;
float gyrox = - (gx-gxo) / GyroRatio * dt;
float gyroy = - (gy-gyo) / GyroRatio * dt;
float gyroz = - (gz-gzo) / GyroRatio * dt;
agx += gyrox;
agy += gyroy;
agz += gyroz;
/* kalman start */
Sx = 0; Rx = 0;
Sy = 0; Ry = 0;
Sz = 0; Rz = 0;
for(int i=1;i<10;i++)
{
a_x[i-1] = a_x[i];
Sx += a_x[i];
a_y[i-1] = a_y[i];
Sy += a_y[i];
a_z[i-1] = a_z[i];
Sz += a_z[i];
}
a_x[9] = aax;
Sx += aax;
Sx /= 10;
a_y[9] = aay;
Sy += aay;
Sy /= 10;
a_z[9] = aaz;
Sz += aaz;
Sz /= 10;
for(int i=0;i<10;i++)
{
Rx += sq(a_x[i] - Sx);
Ry += sq(a_y[i] - Sy);
Rz += sq(a_z[i] - Sz);
}
Rx = Rx / 9;
Ry = Ry / 9;
Rz = Rz / 9;
Px = Px + 0.0025;
Kx = Px / (Px + Rx);
agx = agx + Kx * (aax - agx);
Px = (1 - Kx) * Px;
Py = Py + 0.0025;
Ky = Py / (Py + Ry);
agy = agy + Ky * (aay - agy);
Py = (1 - Ky) * Py;
Pz = Pz + 0.0025;
Kz = Pz / (Pz + Rz);
agz = agz + Kz * (aaz - agz);
Pz = (1 - Kz) * Pz;
/* kalman end */
Serial.print("\t FilteredX:");
Serial.print(agx);
Serial.print("\t FilteredY:");
Serial.print(agy);
Serial.print("\t FilteredZ:");
Serial.print(agz);Serial.println();
Serial.print("\n");
Serial.print("\n");
}
바쁘실텐데 긴 글 읽어주셔서 감사합니다 .
도움을 주신다면 정말 큰 도움이 될 것 같습니다.
다시 한번 감사합니다.
- BASIC4MCU 작성글 SNS에 공유하기
댓글 0
조회수 2,664등록된 댓글이 없습니다.