float pid(int x )
{
if (onground)
{
ierror[x] *= 0.8f;
}
int iwindup = 0;
if (( pidoutput[x] == outlimit[x] )&& ( error[x] > 0) )
{
iwindup = 1;
}
if (( pidoutput[x] == -outlimit[x])&& ( error[x] < 0) )
{
iwindup = 1;
}
if ( !iwindup)
{
// midpoint rule instead of rectangular
ierror[x] = ierror[x] + (error[x] + lasterror[x]) * 0.5f * pidki[x] * looptime;
//ierror[x] = ierror[x] + error[x] * pidki[x] * looptime;
}
limitf( &ierror[x] , integrallimit[x] );
// P term
pidoutput[x] = error[x] * pidkp[x] ;
// P2 (direct feedback) term
pidoutput[x] = pidoutput[x] - gyro[x] *pidkp2[x];
// I term
pidoutput[x] += ierror[x];
// D term
pidoutput[x] = pidoutput[x] - (gyro[x] - lastrate[x]) * pidkd[x] * timefactor ;
limitf( &pidoutput[x] , outlimit[x]);
lastrate[x] = gyro[x];
lasterror[x] = error[x];
return pidoutput[x];
}
float rcexpo(float in, float exp)
{
if (exp > 1)
exp = 1;
if (exp < -1)
exp = -1;
float ans = in * in * in * exp + in * (1 - exp);
limitf(&ans, 1.0);
return ans;
}
float pid(int x)
{
if (onground)
{
ierror[x] *= 0.98f; // 50 ms time-constant
}
int iwindup = 0;
if ((pidoutput[x] == outlimit[x]) && (error[x] > 0))
{
iwindup = 1;
}
if ((pidoutput[x] == -outlimit[x]) && (error[x] < 0))
{
iwindup = 1;
}
if (!iwindup)
{
#ifdef MIDPOINT_RULE_INTEGRAL
// trapezoidal rule instead of rectangular
ierror[x] = ierror[x] + (error[x] + lasterror[x]) * 0.5f * pidki[x] * looptime;
lasterror[x] = error[x];
#endif
#ifdef RECTANGULAR_RULE_INTEGRAL
ierror[x] = ierror[x] + error[x] * pidki[x] * looptime;
lasterror[x] = error[x];
#endif
#ifdef SIMPSON_RULE_INTEGRAL
// assuming similar time intervals
ierror[x] = ierror[x] + 0.166666f * (lasterror2[x] + 4 * lasterror[x] + error[x]) * pidki[x] * looptime;
lasterror2[x] = lasterror[x];
lasterror[x] = error[x];
#endif
}
limitf(&ierror[x], integrallimit[x]);
// P term
pidoutput[x] = error[x] * pidkp[x];
// I term
pidoutput[x] += ierror[x];
// D term
#ifdef NORMAL_DTERM
pidoutput[x] = pidoutput[x] - (gyro[x] - lastrate[x]) * pidkd[x] * timefactor;
lastrate[x] = gyro[x];
#endif
#ifdef SECOND_ORDER_DTERM
pidoutput[x] = pidoutput[x] - (-(0.083333333f) * gyro[x] + (0.666666f) * lastratexx[x][0] - (0.666666f) * lastratexx[x][2] + (0.083333333f) * lastratexx[x][3]) * pidkd[x] * timefactor;
lastratexx[x][3] = lastratexx[x][2];
lastratexx[x][2] = lastratexx[x][1];
lastratexx[x][1] = lastratexx[x][0];
lastratexx[x][0] = gyro[x];
#endif
#ifdef NEW_DTERM
pidoutput[x] = pidoutput[x] - ((0.5f) * gyro[x] - (0.5f) * lastratexx[x][1]) * pidkd[x] * timefactor;
lastratexx[x][1] = lastratexx[x][0];
lastratexx[x][0] = gyro[x];
#endif
#ifdef PID_VOLTAGE_COMPENSATION
pidoutput[x] *= v_compensation;
#endif
limitf(&pidoutput[x], outlimit[x]);
return pidoutput[x];
}
void control(void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
float maxangle;
float anglerate;
#ifndef THREE_D_THROTTLE
if ( aux[INVERTEDMODE] )
{
bridge_sequencer(REVERSE); // reverse
}
else
{
bridge_sequencer(FORWARD); // forward
}
#else
#endif
// pwmdir controls hardware directly so we make a copy here
currentdir = pwmdir;
if (aux[RATES])
{
ratemulti = HIRATEMULTI;
ratemultiyaw = HIRATEMULTIYAW;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
else
{
ratemulti = 1.0f;
ratemultiyaw = 1.0f;
maxangle = MAX_ANGLE_LO;
anglerate = LEVEL_MAX_RATE_LO;
}
for (int i = 0; i < 3; i++)
{
rxcopy[i] = rx[i];
}
if (currentdir == REVERSE)
{
#ifndef NATIVE_INVERTED_MODE
// invert pitch in reverse mode
//rxtemp[ROLL] = - rx[ROLL];
rxcopy[PITCH] = - rx[PITCH];
rxcopy[YAW] = - rx[YAW];
#endif
}
if (auxchange[HEADLESSMODE])
{
yawangle = 0;
}
if ((aux[HEADLESSMODE]) )
{
yawangle = yawangle + gyro[2] * looptime;
while (yawangle < -3.14159265f)
yawangle += 6.28318531f;
while (yawangle > 3.14159265f)
yawangle -= 6.28318531f;
float temp = rxcopy[0];
rxcopy[0] = rxcopy[0] * fastcos( yawangle) - rxcopy[1] * fastsin(yawangle );
rxcopy[1] = rxcopy[1] * fastcos( yawangle) + temp * fastsin(yawangle ) ;
}
else
{
yawangle = 0;
}
// check for acc calibration
int command = gestures2();
if (command)
{
if (command == 3)
{
gyro_cal(); // for flashing lights
acc_cal();
savecal();
// reset loop time
extern unsigned lastlooptime;
lastlooptime = gettime();
}
else
{
ledcommand = 1;
if (command == 2)
{
aux[CH_AUX1] = 1;
}
if (command == 1)
{
aux[CH_AUX1] = 0;
}
if (command == 4)
{
aux[CH_AUX2] = !aux[CH_AUX2];
}
}
}
imu_calc();
pid_precalc();
float attitudecopy[2];
if (currentdir == REVERSE)
{
// account for 180 deg wrap since inverted attitude is near 180
if ( attitude[0] > 0) attitudecopy[0] = attitude[0] - 180;
else attitudecopy[0] = attitude[0] + 180;
if ( attitude[1] > 0) attitudecopy[1] = attitude[1] - 180;
else attitudecopy[1] = attitude[1] + 180;
}
else
{
// normal thrust mode
attitudecopy[0] = attitude[0];
attitudecopy[1] = attitude[1];
}
if (aux[LEVELMODE])
{ // level mode
angleerror[0] = rxcopy[0] * maxangle - attitudecopy[0];
angleerror[1] = rxcopy[1] * maxangle - attitudecopy[1];
error[0] = apid(0) * anglerate * DEGTORAD - gyro[0];
error[1] = apid(1) * anglerate * DEGTORAD - gyro[1];
}
else
{ // rate mode
error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];
// reduce angle Iterm towards zero
extern float aierror[3];
aierror[0] = 0.0f;
aierror[1] = 0.0f;
}
error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
pid(0);
pid(1);
pid(2);
#ifndef THREE_D_THROTTLE
// map throttle so under 10% it is zero
float throttle = mapf(rx[3], 0, 1, -0.1, 1);
if (throttle < 0)
throttle = 0;
if (throttle > 1.0f)
throttle = 1.0f;
#endif
#ifdef THREE_D_THROTTLE
// this changes throttle so under center motor direction is reversed
// map throttle with zero center
float throttle = mapf(rx[3], 0, 1, -1, 1);
limitf(&throttle, 1.0);
if ( throttle > 0 )
{
bridge_sequencer(FORWARD); // forward
}else
{
bridge_sequencer(REVERSE); // reverse
}
if ( !throttlesafe_3d )
{
if (throttle > 0)
{
throttlesafe_3d = 1;
ledcommand = 1;
}
throttle = 0;
}
throttle = fabsf(throttle);
throttle = mapf (throttle , THREE_D_THROTTLE_DEADZONE , 1, 0 , 1);
if ( failsafe ) throttle = 0;
#endif // end 3d throttle remap
// turn motors off if throttle is off and pitch / roll sticks are centered
if (failsafe || (throttle < 0.001f && ( !ENABLESTIX || !onground_long || aux[LEVELMODE] || (fabsf(rx[0]) < (float) ENABLESTIX_TRESHOLD && fabsf(rx[1]) < (float) ENABLESTIX_TRESHOLD)))) { // motors off
onground = 1;
if ( onground_long )
{
if ( gettime() - onground_long > 1000000)
{
onground_long = 0;
}
}
#ifdef MOTOR_BEEPS
extern void motorbeep( void);
motorbeep();
#endif
thrsum = 0;
for (int i = 0; i <= 3; i++)
{
pwm_set(i, 0);
}
// reset the overthrottle filter
lpf(&overthrottlefilt, 0.0f, 0.72f); // 50hz 1khz sample rate
#ifdef MOTOR_FILTER
// reset the motor filter
for (int i = 0; i <= 3; i++)
{
motorfilter(0, i);
}
#endif
#ifdef THROTTLE_TRANSIENT_COMPENSATION
// reset hpf filter;
throttlehpf(0);
#endif
#ifdef STOCK_TX_AUTOCENTER
for( int i = 0 ; i <3;i++)
{
if ( rx[i] == lastrx[i] )
{
consecutive[i]++;
}
else consecutive[i] = 0;
lastrx[i] = rx[i];
if ( consecutive[i] > 1000 && fabsf( rx[i]) < 0.1f )
{
autocenter[i] = rx[i];
}
}
#endif
// end motors off / failsafe / onground
}
else
{
// motors on - normal flight
#ifdef THROTTLE_TRANSIENT_COMPENSATION
throttle += 7.0f * throttlehpf(throttle);
if (throttle < 0)
throttle = 0;
if (throttle > 1.0f)
throttle = 1.0f;
#endif
// throttle angle compensation
#ifdef AUTO_THROTTLE
if (aux[LEVELMODE])
{
float autothrottle = fastcos(attitude[0] * DEGTORAD) * fastcos(attitude[1] * DEGTORAD);
float old_throttle = throttle;
if (autothrottle <= 0.5f)
autothrottle = 0.5f;
throttle = throttle / autothrottle;
// limit to 90%
if (old_throttle < 0.9f)
if (throttle > 0.9f)
throttle = 0.9f;
if (throttle > 1.0f)
throttle = 1.0f;
}
#endif
#ifdef LVC_PREVENT_RESET
extern float vbatt;
if (vbatt < (float) LVC_PREVENT_RESET_VOLTAGE) throttle = 0;
#endif
onground = 0;
onground_long = gettime();
float mix[4];
if ( stage == BRIDGE_WAIT ) onground = 1;
if (currentdir == REVERSE)
{
// inverted flight
//pidoutput[ROLL] = -pidoutput[ROLL];
pidoutput[PITCH] = -pidoutput[PITCH];
//pidoutput[YAW] = -pidoutput[YAW];
}
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];
#endif
mix[MOTOR_FR] = throttle - pidoutput[0] - pidoutput[1] + pidoutput[2]; // FR
mix[MOTOR_FL] = throttle + pidoutput[0] - pidoutput[1] - pidoutput[2]; // FL
mix[MOTOR_BR] = throttle - pidoutput[0] + pidoutput[1] - pidoutput[2]; // BR
mix[MOTOR_BL] = throttle + pidoutput[0] + pidoutput[1] + pidoutput[2]; // BL
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];
#endif
// we invert again cause it's used by the pid internally (for limit)
if (currentdir == REVERSE)
{
// inverted flight
//pidoutput[ROLL] = -pidoutput[ROLL];
pidoutput[PITCH] = -pidoutput[PITCH];
//pidoutput[YAW] = -pidoutput[YAW];
}
#ifdef MIX_LOWER_THROTTLE
// limit reduction to this amount ( 0.0 - 1.0)
// 0.0 = no action
// 0.5 = reduce up to 1/2 throttle
//1.0 = reduce all the way to zero
#define MIX_THROTTLE_REDUCTION_MAX 0.5
float overthrottle = 0;
for (int i = 0; i <= 3; i++)
{
if (mix[i] > overthrottle)
overthrottle = mix[i];
}
overthrottle -= 1.0f;
if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;
#ifdef MIX_THROTTLE_FILTER_LPF
if (overthrottle > overthrottlefilt)
lpf(&overthrottlefilt, overthrottle, 0.82); // 20hz 1khz sample rate
else
lpf(&overthrottlefilt, overthrottle, 0.72); // 50hz 1khz sample rate
#else
if (overthrottle > overthrottlefilt)
overthrottlefilt += 0.005f;
else
overthrottlefilt -= 0.01f;
#endif
if (overthrottlefilt > 0.5f)
overthrottlefilt = 0.5;
if (overthrottlefilt < -0.1f)
overthrottlefilt = -0.1;
overthrottle = overthrottlefilt;
if (overthrottle < 0.0f)
overthrottle = 0.0f;
if (overthrottle > 0)
{ // exceeding max motor thrust
// prevent too much throttle reduction
if (overthrottle > (float)MIX_THROTTLE_REDUCTION_MAX)
overthrottle = (float)MIX_THROTTLE_REDUCTION_MAX;
// reduce by a percentage only, so we get an inbetween performance
overthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
for (int i = 0; i <= 3; i++)
{
mix[i] -= overthrottle;
}
}
#endif
#ifdef MOTOR_FILTER
for (int i = 0; i <= 3; i++)
{
mix[i] = motorfilter(mix[i], i);
}
#endif
#ifdef CLIP_FF
float clip_ff(float motorin, int number);
for (int i = 0; i <= 3; i++)
{
mix[i] = clip_ff(mix[i], i);
}
#endif
for (int i = 0; i < 4; i++)
{
float test = motormap(mix[i]);
#ifdef MOTORS_TO_THROTTLE
test = throttle;
// flash leds in valid throttle range
ledcommand = 1;
// for battery estimation
mix[i] = throttle;
#warning "MOTORS TEST MODE"
#endif
#ifdef MOTOR_MIN_ENABLE
if (test < (float) MOTOR_MIN_VALUE)
{
test = (float) MOTOR_MIN_VALUE;
}
#endif
#ifdef MOTOR_MAX_ENABLE
if (test > (float) MOTOR_MAX_VALUE)
{
test = (float) MOTOR_MAX_VALUE;
}
#endif
#ifndef NOMOTORS
//normal mode
pwm_set( i , test );
#else
#warning "NO MOTORS"
#endif
}
thrsum = 0;
for (int i = 0; i <= 3; i++)
{
if (mix[i] < 0)
mix[i] = 0;
if (mix[i] > 1)
mix[i] = 1;
thrsum += mix[i];
}
thrsum = thrsum / 4;
} // end motors on
}
/** Set the robot pose in the global space,
* where all the values are in the range (-1.0, 1.0)
* @param forward
* the forward percentage
* @param left
* the left percentage
* @param raise
* the raise percentage
* @param grab
* the grab percentage
*/
static void set_robot(double forward, double left, double raise, double grab) {
agent_base.y = limitf(forward, -1.0, 1.0);
agent_base.yaw = limitf(left, -1.0, 1.0);
agent_arm.pitch = limitf(raise, -1.0, 1.0);
agent_arm.yaw = limitf(grab, -1.0, 1.0);
}