/branches/V0.68d Code Redesign killagreg/GPS.c |
---|
4,7 → 4,13 |
#include "ubx.h" |
#include "mymath.h" |
#include "timer0.h" |
#include "uart.h" |
#define TSK_IDLE 0 |
#define TSK_HOLD 1 |
#define TSK_HOME 2 |
int16_t GPS_Pitch = 0; |
int16_t GPS_Roll = 0; |
20,10 → 26,11 |
} GPS_Pos_t; |
// GPS coordinates for hold position |
GPS_Pos_t GPSHoldPoint = {0,0, INVALID}; |
GPS_Pos_t HoldPosition = {0,0, INVALID}; |
// GPS coordinates for home position |
GPS_Pos_t HomePosition = {0,0, INVALID}; |
// --------------------------------------------------------------------------------- |
33,102 → 40,138 |
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0; |
int32_t PD_North = 0, PD_East = 0; |
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0; |
static uint8_t GPS_Task = TSK_IDLE; |
// poti2 enables the gps feature |
if((Poti2 > 70) && (!EmergencyLanding)) // run GPS function only if Poti 2 is larger than 70 (switch on) |
if(Poti2 < 70) GPS_Task = TSK_IDLE; |
else if (Poti2 < 160) GPS_Task = TSK_HOLD; |
//else GPS_Task = TSK_HOME; |
switch(GPSInfo.status) |
{ |
switch(GPSInfo.status) |
case INVALID: // invalid gps data |
GPS_Pitch = 0; |
GPS_Roll = 0; |
if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary |
break; |
case PROCESSED: // if gps data are already processed |
// downcount timeout |
if(GPSTimeout) GPSTimeout--; |
// if no new data arrived within timeout set current data invalid |
// and therefore disable GPS |
else |
{ |
case INVALID: // invalid gps data |
GPS_Pitch = 0; |
GPS_Roll = 0; |
BeepTime = 50; |
break; |
case PROCESSED: // if gps data are already processed |
// downcount timeout |
if(GPSTimeout) GPSTimeout--; |
// if no new data arrived within timeout set current data invalid |
// and therefore disable GPS |
else |
GPSInfo.status = INVALID; |
} |
break; |
case VALID: // new valid data from gps device |
// if the gps data quality is sufficient |
if (GPSInfo.satfix == SATFIX_3D) |
{ |
switch(GPS_Task) // check what's to do |
{ |
GPS_Pitch = 0; |
GPS_Roll = 0; |
GPSInfo.status = INVALID; |
} |
break; |
case VALID: // new valid data from gps device |
// if the gps data quality is sufficient |
if (GPSInfo.satfix == SATFIX_3D) |
{ |
// if sticks are centered and hold position is valid enable position hold control |
if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID)) |
{ |
// Calculate deviation from hold position |
GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing; |
GPSPosDev_East = GPSInfo.utmeast - GPSHoldPoint.Easting; |
case TSK_IDLE: |
// update hold point to current gps position |
HoldPosition.Northing = GPSInfo.utmnorth; |
HoldPosition.Easting = GPSInfo.utmeast; |
HoldPosition.Status = VALID; |
// disable gps control |
GPS_Pitch = 0; |
GPS_Roll = 0; |
break; // eof TSK_IDLE |
case TSK_HOLD: |
// if sticks are centered and hold position is valid enable position hold control |
if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID)) |
{ |
// Calculate deviation from hold position |
GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing; |
GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting; |
DebugOut.Analog[12] = GPSPosDev_North; |
DebugOut.Analog[13] = GPSPosDev_East; |
//Calculate PD-components of the controller (negative sign for compensation) |
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
//Calculate PD-components of the controller (negative sign for compensation) |
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048; |
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512; |
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048; |
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512; |
// PD-controller |
PD_North = P_North + D_North; |
PD_East = P_East + D_East; |
// PD-controller |
PD_North = P_North + D_North; |
PD_East = P_East + D_East; |
// GPS to pitch and roll settings |
// GPS to pitch and roll settings |
//A positive pitch angle moves head downwards (flying forward). |
//A positive roll angle tilts left side downwards (flying left). |
//A positive pitch angle moves head downwards (flying forward). |
//A positive roll angle tilts left side downwards (flying left). |
// If compass heading is 0 the head of the copter is in north direction. |
// A positive pitch angle will fly to north and a positive roll angle will fly to west. |
// In case of a positive north deviation/velocity the |
// copter should fly to south (negative pitch). |
// In case of a positive east position deviation and a positive east velocity the |
// copter should fly to west (positive roll). |
// If compass heading is 0 the head of the copter is in north direction. |
// A positive pitch angle will fly to north and a positive roll angle will fly to west. |
// In case of a positive north deviation/velocity the |
// copter should fly to south (negative pitch). |
// In case of a positive east position deviation and a positive east velocity the |
// copter should fly to west (positive roll). |
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values |
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive |
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll. |
// rotation transformation to compass heading to match any copter orientation |
coscompass = (int32_t)c_cos_8192(CompassHeading); |
sincompass = (int32_t)c_sin_8192(CompassHeading); |
// rotation transformation to compass heading to match any copter orientation |
if (CompassHeading < 0) // no valid compass data |
{ // disable GPS |
GPS_Task = TSK_IDLE; |
GPS_Pitch = 0; |
GPS_Roll = 0; |
} |
else |
{ |
coscompass = (int32_t)c_cos_8192(CompassHeading); |
sincompass = (int32_t)c_sin_8192(CompassHeading); |
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
} |
} |
else // MK controlled by user |
{ |
// update hold point to current gps position |
HoldPosition.Northing = GPSInfo.utmnorth; |
HoldPosition.Easting = GPSInfo.utmeast; |
HoldPosition.Status = VALID; |
// disable gps control |
GPS_Pitch = 0; |
GPS_Roll = 0; |
} |
break; // eof TSK_HOLD |
default: // unhandled task |
GPS_Task = TSK_IDLE; |
GPS_Pitch = 0; |
GPS_Roll = 0; |
break; // eof default |
} // eof switch GPS_Task |
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192); |
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192); |
// limit GPS controls |
#define GPS_CTRL_LIMIT 35 |
if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
} // eof 3D-FIX |
// limit GPS controls |
#define GPS_CTRL_LIMIT 35 |
if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT; |
if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT; |
if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT; |
if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT; |
} |
else // MK controlled by user |
{ |
// update hold point to current gps position |
GPSHoldPoint.Northing = GPSInfo.utmnorth; |
GPSHoldPoint.Easting = GPSInfo.utmeast; |
GPSHoldPoint.Status = VALID; |
// disable gps control |
GPS_Pitch = 0; |
GPS_Roll = 0; |
} |
} // eof 3D-FIX |
else // no 3D-SATFIX |
{ // diable gps control |
GPS_Pitch = 0; |
GPS_Roll = 0; |
BeepTime = 50; |
} |
// set current data as processed to avoid further calculations on the same gps data |
GPSInfo.status = PROCESSED; |
break; |
} // eof GPSInfo.status |
} |
return; |
else // no 3D-SATFIX |
{ // disable gps control |
GPS_Pitch = 0; |
GPS_Roll = 0; |
if(GPS_Task != TSK_IDLE) BeepTime = 50; |
} |
// set current data as processed to avoid further calculations on the same gps data |
GPSInfo.status = PROCESSED; |
break; |
} // eof GPSInfo.status |
DebugOut.Analog[14] = GPS_Pitch; |
DebugOut.Analog[15] = GPS_Roll; |
} |
/branches/V0.68d Code Redesign killagreg/fc.c |
---|
386,10 → 386,10 |
if(MotorTest[3]) Motor_Right = MotorTest[3]; |
} |
DebugOut.Analog[12] = Motor_Front; |
DebugOut.Analog[13] = Motor_Rear; |
DebugOut.Analog[14] = Motor_Left; |
DebugOut.Analog[15] = Motor_Right; |
//DebugOut.Analog[12] = Motor_Front; |
//DebugOut.Analog[13] = Motor_Rear; |
//DebugOut.Analog[14] = Motor_Left; |
//DebugOut.Analog[15] = Motor_Right; |
//Start I2C Interrupt Mode |
twi_state = 0; |
1020,9 → 1020,13 |
updCompass = 49; // update only at 2ms*50 = 100ms (10Hz) |
// get current compass heading (angule between MK head and magnetic north) |
CompassHeading = MM3_Heading(); |
// calculate OffCourse (angular deviation from heading to course) |
if (CompassHeading < 0) // no compass data available |
{ |
CompassOffCourse = 0; |
if(!BeepTime) BeepTime = 100; // make noise at 10 Hz to signal the compass problem |
} |
else // calculate OffCourse (angular deviation from heading to course) |
CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
} |
// reduce compass effect with increasing declination |
1045,7 → 1049,7 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// GPS |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(ParamSet.GlobalConfig & CFG_GPS_ACTIVE) |
if((ParamSet.GlobalConfig & CFG_GPS_ACTIVE) && !EmergencyLanding) |
{ |
GPS_P_Factor = FCParam.UserParam5; |
GPS_D_Factor = FCParam.UserParam6; |
/branches/V0.68d Code Redesign killagreg/mm3.c |
---|
1,8 → 1,8 |
/* |
Copyright 2007, Niklas Nold |
Copyright 2008, by Killagreg |
This program (files compass.c and compass.h) is free software; you can redistribute it and/or modify |
This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify |
it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
either version 3 of the License, or (at your option) any later version. |
This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
10,7 → 10,8 |
GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
Please note: All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
Please note: The original implementation was done by Niklas Nold. |
All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
*/ |
#include <stdlib.h> |
#include <avr/io.h> |
61,6 → 62,7 |
MM3_calib_t MM3_calib; |
volatile MM3_working_t MM3; |
static volatile uint8_t MM3_Timeout = 0; |
107,6 → 109,8 |
MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
MM3_Timeout = 0; |
SREG = sreg; |
} |
114,12 → 118,12 |
/*********************************************/ |
/* Get Data from MM3 */ |
/*********************************************/ |
void MM3_Update() // called every 102.4 ms by timer 0 ISR |
void MM3_Update() // called every 102.4 µs by timer 0 ISR |
{ |
switch (MM3.STATE) |
{ |
case MM3_STATE_RESET: |
PORTC &= ~(1<<PORTC4); // select slave |
PORTC &= ~(1<<PORTC4); // select slave |
PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset |
MM3.STATE = MM3_STATE_START_TRANSFER; |
return; |
129,9 → 133,22 |
// write to SPDR triggers automatically the transfer MOSI MISO |
// MM3 Period, + AXIS code |
if (MM3.AXIS == MM3_X_AXIS) SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
else if (MM3.AXIS == MM3_Y_AXIS) SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
else SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; // MM3_Z_AXIS |
switch(MM3.AXIS) |
{ |
case MM3_X_AXIS: |
SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
break; |
case MM3_Y_AXIS: |
SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
break; |
case MM3_Z_AXIS: |
SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; |
break; |
default: |
MM3.AXIS = MM3_X_AXIS; |
MM3.STATE = MM3_STATE_RESET; |
return; |
} |
// DRDY line is not connected, therefore |
// wait before reading data back |
196,6 → 213,25 |
} |
PORTC |= (1<<PORTC4); // deselect slave |
MM3.STATE = MM3_STATE_RESET; |
// Update timeout is called every 102.4 µs. |
// It takes 2 cycles to write a measurement data request for one axis and |
// at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back. |
// I.e. 81 cycles * 102.4 µs = 8.3ms per axis. |
// The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() - |
// decremtent the MM3_Timeout every 100 ms. |
// incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout. |
if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis)) |
{ // if all axis measurements give diffrent readings the data should be valid |
if(MM3_Timeout < 20) MM3_Timeout++; |
} |
else // something is very strange here |
{ |
if(MM3_Timeout ) MM3_Timeout--; |
} |
return; |
default: |
return; |
} |
} |
214,7 → 250,7 |
ROT_OFF; |
// get maximum and minimum reading of all axis |
while (measurement) |
while (measurement && !MM3_Timeout) |
{ |
if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
233,7 → 269,6 |
beeper = 50; |
} |
beeper--; |
// loop with period of 10 ms / 100 Hz |
timer = SetDelay(10); |
while(!CheckDelay(timer)); |
241,24 → 276,26 |
// If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
} |
if(!MM3_Timeout) |
{ |
// Rage of all axis |
MM3_calib.X_range = (x_max - x_min); |
MM3_calib.Y_range = (y_max - y_min); |
MM3_calib.Z_range = (z_max - z_min); |
// Rage of all axis |
MM3_calib.X_range = (x_max - x_min); |
MM3_calib.Y_range = (y_max - y_min); |
MM3_calib.Z_range = (z_max - z_min); |
// Offset of all axis |
MM3_calib.X_off = (x_max + x_min) / 2; |
MM3_calib.Y_off = (y_max + y_min) / 2; |
MM3_calib.Z_off = (z_max + z_min) / 2; |
// Offset of all axis |
MM3_calib.X_off = (x_max + x_min) / 2; |
MM3_calib.Y_off = (y_max + y_min) / 2; |
MM3_calib.Z_off = (z_max + z_min) / 2; |
// save to EEProm |
SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
// save to EEProm |
SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
} |
} |
273,64 → 310,70 |
uint16_t div_factor; |
int16_t heading; |
// calibration factor for transforming Gyro Integrals to angular degrees |
div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
if (MM3_Timeout) |
{ |
// calibration factor for transforming Gyro Integrals to angular degrees |
div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
// Offset correction and normalization (values of H are +/- 512) |
Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
// Offset correction and normalization (values of H are +/- 512) |
Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
// Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
// assuming the MM3 board is mounted parallel to the frame. |
// User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
// in a top view counter clockwise direction. |
// North is in opposite direction of the small arrow on the MM3 board. |
// Therefore 180 deg must be added to that angle. |
angle = ((int16_t)ParamSet.UserParam4 + 180); |
// wrap angle to interval of 0°- 359° |
angle += 360; |
angle %= 360; |
sin_yaw = (int32_t)(c_sin_8192(angle)); |
cos_yaw = (int32_t)(c_cos_8192(angle)); |
// Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
// assuming the MM3 board is mounted parallel to the frame. |
// User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
// in a top view counter clockwise direction. |
// North is in opposite direction of the small arrow on the MM3 board. |
// Therefore 180 deg must be added to that angle. |
angle = ((int16_t)ParamSet.UserParam4 + 180); |
// wrap angle to interval of 0°- 359° |
angle += 360; |
angle %= 360; |
sin_yaw = (int32_t)(c_sin_8192(angle)); |
cos_yaw = (int32_t)(c_cos_8192(angle)); |
Hx_corr = Hx; |
Hy_corr = Hy; |
Hx_corr = Hx; |
Hy_corr = Hy; |
// rotate |
Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
// rotate |
Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
// tilt compensation |
// tilt compensation |
// calibration factor for transforming Gyro Integrals to angular degrees |
div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
// calibration factor for transforming Gyro Integrals to angular degrees |
div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
// calculate sinus cosinus of pitch and tilt angle |
angle = (IntegralPitch/div_factor); |
sin_pitch = (int32_t)(c_sin_8192(angle)); |
cos_pitch = (int32_t)(c_cos_8192(angle)); |
// calculate sinus cosinus of pitch and tilt angle |
angle = (IntegralPitch/div_factor); |
sin_pitch = (int32_t)(c_sin_8192(angle)); |
cos_pitch = (int32_t)(c_cos_8192(angle)); |
angle = (IntegralRoll/div_factor); |
sin_roll = (int32_t)(c_sin_8192(angle)); |
cos_roll = (int32_t)(c_cos_8192(angle)); |
angle = (IntegralRoll/div_factor); |
sin_roll = (int32_t)(c_sin_8192(angle)); |
cos_roll = (int32_t)(c_cos_8192(angle)); |
Hx_corr = Hx * cos_pitch; |
Hx_corr -= Hz * sin_pitch; |
Hx_corr /= 8192; |
Hx_corr = Hx * cos_pitch; |
Hx_corr -= Hz * sin_pitch; |
Hx_corr /= 8192; |
Hy_corr = Hy * cos_roll; |
Hy_corr += Hz * sin_roll; |
Hy_corr /= 8192; |
Hy_corr = Hy * cos_roll; |
Hy_corr += Hz * sin_roll; |
Hy_corr /= 8192; |
// calculate Heading |
heading = c_atan2(Hy_corr, Hx_corr); |
// calculate Heading |
heading = c_atan2(Hy_corr, Hx_corr); |
// atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
if (heading < 0) heading = -heading; |
else heading = 360 - heading; |
// atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
if (heading < 0) heading = -heading; |
else heading = 360 - heading; |
} |
else // MM3_Timeout = 0 i.e now new data from external board |
{ |
heading = -1; |
} |
return heading; |
} |
/branches/V0.68d Code Redesign killagreg/mm3.h |
---|
15,9 → 15,15 |
extern MM3_calib_t MM3_calib; |
// Initialization of the MM3 communication |
void MM3_Init(void); |
// should be called cyclic to get actual compass reading |
void MM3_Update(void); |
// this function calibrates the MM3 |
// and returns immediately if the communication to the MM3-Board is broken. |
void MM3_Calibrate(void); |
// calculates the current compass heading in a range from 0 to 360 deg. |
// returns -1 of no compass data are available |
int16_t MM3_Heading(void); |
#endif //_MM3_H |
/branches/V0.68d Code Redesign killagreg/timer0.c |
---|
109,7 → 109,7 |
// beeper on if duration is not over |
if(BeepTime > 1) |
if(BeepTime) |
{ |
BeepTime--; // decrement BeepTime |
if(BeepTime & BeepModulation) Beeper_On = 1; |
/branches/V0.68d Code Redesign killagreg/uart.c |
---|
57,10 → 57,10 |
"Voltage ", |
"Receiver Level ", //10 |
"Analog11 ", |
"Motor_Front ", |
"Motor_Rear ", |
"Motor_Left ", |
"Motor_Right ", //15 |
"GPSDevNorth ", |
"GPSDevEast ", |
"GPS_Pitch ", |
"GPS_Roll ", //15 |
"Acc_Z ", |
"MeanAccPitch ", |
"MeanAccRoll ", |