| /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 ", |