Problem with quadcopter

Archive
1

hi guys, i have a big problem with this code, i dont’know why the motor go to the max power and only two motors rotate.

[code]while (ins.num_samples_available() == 0);

ins.update();
float roll = 0, pitch = 0, yaw = 0;
ins.quaternion.to_euler(&roll, &pitch, &yaw);
roll = ToDeg(roll);
pitch = ToDeg(pitch);
yaw = ToDeg(yaw);

Vector3f gyro = ins.get_gyro();
float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);

/* if(!hal.console->available())
{
hal.console->printf_P(
PSTR(“P:%4.1f R:%4.1f Y:%4.1f\n”),
pitch,
roll,
yaw);
}*/

if(command.throttle > UDP_THR_MIN + 5){
float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)command.pitch - pitch, 1), -250, 250);
float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)command.roll - roll, 1), -250, 250);
float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);

if(abs(command.yaw) > 5) {
    yaw_stab_output = command.yaw;
    yaw_target = yaw;   // remember this yaw for when pilot stops
}

long pitch_output =  (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);  
long roll_output =  (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);  
long yaw_output =  (long) constrain(pids[PID_ROLL_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);


int c1 = command.throttle + roll_output + pitch_output + yaw_output;
int c2 = command.throttle + roll_output - pitch_output + yaw_output;
int c3 = command.throttle - roll_output + pitch_output + yaw_output;
int c4 = command.throttle - roll_output - pitch_output - yaw_output;

  
if(!hal.console->available() && (counter++ % 50 == 0))
{
  hal.console->printf_P(
  PSTR("P:%4.1f  R:%4.1f Y:%4.1f\n"),
		  pitch,
		  roll,
		  yaw);
  hal.console->printf_P(PSTR("C1: %d  C2: %d  C3: %d C4: %d\n"),c1,c2,c3,c4); 
  hal.console->printf_P(PSTR("P: %d  T: %d  Y: %d\n"),pitch_output,roll_output,yaw_output); 
 //hal.console->printf_P(PSTR("FL: %d  BL: %d  FR: %d  BR: %d\n"),command.throttle + roll_output + pitch_output - yaw_output,command.throttle + roll_output - pitch_output + yaw_output,command.throttle - roll_output - pitch_output - yaw_output); 
}

hal.rcout->write(MOTOR_FL, (c1 > 1150) ? 1150: c1);
hal.rcout->write(MOTOR_BL, (c2 > 1150) ? 1150 : c2);
hal.rcout->write(MOTOR_FR, (c3 > 1150) ? 1150 : c3);
hal.rcout->write(MOTOR_BR, (c4 > 1150) ? 1150 : c4);

/* if(!hal.console->available())
{
hal.console->printf_P(PSTR(“T2: %d\n”),command.throttle);
// hal.console->printf_P(PSTR(“P: %d T: %d Y: %d\n”),pitch_output,roll_output,yaw_output);
//hal.console->printf_P(PSTR(“FL: %d BL: %d FR: %d BR: %d\n”),command.throttle + roll_output + pitch_output - yaw_output,command.throttle + roll_output - pitch_output + yaw_output,command.throttle - roll_output - pitch_output - yaw_output);
}*/

}else{
hal.rcout->write(MOTOR_FL, 1000);
hal.rcout->write(MOTOR_BL, 1000);
hal.rcout->write(MOTOR_FR, 1000);
hal.rcout->write(MOTOR_BR, 1000);

// reset yaw target so we maintain this on takeoff
yaw_target = yaw;

// reset PID integrals whilst on the ground
for(int i=0; i<6; i++)
  pids[i].reset_I();

}[/code]

Thanks in advance