Working on stabilizing a Quad using Arduino Due. I have slightly modified one of the examples of Jeff Rowberg's library to give yaw, pitch and roll angles (zero initialized angles). Moreover, I am using Servo library's writeMicroseconds() to send pulses to bldc's through ESCs. One observation I have made is that there is quite a difference between the starting pulse values of different ESCs. Roughly, the numbers for my 4 bldc's are 1650, 1680, 1720, 1780. So, in case the same pulse duration value of say 1780 is sent to all ESCs, one of them would just start, while the other three would be spinning at decent rpms.
My aim is to implement PID control the speed of motors by measuiring these angles ( there is no remote control involved ). Also, the quad needs to rise up to a certain height setpoint and hover there.
So, the difficulty I am facing is that; my output speed looks something like this (just taking 1 motor here)
speed += ( ± Kp_yaw*yaw_error ± kp_pitch*pitch_error ± Kp_roll*roll_error ±height_speed) //Height speed equals a small value, say, .5 if current height < setpoint height else it's -.5 if current height > set height, otherwise 0, if height has been achieved.
The ± depends upon motor's sense of rotation. Also, currently, I am using only P gain. The P constants for yaw, pitch and roll are different for all the 4 motors. the quad initially struggles for a few seconds to balance itself and rise, but eventually overturns towards the weakest motor (1650 one) each time. What's wrong with this code? Also, speed variable is also different for each motor and initialized to value of 1600 for each motor.