/**
* @brief A function used to read and filter accelerometer readings
* @param none
* @param none
* @retval
*/
void get_calibrated_acceleration(void)
{
int j;
//Get the accelerometer readings
LIS3DSH_ReadACC(acceleration_reading);
//Filter Ax, Ay and Az values
Kalmanfilter_C(acceleration_reading[0], &x_kstate);
Kalmanfilter_C(acceleration_reading[1], &y_kstate);
Kalmanfilter_C(acceleration_reading[2], &z_kstate);
//Save the filtered Ax, Ay, Az values
acceleration_filtered[0] = x_kstate.x;
acceleration_filtered[1] = y_kstate.x;
acceleration_filtered[2] = z_kstate.x;
//acceleration_filtered[3] = 1; //needed for the normalization calculation
//Calculate the normalized acceleration values
for(i = 0; i < 3; i++) {
for(j = 0; j < 3; j++) {
acceleration_normalized[i] += acceleration_filtered[j]*calibration_param_matrix[j][i];
}
//adding 1*ACC10/20/30
acceleration_normalized[i] += calibration_param_matrix[3][i];
}
//Print the filtered values
//print_filtered_acceleration();
//Calculate the roll and pitch angles
calculate_angles();
}
/**
* @brief Read acceleration values from accelerometer.
* @param ax, ay, az: pointers to float data to store result into.
* @retval None
*/
static void Accelerometer_ReadAccel(float *ax, float *ay, float *az)
{
float acc_components[3];
LIS3DSH_ReadACC(acc_components);
*ax = acc_components[0];
*ay = acc_components[1];
*az = acc_components[2];
}
/**
* @brief Read Acc values, Calibrate them, filter them, and update the pitch value to be displayed
* @param None
* @retval None
*/
void ReadAcc(void){
/* Get values */
LIS3DSH_ReadACC(accValue);
Calibrate(accValue);
/* Filter values */
kalmanUpdate(xState, accValue[0]);
kalmanUpdate(yState, accValue[1]);
kalmanUpdate(zState, accValue[2]);
pitchAngle = calcPitch(xState->x, yState->x, zState->x);
rollAngle = calcRoll(xState->x, yState->x, zState->x);
}
/**
* @brief Calculates angles - filter them and use calibration matrix
* POSITIONING_AXIS specifies which axis to use for positioning
* @param None
* @retval None
*/
void calculateAngles (void) {
float angles[3];
// NOTE1: We initially read acceleration in the interrupt, but on discussion
// with the TA on Friday, we've pushed it over here for the purposes of
// the code submission.
LIS3DSH_ReadACC(out);
Kalmanfilter_C (out, out, &kalman_x, 1);
Kalmanfilter_C (out+1, out+1, &kalman_y, 1);
Kalmanfilter_C (out+2, out+2, &kalman_z, 1);
arm_mat_mult_f32(&w_matrix,&x_matrix,&y_matrix);
convertAccToAngle(acc, angles);
// Get angles
getSetValue(angles[0],1,0);
getSetValue(angles[1],1,1);
}
/**
* @brief Worker thread main superloop that defines how the thread will always display values
* depending on the relevant mode of operation
* @param void*
* @retval void
*/
void Thread_MEMS(void const *argument){
float read_acc[] = {0, 0, 0};
double output_x, output_y, output_z;
Reset_MEMS(&kstate_x);
Reset_MEMS(&kstate_y);
Reset_MEMS(&kstate_z);
// double den_pitch, den_roll;
while(1){
osDelay(200);
// printf("mems: works\n\r");
// printf("mems: interrupt = %d\n\r", interrupt);
if(interrupt != 0){
interrupt = 0;
// printf("mems: now interrupt = %d\n\r", interrupt);
LIS3DSH_ReadACC(read_acc);
// printf("%f | %f | %f\n", read_acc[0], read_acc[1], read_acc[2]);
if(!Kalmanfilter_C(read_acc[0], &output_x, &kstate_x) && !Kalmanfilter_C(read_acc[1], &output_y, &kstate_y)
&& !Kalmanfilter_C(read_acc[2], &output_z, &kstate_z)){
// printf("x = %f out = %f\n", read_acc[0], output_x);
// printf("y = %f out = %f\n", read_acc[1], output_y);
// printf("z = %f out = %f\n", read_acc[2], output_z);
output_x = output_x + 0.10493;
output_y = output_y + 0.143217;
output_z = output_z + 0.665265;
osMutexWait(mems_mutex_id, osWaitForever);
// roll = (atan2(-fYg, fZg)*180.0)/M_PI;
roll = (atan2(-output_y, output_z) * 180.0) / 3.1416;
osMutexRelease(mems_mutex_id);
// printf("mems: roll= %f\n", roll);
osMutexWait(mems_mutex_id, osWaitForever);
// pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;
pitch = (atan2(output_x, sqrt(output_y * output_y + output_z * output_z))*180.0) / 3.1416;
osMutexRelease(mems_mutex_id);
// printf("mems: roll= %f\n", pitch);
}
else printf("mems: Kalman filter returned error!\n");
}
}
}
/**----------------------------------------------------------------------------
* Thread 'Thread_angles': set Pitch and Roll display
*---------------------------------------------------------------------------*/
void Thread_angles (void const *argument) {
pitch_mutex = osMutexCreate(osMutex(pitch_mutex));
roll_mutex = osMutexCreate(osMutex(roll_mutex));
while(1){
float out[3], acc[3];
osSignalWait(1, osWaitForever);
LIS3DSH_ReadACC(out);
/* Calculations based on Eq 8 and 9 in Doc 15*/
/* Y direction is towards mini usb connector, Z is downwards*/
/* Use RHR to digure out X*/
acc[0] = out[0] - XOFFSET;
acc[1] = out[1] - YOFFSET;
acc[2] = out[2] - ZOFFSET;
filtered_acc[0] = kalmanfilter_c(acc[0],&xacc_state);
filteredX = filtered_acc[0];
filtered_acc[1] = kalmanfilter_c(acc[1],&yacc_state);
filteredY = filtered_acc[1];
filtered_acc[2] = kalmanfilter_c(acc[2],&zacc_state);
filteredZ = filtered_acc[2];
osSignalSet(tid_Thread_doubleTap, 1);
osMutexWait(pitch_mutex, osWaitForever);
pitch = 90 + (180/M_PI)*atan2(filtered_acc[1], sqrt(pow(filtered_acc[0], 2) + pow(filtered_acc[2], 2))); //Pitch in degrees
osMutexRelease(pitch_mutex);
osMutexWait(roll_mutex, osWaitForever);
roll = 90 + (180/M_PI)*atan2(filtered_acc[0], sqrt(pow(filtered_acc[1], 2) + pow(filtered_acc[2], 2))); //Roll in degrees
osMutexRelease(roll_mutex);
osSignalClear(tid_Thread_angles, 1);
}
}
void readAccelerometerData(void){
float readings[3];
float* calibrated_matrix;
float* kalman_output;
float Ax, Ay,Az;
LIS3DSH_ReadACC(readings);
kalman_output = multiplyMatrix(readings);
readings[0] = kalmanFilter(readings[0],&kSx);
readings[1] = kalmanFilter(readings[1],&kSy);
readings[2] = kalmanFilter(readings[2],&kSz);
calibrated_matrix = multiplyMatrix(readings);
Ax = calibrated_matrix[0];
Ay = calibrated_matrix[1];
Az = calibrated_matrix[2];
Pitch = atan2(Ax,Az) * 180 / 3.1415926515;
Roll = atan2(Ay,Az)* 180 / 3.1415926515;
//printf("Pitch = %f \n", Pitch);
}
//Rangle function to read the data from the sensor
float Rangle(){
float xyz[3];
float xangle;
float yangle;
float zangle;
LIS3DSH_ReadACC(xyz);
// X offset ~= -19
// Y offset ~= +12
// Z offset ~= -40
// the offset is added to the read value for calibration
xangle = xyz[0] -19;
yangle = xyz[1] +12;
zangle = xyz[2] -40 ;
return calculateangle(xangle,yangle,zangle);
}
