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); }