Rev 242 | Rev 252 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 242 | Rev 248 | ||
---|---|---|---|
Line 156... | Line 156... | ||
156 | 156 | ||
157 | //---------------------------------------------------------------- |
157 | //---------------------------------------------------------------- |
158 | void MK3MAG_SendCommand(u8 command) |
158 | void MK3MAG_SendCommand(u8 command) |
159 | { |
159 | { |
160 | // If I2C no transmission is in progress |
160 | // If I2C no transmission is in progress |
161 | if(I2C_State == I2C_STATE_IDLE) |
161 | if(I2C_LockBuffer(0)) |
162 | { |
162 | { |
- | 163 | u16 TxBytes = 0; |
|
163 | I2C_TxBufferSize = 0; |
164 | u16 RxBytes = 0; |
164 | I2C_pRxHandler_t pRxHandlerFunc = NULL; |
- | |
Line 165... | Line 165... | ||
165 | u8 RxBytes = 0; |
165 | I2C_pRxHandler_t pRxHandlerFunc = NULL; |
166 | 166 | ||
Line 167... | Line 167... | ||
167 | // update current command id |
167 | // update current command id |
168 | I2C_TxBuffer[I2C_TxBufferSize++] = command; |
168 | I2C_Buffer[TxBytes++] = command; |
169 | 169 | ||
170 | // set pointers to data area with respect to the command id |
170 | // set pointers to data area with respect to the command id |
Line 175... | Line 175... | ||
175 | pRxHandlerFunc = &MK3MAG_UpdateVersion; |
175 | pRxHandlerFunc = &MK3MAG_UpdateVersion; |
176 | break; |
176 | break; |
177 | case MK3MAG_CMD_WRITE_CAL: |
177 | case MK3MAG_CMD_WRITE_CAL: |
178 | RxBytes = sizeof(MK3MAG_ReadCal)+1; |
178 | RxBytes = sizeof(MK3MAG_ReadCal)+1; |
179 | pRxHandlerFunc = &MK3MAG_UpdateCalibration; |
179 | pRxHandlerFunc = &MK3MAG_UpdateCalibration; |
180 | memcpy((u8*)I2C_TxBuffer+1, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal)); |
180 | memcpy((u8*)I2C_Buffer+1, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal)); |
181 | I2C_TxBufferSize += sizeof(MK3MAG_WriteCal); |
181 | TxBytes += sizeof(MK3MAG_WriteCal); |
182 | break; |
182 | break; |
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; |
Line 190... | Line 190... | ||
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; |
194 | VIC_ITCmd(SSP0_ITLine, ENABLE); |
194 | VIC_ITCmd(SSP0_ITLine, ENABLE); |
195 | memcpy((u8*)I2C_TxBuffer+1, (u8*)&MK3MAG_WriteAttitude, sizeof(MK3MAG_WriteAttitude)); |
195 | memcpy((u8*)I2C_Buffer+1, (u8*)&MK3MAG_WriteAttitude, sizeof(MK3MAG_WriteAttitude)); |
196 | I2C_TxBufferSize += sizeof(MK3MAG_WriteAttitude); |
196 | TxBytes += sizeof(MK3MAG_WriteAttitude); |
197 | break; |
197 | break; |
198 | default: // unknown command id |
198 | default: // unknown command id |
199 | RxBytes = 0; |
199 | RxBytes = 0; |
200 | pRxHandlerFunc = NULL; |
200 | pRxHandlerFunc = NULL; |
201 | break; |
201 | break; |
202 | } |
202 | } |
203 | // update packet checksum |
203 | // update packet checksum |
204 | I2C_TxBuffer[I2C_TxBufferSize] = MK3MAG_CalcCRC((u8*)I2C_TxBuffer, I2C_TxBufferSize); |
204 | I2C_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C_Buffer, TxBytes); |
205 | I2C_TxBufferSize++; |
205 | TxBytes++; |
206 | // initiate I2C transmission |
206 | // initiate I2C transmission |
207 | I2C_Transmission(MK3MAG_SLAVE_ADDRESS, pRxHandlerFunc, RxBytes); |
207 | I2C_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes); |
208 | } // EOF I2C_State == I2C_IDLE |
208 | } // EOF I2C_State == I2C_IDLE |
209 | } |
209 | } |
Line 210... | Line 210... | ||
210 | 210 |