I've been writing some quad copter software and I am not sure what the best way is to map the throttle and PID inputs to ESC power.
My throttle range is 0-1 and my PID outputs are 0-1. My ESC's have a range of 1060us to 1860us.
I have mapped the motor speeds like this:
_motorPower[0] = map((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
_motorPower[1] = map((_rcConstrainedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
_motorPower[2] = map((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
_motorPower[3] = map((_rcConstrainedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 4.0, 1060, 1860);
This works but if my quad is perfectly level (i.e. the PID outputs are 0) and I apply full throttle (1.0) then map this to ESC power I will only get quarter power (1260us).
How should I be doing this so that if my throttle is on max then I get max power? If my throttle is half (0.5) then I should get half power plus the PID values etc.
Can anyone help me with this?
Thanks Joe