| 14,7 → 14,7 |
|---|
| #include "output.h" |
| #include "attitude.h" |
| |
| #ifdef USE_MK3MAG |
| #ifdef USE_DIRECT_GPS |
| #include "mk3mag.h" |
| #endif |
| |
| 51,7 → 51,7 |
|---|
| uint8_t confirmFrame; |
| |
| typedef struct { |
| int16_t Heading; |
| int16_t heading; |
| }__attribute__((packed)) Heading_t; |
| |
| DebugOut_t debugOut; |
| 62,8 → 62,8 |
|---|
| uint16_t debugData_interval = 0; // in 1ms |
| uint16_t data3D_interval = 0; // in 1ms |
| |
| #ifdef USE_MK3MAG |
| int16_t compass_timer; |
| #ifdef USE_DIRECT_GPS |
| int16_t toMk3MagTimer; |
| #endif |
| |
| // keep lables in flash to save 512 bytes of sram space |
| 170,8 → 170,8 |
|---|
| // no bytes to send |
| txd_complete = TRUE; |
| |
| #ifdef USE_MK3MAG |
| compass_timer = setDelay(220); |
| #ifdef USE_DIRECT_GPS |
| toMk3MagTimer = setDelay(220); |
| #endif |
| |
| versionInfo.SWMajor = VERSION_MAJOR; |
| 446,9 → 446,9 |
|---|
| |
| case FC_ADDRESS: |
| switch (rxd_buffer[2]) { |
| #ifdef USE_MK3MAG |
| #ifdef USE_DIRECT_GPS |
| case 'K':// compass value |
| compassHeading = ((Heading_t *)pRxData)->Heading; |
| compassHeading = ((Heading_t *)pRxData)->heading; |
| // compassOffCourse = ((540 + compassHeading - compassCourse) % 360) - 180; |
| break; |
| #endif |
| 667,17 → 667,17 |
|---|
| request_externalControl = FALSE; |
| } |
| |
| #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; |
| #ifdef USE_DIRECT_GPS |
| if((checkDelay(toMk3MagTimer)) && 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)); |
| // 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); |
| toMk3MagTimer = setDelay(99); |
| } |
| #endif |
| |