Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 17:06:31 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

clean a lot of trailing whitespaces

Browse Source
This commit is contained in:
Felix Ruess
2012-09-13 14:45:10 +02:00
parent 52ede424df
commit 89cd9e4f02
109 changed files with 962 additions and 962 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/button/panic.c
+2 -2
View File
@@ -139,13 +139,13 @@ int main(int argc, char** argv) {
block_nr = strtol(argv[2], NULL, 10);
ml = g_main_loop_new(NULL, FALSE);
IvyInit ("IvyCtrlButton", "IvyCtrlButton READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
btn_init();
g_timeout_add(TIMEOUT_PERIOD, btn_periodic, NULL);
g_timeout_add(TIMEOUT_PERIOD, btn_periodic, NULL);
g_main_loop_run(ml);
btn_close();
sw/in_progress/fdm/booz2_fms_extern.h
+3 -3
View File
@@ -8,9 +8,9 @@
#define FMS_ATTITUDE_OF_DEG(_d) ((int32_t)((_d)*0.0000546))
#define FMS_V_MODE_DIRECT 0
#define FMS_V_MODE_CLIMB 1
#define FMS_V_MODE_HEIGHT 2
#define FMS_V_MODE_DIRECT 0
#define FMS_V_MODE_CLIMB 1
#define FMS_V_MODE_HEIGHT 2
//#define BOOZ2_SEND_IVY_FMS_COMMAND
sw/in_progress/fdm/fdm_step.c
+4 -4
View File
@@ -58,7 +58,7 @@ static gboolean periodic(gpointer data __attribute__ ((unused))) {
pitch = 0;
roll = STEP_VAL;
if ((counter%6) >=3) roll = -roll;
IvySendMsg("dl COMMANDS_RAW %d %d,%d", aircraft_id, roll, pitch);
return TRUE;
}
@@ -67,14 +67,14 @@ static gboolean periodic(gpointer data __attribute__ ((unused))) {
int main ( int argc, char** argv) {
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
parse_args(argc, argv);
IvyInit ("IvyFdmStep", "IvyFdmStep READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
g_main_loop_run(ml);
return 0;
sw/in_progress/fdm/fms_steps_attitude.c
+6 -6
View File
@@ -70,11 +70,11 @@ l_help:
*/
static gboolean periodic(gpointer data __attribute__ ((unused))) {
counter++;
uint8_t h_mode = FMS_H_MODE_ATTITUDE;
uint8_t v_mode = FMS_V_MODE_DIRECT;
uint8_t h_mode = FMS_H_MODE_ATTITUDE;
uint8_t v_mode = FMS_V_MODE_DIRECT;
int32_t pitch = FMS_ATTITUDE_OF_DEG(0);
int32_t roll = FMS_ATTITUDE_OF_DEG(STEP_VAL);
int32_t yaw = FMS_ATTITUDE_OF_DEG(0);
@@ -89,14 +89,14 @@ static gboolean periodic(gpointer data __attribute__ ((unused))) {
int main ( int argc, char** argv) {
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
parse_args(argc, argv);
IvyInit ("IvyBooz2FmsAttStep", "IvyBooz2FmsAttStep READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
g_main_loop_run(ml);
return 0;
sw/in_progress/fdm/fms_steps_position.c
+4 -4
View File
@@ -66,7 +66,7 @@ static gboolean periodic(gpointer data __attribute__ ((unused))) {
pitch = 0;
roll = STEP_VAL;
if ((counter%6) >=3) roll = -roll;
IvySendMsg("dl COMMANDS_RAW %d %d,%d", aircraft_id, roll, pitch);
return TRUE;
}
@@ -75,14 +75,14 @@ static gboolean periodic(gpointer data __attribute__ ((unused))) {
int main ( int argc, char** argv) {
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
parse_args(argc, argv);
IvyInit ("IvyFdmStep", "IvyFdmStep READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
g_timeout_add(TIMEOUT_PERIOD, periodic, NULL);
g_main_loop_run(ml);
return 0;
sw/in_progress/inertial/C/ahrs_data.c
+11 -11
View File
@@ -29,7 +29,7 @@ struct ahrs_data* ahrs_data_new(int len, double dt) {
ad->gyro_p = malloc(ad->nb_samples*sizeof(double));
ad->gyro_q = malloc(ad->nb_samples*sizeof(double));
ad->gyro_r = malloc(ad->nb_samples*sizeof(double));
ad->accel_x = malloc(ad->nb_samples*sizeof(double));
ad->accel_y = malloc(ad->nb_samples*sizeof(double));
ad->accel_z = malloc(ad->nb_samples*sizeof(double));
@@ -45,7 +45,7 @@ struct ahrs_data* ahrs_data_new(int len, double dt) {
ad->est_phi = malloc(ad->nb_samples*sizeof(double));
ad->est_theta = malloc(ad->nb_samples*sizeof(double));
ad->est_psi = malloc(ad->nb_samples*sizeof(double));
ad->est_bias_p = malloc(ad->nb_samples*sizeof(double));
ad->est_bias_q = malloc(ad->nb_samples*sizeof(double));
ad->est_bias_r = malloc(ad->nb_samples*sizeof(double));
@@ -64,7 +64,7 @@ struct ahrs_data* ahrs_data_new(int len, double dt) {
struct ahrs_data* ahrs_data_read_log(const char* filename) {
GError* _err = NULL;
GIOChannel* in_c = g_io_channel_new_file(filename, "r", &_err);
GIOChannel* in_c = g_io_channel_new_file(filename, "r", &_err);
if (_err) {
g_free(_err);
return NULL;
@@ -117,27 +117,27 @@ struct ahrs_data* ahrs_data_read_log(const char* filename) {
ad->t[i] = (double)i * ad->dt;
double* ggx = &g_array_index(ga_gx, double, i);
ad->gyro_p[i] = *ggx;
ad->gyro_p[i] = *ggx;
double* ggy = &g_array_index(ga_gy, double, i);
ad->gyro_q[i] = *ggy;
ad->gyro_q[i] = *ggy;
double* ggz = &g_array_index(ga_gz, double, i);
ad->gyro_r[i] = *ggz;
double* gax = &g_array_index(ga_ax, double, i);
ad->accel_x[i] = *gax;
ad->accel_x[i] = *gax;
double* gay = &g_array_index(ga_ay, double, i);
ad->accel_y[i] = *gay;
ad->accel_y[i] = *gay;
double* gaz = &g_array_index(ga_az, double, i);
ad->accel_z[i] = *gaz;
double* gmx = &g_array_index(ga_mx, double, i);
ad->mag_x[i] = *gmx;
ad->mag_x[i] = *gmx;
double* gmy = &g_array_index(ga_my, double, i);
ad->mag_y[i] = *gmy;
ad->mag_y[i] = *gmy;
double* gmz = &g_array_index(ga_mz, double, i);
ad->mag_z[i] = *gmz;
}
return ad;
}
@@ -184,7 +184,7 @@ void ahrs_data_save_measure(struct ahrs_data* ad, int idx) {
ad->m_phi[idx] = phi_of_accel(ad->accel_x[idx], ad->accel_y[idx], ad->accel_z[idx]);
ad->m_theta[idx] = theta_of_accel(ad->accel_x[idx], ad->accel_y[idx], ad->accel_z[idx]);
ad->m_psi[idx] = psi_of_mag(ad->est_phi[idx], ad->est_theta[idx],
ad->m_psi[idx] = psi_of_mag(ad->est_phi[idx], ad->est_theta[idx],
ad->mag_x[idx], ad->mag_y[idx], ad->mag_z[idx]);
}
sw/in_progress/inertial/C/ahrs_data.h
+1 -1
View File
@@ -10,7 +10,7 @@ struct ahrs_data {
double* phi;
double* theta;
double* psi;
double* bias_p;
double* bias_q;
double* bias_r;
sw/in_progress/inertial/C/ahrs_display.c
+3 -3
View File
@@ -55,7 +55,7 @@ GtkWidget* ahrs_display(struct ahrs_data* ad) {
frame = gtk_frame_new ("angle");
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
databox = gtk_databox_new();
// databox = gtk_databox_create_box_with_scrollbars_and_rulers();
gfloat* f_ephi = g_new0 (gfloat, ad->nb_samples);
@@ -93,7 +93,7 @@ GtkWidget* ahrs_display(struct ahrs_data* ad) {
frame = gtk_frame_new ("biase");
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
databox = gtk_databox_new();
gfloat* f_ebias_p = g_new0 (gfloat, ad->nb_samples);
gfloat* f_ebias_q = g_new0 (gfloat, ad->nb_samples);
@@ -118,7 +118,7 @@ GtkWidget* ahrs_display(struct ahrs_data* ad) {
frame = gtk_frame_new ("covariance");
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
databox = gtk_databox_new();
gfloat* f_p11 = g_new0 (gfloat, ad->nb_samples);
gfloat* f_p22 = g_new0 (gfloat, ad->nb_samples);
sw/in_progress/inertial/C/ahrs_euler_ekf.c
+10 -10
View File
@@ -16,7 +16,7 @@ static int ahrs_state;
void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) {
*dt = ad->dt;
double p = u[0] - X[3];
double q = u[1] - X[4];
double r = u[2] - X[5];
@@ -24,7 +24,7 @@ void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) {
double phi = X[0];
double theta = X[1];
// double psi = X[2];
Xdot[0] = p + sin(phi)*tan(theta) * q + cos(phi)*tan(theta) * r;
Xdot[1] = cos(phi) * q - sin(phi) * r;
Xdot[2] = sin(phi)/cos(theta) * q + cos(phi)/cos(theta) * r;
@@ -49,28 +49,28 @@ void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) {
F[2*6+0] = cos(phi)/cos(theta)*q - sin(phi)/cos(theta)*r;
F[2*6+1] = sin(theta)/(cos(theta)*cos(theta))*(sin(phi)*q+cos(phi)*r);
F[2*6+2] = 0.;
F[2*6+3] = 0.;
F[2*6+3] = 0.;
F[2*6+4] = -sin(phi)/cos(theta);
F[2*6+5] = -cos(phi)/cos(theta);
F[2*6+5] = -cos(phi)/cos(theta);
F[3*6+0] = 0.;
F[3*6+1] = 0.;
F[3*6+2] = 0.;
F[3*6+3] = 0.;
F[3*6+3] = 0.;
F[3*6+4] = 0.;
F[3*6+5] = 0.;
F[4*6+0] = 0.;
F[4*6+1] = 0.;
F[4*6+2] = 0.;
F[4*6+3] = 0.;
F[4*6+3] = 0.;
F[4*6+4] = 0.;
F[4*6+5] = 0.;
F[5*6+0] = 0.;
F[5*6+1] = 0.;
F[5*6+2] = 0.;
F[5*6+3] = 0.;
F[5*6+3] = 0.;
F[5*6+4] = 0.;
F[5*6+5] = 0.;
@@ -115,14 +115,14 @@ void linear_measure(double *y, double* err, double*X, double *H) {
void run_ekf (void) {
/* initial state covariance matrix */
double P[6*6] = {P0E, 0.0, 0.0, 0.0, 0.0, 0.0,
double P[6*6] = {P0E, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, P0E, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, P0E, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, P0B, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, P0B, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, P0B };
/* model noise covariance matrix */
double Q[6*6] = { QE, 0.0, 0.0, 0.0, 0.0, 0.0,
double Q[6*6] = { QE, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, QE, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, QE, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, QB, 0.0, 0.0,
@@ -136,7 +136,7 @@ void run_ekf (void) {
double Y[1];
/* command */
double U[3] = {0.0, 0.0, 0.0};
struct ekf_filter* ekf;
ekf = ekf_filter_new(6, 1, Q, R, linear_filter, linear_measure);
ahrs_euler_init(ad, 150, X);
sw/in_progress/inertial/C/ahrs_quat_ekf.c
+5 -5
View File
@@ -58,8 +58,8 @@ void linear_filter(double *u, double* X, double* dt, double *Xdot, double* F) {
p , 0., r, -q, -q0, q3, -q2,
q, -r, 0., p, -q3, -q0, q1,
r, q, -p, 0., q2, -q1, -q0,
0., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0.,
0., 0., 0., 0., 0., 0., 0. };
memcpy(F, my_f, sizeof(my_f));
@@ -121,7 +121,7 @@ void linear_measure(double *y, double* err, double*X, double *H) {
void run_ekf (void) {
/* initial state covariance matrix */
double P[7*7] = {P0Q, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
double P[7*7] = {P0Q, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, P0Q, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, P0Q, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, P0Q, 0.0, 0.0, 0.0,
@@ -129,7 +129,7 @@ void run_ekf (void) {
0.0, 0.0, 0.0, 0.0, 0.0, P0B, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, P0B };
/* model noise covariance matrix */
double Q[7*7] = {Q0G, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
double Q[7*7] = {Q0G, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, Q0G, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, Q0G, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, Q0G, 0.0, 0.0, 0.0,
@@ -144,7 +144,7 @@ void run_ekf (void) {
double Y[1];
/* command */
double U[3] = {0.0, 0.0, 0.0};
struct ekf_filter* ekf;
ekf = ekf_filter_new(7, 1, Q, R, linear_filter, linear_measure);
ahrs_quat_init(ad, 150, X);
sw/in_progress/inertial/C/ahrs_quat_fast_ekf.c
+4 -4
View File
@@ -9,7 +9,7 @@
#define afe_dt 0.0166667
/* We have seven variables in our state -- the quaternion attitude
* estimate and three gyro bias values
* estimate and three gyro bias values
*/
FLOAT_T afe_q0, afe_q1, afe_q2, afe_q3;
FLOAT_T afe_bias_p, afe_bias_q, afe_bias_r;
@@ -127,7 +127,7 @@ void afe_predict( const FLOAT_T* gyro ) {
afe_q = gyro[1] - afe_bias_q;
afe_r = gyro[2] - afe_bias_r;
/* compute F
/* compute F
F is only needed later on to update the state covariance P.
However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
compute the time derivative of the quaternion, so we compute F now */
@@ -157,7 +157,7 @@ void afe_predict( const FLOAT_T* gyro ) {
afe_q1_dot = afe_F[1][0] * afe_q0 + afe_F[1][2] * afe_q2 + afe_F[1][3] * afe_q3;
afe_q2_dot = afe_F[2][0] * afe_q0 + afe_F[2][1] * afe_q1 + afe_F[2][3] * afe_q3;
afe_q3_dot = afe_F[3][0] * afe_q0 + afe_F[3][1] * afe_q1 + afe_F[3][2] * afe_q2;
/* quat = quat + quat_dot * dt */
afe_q0 += afe_q0_dot * afe_dt;
afe_q1 += afe_q1_dot * afe_dt;
@@ -256,7 +256,7 @@ void afe_predict( const FLOAT_T* gyro ) {
/*
*
*
*/
static void run_kalman( const FLOAT_T R_axis, const FLOAT_T error ) {
int i, j;
sw/in_progress/inertial/C/ahrs_quat_fast_ekf_utils.h
+10 -10
View File
@@ -96,11 +96,11 @@ static inline FLOAT_T afe_phi_of_accel( const FLOAT_T* accel ) {
}
static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
FLOAT_T g2 =
accel[AXIS_X]*accel[AXIS_X] +
accel[AXIS_Y]*accel[AXIS_Y] +
FLOAT_T g2 =
accel[AXIS_X]*accel[AXIS_X] +
accel[AXIS_Y]*accel[AXIS_Y] +
accel[AXIS_Z]*accel[AXIS_Z];
return -asin( accel[AXIS_X] / sqrt( g2 ) );
return -asin( accel[AXIS_X] / sqrt( g2 ) );
}
/*
* The rotation matrix to rotate from NED frame to body frame without
@@ -116,7 +116,7 @@ static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
* [ sin(Phi)*sin(Theta) cos(Phi) sin(Phi)*cos(Theta)]
* [ cos(Phi)*sin(Theta) -sin(Phi) cos(Phi)*cos(Theta)]
*
* However, to untilt the compass reading, we need to use the
* However, to untilt the compass reading, we need to use the
* transpose of this matrix.
*
* [ cos(Theta) sin(Phi)*sin(Theta) cos(Phi)*sin(Theta) ]
@@ -136,9 +136,9 @@ static inline FLOAT_T afe_theta_of_accel( const FLOAT_T* accel) {
static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) {
const FLOAT_T ctheta = cos( afe_theta );
#if 0
const FLOAT_T mn = ctheta * mag[0]
const FLOAT_T mn = ctheta * mag[0]
- (afe_dcm12 * mag[1] + afe_dcm22 * mag[2]) * afe_dcm02 / ctheta;
const FLOAT_T me =
(afe_dcm22 * mag[1] - afe_dcm12 * mag[2]) / ctheta;
#else
@@ -146,16 +146,16 @@ static inline FLOAT_T afe_psi_of_mag( const int16_t* mag ) {
const FLOAT_T cphi = cos( afe_phi );
const FLOAT_T sphi = sin( afe_phi );
const FLOAT_T mn =
const FLOAT_T mn =
ctheta* mag[0]+
sphi*stheta* mag[1]+
cphi*stheta* mag[2];
const FLOAT_T me =
const FLOAT_T me =
0* mag[0]+
cphi* mag[1]+
-sphi* mag[2];
#endif
const FLOAT_T psi = -atan2( me, mn );
return psi;
}
sw/in_progress/inertial/C/ahrs_quat_ukf.c
+3 -3
View File
@@ -40,7 +40,7 @@ void linear_filter(double *x1, double *x0, double *u) {
x1[6] = x0[6];
norm_quat(x1);
}
}
void linear_measure(double *y, double *x) {
double eulers[3];
@@ -52,7 +52,7 @@ void linear_measure(double *y, double *x) {
void run_ukf(void) {
/* initial state covariance matrix */
double P[7*7] = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
double P[7*7] = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
@@ -60,7 +60,7 @@ void run_ukf(void) {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
/* model noise covariance matrix */
double Q[7*7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
double Q[7*7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
sw/in_progress/inertial/C/ahrs_utils.c
+5 -5
View File
@@ -17,11 +17,11 @@ extern double psi_of_mag(double phi, double theta, double mx, double my, double
const float sphi = sin( phi );
const float ctheta = cos( theta );
const float stheta = sin( theta );
const float mn =
const float mn =
ctheta * mx +
sphi * stheta * my +
cphi * stheta * mz;
const float me =
const float me =
0 * mx+
cphi * my+
-sphi * mz;
@@ -61,7 +61,7 @@ void eulers_of_quat(double* euler, double* quat) {
}
void norm_quat(double* quat) {
double mag = quat[0]*quat[0] + quat[1]*quat[1] +
double mag = quat[0]*quat[0] + quat[1]*quat[1] +
quat[2]*quat[2] + quat[3]*quat[3];
mag = sqrt( mag );
quat[0] /= mag;
@@ -82,8 +82,8 @@ void ahrs_euler_init(struct ahrs_data* ad, int len, double* X) {
int iter;
for (iter = 0; iter < len; iter++) {
/* average attitude */
init_phi += phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
init_theta += theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
init_phi += phi_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
init_theta += theta_of_accel(ad->accel_x[iter], ad->accel_y[iter], ad->accel_z[iter]);
init_mx += ad->mag_x[iter];
init_my += ad->mag_y[iter];
init_mz += ad->mag_z[iter];
sw/in_progress/inertial/C/ekf.c
+15 -15
View File
@@ -9,7 +9,7 @@
struct ekf_filter {
unsigned state_dim;
unsigned measure_dim;
/* state */
double* X;
/* state covariance matrix */
@@ -36,7 +36,7 @@ struct ekf_filter {
double* tmp1;
double* tmp2;
double* tmp3;
};
@@ -46,7 +46,7 @@ struct ekf_filter* ekf_filter_new(unsigned state_dim,
double* R,
filter_function ffun,
measure_function mfun) {
struct ekf_filter* ekf = malloc(sizeof(struct ekf_filter));
ekf->state_dim = state_dim;
ekf->measure_dim = measure_dim;
@@ -63,7 +63,7 @@ struct ekf_filter* ekf_filter_new(unsigned state_dim,
ekf->tmp3 = malloc( n * sizeof(double));
ekf->Q = malloc( n * sizeof(double));
memcpy(ekf->Q, Q, n * sizeof(double));
memcpy(ekf->Q, Q, n * sizeof(double));
n = ekf->measure_dim * ekf->measure_dim;
ekf->R = malloc( n * sizeof(double));
@@ -74,7 +74,7 @@ struct ekf_filter* ekf_filter_new(unsigned state_dim,
n = ekf->measure_dim * ekf->state_dim;
ekf->H = malloc( n * sizeof(double));
n = ekf->measure_dim * ekf->measure_dim;
ekf->E = malloc( n * sizeof(double));
@@ -90,8 +90,8 @@ struct ekf_filter* ekf_filter_new(unsigned state_dim,
void ekf_filter_reset(struct ekf_filter *filter, double *x0, double *P0) {
memcpy(filter->X, x0, filter->state_dim * sizeof(double));
memcpy(filter->P, P0, filter->state_dim * filter->state_dim * sizeof(double));
memcpy(filter->X, x0, filter->state_dim * sizeof(double));
memcpy(filter->P, P0, filter->state_dim * filter->state_dim * sizeof(double));
}
void
@@ -103,7 +103,7 @@ ekf_filter_get_state(struct ekf_filter* filter, double *X, double* P){
void ekf_filter_predict(struct ekf_filter* filter, double *u) {
/* prediction :
X += Xdot * dt
Pdot = F * P * F' + Q ( or Pdot = F*P + P*F' + Q for continuous form )
P += Pdot * dt
@@ -115,12 +115,12 @@ void ekf_filter_predict(struct ekf_filter* filter, double *u) {
/* fetch dt, Xdot and F */
filter->ffun(u, filter->X, &dt, filter->Xdot, filter->F);
/* X = X + Xdot * dt */
mat_add_scal_mult(n, 1, filter->X, filter->X, dt, filter->Xdot);
mat_add_scal_mult(n, 1, filter->X, filter->X, dt, filter->Xdot);
#ifdef EKF_UPDATE_CONTINUOUS
/*
/*
continuous update
Pdot = F * P + P * F' + Q
Pdot = F * P + P * F' + Q
*/
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
mat_transpose(n, n, filter->tmp2, filter->F);
@@ -129,9 +129,9 @@ void ekf_filter_predict(struct ekf_filter* filter, double *u) {
mat_add(n, n, filter->Pdot, filter->Pdot, filter->Q);
#endif
#ifdef EKF_UPDATE_DISCRETE
/*
/*
discrete update
Pdot = F * P * F' + Q
Pdot = F * P * F' + Q
*/
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
mat_transpose(n, n, filter->tmp2, filter->F);
@@ -140,7 +140,7 @@ void ekf_filter_predict(struct ekf_filter* filter, double *u) {
#endif
/* P = P + Pdot * dt */
mat_add_scal_mult(n, n, filter->P, filter->P, dt, filter->Pdot);
mat_add_scal_mult(n, n, filter->P, filter->P, dt, filter->Pdot);
}
@@ -177,6 +177,6 @@ void ekf_filter_update(struct ekf_filter* filter, double *y) {
mat_sub(n, n, filter->P, filter->P, filter->tmp2);
/* X = X + err * K */
mat_add_scal_mult(n, m, filter->X, filter->X, err, filter->K);
mat_add_scal_mult(n, m, filter->X, filter->X, err, filter->K);
}
sw/in_progress/inertial/C/linalg.c
+5 -5
View File
@@ -6,14 +6,14 @@ void
ukf_cholesky_decomposition(double *A, unsigned n, double *sigma) {
unsigned i,j,k;
double t;
for(i = 0 ; i < n ; i++) {
if(i > 0) {
for(j = i ; j < n ; j++) {
t = 0.0;
for(k = 0 ; k < i ; k++)
t += A[j * n + k] * A[i * n + k];
A[j * n + i] -= t;
A[j * n + i] -= t;
}
}
if(A[i * n + i] <= 0.0) {
@@ -32,7 +32,7 @@ void
ukf_cholesky_solve(double *A, unsigned n, double *sigma, double *B, unsigned m, double *X) {
int i,j,k;
double t;
for(i = 0 ; i < m ; i++) { // iterate over the lines of B
for(j = 0 ; j < n ; j++) { // solve Ly=B
t = B[i * n + j];
@@ -46,7 +46,7 @@ ukf_cholesky_solve(double *A, unsigned n, double *sigma, double *B, unsigned m,
t -= A[k * n + j] * X[i * n + k];
X[i * n + j] = t / sigma[j];
}
}
}
}
@@ -54,7 +54,7 @@ void
ukf_cholesky_invert(double *A, unsigned n, double *sigma) {
double t;
int i,j,k;
for(i = 0 ; i < n ; i++) {
A[i * n + i] = 1.0 / sigma[i];
for(j = i + 1 ; j < n ; j++) {
sw/in_progress/inertial/C/linalg.h
+3 -3
View File
@@ -15,7 +15,7 @@ ukf_cholesky_decomposition(double *A, unsigned n, double *sigma);
/*
* Solve the linear system AX^t=B^t given the cholesky decomposition of A
* @param A: the cholesky decomposition of A, as given by the
* @param A: the cholesky decomposition of A, as given by the
* routine ukf_cholesky_decomposition
* @param n: the size of the problem
* @param sigma: the vector of diagonal elements of A
@@ -23,7 +23,7 @@ ukf_cholesky_decomposition(double *A, unsigned n, double *sigma);
* @param m: the number of simultaneou systems to solve
* @param X: a matrix holding the result
*/
void
ukf_cholesky_solve(double *A, unsigned n, double *sigma, double *B, unsigned m, double *X);
@@ -33,7 +33,7 @@ ukf_cholesky_solve(double *A, unsigned n, double *sigma, double *B, unsigned m,
* Lower triangle of the inverse of the cholesky decomposition.
* On exit, the lower triangle of A is overwritten with the lower
* triangle of the inverse.
* @param A: the cholesky decomposition of A, as given by the
* @param A: the cholesky decomposition of A, as given by the
* routine ukf_cholesky_decomposition
* @param n: the size of the problem
* @param sigma: the vector of diagonal elements of A
sw/in_progress/inertial/C/matrix.c
+1 -1
View File
@@ -57,7 +57,7 @@ void mat_mult(int n_rowa, int n_cola, int n_colb, double* r, double* a, double*
int row, col, k;
for (row = 0; row<n_rowa; row++) {
for (col = 0; col<n_colb; col++) {
int ridx = col + row * n_colb;
int ridx = col + row * n_colb;
r[ridx] =0.;
for (k=0; k<n_cola; k++) {
int aidx = k + row * n_cola;
sw/in_progress/inertial/C/random.c
+1 -1
View File
@@ -8,7 +8,7 @@ ukf_filter_rand_normal() {
double norm2,x,y;
double bm;
double saved_value;
if(is_ready == 0) {
do {
x = 2.0 * drand48() - 1.0;
sw/in_progress/inertial/C/random.h
+1 -1
View File
@@ -3,7 +3,7 @@
/*
* Random number generation
*/
/*
* Generates a random gaussian number
sw/in_progress/inertial/C/tilt_data.c
+7 -7
View File
@@ -10,7 +10,7 @@
#include <glib.h>
#define DT 0.01
#define T_END 90.
#define T_END 90.
#define NB_POINTS (int)(T_END/DT)
#define T0 10.
#define T1 11.
@@ -41,7 +41,7 @@ struct tilt_data* tilt_data_gen(void) {
td->rate[i] = MAX_RATE * ( -1. + cos((td->t[i] - T1)/DELTA_T2*2*M_PI));
td->angle[i] = td->angle[i-1] + td->rate[i] * td->dt;
}
td->bias[i] = td->t[i] < 30. ? BIAS_GYRO * ( 1. + td->t[i] * DBIAS_GYRO) :
td->bias[i] = td->t[i] < 30. ? BIAS_GYRO * ( 1. + td->t[i] * DBIAS_GYRO) :
BIAS_GYRO * ( 1. + 30. * DBIAS_GYRO);
}
@@ -67,7 +67,7 @@ void tilt_data_save_state(struct tilt_data* td, int idx, double* X, double* P) {
struct tilt_data* tilt_data_read_log(const char* filename) {
GError* _err = NULL;
GIOChannel* in_c = g_io_channel_new_file(filename, "r", &_err);
GIOChannel* in_c = g_io_channel_new_file(filename, "r", &_err);
if (_err) {
g_free(_err);
return NULL;
@@ -102,14 +102,14 @@ struct tilt_data* tilt_data_read_log(const char* filename) {
int i;
for (i=0; i<ga_gyro->len; i++) {
double* ggx = &g_array_index(ga_gyro, double, i);
td->gyro[i] = *ggx;
td->gyro[i] = *ggx;
double* gay = &g_array_index(ga_ay, double, i);
td->ay[i] = *gay;
td->ay[i] = *gay;
double* gaz = &g_array_index(ga_az, double, i);
td->az[i] = *gaz;
td->az[i] = *gaz;
td->m_angle[i] = atan2(td->ay[i], td->az[i]);
}
return td;
}
sw/in_progress/inertial/C/tilt_display.c
+8 -8
View File
@@ -47,11 +47,11 @@ GtkWidget* tilt_display(struct tilt_data* td) {
gtk_container_add (GTK_CONTAINER (frame), databox );
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
frame = gtk_frame_new ("angle");
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
databox = gtk_databox_new();
gfloat* pangle = g_new0 (gfloat, td->nb_samples);
@@ -62,26 +62,26 @@ GtkWidget* tilt_display(struct tilt_data* td) {
pm_angle[idx] = td->m_angle[idx];
pe_angle[idx] = td->est_angle[idx];
}
GtkDataboxGraph *gangle = gtk_databox_lines_new (td->nb_samples, pt, pangle, &red, 1);
gtk_databox_graph_add (GTK_DATABOX(databox), gangle);
GtkDataboxGraph *ge_angle = gtk_databox_lines_new (td->nb_samples, pt, pe_angle, &blue, 1);
gtk_databox_graph_add (GTK_DATABOX(databox), ge_angle);
GtkDataboxGraph *gm_angle = gtk_databox_lines_new (td->nb_samples, pt, pm_angle, &green, 1);
gtk_databox_graph_add (GTK_DATABOX(databox), gm_angle);
grid = gtk_databox_grid_new (10, 10, &black, 1);
gtk_databox_graph_add (GTK_DATABOX(databox), grid);
gtk_container_add (GTK_CONTAINER (frame), databox );
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
frame = gtk_frame_new ("bias");
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
databox = gtk_databox_new();
gfloat* pbias = g_new0 (gfloat, td->nb_samples);
@@ -102,10 +102,10 @@ GtkWidget* tilt_display(struct tilt_data* td) {
gtk_container_add (GTK_CONTAINER (frame), databox );
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
frame = gtk_frame_new ("covariance");
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
databox = gtk_databox_new();
gfloat* pP00 = g_new0 (gfloat, td->nb_samples);
sw/in_progress/inertial/C/tilt_ekf.c
+6 -6
View File
@@ -6,7 +6,7 @@
#include "ekf.h"
/*
Simple 2 state filter for hybridizing gyrometer and accelerometer
Simple 2 state filter for hybridizing gyrometer and accelerometer
on one axis.
The filter wil track the gyro bias.
@@ -18,14 +18,14 @@ static struct tilt_data* td;
static int iter;
void linear_filter(double *u, double* x, double* dt, double *Xdot, double* F) {
/* state prediction
/* state prediction
angle += angle_dot * dt
bias += 0
*/
*dt = td->dt;
Xdot[0] = (u[0] - x[1]);
Xdot[1] = 0.;
/* Jacobian of xdot wrt the state
/* Jacobian of xdot wrt the state
F = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ]
[ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]
*/
@@ -46,15 +46,15 @@ void linear_measure(double *y, double* err, double*x, double *H) {
void run_ekf(struct tilt_data* td) {
/* model noise covariance matrix */
double Q[4]={0.001, 0.0,
double Q[4]={0.001, 0.0,
0.0, 0.003};
/* measurement noise covariance matrix */
double R[1]={1.7};
/* initial x */
double X0[2] = {0.0, 0.0};
/* initial state covariance matrix */
double P0[4] = {1.0, 0.0,
0.0, 1.0};
double P0[4] = {1.0, 0.0,
0.0, 1.0};
/* measure */
double y[1];
/* command */
sw/in_progress/inertial/C/tilt_ekf_optim.c
+11 -11
View File
@@ -16,7 +16,7 @@ void run_tilt(void) {
double P[4] = { 1., 0.,
0., 1. };
/* model noise covariance matrix */
double Q[4]={0.001, 0.0,
double Q[4]={0.001, 0.0,
0.0, 0.003 };
/* jacobian of the measure wrt X */
const double H[2] = { 1., 0. };
@@ -24,7 +24,7 @@ void run_tilt(void) {
const double R = 0.5;
tilt_init(td, 150, X);
int iter;
for (iter=0; iter<td->nb_samples; iter++) {
@@ -33,7 +33,7 @@ void run_tilt(void) {
X[0] += rate * td->dt;
#ifdef EKF_UPDATE_CONTINUOUS
/* Pdot = F*P + P*F' + Q
/* Pdot = F*P + P*F' + Q
* F = { 1, 0,
* 0, 0 };
*/
@@ -46,17 +46,17 @@ void run_tilt(void) {
0. , Q[1*2 + 1] };
#endif
/* P += Pdot * dt */
P[0*2 + 0] += Pdot[0*2 + 0] * td->dt;
P[0*2 + 1] += Pdot[0*2 + 1] * td->dt;
P[1*2 + 0] += Pdot[1*2 + 0] * td->dt;
P[1*2 + 1] += Pdot[1*2 + 1] * td->dt;
P[0*2 + 0] += Pdot[0*2 + 0] * td->dt;
P[0*2 + 1] += Pdot[0*2 + 1] * td->dt;
P[1*2 + 0] += Pdot[1*2 + 0] * td->dt;
P[1*2 + 1] += Pdot[1*2 + 1] * td->dt;
/* E = H * P * H' + R */
const double PHt_0 = P[0*2 + 0] * H[0]; // + P[0*2 + 1] * H[1]
const double PHt_1 = P[1*2 + 0] * H[0]; // + P[1*2 + 1] * H[1]
const double E = H[0] * PHt_0 + // H[1] * PHt1
R;
/* K = P * H' * inv(E) */
const double K_0 = PHt_0 / E;
const double K_1 = PHt_1 / E;
@@ -75,7 +75,7 @@ void run_tilt(void) {
X[1] += K_1 * err;
tilt_data_save_state(td, iter, X, P);
}
}
@@ -90,7 +90,7 @@ int main(int argc, char *argv[]) {
//td = tilt_data_read_log("data/log_ahrs_yaw_pitched");
run_tilt();
tilt_display(td);
gtk_main();
sw/in_progress/inertial/C/tilt_fast_ekf.c
+11 -11
View File
@@ -16,7 +16,7 @@ void run_tilt(void) {
double P[4] = { 1., 0.,
0., 1. };
/* model noise covariance matrix */
double Q[4]={0.001, 0.0,
double Q[4]={0.001, 0.0,
0.0, 0.003 };
/* jacobian of the measure wrt X */
const double H[2] = { 1., 0. };
@@ -24,7 +24,7 @@ void run_tilt(void) {
const double R = 0.5;
tilt_init(td, 150, X);
int iter;
for (iter=0; iter<td->nb_samples; iter++) {
@@ -33,7 +33,7 @@ void run_tilt(void) {
X[0] += rate * td->dt;
#ifdef EKF_UPDATE_CONTINUOUS
/* Pdot = F*P + P*F' + Q
/* Pdot = F*P + P*F' + Q
* F = { 1, 0,
* 0, 0 };
*/
@@ -46,17 +46,17 @@ void run_tilt(void) {
0. , Q[1*2 + 1] };
#endif
/* P += Pdot * dt */
P[0*2 + 0] += Pdot[0*2 + 0] * td->dt;
P[0*2 + 1] += Pdot[0*2 + 1] * td->dt;
P[1*2 + 0] += Pdot[1*2 + 0] * td->dt;
P[1*2 + 1] += Pdot[1*2 + 1] * td->dt;
P[0*2 + 0] += Pdot[0*2 + 0] * td->dt;
P[0*2 + 1] += Pdot[0*2 + 1] * td->dt;
P[1*2 + 0] += Pdot[1*2 + 0] * td->dt;
P[1*2 + 1] += Pdot[1*2 + 1] * td->dt;
/* E = H * P * H' + R */
const double PHt_0 = P[0*2 + 0] * H[0]; // + P[0*2 + 1] * H[1]
const double PHt_1 = P[1*2 + 0] * H[0]; // + P[1*2 + 1] * H[1]
const double E = H[0] * PHt_0 + // H[1] * PHt1
R;
/* K = P * H' * inv(E) */
const double K_0 = PHt_0 / E;
const double K_1 = PHt_1 / E;
@@ -75,7 +75,7 @@ void run_tilt(void) {
X[1] += K_1 * err;
tilt_data_save_state(td, iter, X, P);
}
}
@@ -90,7 +90,7 @@ int main(int argc, char *argv[]) {
//td = tilt_data_read_log("../data/log_ahrs_yaw_pitched");
run_tilt();
tilt_display(td);
gtk_main();
sw/in_progress/inertial/C/tilt_ukf.c
+3 -3
View File
@@ -6,7 +6,7 @@
#include "ukf.h"
/*
Simple 2 state filter for hybridizing gyrometer and accelerometer
Simple 2 state filter for hybridizing gyrometer and accelerometer
on one axis.
The filter wil track the gyro bias.
@@ -32,14 +32,14 @@ void linear_measure(double *y, double *x) {
void run_ukf(struct tilt_data* td) {
/* model noise covariance matrix */
double Q[4]={0.001, 0.0,
double Q[4]={0.001, 0.0,
0.0, 0.003 };
/* measurement noise covariance matrix */
double R[1]={0.5};
/* initial x */
double x[2] = {0.0, 0.0};
/* initial state covariance matrix */
double P[4] = {1.0, 0.0,
double P[4] = {1.0, 0.0,
0.0, 1.0};
/* measure */
double y[1];
sw/in_progress/inertial/C/ukf.c
+76 -76
View File
@@ -24,7 +24,7 @@ struct ukf_filter_t {
measure_function mfun;
// all fields below are specific to UKF
// weights for computing the mean
double *wm;
double *wm;
// weights for computing the covariance
double *wc;
// scaling parameter
@@ -58,8 +58,8 @@ ukf_safe_free(void *p) {
*/
ukf_filter
ukf_filter_new(unsigned state_dim,
unsigned measure_dim,
ukf_filter_new(unsigned state_dim,
unsigned measure_dim,
double *Q,
double *R,
filter_function ffun,
@@ -73,88 +73,88 @@ ukf_filter_new(unsigned state_dim,
// alloc new structure
filter = malloc(sizeof(struct ukf_filter_t));
// returns 0 if allocation fails
if(filter == 0)
if(filter == 0)
return 0;
// fills the structure
filter->state_dim = state_dim;
filter->measure_dim = measure_dim;
filter->ffun = ffun;
filter->mfun = mfun;
filter->x = malloc(state_dim * sizeof(double));
err |= (filter->x == 0);
filter->y = malloc(measure_dim * sizeof(double));
err |= (filter->y == 0);
n = state_dim * state_dim;
filter->P = malloc(n * sizeof(double));
err |= (filter->P == 0);
n = 2 * state_dim + 1;
filter->wm = malloc(n * sizeof(double));
err |= (filter->wm == 0);
filter->wc = malloc(n * sizeof(double));
err |= (filter->wc == 0);
filter->sigma_point = malloc(n * state_dim * sizeof(double));
err |= (filter->sigma_point == 0);
n = filter->state_dim;
filter->sigma = malloc(n * sizeof(double));
err |= (filter->sigma == 0);
filter->PM = malloc(n * n * sizeof(double));
err |= (filter->PM == 0);
filter->PM_save = malloc(n * n * sizeof(double));
err |= (filter->PM == 0);
filter->xm = malloc(n * sizeof(double));
err |= (filter->xm == 0);
filter->ym = malloc(filter->measure_dim * sizeof(double));
err |= (filter->ym == 0);
n = 2 * filter->state_dim + 1;
filter->khi = malloc(n * filter->state_dim * sizeof(double));
err |= (filter->khi == 0);
filter->khi_y = malloc(n * filter->measure_dim * sizeof(double));
err |= (filter->khi_y == 0);
n = filter->measure_dim;
filter->Pyy = malloc(n * n * sizeof(double));
err |= (filter->Pyy == 0);
filter->Pxy = malloc(n * filter->state_dim * sizeof(double));
err |= (filter->Pxy == 0);
filter->dx = malloc(filter->state_dim * sizeof(double));
err |= (filter->dx == 0);
filter->dy = malloc(filter->measure_dim * sizeof(double));
err |= (filter->dy == 0);
filter->gain = malloc(filter->state_dim * filter->measure_dim * sizeof(double));
err |= (filter->gain == 0);
filter->sigma_y = malloc(filter->measure_dim * sizeof(double));
err |= (filter->sigma_y == 0);
filter->KL = malloc(filter->state_dim * filter->measure_dim * sizeof(double));
err |= (filter->KL == 0);
if(err != 0) {
ukf_filter_delete(filter);
return 0;
}
n = filter->state_dim;
filter->Q = malloc(n * n * sizeof(double));
if(filter->Q == 0) {
@@ -162,7 +162,7 @@ ukf_filter_new(unsigned state_dim,
return 0;
}
memcpy(filter->Q, Q, n * n * sizeof(double));
n = filter->measure_dim;
filter->R = malloc(n * n * sizeof(double));
if(filter->R == 0) {
@@ -170,7 +170,7 @@ ukf_filter_new(unsigned state_dim,
return 0;
}
memcpy(filter->R, R, n * n * sizeof(double));
// returns the newly allocated structure
return filter;
}
@@ -214,18 +214,18 @@ ukf_filter_delete(ukf_filter filter) {
void
ukf_filter_compute_weights(ukf_filter filter,
double alpha,
double k,
double alpha,
double k,
double beta) {
double l;
double lam;
unsigned i;
if(filter == 0) return;
l = (double)filter->state_dim;
// lambda parameter
lam = alpha * alpha *( l + k) - l;
filter->wm[0] = lam / (lam + l);
filter->wc[0] = filter->wm[0] + (1.0 - alpha * alpha + beta);
for(i = 1 ; i <= 2*filter->state_dim ; i++) {
@@ -240,12 +240,12 @@ ukf_filter_compute_weights(ukf_filter filter,
* ukf_filter_reset
*/
void
void
ukf_filter_reset(ukf_filter filter,
double *x0,
double *P0) {
if(filter != 0) {
// state of the filter
// state of the filter
memcpy(filter->x, x0, filter->state_dim * sizeof(double));
memcpy(filter->P, P0,
filter->state_dim * filter->state_dim * sizeof(double));
@@ -275,11 +275,11 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
int m = filter->measure_dim;
int i,j,k;
double t;
// cholesky decomposition of the state covariance matrix
ukf_cholesky_decomposition(filter->P, l, filter->sigma);
//=================================
// compute sigma points
for(j = 0 ; j < l ; j++)
@@ -296,14 +296,14 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
filter->sigma_point[(i + 1 + l) * l + j] = filter->x[j] - filter->gamma * filter->P[j * l + i];
}
}
//=================================
// propagate sigma points
for(i = 0 ; i < 2 * l + 1 ; i++) {
filter->ffun(&(filter->khi[i * l]) , &(filter->sigma_point[i * l]) , u);
}
// compute state prediction xm
for(i = 0 ; i < l ; i++) {
filter->xm[i] = filter->wm[0] * filter->khi[i];
@@ -311,14 +311,14 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
filter->xm[i] += filter->wm[j] * filter->khi[j * l + i];
}
}
//================================
// time update
// start with state covariance matrix
for(i = 0 ; i < l * l ; i++)
filter->PM[i] = filter->Q[i];
// accumulate covariances
for(i = 0 ; i < 2 * l + 1 ; i++) {
for(j = 0 ; j < l ; j++)
@@ -327,18 +327,18 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
for(k = 0 ; k < l ; k++) {
filter->PM[j * l + k] += filter->wc[i] * filter->dx[j] * filter->dx[k];
}
}
}
}
// save PM matrix
for(i = 0 ; i < l * l ; i++)
filter->PM_save[i] = filter->PM[i];
// redraw sigma points
ukf_cholesky_decomposition(filter->PM, l, filter->sigma);
for(j = 0 ; j < l ; j++)
filter->sigma_point[j]= filter->xm[j];
for(i = 0 ; i < l ; i++) {
@@ -354,33 +354,33 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
}
}
//=================================
// propagate measurement
for(i = 0 ; i < 2 * l + 1 ; i++) {
filter->mfun(&(filter->khi_y[i * m]) , &(filter->sigma_point[i * l]) );
}
// measurement prediction
// measurement prediction
for(i = 0 ; i < m ; i++) {
filter->ym[i] = filter->wm[0] * filter->khi_y[i];
for(j = 1 ; j < 2 * l + 1 ; j++)
filter->ym[i] += filter->wm[j] * filter->khi_y[j * m + i];
}
// measurement update
// Pyy matrix
// start with measure covariance matrix
for(i = 0 ; i < m * m ; i++)
filter->Pyy[i] = filter->R[i];
// accumulate covariances
for(i = 0 ; i < 2 * l + 1 ; i++) {
for(j = 0 ; j < m ; j++)
filter->dy[j]= filter->khi_y[i * m + j] - filter->ym[j];
@@ -389,14 +389,14 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
filter->Pyy[j * m + k] += filter->wc[i] * filter->dy[j] * filter->dy[k];
}
}
// Pxy matrix
for(i = 0 ; i < m * l ; i++)
filter->Pxy[i] = 0.0;
// accumulate covariances
for(i = 0 ; i < 2 * l + 1 ; i++) {
for(j = 0 ; j < m ; j++) {
filter->dy[j]= filter->khi_y[i * m + j] - filter->ym[j];
@@ -409,19 +409,19 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
filter->Pxy[j * m + k] += filter->wc[i] * filter->dx[j] * filter->dy[k];
}
}
// gain de kalman
ukf_cholesky_decomposition(filter->Pyy, m, filter->sigma_y);
ukf_cholesky_solve(filter->Pyy, m, filter->sigma_y, filter->Pxy, l, filter->gain);
// restore PM matrix
for(i = 0 ; i < l * l ; i++)
filter->P[i]= filter->PM_save[i];
// update state
for(j = 0 ; j < m ; j++)
filter->dy[j] = y[j] - filter->ym[j];
for(i = 0 ; i < l ; i++) {
@@ -431,7 +431,7 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
t += filter->gain[i * m + j] * filter->dy[j];
filter->x[i] += t;
}
for(i = 0 ; i < l ; i++)
for(j = 0 ; j < m ; j++) {
t = 0.0;
@@ -446,8 +446,8 @@ ukf_filter_update(ukf_filter filter, double *y, double *u) {
t += filter->KL[i * m + k] * filter->KL[j * m + k];
filter->P[i * l + j ] -= t;
}
// finished with kalman iteration !
}
sw/in_progress/inertial/C/ukf.h
+10 -10
View File
@@ -20,14 +20,14 @@ typedef void (*measure_function)(double *, double *);
* @param ffun: the funcion describing the filter evolution equation
* @param mfun: the measurement function
*/
ukf_filter
ukf_filter_new(unsigned state_dim,
unsigned measure_dim,
ukf_filter
ukf_filter_new(unsigned state_dim,
unsigned measure_dim,
double *Q,
double *R,
filter_function ffun,
measure_function mfun);
/*
* free filter memory
* @param filter: the filter to delete
@@ -43,28 +43,28 @@ ukf_filter_delete(ukf_filter filter);
*/
void
ukf_filter_compute_weights(ukf_filter filter,
double alpha,
double k,
double alpha,
double k,
double beta);
/*
* Reset filter to new state
* @param x0: initial state
* @param x0: initial state
* @param P0: initial state covariance matrix
*/
void
void
ukf_filter_reset(ukf_filter filter,
double *x0,
double *PO);
/*
* Get filter state
* Get filter state
* @param x: A vector that will hold the state
* @param P: A vector that will hold the state covariance
*/
void
ukf_filter_get_state(ukf_filter filter, double *x, double *P);
/*
* Update filter using a measure
* @param y: The measure vector
sw/in_progress/ir_calibration/calibrator.c
+6 -6
View File
@@ -59,7 +59,7 @@ static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[
//guint ac_id = atoi(argv[0]);
estimator_phi = RAD_OF_DEG(atof(argv[1]));
//g_message("attitude %d %f", ac_id, estimator_phi);
int gps_phi_deg = round(DEG_OF_RAD(gps_phi));
int gps_phi_deg = round(DEG_OF_RAD(gps_phi));
if (fabs(gps_phi_deg) <= NB_POINTS/2) {
unsigned int idx = gps_phi_deg + NB_POINTS/2;
estimator_phi_by_degres[idx] = (1-alpha) * estimator_phi_by_degres[idx] + alpha * DEG_OF_RAD(estimator_phi);
@@ -74,14 +74,14 @@ void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]){
gps_gs_norm = speed / 100.;
gps_gs_angle = RAD_OF_DEG(90. - course / 10.);
NORM_ANGLE_RAD( gps_gs_angle );
gps_gs_east = gps_gs_norm * cos(gps_gs_angle);
gps_gs_north = gps_gs_norm * sin(gps_gs_angle);
gps_as_east = gps_gs_east - est_wind_east;
gps_as_north = gps_gs_north - est_wind_north;
gps_as_angle = atan2(gps_as_north,gps_as_east);
gps_as_angle = atan2(gps_as_north,gps_as_east);
gps_as_norm = sqrt(gps_as_east*gps_as_east + gps_as_north*gps_as_north);
static float old_psi = 0.;
@@ -91,7 +91,7 @@ void on_GPS(IvyClientPtr app, void *user_data, int argc, char *argv[]){
/* tan(phi) = v^2 / (R*g) */
/* R = (V * dt) / dpsi */
if (fabs(delta_psi) < 1e-6)
delta_psi = copysign(1e-6, delta_psi);
@@ -122,13 +122,13 @@ void on_IrSensors(IvyClientPtr app, void *user_data, int argc, char *argv[]){
//guint ac_id = atoi(argv[0]);
float lateral = atof(argv[2]);
float vertical = atof(argv[3]);
float ir_roll = lateral * IR_360_LATERAL_CORRECTION;
float ir_top = vertical * IR_360_LATERAL_CORRECTION;
gnd_ir_phi = atan2(ir_roll, ir_top) - IR_ROLL_NEUTRAL;
if (gnd_ir_phi >= 0)
if (gnd_ir_phi >= 0)
gnd_ir_phi *= IR_CORRECTION_RIGHT;
else
gnd_ir_phi *= IR_CORRECTION_LEFT;
sw/in_progress/ir_calibration/gui.c
+6 -6
View File
@@ -23,11 +23,11 @@ GtkWidget* gui_init(void) {
GtkWidget *vbox1 = gtk_vbox_new (FALSE, 0);
gtk_container_add (GTK_CONTAINER (window), vbox1);
GtkWidget *frame = gtk_frame_new ("Foo");
gtk_container_set_border_width (GTK_CONTAINER (frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), frame, TRUE, TRUE, 0);
databox = gtk_databox_new ();
gtk_container_add (GTK_CONTAINER (frame), databox );
@@ -35,8 +35,8 @@ GtkWidget* gui_init(void) {
Y1 = g_new0 (gfloat, NB_POINTS);
Y2 = g_new0 (gfloat, NB_POINTS);
guint i;
for (i=0; i<NB_POINTS; i++){
X[i] = (gfloat)i - NB_POINTS/2;
for (i=0; i<NB_POINTS; i++){
X[i] = (gfloat)i - NB_POINTS/2;
Y1[i] = (gfloat)i -NB_POINTS/2;
Y2[i] = (gfloat)i -NB_POINTS/2;
}
@@ -51,7 +51,7 @@ GtkWidget* gui_init(void) {
gtk_databox_graph_add (GTK_DATABOX (databox), graph2);
GtkDataboxGraph *grid = gtk_databox_grid_new (11, 11, &cblack, 1);
gtk_databox_graph_add (GTK_DATABOX (databox), grid);
gtk_databox_graph_add (GTK_DATABOX (databox), grid);
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
@@ -61,7 +61,7 @@ GtkWidget* gui_init(void) {
void gui_update(gfloat* values) {
guint i;
for (i=0; i<NB_POINTS; i++){
for (i=0; i<NB_POINTS; i++){
Y2[i] = values[i];
}
gtk_databox_auto_rescale (GTK_DATABOX(databox), 0.);
sw/in_progress/ir_calibration/main.c
+3 -3
View File
@@ -17,15 +17,15 @@ gboolean timeout_callback(gpointer data) {
int main ( int argc, char** argv) {
// GMainLoop *ml = g_main_loop_new(NULL, FALSE);
gtk_init(&argc, &argv);
g_timeout_add(500, timeout_callback, NULL);
calibrator_init();
gui_init();
//g_main_loop_run(ml);
sw/in_progress/ir_usb_i2c/i2c_usb.c
+73 -73
View File
@@ -54,7 +54,7 @@ unsigned crc8(unsigned char* dat, int len)
{
unsigned bits = 0;
int i;
/* x^8 + x^2 + x^1 + x^0 */
while (len-- > 0) {
for (i = 7; i >= 0; i--) {
@@ -70,13 +70,13 @@ unsigned crc8(unsigned char* dat, int len)
if (bits > 0xFF)
bits ^= 0x107;
}
return bits;
}
/* write a set of bytes to the i2c_tiny_usb device */
int i2c_tiny_usb_write(int request, int value, int index) {
if(usb_control_msg(handle, USB_CTRL_OUT, request,
if(usb_control_msg(handle, USB_CTRL_OUT, request,
value, index, NULL, 0, 1000) < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
@@ -89,8 +89,8 @@ int i2c_tiny_usb_read(unsigned char cmd, void *data, int len) {
int nBytes;
/* send control request and accept return value */
nBytes = usb_control_msg(handle,
USB_CTRL_IN,
nBytes = usb_control_msg(handle,
USB_CTRL_IN,
cmd, 0, 0, data, len, 1000);
if(nBytes < 0) {
@@ -104,15 +104,15 @@ int i2c_tiny_usb_read(unsigned char cmd, void *data, int len) {
/* get i2c usb interface firmware version */
void i2c_tiny_usb_get_func(void) {
unsigned long func;
if(i2c_tiny_usb_read(CMD_GET_FUNC, &func, sizeof(func)) == 0)
printf("Functionality = %lx\n", func);
}
/* set a value in the I2C_USB interface */
void i2c_tiny_usb_set(unsigned char cmd, int value) {
if(usb_control_msg(handle,
USB_TYPE_VENDOR, cmd, value, 0,
if(usb_control_msg(handle,
USB_TYPE_VENDOR, cmd, value, 0,
NULL, 0, 1000) < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
}
@@ -122,7 +122,7 @@ void i2c_tiny_usb_set(unsigned char cmd, int value) {
int i2c_tiny_usb_get_status(void) {
int i;
unsigned char status;
if((i=i2c_tiny_usb_read(CMD_GET_STATUS, &status, sizeof(status))) < 0) {
fprintf(stderr, "Error reading status\n");
return i;
@@ -138,17 +138,17 @@ int i2c_read_with_cmd(unsigned char addr, char cmd, int length) {
if((length < 0) || (length > sizeof(result))) {
fprintf(stderr, "request exceeds %d bytes\n", sizeof(result));
return -1;
}
}
/* write one byte register address to chip */
if(usb_control_msg(handle, USB_CTRL_OUT,
if(usb_control_msg(handle, USB_CTRL_OUT,
CMD_I2C_IO + CMD_I2C_BEGIN
+ ((!length)?CMD_I2C_END:0),
0, addr, &cmd, 1,
0, addr, &cmd, 1,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "write command status failed\n");
@@ -158,14 +158,14 @@ int i2c_read_with_cmd(unsigned char addr, char cmd, int length) {
// just a test? return ok
if(!length) return 0;
if(usb_control_msg(handle,
USB_CTRL_IN,
if(usb_control_msg(handle,
USB_CTRL_IN,
CMD_I2C_IO + CMD_I2C_END,
I2C_M_RD, addr, (char*)result, length,
I2C_M_RD, addr, (char*)result, length,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "read data status failed\n");
@@ -177,7 +177,7 @@ int i2c_read_with_cmd(unsigned char addr, char cmd, int length) {
return 256*result[0] + result[1];
// return 8 bit result
return result[0];
return result[0];
}
/* write command and read an 16 bit value from mlx */
@@ -186,27 +186,27 @@ int i2c_mlx_read_word_with_cmd(unsigned char addr, char cmd) {
unsigned char pec[6];
/* write one byte register address to chip */
if(usb_control_msg(handle, USB_CTRL_OUT,
if(usb_control_msg(handle, USB_CTRL_OUT,
CMD_I2C_IO + CMD_I2C_BEGIN,
0, addr, &cmd, 1,
0, addr, &cmd, 1,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "write command status failed\n");
return -1;
}
if(usb_control_msg(handle,
USB_CTRL_IN,
if(usb_control_msg(handle,
USB_CTRL_IN,
CMD_I2C_IO + CMD_I2C_END,
I2C_M_RD, addr, (char*)result, 4,
I2C_M_RD, addr, (char*)result, 4,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "read data status failed\n");
@@ -230,20 +230,20 @@ int i2c_mlx_read_word_with_cmd(unsigned char addr, char cmd) {
int i2c_write_byte(unsigned char addr, char data) {
/* write one byte register address to chip */
if(usb_control_msg(handle, USB_CTRL_OUT,
if(usb_control_msg(handle, USB_CTRL_OUT,
CMD_I2C_IO + CMD_I2C_BEGIN + CMD_I2C_END,
0, addr, &data, 1,
0, addr, &data, 1,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "write command status failed\n");
return -1;
}
return 0;
return 0;
}
/* write a command byte and a single byte to the i2c client */
@@ -254,27 +254,27 @@ int i2c_write_cmd_and_byte(unsigned char addr, char cmd, char data) {
msg[1] = data;
/* write one byte register address to chip */
if(usb_control_msg(handle, USB_CTRL_OUT,
if(usb_control_msg(handle, USB_CTRL_OUT,
CMD_I2C_IO + CMD_I2C_BEGIN + CMD_I2C_END,
0, addr, msg, 2,
0, addr, msg, 2,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "write command status failed\n");
return -1;
}
return 0;
return 0;
}
/* write a command byte and a 16 bit value to the mlx */
int i2c_mlx_write_cmd_and_word(unsigned char addr, char cmd, int data) {
char msg[4];
unsigned char pec[5];
pec[0]=addr<<1;
pec[1]=cmd;
pec[2]=data & 0xff;
@@ -286,20 +286,20 @@ int i2c_mlx_write_cmd_and_word(unsigned char addr, char cmd, int data) {
msg[3] = crc8(pec, 4);
/* write one byte register address to chip */
if(usb_control_msg(handle, USB_CTRL_OUT,
if(usb_control_msg(handle, USB_CTRL_OUT,
CMD_I2C_IO + CMD_I2C_BEGIN + CMD_I2C_END,
0, addr, msg, 4,
0, addr, msg, 4,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "write command status failed\n");
return -1;
}
return 0;
return 0;
}
/* write a command byte and a 16 bit value to the mlx i2c client */
@@ -311,20 +311,20 @@ int i2c_write_cmd_and_word(unsigned char addr, char cmd, int data) {
msg[2] = data & 0xff;
/* write one byte register address to chip */
if(usb_control_msg(handle, USB_CTRL_OUT,
if(usb_control_msg(handle, USB_CTRL_OUT,
CMD_I2C_IO + CMD_I2C_BEGIN + CMD_I2C_END,
0, addr, msg, 3,
0, addr, msg, 3,
1000) < 1) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
return -1;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
fprintf(stderr, "write command status failed\n");
return -1;
}
return 0;
return 0;
}
int main(int argc, char *argv[]) {
@@ -334,34 +334,34 @@ int main(int argc, char *argv[]) {
#ifndef WIN
int ret;
#endif
printf("-- i2c-tiny-usb test application --\n");
printf("-- (c) 2006 by Till Harbaum --\n");
printf("-- http://www.harbaum.org/till/i2c_tiny_usb --\n");
usb_init();
usb_find_busses();
usb_find_devices();
for(bus = usb_get_busses(); bus; bus = bus->next) {
for(dev = bus->devices; dev; dev = dev->next) {
if((dev->descriptor.idVendor == I2C_TINY_USB_VID) &&
if((dev->descriptor.idVendor == I2C_TINY_USB_VID) &&
(dev->descriptor.idProduct == I2C_TINY_USB_PID)) {
printf("Found i2c_tiny_usb device on bus %s device %s.\n",
printf("Found i2c_tiny_usb device on bus %s device %s.\n",
bus->dirname, dev->filename);
/* open device */
if(!(handle = usb_open(dev)))
fprintf(stderr, "Error: Cannot open the device: %s\n",
if(!(handle = usb_open(dev)))
fprintf(stderr, "Error: Cannot open the device: %s\n",
usb_strerror());
break;
}
}
}
if(!handle) {
fprintf(stderr, "Error: Could not find i2c_tiny_usb device\n");
@@ -382,7 +382,7 @@ int main(int argc, char *argv[]) {
exit(1);
}
#endif
/* do some testing */
i2c_tiny_usb_get_func();
@@ -398,17 +398,17 @@ int main(int argc, char *argv[]) {
printf("Probing for MLX90614 ... ");
/* try to access mlx90614 at address MLX90614_ADDR */
if(usb_control_msg(handle, USB_CTRL_IN,
if(usb_control_msg(handle, USB_CTRL_IN,
CMD_I2C_IO + CMD_I2C_BEGIN + CMD_I2C_END,
0, MLX90614_ADDR, NULL, 0,
0, MLX90614_ADDR, NULL, 0,
1000) < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
goto quit;
}
}
if(i2c_tiny_usb_get_status() == STATUS_ADDRESS_ACK) {
int tp;
printf("success at address 0x%02x\n", MLX90614_ADDR);
usleep(10000);
@@ -427,13 +427,13 @@ int main(int argc, char *argv[]) {
/* write new i2c address, always set bit0 ! */
// i2c_mlx_write_cmd_and_word(MLX90614_ADDR, 0x2E, 1);
// usleep(1000000);
tp = i2c_mlx_read_word_with_cmd(MLX90614_ADDR, 0x2E);
printf("i2c addr = 0x%04X\n", tp);
tp = i2c_mlx_read_word_with_cmd(MLX90614_ADDR, 0x06);
printf("Ta = %2.2f°C (0x%04X)\n", (tp*0.02)-273.15, tp);
tp = i2c_mlx_read_word_with_cmd(MLX90614_ADDR, 0x07);
printf("Tobj1 = %2.2f°C (0x%04X)\n", (tp*0.02)-273.15, tp);
@@ -446,35 +446,35 @@ int main(int argc, char *argv[]) {
/* -------- begin of mlx90614 multi client processing --------- */
/* try to access mlx90614 at address MLX90614_ADDR_1 */
if(usb_control_msg(handle, USB_CTRL_IN,
if(usb_control_msg(handle, USB_CTRL_IN,
CMD_I2C_IO + CMD_I2C_BEGIN + CMD_I2C_END,
0, MLX90614_ADDR_1, NULL, 0,
0, MLX90614_ADDR_1, NULL, 0,
1000) < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
goto quit;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
printf("no device at address 0x%02x\n", MLX90614_ADDR_1);
goto quit;
}
}
/* try to access mlx90614 at address MLX90614_ADDR_2 */
if(usb_control_msg(handle, USB_CTRL_IN,
if(usb_control_msg(handle, USB_CTRL_IN,
CMD_I2C_IO + CMD_I2C_BEGIN + CMD_I2C_END,
0, MLX90614_ADDR_1, NULL, 0,
0, MLX90614_ADDR_1, NULL, 0,
1000) < 0) {
fprintf(stderr, "USB error: %s\n", usb_strerror());
goto quit;
}
}
if(i2c_tiny_usb_get_status() != STATUS_ADDRESS_ACK) {
printf("no device at address 0x%02x\n", MLX90614_ADDR_2);
goto quit;
}
{
}
{
int tp1, tp2;
usleep(100000);
while(1) {
sw/in_progress/log_parser/linuxtest.c
+12 -12
View File
@@ -1,6 +1,6 @@
/*
* Paparazzi $Id$
*
*
* Copyright (C) 2003-2009 Pascal Brisset, Antoine Drouin, Martin Mueller
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*
*/
@@ -38,7 +38,7 @@
2 MSG_PAYLOAD
. DATA (messages.xml)
E XBEE_CHECKSUM (sum[A->D])
ID is AC_ID for aircraft, 0x100 for ground station
*/
@@ -136,7 +136,7 @@ int bytes_in = 0;
int bytes_out = 0;
//dummy
int fdw;
int fdw;
unsigned int filew;
int file_write (void* file, int size, unsigned char* buf)
{
@@ -157,7 +157,7 @@ unsigned char checksum(unsigned char start, unsigned char* data, int length)
void log_payload(int len, unsigned char source, unsigned int timestamp)
{
unsigned char chk, i;
/* start delimiter */
log_buffer[0] = STX;
@@ -166,7 +166,7 @@ void log_payload(int len, unsigned char source, unsigned int timestamp)
/* source */
log_buffer[2] = source;
/* add a 32bit timestamp */
log_buffer[3] = (timestamp) & 0xFF; // LSB first
log_buffer[4] = (timestamp >> 8) & 0xFF;
@@ -190,11 +190,11 @@ void log_payload(int len, unsigned char source, unsigned int timestamp)
/* write data, start+length+timestamp+data+checksum */
chk = file_write(&filew, LOG_DATA_OFFSET+len+1, log_buffer);
if (len != chk)
if (len != chk)
{
nb_fail_write++;
}
bytes_out += chk;
nb_messages++;
// dl_parse_msg();
@@ -270,7 +270,7 @@ void log_pprz(unsigned char c)
switch (pprzl_status) {
case UNINIT:
if (c == STX)
pprzl_timestamp++;
pprzl_timestamp++;
pprzl_status++;
printf(">");
break;
@@ -320,9 +320,9 @@ void log_pprz(unsigned char c)
int main(void)
{
int fd;
int fd;
unsigned char inc;
fd = open("xbee.bin", O_RDONLY);
// fd = open("pprz.bin", O_RDONLY);
if(fd < 0)
@@ -356,7 +356,7 @@ int main(void)
close(fdw);
close(fd);
printf("\nmsg detected %d\nbytes in %d, out %d\nerr xbee %d, pprz %d\n",
printf("\nmsg detected %d\nbytes in %d, out %d\nerr xbee %d, pprz %d\n",
nb_messages,
bytes_in,
bytes_out,
sw/in_progress/maptests/osm.c
+10 -10
View File
@@ -9,14 +9,14 @@
#include <stdio.h>
#include <math.h>
int long2tile(double lon, int z)
{
return (int)(floor((lon + 180.0) / 360.0 * pow(2.0, z)));
int long2tile(double lon, int z)
{
return (int)(floor((lon + 180.0) / 360.0 * pow(2.0, z)));
}
int lat2tile(double lat, int z)
{
return (int)(floor((1.0 - log( tan(lat * M_PI/180.0) + 1.0 / cos(lat * M_PI/180.0)) / M_PI) / 2.0 * pow(2.0, z)));
{
return (int)(floor((1.0 - log( tan(lat * M_PI/180.0) + 1.0 / cos(lat * M_PI/180.0)) / M_PI) / 2.0 * pow(2.0, z)));
}
void gm_quadtree(int x, int y, int z, char *buffer)
@@ -39,11 +39,11 @@ void ms_quadtree(int x, int y, int z, char *buffer)
{
int i;
char *ptr = buffer;
for (i = z; i > 0; i--)
{
int mask = 1 << (i - 1);
char digit = '0';
char digit = '0';
if ((x & mask) != 0)
{
digit+=1;
@@ -68,13 +68,13 @@ int main(void)
y = lat2tile(lat, z);
ms_quadtree(x, y, z, ms_qkey);
gm_quadtree(x, y, z, gm_qkey);
printf("http://tile.openstreetmap.org/%d/%d/%d.png\n", z, x, y);
printf("http://khm0.google.com/kh/v=45&x=%d&s=&y=%d&z=%d\n", x, y, z);
// printf("http://kh.google.com/kh?v=3&t=%s\n", gm_qkey);
// printf("http://mt1.google.com/mt/v=ap.95&hl=en&x=%d&y=%d&z=%d&s=G\n", x, y, z);
printf("http://a0.ortho.tiles.virtualearth.net/tiles/a%s.jpeg?g=%d\n", ms_qkey, z+32);
printf("http://r0.ortho.tiles.virtualearth.net/tiles/r%s.png?g=%d\n", ms_qkey, z+32);
return(0);
};
sw/in_progress/motor_bench/main.c
+9 -9
View File
@@ -114,7 +114,7 @@ static void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, c
g_string_printf(str, "%.4f %.3f %.0f %.1f %.1f %.1f\n", mb_state.time, mb_state.throttle, mb_state.rpms, mb_state.amps, mb_state.thrust, mb_state.torque);
gsize b_writen;
GError* my_err = NULL;
GIOStatus stat = g_io_channel_write_chars(mb_state.log_channel,str->str, str->len, &b_writen, &my_err);
GIOStatus stat = g_io_channel_write_chars(mb_state.log_channel,str->str, str->len, &b_writen, &my_err);
g_string_free(str, TRUE);
}
// g_message("foo %f %f %f %f %d", mb_state.time, throttle, rpm, amp, mode);
@@ -131,7 +131,7 @@ static void on_MOTOR_BENCH_STATIC(IvyClientPtr app, void *user_data, int argc, c
g_string_printf(str, "%0f %.3f %.2f %.1f\n", mb_state.av_throttle, mb_state.av_rpm, mb_state.av_amps, mb_state.av_thrust);
gsize b_writen;
GError* my_err = NULL;
GIOStatus stat = g_io_channel_write_chars(mb_state.log_channel_static,str->str, str->len, &b_writen, &my_err);
GIOStatus stat = g_io_channel_write_chars(mb_state.log_channel_static,str->str, str->len, &b_writen, &my_err);
g_string_free(str, TRUE);
}
g_message("in_static %f %f %f %f", mb_state.av_throttle, mb_state.av_rpm, mb_state.av_amps, mb_state.av_thrust);
@@ -163,7 +163,7 @@ static void on_log_button_toggled (GtkWidget *widget, gpointer data) {
GError* my_err = NULL;
mb_state.log_channel = g_io_channel_new_file (log_file_name, "w", &my_err);
GString* static_name = g_string_sized_new(128);
g_string_printf(static_name,"%s%s", log_file_name, "_static");
g_string_printf(static_name,"%s%s", log_file_name, "_static");
mb_state.log_channel_static = g_io_channel_new_file (static_name->str, "w", &my_err);
g_string_free(static_name, TRUE);
}
@@ -218,7 +218,7 @@ int main (int argc, char** argv) {
IvyStart("127.255.255.255");
g_timeout_add(40, timeout_callback, NULL);
mb_state.log_channel = NULL;
gtk_main();
@@ -302,7 +302,7 @@ static GtkWidget* build_gui ( void ) {
GtkWidget* table1 = gtk_table_new (2, 3, FALSE);
gtk_container_add (GTK_CONTAINER (measure_frame), table1);
gtk_table_set_col_spacings (GTK_TABLE (table1), 5);
GtkWidget* t_time = gtk_label_new ("time");
gtk_table_attach (GTK_TABLE (table1), t_time, 0, 1, 0, 1,
(GtkAttachOptions) (GTK_FILL),
@@ -376,9 +376,9 @@ static GtkWidget* build_gui ( void ) {
GtkWidget *log_frame = gtk_frame_new ("Log");
gtk_container_set_border_width (GTK_CONTAINER (log_frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), log_frame, TRUE, TRUE, 0);
GtkWidget* bbox = gtk_hbutton_box_new ();
GtkWidget* bbox = gtk_hbutton_box_new ();
gtk_container_add (GTK_CONTAINER (log_frame), bbox);
GtkWidget* log_button = gtk_toggle_button_new_with_label( "Log" );
@@ -394,7 +394,7 @@ static GtkWidget* build_gui ( void ) {
//
// Asctech
//
GtkWidget *asctech_frame = gtk_frame_new ("Asctech");
gtk_container_set_border_width (GTK_CONTAINER (asctech_frame), 10);
gtk_box_pack_start (GTK_BOX (vbox1), asctech_frame, TRUE, TRUE, 0);
@@ -428,7 +428,7 @@ static GtkWidget* build_gui ( void ) {
GtkWidget* as_bbox = gtk_hbutton_box_new ();
GtkWidget* as_bbox = gtk_hbutton_box_new ();
gtk_box_pack_start (GTK_BOX (as_vbox2), as_bbox, TRUE, TRUE, 0);
//gtk_container_add (GTK_CONTAINER (asctech_frame), as_bbox);
sw/in_progress/rctx/main_rctx.c
+18 -18
View File
@@ -1,5 +1,5 @@
/*
/*
- 0x101..0x10F reserved for rc transmitters
- rc transmitters broadcast their messages (dest = 0xFFFF)
@@ -11,7 +11,7 @@
- XBee-message
ID is AC_ID for aircraft, 0x100 for ground station
1 A XBEE_START (0x7E)
2 B LENGTH_MSB (A->E)
3 C LENGTH_LSB
@@ -25,7 +25,7 @@
9 0 SENDER_ID
10 1 MSG_ID
MSG_PAYLOAD
11 0 RCTX_MODE
11 0 RCTX_MODE
12 1 THOTTLE_LSB
13 2 THOTTLE_MSB
14 3 ROLL_LSB
@@ -33,7 +33,7 @@
16 5 PITCH_LSB
17 6 PITCH_MSB
18 E XBEE_CHECKSUM (sum[A->D])
- messages.xml
<message name="RC_3CH" ID="27">
@@ -51,7 +51,7 @@
#define GROUND_STATION_ADDR 0x0100
#endif
- datalink.c
- datalink.c
#ifdef USE_RC_TELEMETRY
if (msg_id == DL_RC_3CH && DL_RC_3CH_ac_id(dl_buffer) == TX_ID) {
@@ -62,7 +62,7 @@ LED_TOGGLE(3);
} else
#endif // USE_RC_TELEMETRY
*/
*/
#include <stdio.h>
#include "std.h"
@@ -152,7 +152,7 @@ void init_rctx( void ) {
ppm_init();
#endif
int_enable();
/** - wait 0.5s (for modem init ?) */
uint8_t init_cpt = 30;
while (init_cpt) {
@@ -164,7 +164,7 @@ void init_rctx( void ) {
#if DATALINK == XBEE
xbee_init();
#endif
#endif /* DATALINK */
#endif /* DATALINK */
}
/********** EVENT ************************************************************/
@@ -173,7 +173,7 @@ void event_task_rctx( void) {
if (ppm_valid) {
ppm_valid = FALSE;
radio_control_event_task();
#ifdef USE_RCTX_MODE_SWITCH
// TODO: set rxtx_mode from GPIO connected switch (e.g. I2C pins)
#else
@@ -182,7 +182,7 @@ void event_task_rctx( void) {
rctx_mode |= rctx_under_voltage << 2;
LED_TOGGLE(3);
if (1)
// TODO: check XBee busy pin
// TODO: send only if aircraft is listening
@@ -196,8 +196,8 @@ LED_TOGGLE(3);
}
}
#endif
#if defined DATALINK
#if defined DATALINK
#if DATALINK == XBEE
if (XBeeBuffer()) {
@@ -208,12 +208,12 @@ LED_TOGGLE(3);
}
}
#endif
if (dl_msg_available) {
dl_parse_msg();
dl_msg_available = FALSE;
}
#endif /* DATALINK */
#endif /* DATALINK */
}
/************* PERIODIC ******************************************************/
@@ -224,8 +224,8 @@ void periodic_task_rctx( void ) {
_10Hz++;
if (_10Hz >= 6) _10Hz = 0;
_1Hz++;
if (_1Hz>=60) _1Hz=0;
if (_1Hz>=60) _1Hz=0;
#ifdef RADIO_CONTROL
radio_control_periodic_task();
#endif
@@ -250,7 +250,7 @@ void periodic_task_rctx( void ) {
LED_TOGGLE(1);
rctx_under_voltage = 1;
}
if (0)
// TODO: send (here) only in auto2
{
@@ -259,7 +259,7 @@ void periodic_task_rctx( void ) {
&rc_values[RADIO_THROTTLE],
&rc_values[RADIO_ROLL],
&rc_values[RADIO_PITCH]);
}
}
}
#endif
}
sw/in_progress/river_tracking/src/river_track.c
+8 -8
View File
@@ -62,7 +62,7 @@ void init_river_tracking( GtkWidget *widget, gpointer data )
}
/* Called when aircraft reaches "Block 1" (the second
/* Called when aircraft reaches "Block 1" (the second
* block defined in the flight plan)
*/
void start_track(IvyClientPtr app, void *data, int argc, char **argv)
@@ -71,7 +71,7 @@ void start_track(IvyClientPtr app, void *data, int argc, char **argv)
/* Call function get_next_waypoint (defined in nexcwp.c)
* which returns a waypoint. */
Waypoint new_wp = get_next_waypoint();
if(CONTINUE) {
IvySendMsg("gcs MOVE_WAYPOINT %d %d %f %f %f", \
new_wp.ac_id, 4, new_wp.lat, new_wp.lon, new_wp.alt); //Always move waypoint 4
@@ -94,23 +94,23 @@ int main( int argc, char *argv[] )
GtkWidget *window;
GtkWidget *button;
GtkWidget *box1;
char *bus=getenv("IVYBUS");
gtk_init(&argc, &argv);
window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
gtk_container_set_border_width(GTK_CONTAINER(window), 10);
gtk_window_set_title (GTK_WINDOW (window), "River Tracking");
box1 = gtk_hbox_new (FALSE, 0);
gtk_container_add (GTK_CONTAINER (window), box1);
//first button...
button = gtk_button_new_with_label("(Re)define region of interest");
g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(init_river_tracking), 0);
gtk_box_pack_start(GTK_BOX(box1), button, TRUE, TRUE, 0);
gtk_widget_show (button);
//second button...
button = gtk_button_new_with_label("Quit");
g_signal_connect(G_OBJECT(button), "clicked", G_CALLBACK(destroy), 0);
@@ -125,7 +125,7 @@ int main( int argc, char *argv[] )
IvyBindMsg(start_track,0,"(NAV_STATUS 1 1 +.*)");
IvyBindMsg(set_end,0,"(WAYPOINT_MOVED 1 5 +.*)");
IvyStart(bus);
gtk_main();
return 0;
}
sw/in_progress/satcom/email2udp.c
+7 -7
View File
@@ -26,7 +26,7 @@
*
*/
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
@@ -143,13 +143,13 @@ int base64_decode( unsigned char *dst, int *dlen,
int base64_invalid(char *src, int slen ) {
int i;
for( i=0; i < slen; i++ ) {
if( ( slen - i ) >= 2 && src[i] == '\r' && src[i + 1] == '\n' )
return 0;
if( src[i] == '\n' )
return 0;
if(src[i] > 127 || base64_dec_map[(unsigned char)src[i]] == 127)
if(src[i] > 127 || base64_dec_map[(unsigned char)src[i]] == 127)
return 1;
}
return 0;
@@ -174,7 +174,7 @@ int main( int argc, char** argv) {
line[0] = '\0';
line[sizeof(line)-1] = ~'\0';
/* setup UDP */
/* setup UDP */
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock < 0) perror("socket");
server.sin_family = AF_INET;
@@ -184,7 +184,7 @@ int main( int argc, char** argv) {
exit(1);
}
memcpy((char *)&server.sin_addr,
(char *)hent->h_addr,
(char *)hent->h_addr,
hent->h_length);
server.sin_port = htons(PORT);
length=sizeof(struct sockaddr_in);
@@ -211,13 +211,13 @@ int main( int argc, char** argv) {
len = strnlen(line, sizeof(line));
if ((base64_crlf(line, len) == 1) || (base64_invalid(line, len) == 1)) break;
if ((total_len+len) > sizeof(buf)) return 0;
/* fill buffer */
memcpy(buf+total_len, line, len);
total_len += len;
} while (eof != NULL);
if (eof == NULL) return 0;
len_out = sizeof(out);
if (base64_decode(out, &len_out, buf, total_len) != 0) return 0;
sw/in_progress/satcom/tcp2ivy.c
+2 -2
View File
@@ -54,7 +54,7 @@ unsigned char md5[] = {"\207\151\313\256\355\252\016\273\072\126\273\222\017\372
#define RadOfDeg(x) ((x) * (M_PI/180.))
static const char usage_str[] =
static const char usage_str[] =
"tcp2ivy [options]\n"
"options:\n"
" -s <server address>\n";
@@ -196,7 +196,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
pprz_mode = buf[19];
// com_trans.buf[20] = nav_block;
nav_block = buf[20];
// FillBufWith16bit(com_trans.buf, 21, autopilot_flight_time);
// FillBufWith16bit(com_trans.buf, 21, autopilot_flight_time);
autopilot_flight_time = buf2ushort(&buf[21]);
//gps_lat = 52.2648312 * 1e7;
sw/in_progress/satcom/tcp2ivy_generic.c
+2 -2
View File
@@ -57,7 +57,7 @@ unsigned char* md5 = (unsigned char*)MD5SUM;
#define RadOfDeg(x) ((x) * (M_PI/180.))
#define DegOfRad(x) ((x) * (180./M_PI))
static const char usage_str[] =
static const char usage_str[] =
"tcp2ivy [options]\n"
"options:\n"
" -s <server address>\n";
@@ -133,7 +133,7 @@ static gboolean read_data(GIOChannel *chan, GIOCondition cond, gpointer data) {
pprz_mode = buf[19];
// com_trans.buf[21] = nav_block;
nav_block = buf[20];
// FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time);
// FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time);
autopilot_flight_time = buf2ushort(&buf[21]);
#if 0
sw/in_progress/satcom/udp2tcp.c
+13 -13
View File
@@ -1,6 +1,6 @@
/*
* Paparazzi udp to tcp handling for sat based telemetry
*
*
* Copyright (C) 2011 Martin Mueller <martinmm@pfump.org>
*
* This file is part of paparazzi.
@@ -18,7 +18,7 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*
*/
@@ -68,11 +68,11 @@ int main(int argc, char *argv[]) {
length = sizeof(sourceaddr);
memset(&sourceaddr, 0, sizeof(sourceaddr));
memset(&sinkaddr, 0, sizeof(sinkaddr));
sourceaddr.sin_family = AF_INET;
sourceaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
sourceaddr.sin_port = htons(PORT_IN);
sinkaddr.sin_family = AF_INET;
sinkaddr.sin_addr.s_addr = INADDR_ANY;
sinkaddr.sin_port = htons(PORT_OUT);
@@ -109,12 +109,12 @@ int main(int argc, char *argv[]) {
tvt.tv_sec = 1;
tvt.tv_usec = 0;
count = select(fdmax+1, &fds, NULL, NULL, &tvt);
count = select(fdmax+1, &fds, NULL, NULL, &tvt);
for (i=0; i<NUM_STREAMS; i++) {
if (FD_ISSET(consock[i], &fds)) {
n = recv(consock[i], buf, sizeof(buf), 0);
if (n <= 0) {
if (n < 0) perror("recv");
printf("disconnect\n");
@@ -127,18 +127,18 @@ int main(int argc, char *argv[]) {
printf("receive uplink %d\n", n);
/* create email to satcom */
}
}
}
}
if (FD_ISSET(sourcesock, &fds)) {
gettimeofday(&tvcur, NULL);
n = recvfrom(sourcesock, buf, sizeof(buf), 0,
n = recvfrom(sourcesock, buf, sizeof(buf), 0,
(struct sockaddr *)&sourceaddr, (socklen_t *)&fromlen);
if (n < 0) perror("recvfrom");
printf("receive downlink %d\n", n);
for (i=0; i<NUM_STREAMS; i++) {
if (consock[i] != 0) {
printf("send downlink %d\n", i);
@@ -178,6 +178,6 @@ int main(int argc, char *argv[]) {
}
}
}
}
}
return 0;
}
}
sw/in_progress/turbine/turb_simu.c
+15 -15
View File
@@ -1,6 +1,6 @@
/*
* turbine message simulator $Id: turb_simu.c 2145 2007-12-16 00:10:07Z mmm $
*
*
* Copyright (C) 2009 Martin Mueller
*
* This file is part of paparazzi.
@@ -18,10 +18,10 @@
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
* Boston, MA 02111-1307, USA.
*
*/
#include <stdio.h>
#include <unistd.h>
#include <errno.h>
@@ -111,44 +111,44 @@ int main (void)
return( -1 );
}
if (tcgetattr(serial_fd, &orig_termios))
if (tcgetattr(serial_fd, &orig_termios))
{
perror("getting modem serial device attr");
return( -1 );
}
cur_termios = orig_termios;
/* input modes */
cur_termios.c_iflag &= ~(IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK|ISTRIP|INLCR|IGNCR
|ICRNL |IXON|IXANY|IXOFF|IMAXBEL);
/* IGNCR does not pass 0x0D */
cur_termios.c_iflag |= BRKINT;
/* output_flags */
cur_termios.c_oflag &=~(OPOST|ONLCR|OCRNL|ONOCR|ONLRET);
/* control modes */
cur_termios.c_cflag &= ~(CSIZE|CSTOPB|CREAD|PARENB|PARODD|HUPCL|CLOCAL|CRTSCTS);
cur_termios.c_cflag |= CREAD|CS8|CLOCAL;
/* local modes */
cur_termios.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN);
cur_termios.c_lflag |= NOFLSH;
if (cfsetispeed(&cur_termios, B0))
{
perror("setting input modem serial device speed");
return( -1 );
}
if (cfsetospeed(&cur_termios, br))
{
perror("setting modem serial device speed");
return( -1 );
}
if (tcsetattr(serial_fd, TCSADRAIN, &cur_termios))
{
perror("setting modem serial device attr");
@@ -183,9 +183,9 @@ printf(" 0x%02X %c\n", data_temp, data_temp);
}
}
#endif
/* this is the interesting part */
/* this is the interesting part */
{
/*
@@ -224,7 +224,7 @@ printf(" 0x%02X %c\n", data_temp, data_temp);
0x00, // TX16_OPTIONS (none)
// Paparazzi message
0x00, // SENDER_ID (0)
0x32, // MSG_ID 0x32 = 50 (WINDTURBINE_STATUS)
0x32, // MSG_ID 0x32 = 50 (WINDTURBINE_STATUS)
0x2A, // ac_id 42 (target aircraft id)
0x01, // tb_id 01 (source turbine id)
0x39, 0x30, 0x00, 0x00, // sync_itow
sw/in_progress/wind_tunnel/main.c
+8 -8
View File
@@ -34,7 +34,7 @@ static void configure_term(struct termios *termios, speed_t *speed) {
}
static void dump_buf ( int len, char* buf) {
static gchar buf2[BUF_SIZE];
static int cur_len = 0;
int i;
@@ -83,7 +83,7 @@ static gboolean on_serial_data_received(GIOChannel *source,
#if 0
static gchar buf[BUF_SIZE];
gsize len;
g_io_channel_read_chars(source, buf, BUF_SIZE, &len, NULL);
dump_buf(len, buf);
#endif
@@ -95,19 +95,19 @@ gboolean timeout_callback(gpointer data) {
const char* msg = "GT\n";
gsize bw;
g_io_channel_write_chars(ioc, msg, 3, &bw, NULL);
g_io_channel_write_chars(ioc, msg, 3, &bw, NULL);
g_io_channel_flush(ioc, NULL);
return TRUE;
}
int main ( int argc, char** argv) {
sp = serial_port_new();
serial_port_open(sp, "/dev/ttyUSB1", configure_term);
ioc = g_io_channel_unix_new(sp->fd);
g_io_channel_set_encoding(ioc, NULL, NULL);
g_io_channel_set_flags (ioc,G_IO_FLAG_NONBLOCK, NULL );
@@ -120,7 +120,7 @@ int main ( int argc, char** argv) {
IvyStart("127.255.255.255");
g_timeout_add(500, timeout_callback, NULL);
g_main_loop_run(ml);
return 0;
sw/in_progress/wind_tunnel/serial_port.c
+10 -10
View File
@@ -1,4 +1,4 @@
/*
/*
pc2rc serial port functions
Copyright (C) 2001 Antoine Drouin
@@ -17,7 +17,7 @@
You should have received a copy of the GNU General Public License
along with paparazzi; see the file COPYING. If not, write to
the Free Software Foundation, 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.
Boston, MA 02111-1307, USA.
*/
@@ -43,8 +43,8 @@ struct SerialPort* serial_port_new() {
/*
* opens serial port and setup the terminal
*/
guint
serial_port_open(struct SerialPort* this, const char* device,
guint
serial_port_open(struct SerialPort* this, const char* device,
void(*term_conf_callback)(struct termios*, speed_t*)) {
speed_t speed;
if ((this->fd = open(device, O_RDWR)) < 0) {
@@ -54,7 +54,7 @@ serial_port_open(struct SerialPort* this, const char* device,
if (tcgetattr(this->fd, &this->orig_termios) < 0) {
TRACE(TRACE_ERROR,"getting term settings (%s)\n", strerror(errno));
return -1;
}
}
this->cur_termios = this->orig_termios;
term_conf_callback(&this->cur_termios, &speed);
if (cfsetispeed(&this->cur_termios, speed)) {
@@ -69,21 +69,21 @@ serial_port_open(struct SerialPort* this, const char* device,
}
/*
* closes serial port and restore term settings
* closes serial port and restore term settings
*/
guint
guint
serial_port_close(struct SerialPort* this) {
if (tcflush(this->fd, TCIOFLUSH)) {
TRACE(TRACE_ERROR,"flushing (%s)\n", strerror(errno));
return -1;
return -1;
}
if (tcsetattr(this->fd, TCSADRAIN, &this->orig_termios)) { // Restore modes.
TRACE(TRACE_ERROR,"restoring term attributes (%s)\n", strerror(errno));
return -1;
return -1;
}
if (close(this->fd)) {
TRACE(TRACE_ERROR,"closing (%s)\n", strerror(errno));
return -1;
return -1;
}
return 0;
}