void Dct::doDCT(Matrix<short> *originMatrix, long (*resultMatrix)[MATRIX_SIZE+4], long dctRadius){
this->originMatrix = originMatrix;
this->resultMatrix = resultMatrix;
dct();
lpf(dctRadius);
idct();
}
static int start(sox_effect_t * effp)
{
priv_t * p = (priv_t *)effp->priv;
dft_filter_t * f = p->base.filter_ptr;
if (!f->num_taps) {
double Fn = effp->in_signal.rate * .5;
double * h[2];
int i, n, post_peak, longer;
if (p->Fc0 >= Fn || p->Fc1 >= Fn) {
lsx_fail("filter frequency must be less than sample-rate / 2");
return SOX_EOF;
}
h[0] = lpf(Fn, p->Fc0, p->tbw0, &p->num_taps[0], p->att, &p->beta,p->round);
h[1] = lpf(Fn, p->Fc1, p->tbw1, &p->num_taps[1], p->att, &p->beta,p->round);
if (h[0])
invert(h[0], p->num_taps[0]);
longer = p->num_taps[1] > p->num_taps[0];
n = p->num_taps[longer];
if (h[0] && h[1]) {
for (i = 0; i < p->num_taps[!longer]; ++i)
h[longer][i + (n - p->num_taps[!longer])/2] += h[!longer][i];
if (p->Fc0 < p->Fc1)
invert(h[longer], n);
free(h[!longer]);
}
if (p->phase != 50)
lsx_fir_to_phase(&h[longer], &n, &post_peak, p->phase);
else post_peak = n >> 1;
if (effp->global_info->plot != sox_plot_off) {
char title[100];
sprintf(title, "SoX effect: sinc filter freq=%g-%g",
p->Fc0, p->Fc1? p->Fc1 : Fn);
lsx_plot_fir(h[longer], n, effp->in_signal.rate,
effp->global_info->plot, title, -p->beta * 10 - 25, 5.);
return SOX_EOF;
}
lsx_set_dft_filter(f, h[longer], n, post_peak);
}
float adc_read(int channel)
{
switch(channel)
{
case 0:
#ifdef DEBUG
lpf(&debug.adcfilt , (float) adcarray[0] , 0.998);
#endif
return (float) adcarray[0] * ((float) ADC_SCALEFACTOR) ;
case 1:
#ifdef DEBUG
lpf(&debug.adcreffilt , (float) adcarray[1] , 0.998);
#endif
return vref_cal / (float) adcarray[1];
default:
return 0;
}
}
/// Get the Möbius function value of the number get_number(index).
/// For performance reasons mu(index) == 0 is not supported.
/// @pre mu(index) != 0 (i.e. lpf(index) != 0)
///
int64_t mu(int64_t index) const
{
assert(lpf(index) != 0);
return (factors_[index] & 1) ? -1 : 1;
}
int get_tracker_sensor(unsigned int id, float p[3], float R[16])
{
onit_point *P;
if ((P = onitcs_acquire_points()))
{
float a[3];
float b[3];
float x[3] = { 1.0f, 0.0f, 0.0f };
float y[3] = { 0.0f, 1.0f, 0.0f };
float z[3] = { 0.0f, 0.0f, 1.0f };
/* Synthesize a head sensor from the position of the head and neck. */
if (id == 0)
{
get_tracker_point(P, 0, a);
get_tracker_point(P, 1, b);
lpf(head_p, a);
lpf(neck_p, b);
y[0] = head_p[0] - neck_p[0];
y[1] = head_p[1] - neck_p[1];
y[2] = head_p[2] - neck_p[2];
normalize(y);
cross(x, y, z);
normalize(x);
cross(z, x, y);
normalize(z);
load_idt(R);
p[0] = head_p[0];
p[1] = head_p[1];
p[2] = head_p[2];
R[0] = x[0]; R[1] = x[1], R[ 2] = x[2];
R[4] = y[0]; R[5] = y[1], R[ 6] = y[2];
R[8] = z[0]; R[9] = z[1], R[10] = z[2];
}
/* Synthesize a hand sensor from the position of the hand and elbow. */
if (id == 1)
{
get_tracker_point(P, 14, a);
get_tracker_point(P, 12, b);
lpf(hand_p, a);
lpf(elbo_p, b);
z[0] = elbo_p[0] - hand_p[0];
z[1] = elbo_p[1] - hand_p[1];
z[2] = elbo_p[2] - hand_p[2];
normalize(z);
cross(x, y, z);
normalize(x);
cross(y, z, x);
normalize(y);
load_idt(R);
p[0] = hand_p[0];
p[1] = hand_p[1];
p[2] = hand_p[2];
R[0] = x[0]; R[1] = x[1], R[ 2] = x[2];
R[4] = y[0]; R[5] = y[1], R[ 6] = y[2];
R[8] = z[0]; R[9] = z[1], R[10] = z[2];
}
onitcs_release_points();
return 1;
}
return 0;
}
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
}
int main(void)
{
int temp = 0; // Read temperature
//int sp_temp_get;
float temp_real = 0; // Real format temperature
//int t_sample = 100; // Time period samples read ms
//int t_control = 1000; // Time period control system ms
uint16_t t_sample = 0; // Time period samples read ms
int t_control = 0; // Time period control system ms
//long millis_ant1; // Aux timer
//long millis_ant2; // Aux timer
uint16_t out_pwm; // Out PWM control
double y_temp[PmA+2] = {0}; // Array out incremental y(k), y(k-1), y(k-2), y(k-3)
double u_temp[PmB+3] = {0}; // Array process input incremental u(k), u(k-1), u(k-2), u(k-3), u(k-4)
double t_temp[5] = {1.0, -0.3, 0.1, 0.1, 0.1}; // Array parameters adaptive mechanism a1k, a2k, b1k, b2k, b3k
double sp_temp[2] = {0}; // Set point process sp(k)
double yp_temp[2] = {0}; // Array process out yp(k)
cli(); // Disable interrupts
conductor_block(); // Calculate parameters conductor block
usart_init(); // Initialize USART
UCSR0B |= (1 << RXCIE0); // Enable interrupt RX (If enabled do not use functions get_xx)
adc_Setup(); // Initialize ADC
timer1_init(); // Timer system without interrupts
//timer0_init(); // Initialize timer0 system ms
timer2_init(); // Initialize timer2 PWM
sei();
DDRD |= (1 << PIND3); // Out PWM OC2B PIND3 Digital PIN 3
temp = adc_read(0); // Initialize temperature readers
//millis_ant1 = millis; // Initialize period samples
//millis_ant2 = millis; // Initialize period control
//eeprom_update_float(&eeprom_float,f);
//float eeprom_float_read;
//eeprom_float_read = eeprom_read_float(&eeprom_float);
//put_float(eeprom_float_read);
//put_string("\n Input SP temperature: ");
//sp_temp_get = get_int();
while(1)
{
// Samples read
//if ((millis - millis_ant1) >= t_sample){
//millis_ant1 = millis;
t_sample = TCNT1; // Catch sample time for integer angle gyro
T_CNT = T_SAMPLE * 1000L/64; // Number count temp1. 1 Count Temp1 64us => T_sample = Value_CNT1 = ms/64us
if (t_sample >= T_CNT){ // Attitude calculates Read IMU (Accel and Gyro)
TCNT1 = 0; // Restart sample time
t_control++; // Increment time Period control
temp = lpf(adc_read(0), temp, 0.1);
}
//if ((millis - millis_ant2) >= t_control){
//millis_ant2 = millis;
if (t_control >= T_CONTROL){ //Control action balancer
t_control = 0;
temp_real = temp * 110.0 / 1023.0; // solo en el control
//Adaptive predictive balancer process
sp_temp[0] = sp_temp_get; // Set point
yp_temp[0] = temp_real; // Process out y(k).
adaptive(sp_temp, t_temp, y_temp, u_temp, yp_temp, UP_PWM); // Call adaptive function
out_pwm = u_temp[0]; // Out Controller adaptive
if (out_pwm > UP_PWM) out_pwm = UP_PWM; // Upper limit out
else if (out_pwm < 0) out_pwm = 0;
//out_pwm = 0; // Off control
OCR2B = 255 - out_pwm; // Out PWM
put_float(temp_real);
put_string(" ");
put_int(OCR2B);
put_string(" ");
put_int(sp_temp_get);
//put_string(" ");
//put_float(NL);
//put_string(" ");
//put_float(GainA);
//put_string(" ");
//put_float(GainB);
//put_string(" ");
//put_int(T_SAMPLE);
//put_string(" ");
//put_int(T_CONTROL);
put_string("\n");
}
//TODO:: Please write your application code
}
}
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
}
PlanePrimitive::PlanePrimitive(const LidarOctree* octree,const Primitive::Vector& translation,Cluster::MulticastPipe* pipe)
{
/* Create a LiDAR plane extractor: */
LidarPlaneExtractor lpe;
/* Process all selected points: */
octree->processSelectedPoints(lpe);
if(lpe.getNumPoints()>=3)
{
/* Extract the plane's coordinate frame: */
LidarPlaneExtractor::Point centroid;
LidarPlaneExtractor::Vector planeFrame[3];
double lengths[3];
lpe.calcPlane(centroid,planeFrame,lengths);
/* Ensure that (planeFrame, planeNormal) is a right-handed system, and that planeNormal points "up:" */
if(planeFrame[2][2]<0.0)
planeFrame[2]=-planeFrame[2];
if(Geometry::cross(planeFrame[1],planeFrame[2])*planeFrame[0]<0.0)
planeFrame[0]=-planeFrame[0];
plane=Plane(planeFrame[2],centroid);
/* Calculate the bounding box of the selected points in plane coordinates: */
LidarPlaneFitter lpf(centroid,planeFrame);
octree->processSelectedPoints(lpf);
/* Store the number of points and the RMS residual: */
numPoints=lpe.getNumPoints();
rms=lpf.getRMS();
/* Calculate the extracted plane's rectangle: */
double min[2],max[2];
for(int i=0;i<2;++i)
{
min[i]=lpf.getMin(i);
max[i]=lpf.getMax(i);
}
double size=max[0]-min[0];
if(size<max[1]-min[1])
size=max[1]-min[1];
for(int i=0;i<2;++i)
{
min[i]-=0.1*size;
max[i]+=0.1*size;
}
for(int i=0;i<4;++i)
{
points[i]=centroid;
points[i]+=planeFrame[0]*(i&0x1?max[0]:min[0]);
points[i]+=planeFrame[1]*(i&0x2?max[1]:min[1]);
}
/* Compute an appropriate number of grid lines in x and y: */
double aspect=(max[0]-min[0])/(max[1]-min[1]);
if(aspect>=1.0)
{
numX=10;
numY=int(Math::floor(10.0/aspect+0.5));
}
else
{
numY=10;
numX=int(Math::floor(10.0*aspect+0.5));
}
/* Print the plane equation: */
std::cout<<"Plane fitting "<<numPoints<<" points"<<std::endl;
LidarPlaneExtractor::Point tCentroid=centroid;
tCentroid+=translation;
std::cout<<"Centroid: ("<<tCentroid[0]<<", "<<tCentroid[1]<<", "<<tCentroid[2]<<")"<<std::endl;
LidarPlaneExtractor::Vector normal=planeFrame[2];
normal.normalize();
std::cout<<"Normal vector: ("<<normal[0]<<", "<<normal[1]<<", "<<normal[2]<<")"<<std::endl;
std::cout<<"RMS approximation residual: "<<rms<<std::endl;
if(pipe!=0)
{
/* Send the extracted primitive over the pipe: */
pipe->write<int>(1);
pipe->write<unsigned int>((unsigned int)(numPoints));
pipe->write<Scalar>(rms);
pipe->write<LidarPlaneExtractor::Point::Scalar>(centroid.getComponents(),3);
pipe->write<LidarPlaneExtractor::Vector::Scalar>(planeFrame[2].getComponents(),3);
for(int i=0;i<4;++i)
pipe->write<Point::Scalar>(points[i].getComponents(),3);
pipe->write<int>(numX);
pipe->write<int>(numY);
pipe->flush();
}
}
else
{
if(pipe!=0)
{
pipe->write<int>(0);
pipe->flush();
}
Misc::throwStdErr("PlanePrimitive::PlanePrimitive: Not enough selected points");
}
}
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 (int argc, char **argv) {
FILE *fp;
struct wavfile header;
char *filename;
float buf[buflen];
int channels;
// no need for commandline -f and -d switches (file/device)
if (argc != 2) {
usage();
}
filename = argv[1];
if ((use_wav = ends_with(".wav", filename)) == 0) {
fp = fopen(filename, "rb");
if (fp == NULL) {
fprintf(stderr, "Not able to open input file %s.\n", filename);
return 1;
}
// read header
if (fread(&header, sizeof(header), 1, fp) < 1) {
fprintf(stderr, "Can't read file header\n");
return 1;
}
// tarkitaa että wav-tiedosto varmasti on wav
if (strncmp(header.id, "RIFF", 4) != 0 ||
strncmp(header.data, "data", 4) != 0 ||
strncmp(header.wavefmt, "WAVEfmt", 7) != 0) {
fprintf(stderr, "file is not wav\n");
return 1;
}
channels = header.channels;
int frames = header.bytes_in_data / channels / 2;
if (frames < buflen) {
fprintf(stderr, "wav file was too short\n");
return 1;
}
} else if ((use_soundcard = starts_with("hw:", filename)) == 0) {
prepare_soundcard(filename);
channels = 2; // pakko olla mun äänikortilla
} else {
usage();
}
// gotta allocate twice as much for stereo
// one frame contains both left and right channel
int size = buflen * channels;
int16_t buffer[size];
// catch ctrl+c to quit program nicely
signal(SIGINT, int_handler);
while (running) {
if (use_soundcard == 0) {
// read from alsa device to float buffer
buffer_from_soundcard(buffer, buflen);
} else if (use_wav == 0) {
// read file to float buffer
if ((fread(buffer, sizeof(buffer), 1, fp)) != 1) {
break;
}
}
// muuttaa taulukon arvot välille [-1, 1] jotta niille voidaan tehdä
// signaalinkäsittelyjuttuja
for (int k = 0; k < buflen; k++) {
float s = (float) buffer[k * channels] / INT16_MAX;
buf[k] = s;
}
// apply simple lowpass filtering to buf
float *buf2 = lpf(buf);
int freq = pitchdetect(buf2);
if (print_pitch == 1) {
pretty_print_pitch(freq);
} else {
printf("%d\n", freq);
}
}
fclose(fp);
return 0;
}
void gyro_cal(void)
{
int data[6];
unsigned long time = gettime();
unsigned long timestart = time;
unsigned long timemax = time;
unsigned long lastlooptime;
// 2 and 15 seconds
while ( time - timestart < 2e6 && time - timemax < 15e6 )
{
unsigned long looptime;
looptime = time - lastlooptime;
lastlooptime = time;
if ( looptime == 0 ) looptime = 1;
//softi2c_readdata( 0x68 , 67 , data, 6 );
i2c_readdata( 67 , data, 6 );
gyro[0] = (int16_t) ((data[2]<<8) + data[3]);
gyro[1] = (int16_t) ((data[0]<<8) + data[1]);
gyro[2] = (int16_t) ((data[4]<<8) + data[5]);
if ( (time - timestart)%200000 > 100000)
{
ledon(B00000101);
ledoff(B00001010);
}
else
{
ledon(B00001010);
ledoff(B00000101);
}
for ( int i = 0 ; i < 3 ; i++)
{
if ( fabs(gyro[i]) > 100 )
{
count = 0;
timestart = gettime();
}
else
{
lpf( &gyrocal[i] , gyro[i], lpfcalc( (float) looptime , 0.5 * 1e6) );
count++;
}
}
time = gettime();
}
if ( count < 100 )
{
for ( int i = 0 ; i < 3; i++)
{
gyrocal[i] = 0;
}
}
#ifdef SERIAL
printf("gyro calibration %f %f %f \n " , gyrocal[0] , gyrocal[1] , gyrocal[2]);
#endif
}
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
}
