CLatLongPoint CGnomonicProjection::InverseProjectPoint(const CXYPoint& p, const CWorld3DModel& worldModel)
/****************************************************/
{
//Inverse Gnomonic Projection (from J. Snyder - Map Projections: A Working Manual)
double const a = worldModel.SemiMajorAxis();
double const lambda0 = toRad(OriginMeridian());
double const phi1 = toRad(OriginParallel());
double const x = p.X();
double const y = p.Y();
double rho = Calculate_rho(p);
double c = Calculate_c(p, worldModel);
double phi = asin(cos(c)*sin(phi1)+(y*sin(c)*cos(phi1)/rho));
double lambda = 0.0;
if ((phi1<90.0)&&(phi1>-90.0))
{
lambda = lambda0 + atan(x*sin(c)/(rho*cos(phi1)*cos(c)-y*sin(phi1)*sin(c)));
}
else if (phi1==90)
{
lambda = lambda0 + atan(x/(-y));
}
else if (phi1==-90)
{
lambda = lambda0 + atan(x/y);
}
return CLatLongPoint(toDeg(phi), toDeg(lambda));
}
/*
* GGA Global Positioning System Fix Data. Time, Position and fix related data for a GPS receiver
*
* 11
* 1 2 3 4 5 6 7 8 9 10 | 12 13 14
* | | | | | | | | | | | | | |
* hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx
* 1) Time (UTC)
* 2) Latitude
* 3) N or S (North or South)
* 4) Longitude
* 5) E or W (East or West)
* 6) GPS Quality Indicator,
* 0 - fix not available,
* 1 - GPS fix,
* 2 - Differential GPS fix
* 7) Number of satellites in view, 00 - 12
* 8) Horizontal Dilution of precision
* 9) Antenna Altitude above/below mean-sea-level (geoid)
* 10) Units of antenna altitude, meters
* 11) Geoidal separation, the difference between the WGS-84 earth
* ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level below ellipsoid
* 12) Units of geoidal separation, meters
* 13) Age of differential GPS data, time in seconds since last SC104
* type 1 or 9 update, null field when DGPS is not used
* 14) Differential reference station ID, 0000-1023
*/
static void parseGGA(un8 index, char *value)
{
if (*value == 0)
return;
switch (index)
{
case GPS_GGA_QUALITY_INDICATOR:
/* 1+: fix; 0: no fix */
veVariantUn8(&local.fix, value[0] != '0');
return;
case GPS_GGA_LATITUDE:
errno = 0;
veVariantFloat(&local.latitude, toDeg((float)atof(value)));
if (errno)
veVariantInvalidate(&local.latitude);
return;
case GPS_GGA_LAT_NORTH_SOUTH:
/* North is positive, south is negative */
if (value[0] == 'S')
local.latitude.value.Float *= -1;
return;
case GPS_GGA_LONGITUDE:
errno = 0;
veVariantFloat(&local.longitude, toDeg((float)atof(value)));
if (errno)
veVariantInvalidate(&local.longitude);
return;
case GPS_GGA_LONG_EAST_WEST:
/* East is positive, west is negative */
if (value[0] == 'W')
local.longitude.value.Float *= -1;
return;
case GPS_GGA_ALTITUDE:
errno = 0;
veVariantFloat(&local.altitude, (float)atof(value));
if (errno)
veVariantInvalidate(&local.altitude);
return;
case GPS_GGA_NR_OF_SATELITES:
errno = 0;
veVariantUn8(&local.nrOfSats, (un8)atol(value));
if (errno)
veVariantInvalidate(&local.nrOfSats);
return;
}
}
bool DistVincenty(double lat1, double lon1, double lat2, double lon2, double *dist, double *fwdAz, double *revAz){
InverseResult result;
LLPoint pt1, pt2;
pt1.latitude=toRad(lat1);
pt1.longitude=toRad(lon1);
pt2.latitude=toRad(lat2);
pt2.longitude=toRad(lon2);
bool test=DistVincenty( pt1, pt2, & result);
*dist = mtoNM(result.distance);
*fwdAz = toDeg(result.azimuth);
*revAz = toDeg(result.azimuth);
return test;
}
/**
* Calculates destination point given start point lat/long, bearing & distance,
* using Vincenty inverse formula for ellipsoids
*
* -> double lat1, lon1: first point in decimal degrees
* -> double brng: initial bearing in decimal degrees
* -> double dist: distance along bearing in nautical miles
* <- double lat2, lat2 final point in decimal degrees
* <- double final bearing in decimal degrees
*/
void DestVincenty(double lat1, double lon1, double brng, double distNM, double* lat2, double* lon2, double* revAz) {
LLPoint pt;
pt.latitude=toRad(lat1);
pt.longitude=toRad(lon1);
double distm=NmToMeters(distNM);
double brngrad=toRad(brng);
double revAzrad;
LLPoint Result_dest=DestVincenty(pt, brngrad, distm, &revAzrad );
*lat2=toDeg(Result_dest.latitude);
*lon2=toDeg(Result_dest.longitude);
*revAz =toDeg(revAzrad); // final bearing, if required
}
float TS_Util::bind(std::string op, float a)
{
if (op == "sin") return (float) sin(a);
if (op == "cos") return (float) cos(a);
if (op == "tan") return (float) tan(a);
if (op == "csc") return safeDiv2(1.0f, (float) sin(a));
if (op == "sec") return safeDiv2(1.0f, (float) cos(a));
if (op == "cot") return safeDiv2(1.0f, (float) tan(a));
if (op == "asin") return (float) asin(a);
if (op == "acos") return (float) acos(a);
if (op == "atan") return (float) atan(a);
if (op == "rad") return (float) toRad(a);
if (op == "deg") return (float) toDeg(a);
if (op == "ln") return (float) log(a);
if (op == "log") return (float) log10(a);
if (op == "exp") return (float) exp(a);
if (op == "abs") return (float) fabs(a);
if (op == "sgn") return sgn(a);
if (op == "neg") return -a;
if (op == "not") return !a;
if (op == "bnot") return (float) ~((int) a);
if (op == "int") return (float) ((int) a);
if (op == "floor") return (float) ((int) a);
if (op == "ciel") return (float) ((int) a + 1);
if (op == "round") return (float) ((int) (a + 0.5f));
if (op == "rnd") return !a ? rnd() : rndEx(0, a);
if (op == "sqrt") return (float) sqrt(a);
if (op == "fact") return (float) fact((int) a);
throw Exception("unary operator not found");
}
void renderPlayer(Display *gDisplay, Assets *gAssets, ControlState *gControlState) {
int cx, cy;
double rotate;
cy = gAssets->rPlayer.y + P_IMAGE_H / 2;
cx = gAssets->rPlayer.x + P_IMAGE_W / 2;
rotate = toDeg(atan2(gControlState->mouseY - cy, gControlState->mouseX - cx));
SDL_RenderCopyEx(gDisplay->renderer, gAssets->tPlayer, NULL, &gAssets->rPlayer, rotate, NULL, SDL_FLIP_NONE);
}
bool destRhumb(double lat1, double lon1, double brng, double dist, double* lat2, double* lon2) {
lat1=toRad(lat1);
lon1=toRad(lon1);
double d=NMtorad(dist);
double tc=toRad(brng);
double lat= lat1+d*cos(tc);
if (std::abs(lat) > M_PI/2) return false;//"d too large. You can't go this far along this rhumb line!"
double q;
if (std::abs(lat-lat1) < sqrt(Tol())){
q=cos(lat1);
} else {
double dphi=log(tan(lat/2+M_PI/4)/tan(lat1/2+M_PI/4));
q= (lat-lat1)/dphi;
}
double dlon=-d*sin(tc)/q;
*lon2=toDeg(mod(lon1+dlon+M_PI,2*M_PI)-M_PI);
*lat2=toDeg(lat);
return true;
}
int calculateBearing(float lon1, float lat1, float lon2, float lat2) {
// bearing calc, feeded in radian, output degrees
float a;
a = atan2(sin(lon2 - lon1)*cos(lat2), cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(lon2 - lon1));
a = toDeg(a);
if (a < 0) a = 360 + a;
a = 360-a;
return (int)round(a);
}
void tracing(Bullet *bullet, World *world) {
//bullet->velocity *= (1.0 - 0.002);
double deltaX = (double) world->player.x - bullet->x;
double deltaY = (double) bullet->y - world->player.y;
double thetaCorrection = toDeg(atan2(deltaY, deltaX));
bullet->theta = thetaCorrection;
}
void global_aimming_angle(float *yaw,float *pitch)
{
float diffx=0;
float diffy=0;
float diffz=0;
float diffxy=0;
diffx = target.x - gps.x;
diffy = target.y - gps.y;
diffz = target.z - gps.z;
/*vector in global frame*/
vector_x = diffx;
vector_y = diffy;
vector_z = diffz;
//printf("%f,%f,%f\r\n",vector_x,vector_y,vector_z);
diffxy = sqrt(pow2(diffx)+pow2(diffy));
*yaw = toDeg(atan2f(diffx,diffy));
*pitch = toDeg(atan2f(diffz,diffxy));
}
void body_aimming_angle()
{
global_aimming_angle(&(global_yaw),&(global_pitch));
/*transform global vetor form global to body*/
vector_x_fc = vector_y;
vector_y_fc = -vector_x;
vector_z_fc = vector_z;
//printf("%f,%f,%f\r\n",vector_x_fc,vector_y_fc,vector_z_fc);
/*turn body by yaw,pitch,roll*/
vector_body_x = Mq11*vector_x_fc + Mq12*vector_y_fc + Mq13*vector_z_fc;
vector_body_y = Mq21*vector_x_fc + Mq22*vector_y_fc + Mq23*vector_z_fc;
vector_body_z = Mq31*vector_x_fc + Mq32*vector_y_fc + Mq33*vector_z_fc;
//printf("%f,%f,%f\r\n",vector_body_x,vector_body_y,vector_body_z);
//printf("global_yaw,%f,global_pitch,%f\r\n",global_yaw,global_pitch);
body_yaw = (atan2f(vector_body_y,vector_body_x));
body_pitch = toDeg(atan2f(-vector_body_z,(arm_cos_f32(body_yaw) * vector_body_x + arm_sin_f32(body_yaw) * vector_body_y)));
body_yaw = toDeg(body_yaw);
if(body_pitch > 90.0)
{
body_yaw = body_yaw + 180.0;
body_pitch= 180.0 - body_pitch;
}
/* else if(body_pitch < 0.0)
{
body_yaw = body_yaw + 180.0;
body_pitch= -body_pitch;
}
*/
//printf("body_yaw,%f,body_pitch,%f\r\n",body_yaw,body_pitch);
}
/**
* Calculates rhumb line distance between two points specified by latitude/longitude
* http://williams.best.vwh.net/avform.htm#Rhumb
* -> double lat1, lon1: first point in decimal degrees
* -> double lat2, lon2: second point in decimal degrees
* <- double dist: distance along bearing in nautical miles
* <- double bearing in decimal degrees
* As stated in the introduction, North latitudes and West longitudes are treated as positive,
* and South latitudes and East longitudes negative. It's easier to go with the flow, but if
* you prefer another convention you can change the signs in the formulae.
*/
void distRhumb(double lat1,double lon1, double lat2, double lon2, double *distance, double *brng){
lat1=toRad(lat1);
lat2=toRad(lat2);
lon1=toRad(lon1);
lon2=toRad(lon2);
double dlon_W=mod(lon2-lon1,(2*M_PI));
double dlon_E=mod(lon1-lon2,(2*M_PI));
double dphi=log(tan(lat2/2+M_PI/4)/tan(lat1/2+M_PI/4));
double q=0;
if (std::abs(lat2-lat1) < sqrt(Tol())){
q=cos(lat1);
} else {
q= (lat2-lat1)/dphi;
}
if (dlon_W < dlon_E){// Westerly rhumb line is the shortest
*brng=toDeg(mod(atan2(-dlon_W,dphi),(2*M_PI)));
*distance= radtoNM(sqrt(sqr(q)*(sqr(dlon_W)) + sqr(lat2-lat1)));
} else{
*brng=toDeg(mod(atan2(dlon_E,dphi),(2*M_PI)));
*distance= radtoNM(sqrt(sqr(q)*(sqr(dlon_E)) + sqr(lat2-lat1)));
}
}
int calculateElevation(float lon1, float lat1, float lon2, float lat2, int alt) {
// feeded in radian, output in degrees
float a, el, c, d, dLat, dLon;
//calculating distance between uav & home
dLat = (lat2 - lat1);
dLon = (lon2 - lon1);
a = sin(dLat / 2) * sin(dLat / 2) + sin(dLon / 2) * sin(dLon / 2) * cos(lat1) * cos(lat2);
c = 2 * asin(sqrt(a));
d = (R * c);
uavDistanceToHome = d;
el = atan((float)alt / (10 * d));// in radian
el = toDeg(el); // in degree
return (int)round(el);
}
/*====================================================================================================*/
void SysTick_Handler( void )
{
u16 MOTOR[4] = {0};
s16 Pitch = 0, Roll = 0, Yaw = 0;
static u16 SysTick_Cnt = 0;
static s16 *FIFO_X, *FIFO_Y, *FIFO_Z;
static s16 FIFO_ACC[3][16] = {0}, FIFO_GYR[3][16] = {0}/*, FIFO_MAG[3][16] = {0}*/;
static u32 Correction_Time = 0;
/* Time Count */
if(SysTick_Cnt == SampleRateFreg) {
SysTick_Cnt = 0;
Time_Sec++;
if(Time_Sec == 60) { // 0~59
Time_Sec = 0;
Time_Min++;
if(Time_Sec == 60)
Time_Min = 0;
}
}
SysTick_Cnt++;
/* 500Hz, Read Accelerometer, Gyroscope, Magnetometer */
Sensor_Read(SampleRateFreg);
/* Offset */
Acc.X -= Acc.OffsetX;
Acc.Y -= Acc.OffsetY;
Acc.Z -= Acc.OffsetZ;
Gyr.X -= Gyr.OffsetX;
Gyr.Y -= Gyr.OffsetY;
Gyr.Z -= Gyr.OffsetZ;
#define MAFIFO_SIZE 250
switch(SEN_STATE) {
/************************** CorrectSelect ***********************************/
case SEN_CORR:
// SEN_STATE = (KEY == KEY_ON) ? SEN_GYR : SEN_NUMQ;
SEN_STATE = SEN_GYR;
break;
/************************** CorrectGyr **************************************/
case SEN_GYR:
switch((u16)(Correction_Time/SampleRateFreg)) {
case 0: // 分配記憶體給 MaveAve 使用
FIFO_X = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Y = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Z = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
memset(FIFO_X, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Y, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Z, 0, MAFIFO_SIZE*sizeof(s16));
Correction_Time = SampleRateFreg;
break;
case 1: // 等待 FIFO 填滿靜態資料
/* 移動平均 Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, FIFO_X, MAFIFO_SIZE);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, FIFO_Y, MAFIFO_SIZE);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, FIFO_Z, MAFIFO_SIZE);
Correction_Time++;
break;
case 2: // 釋放記憶體 & 計算陀螺儀偏移量
free(FIFO_X);
free(FIFO_Y);
free(FIFO_Z);
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SEN_STATE = SEN_ACC;
break;
}
break;
/************************** CorrectAcc **************************************/
case SEN_ACC:
switch((u16)(Correction_Time/SampleRateFreg)) {
case 0: // 分配記憶體給 MaveAve 使用
FIFO_X = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Y = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Z = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
memset(FIFO_X, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Y, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Z, 0, MAFIFO_SIZE*sizeof(s16));
Correction_Time = SampleRateFreg;
break;
case 1: // 等待 FIFO 填滿靜態資料
/* 移動平均 Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, FIFO_X, MAFIFO_SIZE);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, FIFO_Y, MAFIFO_SIZE);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, FIFO_Z, MAFIFO_SIZE);
Correction_Time++;
break;
case 2: // 釋放記憶體 & 計算加速度計偏移量
free(FIFO_X);
free(FIFO_Y);
free(FIFO_Z);
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
// SEN_STATE = SEN_MAG;
SEN_STATE = SEN_NUMQ;
break;
}
break;
/************************** CorrectMag **************************************/
case SEN_MAG:
SEN_STATE = SEN_NUMQ;
break;
/************************** Quaternion **************************************/
case SEN_NUMQ:
switch((u16)(Correction_Time/SampleRateFreg)) {
case 0: // 等待 FIFO 填滿靜態資料
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 16);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 16);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 16);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 16);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 16);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 16);
Correction_Time++;
break;
case 1:
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 16);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 16);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 16);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 16);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 16);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 16);
/* To Physical */
Acc.TrueX = Acc.X*MPU9250A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9250A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9250A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9250G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9250G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9250G_2000dps; // dps/LSB
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
// AngE.Yaw = toDeg(atan2f(Ellipse[3], Ellipse[4]))+180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
Correction_Time = 0;
SEN_STATE = SEN_ALG;
break;
}
break;
/************************** Algorithm ***************************************/
case SEN_ALG:
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 8);
/* To Physical */
Acc.TrueX = Acc.X*MPU9250A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9250A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9250A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9250G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9250G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9250G_2000dps; // dps/LSB
/* Get Attitude */
AHRS_Update();
if((KEY_LL == 1) && (KEY_RR == 1)) { PID_Roll.Kp += 0.001f; PID_Pitch.Kp += 0.001f; }
if((KEY_LL == 1) && (KEY_RP == 1)) { PID_Roll.Kp -= 0.001f; PID_Pitch.Kp -= 0.001f; }
// if((KEY_LL == 1) && (KEY_RR == 1)) { PID_Roll.Ki += 0.0001f; PID_Pitch.Ki += 0.0001f; }
// if((KEY_LL == 1) && (KEY_RR == 1)) { PID_Roll.Ki -= 0.0001f; PID_Pitch.Ki -= 0.0001f; }
if((KEY_LR == 1) && (KEY_RR == 1)) { PID_Roll.Kd += 0.0001f; PID_Pitch.Kd += 0.0001f; }
if((KEY_LR == 1) && (KEY_RP == 1)) { PID_Roll.Kd -= 0.0001f; PID_Pitch.Kd -= 0.0001f; }
// if((KEY_RR == 1) && (KEY_RR == 1)) { PID_Yaw.Kd += 0.0001f; }
// if((KEY_RR == 1) && (KEY_RR == 1)) { PID_Yaw.Kd -= 0.0001f; }
/* Get ZeroErr */
PID_Pitch.ZeroErr = (fp32)((s16)EXP_PITCH/4.5f);
PID_Roll.ZeroErr = (fp32)((s16)EXP_ROLL/4.5f);
PID_Yaw.ZeroErr = (fp32)((s16)EXP_YAW)+180.0f;
/* PID */
Roll = (s16)PID_AHRS_Cal(&PID_Roll, AngE.Roll, Gyr.TrueX);
Pitch = (s16)PID_AHRS_Cal(&PID_Pitch, AngE.Pitch, Gyr.TrueY);
Yaw = (s16)(PID_Yaw.Kd*Gyr.TrueZ);
// Roll = 0;
// Pitch = 0;
Yaw = 0;
/* Motor Ctrl */
MOTOR[0] = BasicThr + Pitch + Roll + Yaw;
MOTOR[1] = BasicThr - Pitch + Roll - Yaw;
MOTOR[2] = BasicThr - Pitch - Roll + Yaw;
MOTOR[3] = BasicThr + Pitch - Roll - Yaw;
/* Check Connection */
#define NoSignal 1 // 1 sec
if(KEY_RL == 1) {
// Close Thr
PID_Pitch.SumErr = 0.0f;
PID_Roll.SumErr = 0.0f;
PID_Yaw.SumErr = 0.0f;
Ctrl_MotorTHR(MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN);
}
else if(((Time_Sec-RF_RecvData.Time.Sec)>NoSignal) || ((Time_Sec-RF_RecvData.Time.Sec)<-NoSignal))
Ctrl_MotorTHR(MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN, MOTOR_THR_MIN);
else {
// Thr Ctrl
Ctrl_MotorTHR(MOTOR[0], MOTOR[1], MOTOR[2], MOTOR[3]);
}
break;
/************************** Error *******************************************/
default:
while(1);
}
}
void correct_sensor()
{ /* IMU calibration */
#define MovegAveFIFO_Size 250
switch (SensorMode) {
/************************** Mode_CorrectGyr **************************************/
case Mode_GyrCorrect:
/* Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, GYR_FIFO[0], MovegAveFIFO_Size);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, GYR_FIFO[1], MovegAveFIFO_Size);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, GYR_FIFO[2], MovegAveFIFO_Size);
//將上述讀取出來的陀螺儀數值作平均
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if (Correction_Time == SampleRateFreg) {
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SensorMode = Mode_AccCorrect;
}
break;
/************************** Mode_CorrectAcc **************************************/
case Mode_AccCorrect:
/* Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, ACC_FIFO[0], MovegAveFIFO_Size);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, ACC_FIFO[1], MovegAveFIFO_Size);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, ACC_FIFO[2], MovegAveFIFO_Size);
//將上述讀取出來的加速規數值作平均
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if (Correction_Time == SampleRateFreg) {
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
SensorMode = Mode_Quaternion; // Mode_MagCorrect;
}
break;
/************************** Algorithm Mode **************************************/
case Mode_Quaternion:
/* To Physical */
/* 將數值轉換為現實世界中單位 */
Acc.TrueX = Acc.X * MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y * MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z * MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X * MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y * MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z * MPU9150G_2000dps; // dps/LSB
#if USE_MPU9150_and_MS5611
Mag.TrueX = Mag.X * MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y * MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z * MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T * MPU9150T_85degC; // degC/LSB
#endif
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
AngE.Yaw = toDeg(atan2f(Mag.TrueX, Mag.TrueY)) + 180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
SensorMode = Mode_Algorithm;
break;
default:
serial.printf("correct_sensor() [WARNING] unhandled case %d", SensorMode);
break;
}
}
/// @brief Calculates radians needed to turn towards a point.
/// @param mX1 First point X.
/// @param mY1 First point Y.
/// @param mX2 Second point X.
/// @param mY2 Second point Y.
/// @return Returns the needed radians.
template<typename T1, typename T2, typename T3, typename T4> inline constexpr Common<T1, T2, T3, T4> getDegTowards(const T1& mX1, const T2& mY1, const T3& mX2, const T4& mY2) noexcept
{
return toDeg(getRadTowards(mX1, mY1, mX2, mY2));
}
/// @brief Gets a rotated angle in radians.
/// @details Example use: a character slowly aiming towards the mouse position.
/// @param mStart Angle to start from.
/// @param mEnd Target angle.
/// @param mSpeed Rotation speed.
/// @return Returns the rotated angle in radians.
template<typename T1, typename T2, typename T3> inline constexpr auto getRotatedRad(const T1& mStart, const T2& mEnd, const T3& mSpeed) noexcept
{
return getRotatedDeg(toDeg(mStart), toDeg(mEnd), mSpeed);
}
void correct_sensor()
{
float Ellipse[5] = {0};
#define MovegAveFIFO_Size 250
switch (SensorMode) {
/************************** Mode_CorrectGyr **************************************/
case Mode_GyrCorrect:
/* Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, GYR_FIFO[0], MovegAveFIFO_Size);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, GYR_FIFO[1], MovegAveFIFO_Size);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, GYR_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if (Correction_Time == SampleRateFreg) {
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SensorMode = Mode_AccCorrect;
}
break;
/************************** Mode_CorrectAcc **************************************/
case Mode_AccCorrect:
/* Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, ACC_FIFO[0], MovegAveFIFO_Size);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, ACC_FIFO[1], MovegAveFIFO_Size);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, ACC_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if (Correction_Time == SampleRateFreg) {
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
SensorMode = Mode_Quaternion; // Mode_MagCorrect;
}
break;
/************************** Mode_CorrectMag **************************************/
#define MagCorrect_Ave 100
#define MagCorrect_Delay 600 // DelayTime : SampleRate * 600
case Mode_MagCorrect:
Correction_Time++;
switch ((u16)(Correction_Time / MagCorrect_Delay)) {
case 0:
MagDataX[0] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[0] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 1:
MagDataX[1] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[1] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 2:
MagDataX[2] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[2] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 3:
MagDataX[3] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[3] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 4:
MagDataX[4] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[4] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 5:
MagDataX[5] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[5] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 6:
MagDataX[6] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[6] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 7:
MagDataX[7] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[7] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
default:
EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
Mag.EllipseSita = Ellipse[0];
Mag.EllipseX0 = Ellipse[1];
Mag.EllipseY0 = Ellipse[2];
Mag.EllipseA = Ellipse[3];
Mag.EllipseB = Ellipse[4];
Correction_Time = 0;
SensorMode = Mode_Quaternion;
break;
}
break;
/************************** Algorithm Mode **************************************/
case Mode_Quaternion:
/* To Physical */
Acc.TrueX = Acc.X * MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y * MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z * MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X * MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y * MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z * MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X * MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y * MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z * MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T * MPU9150T_85degC; // degC/LSB
Ellipse[3] = (Mag.X * arm_cos_f32(Mag.EllipseSita) + Mag.Y * arm_sin_f32(Mag.EllipseSita)) / Mag.EllipseB;
Ellipse[4] = (-Mag.X * arm_sin_f32(Mag.EllipseSita) + Mag.Y * arm_cos_f32(Mag.EllipseSita)) / Mag.EllipseA;
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
AngE.Yaw = toDeg(atan2f(Ellipse[3], Ellipse[4])) + 180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
SensorMode = Mode_Algorithm;
break;
}
}
/*=====================================================================================================*/
void SysTick_Handler( void )
{
s16 IMU_Buf[10] = {0};
static s16 *FIFO_X, *FIFO_Y, *FIFO_Z;
static s16 FIFO_ACC[3][16] = {0}, FIFO_GYR[3][16] = {0}, FIFO_MAG[3][16] = {0};
static u32 Correction_Time = 0;
/* 500Hz, Read Accelerometer, Gyroscope, Magnetometer */
MPU9150_Read(IMU_Buf);
/* 100Hz, Read Barometer */
if((SysTick_Cnt%(SampleRateFreg/100)) == 0)
MS5611_Read(&Baro, MS5611_D1_OSR_4096);
/* Offset */
Acc.X = IMU_Buf[0] - Acc.OffsetX;
Acc.Y = IMU_Buf[1] - Acc.OffsetY;
Acc.Z = IMU_Buf[2] - Acc.OffsetZ;
Gyr.X = IMU_Buf[3] - Gyr.OffsetX;
Gyr.Y = IMU_Buf[4] - Gyr.OffsetY;
Gyr.Z = IMU_Buf[5] - Gyr.OffsetZ;
Mag.X = IMU_Buf[6] * Mag.AdjustX;
Mag.Y = IMU_Buf[7] * Mag.AdjustY;
Mag.Z = IMU_Buf[8] * Mag.AdjustZ;
Temp.T = IMU_Buf[9];
#define MAFIFO_SIZE 250
switch(SEN_STATE) {
/************************** CorrectSelect ***********************************/
case SEN_CORR:
SEN_STATE = (KEY == KEY_ON) ? SEN_GYR : SEN_NUMQ;
break;
/************************** CorrectGyr **************************************/
case SEN_GYR:
LED_R = !LED_R;
switch((u16)(Correction_Time/SampleRateFreg)) {
case 0: // 分配記憶體給 MaveAve 使用
FIFO_X = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Y = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Z = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
memset(FIFO_X, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Y, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Z, 0, MAFIFO_SIZE*sizeof(s16));
Correction_Time = SampleRateFreg;
break;
case 1: // 等待 FIFO 填滿靜態資料
/* 移動平均 Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, FIFO_X, MAFIFO_SIZE);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, FIFO_Y, MAFIFO_SIZE);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, FIFO_Z, MAFIFO_SIZE);
Correction_Time++;
break;
case 2: // 釋放記憶體 & 計算陀螺儀偏移量
free(FIFO_X);
free(FIFO_Y);
free(FIFO_Z);
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SEN_STATE = SEN_ACC;
break;
}
break;
/************************** CorrectAcc **************************************/
case SEN_ACC:
LED_R = !LED_R;
switch((u16)(Correction_Time/SampleRateFreg)) {
case 0: // 分配記憶體給 MaveAve 使用
FIFO_X = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Y = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
FIFO_Z = (s16*)malloc(MAFIFO_SIZE*sizeof(s16));
memset(FIFO_X, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Y, 0, MAFIFO_SIZE*sizeof(s16));
memset(FIFO_Z, 0, MAFIFO_SIZE*sizeof(s16));
Correction_Time = SampleRateFreg;
break;
case 1: // 等待 FIFO 填滿靜態資料
/* 移動平均 Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, FIFO_X, MAFIFO_SIZE);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, FIFO_Y, MAFIFO_SIZE);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, FIFO_Z, MAFIFO_SIZE);
Correction_Time++;
break;
case 2: // 釋放記憶體 & 計算加速度計偏移量
free(FIFO_X);
free(FIFO_Y);
free(FIFO_Z);
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
SEN_STATE = SEN_MAG;
break;
}
break;
/************************** CorrectMag **************************************/
case SEN_MAG:
LED_R = !LED_R;
SEN_STATE = SEN_NUMQ;
break;
/************************** Quaternion **************************************/
case SEN_NUMQ:
switch((u16)(Correction_Time/SampleRateFreg)) {
case 0: // 等待 FIFO 填滿靜態資料
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 8);
Mag.X = (s16)MoveAve_WMA(Mag.X, FIFO_MAG[0], 16);
Mag.Y = (s16)MoveAve_WMA(Mag.Y, FIFO_MAG[1], 16);
Mag.Z = (s16)MoveAve_WMA(Mag.Z, FIFO_MAG[2], 16);
Correction_Time++;
break;
case 1:
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 8);
Mag.X = (s16)MoveAve_WMA(Mag.X, FIFO_MAG[0], 16);
Mag.Y = (s16)MoveAve_WMA(Mag.Y, FIFO_MAG[1], 16);
Mag.Z = (s16)MoveAve_WMA(Mag.Z, FIFO_MAG[2], 16);
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
// Ellipse[3] = ( Mag.X*arm_cos_f32(Mag.EllipseSita)+Mag.Y*arm_sin_f32(Mag.EllipseSita))/Mag.EllipseB;
// Ellipse[4] = (-Mag.X*arm_sin_f32(Mag.EllipseSita)+Mag.Y*arm_cos_f32(Mag.EllipseSita))/Mag.EllipseA;
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
// AngE.Yaw = toDeg(atan2f(Ellipse[3], Ellipse[4]))+180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
Correction_Time = 0;
SEN_STATE = SEN_ALG;
break;
}
break;
/************************** Algorithm ***************************************/
case SEN_ALG:
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, FIFO_ACC[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, FIFO_ACC[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, FIFO_ACC[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, FIFO_GYR[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, FIFO_GYR[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, FIFO_GYR[2], 8);
Mag.X = (s16)MoveAve_WMA(Mag.X, FIFO_MAG[0], 16);
Mag.Y = (s16)MoveAve_WMA(Mag.Y, FIFO_MAG[1], 16);
Mag.Z = (s16)MoveAve_WMA(Mag.Z, FIFO_MAG[2], 16);
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T*MPU9150T_85degC; // degC/LSB
/* Get Attitude Angle */
AHRS_Update();
break;
/************************** Error *******************************************/
default:
LED_R = 1;
LED_G = 1;
LED_B = 1;
while(1) {
LED_R = !LED_R;
Delay_100ms(10);
}
}
}
Vector<dim> radToDeg(Vector<dim> rad) {
for (unsigned int i = 0; i < dim; ++i)
rad[i] *= toDeg();
return rad;
}
/*=====================================================================================================*/
void AHRS_Update(void)
{
float tempX = 0, tempY = 0;
float Normalize;
float gx, gy, gz;
// float hx, hy, hz;
// float wx, wy, wz;
// float bx, bz;
float ErrX, ErrY, ErrZ;
float AccX, AccY, AccZ;
float GyrX, GyrY, GyrZ;
// float MegX, MegY, MegZ;
float /*Mq11, Mq12, */Mq13,/* Mq21, Mq22, */Mq23,/* Mq31, Mq32, */Mq33;
static float AngZ_Temp = 0.0f;
static float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;
// Mq11 = NumQ.q0*NumQ.q0 + NumQ.q1*NumQ.q1 - NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
// Mq12 = 2.0f*(NumQ.q1*NumQ.q2 + NumQ.q0*NumQ.q3);
Mq13 = 2.0f * (NumQ.q1 * NumQ.q3 - NumQ.q0 * NumQ.q2);
// Mq21 = 2.0f*(NumQ.q1*NumQ.q2 - NumQ.q0*NumQ.q3);
// Mq22 = NumQ.q0*NumQ.q0 - NumQ.q1*NumQ.q1 + NumQ.q2*NumQ.q2 - NumQ.q3*NumQ.q3;
Mq23 = 2.0f * (NumQ.q0 * NumQ.q1 + NumQ.q2 * NumQ.q3);
// Mq31 = 2.0f*(NumQ.q0*NumQ.q2 + NumQ.q1*NumQ.q3);
// Mq32 = 2.0f*(NumQ.q2*NumQ.q3 - NumQ.q0*NumQ.q1);
Mq33 = NumQ.q0 * NumQ.q0 - NumQ.q1 * NumQ.q1 - NumQ.q2 * NumQ.q2 + NumQ.q3 * NumQ.q3;
Normalize = invSqrtf(squa(Acc.TrueX) + squa(Acc.TrueY) + squa(Acc.TrueZ));
AccX = Acc.TrueX * Normalize;
AccY = Acc.TrueY * Normalize;
AccZ = Acc.TrueZ * Normalize;
// Normalize = invSqrtf(squa(Meg.TrueX) + squa(Meg.TrueY) + squa(Meg.TrueZ));
// MegX = Meg.TrueX*Normalize;
// MegY = Meg.TrueY*Normalize;
// MegZ = Meg.TrueZ*Normalize;
gx = Mq13;
gy = Mq23;
gz = Mq33;
// hx = MegX*Mq11 + MegY*Mq21 + MegZ*Mq31;
// hy = MegX*Mq12 + MegY*Mq22 + MegZ*Mq32;
// hz = MegX*Mq13 + MegY*Mq23 + MegZ*Mq33;
// bx = sqrtf(squa(hx) + squa(hy));
// bz = hz;
// wx = bx*Mq11 + bz*Mq13;
// wy = bx*Mq21 + bz*Mq23;
// wz = bx*Mq31 + bz*Mq33;
ErrX = (AccY * gz - AccZ * gy)/* + (MegY*wz - MegZ*wy)*/;
ErrY = (AccZ * gx - AccX * gz)/* + (MegZ*wx - MegX*wz)*/;
ErrZ = (AccX * gy - AccY * gx)/* + (MegX*wy - MegY*wx)*/;
exInt = exInt + ErrX * Ki;
eyInt = eyInt + ErrY * Ki;
ezInt = ezInt + ErrZ * Ki;
GyrX = toRad(Gyr.TrueX);
GyrY = toRad(Gyr.TrueY);
GyrZ = toRad(Gyr.TrueZ);
GyrX = GyrX + Kp * ErrX + exInt;
GyrY = GyrY + Kp * ErrY + eyInt;
GyrZ = GyrZ + Kp * ErrZ + ezInt;
Quaternion_RungeKutta(&NumQ, GyrX, GyrY, GyrZ, SampleRateHelf);
Quaternion_Normalize(&NumQ);
Quaternion_ToAngE(&NumQ, &AngE);
tempX = (Mag.X * arm_cos_f32(Mag.EllipseSita) + Mag.Y * arm_sin_f32(Mag.EllipseSita)) / Mag.EllipseB;
tempY = (-Mag.X * arm_sin_f32(Mag.EllipseSita) + Mag.Y * arm_cos_f32(Mag.EllipseSita)) / Mag.EllipseA;
AngE.Yaw = atan2f(tempX, tempY);
AngE.Pitch = toDeg(AngE.Pitch);
AngE.Roll = toDeg(AngE.Roll);
AngE.Yaw = toDeg(AngE.Yaw) + 180.0f;
/* 互補濾波 Complementary Filter */
#define CF_A 0.9f
#define CF_B 0.1f
AngZ_Temp = AngZ_Temp + GyrZ * SampleRate;
AngZ_Temp = CF_A * AngZ_Temp + CF_B * AngE.Yaw;
if (AngZ_Temp > 360.0f)
AngE.Yaw = AngZ_Temp - 360.0f;
else if (AngZ_Temp < 0.0f)
AngE.Yaw = AngZ_Temp + 360.0f;
else
AngE.Yaw = AngZ_Temp;
}
/*=====================================================================================================*/
void SysTick_Handler( void )
{
u8 IMU_Buf[20] = {0};
u16 Final_M1 = 0;
u16 Final_M2 = 0;
u16 Final_M3 = 0;
u16 Final_M4 = 0;
s16 Thr = 0, Pitch = 0, Roll = 0, Yaw = 0;
float Ellipse[5] = {0};
static u8 BaroCnt = 0;
static s16 ACC_FIFO[3][256] = {0};
static s16 GYR_FIFO[3][256] = {0};
static s16 MAG_FIFO[3][256] = {0};
static s16 MagDataX[8] = {0};
static s16 MagDataY[8] = {0};
static u16 Correction_Time = 0;
/* Time Count */
SysTick_Cnt++;
if(SysTick_Cnt == SampleRateFreg) {
SysTick_Cnt = 0;
Time_Sec++;
if(Time_Sec == 60) { // 0~59
Time_Sec = 0;
Time_Min++;
if(Time_Sec == 60)
Time_Min = 0;
}
}
/* 400Hz, Read Sensor ( Accelerometer, Gyroscope, Magnetometer ) */
MPU9150_Read(IMU_Buf);
/* 100Hz, Read Barometer */
BaroCnt++;
if(BaroCnt == 4) {
MS5611_Read(&Baro, MS5611_D1_OSR_4096);
BaroCnt = 0;
}
Acc.X = (s16)((IMU_Buf[0] << 8) | IMU_Buf[1]);
Acc.Y = (s16)((IMU_Buf[2] << 8) | IMU_Buf[3]);
Acc.Z = (s16)((IMU_Buf[4] << 8) | IMU_Buf[5]);
Temp.T = (s16)((IMU_Buf[6] << 8) | IMU_Buf[7]);
Gyr.X = (s16)((IMU_Buf[8] << 8) | IMU_Buf[9]);
Gyr.Y = (s16)((IMU_Buf[10] << 8) | IMU_Buf[11]);
Gyr.Z = (s16)((IMU_Buf[12] << 8) | IMU_Buf[13]);
Mag.X = (s16)((IMU_Buf[15] << 8) | IMU_Buf[14]);
Mag.Y = (s16)((IMU_Buf[17] << 8) | IMU_Buf[16]);
Mag.Z = (s16)((IMU_Buf[19] << 8) | IMU_Buf[18]);
/* Offset */
Acc.X -= Acc.OffsetX;
Acc.Y -= Acc.OffsetY;
Acc.Z -= Acc.OffsetZ;
Gyr.X -= Gyr.OffsetX;
Gyr.Y -= Gyr.OffsetY;
Gyr.Z -= Gyr.OffsetZ;
Mag.X *= Mag.AdjustX;
Mag.Y *= Mag.AdjustY;
Mag.Z *= Mag.AdjustZ;
#define MovegAveFIFO_Size 250
switch(SensorMode) {
/************************** Mode_CorrectGyr **************************************/
case Mode_GyrCorrect:
LED_R = !LED_R;
/* Simple Moving Average */
Gyr.X = (s16)MoveAve_SMA(Gyr.X, GYR_FIFO[0], MovegAveFIFO_Size);
Gyr.Y = (s16)MoveAve_SMA(Gyr.Y, GYR_FIFO[1], MovegAveFIFO_Size);
Gyr.Z = (s16)MoveAve_SMA(Gyr.Z, GYR_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if(Correction_Time == 400) {
Gyr.OffsetX += (Gyr.X - GYR_X_OFFSET); // 角速度為 0dps
Gyr.OffsetY += (Gyr.Y - GYR_Y_OFFSET); // 角速度為 0dps
Gyr.OffsetZ += (Gyr.Z - GYR_Z_OFFSET); // 角速度為 0dps
Correction_Time = 0;
SensorMode = Mode_MagCorrect; // Mode_AccCorrect;
}
break;
/************************** Mode_CorrectAcc **************************************/
case Mode_AccCorrect:
LED_R = ~LED_R;
/* Simple Moving Average */
Acc.X = (s16)MoveAve_SMA(Acc.X, ACC_FIFO[0], MovegAveFIFO_Size);
Acc.Y = (s16)MoveAve_SMA(Acc.Y, ACC_FIFO[1], MovegAveFIFO_Size);
Acc.Z = (s16)MoveAve_SMA(Acc.Z, ACC_FIFO[2], MovegAveFIFO_Size);
Correction_Time++; // 等待 FIFO 填滿空值 or 填滿靜態資料
if(Correction_Time == 400) {
Acc.OffsetX += (Acc.X - ACC_X_OFFSET); // 重力加速度為 0g
Acc.OffsetY += (Acc.Y - ACC_Y_OFFSET); // 重力加速度為 0g
Acc.OffsetZ += (Acc.Z - ACC_Z_OFFSET); // 重力加速度為 1g
Correction_Time = 0;
SensorMode = Mode_Quaternion; // Mode_MagCorrect;
}
break;
/************************** Mode_CorrectMag **************************************/
#define MagCorrect_Ave 100
#define MagCorrect_Delay 600 // 2.5ms * 600 = 1.5s
case Mode_MagCorrect:
LED_R = !LED_R;
Correction_Time++;
switch((u16)(Correction_Time/MagCorrect_Delay)) {
case 0:
LED_B = 0;
MagDataX[0] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[0] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 1:
LED_B = 1;
MagDataX[1] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[1] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 2:
LED_B = 0;
MagDataX[2] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[2] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 3:
LED_B = 1;
MagDataX[3] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[3] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 4:
LED_B = 0;
MagDataX[4] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[4] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 5:
LED_B = 1;
MagDataX[5] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[5] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 6:
LED_B = 0;
MagDataX[6] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[6] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
case 7:
LED_B = 1;
MagDataX[7] = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], MagCorrect_Ave);
MagDataY[7] = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], MagCorrect_Ave);
break;
default:
LED_B = 1;
EllipseFitting(Ellipse, MagDataX, MagDataY, 8);
Mag.EllipseSita = Ellipse[0];
Mag.EllipseX0 = Ellipse[1];
Mag.EllipseY0 = Ellipse[2];
Mag.EllipseA = Ellipse[3];
Mag.EllipseB = Ellipse[4];
Correction_Time = 0;
SensorMode = Mode_Quaternion;
break;
}
break;
/************************** Algorithm Mode **************************************/
case Mode_Quaternion:
LED_R = !LED_R;
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T*MPU9150T_85degC; // degC/LSB
Ellipse[3] = ( Mag.X*arm_cos_f32(Mag.EllipseSita)+Mag.Y*arm_sin_f32(Mag.EllipseSita))/Mag.EllipseB;
Ellipse[4] = (-Mag.X*arm_sin_f32(Mag.EllipseSita)+Mag.Y*arm_cos_f32(Mag.EllipseSita))/Mag.EllipseA;
AngE.Pitch = toDeg(atan2f(Acc.TrueY, Acc.TrueZ));
AngE.Roll = toDeg(-asinf(Acc.TrueX));
AngE.Yaw = toDeg(atan2f(Ellipse[3], Ellipse[4]))+180.0f;
Quaternion_ToNumQ(&NumQ, &AngE);
SensorMode = Mode_Algorithm;
break;
/************************** Algorithm Mode ****************************************/
case Mode_Algorithm:
/* 加權移動平均法 Weighted Moving Average */
Acc.X = (s16)MoveAve_WMA(Acc.X, ACC_FIFO[0], 8);
Acc.Y = (s16)MoveAve_WMA(Acc.Y, ACC_FIFO[1], 8);
Acc.Z = (s16)MoveAve_WMA(Acc.Z, ACC_FIFO[2], 8);
Gyr.X = (s16)MoveAve_WMA(Gyr.X, GYR_FIFO[0], 8);
Gyr.Y = (s16)MoveAve_WMA(Gyr.Y, GYR_FIFO[1], 8);
Gyr.Z = (s16)MoveAve_WMA(Gyr.Z, GYR_FIFO[2], 8);
Mag.X = (s16)MoveAve_WMA(Mag.X, MAG_FIFO[0], 64);
Mag.Y = (s16)MoveAve_WMA(Mag.Y, MAG_FIFO[1], 64);
Mag.Z = (s16)MoveAve_WMA(Mag.Z, MAG_FIFO[2], 64);
/* To Physical */
Acc.TrueX = Acc.X*MPU9150A_4g; // g/LSB
Acc.TrueY = Acc.Y*MPU9150A_4g; // g/LSB
Acc.TrueZ = Acc.Z*MPU9150A_4g; // g/LSB
Gyr.TrueX = Gyr.X*MPU9150G_2000dps; // dps/LSB
Gyr.TrueY = Gyr.Y*MPU9150G_2000dps; // dps/LSB
Gyr.TrueZ = Gyr.Z*MPU9150G_2000dps; // dps/LSB
Mag.TrueX = Mag.X*MPU9150M_1200uT; // uT/LSB
Mag.TrueY = Mag.Y*MPU9150M_1200uT; // uT/LSB
Mag.TrueZ = Mag.Z*MPU9150M_1200uT; // uT/LSB
Temp.TrueT = Temp.T*MPU9150T_85degC; // degC/LSB
/* Get Attitude Angle */
AHRS_Update();
// if(KEYL_U == 0) { PID_Roll.Kp += 0.001f; PID_Pitch.Kp += 0.001f; }
// if(KEYL_L == 0) { PID_Roll.Kp -= 0.001f; PID_Pitch.Kp -= 0.001f; }
// if(KEYL_R == 0) { PID_Roll.Ki += 0.0001f; PID_Pitch.Ki += 0.0001f; }
// if(KEYL_D == 0) { PID_Roll.Ki -= 0.0001f; PID_Pitch.Ki -= 0.0001f; }
// if(KEYR_R == 0) { PID_Roll.Kd += 0.0001f; PID_Pitch.Kd += 0.0001f; }
// if(KEYR_D == 0) { PID_Roll.Kd -= 0.0001f; PID_Pitch.Kd -= 0.0001f; }
// if(KEYR_L == 0) { PID_Roll.SumErr = 0.0f; PID_Pitch.SumErr = 0.0f; }
// if(KEYL_U == 0) { PID_Yaw.Kp += 0.001f; }
// if(KEYL_L == 0) { PID_Yaw.Kp -= 0.001f; }
// if(KEYL_R == 0) { PID_Yaw.Ki += 0.001f; }
// if(KEYL_D == 0) { PID_Yaw.Ki -= 0.001f; }
// if(KEYR_R == 0) { PID_Yaw.Kd += 0.0001f; }
// if(KEYR_D == 0) { PID_Yaw.Kd -= 0.0001f; }
// if(KEYR_L == 0) { PID_Roll.SumErr = 0.0f; }
/* Get ZeroErr */
PID_Pitch.ZeroErr = (float)((s16)Exp_Pitch/4.5f);
PID_Roll.ZeroErr = (float)((s16)Exp_Roll/4.5f);
PID_Yaw.ZeroErr = (float)((s16)Exp_Yaw)+180.0f;
/* PID */
Roll = (s16)PID_AHRS_Cal(&PID_Roll, AngE.Roll, Gyr.TrueX);
Pitch = (s16)PID_AHRS_Cal(&PID_Pitch, AngE.Pitch, Gyr.TrueY);
// Yaw = (s16)PID_AHRS_CalYaw(&PID_Yaw, AngE.Yaw, Gyr.TrueZ);
Yaw = (s16)(PID_Yaw.Kd*Gyr.TrueZ);
Thr = (s16)Exp_Thr;
/* Motor Ctrl */
Final_M1 = PWM_M1 + Thr + Pitch + Roll + Yaw;
Final_M2 = PWM_M2 + Thr - Pitch + Roll - Yaw;
Final_M3 = PWM_M3 + Thr - Pitch - Roll + Yaw;
Final_M4 = PWM_M4 + Thr + Pitch - Roll - Yaw;
/* Check Connection */
#define NoSignal 1 // 1 sec
if(KEYR_L == 0)
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
else if(((Time_Sec-RecvTime_Sec)>NoSignal) || ((Time_Sec-RecvTime_Sec)<-NoSignal))
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
else
Motor_Control(Final_M1, Final_M2, Final_M3, Final_M4);
/* DeBug */
Tmp_PID_KP = PID_Yaw.Kp*1000;
Tmp_PID_KI = PID_Yaw.Ki*1000;
Tmp_PID_KD = PID_Yaw.Kd*1000;
Tmp_PID_Pitch = Yaw;
break;
}
}
Arc::Direction Arc::angleRadToDirEight( float angle )
{
return angleDegToDirEight(toDeg(angle));
}
Arc::Direction Arc::angleRadToDirFour( float angle )
{
return angleDegToDirFour(toDeg(angle));
}
void display(void)
{
// Buffer clearen
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// View Matrix erstellen
glLoadIdentity();
float x = distance * sin(theta) * cos(phi);
float y = distance * cos(theta);
float z = distance * sin(theta) * sin(phi);
gluLookAt(x, y, z, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
// Teekanne rendern.
glutSolidTeapot(1);
// Den Matrix-Stack sichern.
glPushMatrix();
// Zeichnen der Kugelkreisbahn.
drawCircle(10.0f, 50);
// Zeichnen der Kugel.
// Wenden Sie eine Translation und eine Rotation an, bevor sie die Kugel zeichnen. Sie können die Variable 'angle' für die Rotation verwenden.
// Bedenken Sie dabei die richtige Reihenfolge der beiden Transformationen.
glRotated(angle, 0, 1.0f, 0);
glTranslatef(10.0f, 0, 0);
glutSolidSphere(1.0f, 32, 32);
// Zeichnen der Würfelkreisbahn.
// Hinweis: Der Ursprung des Koordinatensystems befindet sich nun im Zentrum des Würfels.
// Drehen Sie das Koordinatensystem um 90° entlang der Achse, die für die Verschiebung des Würfels genutzt wurde.
// Danach steht die Würfelkreisbahn senkrecht zur Tangentialrichtung der Kugelkreisbahn.
glRotated(90.0f, 1.0f, 0, 0);
drawCircle(5.0f, 50);
// Zeichnen des Würfels.
// Wenden Sie die entsprechende Translation und Rotation an, bevor sie den Würfel zeichnen.
glRotated(angle, 0, 1.0f, 0);
glTranslatef(5.0f, 0, 0);
glutSolidCube(1.0f);
// Zeichnen einer Linie von Würfel zu Kegel.
glDisable(GL_LIGHTING);
glTranslatef(3.0f, 0, 0);
glBegin(GL_LINE_STRIP);
glVertex3f(0, 0, 0);
glVertex3f(-3.0f, 0, 0);
glEnd();
glEnable(GL_LIGHTING);
// Drehung anwenden, sodass Koordinatensystem in Richtung Ursprung orientiert ist. (Hinweis: Implementieren Sie dies zuletzt.)
GLfloat height = 8*cos(toRad(angle-90));
GLfloat d = 8*sin(toRad(angle-90));
GLfloat e = 10 - d;
GLfloat l = pow(pow(height,2) + pow(e,2), 0.5f);
GLfloat alpha = toDeg(acos(e/l)) - 90;
if(static_cast<int>(angle)%360 > 180) alpha = -alpha;
glRotated(alpha, 0, 1.0f, 0);
if(static_cast<int>(angle)%360 > 180)
{
glRotated(180 - angle, 0, 1.0f, 0);
std::cout << alpha + 180 - (int)angle%360 << std::endl;
}
else
{
glRotated(-angle, 0, 1.0f, 0);
std::cout << alpha - (int)angle%360 << std::endl;
}
// Zeichnen der Linie von Kegel zu Urpsrung.
glDisable(GL_LIGHTING);
glBegin(GL_LINE_STRIP);
glVertex3f(0, 0, 0);
glVertex3f(0, 0, l);
glEnd();
glEnable(GL_LIGHTING);
// Zeichnen des Kegels.
glutSolidCone(0.5f, 1.0f, 32, 4);
// Den Matrix-Stack wiederherstellen.
glPopMatrix();
glutSwapBuffers();
angle += 5.0f / 60.0f;
}
/*
* RMC Recommended Minimum Navigation Information
*
* 1 2 3 4 5 6 7 8 9 10 11
* | | | | | | | | | | |
* hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a
*
* 1) Time (UTC)
* 2) Status, V = Navigation receiver warning
* 3) Latitude
* 4) N or S
* 5) Longitude
* 6) E or W
* 7) Speed over ground, knots
* 8) Track made good, degrees true
* 9) Date, ddmmyy
* 10) Magnetic Variation, degrees
* 11) E or W
*/
static void parseRMC(un8 index, char *value)
{
if (*value == 0)
return;
switch (index)
{
case GPS_RMC_STATUS:
/* A: fix; V: no fix */
veVariantUn8(&local.fix, value[0] == 'A');
return;
case GPS_RMC_LATITUDE:
errno = 0;
veVariantFloat(&local.latitude, toDeg((float)atof(value)));
if (errno)
veVariantInvalidate(&local.latitude);
return;
case GPS_RMC_LAT_NORTH_SOUTH:
/* North is positive, south is negative */
if (value[0] == 'S')
local.latitude.value.Float *= -1;
return;
case GPS_RMC_LONGITUDE:
errno = 0;
veVariantFloat(&local.longitude, toDeg((float)atof(value)));
if (errno)
veVariantInvalidate(&local.longitude);
return;
case GPS_RMC_LONG_EAST_WEST:
/* East is positive, west is negative */
if (value[0] == 'W')
local.longitude.value.Float *= -1;
return;
case GPS_RMC_VARIATION:
errno = 0;
veVariantFloat(&local.variation, toDeg((float)atof(value)));
if (errno)
veVariantInvalidate(&local.variation);
return;
case GPS_RMC_VAR_EAST_WEST:
/* East is positive, west is negative */
if (value[0] == 'W')
local.variation.value.Float *= -1;
return;
case GPS_RMC_UTC_TIME:
memset(&local.time, 0, sizeof(struct tm));
local.time.tm_hour = a2b(*value++) * 10;
local.time.tm_hour += a2b(*value++);
local.time.tm_min = a2b(*value++) * 10;
local.time.tm_min += a2b(*value++);
local.time.tm_sec = a2b(*value++ ) * 10;
local.time.tm_sec += a2b(*value++);
return;
case GPS_RMC_UTC_DATE:
local.time.tm_mday = a2b(*value++) * 10;
local.time.tm_mday += a2b(*value++);
local.time.tm_mon = a2b(*value++) * 10;
local.time.tm_mon += a2b(*value++) - 1; /* 0-11 */
local.time.tm_year = a2b(*value++) * 10;
local.time.tm_year += a2b(*value++) + 100; /* Since 1900 */
veVariantUn32(&local.timestamp, (un32)mktime(&local.time));
return;
case GPS_RMC_SPEED:
errno = 0;
veVariantFloat(&local.speed, (float)atof(value));
if (errno) {
veVariantInvalidate(&local.speed);
return;
}
/* Knots to m/s */
local.speed.value.Float *= (1852.0 / 3600.0);
return;
case GPS_RMC_TRUE_COURSE:
/* Prevent bouncing when not moving */
if (!veVariantIsValid(&local.speed) || local.speed.value.Float == 0) {
veVariantInvalidate(&local.course);
return;
}
errno = 0;
veVariantFloat(&local.course, (float)atof(value));
if (errno)
veVariantInvalidate(&local.course);
return;
}
}
Angle::Angle(const sf::Vector2f &vec) :
Angle(toDeg((float) atan2(vec.y, vec.x))) { }
void mavlink_handleMessage(mavlink_message_t* msg) {
mavlink_message_t msg2;
char sysmsg_str[1024];
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
droneType = packet.type;
autoPilot = packet.autopilot;
if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
ModelData.mode = MODEL_MODE_MANUAL;
} else if (packet.base_mode == 128 + 64 + 16) {
ModelData.mode = MODEL_MODE_RTL;
} else if (packet.base_mode == 128 + 16) {
ModelData.mode = MODEL_MODE_POSHOLD;
} else if (packet.base_mode == 128 + 4) {
ModelData.mode = MODEL_MODE_MISSION;
}
if (packet.system_status == MAV_STATE_ACTIVE) {
ModelData.armed = MODEL_ARMED;
} else {
ModelData.armed = MODEL_DISARMED;
}
// SDL_Log("Heartbeat: %i, %i, %i\n", ModelData.armed, ModelData.mode, ModelData.status);
ModelData.heartbeat = 100;
// sprintf(sysmsg_str, "Heartbeat: %i", (int)time(0));
if ((*msg).sysid != 0xff) {
ModelData.sysid = (*msg).sysid;
ModelData.compid = (*msg).compid;
if (mavlink_maxparam == 0) {
mavlink_start_feeds();
}
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
mavlink_rc_channels_scaled_t packet;
mavlink_msg_rc_channels_scaled_decode(msg, &packet);
// SDL_Log("Radio: %i,%i,%i\n", packet.chan1_scaled, packet.chan2_scaled, packet.chan3_scaled);
/* if ((int)packet.chan6_scaled > 1000) {
mode = MODE_MISSION;
} else if ((int)packet.chan6_scaled < -1000) {
mode = MODE_MANUEL;
} else {
mode = MODE_POSHOLD;
}
if ((int)packet.chan7_scaled > 1000) {
mode = MODE_RTL;
} else if ((int)packet.chan7_scaled < -1000) {
mode = MODE_SETHOME;
}
*/
ModelData.radio[0] = (int)packet.chan1_scaled / 100.0;
ModelData.radio[1] = (int)packet.chan2_scaled / 100.0;
ModelData.radio[2] = (int)packet.chan3_scaled / 100.0;
ModelData.radio[3] = (int)packet.chan4_scaled / 100.0;
ModelData.radio[4] = (int)packet.chan5_scaled / 100.0;
ModelData.radio[5] = (int)packet.chan6_scaled / 100.0;
ModelData.radio[6] = (int)packet.chan7_scaled / 100.0;
ModelData.radio[7] = (int)packet.chan8_scaled / 100.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
// SDL_Log("BAR;%i;%0.2f;%0.2f;%0.2f\n", time(0), packet.press_abs, packet.press_diff, packet.temperature / 100.0);
// redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
ModelData.roll = toDeg(packet.roll);
ModelData.pitch = toDeg(packet.pitch);
if (toDeg(packet.yaw) < 0.0) {
ModelData.yaw = 360.0 + toDeg(packet.yaw);
} else {
ModelData.yaw = toDeg(packet.yaw);
}
mavlink_update_yaw = 1;
// SDL_Log("ATT;%i;%0.2f;%0.2f;%0.2f\n", time(0), toDeg(packet.roll), toDeg(packet.pitch), toDeg(packet.yaw));
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_IMU: {
// SDL_Log("SCALED_IMU\n");
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(msg, &packet);
if (packet.lat != 0.0) {
GPS_found = 1;
ModelData.p_lat = (float)packet.lat / 10000000.0;
ModelData.p_long = (float)packet.lon / 10000000.0;
ModelData.p_alt = (float)packet.alt / 1000.0;
ModelData.speed = (float)packet.vel / 100.0;
ModelData.numSat = packet.satellites_visible;
ModelData.gpsfix = packet.fix_type;
redraw_flag = 1;
}
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
// SDL_Log("RC_CHANNELS_RAW\n");
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
// SDL_Log("SERVO_OUTPUT_RAW\n");
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t packet;
mavlink_msg_sys_status_decode(msg, &packet);
// SDL_Log("%0.1f %%, %0.3f V)\n", packet.load / 10.0, packet.voltage_battery / 1000.0);
ModelData.voltage = packet.voltage_battery / 1000.0;
ModelData.load = packet.load / 10.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t packet;
mavlink_msg_statustext_decode(msg, &packet);
SDL_Log("mavlink: ## %s ##\n", packet.text);
sys_message((char *)packet.text);
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
char var[101];
uint16_t n1 = 0;
uint16_t n2 = 0;
for (n1 = 0; n1 < strlen(packet.param_id); n1++) {
if (packet.param_id[n1] != 9 && packet.param_id[n1] != ' ' && packet.param_id[n1] != '\t') {
var[n2++] = packet.param_id[n1];
}
}
var[n2++] = 0;
// MAV_VAR_FLOAT=0, /* 32 bit float | */
// MAV_VAR_UINT8=1, /* 8 bit unsigned integer | */
// MAV_VAR_INT8=2, /* 8 bit signed integer | */
// MAV_VAR_UINT16=3, /* 16 bit unsigned integer | */
// MAV_VAR_INT16=4, /* 16 bit signed integer | */
// MAV_VAR_UINT32=5, /* 32 bit unsigned integer | */
// MAV_VAR_INT32=6, /* 32 bit signed integer | */
sprintf(sysmsg_str, "PARAM_VALUE (%i/%i): #%s# = %f (Type: %i)", packet.param_index + 1, packet.param_count, var, packet.param_value, packet.param_type);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
mavlink_maxparam = packet.param_count;
mavlink_timeout = 0;
mavlink_set_value(var, packet.param_value, packet.param_type, packet.param_index);
if (packet.param_index + 1 == packet.param_count || packet.param_index % 10 == 0) {
mavlink_param_xml_meta_load();
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t packet;
mavlink_msg_mission_count_decode(msg, &packet);
sprintf(sysmsg_str, "MISSION_COUNT: %i\n", packet.count);
sys_message(sysmsg_str);
mission_max = packet.count;
if (mission_max > 0) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 0);
mavlink_send_message(&msg2);
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ACK: {
SDL_Log("mavlink: Mission-Transfer ACK\n");
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
uint16_t id = packet.seq;
uint16_t id2 = packet.seq;
uint16_t type = 0;
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (id2 > 0) {
id2 = id2 - 1;
} else {
SDL_Log("mavlink: WORKAROUND: first WP == HOME ?\n");
}
}
sprintf(sysmsg_str, "sending Waypoint (%i): %s\n", id, WayPoints[1 + id2].name);
sys_message(sysmsg_str);
if (strcmp(WayPoints[1 + id2].command, "WAYPOINT") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_WAYPOINT\n");
type = MAV_CMD_NAV_WAYPOINT;
} else if (strcmp(WayPoints[1 + id2].command, "RTL") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
type = MAV_CMD_NAV_RETURN_TO_LAUNCH;
} else if (strcmp(WayPoints[1 + id2].command, "LAND") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_LAND\n");
type = MAV_CMD_NAV_LAND;
} else if (strcmp(WayPoints[1 + id2].command, "TAKEOFF") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_TAKEOFF\n");
type = MAV_CMD_NAV_TAKEOFF;
} else {
SDL_Log("mavlink: Type: UNKNOWN\n");
type = MAV_CMD_NAV_WAYPOINT;
}
sprintf(sysmsg_str, "SENDING MISSION_ITEM: %i: %f, %f, %f\n", id, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
SDL_Log("mavlink: %s\n", sysmsg_str);
mavlink_msg_mission_item_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, id, 0, type, 0.0, 0.0, WayPoints[1 + id2].radius, WayPoints[1 + id2].wait, WayPoints[1 + id2].orbit, WayPoints[1 + id2].yaw, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
mavlink_send_message(&msg2);
/*
mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t packet;
mavlink_msg_mission_item_decode(msg, &packet);
sprintf(sysmsg_str, "RECEIVED MISSION_ITEM: %i/%i: %f, %f, %f (%i)\n", packet.seq, mission_max, packet.x, packet.y, packet.z, packet.frame);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
if (packet.seq < mission_max - 1) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, packet.seq + 1);
mavlink_send_message(&msg2);
} else {
mavlink_msg_mission_ack_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 15);
mavlink_send_message(&msg2);
}
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (packet.seq > 0) {
packet.seq = packet.seq - 1;
} else {
SDL_Log("mavlink: WORKAROUND: ignore first WP\n");
break;
}
}
SDL_Log("mavlink: getting WP(%i): %f, %f\n", packet.seq, packet.x, packet.y);
switch (packet.command) {
case MAV_CMD_NAV_WAYPOINT: {
strcpy(WayPoints[1 + packet.seq].command, "WAYPOINT");
break;
}
case MAV_CMD_NAV_LOITER_UNLIM: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_UNLIM");
break;
}
case MAV_CMD_NAV_LOITER_TURNS: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TURNS");
break;
}
case MAV_CMD_NAV_LOITER_TIME: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TIME");
break;
}
case MAV_CMD_NAV_RETURN_TO_LAUNCH: {
strcpy(WayPoints[1 + packet.seq].command, "RTL");
break;
}
case MAV_CMD_NAV_LAND: {
strcpy(WayPoints[1 + packet.seq].command, "LAND");
break;
}
case MAV_CMD_NAV_TAKEOFF: {
strcpy(WayPoints[1 + packet.seq].command, "TAKEOFF");
break;
}
default: {
sprintf(WayPoints[1 + packet.seq].command, "CMD:%i", packet.command);
break;
}
}
if (packet.x == 0.0) {
packet.x = 0.00001;
}
if (packet.y == 0.0) {
packet.y = 0.00001;
}
if (packet.z == 0.0) {
packet.z = 0.00001;
}
WayPoints[1 + packet.seq].p_lat = packet.x;
WayPoints[1 + packet.seq].p_long = packet.y;
WayPoints[1 + packet.seq].p_alt = packet.z;
WayPoints[1 + packet.seq].yaw = packet.param4;
sprintf(WayPoints[1 + packet.seq].name, "WP%i", packet.seq + 1);
WayPoints[1 + packet.seq + 1].p_lat = 0.0;
WayPoints[1 + packet.seq + 1].p_long = 0.0;
WayPoints[1 + packet.seq + 1].p_alt = 0.0;
WayPoints[1 + packet.seq + 1].yaw = 0.0;
WayPoints[1 + packet.seq + 1].name[0] = 0;
WayPoints[1 + packet.seq + 1].command[0] = 0;
/*
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="0" name="MAV_FRAME_GLOBAL">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="1" name="MAV_FRAME_LOCAL_NED">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="2" name="MAV_FRAME_MISSION">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="3" name="MAV_FRAME_GLOBAL_RELATIVE_ALT">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="4" name="MAV_FRAME_LOCAL_ENU">
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT: {
mavlink_mission_current_t packet;
mavlink_msg_mission_current_decode(msg, &packet);
// SDL_Log("mavlink: ## Active_WP %f ##\n", packet.seq);
uav_active_waypoint = (uint8_t)packet.seq;
break;
}
case MAVLINK_MSG_ID_RAW_IMU: {
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
/*
SDL_Log("## IMU_RAW_ACC_X %i ##\n", packet.xacc);
SDL_Log("## IMU_RAW_ACC_Y %i ##\n", packet.yacc);
SDL_Log("## IMU_RAW_ACC_Z %i ##\n", packet.zacc);
SDL_Log("## IMU_RAW_GYRO_X %i ##\n", packet.xgyro);
SDL_Log("## IMU_RAW_GYRO_Y %i ##\n", packet.ygyro);
SDL_Log("## IMU_RAW_GYRO_Z %i ##\n", packet.zgyro);
SDL_Log("## IMU_RAW_MAG_X %i ##\n", packet.xmag);
SDL_Log("## IMU_RAW_MAG_Y %i ##\n", packet.ymag);
SDL_Log("## IMU_RAW_MAG_Z %i ##\n", packet.zmag);
*/
ModelData.acc_x = (float)packet.xacc / 1000.0;
ModelData.acc_y = (float)packet.yacc / 1000.0;
ModelData.acc_z = (float)packet.zacc / 1000.0;
ModelData.gyro_x = (float)packet.zgyro;
ModelData.gyro_y = (float)packet.zgyro;
ModelData.gyro_z = (float)packet.zgyro;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t packet;
mavlink_msg_nav_controller_output_decode(msg, &packet);
/*
nav_roll
nav_pitch
alt_error
aspd_error
xtrack_error
nav_bearing
target_bearing
wp_dist
*/
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t packet;
mavlink_msg_vfr_hud_decode(msg, &packet);
// SDL_Log("## pa %f ##\n", packet.airspeed);
// SDL_Log("## pg %f ##\n", packet.groundspeed);
// SDL_Log("## palt %f ##\n", packet.alt);
if (GPS_found == 0) {
ModelData.p_alt = packet.alt;
}
// SDL_Log("## pc %f ##\n", packet.climb);
// SDL_Log("## ph %i ##\n", packet.heading);
// SDL_Log("## pt %i ##\n", packet.throttle);
break;
}
case MAVLINK_MSG_ID_RADIO: {
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
SDL_Log("mavlink: ## rxerrors %i ##\n", packet.rxerrors);
SDL_Log("mavlink: ## fixed %i ##\n", packet.fixed);
SDL_Log("mavlink: ## rssi %i ##\n", packet.rssi);
SDL_Log("mavlink: ## remrssi %i ##\n", packet.remrssi);
SDL_Log("mavlink: ## txbuf %i ##\n", packet.txbuf);
SDL_Log("mavlink: ## noise %i ##\n", packet.noise);
SDL_Log("mavlink: ## remnoise %i ##\n", packet.remnoise);
break;
}
default: {
// SDL_Log(" ## MSG_ID == %i ##\n", msg->msgid);
break;
}
}
}
static void parseGNS(un8 index, char *value)
{
un8 i, fix;
if (*value == 0)
return;
switch (index)
{
case GPS_GNS_MODE:
/* One character per system, N: no fix */
fix = 0;
for (i = 0; value[i] != 0; i++) {
fix |= (value[i] != 'N');
}
veVariantUn8(&local.fix, fix);
return;
case GPS_GNS_LATITUDE:
errno = 0;
veVariantFloat(&local.latitude, toDeg((float)atof(value)));
if (errno)
veVariantInvalidate(&local.latitude);
return;
case GPS_GNS_LAT_NORTH_SOUTH:
/* North is positive, south is negative */
if (value[0] == 'S')
local.latitude.value.Float *= -1;
return;
case GPS_GNS_LONGITUDE:
errno = 0;
veVariantFloat(&local.longitude, toDeg((float)atof(value)));
if (errno)
veVariantInvalidate(&local.longitude);
return;
case GPS_GNS_LONG_EAST_WEST:
/* East is positive, west is negative */
if (value[0] == 'W')
local.longitude.value.Float *= -1;
return;
case GPS_GNS_UTC_TIME:
memset(&local.time, 0, sizeof(struct tm));
local.time.tm_hour = a2b(*value++) * 10;
local.time.tm_hour += a2b(*value++);
local.time.tm_min = a2b(*value++) * 10;
local.time.tm_min += a2b(*value++);
local.time.tm_sec = a2b(*value++ ) * 10;
local.time.tm_sec += a2b(*value++);
return;
case GPS_GNS_ALTITUDE:
errno = 0;
veVariantFloat(&local.altitude, (float)atof(value));
if (errno)
veVariantInvalidate(&local.altitude);
return;
case GPS_GNS_NR_OF_SATELITES:
errno = 0;
veVariantUn8(&local.nrOfSats, (un8)atol(value));
if (errno)
veVariantInvalidate(&local.nrOfSats);
return;
}
}
