static void _OnBrtChanged(WM_MESSAGE * pMsg,int val)
{
if(agentConf.Brt != val)
{
agentConf.Brt = val;
PWM_SET(val);
}
}
void bldc_set_pwm(uint8_t val) {
unsigned char pwm_tmp;
pwm_tmp = bldc_pwm = val;
if (pwm_tmp > bldc_pwm_max) {
pwm_tmp = bldc_pwm_max;
LED_RED_ON();
}
PWM_SET(pwm_tmp);
}
void stkEvaluateRxMessage(void) /* not static to prevent inlining */
{
_u8 i, cmd;
utilWord_t len = {2}; /* defaults to cmd + error code */
void *param;
//
LED_RX_ON();
/*
NOTICE: Please do NOT forget to set the following values
txBuffer[STK_TXMSG_START] : cmd
&txBuffer[STK_TXMSG_START+1] : payload data
len.word : sizeof(cmd) + sizeof(payload data)
len.byte[0] : len.word & 0xFF
*/
cmd = rxBuffer[STK_TXMSG_START];
param = &rxBuffer[STK_TXMSG_START + 1];
// The default return value
txBuffer[STK_TXMSG_START] = cmd;
txBuffer[STK_TXMSG_START + 1] = STK_STATUS_CMD_OK;
// reduce the code size by removing the switch-case jump table
if (cmd == RPINFRA_STK_CMD_ECHO){
// simple echo back
}
else if (cmd == RPINFRA_STK_CMD_GET_ADC){
*(_u16 *)&txBuffer[STK_TXMSG_START+1] = analogRead(A_ADC0);
len.bytes[0] = sizeof(unsigned long) + 1;
}else if (cmd == RPINFRA_STK_CMD_SET_PWM){
PWM_ENABLE( IO_PWM0 );
PWM_SET( IO_PWM0, rxBuffer[STK_TXMSG_START + 1] );
}else if (cmd == SCANNER_CMD_SET_HEADING) {
// set heading
onSetHeading(*(_u16 *)&rxBuffer[STK_TXMSG_START + 1]);
}else if (cmd == SCANNER_CMD_ENABLE_LASER) {
onEnableLaser();
}else if (cmd == SCANNER_CMD_DISABLE_LASER) {
onDisableLaser();
}
LED_RX_OFF();
//
stkSetTxMessage(len.word);
}
__interrupt void PID_CONTROL()
{
TA1CCTL1 &=~ CCIFG;
switch(CpuTimePoll)
{
//第一个3ms给控制 每6ms控制一次
case 0 :
ANGLE_CALCULATE(); //四元数转欧拉角 这个执行了1.2MS左右
PID_ROLL(); //间接计算PID占空比 PID时间 不长
PID_PITCH();
PID_YALL();
PWM_CALCULATE(); //PID输出值转换为四路电机的PWM占空比
PWM_SET(); //PWM占空比设置
//以上执行2.2ms
break;
//第二给3ms给接收控制信息 发送周期为15ms其中包括一个3ms的接收
case 1 :
if(time1 % 4 == 0)
{
Data_Send_PID1();
}
else if(time1 % 4 == 1)
{
Data_Send_MotoPWM(); //设置电机
}
else if(time1 % 4 == 2)
{
Data_Send_Status();
}
else if(time1 % 4 == 3)
{
Data_Send_Senser();
}
if(time1++ == 3)
time1 = 0;
break;
}
if(CpuTimePoll++ == 1)
CpuTimePoll = 0;
}
/**
* Autamatically called by @ref gprot_register_changed()
*
* Implements setting PWM power in the PWM subsystem
*/
void gprot_update_pwm_power(void)
{
PWM_SET(gprot_pwm_power);
}