int main(void)
{
HardwareInit();
SoftwareInit();
UART0Send("Main\r\n",6);
while (true)
{
PrintTelemetry();
}
}
/**
* @brief main函数
* @param 参数名 参数说明
* @param 无
* @retval 无
* @par 使用全局变量 \n
* @note ● 执行时间: \n
* ● 调用周期: \n
* ● 可否打断: 可以 \n
*
* @par 注意:
* ● 无 \n
*/
int main()
{
SystemInit();
SoftwareInit();
while(1)
{
if (ReqHeartBeat)
{
Task();
ReqHeartBeat = 0;
}
}
}
void BatteryController_Task(void)
{
SoftwareInit();
uint16_t i;
timer_t balanceDoneTimer;
Timer_Setup(&balanceDoneTimer, NULL);
// Run this task forever.
// todo: Update this to variable that disables on error
while(1)
{
I2C_Update();
state_t state = GetState();
if ((state == CHARGE) || (state == CHARGE_BALANCE))
{
for (i = 0; i < CELLS_IN_SERIES; i++)
{
// If any cell above maximum cell voltage
if (cells[i].voltage >= MAX_CELL_VOLTAGE)
{
// If only charging, go to wait. If charge and balance,
// go to balance.
(state == CHARGE) ? SetState(WAIT) : SetState(BALANCE);
}
if (cells[i].voltage >= MAX_CELL_CRITICAL_VOLTAGE)
{
// Go into error state.
// todo: Assert error
SetState(ERROR);
}
}
}
if ((state == BALANCE) || (state == CHARGE_BALANCE))
{
// todo: This probably needs changed. We will leave balancing
// as soon as no cells are balancing but we should wait for
// relaxation time before knowing for sure to leave this state
Bool doneBalancing = TRUE;
for (i = 0; i < CELLS_IN_SERIES; i++)
{
if (batteryController_NeedsBalanced(&cells[i]))
{
cells[i].balance = TRUE;
doneBalancing = FALSE;
}
else
{
cells[i].balance = FALSE;
}
}
if (doneBalancing)
{
/// No cells balancing. Start timer that will transition
/// states if cell doesn't balance in #BALANCE_RELAXATION_TIME
if (!Timer_IsActive(&balanceDoneTimer))
{
Timer_Start(&balanceDoneTimer, BALANCE_RELAXATION_TIME);
}
}
else
{
/// Cell still balancing. Stop balancer timeout timer.
Timer_Stop(&balanceDoneTimer);
}
}
if (state == WAIT)
{
//TEMPORARY
uint8_t expanderInputPort0 = I2C_GetPortInput(PORT_0);
uint8_t expanderInputPort1 = I2C_GetPortInput(PORT_1);
uint16_t * testPtr = NULL;
/// Update SPI outputs
Uint16 i = 0;
for (i = 0; i < DRV8860_IN_SERIES; i++)
{
/// Open all relays
SPI_PushToQueue(0xFF, RELAYS);
}
SPI_SendTx(RELAYS);
//SPI_DRV8860_GetFaults(testPtr, 1);
if (expanderInputPort0 & START_BUTTON)
{
if (expanderInputPort0 & SWITCH_CHARGE_AND_BALANCE)
{
SetState(CHARGE_BALANCE);
}
else if (expanderInputPort0 & SWITCH_CHARGE)
{
// Set next state to CHARGE
SetState(CHARGE);
}
else
{
// Balance only mode
SetState(BALANCE);
}
}
}
}
}
int main(void)
{
unsigned long last_telemtry;
char pbuff[100];
long lTemp;
//
// The FPU should be enabled because some compilers will use floating-
// point registers, even for non-floating-point code. If the FPU is not
// enabled this will cause a fault. This also ensures that floating-
// point operations could be added to this application and would work
// correctly and use the hardware floating-point unit. Finally, lazy
// stacking is enabled for interrupt handlers. This allows floating-
// point instructions to be used within interrupt handlers, but at the
// expense of extra stack usage.
//
FPUEnable();
FPULazyStackingEnable();
HardwareInit();
SoftwareInit();
//last_telemtry = g_ulTickCount;
while(1)
{
// Execute main loop to interpretation protocol command
MainModeLoop();
EncoderTick(QEI0_BASE); // PITCH Encoder
EncoderTick(QEI1_BASE); // ROLL Encoder
#if 0
// restrict telemtry to about 10Hz,
if ((g_ulTickCount - last_telemtry) > 10)
//if(magready)
{
last_telemtry = g_ulTickCount;
eCompassCalculate();
sprintf(pbuff,"%.2f,%.2f, ACC: %hd,%hd,%hd MAG: %hd,%hd,%hd\r\n",fRho6DOF,fFitErrorpc,iGpx,iGpy,iGpz,iBpx,iBpy,iBpz);
UART0Send(pbuff, strlen(pbuff));
//sprintf(pbuff,"%.2f,%.2f,%.2f,%.2f, ACC: %hd,%hd,%hd MAG: %hd,%hd,%hd\r\n",fRho6DOF,fPhi6DOF,fThe6DOF,fPsi6DOF,iGpx,iGpy,iGpz,iBpx,iBpy,iBpz);
//UART0Send(pbuff, strlen(pbuff));
//sprintf(pbuff,"%.2f,%.2f,%.2f,%.2f,ACC: %hd,%hd,%hd MAG: %hd,%hd,%hd\r\n",fRho6DOF,fPhi6DOF,fThe6DOF,fPsi6DOF,iGpx,iGpy,iGpz,iBpx,iBpy,iBpz);
//UART0Send(pbuff, strlen(pbuff));
//sprintf(pbuff,"ROLL %.2f,PITCH %.2f,YAW %.2f\r\n",fPhi6DOF,fThe6DOF,fPsi6DOF);
//UART0Send(pbuff, strlen(pbuff));
//sprintf(pbuff,"Phi %.2f The %.2f Psi %.2f Rho %.2f delta %.2f\n", fPhi6DOF, fThe6DOF, fPsi6DOF, fRho6DOF, fdelta6DOF);
//sprintf(pbuff,"%lu,%.2f\r\n",g_ulTickCount,fRho6DOF);
//UART0Send(pbuff, strlen(pbuff));
}
#endif
if(itgready == true)
{
//DEBUG_TP6(0);
/*------------------------------------------------------------*/
/* ITG3200getXYZ() time is 30~ microsecond */
/*------------------------------------------------------------*/
ITG3200getXYZ();
#ifdef USE_BMA180_INTERRUPT
/*------------------------------------------------------------*/
/* BMA180GetXYZ() time is 30~ microsecond */
/*------------------------------------------------------------*/
//BMA180GetXYZ();
/*------------------------------------------------------------*/
/* ITG3200getXYZ() time is 30~ microsecond */
/*------------------------------------------------------------*/
//MAG3110getXYZ();
eCompassCalculate();
/*------------------------------------------------------------*/
/* AHRS() time is 25~ microsecond */
/*------------------------------------------------------------*/
AHRS();
#endif
if(g_tbstabilizer == true)
{
/*------------------------------------------------------------*/
/* Stabilize() time is 4~ microsecond */
/*------------------------------------------------------------*/
//Stabilize(ROLL_MOTOR);
/*------------------------------------------------------------*/
/* Stabilize() time is 4~ microsecond */
/*------------------------------------------------------------*/
//Stabilize(PITCH_MOTOR);
/*------------------------------------------------------------*/
/* Total time is 115~ microsecond test by EranS */
/* Gyro and Encoder dose not test yet. Need testing with new board */
/*------------------------------------------------------------*/
}
}
//DEBUG_TP6(0);
#if 0
if(1)//new_adc == true)
{
new_adc=false;
//PitchAmp = (float)PitchRollAmp[PITCH_MOTOR];
//PitchAmp = (PitchAmp /FULL_SCALE_BIT)*3; // 2Exp12 * 3V
//PitchAmp = PitchAmp / 5;
//PitchAmp*=1000;
//sprintf(pbuff,"%.2fmA\r\n",PitchAmp);
//sprintf(pbuff,"%lu,%u,%u\r\n",g_ulTickCount,PitchRollAmp[PITCH_MOTOR],PitchRollAmp[ROLL_MOTOR]);
sprintf(pbuff,"%.2f\r\n",y_rate_roll);
//sprintf(pbuff,"%lu,%lu\r\n",g_ulTickCount,g_ulAverage);
UART0Send(pbuff, strlen(pbuff));
}
#endif
}
}
