Subversion Repositories MK3Mag

Rev

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

Rev 26 Rev 27
Line 98... Line 98...
98
int16_t Heading = -1;
98
int16_t Heading = -1;
99
 
99
 
100
 
100
 
101
void CalcFields(void)
101
void CalcFields(void)
102
{
102
{
103
        UncalMagnetX = (1 * UncalMagnetX + (RawMagnet1a - RawMagnet1b)) / 2;
103
        UncalMagnetX = (RawMagnet1a - RawMagnet1b) / 2;
104
        UncalMagnetY = (1 * UncalMagnetY + (RawMagnet3a - RawMagnet3b)) / 2;
104
        UncalMagnetY = (RawMagnet3a - RawMagnet3b) / 2;
105
        UncalMagnetZ = (1 * UncalMagnetZ + (RawMagnet2a - RawMagnet2b)) / 2;
105
        UncalMagnetZ = (RawMagnet2a - RawMagnet2b) / 2;
106
 
106
 
107
        if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range);
107
        if(Calibration.X.Range != 0) MagnetX = (1024L * (int32_t)(UncalMagnetX - Calibration.X.Offset)) / (Calibration.X.Range);
108
        else MagnetX = 0;
108
        else MagnetX = 0;
109
        if(Calibration.Y.Range != 0) MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range);
109
        if(Calibration.Y.Range != 0) MagnetY = (1024L * (int32_t)(UncalMagnetY - Calibration.Y.Offset)) / (Calibration.Y.Range);
110
        else MagnetY = 0;
110
        else MagnetY = 0;
Line 135... Line 135...
135
                Cy = -MagnetY;
135
                Cy = -MagnetY;
136
                Cz = MagnetZ;
136
                Cz = MagnetZ;
137
        }
137
        }
138
 
138
 
139
        // calculate nick and roll angle in rad
139
        // calculate nick and roll angle in rad
140
        nick_rad = ((double)ExternData.Attitude[NICK]) * M_PI / (double)(1800.0);
140
        nick_rad = ((double)I2C_WriteAttitude.Nick) * M_PI / (double)(1800.0);
141
        roll_rad = ((double)ExternData.Attitude[ROLL]) * M_PI / (double)(1800.0);
141
        roll_rad = ((double)I2C_WriteAttitude.Roll) * M_PI / (double)(1800.0);
142
        // calculate attitude correction
142
        // calculate attitude correction
143
        Hx = Cx * (double)cos(nick_rad) - Cz * (double)sin(nick_rad);
143
        Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad);
144
        Hy = Cy * (double)cos(roll_rad) + Cz * (double)sin(roll_rad);
144
        Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad);
145
 
145
 
146
        // calculate Heading
146
        // calculate Heading
147
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
147
        heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI);
148
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
148
        // atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation
149
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
149
        // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation.
Line 264... Line 264...
264
        DebugOut.Analog[1] =  MagnetY;
264
        DebugOut.Analog[1] =  MagnetY;
265
        DebugOut.Analog[2] =  MagnetZ;
265
        DebugOut.Analog[2] =  MagnetZ;
266
        DebugOut.Analog[3] =  UncalMagnetX;
266
        DebugOut.Analog[3] =  UncalMagnetX;
267
        DebugOut.Analog[4] =  UncalMagnetY;
267
        DebugOut.Analog[4] =  UncalMagnetY;
268
        DebugOut.Analog[5] =  UncalMagnetZ;
268
        DebugOut.Analog[5] =  UncalMagnetZ;
269
        DebugOut.Analog[6] =  ExternData.Attitude[NICK];
269
        DebugOut.Analog[6] =  I2C_WriteAttitude.Nick;
270
        DebugOut.Analog[7] =  ExternData.Attitude[ROLL];
270
        DebugOut.Analog[7] =  I2C_WriteAttitude.Roll;
271
        DebugOut.Analog[8] =  Calibration.X.Offset;
271
        DebugOut.Analog[8] =  Calibration.X.Offset;
272
        DebugOut.Analog[9] =  Calibration.X.Range;
272
        DebugOut.Analog[9] =  Calibration.X.Range;
273
        DebugOut.Analog[10] = Calibration.Y.Offset;
273
        DebugOut.Analog[10] = Calibration.Y.Offset;
274
        DebugOut.Analog[11] = Calibration.Y.Range;
274
        DebugOut.Analog[11] = Calibration.Y.Range;
275
        DebugOut.Analog[12] = Calibration.Z.Offset;
275
        DebugOut.Analog[12] = Calibration.Z.Offset;