UINT16 GetChanVal(BYTE pin){
UINT16 val;
BYTE mode = GetChannelMode(pin);
switch (mode){
case HIGH_IMPEDANCE:
val=1;
break;
case IS_DI:
case IS_DO:
case IS_COUNTER_INPUT_HOME:
case IS_COUNTER_OUTPUT_HOME:
val = GetDIO(pin);
break;
case IS_SERVO:
val = GetServoPos(pin);
break;
case IS_PWM:
val = GetPWM(pin);
break;
case IS_DC_MOTOR_VEL:
case IS_DC_MOTOR_DIR:
val = GetDCMotor(pin);
break;
case IS_ANALOG_IN:
val=GetADC(pin);
break;
default:
val=1;
break;
}
return val;
}
int main (void)
{
//Test
//Test();
//Init
InitDevice();
//Send Startup Message
FCMessageSend("vFlyOK");
//等待INS的GPS完成启动
//WaitGPS();
//改变运行状态并发送状态信息
RunState=DefaultRunState;
FCEventSend(RunState);
//初始化参数
InitPara();
FCMessageSend("InitParaOK");
//启动主计时器
TimerEnable(0);
while(1)
{
if(Do==DoWorkLoop)
{
LED2ON();
INS();
SwitchMode();
if(FPGAState==TRUE)
{
GetPWM();
}
//FCA();
//FCALimitPWM(void);
FCAGetNav();
FCAPWMOut();
SmoothPWM(40);
SetPWM();
//CollectMedianDataFun(40);
FCACollectPosYaw(5);
FCACollectMedianDataFun(40);
FCFrameProcess();
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
MainLoopRunCounter++;
LED2OFF();
Do=DoNothing;
}
if(Do==DoFactoryLoop)
{
LED1ON();
GpioClrLOW(P1_25);//一律切换到手驾
INS();
FCFrameProcess();
if(DataToDownload==0)
{
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
/*
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
*/
}
else
{
DataDownload();
}
MainLoopRunCounter++;
LED1OFF();
Do=DoNothing;
}
}
return 0;
}
//////////////////////////////////////////////////////////////////////////////////////////
//
// TransducerUpdate - Update the transducer driver
//
// Inputs: None
//
// Outputs: None.
//
// NOTE: Called by timer update, not for public consumption
//
void TransducerUpdate(void) {
//
// Update all subordinate components
//
PWMUpdate();
ACS712Update();
InputsUpdate();
//
// If we're running, use the PWM version of frequency since it's the most accurate.
// Otherwise the PWM is offline so use the less-accurate count to keep us in the
// ballpark.
//
// Note: SG3525 counted output is twice the actual frequency
//
TransducerCurr.Freq = GetPWMFreq();
TransducerCurr.PWM = GetPWM()*2;
TransducerCurr.Current = ACS712GetCurrent();
//
// PWM is % x 10, Current is in Amps x 10, and drive is 12 volts
//
// Multiply everything together and divide by 10 to get power in Watts x 10
//
uint32_t PwrTemp;
PwrTemp = TransducerCurr.Current*TRANSDUCER_DRIVE_VOLTS;
PwrTemp *= TransducerCurr.PWM;
PwrTemp /= 1000;
TransducerCurr.Power = (uint16_t) PwrTemp;
//
// If we're running on timer, decrement and possibly stop
//
if( TransducerCurr.On && TransducerSet.RunMode == RUN_TIMED ) {
if( --TransducerCurr.RunTimer == 0 )
TransducerOn(false);
}
switch(TransducerSet.CtlMode) {
//////////////////////////////////////////////////////////////////////////////////
//
// CTL_CONST_FREQ - Set constant frequency and power
//
case CTL_CONST_FREQ:
// AdjustPower();
break;
//////////////////////////////////////////////////////////////////////////////////
//
// CTL_MAX_EFF - Keep maximum efficiency
//
case CTL_MAX_EFF:
break;
default:
break;
}
}
