/*----------------------------------------------------------------------------
* Thread 'Accelerometer': Reads Accelerometer
*---------------------------------------------------------------------------*/
void Thread_ACCELEROMETER (void const *argument)
{
/* These are preserved between function calls (static) */
KalmanState kstate_pitch = {0.001, 0.0032, 0.0, 0.0, 0.0}; /* Filter parameters obtained by experiment and variance calculations */
KalmanState kstate_roll = {0.001, 0.0032, 0.0, 0.0, 0.0}; /* Filter parameters obtained by experiment and variance calculations */
float ax, ay, az;
float roll, pitch;
osTimerId doubletap_timer_id, accel_dataready_timer_id;
doubletap_timer_id = osTimerCreate(osTimer(doubletap_timer), osTimerOnce, NULL);
accel_dataready_timer_id = osTimerCreate(osTimer(accel_dataready_timer), osTimerOnce, NULL);
while(1)
{
/* Wait for accelerometer new data signal or Nucleo board SPI signal for read request.
No need to send data all the time. */
osSignalWait(ACCELEROMETER_SIGNAL, osWaitForever);
Accelerometer_ReadAccel(&ax, &ay, &az);
Accelerometer_Calibrate(&ax, &ay, &az);
Accelerometer_GetRollPitch(ax, ay, az, &pitch, &roll);
Kalmanfilter_asm(&pitch, &filtered_pitch, 1, &kstate_pitch); /* filter the pitch angle */
Kalmanfilter_asm(&roll, &filtered_roll, 1, &kstate_roll); /* filter the roll angle */
if (Accelerometer_DetectDoubletap(sqrtf(ax * ax + ay * ay + az * az)))
NucleoSPI_SetDoubletap(doubletap_timer_id, DOUBLETAP_TIMEOUT_MS);
NucleoSPI_SetAccelDataready(accel_dataready_timer_id, ACCEL_DATAREADY_TIMEOUT_MS);
}
}
int main(){
const int length = 5;
float input[length] = {-0.665365, -0.329988, 0.164465, 0.043962, 0.295885};
float output[length]; // array for storing Kalman processed values
float temp[length]; // array for storing subtraction results
float temp2[length];
float miscresult[2] = {0, 0}; // array for storing mean and std dev results
float holder[length]; // array for storing convolution results
int i, j;
float corr_temp[1] = {0};
/*START OF PART II*/
kalman_state kstate;
reset(&kstate);
//Kalmanfilter_C(input, output, &kstate, length);
Kalmanfilter_asm(output, input, length, &kstate);
printf("\n");
/*END OF PART II*/
/*START OF PART III*/
// subtract
printf("subtraction:\n");
subtract(temp, input, output, length);
arm_sub_f32(input, output, temp2, length);
for(j = 0; j < length; j++){
printf("My implementation: %f CMSIS: %f\n", temp[j], temp2[j]);
}
// misc
printf("\n");
//misc(miscresult, temp, length);
arm_mean_f32(temp, length, &miscresult[0]);
arm_std_f32(temp, length, &miscresult[1]);
printf("mean: %f stdev: %f\n", miscresult[0], miscresult[1]);
// correlation
//corr_temp[0] = correlation(input, output, length);
arm_correlate_f32(input, length, output, length, &corr_temp[0]);
printf("correlation: %f\n", corr_temp[0]);
// convolution
printf("\n");
for(i = 0; i < length; i++){
holder[i] = 0;
}
convolve(holder, input, output, length);
//arm_conv_f32(input, length, output, length, holder);
for(i = 0; i < length; i++){
printf("convolution %f \n", holder[i]);
}
/*END OF PART III*/
return 0;
}
// Call to ASM Kalman filter function ***********************************************************************
int call_asm(float* inputArray, float* outputArray, int Length, kalman_t* kstate){
// Set initial values of kstate
kstate->q = DEF_q;
kstate->r = DEF_r;
kstate->x = DEF_x;
kstate->p = DEF_p;
kstate->k = DEF_k;
// Call assembly function to update kalman state with input
return Kalmanfilter_asm(inputArray, outputArray, length, kstate);
}
/** Get temperature
* @brief Obtains temperature voltage readout from ADC1 Channel 16
* @param None
* @retval Temperature float in celcius
*/
void updateTemp(void) {
float VSENSE;
// Obtain temperature voltage value from ADC
//need to get poll working
VSENSE = HAL_ADC_GetValue(&ADC1_Handle);
// Filter raw temperature sensor values
Kalmanfilter_asm(&VSENSE, &VSENSE, 1, &kalman_temperature);
//printf("%f\n", VSENSE);
/* Obtain permission for access to tempValue and then Supdate
---------------------------------------------------------
ADC 3.0 Volts per 2^12 steps (12 bit resolution in configuration)
Voltage at 25C is 760mV
Avg slop is 25mV/1C
--------------------------------------------------------- */
osMutexWait(temperatureMutex, (uint32_t) THREAD_TIMEOUT);
temperatureValue = (VSENSE*(3.0f/ 4096.0f) - 0.76f)/0.025f + 25.0f;
//printf("Temperature Value: %f\n", temperatureValue);
osMutexRelease(temperatureMutex);
}
int main()
{
//initialize testing array
float testVector[] = {0.1f,0.2f,0.3f,0.4f,0.5f};
/*COMMENTED OUT LENGTH PARAM AS IT IS INCLUDED IN HEADER FILE*/
//get the size of the array
//int length = sizeof(testVector)/sizeof(float);
//initiate empty output array of size length
float outputArrayC[length];
//initialize the struct at p=r=q 0.1 and x=k=0
kalman_state currentState = {0.1f, 0.1f, 0.0f , 0.1f, 0.0f};
//call function Kalmanfilter_C
Kalmanfilter_C(measurements, outputArrayC, ¤tState, length);
//initiate empty output array of size length
float outputArrayASM[length];
//reinitialize the struct at p=r=q 0.1 and x=k=0
currentState.p = DEF_p;
currentState.r = DEF_r;
currentState.k = DEF_k;
currentState.q = DEF_q;
currentState.x = DEF_x;
//call subroutine Kalmanfilter_asm
Kalmanfilter_asm(measurements, outputArrayASM, ¤tState, length );
//Check for correctness with a error tolerance of 0.000001
float errorTolerance = 0.000001f;
float errorPercentage = 0.01;
//is_valid(outputArrayC, outputArrayASM, length, errorTolerance, "c vs asm");
//is_valid_relative(outputArrayC, outputArrayASM, length, errorTolerance, errorPercentage,"c vs asm");
int p;
//print KalmanFilter output
for ( p = 0; p < length; p++ )
{
printf("OutputASM: %f & OutputC %f\n", outputArrayASM[p], outputArrayC[p]);
}
float differenceC[length];
float differenceCMSIS[length];
//Difference
arm_sub_f32 (measurements, outputArrayC, differenceCMSIS, length);
c_sub(measurements, outputArrayC, differenceC, length);
//is_valid(differenceC, differenceCMSIS, length, errorTolerance, "Difference");
//is_valid_relative(differenceC, differenceCMSIS, length, errorTolerance, errorPercentage,"Difference");
//Print difference vector
for ( p = 0; p < length; p++ )
{
printf("DifferenceC: %f & DifferenceCMSIS %f \n", differenceC[p], differenceCMSIS[p]);
}
//Mean
float meanCMSIS;
float meanC;
arm_mean_f32 (differenceCMSIS, length , &meanCMSIS);
c_mean(differenceC,length, &meanC);
//is_valid(&meanC, &meanCMSIS, 1, errorTolerance, "mean");
//is_valid_relative(&meanC, &meanCMSIS, 1, errorTolerance, errorPercentage, "mean");
//Print mean values
printf("MeanC: %f & MeanCMSIS %f \n", meanC, meanCMSIS);
//STD
float stdC;
float stdCMSIS;
arm_std_f32 (differenceCMSIS, length, &stdCMSIS);
c_std(differenceC, length, &stdC);
//is_valid(&stdC, &stdCMSIS, 1, errorTolerance, "STD");
//is_valid_relative(&stdC, &stdCMSIS, 1, errorTolerance, errorPercentage,"STD");
//Print std values
printf("StandardDevC: %f & StandardDevCMSIS %f \n", stdC, stdCMSIS);
//correlation
float corC[2*length-1];
float corCMSIS[2*length-1];
arm_correlate_f32 (measurements, length, outputArrayC, length, corCMSIS);
c_correlate(measurements, outputArrayC, corC, length);
//is_valid(corC, corCMSIS, 2*length-1, errorTolerance, "correlation");
//is_valid_relative(corC, corCMSIS, 2*length-1, errorTolerance, errorPercentage, "correlation");
//convolution
float convC[2*length-1];
float convCMSIS[2*length-1];
arm_conv_f32 (measurements, length, outputArrayC, length, convCMSIS);
c_conv(measurements, outputArrayC, convC, length);
//is_valid(convC, convCMSIS, 2*length-1, errorTolerance, "convolution");
//is_valid_relative(convC, convCMSIS, 2*length-1, errorTolerance, errorPercentage, "convolution");
//Print correlation and convolution values
for ( p = 0; p < (2*length-1); p++ )
{
printf("ConvC: %f & ConvCMSIS: %f \n", convC[p], convCMSIS[p]);
}
for ( p = 0; p < (2*length-1); p++ )
{
printf("CorrelateC: %f & CorrelatCMSIS: %f \n", corC[p], corCMSIS[p]);
}
return 0;
}
int main(void)
{
uint32_t v_sense;
float tempinvolts, temperature;
int counter = 0;
HAL_StatusTypeDef rc;
KalmanState kstate = {0.01, 0.3, 0.0, 0.1, 0.0};
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* Configure the system clock */
SystemClock_Config();
/* Initialize the ADC and GPIO and LCD display */
initialize_ADC();
initialize_GPIO();
initialize_LCD();
/* then call start */
if (HAL_ADC_Start(&ADC1_handle) != HAL_OK)
Error_Handler(ADC_INIT_FAIL);
while (1) {
if (SYSTICK_READ_TEMP_FLAG == 1) {
if ((rc = HAL_ADC_PollForConversion(&ADC1_handle, 10000)) != HAL_OK)
printf("Error: %d\n", rc);
v_sense = HAL_ADC_GetValue(&ADC1_handle); /* Read register from the ADC */
tempinvolts = v_sense * (V_REF / 4096); /* Scale the reading into V (resolution is 12bits with VREF+ = 3.3V) */
temperature = (tempinvolts - V_25) / AVG_SLOPE + TEMP_REF + FUDGE_FACTOR; /* Formula for temperature from doc_05 p.230 */
/* Grind through the Kalman filter */
if (Kalmanfilter_asm(&temperature, &filtered_temp, 1, &kstate) != 0)
printf("Overflow error\n");
unfiltered_temp = temperature;
// printf("%d %f\n", counter, filtered_temp);
/* Display only once out of this many times the 7-segment display */
if (counter % SEGMENT_DISPLAY_PERIOD == 0) {
displayed_segment_value = filtered_temp;
display_LCD_num(filtered_temp);
}
if (ALARM_TRIGGERED_FLAG == 1) {
/* Avoid spurious noise by waiting for a lower threshold before turning off alarm */
if (filtered_temp < LOWER_THRESHOLD_TEMP) {
ALARM_TRIGGERED_FLAG = 0;
turn_off_LEDs();
}
} else {
if (filtered_temp > THRESHOLD_TEMP)
ALARM_TRIGGERED_FLAG = 1;
}
if (ALARM_TRIGGERED_FLAG == 1) {
if (counter % ALARM_LED_TOGGLE_PERIOD == 0)
toggle_LEDs();
}
printf("%f, %f\n", unfiltered_temp, filtered_temp);
++counter;
SYSTICK_READ_TEMP_FLAG = 0; /* Reset the flag */
}
/* Systick_Handler triggers the flag once every 50ms */
if (SYSTICK_DISPLAY_SEGMENT_FLAG == 1) {
display_segment_val();
SYSTICK_DISPLAY_SEGMENT_FLAG = 0;
}
}
}
