Rev 242 | Go to most recent revision | Show entire file | Ignore 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; |
- | |
| 165 | u8 RxBytes = 0; |
165 | I2C_pRxHandler_t pRxHandlerFunc = NULL; |
| 166 | 166 | ||
| 167 | // update current command id |
167 | // update current command id |
| Line 168... | Line 168... | ||
| 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 |
| 171 | switch (command) |
171 | switch (command) |
| 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 | ||