//--------------------------------------------------------------------------------------------------------------
// Write dashboard type data to RS232 port for now .. eventually probably should be CAN or USB to vehicle display module.
void DashBoard::displayData(void) {
//signed short phaseC = getPhaseC(O_stm32->getPhase1(), O_stm32->getPhase2());
s16 phaseC = O_stm32->getPhase3();
float current = TK_ABS(O_stm32->getPhase1()) + TK_ABS(O_stm32->getPhase2()) + TK_ABS(phaseC);
O_serial->usartWriteDisclaimer();
O_serial->printf(" Bus (V): %d ",O_stm32->busVoltage());
O_serial->printf("pwrStgTemp (C): %d \r\n",O_stm32->powerStageTemperature());
O_serial->printf(" Motor Frq: %d ",O_stm32->getFrq());
O_serial->printf(" motorTemp (C): %d \r\n",m_MotorTemperature);
O_serial->printf(" RPM: %d ",O_stm32->getRPM());
O_serial->printf(" Flux: %d \r\n",O_stm32->getFlux());
O_serial->printf(" RawAccelPOT: %d ",O_stm32->readADC(ACCEL_AI));
O_serial->printf(" Torque Out: %d \r\n",O_stm32->getTorque());
O_serial->printf(" Phase 1 (A): %d ",O_stm32->getPhase1());
O_serial->printf(" Phase 2 (A): %d \r\n",O_stm32->getPhase2());
O_serial->printf(" PhAOffset (A): %d ",O_stm32->getPhaseAOffset());
O_serial->printf(" PhBOffset (A): %d \r\n",O_stm32->getPhaseBOffset());
O_serial->printf(" phase3 (A): %d ",phaseC);
O_serial->printf(" Total (A): %d.%d \r\n",(int)current,((int)(current*100.0))%100);
O_serial->printf("RotorTimeConst: %d ",O_stm32->getRotorTimeConstant());
O_serial->printf(" SlipFreq: %d \r\n",O_sine->GetCurFrq());
O_serial->printf(" Slip: %d ",O_stm32->getSlip());
O_serial->printf(" FluxAngle: %d \r\n",convertToDegrees(O_stm32->getFluxAngle()));
}
/**
* @brief System starts here and delegates different actions.
* @param None
* @retval None
*/
void acc_thread(void const *argument){
st_system_state state = ST_INIT ;
err_type error = NONE;
float pitchRadians, rollRadians, pitchDegrees, rollDegrees;
/* global variable */
int init_time = 0;
filterState accPitchFilter;
filterState accRollFilter;
float filteredPitch=0.0;
float filteredRoll=0.0;
/* Main function */
while (1) {
/* The system can be in multiple states, this is just good coding practice */
switch ( state ){
/* Initialization state */
case ST_INIT :
/**************************************/
/************ initialization **********/
/**************************************/
//acc_init();
acc_ext_init();
initializeFilter(&accPitchFilter, 35);
initializeFilter(&accRollFilter, 35);
/**************************************/
state = ST_IDLE ;
break ;
/* Idle State */
case ST_IDLE :
/**************************************/
/**************** idle ****************/
/**************************************/
delay(init_time);
/**************************************/
state = ST_RUNNING ;
break ;
/* The system is running here the majority of the time */
case ST_RUNNING :
/**************************************/
/***************** run ****************/
/**************************************/
/* Accelerometer Interrupt. When high run the following: */
//if(interrupt_EXTI0 == 1){
osSignalWait(0x1,osWaitForever);
/* Reset flag */
interrupt_EXTI0 = 0;
/* Read accelerometer data X,Y,Z into */
float data[3];
//LIS3DSH_ReadACC(data);
//LSM9DS1_ReadACC(data);
LSM9DS1_ReadGYRO(data);
offsetAccData(data);
//printf("%f\t%f\t%f\n", data[0]/*x*/, data[1]/*y*/, data[2]/*z*/);
/* Calculate the pitch and roll in radians based on the following equation: */
/* Pitch = arctan(Ax1/sqrt((Ay1)^2+(Az1)^2 */
/* Roll = arctan(Ay1/sqrt((Ax1)^2+(Az1)^2 */
pitchRadians = calculatePitch(data);
rollRadians = calculateRoll(data);
/* Converting pitch and roll to degrees */
pitchDegrees = convertToDegrees(pitchRadians);
rollDegrees = convertToDegrees(rollRadians);
/* Filtering the pitch and roll angles in degrees using a moving average filter*/
filteredPitch = modify_filterState(&accPitchFilter, pitchDegrees);
filteredRoll = modify_filterState(&accRollFilter, rollDegrees);
/* Create message */
message_acc* acc_message = (message_acc*) osPoolAlloc(acc_mpool);
acc_message->pitch = filteredPitch;
acc_message->roll = filteredRoll;
osMessagePut(acc_mqueue, (uint32_t)acc_message, osWaitForever);
printf("%lf\t%f\t%lf\t%f\n", pitchDegrees, filteredPitch, rollDegrees, filteredRoll);
//}
/**************************************/
break ;
case ST_ERROR :
/**************************************/
/**************** error ***************/
/**************************************/
switch ( error ){
case NONE :
//You didn't set error type...
break ;
case ERR_ERROR_1 :
break ;
case ERR_ERROR_2 :
break ;
case ERR_ERROR_3 :
break ;
case ERR_ERROR_4 :
break ;
}
/**************************************/
break ;
}
//osDelay(250);
}
}
