void writeKFValues() {
Kalman filterx = filterInit(1);//Create Kalman Object X
//Kalman filtery = filterInit(2);//Create Kalman Object Y
analogCalibrate(1);//Calibrate Analog Port
file = fopen("kf.csv", "w");
while(1){
//accelRead(KalmanObject)
fprintf(file, "%d,%d\n", analogReadCalibrated(1), accelRead(filterx));
printf("RawX: %d \t FilteredX: %d\n\r",analogReadCalibrated(1),accelRead(filterx));
//printf("RawY: %d\t FilteredY: %d\n\r",analogReadCalibrated(2),accelRead(filtery));
delay(2);
}
}
void _ISR _NOPSV _T1Interrupt(void) //Running at 10Hz
{
OUTER_LED = ON;
tiltX = (INT32)(accelRead(XINCL)*TILT_MULT);
gRead = (INT32)(gyroRead(GRATE)*GYRO_MULT);
//convert from quadrants to pitch angle
tilt = tiltX;
//update filter
do_kalman(gRead, tilt);
newData = TRUE;
_T1IF = 0;
}
int main()
{
TRISA = 0;
TRISB = BIT(0)|BIT(1)|BIT(3)|BIT(7)|BIT(12);
TRISC = 0;
OUTER_LED = OFF;
INNER_LED = OFF;
init_pll();
peripheral_pin_config();
init_uart1();
init_spi();
init_adc();
init_timer1();
TX_string("Wait\r\n");
delay(1000);
TX_string("Start\r\n");
T1CONbits.TON = 1;
//remove first trash reading
tiltX = accelRead(XINCL);
gRead = gyroRead(GRATE);
while(1)
{
if( newData == TRUE)
{
TX_snum5(gRead);
TX('\t');
TX_snum5(tilt);
TX('\t');
TX_snum5(x_00);
TX_string("\r\n");
OUTER_LED = OFF;
newData = FALSE;
}
}
return 0;
};
/*********************************************************************
* @fn KeyFobApp_ProcessEvent
*
* @brief Key Fob Application Task event processor. This function
* is called to process all events for the task. Events
* include timers, messages and any other user defined events.
*
* @param task_id - The OSAL assigned task ID.
* @param events - events to process. This is a bit map and can
* contain more than one event.
*
* @return none
*/
uint16 KeyFobApp_ProcessEvent( uint8 task_id, uint16 events )
{
if ( events & SYS_EVENT_MSG )
{
uint8 *pMsg;
if ( (pMsg = osal_msg_receive( keyfobapp_TaskID )) != NULL )
{
keyfobapp_ProcessOSALMsg( (osal_event_hdr_t *)pMsg );
// Release the OSAL message
VOID osal_msg_deallocate( pMsg );
}
// return unprocessed events
return (events ^ SYS_EVENT_MSG);
}
if ( events & KFD_START_DEVICE_EVT )
{
// Start the Device
VOID GAPRole_StartDevice( &keyFob_PeripheralCBs );
// Start Bond Manager
VOID GAPBondMgr_Register( &keyFob_BondMgrCBs );
// Start the Proximity Profile
VOID ProxReporter_RegisterAppCBs( &keyFob_ProximityCBs );
// Set timer for first battery read event
osal_start_timerEx( keyfobapp_TaskID, KFD_BATTERY_CHECK_EVT, BATTERY_CHECK_PERIOD );
// Start the Accelerometer Profile
VOID Accel_RegisterAppCBs( &keyFob_AccelCBs );
//Set the proximity attribute values to default
ProxReporter_SetParameter( PP_LINK_LOSS_ALERT_LEVEL, sizeof ( uint8 ), &keyfobProxLLAlertLevel );
ProxReporter_SetParameter( PP_IM_ALERT_LEVEL, sizeof ( uint8 ), &keyfobProxIMAlertLevel );
ProxReporter_SetParameter( PP_TX_POWER_LEVEL, sizeof ( int8 ), &keyfobProxTxPwrLevel );
// Set LED1 on to give feedback that the power is on, and a timer to turn off
HalLedSet( HAL_LED_1, HAL_LED_MODE_ON );
osal_pwrmgr_device( PWRMGR_ALWAYS_ON ); // To keep the LED on continuously.
osal_start_timerEx( keyfobapp_TaskID, KFD_POWERON_LED_TIMEOUT_EVT, 1000 );
return ( events ^ KFD_START_DEVICE_EVT );
}
if ( events & KFD_POWERON_LED_TIMEOUT_EVT )
{
osal_pwrmgr_device( PWRMGR_BATTERY ); // Revert to battery mode after LED off
HalLedSet( HAL_LED_1, HAL_LED_MODE_OFF );
return ( events ^ KFD_POWERON_LED_TIMEOUT_EVT );
}
if ( events & KFD_ACCEL_READ_EVT )
{
bStatus_t status = Accel_GetParameter( ACCEL_ENABLER, &accelEnabler );
if (status == SUCCESS)
{
if ( accelEnabler )
{
// Restart timer
if ( ACCEL_READ_PERIOD )
{
osal_start_timerEx( keyfobapp_TaskID, KFD_ACCEL_READ_EVT, ACCEL_READ_PERIOD );
}
// Read accelerometer data
accelRead();
}
else
{
// Stop the acceleromter
osal_stop_timerEx( keyfobapp_TaskID, KFD_ACCEL_READ_EVT);
}
}
else
{
//??
}
return (events ^ KFD_ACCEL_READ_EVT);
}
if ( events & KFD_BATTERY_CHECK_EVT )
{
// Restart timer
if ( BATTERY_CHECK_PERIOD )
{
osal_start_timerEx( keyfobapp_TaskID, KFD_BATTERY_CHECK_EVT, BATTERY_CHECK_PERIOD );
}
// perform battery level check
Batt_MeasLevel( );
return (events ^ KFD_BATTERY_CHECK_EVT);
}
if ( events & KFD_TOGGLE_BUZZER_EVT )
{
// if this event was triggered while buzzer is on, turn it off, increment beep_count,
// check whether max has been reached, and if not set the OSAL timer for next event to
// turn buzzer back on.
if ( buzzer_state == BUZZER_ON )
{
buzzerStop();
buzzer_state = BUZZER_OFF;
buzzer_beep_count++;
#if defined ( POWER_SAVING )
osal_pwrmgr_device( PWRMGR_BATTERY );
#endif
// check to see if buzzer has beeped maximum number of times
// if it has, then don't turn it back on
if ( ( buzzer_beep_count < BUZZER_MAX_BEEPS ) &&
( ( keyfobProximityState == KEYFOB_PROXSTATE_LINK_LOSS ) ||
( keyfobProximityState == KEYFOB_PROXSTATE_PATH_LOSS ) ) )
{
osal_start_timerEx( keyfobapp_TaskID, KFD_TOGGLE_BUZZER_EVT, 800 );
}
}
else if ( keyfobAlertState != ALERT_STATE_OFF )
{
// if this event was triggered while the buzzer is off then turn it on if appropriate
keyfobapp_PerformAlert();
}
return (events ^ KFD_TOGGLE_BUZZER_EVT);
}
#if defined ( PLUS_BROADCASTER )
if ( events & KFD_ADV_IN_CONNECTION_EVT )
{
uint8 turnOnAdv = TRUE;
// Turn on advertising while in a connection
GAPRole_SetParameter( GAPROLE_ADVERT_ENABLED, sizeof( uint8 ), &turnOnAdv );
}
#endif
// Discard unknown events
return 0;
}
