int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//-------------------------------------------------------------------
BLIAsyncStart(100, 100);
TMRDelay(5000); // To avoid false-start at Reset
BLIAsyncStop();
BLISignalOFF();
//*******************************************************************
// Switch 1 controls the Serial Data Logger (SDL) communication speed
//-------------------------------------------------------------------
if (_SW1)
// Switch 1 is ON - Configuring SDL for PIC-to-PIC
// high-speed communication at 1 MBaud
SDLInit(3, BAUD_1M);
else
// Switch 1 is OFF - Configuring SDL for ZigBEE
// wireless communication at 115.2 KBaud
SDLInit(3, BAUD_115200);
//*******************************************************************
// HMC5983 Magnetometer initialization
//-------------------------------------------------------------------
byte IL = 5; // Interrupt level
//------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Output Data Rate (ODR">
//------------------------------------------------------
// Typical Data Output Rate (Hz) for various ODR values
//------------------------------------------------------
// ODR = 0: 0.75
// ODR = 2: 1.5
// ODR = 2: 3
// ODR = 3: 7.5
// ODR = 4: 15 (Default)
// ODR = 5: 30
// ODR = 6: 75
// ODR = 7: 220 (fastest)
// </editor-fold>
byte ODR = 7; // Rate (fastest): 220 Hz
// <editor-fold defaultstate="collapsed" desc="Low-pass filtering (DLPF)">
//------------------------------------------------------
// Low-pass filtering achieved through sample averaging.
// Averaging does not affect effective ODR, so I assume
// that wit averaging enabled each successive reported
// sample is an average of the new measurement with "n"
// previous measurements - something like a FIR filter.
//------------------------------------------------------
// DLPF = 0 => 1-average
// DLPF = 1 => 2-average
// DLPF = 2 => 4-average
// DLPF = 3 => 8-average
//------------------------------------------------------
// </editor-fold>
byte DLPF = 0;
// <editor-fold defaultstate="collapsed" desc="Sensor Field Range (Gain)">
//------------------------------------------------------
// Recommended sensor field range (Ga) for various Gains
//------------------------------------------------------
// Gain = 0: 0.9
// Gain = 1: 1.3
// Gain = 2: 1.9
// Gain = 3: 2.5
// Gain = 4: 4.0
// Gain = 5: 4.7
// Gain = 6: 5.6
// Gain = 7: 8.1
//------------------------------------------------------
// The magnitude of Earth magnetic field varies over the
// surface of the Earth in the range 0.3 to 0.6 Gauss.
//------------------------------------------------------
// </editor-fold>
byte Gain = 1; // +/- 1.3 Ga
//------------------------------------------------------
HMC_Init(IL, ODR, Gain, DLPF);
//*******************************************************************
HMC_RC RC;
//----------------------
ulong i = 0;
//-----------------------------------------
byte RegA;
byte RegB;
//-------------------------------
RC = HMC_ReadA(&RegA);
i++;
//-------------------------------
RC = HMC_ReadB(&RegB);
i++;
//-------------------------------
//----------------------
struct
{
HMCData Sample;
ulong Count;
} SDLData;
SDLData.Count = 0;
//----------------------
while (1)
{
//-------------------------------
if ( HMC_OK != (RC = HMC_ReadSample(&SDLData.Sample)) )
BLIDeadStop("SOS", 3);
SDLData.Count++;
//-------------------------------
BLISignalFlip();
//-------------------------------
SDLPostIfReady((byte*)&SDLData, sizeof(SDLData));
//-------------------------------
}
//*******************************************************************
return 0;
}
static portTASK_FUNCTION( vMPDTask, pvParameters )
{
TickType_t xRate, xLastTime;
mpu9050_t mpu9050;
HMC_Data_t hmc_data;
BaseType_t uxBits = 0;
const TickType_t xTickToWait = 100;
//uint8_t counter[2] = {0, 0};
/* The parameters are not used. */
( void ) pvParameters;
Init_MPU9050();
HMC_Init();
xRate = 100;
xRate /= portTICK_PERIOD_MS;
/* We need to initialise xLastFlashTime prior to the first call to
vTaskDelayUntil(). */
xLastTime = xTaskGetTickCount();
for(;;)
{
uxBits = xEventGroupWaitBits(xEventGroup, MPU_DATA_READY|HMC_DATA_READY, pdTRUE, pdFALSE, xTickToWait);
if (uxBits & MPU_DATA_READY)
{
MPU9050_Read(&mpu9050);
vParTestSetLED( 4, 1);
vParTestSetLED( 5, 0);
/*if (counter[0]++ > 50)
{
printf("MPU:\nACCEL:\t");
printf("x=%d\ty=%d\tz=%d\n", mpu9050.accel.x, mpu9050.accel.y, mpu9050.accel.z);
printf("GYRO: x=%d\ty=%d\tz=%d\n", mpu9050.gyro.x, mpu9050.gyro.y, mpu9050.gyro.z);
counter[0] = 0;
}*/
}
if (uxBits & HMC_DATA_READY)
{
HMC_Read(&hmc_data);
vParTestSetLED(2, 1);
vParTestSetLED(3, 0);
/*if (counter[1]++ > 4)
{
printf("HMC:\tDirect: x=%d\ty=%d\tz=%d\n", hmc_data.direct.x, hmc_data.direct.y, hmc_data.direct.z);
printf("\tAngle: x=%.2f\ty=%.2f\tz=%.2f\n", hmc_data.angle.x, hmc_data.angle.x, hmc_data.angle.x);
counter[1] = 0;
}*/
}
if (!(uxBits & (MPU_DATA_READY|HMC_DATA_READY)))
{
vParTestSetLED(2, 0);
vParTestSetLED(3, 1);
vParTestSetLED( 4, 0);
vParTestSetLED( 5, 1);
vTaskDelayUntil(&xLastTime, xRate);
//printf("\n#########Error##########\n\n");
}
}
}
