Subversion Repositories MK3Mag

Rev

Rev 19 | Rev 21 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 19 Rev 20
Line 104... Line 104...
104
        MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range);
104
        MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range);
105
        MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range);
105
        MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range);
106
        MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range);
106
        MagnetZ = (1024L * (int32_t)(UncalMagnetZ - Calibration.Z.Offset)) / (Calibration.Z.Range);
107
}
107
}
108
 
108
 
-
 
109
 
109
void CalcHeading(void)
110
void CalcHeading(void)
110
{
111
{
111
        double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
112
        double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz;
112
        int16_t heading = -1;
113
        int16_t heading = -1;
113
 
114
 
114
        // calculate nick and roll angle i rad
-
 
115
        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800);
-
 
116
        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800);
-
 
117
 
115
 
118
        Cx = MagnetX;
116
        Cx = MagnetX;
119
        Cy = MagnetY;
117
        Cy = MagnetY;
120
        Cz = MagnetZ;
118
        Cz = MagnetZ;
121
 
119
 
Line 124... Line 122...
124
                Cx = MagnetX;
122
                Cx = MagnetX;
125
                Cy = -MagnetY;
123
                Cy = -MagnetY;
126
                Cz = MagnetZ;
124
                Cz = MagnetZ;
127
        }
125
        }
128
 
126
 
129
        Hx = Cx * (double)cos(nick_rad) +
127
        // calculate nick and roll angle in rad
130
                Cy * (double)sin(nick_rad) * (double)sin(roll_rad) -
128
        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
131
                Cz * (double)sin(nick_rad) * (double)cos(roll_rad);
129
        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
132
 
-
 
-
 
130
        // calculate attitude correction
133
        Hy = Cy * (double)cos(roll_rad) +
131
        Hx = Cx * (double)cos(nick_rad) - Cz * (double)sin(nick_rad);
134
                Cz * (double)sin(roll_rad);
132
        Hy = Cy * (double)cos(roll_rad) + Cz * (double)sin(roll_rad);
135
 
-
 
136
 
133
 
137
        if(Hx == 0 && Hy < 0) heading = 90;
134
        // calculate Heading
138
        else if(Hx == 0 && Hy > 0) heading = 270;
135
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
139
        else if(Hx < 0) heading  = 180 - (atan(Hy / Hx) * 180.0) / M_PI;
136
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
-
 
137
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
140
        else if(Hx > 0 && Hy < 0) heading = - (atan(Hy / Hx) * 180.0) / M_PI;
138
        if (heading < 0) heading = -heading;
141
        else if(Hx > 0 && Hy > 0) heading  = 360 - (atan(Hy / Hx) * 180.0) / M_PI;
139
        else heading = 360 - heading;
142
 
140
 
143
        if(abs(heading) < 361) Heading = heading;
141
        if(abs(heading) < 361) Heading = heading;
144
 
-
 
-
 
142
        else (Heading = -1);
145
}
143
}
146
 
144
 
147
 
145
 
148
void Calibrate(void)
146
void Calibrate(void)
149
{
147
{