void control(void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
float maxangle;
float anglerate;
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++)
{
#ifdef STOCK_TX_AUTOCENTER
rxcopy[i] = rx[i] - autocenter[i];
#else
rxcopy[i] = rx[i];
#endif
}
flip_sequencer();
if ( (!aux[STARTFLIP])&&auxchange[STARTFLIP] )
{
start_flip();
auxchange[STARTFLIP]= 0;
}
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 ( controls_override)
{
for ( int i = 0 ; i < 3 ; i++)
{
rxcopy[i] = rx_override[i];
}
ratemulti = 1.0f;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
imu_calc();
pid_precalc();
if ((aux[LEVELMODE]||level_override)&&!acro_override)
{ // level mode
angleerror[0] = rxcopy[0] * maxangle - attitude[0] + (float) TRIM_ROLL;
angleerror[1] = rxcopy[1] * maxangle - attitude[1] + (float) TRIM_PITCH;
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];
for (int i = 0; i <= 2; i++)
aierror[i] *= 0.8f;
}
error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
pid(0);
pid(1);
pid(2);
// 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;
// turn motors off if throttle is off and pitch / roll sticks are centered
if (failsafe || (throttle < 0.001f && ( !ENABLESTIX || !onground_long || aux[LEVELMODE] || level_override || (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
}
else
{
// motors on - normal flight
onground_long = gettime();
#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;
float mix[4];
if ( controls_override)
{// change throttle in flip mode
throttle = rx_override[3];
}
#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
#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 < 4; 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 < 4; i++)
{
mix[i] -= overthrottle;
}
}
#endif
#ifdef MOTOR_FILTER
for (int i = 0; i < 4; i++)
{
mix[i] = motorfilter(mix[i], i);
}
#endif
#ifdef CLIP_FF
float clip_ff(float motorin, int number);
for (int i = 0; i < 4; 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 < 4; i++)
{
if (mix[i] < 0)
mix[i] = 0;
if (mix[i] > 1)
mix[i] = 1;
thrsum += mix[i];
}
thrsum = thrsum / 4;
} // end motors on
}
void control( void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
float maxangle;
float anglerate;
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 ( auxchange[HEADLESSMODE] )
{
yawangle = 0;
}
if ( (aux[HEADLESSMODE] )&&!aux[LEVELMODE] )
{
yawangle = yawangle + gyro[2]*looptime;
float temp = rxcopy[0];
rxcopy[0] = rxcopy[0] * cosf( yawangle) - rxcopy[1] * sinf(yawangle );
rxcopy[1] = rxcopy[1] * cosf( yawangle) + temp * sinf(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;
}
}
}
imu_calc();
pid_precalc();
if ( aux[LEVELMODE] )
{ // level mode
angleerror[0] = rxcopy[0] * maxangle - attitude[0];
angleerror[1] = rxcopy[1] * maxangle - attitude[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];
for ( int i = 0 ; i <= 2 ; i++) aierror[i] *= 0.8f;
}
error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
pid(0);
pid(1);
pid(2);
// 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;
// turn motors off if throttle is off and pitch / roll sticks are centered
if ( failsafe || (throttle < 0.001f && (!ENABLESTIX|| (fabs(rx[0]) < 0.5f && fabs(rx[1]) < 0.5f ) ) ) )
{ // motors off
onground = 1;
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
}
else
{
#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]||AUTO_THROTTLE_ACRO_MODE )
{
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
onground = 0;
float mix[4];
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 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 <= 3 ; i++)
{
float test = motormap( mix[i] );
#ifndef NOMOTORS
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
}
void control(void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
float maxangle;
float anglerate;
#ifdef TOGGLE_IN
if ( auxchange[TOGGLE_IN] && !aux[TOGGLE_IN] )
{
ledcommand = 1;
aux[TOGGLE_OUT]=!aux[TOGGLE_OUT];
}
#endif
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++)
{
#ifdef STOCK_TX_AUTOCENTER
rxcopy[i] = rx[i] - autocenter[i];
#else
rxcopy[i] = rx[i];
#endif
}
flip_sequencer();
if ( (!aux[STARTFLIP])&&auxchange[STARTFLIP] )
{
start_flip();
auxchange[STARTFLIP]= 0;
}
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 ( controls_override)
{
for ( int i = 0 ; i < 3 ; i++)
{
rxcopy[i] = rx_override[i];
}
ratemulti = 1.0f;
maxangle = MAX_ANGLE_HI;
anglerate = LEVEL_MAX_RATE_HI;
}
pid_precalc();
if ((aux[LEVELMODE]||level_override)&&!acro_override)
{ // level mode
extern void stick_vector( float );
extern float errorvect[];
stick_vector( maxangle);
float yawerror[3];
extern float GEstG[3];
float yawrate = rxcopy[2] * (float) MAX_RATEYAW * DEGTORAD * ratemultiyaw* ( 1/ 2048.0f);
yawerror[0] = GEstG[1] * yawrate;
yawerror[1] = - GEstG[0] * yawrate;
yawerror[2] = GEstG[2] * yawrate;
angleerror[0] = errorvect[0] * RADTODEG *1.1f;
angleerror[1] = errorvect[1] * RADTODEG *1.1f;
for ( int i = 0 ; i <2; i++)
{
error[i] = apid(i) * anglerate * DEGTORAD + yawerror[i] - gyro[i];
}
error[2] = yawerror[2] - gyro[2];
}
else
{ // rate mode
error[0] = rxcopy[0] * MAX_RATE * DEGTORAD * ratemulti - gyro[0];
error[1] = rxcopy[1] * MAX_RATE * DEGTORAD * ratemulti - gyro[1];
error[2] = rxcopy[2] * MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[2];
// reduce angle Iterm towards zero
extern float aierror[3];
aierror[0] = 0.0f;
aierror[1] = 0.0f;
}
pid(0);
pid(1);
pid(2);
// 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;
#ifdef AIRMODE_HOLD_SWITCH
if (failsafe || aux[AIRMODE_HOLD_SWITCH] || throttle < 0.001f && !onground_long)
{
onground_long = 0;
#else
// turn motors off if throttle is off and pitch / roll sticks are centered
if (failsafe || (throttle < 0.001f && ( !ENABLESTIX || !onground_long || aux[LEVELMODE] || level_override || (fabsf(rx[0]) < (float) ENABLESTIX_TRESHOLD && fabsf(rx[1]) < (float) ENABLESTIX_TRESHOLD))))
{ // motors off
#endif
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
lpf(&underthrottlefilt, 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
onground_long = gettime();
#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;
float mix[4];
if ( controls_override)
{// change throttle in flip mode
throttle = rx_override[3];
}
#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
#if ( defined MIX_LOWER_THROTTLE || defined MIX_INCREASE_THROTTLE)
//#define MIX_INCREASE_THROTTLE
// options for mix throttle lowering if enabled
// 0 - 100 range ( 100 = full reduction / 0 = no reduction )
#ifndef MIX_THROTTLE_REDUCTION_PERCENT
#define MIX_THROTTLE_REDUCTION_PERCENT 100
#endif
// lpf (exponential) shape if on, othewise linear
//#define MIX_THROTTLE_FILTER_LPF
// limit reduction and increase 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
#ifndef MIX_THROTTLE_REDUCTION_MAX
#define MIX_THROTTLE_REDUCTION_MAX 0.5
#endif
#ifndef MIX_THROTTLE_INCREASE_MAX
#define MIX_THROTTLE_INCREASE_MAX 0.2
#endif
#ifndef MIX_MOTOR_MAX
#define MIX_MOTOR_MAX 1.0f
#endif
float overthrottle = 0;
float underthrottle = 0.001f;
for (int i = 0; i < 4; i++)
{
if (mix[i] > overthrottle)
overthrottle = mix[i];
if (mix[i] < underthrottle)
underthrottle = mix[i];
}
overthrottle -= MIX_MOTOR_MAX ;
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
// over
if (overthrottlefilt > (float)MIX_THROTTLE_REDUCTION_MAX)
overthrottlefilt = (float)MIX_THROTTLE_REDUCTION_MAX;
if (overthrottlefilt < -0.1f)
overthrottlefilt = -0.1;
overthrottle = overthrottlefilt;
if (overthrottle < 0.0f)
overthrottle = -0.0001f;
// reduce by a percentage only, so we get an inbetween performance
overthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
#ifndef MIX_LOWER_THROTTLE
// disable if not enabled
overthrottle = -0.0001f;
#endif
#ifdef MIX_INCREASE_THROTTLE
// under
if (underthrottle < -(float)MIX_THROTTLE_INCREASE_MAX)
underthrottle = -(float)MIX_THROTTLE_INCREASE_MAX;
#ifdef MIX_THROTTLE_FILTER_LPF
if (underthrottle < underthrottlefilt)
lpf(&underthrottlefilt, underthrottle, 0.82); // 20hz 1khz sample rate
else
lpf(&underthrottlefilt, underthrottle, 0.72); // 50hz 1khz sample rate
#else
if (underthrottle < underthrottlefilt)
underthrottlefilt -= 0.005f;
else
underthrottlefilt += 0.01f;
#endif
// under
if (underthrottlefilt < - (float)MIX_THROTTLE_REDUCTION_MAX)
underthrottlefilt = - (float)MIX_THROTTLE_REDUCTION_MAX;
if (underthrottlefilt > 0.1f)
underthrottlefilt = 0.1;
underthrottle = underthrottlefilt;
if (underthrottle > 0.0f)
underthrottle = 0.0001f;
underthrottle *= ((float)MIX_THROTTLE_REDUCTION_PERCENT / 100.0f);
#endif
if (overthrottle > 0 || underthrottle < 0 )
{ // exceeding max motor thrust
float temp = overthrottle + underthrottle;
for (int i = 0; i < 4; i++)
{
mix[i] -= temp;
}
}
// end MIX_LOWER_THROTTLE
#endif
#ifdef MOTOR_FILTER
for (int i = 0; i < 4; i++)
{
mix[i] = motorfilter(mix[i], i);
}
#endif
#ifdef CLIP_FF
float clip_ff(float motorin, int number);
for (int i = 0; i < 4; 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 < 4; i++)
{
if (mix[i] < 0)
mix[i] = 0;
if (mix[i] > 1)
mix[i] = 1;
thrsum += mix[i];
}
thrsum = thrsum / 4;
} // end motors on
imu_calc();
}
/////////////////////////////
/////////////////////////////
#ifdef MOTOR_CURVE_6MM_490HZ
// the old map for 490Hz
float motormap(float input)
{
// this is a thrust to pwm function
// float 0 to 1 input and output
// output can go negative slightly
// measured eachine motors and prop, stock battery
// a*x^2 + b*x + c
// a = 0.262 , b = 0.771 , c = -0.0258
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.262f + input * (0.771f);
input += -0.0258f;
return input;
}
#endif
// 8k pwm is where the motor thrust is relatively linear for the H8 6mm motors
// it's due to the motor inductance cancelling the nonlinearities.
#ifdef MOTOR_CURVE_NONE
float motormap(float input)
{
return input;
}
#endif
#ifdef MOTOR_CURVE_85MM_8KHZ
// Hubsan 8.5mm 8khz pwm motor map
// new curve
float motormap(float input)
{
// Hubsan 8.5mm motors and props
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.683f + input * (0.262f);
input += 0.06f;
return input;
}
#endif
#ifdef MOTOR_CURVE_85MM_8KHZ_OLD
// Hubsan 8.5mm 8khz pwm motor map
float motormap(float input)
{
// Hubsan 8.5mm motors and props
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.789f + input * (0.172f);
input += 0.04f;
return input;
}
#endif
#ifdef MOTOR_CURVE_85MM_32KHZ
// Hubsan 8.5mm 8khz pwm motor map
float motormap(float input)
{
// Hubsan 8.5mm motors and props
if (input > 1)
input = 1;
if (input < 0)
input = 0;
input = input * input * 0.197f + input * (0.74f);
input += 0.067f;
return input;
}
#endif
float hann_lastsample[4];
float hann_lastsample2[4];
// hanning 3 sample filter
float motorfilter(float motorin, int number)
{
float ans = motorin * 0.25f + hann_lastsample[number] * 0.5f + hann_lastsample2[number] * 0.25f;
hann_lastsample2[number] = hann_lastsample[number];
hann_lastsample[number] = motorin;
return ans;
}
void control( void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
if ( aux[RATES] )
{
ratemulti = HIRATEMULTI;
ratemultiyaw = HIRATEMULTIYAW;
}
else
{
ratemulti = 1.0;
ratemultiyaw = 1.0;
}
yawangle = yawangle + gyro[YAW]*looptime;
if ( auxchange[HEADLESSMODE] )
{
yawangle = 0;
}
if ( aux[HEADLESSMODE] )
{
float temp = rx[ROLL];
rx[ROLL] = rx[ROLL] * cosf( yawangle) - rx[PITCH] * sinf(yawangle );
rx[PITCH] = rx[PITCH] * cosf( yawangle) + temp * sinf(yawangle ) ;
}
error[ROLL] = rx[ROLL] * (float) MAX_RATE * DEGTORAD * ratemulti - gyro[ROLL];
error[PITCH] = rx[PITCH] * (float) MAX_RATE * DEGTORAD * ratemulti - gyro[PITCH];
error[YAW] = rx[YAW] * (float) MAX_RATEYAW * DEGTORAD * ratemultiyaw - gyro[YAW];
pid_precalc();
pid(ROLL);
pid(PITCH);
pid(YAW);
// map throttle so under 10% it is zero
float throttle = mapf(rx[3], 0 , 1 , -0.1 , 1 );
if ( throttle < 0 ) throttle = 0;
// turn motors off if throttle is off and pitch / roll sticks are centered
if ( failsafe || (throttle < 0.001f && (!ENABLESTIX|| (fabs(rx[ROLL]) < 0.5f && fabs(rx[PITCH]) < 0.5f ) ) ) )
{ // motors off
for ( int i = 0 ; i <= 3 ; i++)
{
pwm_set( i , 0 );
}
onground = 1;
thrsum = 0;
#ifdef MOTOR_FILTER
// reset the motor filter
for ( int i = 0 ; i <= 3 ; i++)
{
motorfilter( 0 , i);
}
#endif
}
else
{
onground = 0;
float mix[4];
// pidoutput[2] += motorchange;
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];
#endif
mix[MOTOR_FR] = throttle - pidoutput[ROLL] - pidoutput[PITCH] + pidoutput[YAW]; // FR
mix[MOTOR_FL] = throttle + pidoutput[ROLL] - pidoutput[PITCH] - pidoutput[YAW]; // FL
mix[MOTOR_BR] = throttle - pidoutput[ROLL] + pidoutput[PITCH] - pidoutput[YAW]; // BR
mix[MOTOR_BL] = throttle + pidoutput[ROLL] + pidoutput[PITCH] + pidoutput[YAW]; // BL
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];
#endif
#ifdef MOTOR_FILTER
for ( int i = 0 ; i <= 3 ; i++)
{
mix[i] = motorfilter( mix[i] , i);
}
#endif
for ( int i = 0 ; i <= 3 ; i++)
{
#ifndef NOMOTORS
pwm_set( i ,motormap( mix[i] ) );
#else
tempx[i] = motormap( mix[i] );
#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
}
void control( void)
{
// hi rates
float ratemulti;
float ratemultiyaw;
if ( aux[RATES] )
{
ratemulti = HIRATEMULTI;
ratemultiyaw = HIRATEMULTIYAW;
}
else
{
ratemulti = 1.0;
ratemultiyaw = 1.0;
}
yawangle = yawangle + gyro[YAW]*looptime;
if ( auxchange[HEADLESSMODE] )
{
yawangle = 0;
}
if ( aux[HEADLESSMODE] )
{
float temp = rx[ROLL];
rx[ROLL] = rx[ROLL] * cosf( yawangle) - rx[PITCH] * sinf(yawangle );
rx[PITCH] = rx[PITCH] * cosf( yawangle) + temp * sinf(yawangle ) ;
}
if ( aux[INVERTEDMODE] )
{
bridge_sequencer(REVERSE); // reverse
}
else
{
bridge_sequencer(FORWARD); // forward
}
// pwmdir controls hardware directly so we make a copy here
currentdir = pwmdir;
float rxtemp[3];
for (int i = 0 ; i < 3 ; i++)
{
rxtemp[i] = rx[i];
}
if (currentdir == REVERSE)
{
// invert pitch in reverse mode
//rxtemp[ROLL] = - rx[ROLL];
rxtemp[PITCH] = - rx[PITCH];
rxtemp[YAW] = - rx[YAW];
}
else
{
// normal thrust mode
//rxtemp[ROLL] = - rx[ROLL];
//rxtemp[PITCH] = - rx[PITCH];
//rxtemp[YAW] = - rx[YAW];
}
error[ROLL] = rxtemp[ROLL] * (float)MAX_RATE * (float)DEGTORAD * ratemulti - gyro[ROLL];
error[PITCH] = rxtemp[PITCH] * (float)MAX_RATE *(float) DEGTORAD * ratemulti - gyro[PITCH];
error[YAW] = ( - 1) * rxtemp[YAW] * (float)MAX_RATEYAW * (float)DEGTORAD * ratemultiyaw - gyro[YAW];
pid_precalc();
pid(ROLL);
pid(PITCH);
pid(YAW);
// map throttle so under 10% it is zero
float throttle = mapf(rx[3], 0 , 1 , -0.1 , 1 );
if ( throttle < 0 ) throttle = 0;
// turn motors off if throttle is off and pitch / roll sticks are centered
if ( failsafe || (throttle < 0.001f && (!ENABLESTIX|| (fabsf(rx[ROLL]) < 0.5f && fabsf(rx[PITCH]) < 0.5f ) ) ) )
{ // motors off
for ( int i = 0 ; i <= 3 ; i++)
{
pwm_set( i , 0 );
}
onground = 1;
thrsum = 0;
#ifdef MOTOR_FILTER
// reset the motor filter
for ( int i = 0 ; i <= 3 ; i++)
{
motorfilter( 0 , i);
}
#endif
}
else
{
onground = 0;
if ( stage == BRIDGE_WAIT ) onground = 1;
float mix[4];
if (currentdir == REVERSE)
{
// inverted flight
pidoutput[ROLL] = -pidoutput[ROLL];
pidoutput[PITCH] = -pidoutput[PITCH];
//pidoutput[YAW] = -pidoutput[YAW];
}
else
{
pidoutput[ROLL] = -pidoutput[ROLL];
//pidoutput[PITCH] = -pidoutput[PITCH];
//pidoutput[YAW] = -pidoutput[YAW];
}
// pidoutput[2] += motorchange;
#ifdef INVERT_YAW_PID
pidoutput[2] = -pidoutput[2];
#endif
mix[MOTOR_FR] = throttle - pidoutput[ROLL] - pidoutput[PITCH] + pidoutput[YAW]; // FR
mix[MOTOR_FL] = throttle + pidoutput[ROLL] - pidoutput[PITCH] - pidoutput[YAW]; // FL
mix[MOTOR_BR] = throttle - pidoutput[ROLL] + pidoutput[PITCH] - pidoutput[YAW]; // BR
mix[MOTOR_BL] = throttle + pidoutput[ROLL] + pidoutput[PITCH] + pidoutput[YAW]; // BL
#ifdef INVERT_YAW_PID
// we invert again cause it's used by the pid internally (for limit)
pidoutput[2] = -pidoutput[2];
#endif
#ifdef MOTOR_FILTER
for ( int i = 0 ; i <= 3 ; i++)
{
mix[i] = motorfilter( mix[i] , i);
}
#endif
for ( int i = 0 ; i <= 3 ; i++)
{
#ifndef NOMOTORS
pwm_set( i ,motormap( mix[i] ) );
#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
}