Skip to content

Commit

Permalink
KALMAN OK After flight test
Browse files Browse the repository at this point in the history
  • Loading branch information
@tilaktilak
tilaktilak committed Mar 12, 2017
1 parent 44efe90 commit e690d9b
Show file tree
Hide file tree
Showing 3 changed files with 64 additions and 47 deletions.
45 changes: 23 additions & 22 deletions kalman.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,42 +3,42 @@
#include <avr/io.h>
#include <stdfix.h>

float measure[2] = {0.f,0.f};
_Accum measure[2] = {0.f,0.f};

float H[2][2]={{1.f,0.f},
_Accum H[2][2]={{1.f,0.f},
{0.f,0.f}};

float R[2][2]={{2.f*2.f,0.f},
_Accum R[2][2]={{2.f*2.f,0.f},
{0.f ,0.f}};

float A[2][2]={{1.f ,0.008f},
_Accum A[2][2]={{1.f ,0.008f},
{0.f ,1.f }};

float Q[2][2]={{0.f ,0.f},
_Accum Q[2][2]={{0.f ,0.f},
{0.f ,1.f }};
//float Q[2][2]={{.3f ,0.f},
//_Accum Q[2][2]={{.3f ,0.f},
// {0.f ,0.05f }};
// Réglages cool : Q:0.2, R:1;

float Pp[2][2]={{0.f},{0.f}};
float P[2][2]={{0.f},{0.f}};
float K[2][2]={{0.f},{0.f}};
float Xp[2]={0.f};
_Accum Pp[2][2]={{0.f},{0.f}};
_Accum P[2][2]={{0.f},{0.f}};
_Accum K[2][2]={{0.f},{0.f}};
_Accum Xp[2]={0.f};

float X[2];
int kalman_init(float altitude_init){
_Accum X[2];
int kalman_init(_Accum altitude_init){
X[0] = altitude_init;
X[1] = 0.f;
return 0;

}

/*int kalman_update(float *Pp,
float *H,
float *R,
float *Xp){
/*int kalman_update(_Accum *Pp,
_Accum *H,
_Accum *R,
_Accum *Xp){
*/
int kalman_update(float alt){
int kalman_update(_Accum alt){
/*
alt = 73.4156f;
Expand Down Expand Up @@ -107,12 +107,13 @@ printf("%f %f\r\n %f %f\r\n",K[0][0],
return 0;
}

/*int kalman_predit(float *P,
float *A,
float *Q,
float *X){
/*
int kalman_predit(_Accum *P,
_Accum *A,
_Accum *Q,
_Accum *X){
*/
int kalman_predict(float dt){
int kalman_predict(_Accum dt){
//X[0] = 73.5231f;
//X[1] = 0.0006f;

Expand Down
8 changes: 4 additions & 4 deletions kalman.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,12 @@
#define KALMAN_H


extern float X[2];
extern _Accum X[2];

int kalman_init(float altitude_init);
int kalman_init(_Accum altitude_init);

int kalman_update(float alt);
int kalman_update(_Accum alt);

int kalman_predict(float dt);
int kalman_predict(_Accum dt);

#endif
58 changes: 37 additions & 21 deletions main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,15 +91,18 @@ FILE uart_output = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);

#define TIMER_FREQ_HZ 1

//const float dt = powf(2,16)*(1.f/8E6);
const float dt = 0.010f;
float time = 0.f;
//const _Accum dt = powf(2,16)*(1.f/8E6);
_Accum dt = 0.0f;
// Count until 8192
_Accum time = 0.f;
void timer1_init(void){
// TIMER 1 Config for timing while 1
TCCR1A = 0;
TCCR1B = 0;
TCCR1C = 0;
TCCR1B |=(1<<CS10);//prescaler=1
TCCR1B |=(1<<CS10) ;//| (1<<CS11);//prescaler=8
//OCR1AL = 0b00000000;
//OCR1AH = 0b00100000;
}

void timer2_init(void){
Expand All @@ -117,7 +120,7 @@ void timer2_init(void){
PORTC &= ~(1<<PORTC3);
}
// Fmin = 250Hz
void timer2_set_freq(float freq){
void timer2_set_freq(_Accum freq){
cli();
OCR2A = (uint8_t)((1./freq)*8E6/128.f);
// PERIOD = 1s -> 8E6/128f
Expand All @@ -130,21 +133,21 @@ volatile int32_t tim2_period = 0;
volatile uint8_t mute = 0;
volatile uint8_t state = 0;

void timer2_set_duration(float d_sec){
void timer2_set_duration(_Accum d_sec){
cli();
duration = (int)(d_sec/2.f*(8E6/128.f)/(float)OCR2A);
duration = (int)(d_sec/2.f*(8E6/128.f)/(_Accum)OCR2A);
tim2_period = duration;//Launch new period
sei();
}

void timer2_set_volume(float volume){
void timer2_set_volume(_Accum volume){
cli();
OCR2B = (uint8_t)((OCR2A/2)*(volume/100.f));// Pourcent of OCR2A

sei();
}

float altitude = 0.f;
_Accum altitude = 0.f;

int parse_nmea(uint8_t c){
static int i = 0;
Expand Down Expand Up @@ -221,16 +224,18 @@ int main(void)

InitBMP280();

float alt = 0.f;
float rate = 0.f;
_Accum alt = 0.f;
_Accum rate = 0.f;
//_Accum ralt = 0.f;
//_Accum old_alt = 0.f;

float freq = 300.f;
float const low_level = -1.f;
float const high_level = 0.3f;
float const low_gain = -50.f;
float const low_offset = 300.f;
float const high_gain = 150.f;
float const high_offset = 1100.f;
_Accum freq = 300.f;
_Accum const low_level = -0.3f;
_Accum const high_level = 0.2f;
_Accum const low_gain = -50.f;
_Accum const low_offset = 300.f;
_Accum const high_gain = 150.f;
_Accum const high_offset = 1100.f;


long int const prs_count_max = 10000;
Expand All @@ -248,8 +253,13 @@ int main(void)
kalman_init(alt);
char c = ' ';

//uint16_t count_dt = 0;

for (;;) {
while((TIFR1 & (1<<TOV1))!=(1<<TOV1)){// Wait until flag set
//count_dt = TCNT1L;
//count_dt |= TCNT1H<<8;
//while(count_dt < 8192){

while( softuart_kbhit() ) {
c = softuart_getchar();
Expand All @@ -261,20 +271,26 @@ int main(void)
uint32_t inttp = (uint32_t)PressureBMP280();
printf("PRS %05lx\r\n",(unsigned long int)inttp);
}
/*
if(flag_volume_recv){
// SNPRINTF to get volume
flag_volume_recv = 0;
}
}*/
/*
count_dt = TCNT1L;
count_dt |= TCNT1H<<8;*/
}
//dt = count_dt* (1/1E6);
TIFR1 |= (1 << TOV1);
time += dt;

alt = AltitudeBMP280();
kalman_predict(dt);
kalman_predict(0.01f);
kalman_update(alt);
//rate = 0.7*rate + 0.3*X[1];
rate = X[1];
//printf("%f,%f,%f,%f\r\n",(double)time, (double)alt,(double)X[0],(double) rate);
//printf("%f,%f,%f,%f\r\n",(double)time, (double)X[0],(double)alt,(double) rate);
//printf("B:%u - A:%u\r\n",(unsigned int)b_int,(unsigned int)a_int);


Expand Down

0 comments on commit e690d9b

Please sign in to comment.