| 0,0 → 1,492 |
|---|
| /*####################################################################################### |
| MK3Mag 3D-Magnet sensor |
| !!! THIS IS NOT FREE SOFTWARE !!! |
| #######################################################################################*/ |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Copyright (c) 05.2008 Holger Buss |
| // + Thanks to Ilja Fähnrich (P_Latzhalter) |
| // + Nur für den privaten Gebrauch |
| // + www.MikroKopter.com |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
| // + mit unserer Zustimmung zulässig |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
| // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
| // + AUSNAHME: Ein bei www.mikrokopter.de erworbener vorbestückter MK3Mag darf als Baugruppe auch in kommerziellen Systemen verbaut werden |
| // + Im Zweifelsfall bitte anfragen bei: info@mikrokopter.de |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
| // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
| // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
| // + eindeutig als Ursprung verlinkt werden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
| // + Benutzung auf eigene Gefahr |
| // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
| // + this list of conditions and the following disclaimer. |
| // + * PORTING this software (or parts of it) to systems (other than hardware from www.mikrokopter.de) is NOT allowed |
| // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
| // + from this software without specific prior written permission. |
| // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
| // + for non-commercial use (directly or indirectly) |
| // + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
| // + with our written permission |
| // + Exception: A preassembled MK3Mag, purchased from www.mikrokopter.de may be used as a part of commercial systems |
| // + In case of doubt please contact: info@MikroKopter.de |
| // + * If sources or documentations are redistributet on other webpages, our webpage (http://www.MikroKopter.de) must be |
| // + clearly linked as origin |
| // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
| // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| // + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| // + POSSIBILITY OF SUCH DAMAGE. |
| // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| #include <avr/interrupt.h> |
| #include <math.h> |
| #include <stdlib.h> |
| #include <stdio.h> |
| |
| #include "main.h" |
| #include "timer0.h" |
| #include "twislave.h" |
| #include "led.h" |
| #include "analog.h" |
| #include "uart.h" |
| |
| #define CALIBRATION_VERSION 1 |
| |
| AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
| Orientation_t Orientation = ORIENTATION_FC; |
| |
| uint16_t Led_Timer = 0; |
| |
| typedef struct |
| { |
| int16_t Range; |
| int16_t Offset; |
| } Scaling_t; |
| |
| typedef struct |
| { |
| Scaling_t MagX; |
| Scaling_t MagY; |
| Scaling_t MagZ; |
| Scaling_t AccX; |
| Scaling_t AccY; |
| Scaling_t AccZ; |
| unsigned char Version; |
| } Calibration_t; |
| |
| Calibration_t eeCalibration EEMEM; // calibration data in EEProm |
| Calibration_t Calibration; // calibration data in RAM |
| |
| // magnet sensor variable |
| int16_t RawMagnet1a, RawMagnet1b; // raw magnet sensor data |
| int16_t RawMagnet2a, RawMagnet2b; |
| int16_t RawMagnet3a, RawMagnet3b; |
| int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
| int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings |
| |
| // acceleration sensor variables |
| int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0; // raw acceleration readings |
| int16_t AccX = 0, AccY = 0, AccZ = 0; // rescaled acceleration readings |
| int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc |
| |
| int16_t Heading = -1; // the current compass heading in deg |
| |
| |
| void CalcFields(void) |
| { |
| UncalMagX = (RawMagnet1a - RawMagnet1b); |
| UncalMagY = (RawMagnet3a - RawMagnet3b); |
| UncalMagZ = (RawMagnet2a - RawMagnet2b); |
| |
| if(Calibration.MagX.Range != 0) MagX = (1024L * (int32_t)(UncalMagX - Calibration.MagX.Offset)) / (Calibration.MagX.Range); |
| else MagX = 0; |
| if(Calibration.MagY.Range != 0) MagY = (1024L * (int32_t)(UncalMagY - Calibration.MagY.Offset)) / (Calibration.MagY.Range); |
| else MagY = 0; |
| if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range); |
| else MagZ = 0; |
| |
| if(AccPresent) |
| { |
| AccX = (RawAccX - Calibration.AccX.Offset); |
| AccY = (RawAccY - Calibration.AccY.Offset); |
| AccZ = (Calibration.AccZ.Offset - RawAccZ); |
| #if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL |
| // acc mode assumes orientation like FC |
| if(AccX > 136) AccAttitudeNick = -800; |
| else |
| if(AccX < -136) AccAttitudeNick = 800; |
| else AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI); |
| |
| |
| if(AccY > 136) AccAttitudeRoll = 800; |
| else |
| if(AccY < -136) AccAttitudeRoll = -800; |
| else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI); |
| |
| #else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z) |
| // acc mode assumes orientation like FC |
| if(AccY > 136) AccAttitudeNick = 800; |
| else |
| if(AccY < -136) AccAttitudeNick = -800; |
| else AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI); |
| |
| |
| if(AccX > 136) AccAttitudeRoll = 800; |
| else |
| if(AccX < -136) AccAttitudeRoll = -800; |
| else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI); |
| #endif |
| } |
| } |
| |
| |
| void CalcHeading(void) |
| { |
| double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0; |
| int16_t nick, roll; |
| int16_t heading = -1; |
| |
| // blink code for normal operation |
| if(CheckDelay(Led_Timer)) |
| { |
| if(Calibration.Version != CALIBRATION_VERSION) LED_GRN_TOGGLE; |
| else LED_GRN_ON; |
| Led_Timer = SetDelay(150); |
| } |
| switch(Orientation) |
| { |
| case ORIENTATION_NC: |
| Cx = MagX; |
| Cy = MagY; |
| Cz = MagZ; |
| break; |
| |
| case ORIENTATION_FC: |
| // rotation of 90 deg compared to NC setup |
| Cx = MagY; |
| Cy = -MagX; |
| Cz = MagZ; |
| break; |
| } |
| |
| // calculate nick and roll angle in rad |
| switch(AttitudeSource) |
| { |
| case ATTITUDE_SOURCE_I2C: |
| cli(); // stop interrupts |
| nick = I2C_WriteAttitude.Nick; |
| roll = I2C_WriteAttitude.Roll; |
| sei(); // start interrupts |
| break; |
| case ATTITUDE_SOURCE_UART: |
| cli(); // stop interrupts |
| nick = ExternData.Attitude[NICK]; |
| roll = ExternData.Attitude[ROLL]; |
| sei(); // start interrupts |
| break; |
| case ATTITUDE_SOURCE_ACC: |
| nick = AccAttitudeNick; |
| roll = AccAttitudeRoll; |
| break; |
| default: |
| nick = 0; |
| roll = 0; |
| break; |
| } |
| |
| nick_rad = ((double)nick) * M_PI / (double)(1800.0); |
| roll_rad = ((double)roll) * M_PI / (double)(1800.0); |
| |
| // calculate attitude correction |
| Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
| Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
| |
| DebugOut.Analog[27] = (int16_t)Hx; |
| DebugOut.Analog[28] = (int16_t)Hy; |
| |
| // calculate Heading |
| heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
| // atan2 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; |
| |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match |
| //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| cli(); // stop interrupts |
| if(abs(heading) < 361) Heading = heading; |
| else (Heading = -1); |
| sei(); // start interrupts |
| |
| } |
| |
| void Calibrate(void) |
| { |
| uint8_t cal; |
| static uint8_t calold = 0; |
| static int16_t Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
| static uint8_t blinkcount = 0; |
| static uint8_t invert_blinking = 0; |
| |
| // check both sources of communication for calibration request |
| if(I2C_WriteCal.CalByte) cal = I2C_WriteCal.CalByte; |
| else cal = ExternData.CalState; |
| |
| |
| if(cal > 5) cal = 0; |
| // blink code for current calibration state |
| if(cal) |
| { |
| if(CheckDelay(Led_Timer) || (cal != calold)) |
| { |
| if(blinkcount & 0x01) if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF; |
| else if(invert_blinking) LED_GRN_OFF; else LED_GRN_ON; |
| |
| // end of blinkcount sequence |
| if((blinkcount + 1 ) >= (2 * cal)) |
| { |
| blinkcount = 0; |
| Led_Timer = SetDelay(1500); |
| } |
| else |
| { |
| blinkcount++; |
| Led_Timer = SetDelay(100); |
| } |
| } |
| } |
| else |
| { |
| if(invert_blinking) LED_GRN_ON; else LED_GRN_OFF; |
| } |
| // calibration state machine |
| switch(cal) |
| { |
| case 1: // 1st step of calibration |
| // initialize ranges |
| // used to change the orientation of the MK3MAG in the horizontal plane |
| Xmin = 10000; |
| Xmax = -10000; |
| Ymin = 10000; |
| Ymax = -10000; |
| Zmin = 10000; |
| Zmax = -10000; |
| Calibration.AccX.Offset = RawAccX; |
| Calibration.AccY.Offset = RawAccY; |
| Calibration.AccZ.Offset = RawAccZ; |
| invert_blinking = 0; |
| break; |
| |
| case 2: // 2nd step of calibration |
| // find Min and Max of the X- and Y-Sensors during rotation in the horizontal plane |
| if(UncalMagX < Xmin) Xmin = UncalMagX; |
| if(UncalMagX > Xmax) Xmax = UncalMagX; |
| if(UncalMagY < Ymin) Ymin = UncalMagY; |
| if(UncalMagY > Ymax) Ymax = UncalMagY; |
| invert_blinking = 1; |
| break; |
| |
| case 3: // 3rd step of calibration |
| // used to change the orientation of the MK3MAG vertical to the horizontal plane |
| invert_blinking = 0; |
| break; |
| |
| case 4: |
| // find Min and Max of the Z-Sensor |
| if(UncalMagZ < Zmin) Zmin = UncalMagZ; |
| if(UncalMagZ > Zmax) Zmax = UncalMagZ; |
| invert_blinking = 1; |
| break; |
| |
| case 5: |
| // Save values |
| if(cal != calold) // avoid continously writing of eeprom! |
| { |
| Calibration.MagX.Range = Xmax - Xmin; |
| Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
| Calibration.MagY.Range = Ymax - Ymin; |
| Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
| Calibration.MagZ.Range = Zmax - Zmin; |
| Calibration.MagZ.Offset = (Zmin + Zmax) / 2; |
| if((Calibration.MagX.Range > 150) && (Calibration.MagY.Range > 150) && (Calibration.MagZ.Range > 150)) |
| { |
| Calibration.Version = CALIBRATION_VERSION; |
| // indicate write process by setting the led off for 2 seconds |
| LED_GRN_OFF; |
| eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
| Led_Timer = SetDelay(2000); |
| // reset blinkcode |
| blinkcount = 0; |
| } |
| } |
| invert_blinking = 0; |
| break; |
| |
| default: |
| break; |
| } |
| calold = cal; |
| } |
| |
| |
| void SetDebugValues(void) |
| { |
| DebugOut.Analog[0] = MagX; |
| DebugOut.Analog[1] = MagY; |
| DebugOut.Analog[2] = MagZ; |
| DebugOut.Analog[3] = UncalMagX; |
| DebugOut.Analog[4] = UncalMagY; |
| DebugOut.Analog[5] = UncalMagZ; |
| switch(AttitudeSource) |
| { |
| case ATTITUDE_SOURCE_ACC: |
| DebugOut.Analog[6] = AccAttitudeNick; |
| DebugOut.Analog[7] = AccAttitudeRoll; |
| break; |
| |
| case ATTITUDE_SOURCE_UART: |
| DebugOut.Analog[6] = ExternData.Attitude[NICK]; |
| DebugOut.Analog[7] = ExternData.Attitude[ROLL]; |
| break; |
| |
| |
| case ATTITUDE_SOURCE_I2C: |
| DebugOut.Analog[6] = I2C_WriteAttitude.Nick; |
| DebugOut.Analog[7] = I2C_WriteAttitude.Roll; |
| break; |
| } |
| DebugOut.Analog[8] = Calibration.MagX.Offset; |
| DebugOut.Analog[9] = Calibration.MagX.Range; |
| DebugOut.Analog[10] = Calibration.MagY.Offset; |
| DebugOut.Analog[11] = Calibration.MagY.Range; |
| DebugOut.Analog[12] = Calibration.MagZ.Offset; |
| DebugOut.Analog[13] = Calibration.MagZ.Range; |
| if(I2C_WriteCal.CalByte) DebugOut.Analog[14] = I2C_WriteCal.CalByte; |
| else DebugOut.Analog[14] = ExternData.CalState; |
| DebugOut.Analog[15] = Heading; |
| DebugOut.Analog[16] = ExternData.UserParam[0]; |
| DebugOut.Analog[17] = ExternData.UserParam[1]; |
| DebugOut.Analog[18] = AccX; |
| DebugOut.Analog[19] = AccY; |
| DebugOut.Analog[20] = AccZ; |
| DebugOut.Analog[21] = RawAccX; |
| DebugOut.Analog[22] = RawAccY; |
| DebugOut.Analog[23] = RawAccZ; |
| DebugOut.Analog[24] = Calibration.AccX.Offset; |
| DebugOut.Analog[25] = Calibration.AccY.Offset; |
| DebugOut.Analog[26] = Calibration.AccZ.Offset; |
| DebugOut.Analog[29] = AttitudeSource; |
| } |
| |
| void AccMeasurement(void) |
| { |
| if(AccPresent) |
| { |
| RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2; |
| RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2; |
| RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2; |
| } |
| else |
| { |
| RawAccX = 0; |
| RawAccY = 0; |
| RawAccZ = 0; |
| } |
| } |
| |
| int main (void) |
| { |
| // reset input pullup |
| DDRC &=~((1<<DDC6)); |
| PORTC |= (1<<PORTC6); |
| |
| LED_Init(); |
| TIMER0_Init(); |
| USART0_Init(); |
| ADC_Init(); |
| I2C_Init(); |
| |
| sei(); // enable globale interrupts |
| |
| if(AccPresent) |
| { |
| USART0_Print("ACC present\n"); |
| } |
| |
| LED_GRN_ON; |
| |
| Led_Timer = SetDelay(200); |
| |
| // read calibration info from eeprom |
| eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
| |
| ExternData.CalState = 0; |
| I2C_WriteCal.CalByte = 0; |
| |
| |
| // main loop |
| while (1) |
| { |
| FLIP_LOW; |
| Delay_ms(2); |
| RawMagnet1a = ADC_GetValue(MAG_X); |
| RawMagnet2a = -ADC_GetValue(MAG_Y); |
| RawMagnet3a = ADC_GetValue(MAG_Z); |
| AccMeasurement(); |
| Delay_ms(1); |
| |
| FLIP_HIGH; |
| Delay_ms(2); |
| RawMagnet1b = ADC_GetValue(MAG_X); |
| RawMagnet2b = -ADC_GetValue(MAG_Y); |
| RawMagnet3b = ADC_GetValue(MAG_Z); |
| AccMeasurement(); |
| Delay_ms(1); |
| |
| CalcFields(); |
| |
| if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
| else CalcHeading(); |
| |
| // check data from USART |
| USART0_ProcessRxData(); |
| |
| if(NC_Connected) NC_Connected--; |
| if(FC_Connected) FC_Connected--; |
| // fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data |
| if(!FC_Connected && ! NC_Connected) |
| { |
| AttitudeSource = ATTITUDE_SOURCE_ACC; |
| Orientation = ORIENTATION_FC; |
| } |
| |
| if(PC_Connected) |
| { |
| USART0_EnableTXD(); |
| USART0_TransmitTxData(); |
| PC_Connected--; |
| } |
| else |
| { |
| USART0_DisableTXD(); |
| } |
| } // while(1) |
| } |
| |