void CalibrateData::callbackAccelGyro(const sensor_msgs::ImuConstPtr & msg)
{
Eigen::Vector3d accel_raw;
Eigen::Vector3d accel_cal;
Eigen::Vector3d gyro_raw;
Eigen::Vector3d gyro_cal;
accel_raw(0) = msg->linear_acceleration.x;
accel_raw(1) = msg->linear_acceleration.y;
accel_raw(2) = msg->linear_acceleration.z;
gyro_raw(0) = msg->angular_velocity.x;
gyro_raw(1) = msg->angular_velocity.y;
gyro_raw(2) = msg->angular_velocity.z;
accel_cal = calibrate(accel_raw,
accel_alignment_matrix_,
accel_sensitivity_matrix_,
accel_offset_vector_);
gyro_cal = calibrate(gyro_raw,
gyro_alignment_matrix_,
gyro_sensitivity_matrix_,
gyro_offset_vector_);
sensor_msgs::Imu imu_msg = *msg;
imu_msg.linear_acceleration.x = accel_cal(0);
imu_msg.linear_acceleration.y = accel_cal(1);
imu_msg.linear_acceleration.z = accel_cal(2);
imu_msg.angular_velocity.x = gyro_cal(0) * M_PI / 180.0;
imu_msg.angular_velocity.y = gyro_cal(1) * M_PI / 180.0;
imu_msg.angular_velocity.z = gyro_cal(2) * M_PI / 180.0;
imu_pub_.publish(imu_msg);
}
void mpu6050_init() {
// printf("rspt: %x\n", MCM_ETBCC&6);
// SetIsr(NonMaskableInt_VECTORn, NMI_Handler);
// EnableIsr(NonMaskableInt_VECTORn);
i2c_init(I2C0, 400000);
DELAY_MS(100);
printf("init start\n");
mpu6050_short_init();
DELAY_MS(1000);
gyro_cal();
printf("init ends\n");
// float Rx = 0, raw_accel_angle = 0, sum = 0;
// for(int i=0; i<100; i++){
// float Rx = (((float) (adc_once(ADC0_SE17, ADC_10bit) - 525) * 3.3/ 1023))/ 0.8f;
//
// if(Rx > 1.0){
// Rx = 1.0;
// }else if(Rx < -1.0){
// Rx = -1.0;
// }
// raw_accel_angle = 90 - (acos(Rx) * 180 / 3.1415f - 90);
// sum += raw_accel_angle;
// }
// angle[0] = sum/100;
}
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;
}
int main(void)
{
clk_init();
gpio_init();
#ifdef SERIAL
serial_init();
#endif
i2c_init();
spi_init();
pwm_init();
pwm_set(MOTOR_FL, 0); // FL
pwm_set(MOTOR_FR, 0);
pwm_set(MOTOR_BL, 0); // BL
pwm_set(MOTOR_BR, 0); // FR
time_init();
if (RCC_GetCK_SYSSource() == 8)
{
}
else
{
failloop(5);
}
sixaxis_init();
if (sixaxis_check())
{
#ifdef SERIAL
printf(" MPU found \n");
#endif
}
else
{
#ifdef SERIAL
printf("ERROR: MPU NOT FOUND \n");
#endif
failloop(4);
}
adc_init();
rx_init();
int count = 0;
vbattfilt = 0.0;
while (count < 64)
{
vbattfilt += adc_read(1);
count++;
}
// for randomising MAC adddress of ble app - this will make the int = raw float value
random_seed = *(int *)&vbattfilt ;
random_seed = random_seed&0xff;
vbattfilt = vbattfilt / 64;
#ifdef SERIAL
printf("Vbatt %2.2f \n", vbattfilt);
#ifdef NOMOTORS
printf("NO MOTORS\n");
#warning "NO MOTORS"
#endif
#endif
#ifdef STOP_LOWBATTERY
// infinite loop
if (vbattfilt < STOP_LOWBATTERY_TRESH)
failloop(2);
#endif
// loads acc calibration and gyro dafaults
loadcal();
gyro_cal();
rgb_init();
imu_init();
extern unsigned int liberror;
if (liberror)
{
#ifdef SERIAL
printf("ERROR: I2C \n");
#endif
failloop(7);
}
lastlooptime = gettime();
extern int rxmode;
extern int failsafe;
float thrfilt;
//
//
// MAIN LOOP
//
//
checkrx();
while (1)
{
// gettime() needs to be called at least once per second
maintime = gettime();
looptime = ((uint32_t) (maintime - lastlooptime));
if (looptime <= 0)
looptime = 1;
looptime = looptime * 1e-6f;
if (looptime > 0.02f) // max loop 20ms
{
failloop(3);
//endless loop
}
lastlooptime = maintime;
if (liberror > 20)
{
failloop(8);
// endless loop
}
sixaxis_read();
control();
// battery low logic
float hyst;
float battadc = adc_read(1);
vbatt = battadc;
// average of all 4 motor thrusts
// should be proportional with battery current
extern float thrsum; // from control.c
// filter motorpwm so it has the same delay as the filtered voltage
// ( or they can use a single filter)
lpf ( &thrfilt , thrsum , 0.9968f); // 0.5 sec at 1.6ms loop time
lpf ( &vbattfilt , battadc , 0.9968f);
#ifdef AUTO_VDROP_FACTOR
static float lastout[12];
static float lastin[12];
static float vcomp[12];
static float score[12];
static int current_index = 0;
int minindex = 0;
float min = score[0];
{
int i = current_index;
vcomp[i] = vbattfilt + (float) i *0.1f * thrfilt;
if ( lastin[i] < 0.1f ) lastin[i] = vcomp[i];
float temp;
// y(n) = x(n) - x(n-1) + R * y(n-1)
// out = in - lastin + coeff*lastout
// hpf
temp = vcomp[i] - lastin[i] + FILTERCALC( 1000*12 , 1000e3) *lastout[i];
lastin[i] = vcomp[i];
lastout[i] = temp;
lpf ( &score[i] , fabsf(temp) , FILTERCALC( 1000*12 , 10e6 ) );
}
current_index++;
if ( current_index >= 12 ) current_index = 0;
for ( int i = 0 ; i < 12; i++ )
{
if ( score[i] < min )
{
min = score[i];
minindex = i;
}
}
#undef VDROP_FACTOR
#define VDROP_FACTOR minindex * 0.1f
#endif
if ( lowbatt ) hyst = HYST;
else hyst = 0.0f;
vbatt_comp = vbattfilt + (float) VDROP_FACTOR * thrfilt;
if ( vbatt_comp <(float) VBATTLOW + hyst ) lowbatt = 1;
else lowbatt = 0;
// led flash logic
if (rxmode != RX_MODE_BIND)
{
// non bind
if (failsafe)
{
if (lowbatt)
ledflash(500000, 8);
else
ledflash(500000, 15);
}
else
{
if (lowbatt)
ledflash(500000, 8);
else
{
if (ledcommand)
{
if (!ledcommandtime)
ledcommandtime = gettime();
if (gettime() - ledcommandtime > 500000)
{
ledcommand = 0;
ledcommandtime = 0;
}
ledflash(100000, 8);
}
else
{
if ( aux[LEDS_ON] )
ledon( 255);
else
ledoff( 255);
}
}
}
}
else
{ // bind mode
ledflash(100000 + 500000 * (lowbatt), 12);
}
// rgb strip logic
#if (RGB_LED_NUMBER > 0)
extern void rgb_led_lvc( void);
rgb_led_lvc( );
#endif
#ifdef BUZZER_ENABLE
buzzer();
#endif
#ifdef FPV_ON
static int fpv_init = 0;
if ( rxmode == RX_MODE_NORMAL && ! fpv_init ) {
fpv_init = gpio_init_fpv();
}
if ( fpv_init ) {
if ( failsafe ) {
GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, Bit_RESET );
} else {
GPIO_WriteBit( FPV_PIN_PORT, FPV_PIN, aux[ FPV_ON ] ? Bit_SET : Bit_RESET );
}
}
#endif
checkrx();
// loop time 1ms
while ((gettime() - maintime) < (1000 - 22) )
delay(10);
} // end loop
}
int main(void)
{
clk_init();
gpio_init();
#ifdef SERIAL
serial_init();
#endif
i2c_init();
spi_init();
pwm_init();
pwm_set(MOTOR_FL, 0); // FL
pwm_set(MOTOR_FR, 0);
pwm_set(MOTOR_BL, 0); // BL
pwm_set(MOTOR_BR, 0); // FR
time_init();
#ifdef SERIAL
printf("\n clock source:");
#endif
if (RCC_GetCK_SYSSource() == 8)
{
#ifdef SERIAL
printf(" PLL \n");
#endif
}
else
{
#ifdef SERIAL
if (RCC_GetCK_SYSSource() == 0)
printf(" HSI \n");
else
printf(" OTHER \n");
#endif
failloop(5);
}
sixaxis_init();
if (sixaxis_check())
{
#ifdef SERIAL
printf(" MPU found \n");
#endif
}
else
{
#ifdef SERIAL
printf("ERROR: MPU NOT FOUND \n");
#endif
failloop(4);
}
adc_init();
rx_init();
int count = 0;
float vbattfilt = 0.0;
while (count < 64)
{
vbattfilt += adc_read(1);
count++;
}
vbattfilt = vbattfilt / 64;
#ifdef SERIAL
printf("Vbatt %2.2f \n", vbattfilt);
#ifdef NOMOTORS
printf("NO MOTORS\n");
#warning "NO MOTORS"
#endif
#endif
#ifdef STOP_LOWBATTERY
// infinite loop
if (vbattfilt < STOP_LOWBATTERY_TRESH)
failloop(2);
#endif
// loads acc calibration and gyro dafaults
loadcal();
gyro_cal();
imu_init();
extern unsigned int liberror;
if (liberror)
{
#ifdef SERIAL
printf("ERROR: I2C \n");
#endif
failloop(7);
}
lastlooptime = gettime();
extern int rxmode;
extern int failsafe;
float thrfilt;
//
//
// MAIN LOOP
//
//
checkrx();
while (1)
{
// gettime() needs to be called at least once per second
maintime = gettime();
looptime = ((uint32_t) (maintime - lastlooptime));
if (looptime <= 0)
looptime = 1;
looptime = looptime * 1e-6f;
if (looptime > 0.02f) // max loop 20ms
{
failloop(3);
//endless loop
}
lastlooptime = maintime;
if (liberror > 20)
{
failloop(8);
// endless loop
}
sixaxis_read();
control();
// battery low logic
float battadc = adc_read(1);
// average of all 4 motor thrusts
// should be proportional with battery current
extern float thrsum; // from control.c
// filter motorpwm so it has the same delay as the filtered voltage
// ( or they can use a single filter)
lpf(&thrfilt, thrsum, 0.9968); // 0.5 sec at 1.6ms loop time
lpf(&vbattfilt, battadc, 0.9968);
if (vbattfilt + VDROP_FACTOR * thrfilt < VBATTLOW)
lowbatt = 1;
else
lowbatt = 0;
// led flash logic
if (rxmode != RX_MODE_BIND)
{ // non bind
if (failsafe)
{
if (lowbatt)
ledflash(500000, 8);
else
ledflash(500000, 15);
}
else
{
if (lowbatt)
ledflash(500000, 8);
else
{
if (ledcommand)
{
if (!ledcommandtime)
ledcommandtime = gettime();
if (gettime() - ledcommandtime > 500000)
{
ledcommand = 0;
ledcommandtime = 0;
}
ledflash(100000, 8);
}
else
ledon(255);
}
}
}
else
{ // bind mode
ledflash(100000 + 500000 * (lowbatt), 12);
}
checkrx();
#ifdef DEBUG
elapsedtime = gettime() - maintime;
#endif
// loop time 1ms
while ((gettime() - maintime) < 1000)
delay(10);
} // end loop
}
int main(void)
{
clk_init();
delay(1000);
gpio_init();
#ifdef SERIAL
serial_init();
#endif
i2c_init();
spi_init();
pwm_init();
pwm_set( MOTOR_FL , 0); // FL
pwm_set( MOTOR_FR , 0);
pwm_set( MOTOR_BL , 0); // BL
pwm_set( MOTOR_BR , 0); // FR
time_init();
#ifdef SERIAL
printf( "\n clock source:" );
#endif
if ( RCC_GetCK_SYSSource() == 8)
{
#ifdef SERIAL
printf( " PLL \n" );
#endif
}
else
{
#ifdef SERIAL
if ( RCC_GetCK_SYSSource() == 0) printf( " HSI \n" );
else printf( " OTHER \n" );
#endif
failloop( 5 );
}
sixaxis_init();
if ( sixaxis_check() )
{
#ifdef SERIAL
printf( " MPU found \n" );
#endif
}
else
{
#ifdef SERIAL
printf( "ERROR: MPU NOT FOUND \n" );
#endif
failloop(4);
}
adc_init();
rx_init();
int count = 0;
while ( count < 64 )
{
vbattfilt += adc_read(1);
count++;
}
vbattfilt = vbattfilt/64;
#ifdef SERIAL
printf( "Vbatt %2.2f \n", vbattfilt );
#ifdef NOMOTORS
printf( "NO MOTORS\n" );
#warning "NO MOTORS"
#endif
#endif
#ifdef STOP_LOWBATTERY
// infinite loop
if ( vbattfilt < (float) STOP_LOWBATTERY_TRESH) failloop(2);
#endif
gyro_cal();
extern unsigned int liberror;
if ( liberror )
{
#ifdef SERIAL
printf( "ERROR: I2C \n" );
#endif
failloop(7);
}
static unsigned lastlooptime;
lastlooptime = gettime();
extern int rxmode;
extern int failsafe;
float thrfilt;
//
//
// MAIN LOOP
//
//
#ifdef DEBUG
static float timefilt;
#endif
while(1)
{
// gettime() needs to be called at least once per second
unsigned long time = gettime();
looptime = ((uint32_t)( time - lastlooptime));
if ( looptime <= 0 ) looptime = 1;
looptime = looptime * 1e-6f;
if ( looptime > 0.02f ) // max loop 20ms
{
failloop( 3);
//endless loop
}
#ifdef DEBUG
totaltime += looptime;
lpf ( &timefilt , looptime, 0.998 );
#endif
lastlooptime = time;
if ( liberror > 20)
{
failloop(8);
// endless loop
}
checkrx();
gyro_read();
control();
// battery low logic
static int lowbatt = 0;
float hyst;
float battadc = adc_read(1);
// average of all 4 motor thrusts
// should be proportional with battery current
extern float thrsum; // from control.c
// filter motorpwm so it has the same delay as the filtered voltage
// ( or they can use a single filter)
lpf ( &thrfilt , thrsum , 0.9968f); // 0.5 sec at 1.6ms loop time
lpf ( &vbattfilt , battadc , 0.9968f);
if ( lowbatt ) hyst = HYST;
else hyst = 0.0f;
if ( vbattfilt + (float) VDROP_FACTOR * thrfilt <(float) VBATTLOW + hyst ) lowbatt = 1;
else lowbatt = 0;
// led flash logic
if ( rxmode == 0)
{// bind mode
ledflash ( 100000+ 500000*(lowbatt) , 12);
}else
{// non bind
if ( failsafe)
{
if ( lowbatt )
ledflash ( 500000 , 8);
else
ledflash ( 500000, 15);
}
else
{
if ( lowbatt)
ledflash ( 500000 , 8);
else ledon( 255);
}
}
// the delay is required or it becomes endless loop ( truncation in time routine)
while ( (gettime() - time) < 1000 ) delay(10);
}// end loop
}