Rev 252 | Rev 256 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 252 | Rev 254 | ||
|---|---|---|---|
| Line 146... | Line 146... | ||
| 146 | } |
146 | } |
| Line 147... | Line 147... | ||
| 147 | 147 | ||
| 148 | // rx data handler for heading request |
148 | // rx data handler for heading request |
| 149 | void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize) |
149 | void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize) |
| 150 | { // if crc is ok and number of byte are matching |
150 | { // if crc is ok and number of byte are matching |
| 151 | if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(CompassHeading)+1)) ) |
151 | if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(Compass_Heading)+1)) ) |
| 152 | { |
152 | { |
| 153 | memcpy((u8 *)&CompassHeading, pRxBuffer, sizeof(CompassHeading)); |
153 | memcpy((u8 *)&Compass_Heading, pRxBuffer, sizeof(Compass_Heading)); |
| 154 | } |
154 | } |
| Line 155... | Line 155... | ||
| 155 | } |
155 | } |
| 156 | 156 | ||
| Line 183... | Line 183... | ||
| 183 | case MK3MAG_CMD_READ_MAGVECT: |
183 | case MK3MAG_CMD_READ_MAGVECT: |
| 184 | RxBytes = sizeof(MagVector)+1; |
184 | RxBytes = sizeof(MagVector)+1; |
| 185 | pRxHandlerFunc = &MK3MAG_UpdateMagVector; |
185 | pRxHandlerFunc = &MK3MAG_UpdateMagVector; |
| 186 | break; |
186 | break; |
| 187 | case MK3MAG_CMD_READ_HEADING: |
187 | case MK3MAG_CMD_READ_HEADING: |
| 188 | RxBytes = sizeof(CompassHeading)+1; |
188 | RxBytes = sizeof(Compass_Heading)+1; |
| 189 | pRxHandlerFunc = &MK3MAG_UpdateHeading; |
189 | pRxHandlerFunc = &MK3MAG_UpdateHeading; |
| 190 | // update attitude from spi rx buffer |
190 | // update attitude from spi rx buffer |
| 191 | VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy |
191 | VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy |
| 192 | MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll; |
192 | MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll; |
| 193 | MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick; |
193 | MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick; |
| Line 221... | Line 221... | ||
| 221 | MK3MAG_Version.Major = 0xFF; |
221 | MK3MAG_Version.Major = 0xFF; |
| 222 | MK3MAG_Version.Minor = 0xFF; |
222 | MK3MAG_Version.Minor = 0xFF; |
| 223 | MK3MAG_Version.Patch = 0xFF; |
223 | MK3MAG_Version.Patch = 0xFF; |
| 224 | MK3MAG_Version.Compatible = 0xFF; |
224 | MK3MAG_Version.Compatible = 0xFF; |
| Line 225... | Line 225... | ||
| 225 | 225 | ||
| Line 226... | Line 226... | ||
| 226 | CompassHeading = -1; |
226 | Compass_Heading = -1; |
| 227 | 227 | ||
| 228 | // polling of version info |
228 | // polling of version info |
| 229 | repeat = 0; |
229 | repeat = 0; |
| Line 264... | Line 264... | ||
| 264 | if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return; |
264 | if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return; |
| Line 265... | Line 265... | ||
| 265 | 265 | ||
| 266 | if(CheckDelay(TimerCompassUpdate)) |
266 | if(CheckDelay(TimerCompassUpdate)) |
| 267 | { |
267 | { |
| 268 | // check for incomming compass calibration request |
268 | // check for incomming compass calibration request |
| 269 | // update CalByte from spi input queue |
269 | Compass_UpdateCalState(); |
| 270 | fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte)); |
270 | MK3MAG_WriteCal.CalByte = Compass_CalState; |
| 271 | // send new calstate |
271 | // send new calstate |
| 272 | if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
272 | if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte) |
| 273 | { |
273 | { |
| 274 | MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL); |
274 | MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL); |