WebSVN - NaviCtrl - Diff - Rev 288 and 292

 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // +  POSSIBILITY OF SUCH DAMAGE.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+#include <stdio.h>
 #include <string.h>
 #include "91x_lib.h"
 #include "mk3mag.h"
 #include "i2c.h"
 #include "timer1.h"
 #include "led.h"
-#include "spi_slave.h"
 #include "main.h"
 #include "uart1.h"
 #include "compass.h"
+{
   s16 Nick;
   s16 Roll;
 } __attribute__((packed)) MK3MAG_WriteAttitude_t;
+typedef struct
+{
+  u8 Major;
+  u8 Minor;
+  u8 Patch;
+  u8 Compatible;
+} __attribute__((packed)) MK3MAG_Version_t;
+typedef struct
+{
+  u8 CalByte;
+  u8 Dummy1;
+  u8 Dummy2;
+} __attribute__((packed)) MK3MAG_Cal_t;
 // Transfer buffers
 volatile MK3MAG_WriteAttitude_t         MK3MAG_WriteAttitude;
 volatile MK3MAG_Version_t                       MK3MAG_Version;
 volatile MK3MAG_Cal_t                           MK3MAG_WriteCal;
 // rx data handler for magnetic vector request
 void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
 {       // if crc is ok and number of byte are matching
         if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )
+        {
-                memcpy((u8 *)&MagVector, pRxBuffer, sizeof(MagVector));
+                s16vec_t *pMagVector = (s16vec_t*)pRxBuffer;
-        }
-}
-// rx data handler for heading request
+                MagVector.X = pMagVector->Y;
-void MK3MAG_UpdateHeading(u8* pRxBuffer, u8 RxBufferSize)
+                MagVector.Y = pMagVector->X;
-{       // if crc is ok and number of byte are matching
+                MagVector.Z = -pMagVector->Z;
-        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(Compass_Heading)+1)) )
-        {
-                memcpy((u8 *)&Compass_Heading, pRxBuffer, sizeof(Compass_Heading));
+                Compass_CalcHeading();
+        }
+}
+//----------------------------------------------------------------
+#define MK3MAG_CMD_VERSION              0x01
+#define MK3MAG_CMD_READ_MAGVECT         0x02
+#define MK3MAG_CMD_WRITE_CAL            0x04
 //----------------------------------------------------------------
 void MK3MAG_SendCommand(u8 command)
+{
         // try to catch the I2C buffer
         if(I2C_LockBuffer(0))
                                 break;
                         case MK3MAG_CMD_READ_MAGVECT:
                                 RxBytes = sizeof(MagVector)+1;
                                 pRxHandlerFunc = &MK3MAG_UpdateMagVector;
                                 break;
-                        case MK3MAG_CMD_READ_HEADING:
-                                RxBytes = sizeof(Compass_Heading)+1;
-                                pRxHandlerFunc = &MK3MAG_UpdateHeading;
-                                // update attitude from spi rx buffer
-                                VIC_ITCmd(SSP0_ITLine, DISABLE); // avoid spi buffer update during copy
-                                MK3MAG_WriteAttitude.Roll = FromFlightCtrl.AngleRoll;
-                                MK3MAG_WriteAttitude.Nick = FromFlightCtrl.AngleNick;
-                                VIC_ITCmd(SSP0_ITLine, ENABLE);
-                                memcpy((u8*)I2C_Buffer+1, (u8*)&MK3MAG_WriteAttitude, sizeof(MK3MAG_WriteAttitude));
-                                TxBytes += sizeof(MK3MAG_WriteAttitude);
-                                break;
                         default: // unknown command id
                                 RxBytes = 0;
                                 pRxHandlerFunc = NULL;
                                 break;
+                }
+        }
         return(MK3MAG_Present);
+}
 //----------------------------------------------------------------
-void MK3MAG_UpdateCompass(void)
+void MK3MAG_Update(void)
 {
-        static u32 TimerCompassUpdate = 0;
+        static u32 TimerUpdate = 0;
-        static s32 x_max,y_max,z_max,x_min,y_min,z_min;
+        static s16 x_max,y_max,z_max,x_min,y_min,z_min;
         if( (I2C_State == I2C_STATE_OFF) || !MK3MAG_Present ) return;
-        if(CheckDelay(TimerCompassUpdate))
+        if(CheckDelay(TimerUpdate))
+        {
-                // check for incomming compass calibration request
+                // check for incomming compass calibration request
                 Compass_UpdateCalState();
                 MK3MAG_WriteCal.CalByte = Compass_CalState;
                 // send new calstate
-                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
-                {
-                        MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
-                        x_max = -30000; y_max = -30000; z_max = -30000;
-                        x_min = 30000; y_min = 30000; z_min = 30000;
-                }
-                if(MK3MAG_WriteCal.CalByte == 2)
-                {
-                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
-                        if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 30; };
-                        if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 30; };
+                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
+                {       // send new calibration state
+                        MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
+                }
+                else
-                        if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 30; };
+                {       // calibration state matches
-                        if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 30; };
+                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer
-                }
+                        switch(Compass_CalState)
+                        {
+                                case 1:
+                                        x_max = -30000; y_max = -30000; z_max = -30000;
+                                        x_min = 30000; y_min = 30000; z_min = 30000;
+                                        break;
+                                case 2:
+                                                 if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }
+                                        else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }
+                                                 if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }
+                                        else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }
-                if(MK3MAG_WriteCal.CalByte == 4)
+                                        break;
-                {
+                                case 4:
-                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT);
+                                                 if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }
-                        if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 30; };
+                                        else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }
+                                        break;
-                        if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 30; };
-                }
-                else // request current heading
+                                default:
-                {
+                                        break;
                         MK3MAG_SendCommand(MK3MAG_CMD_READ_HEADING);
+                }

Subversion Repositories NaviCtrl

(root)/tags/V0.24b/mk3mag.c @ 335 - Rev 288 → 292