void ANO_Motor::up(uint16_t throttle, int32_t pidTermRoll, int32_t pidTermPitch, int32_t pidTermYaw)
{
motorPWM[0] = throttle; //后右
motorPWM[1] = throttle; //前右
motorPWM[2] = throttle; //后左
motorPWM[3] = throttle; //前左
motorPWM[4] = throttle; //右
motorPWM[5] = throttle; //左
int16_t maxMotor = motorPWM[0];
for (u8 i = 1; i < MAXMOTORS; i++)
{
if (motorPWM[i] > maxMotor)
maxMotor = motorPWM[i];
}
for (u8 i = 0; i < MAXMOTORS; i++)
{
if (maxMotor > MAXTHROTTLE)
motorPWM[i] -= maxMotor - MAXTHROTTLE;
//限制电机PWM的最小和最大值
motorPWM[i] = constrain_uint16(motorPWM[i], throttle, throttle);
}
//如果未解锁，则将电机输出设置为最低
if(!ano.f.ARMED)
ResetPWM();
if(!ano.f.ALTHOLD && rc.rawData[THROTTLE] < 1200)
ResetPWM();
//写入电机PWM
pwm.SetPwm(motorPWM);
}
void ANO_RC::Cal_Command(void)
{
int32_t tmp, tmp2;
for (u8 i = 0; i < 3; i++)
{ //处理ROLL,PITCH,YAW这三个轴的数据
tmp = min(abs(rawData[i] - RC_MID), 500);
if (i != 2)
{ // ROLL & PITCH
tmp2 = tmp / 100;
Command[i] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
}
else
{ // YAW
Command[i] = -tmp;
}
if (rawData[i] < RC_MID)
Command[i] = -Command[i];
}
tmp = constrain_uint16(rawData[THROTTLE], RC_MINCHECK, 2000);
tmp = (uint32_t) (tmp - RC_MINCHECK) * 1000 / (2000 - RC_MINCHECK); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
if(!ano.f.ALTHOLD){
Command[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
Command[THROTTLE] = constrain_uint16(rawData[THROTTLE], RC_MINCHECK, 2000);
}
//-------------------航向锁定------------------
if (abs(Command[YAW]) < 70 && rawData[THROTTLE] > RC_MINCHECK)
{
int16_t dif = imu.angle.z - imu.magHold;
if (dif <= -180)
dif += 360;
if (dif >= +180)
dif -= 360;
dif = -dif;
Command[YAW] -= dif * fc.pid[PIDMAG].kP;
}
else
imu.magHold = imu.angle.z;
}
void ANO_Motor::balance(uint16_t throttle, int32_t pidTermRoll, int32_t pidTermPitch, int32_t pidTermYaw)
{
motorPWM[0] = 0; //后右
motorPWM[1] = throttle; //前右
motorPWM[2] = 0; //后左
motorPWM[3] = throttle; //前左
motorPWM[4] = 0; //右
motorPWM[5] = 0; //左
int16_t maxMotor = motorPWM[0];
for (u8 i = 1; i < MAXMOTORS; i++)
{
if (motorPWM[i] > maxMotor)
maxMotor = motorPWM[i];
}
for (u8 i = 0; i < MAXMOTORS; i++)
{
if (maxMotor > MAXTHROTTLE)
motorPWM[i] -= maxMotor - MAXTHROTTLE;
//限制电机PWM的最小和最大值
//motorPWM[i] = constrain_uint16(motorPWM[i], MINTHROTTLE, MAXTHROTTLE);
if(i == 1 || i == 3)
motorPWM[i] = constrain_uint16(motorPWM[i], faster, faster); //前面加速
else if(i == 0 || i == 2)
motorPWM[i] = constrain_uint16(motorPWM[i], slower, slower); //后面减速
else
motorPWM[i] = constrain_uint16(motorPWM[i], faster, faster); //保持原速
}
//如果未解锁，则将电机输出设置为最低
if(!ano.f.ARMED)
ResetPWM();
if(!ano.f.ALTHOLD && rc.rawData[THROTTLE] < 1200)
ResetPWM();
//写入电机PWM
pwm.SetPwm(motorPWM);
}
void ANO_Motor::writeMotor(uint16_t throttle, int32_t pidTermRoll, int32_t pidTermPitch, int32_t pidTermYaw)
{
//六轴X型
motorPWM[0] = throttle - pidTermRoll + pidTermPitch - pidTermYaw; //后右
motorPWM[1] = throttle - pidTermRoll - pidTermPitch + pidTermYaw; //前右
motorPWM[2] = throttle + pidTermRoll + pidTermPitch + pidTermYaw; //后左
motorPWM[3] = throttle + pidTermRoll - pidTermPitch - pidTermYaw; //前左
for (u8 i = 0; i < 4; i++)
motorPWM[i] = constrain_uint16(motorPWM[i], MINTHROTTLE, MAXTHROTTLE);
//如果未解锁，则将电机输出设置为最低
if(!ano.f.ARMED || rc.rawData[THROTTLE] < 1200)
for(u8 i=0; i< 4 ; i++)
motorPWM[i] = 1000;
//写入电机PWM
motors_set_pwm(motorPWM);
}
void pidctrl::Motors_Ctrl(uint16_t throttle, int32_t pidTermRoll, int32_t pidTermPitch, int32_t pidTermYaw)
{
uint8_t i = 0;
//六轴X型
motorPWM[0] = throttle - pidTermRoll - pidTermPitch + pidTermYaw; //后右
motorPWM[1] = throttle + pidTermRoll - pidTermPitch - pidTermYaw; //前右
motorPWM[2] = throttle + pidTermRoll + pidTermPitch + pidTermYaw; //后左
motorPWM[3] = throttle - pidTermRoll + pidTermPitch - pidTermYaw; //前左
for (i = 0; i < MOTORS_NUM_MAX; i++)
motorPWM[i] = constrain_uint16(motorPWM[i], THROTTLE_MIN, THROTTLE_MAX);
//如果未解锁，则将电机输出设置为最低
if(!config.f.ARMED || rc.rawData[THROTTLE] < 1200)
for(i=0; i< MOTORS_NUM_MAX ; i++)
motorPWM[i] = 1000;
//写入电机PWM
rt_device_write(motor_dev,0,motorPWM,MOTORS_NUM_MAX);
}
void ANO_Motor::writeMotor(uint16_t throttle, int32_t pidTermRoll, int32_t pidTermPitch, int32_t pidTermYaw)
{
//六轴X型
motorPWM[0] = throttle - 0.5 * pidTermRoll + 0.866 * pidTermPitch + pidTermYaw; //后右
motorPWM[1] = throttle - 0.5 * pidTermRoll - 0.866 * pidTermPitch + pidTermYaw; //前右
motorPWM[2] = throttle + 0.5 * pidTermRoll + 0.866 * pidTermPitch - pidTermYaw; //后左
motorPWM[3] = throttle + 0.5 * pidTermRoll - 0.866 * pidTermPitch - pidTermYaw; //前左
motorPWM[4] = throttle - pidTermRoll - pidTermYaw; //右
motorPWM[5] = throttle + pidTermRoll + pidTermYaw; //左
int16_t maxMotor = motorPWM[0];
for (u8 i = 1; i < MAXMOTORS; i++)
{
if (motorPWM[i] > maxMotor)
maxMotor = motorPWM[i];
}
for (u8 i = 0; i < MAXMOTORS; i++)
{
if (maxMotor > MAXTHROTTLE)
motorPWM[i] -= maxMotor - MAXTHROTTLE;
//限制电机PWM的最小和最大值
motorPWM[i] = constrain_uint16(motorPWM[i], MINTHROTTLE, MAXTHROTTLE);
}
//如果未解锁，则将电机输出设置为最低
if(!ano.f.ARMED)
ResetPWM();
//if(!ano.f.ALTHOLD && rc.rawData[THROTTLE] < 1200)
//S ResetPWM();
if(!ano.f.FLAG && rc.rawData[THROTTLE] < 1200)
ResetPWM();
//写入电机PWM
pwm.SetPwm(motorPWM);
}
