/branches/dongfang_FC_rewrite/Flight-Ctrl_MEGA644p_NAVICTRL__ADXRS610_FC2.0_V0_74df_SVN0001.hex |
---|
File deleted |
/branches/dongfang_FC_rewrite/ENC-03_FC1.3.c |
---|
3,6 → 3,7 |
#include "analog.h" |
#include "twimaster.h" |
#include "configuration.h" |
#include "eeprom.h" |
#include "timer0.h" |
#define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510 |
/branches/dongfang_FC_rewrite/analog.c |
---|
73,6 → 73,8 |
// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit |
#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE)) |
const char* recal = ", recalibration needed."; |
/* |
* For each A/D conversion cycle, each analog channel is sampled a number of times |
* (see array channelsForStates), and the results for each channel are summed. |
103,9 → 105,9 |
* to be centered on zero. |
*/ |
volatile sensorOffset_t gyroOffset; |
volatile sensorOffset_t accOffset; |
volatile sensorOffset_t gyroAmplifierOffset; |
sensorOffset_t gyroOffset; |
sensorOffset_t accOffset; |
sensorOffset_t gyroAmplifierOffset; |
/* |
* This allows some experimentation with the gyro filters. |
342,10 → 344,6 |
yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW]; |
else |
yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW]; |
debugOut.analog[3] = rawGyroSum[0]; |
debugOut.analog[4] = rawGyroSum[1]; |
debugOut.analog[5] = rawGyroSum[2]; |
} |
void analog_updateAccelerometers(void) { |
473,12 → 471,13 |
void analog_setNeutral() { |
if (gyroAmplifierOffset_readFromEEProm()) { |
printf("gyro amp invalid, you must recalibrate."); |
printf("gyro amp invalid%s",recal); |
gyro_loadOffsets(1); |
} |
} else |
gyro_loadOffsets(0); |
if (gyroOffset_readFromEEProm()) { |
printf("gyro offsets invalid, you must recalibrate."); |
printf("gyro offsets invalid%s",recal); |
gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_SUMMATION_FACTOR_PITCHROLL; |
gyroOffset.offsets[YAW] = 512 * GYRO_SUMMATION_FACTOR_YAW; |
} |
487,7 → 486,7 |
debugOut.analog[7] = gyroOffset.offsets[ROLL]; |
if (accOffset_readFromEEProm()) { |
printf("acc. meter offsets invalid, you must recalibrate."); |
printf("acc. meter offsets invalid%s",recal); |
accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_SUMMATION_FACTOR_PITCHROLL; |
accOffset.offsets[Z] = 512 * ACC_SUMMATION_FACTOR_Z; |
} |
/branches/dongfang_FC_rewrite/analog.h |
---|
185,9 → 185,9 |
// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3. |
#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3)) |
extern volatile sensorOffset_t gyroOffset; |
extern volatile sensorOffset_t accOffset; |
extern volatile sensorOffset_t gyroAmplifierOffset; |
extern sensorOffset_t gyroOffset; |
extern sensorOffset_t accOffset; |
extern sensorOffset_t gyroAmplifierOffset; |
/* |
* This is not really for external use - but the ENC-03 gyro modules needs it. |
/branches/dongfang_FC_rewrite/commands.c |
---|
82,6 → 82,7 |
setActiveParamSet(argument); |
} |
paramSet_readFromEEProm(getActiveParamSet()); |
analog_calibrateGyros(); |
attitude_setNeutral(); |
flight_setNeutral(); |
controlMixer_setNeutral(); |
/branches/dongfang_FC_rewrite/configuration.c |
---|
55,12 → 55,12 |
#include "configuration.h" |
#include "sensors.h" |
#include "rc.h" |
int16_t variables[VARIABLE_COUNT]; |
paramset_t staticParams; |
channelMap_t channelMap; |
mixerMatrix_t mixerMatrix; |
dynamicParam_t dynamicParams; |
volatile dynamicParam_t dynamicParams; |
uint8_t CPUType = ATMEGA644; |
uint8_t boardRelease = 13; |
uint8_t requiredMotors; |
/branches/dongfang_FC_rewrite/configuration.h |
---|
43,7 → 43,7 |
uint8_t motorSmoothing; |
} dynamicParam_t; |
extern dynamicParam_t dynamicParams; |
extern volatile dynamicParam_t dynamicParams; |
typedef struct { |
uint8_t sourceIdx, targetIdx; |
/branches/dongfang_FC_rewrite/eeprom.c |
---|
111,7 → 111,8 |
// length is the length of the pure data not including checksum and revision number. |
void writeChecksummedBlock(uint8_t revisionNumber, uint8_t* data, uint16_t offset, uint16_t length) { |
uint16_t CRC = CRC16(data, length); |
eeprom_write_word(&EEPromArray[offset], CRC); |
eeprom_write_byte(&EEPromArray[offset], CRC&0xff); |
eeprom_write_byte(&EEPromArray[offset+1], CRC>>8); |
eeprom_write_byte(&EEPromArray[offset+2], revisionNumber); |
eeprom_write_block(data, &EEPromArray[offset+3], length); |
} |
119,10 → 120,10 |
// offset is where the checksum is stored, offset+1 is the revision number, and offset+2... are the data. |
// length is the length of the pure data not including checksum and revision number. |
uint8_t readChecksummedBlock(uint8_t revisionNumber, uint8_t* target, uint16_t offset, uint16_t length) { |
uint16_t CRCRead = eeprom_read_word(&EEPromArray[offset]); |
uint16_t CRCRead = eeprom_read_byte(&EEPromArray[offset]) | (eeprom_read_byte(&EEPromArray[offset+1])<<8); |
uint8_t revisionNumberRead = eeprom_read_byte(&EEPromArray[offset+2]); |
eeprom_read_block(target, &EEPromArray[offset+3], length); |
uint16_t CRCCalculated = CRC(target, length); |
uint16_t CRCCalculated = CRC16(target, length); |
uint8_t CRCError = (CRCRead != CRCCalculated); |
uint8_t revisionMismatch = (revisionNumber != revisionNumberRead); |
130,7 → 131,7 |
if (CRCError && revisionMismatch) printf("\n\rEEPROM CRC error and revision mismatch; "); |
else if (CRCError) printf("\n\rEEPROM CRC error; "); |
else if (revisionMismatch) printf("\n\rEEPROM revision mismatch; "); |
return (checksumError || revisionMismatch); |
return (CRCError || revisionMismatch); |
} |
/***************************************************/ |
159,7 → 160,7 |
// parameter version check |
if (setnumber<1 ||setnumber>5 || paramSet_readFromEEProm(setnumber)) { |
// if version check faild |
printf("writing default parameter sets"); |
printf("\n\rwriting default parameter sets"); |
for (uint8_t i=5; i>0; i--) { |
paramSet_default(i); |
paramSet_writeToEEProm(i); |
174,10 → 175,6 |
/***************************************************/ |
/* MixerTable */ |
/***************************************************/ |
uint8_t mixerMatrix_readFromEEProm(void) { |
return readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t)); |
} |
void mixerMatrix_writeToEEProm(void) { |
writeChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t)); |
} |
184,7 → 181,7 |
void mixerMatrix_readOrDefault(void) { |
// load mixer table |
if (mixerMatrix_readFromEEProm()) { |
if (readChecksummedBlock(EEMIXER_REVISION, (uint8_t*)&mixerMatrix, EEPROM_ADR_MIXER_TABLE, sizeof(mixerMatrix_t))) { |
printf("writing default mixerMatrix"); |
mixerMatrix_default(); // Quadro |
mixerMatrix_writeToEEProm(); |
196,23 → 193,19 |
requiredMotors++; |
} |
printf("\n\r\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors); |
printf("\n\r\r=============================="); |
printf("\n\rMixer-Config: '%s' (%u Motors)",mixerMatrix.name, requiredMotors); |
printf("\n\r==================================="); |
} |
/***************************************************/ |
/* ChannelMap */ |
/***************************************************/ |
uint8_t channelMap_readFromEEProm(void) { |
return readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t)); |
} |
void channelMap_writeToEEProm(void) { |
writeChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t)); |
} |
void channelMap_readOrDefault(void) { |
if (channelMap_readFromEEProm()) { |
if (readChecksummedBlock(CHANNELMAP_REVISION, (uint8_t*)&channelMap, EEPROM_ADR_CHANNELMAP, sizeof(channelMap_t))) { |
printf("writing default channel map"); |
channelMap_default(); |
channelMap_writeToEEProm(); |
/branches/dongfang_FC_rewrite/eeprom.h |
---|
8,24 → 8,24 |
#define EEPROM_ADR_PARAM_BEGIN 0 |
#define EEPROM_CHECKSUMMED_BLOCK_OVERHEAD 3 |
#define PID_ACTIVE_SET 0 // byte |
#define EEPROM_ADR_ACCOFFSET 1 |
#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
#define EEPROM_ADR_GYROAMPLIFIER (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROAMPLIFIER+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
#define EEPROM_ADR_MIXER_TABLE (EEPROM_ADR_CHANNELMAP+sizeof(channelMap_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(mixerMatrix_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
#define PID_ACTIVE_SET 0 // byte |
//#define EEPROM_ADR_ACCOFFSET 1 |
//#define EEPROM_ADR_GYROOFFSET (EEPROM_ADR_ACCOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_GYROAMPLIFIER (EEPROM_ADR_GYROOFFSET+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_CHANNELMAP (EEPROM_ADR_GYROAMPLIFIER+sizeof(sensorOffset_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_MIXER_TABLE (EEPROM_ADR_CHANNELMAP+sizeof(channelMap_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_PARAMSET_BEGIN (EEPROM_ADR_MIXER_TABLE+sizeof(mixerMatrix_t)+EEPROM_CHECKSUMMED_BLOCK_OVERHEAD) |
//#define EEPROM_ADR_ACCOFFSET 10 |
//#define EEPROM_ADR_GYROOFFSET 20 |
//#define EEPROM_ADR_GYROAMPLIFIER 30 |
//#define EEPROM_ADR_CHANNELMAP 40 |
//#define EEPROM_ADR_MIXER_TABLE 60 |
//#define EEPROM_ADR_PARAMSET_BEGIN 200 |
#define EEPROM_ADR_ACCOFFSET 16 |
#define EEPROM_ADR_GYROOFFSET 32 |
#define EEPROM_ADR_GYROAMPLIFIER 48 |
#define EEPROM_ADR_CHANNELMAP 64 |
#define EEPROM_ADR_MIXER_TABLE 128 |
#define EEPROM_ADR_PARAMSET_BEGIN 256 |
#define CHANNELMAP_REVISION 0 |
#define EEPARAM_REVISION 0 |
#define EEMIXER_REVISION 0 |
#define EEPARAM_REVISION 0 |
#define EEMIXER_REVISION 0 |
#define SENSOROFFSET_REVISION 0 |
void paramSet_readOrDefault(void); |
35,10 → 35,10 |
uint8_t paramSet_readFromEEProm(uint8_t setnumber); |
void paramSet_writeToEEProm(uint8_t setnumber); |
uint8_t channelMap_readFromEEProm(void); |
//uint8_t channelMap_readFromEEProm(void); |
void channelMap_writeToEEProm(void); |
uint8_t mixerMatrix_eeadFromEEProm(void); |
//uint8_t mixerMatrix_readFromEEProm(void); |
void mixerMatrix_writeToEEProm(void); |
uint8_t gyroAmplifierOffset_readFromEEProm(void); |
/branches/dongfang_FC_rewrite/main.c |
---|
68,6 → 68,7 |
#include "configuration.h" |
#include "printf_P.h" |
#include "twimaster.h" |
#include "controlMixer.h" |
#ifdef USE_NAVICTRL |
#include "spi.h" |
#endif |
91,6 → 92,10 |
WDTCSR |= (1 << WDCE) | (1 << WDE); |
WDTCSR = 0; |
// This is strange: It should NOT be necessarty to do. But the call of the same, |
// in channelMap_readOrDefault (if eeprom read fails) just sets all to 0,0,0,.... |
channelMap_default(); |
// initalize modules |
output_init(); |
timer0_init(); |
107,6 → 112,11 |
MK3MAG_Init(); |
#endif |
// Parameter Set handling |
channelMap_readOrDefault(); |
mixerMatrix_readOrDefault(); |
paramSet_readOrDefault(); |
// enable interrupts global |
sei(); |
119,11 → 129,6 |
printf("\n\rSoftware: V%d.%d%c",VERSION_MAJOR, VERSION_MINOR, VERSION_PATCH + 'a'); |
printf("\n\r==================================="); |
// Parameter Set handling |
channelMap_readOrDefault(); |
mixerMatrix_readOrDefault(); |
paramSet_readOrDefault(); |
// Wait for a short time (otherwise the RC channel check won't work below) |
// timer = SetDelay(500); |
// while(!CheckDelay(timer)); |
/branches/dongfang_FC_rewrite/makefile |
---|
1,6 → 1,6 |
#-------------------------------------------------------------------- |
# MCU name |
MCU = atmega644 |
MCU = atmega644p |
F_CPU = 20000000 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
16,7 → 16,8 |
# Use one of the extensions for a gps solution |
#EXT = NAVICTRL |
EXT = MK3MAG |
#EXT = MK3MAG |
EXT = |
# Use optional one the RCs if EXT = NAVICTRL has been used |
#RC = DSL |
514,9 → 515,7 |
# Remove the '-' if you want to see the dependency files generated. |
-include $(SRC:.c=.d) |
# Listing of phony targets. |
.PHONY : all begin finish end sizebefore sizeafter gccversion coff extcoff \ |
clean clean_list program |
/branches/dongfang_FC_rewrite/mk3mag.c |
---|
60,7 → 60,7 |
#include "output.h" |
uint8_t PWMTimeout = 12; |
ToMk3Mag_t ToMk3Mag; |
ToMk3Mag_t toMk3Mag; |
/*********************************************/ |
/* Initialize Interface to MK3MAG Compass */ |
72,8 → 72,8 |
PWMTimeout = 0; |
ToMk3Mag.CalState = 0; |
ToMk3Mag.Orientation = 1; |
toMk3Mag.CalState = 0; |
toMk3Mag.Orientation = 1; |
} |
/*********************************************/ |
/branches/dongfang_FC_rewrite/mk3mag.h |
---|
8,7 → 8,7 |
uint8_t Orientation; |
} ToMk3Mag_t; |
extern ToMk3Mag_t ToMk3Mag; |
extern ToMk3Mag_t toMk3Mag; |
// Initialization |
void MK3MAG_Init(void); |
/branches/dongfang_FC_rewrite/rc.c |
---|
274,6 → 274,10 |
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
= RC_PRTY[CONTROL_YAW] = 0; |
} |
debugOut.analog[3] = channelMap.channels[0]; |
debugOut.analog[4] = channelMap.channels[1]; |
debugOut.analog[5] = channelMap.channels[2]; |
} |
/* |
/branches/dongfang_FC_rewrite/sensors.h |
---|
13,7 → 13,7 |
* Whether (pitch, roll, Z) acc. meters are reversed(see analog.h). |
*/ |
extern const uint8_t ACC_REVERSED[3]; |
extern volatile sensorOffset_t gyroAmplifierOffset; |
extern sensorOffset_t gyroAmplifierOffset; |
/* |
* Common procedures for all gyro types. |
/branches/dongfang_FC_rewrite/uart0.c |
---|
112,7 → 112,7 |
uint16_t DebugData_Timer; |
uint16_t Data3D_Timer; |
uint16_t DebugData_Interval = 500; // in 1ms |
uint16_t DebugData_Interval = 0; // in 1ms |
uint16_t Data3D_Interval = 0; // in 1ms |
#ifdef USE_MK3MAG |
125,9 → 125,9 |
"AnglePitch ", //0 |
"AngleRoll ", |
"AngleYaw ", |
"GyroPitch(PID) ", |
"GyroRoll(PID) ", |
"GyroYaw ", //5 |
"rc 0 ", |
"rc 1 ", |
"rc 2 ", //5 |
"OffsPitch ", |
"OffsRoll ", |
"AttitudeControl ", |
264,9 → 264,9 |
/****************************************************************/ |
ISR(USART0_RX_vect) |
{ |
static uint16_t crc; |
static uint16_t checksum; |
static uint8_t ptr_rxd_buffer = 0; |
uint8_t crc1, crc2; |
uint8_t checksum1, checksum2; |
uint8_t c; |
c = UDR0; // catch the received byte |
277,29 → 277,29 |
// the rxd buffer is unlocked |
if ((ptr_rxd_buffer == 0) && (c == '#')) { // if rxd buffer is empty and syncronisation character is received |
rxd_buffer[ptr_rxd_buffer++] = c; // copy 1st byte to buffer |
crc = c; // init crc |
checksum = c; // init checksum |
} |
#if 0 |
else if (ptr_rxd_buffer == 1) { // handle address |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
checksum += c; // update checksum |
} |
#endif |
else if (ptr_rxd_buffer < RXD_BUFFER_LEN) { // collect incomming bytes |
if (c != '\r') { // no termination character |
rxd_buffer[ptr_rxd_buffer++] = c; // copy byte to rxd buffer |
crc += c; // update crc |
checksum += c; // update checksum |
} else { // termination character was received |
// the last 2 bytes are no subject for checksum calculation |
// they are the checksum itself |
crc -= rxd_buffer[ptr_rxd_buffer - 2]; |
crc -= rxd_buffer[ptr_rxd_buffer - 1]; |
checksum -= rxd_buffer[ptr_rxd_buffer - 2]; |
checksum -= rxd_buffer[ptr_rxd_buffer - 1]; |
// calculate checksum from transmitted data |
crc %= 4096; |
crc1 = '=' + crc / 64; |
crc2 = '=' + crc % 64; |
checksum %= 4096; |
checksum1 = '=' + checksum / 64; |
checksum2 = '=' + checksum % 64; |
// compare checksum to transmitted checksum bytes |
if ((crc1 == rxd_buffer[ptr_rxd_buffer - 2]) && (crc2 |
if ((checksum1 == rxd_buffer[ptr_rxd_buffer - 2]) && (checksum2 |
== rxd_buffer[ptr_rxd_buffer - 1])) { |
// checksum valid |
rxd_buffer[ptr_rxd_buffer] = '\r'; // set termination character |
321,14 → 321,14 |
} |
// -------------------------------------------------------------------------- |
void AddCRC(uint16_t datalen) { |
uint16_t tmpCRC = 0, i; |
void Addchecksum(uint16_t datalen) { |
uint16_t tmpchecksum = 0, i; |
for (i = 0; i < datalen; i++) { |
tmpCRC += txd_buffer[i]; |
tmpchecksum += txd_buffer[i]; |
} |
tmpCRC %= 4096; |
txd_buffer[i++] = '=' + tmpCRC / 64; |
txd_buffer[i++] = '=' + tmpCRC % 64; |
tmpchecksum %= 4096; |
txd_buffer[i++] = '=' + tmpchecksum / 64; |
txd_buffer[i++] = '=' + tmpchecksum % 64; |
txd_buffer[i++] = '\r'; |
txd_complete = FALSE; |
UDR0 = txd_buffer[0]; // initiates the transmittion (continued in the TXD ISR) |
381,7 → 381,7 |
txd_buffer[txd_bufferIndex] = 0; |
} |
va_end(ap); |
AddCRC(pt); // add checksum after data block and initates the transmission |
Addchecksum(pt); // add checksum after data block and initates the transmission |
} |
*/ |
447,7 → 447,7 |
txd_buffer[pt++] = '=' + (c & 0x3f); |
} |
va_end(ap); |
AddCRC(pt); // add checksum after data block and initates the transmission |
Addchecksum(pt); // add checksum after data block and initates the transmission |
} |
// -------------------------------------------------------------------------- |
695,9 → 695,8 |
request_DebugLabel = 0xFF; |
} |
if (ConfirmFrame && txd_complete) { // Datensatz ohne CRC best�tigen |
SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, |
sizeof(ConfirmFrame)); |
if (ConfirmFrame && txd_complete) { // Datensatz ohne checksum bestätigen |
SendOutData('B', FC_ADDRESS, 1, (uint8_t*) &ConfirmFrame, sizeof(ConfirmFrame)); |
ConfirmFrame = 0; |
} |
728,12 → 727,12 |
#ifdef USE_MK3MAG |
if((checkDelay(Compass_Timer)) && txd_complete) { |
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
ToMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
ToMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
ToMk3Mag.CalState = compassCalState; |
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &ToMk3Mag,sizeof(ToMk3Mag)); |
toMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
toMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
toMk3Mag.UserParam[0] = dynamicParams.userParams[0]; |
toMk3Mag.UserParam[1] = dynamicParams.userParams[1]; |
toMk3Mag.CalState = compassCalState; |
SendOutData('w', MK3MAG_ADDRESS, 1,(uint8_t *) &toMk3Mag,sizeof(toMk3Mag)); |
// the last state is 5 and should be send only once to avoid multiple flash writing |
if(compassCalState > 4) compassCalState = 0; |
Compass_Timer = setDelay(99); |