// ******** Main Loop *********
void controllercycle() {
double pos_current = (double) ENCODER_ReadPos();
double speed;
double rc_speed_signal = (double) (rcData[SERVO_ESC] - SERVO_CENTER);
/*
* Servo AUX is handled always, independent from the mode
*/
if (rcData[SERVO_AUX] >= SERVO_MIN && rcData[SERVO_AUX] <= SERVO_MAX) {
double diff_aux = (double) (rcData[SERVO_AUX] - SERVO_CENTER);
if (diff_aux < -NEUTRAL_RANGE || diff_aux > +NEUTRAL_RANGE) { // As long as it remains in the neutral zone input values from the serial protocol take precedence, else we overwrite with rc inpput
servo[SERVO_AUX] = rcData[SERVO_AUX];
in_neutral_aux = 0;
} else if (in_neutral_aux == 0) {
servo[SERVO_AUX] = SERVO_CENTER;
in_neutral_aux = 1;
}
}
if (activesettings.mode == MODE_PASSTHROUGH) {
/*
* In passthrough mode we send the input values directly to the output
*/
PWM_SetPWM(SERVO_1_OUT, rcData[SERVO_ESC]);
PWM_SetPWM(SERVO_2_OUT, rcData[SERVO_PITCH]);
PWM_SetPWM(SERVO_3_OUT, rcData[SERVO_YAW]);
// We need that to make sure when enabling the MODE_ABSOLUTE, we start slowly
pos_target = pos_current;
speed = 0;
} else {
/*
* Step 1 is to check the pitch and yaw values.
* There are multiple cases:
* 1. the input is outside the allowed range, e.g. no RC connected or the input mapping is wrong
* 2. the stick was moved from outside neutral into neutral, hence the value should be neutral
* 3. the stick was in neutral all the time and the value was overwritten using the serial protocol - then we should not change anything here
*
* The neutral ranges of the stick are compensated, e.g. the stick might say
* rc=1521us
* center=1500us
* NEUTRAL_RANGE=20us
* ==> output=1501us
*
*/
if (rcData[SERVO_PITCH] >= SERVO_MIN && rcData[SERVO_PITCH] <= SERVO_MAX) { // maybe we never got an input because no receiver is connected
double diff_pitch = (double) (rcData[SERVO_PITCH] - SERVO_CENTER);
if (diff_pitch < -NEUTRAL_RANGE) { // As long as it remains in the neutral zone input values from the serial protocol take precedence, else we overwrite with rc inpput
servo[SERVO_PITCH] = rcData[SERVO_PITCH] + (int16_t)NEUTRAL_RANGE; // Pitch below the neutral zone hence we start to give signal slowly
in_neutral_pitch = 0;
} else if (diff_pitch > +NEUTRAL_RANGE) { // As long as it remains in the neutral zone input values from the serial protocol take precedence, else we overwrite with rc inpput
servo[SERVO_PITCH] = rcData[SERVO_PITCH] - (int16_t)NEUTRAL_RANGE; // Pitch above the neutral zone hence we start to give signal slowly
in_neutral_pitch = 0;
} else if (in_neutral_pitch == 0) {
servo[SERVO_PITCH] = SERVO_CENTER;
in_neutral_pitch = 1;
}
}
if (rcData[SERVO_YAW] >= SERVO_MIN && rcData[SERVO_YAW] <= SERVO_MAX) { // maybe we never got an input because no receiver is connected
double diff_yaw = (double) (rcData[SERVO_YAW] - SERVO_CENTER);
if (diff_yaw < -NEUTRAL_RANGE) { // As long as it remains in the neutral zone input values from the serial protocol take precedence, else we overwrite with rc inpput
servo[SERVO_YAW] = rcData[SERVO_YAW] + (int16_t)NEUTRAL_RANGE; // Yaw below the neutral zone hence we start to give signal slowly
in_neutral_yaw = 0;
} else if (diff_yaw > +NEUTRAL_RANGE) { // As long as it remains in the neutral zone input values from the serial protocol take precedence, else we overwrite with rc inpput
servo[SERVO_YAW] = rcData[SERVO_YAW] - (int16_t)NEUTRAL_RANGE; // Yaw above the neutral zone hence we start to give signal slowly
in_neutral_yaw = 0;
} else if (in_neutral_yaw == 0) {
servo[SERVO_YAW] = SERVO_CENTER;
in_neutral_yaw = 1;
}
}
/*
* ESC speed is similar with the additional requirement that the stick has to be in neutral at start, else we stay in neutral
*/
if (rcData[SERVO_ESC] >= SERVO_MIN && rcData[SERVO_ESC] <= SERVO_MAX) {
if (rc_speed_signal < NEUTRAL_RANGE && rc_speed_signal > -NEUTRAL_RANGE) { // limit the noise -> everything around the zero point is zero as requested speed
rc_speed_signal = 0.0;
if (controller_status == CONTROLLER_INIT) {
controller_status = CONTROLLER_FIND_END_POS;
}
} else if (controller_status == CONTROLLER_INIT ) {
if (is1Hz()) {
printDebug_string("We have a speed signal at start! ", DEBUG_LEVEL_ALWAYS);
printlnDebug_double(rc_speed_signal, DEBUG_LEVEL_ALWAYS);
}
rc_speed_signal = 0.0;
} else if (rc_speed_signal >= NEUTRAL_RANGE) {
rc_speed_signal -= NEUTRAL_RANGE; // speed signal was e.g. 55ms, we consider +-20ms the NEUTRAL_RANGE hence the stick has been 35ms above neutral
} else if (rc_speed_signal <= -NEUTRAL_RANGE) {
rc_speed_signal += NEUTRAL_RANGE;
}
} else {
rc_speed_signal = 0.0;
}
/*
* The speed is the number of sensor signals per cycle. In MODE_ABSOLUTE_POSITION we define the target speed by increasing the target position.
* But in MODE_LIMITER the RC speed signal sets the ESC signal, there is no speed as such. All we know is the current speed by checking the current position.
*/
if (activesettings.mode == MODE_LIMITER || activesettings.mode == MODE_LIMITER_ENDPOINTS) {
speed = pos_current - pos_current_alt;
pos_target = pos_current;
if (activesettings.mode == MODE_LIMITER) {
/*
* In this mode some hacking takes place. Imagine the motor stands still. Hence the measured speed from the sensor is zero and
* the speed signal from the rc stick is zero.
* Now the user cranks the speed signal to the max at once. The ESC starts to accelerate the motor as quickly it can, resulting in a higher
* acceleration than allowed. Therefore the speed signal needs to be reduced but to what value? In the absolute mode the error is constantly
* calculated and the PID loop tries to adjust the speed signal. But in this mode there is no error value.
* Hence two limiters are combined. First the speed signal is not allowed to change abruptly, it is limited to up
* to rs_speed_signal_max_acceleration. This allows the ESC to reach terminal velocity for the current speed signal. Now, if that
* terminal velocity is too high for the acceleration limiter based on the actual speed, the speed signal is kept at that level for another cycle.
* Since there is a relationship between those two acceleration limiters, setting the values needs some care. Best would be a low rc speed signal
* acceleration so that the actual acceleration limiter never kicks in. The more aggressive the speed signal is allowed to be changed, the less precise
* the actual acceleration can be controlled.
*/
// For slow accelerations in this mode, the speed signal has its own acceleration limiter
if (rc_speed_signal > rc_speed_signal_old + activesettings.rs_speed_signal_max_acceleration) {
rc_speed_signal = rc_speed_signal_old + activesettings.rs_speed_signal_max_acceleration;
} else if (rc_speed_signal < rc_speed_signal_old - activesettings.rs_speed_signal_max_acceleration) {
rc_speed_signal = rc_speed_signal_old - activesettings.rs_speed_signal_max_acceleration;
}
// In mode limiter all we can do is keeping the speed signal at the same level as in the previous cycle in case we are too fast
// we have a speed limiter
if (speed > max_speed && rc_speed_signal > rc_speed_signal_old) {
rc_speed_signal = rc_speed_signal_old;
}
if (speed < -max_speed && rc_speed_signal < rc_speed_signal_old) {
rc_speed_signal = rc_speed_signal_old;
}
// and an acceleration limiter
if ((speed - speed_old) > max_acceleration && rc_speed_signal > rc_speed_signal_old) {
rc_speed_signal = rc_speed_signal_old;
} else if ((speed - speed_old) < -max_acceleration && rc_speed_signal < rc_speed_signal_old) {
rc_speed_signal = rc_speed_signal_old;
}
}
/*
* One thing might happen here: You drive directly into the end point but the speed sensor got disconnected. Hence speed remains zero and
* controller believes it is standing still. In this case we do an emergency brake.
*/
if (speed == 0.0 && (rc_speed_signal > 200.0 || rc_speed_signal < -200.0)) {
rc_speed_signal = 0.0;
if (is5s()) {
printlnDebug_string("ERMEGENCY: speed=0 and rc signal is high - there is something severly wrong", DEBUG_LEVEL_VERYHIGH);
}
}
} else if (activesettings.mode == MODE_ABSOLUTE_POSITION) {
speed = rc_speed_signal / 5.0;
// we have a speed limiter
if (speed > max_speed) {
speed = max_speed;
}
if (speed < -max_speed) {
speed = -max_speed;
}
// and an acceleration limiter
if ((speed - speed_old) > max_acceleration) {
speed = speed_old + max_acceleration;
} else if ((speed - speed_old) < -max_acceleration) {
speed = speed_old - max_acceleration;
}
} else {
// Impossible to reach this code but...
speed = 0.0;
pos_target = pos_current;
}
pos_target += speed;
double distance_to_stop = abs_d(speed_old) * (abs_d(speed_old)-max_acceleration) / 2.0f / max_acceleration;
/*
We need to work with the average speed, and that is the motor speed and the new speed at max deceleration
speed_old accel new_speed distance_to_stop old_pos_target new_pos_target old_pos_target+distance_to_stop
100 1 99 4950 0 99 4950
99 1 98 4851 99 197 4950
98 1 97 4753 197 294 4950
97 1 96 4656 294 390 4950
96 1 95 4560 390 485 4950
95 1 94 4465 485 579 4950
94 1 93 4371 579 672 4950
93 1 92 4278 672 764 4950
92 1 91 4186 764 855 4950
91 1 90 4095 855 945 4950
..
..
..
*/
if (pos_end == POS_END_NOT_SET) {
if (rc_speed_signal == 0.0 || speed == 0.0) { // either the stick is in neutral or the cablecam does not move by itself
if (pos_target > 1000.0 || pos_target < -1000.0) {
if (number_of_idle_cycles > 500) {
// initially the end_pos is not set (very high) but if the stick was in neutral pos for 10 seconds and we traveled at least 1000 steps forward, then we take this position as final end pos
setPosStartEnd(pos_target, 0.0);
} else if (is1Hz()) {
printDebug_string("If we stay there 10 seconds we have an end pos: pos=", DEBUG_LEVEL_ALWAYS);
printDebug_double(pos_target, DEBUG_LEVEL_ALWAYS);
printDebug_string(" wait time=", DEBUG_LEVEL_ALWAYS);
printlnDebug_int((int) (number_of_idle_cycles/50), DEBUG_LEVEL_ALWAYS);
}
} else if (pos_target != 0.0) {
if (is1Hz()) {
printDebug_string("This is too close to the start position for being a valid end position: pos=", DEBUG_LEVEL_VERYLOW);
printlnDebug_double(pos_target, DEBUG_LEVEL_VERYLOW);
}
}
number_of_idle_cycles++;
} else {
number_of_idle_cycles = 0;
}
} else {
if (pos_target + distance_to_stop >= pos_end && rc_speed_signal >= 0.0) {
speed = speed_old - max_acceleration;
if (speed < 0.0) speed = 0.0; // do brake but do not drive reverse
rc_speed_signal = 0.0; // in MODE_LIMITER activate the brakes
} else if (pos_target - distance_to_stop <= pos_start && rc_speed_signal <= 0.0) {
speed = speed_old + max_acceleration;
if (speed > 0.0) speed = 0.0;
rc_speed_signal = 0.0; // in MODE_LIMITER activate the brakes
}
if (pos_target > pos_end) {
pos_target = pos_end;
if (rc_speed_signal > 0.0) {
rc_speed_signal = 0.0;
}
} else if (pos_target < pos_start) {
pos_target = pos_start;
if (rc_speed_signal < 0.0) {
rc_speed_signal = 0.0;
}
}
}
if (activesettings.mode == MODE_LIMITER || activesettings.mode == MODE_LIMITER_ENDPOINTS) {
int16_t s = SERVO_CENTER + (int16_t) (rc_speed_signal);
/* if (rc_speed_signal < 0.0f) {
s = (int16_t) (rc_speed_signal - NEUTRAL_RANGE);
} else if (rc_speed_signal > 0.0f) {
s = (int16_t) (rc_speed_signal + NEUTRAL_RANGE);
} else {
s = 0;
}
s += SERVO_CENTER; */
PWM_SetPWM(SERVO_1_OUT, s);
if (is5s() && activesettings.debuglevel >= DEBUG_LEVEL_LOW) {
printDebug_string("target pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_target, DEBUG_LEVEL_LOW);
printDebug_string(" current pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_current, DEBUG_LEVEL_LOW);
printDebug_string(" speed=", DEBUG_LEVEL_LOW);
printDebug_double(speed, DEBUG_LEVEL_LOW);
printDebug_string(" rc_out=", DEBUG_LEVEL_LOW);
printlnDebug_int(s, DEBUG_LEVEL_LOW);
}
} else {
/* The target position calculation does not care about the physics, all it said was "In an ideal world the target pos should be here,
the requested (filtered with max speed and max accel and end point limiters) speed is that, hence the new target pos = target pos + speed."
Example:
target_pos = 0m;
speed = 0m/s
speed as provided by the stick is 400/10 = 40m/s
--> kick in the accel limiter, 40m/s² is way too high, limited accel to 2m/s²
new_speed = 0m/s + 1s*2m/s² = 2m/s
target_pos = target_pos + 1s * 2m/s = 0m + 2m
In the code below is the PID logic to keep the motor following the target position value.
*/
double e, y = 0.0f;
e = pos_target - pos_current; // This is the amount of steps the target pos does not match the reality
// let's not be overly precise. If the error is below a threshold we are good enough
// Not sure that is a good idea though, as it turns the control loop function into an even more unsteady curve around neutral.
if (e >= -activesettings.tolerated_positional_error_limit && e <= activesettings.tolerated_positional_error_limit) {
e = 0.0;
}
y = yalt + Q0*e + Q1*ealt + Q2*ealt2; // PID loop calculation
if (is5s() && activesettings.debuglevel >= DEBUG_LEVEL_VERYLOW) {
printDebug_string("target pos=", DEBUG_LEVEL_VERYLOW);
printDebug_double(pos_target, DEBUG_LEVEL_VERYLOW);
printDebug_string(" current pos=", DEBUG_LEVEL_VERYLOW);
printDebug_double(pos_current, DEBUG_LEVEL_VERYLOW);
printDebug_string(" esc=", DEBUG_LEVEL_VERYLOW);
printDebug_double(y, DEBUG_LEVEL_VERYLOW);
printDebug_string("=", DEBUG_LEVEL_VERYLOW);
printDebug_double(yalt, DEBUG_LEVEL_VERYLOW);
printDebug_string(" + ", DEBUG_LEVEL_VERYLOW);
printDebug_double(Q0, DEBUG_LEVEL_VERYLOW);
printDebug_string("*", DEBUG_LEVEL_VERYLOW);
printDebug_double(e, DEBUG_LEVEL_VERYLOW);
printDebug_string(" + ", DEBUG_LEVEL_VERYLOW);
printDebug_double(Q1, DEBUG_LEVEL_VERYLOW);
printDebug_string("*", DEBUG_LEVEL_VERYLOW);
printDebug_double(ealt, DEBUG_LEVEL_VERYLOW);
printDebug_string(" + ", DEBUG_LEVEL_VERYLOW);
printDebug_double(Q2, DEBUG_LEVEL_VERYLOW);
printDebug_string("*", DEBUG_LEVEL_VERYLOW);
printlnDebug_double(ealt2, DEBUG_LEVEL_VERYLOW);
}
// remove the ESC neutral range
int16_t esc_speed = (int16_t) y;
if (e > activesettings.tolerated_positional_error_exceeded || e < -activesettings.tolerated_positional_error_exceeded) {
// EMERGENCY there is something severely wrong, variable overflow, hall sensor counter, hall sensor direction, anything...
// hence we stop. User can change the mode to passthrough to move the cablecam
if (is5s()) {
printDebug_string("ERMEGENCY: target pos=", DEBUG_LEVEL_VERYHIGH);
printDebug_double(pos_target, DEBUG_LEVEL_VERYHIGH);
printDebug_string(" current pos=", DEBUG_LEVEL_VERYHIGH);
printlnDebug_double(pos_current, DEBUG_LEVEL_VERYHIGH);
}
resetThrottle();
} else {
servo[SERVO_ESC] = esc_neutral_range_center + (esc_speed * activesettings.esc_direction);
if (esc_speed>0) {
servo[SERVO_ESC]+=(esc_neutral_range_forward * activesettings.esc_direction); // a speed request of +1us means +40+1 as the ESC starts at +40us only -> ESC's neutral range
if (is5s() && activesettings.debuglevel >= DEBUG_LEVEL_LOW) {
printDebug_string(" dist=", DEBUG_LEVEL_LOW);
printDebug_double(e, DEBUG_LEVEL_LOW);
printDebug_string("target pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_target, DEBUG_LEVEL_LOW);
printDebug_string("current pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_current, DEBUG_LEVEL_LOW);
printDebug_string(" old pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_current_alt, DEBUG_LEVEL_LOW);
printDebug_string("esc=", DEBUG_LEVEL_LOW);
printlnDebug_int(servo[SERVO_ESC], DEBUG_LEVEL_LOW);
}
} else if (esc_speed<0) {
servo[SERVO_ESC]-= (esc_neutral_range_reverse * activesettings.esc_direction);
if (is5s() && activesettings.debuglevel >= DEBUG_LEVEL_LOW) {
printDebug_string(" dist=", DEBUG_LEVEL_HIGH);
printDebug_double(e, DEBUG_LEVEL_LOW);
printDebug_string("target pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_target, DEBUG_LEVEL_LOW);
printDebug_string("current pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_current, DEBUG_LEVEL_LOW);
printDebug_string(" old pos=", DEBUG_LEVEL_LOW);
printDebug_double(pos_current_alt, DEBUG_LEVEL_LOW);
printDebug_string("esc=", DEBUG_LEVEL_LOW);
printlnDebug_int(servo[SERVO_ESC], DEBUG_LEVEL_LOW);
}
}
if (servo[SERVO_ESC] < SERVO_MIN) servo[SERVO_ESC] = SERVO_MIN;
if (servo[SERVO_ESC] > SERVO_MAX) servo[SERVO_ESC] = SERVO_MAX;
}
PWM_SetPWM(SERVO_1_OUT, servo[SERVO_ESC]);
if (resetthrottle == 1) {
y = 0.0f;
yalt = 0.0f;
e = 0.0f;
ealt = 0.0f;
ealt2 = 0.0f;
resetthrottle = 0;
} else {
ealt2 = ealt;
ealt = e;
yalt = y;
}
}
PWM_SetPWM(SERVO_2_OUT, servo[SERVO_PITCH]);
PWM_SetPWM(SERVO_3_OUT, servo[SERVO_YAW]);
PWM_SetPWM(SERVO_5_OUT, servo[SERVO_AUX]);
}
rc_speed_signal_old = rc_speed_signal;
pos_current_alt = pos_current;
speed_old = speed;
}
int equal(double x, double y) {
return (x - y) < 0.003 * abs_d(y) && (x - y) > -0.003 * abs_d(y);
}
/*
* === FUNCTION ======================================================================
* Name: m2Degree
* Description: AT SMALL SCALE, a approximately(so also fast) conversion from distance
* in meter to degree on latitude circle.
* When wishing to convert to longitude,
* you can fill latitude to longitude parameter,
* with some loss of precision because the earth is taken as sphere
* WARNING: Here I calculate small circle arc, but since every
* greate circle arc connect between the original boundary and
* extended boundary will be greater than it,
* (maybe) there is no corretness problem.
*
* =====================================================================================
*/
double m2Degree ( double m, double lon ) {
return abs_d(m / (R * cos(lon * PI / 180)) * 180 / PI);
} /* ----- end of function m2Degree ----- */