From ac215185a9604a88665d0b5b8382659f8cedaf56 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Jan 2013 17:30:53 +0100 Subject: [PATCH 01/30] Better attitude filter, not sensitive to sudden accelerations --- apps/attitude_estimator_ekf/Makefile | 2 - .../attitude_estimator_ekf_main.c | 4 + .../attitude_estimator_ekf_params.c | 40 +- .../attitude_estimator_ekf_params.h | 4 +- .../codegen/attitudeKalmanfilter.c | 898 ++++++++++++------ .../codegen/attitudeKalmanfilter.h | 4 +- .../codegen/attitudeKalmanfilter_initialize.c | 2 +- .../codegen/attitudeKalmanfilter_initialize.h | 2 +- .../codegen/attitudeKalmanfilter_terminate.c | 2 +- .../codegen/attitudeKalmanfilter_terminate.h | 2 +- .../codegen/attitudeKalmanfilter_types.h | 2 +- apps/attitude_estimator_ekf/codegen/cross.c | 2 +- apps/attitude_estimator_ekf/codegen/cross.h | 2 +- apps/attitude_estimator_ekf/codegen/eye.c | 2 +- apps/attitude_estimator_ekf/codegen/eye.h | 2 +- .../attitude_estimator_ekf/codegen/mrdivide.c | 2 +- .../attitude_estimator_ekf/codegen/mrdivide.h | 2 +- apps/attitude_estimator_ekf/codegen/norm.c | 2 +- apps/attitude_estimator_ekf/codegen/norm.h | 2 +- apps/attitude_estimator_ekf/codegen/rdivide.c | 2 +- apps/attitude_estimator_ekf/codegen/rdivide.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetInf.h | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.c | 2 +- .../attitude_estimator_ekf/codegen/rtGetNaN.h | 2 +- .../codegen/rt_defines.h | 2 +- .../codegen/rt_nonfinite.c | 2 +- .../codegen/rt_nonfinite.h | 2 +- .../attitude_estimator_ekf/codegen/rtwtypes.h | 6 +- apps/examples/kalman_demo/KalmanNav.cpp | 1 - apps/mavlink/mavlink_receiver.c | 1 - apps/uORB/topics/vehicle_attitude.h | 35 +- 32 files changed, 679 insertions(+), 360 deletions(-) mode change 100644 => 100755 apps/attitude_estimator_ekf/Makefile mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c mode change 100644 => 100755 apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/cross.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/cross.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/eye.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/eye.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/mrdivide.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/mrdivide.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/norm.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/norm.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rdivide.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rdivide.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetInf.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtGetNaN.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rt_defines.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.c mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rt_nonfinite.h mode change 100644 => 100755 apps/attitude_estimator_ekf/codegen/rtwtypes.h mode change 100644 => 100755 apps/uORB/topics/vehicle_attitude.h diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile old mode 100644 new mode 100755 index 4d531867ce..734af7cc1c --- a/apps/attitude_estimator_ekf/Makefile +++ b/apps/attitude_estimator_ekf/Makefile @@ -47,8 +47,6 @@ CSRCS = attitude_estimator_ekf_main.c \ codegen/rtGetInf.c \ codegen/rtGetNaN.c \ codegen/norm.c \ - codegen/diag.c \ - codegen/power.c \ codegen/cross.c diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c old mode 100644 new mode 100755 index 1c3b9976b3..7ca77e513d --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -431,6 +431,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; + att.rollacc = x_aposteriori[3]; + att.pitchacc = x_aposteriori[4]; + att.yawacc = x_aposteriori[5]; + //att.yawspeed =z_k[2] ; /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c old mode 100644 new mode 100755 index 91fec9ccbe..5b77566119 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -50,15 +50,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f); /* gyro offsets process noise */ PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f); PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f); -/* accelerometer process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f); -/* magnetometer process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f); /* gyro measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); @@ -66,12 +57,7 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); /* accelerometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f); -PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f); -/* magnetometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f); + /* offsets in roll, pitch and yaw of sensor plane and body */ PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); @@ -85,23 +71,11 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q2 = param_find("EKF_ATT_Q2"); h->q3 = param_find("EKF_ATT_Q3"); h->q4 = param_find("EKF_ATT_Q4"); - h->q5 = param_find("EKF_ATT_Q5"); - h->q6 = param_find("EKF_ATT_Q6"); - h->q7 = param_find("EKF_ATT_Q7"); - h->q8 = param_find("EKF_ATT_Q8"); - h->q9 = param_find("EKF_ATT_Q9"); - h->q10 = param_find("EKF_ATT_Q10"); - h->q11 = param_find("EKF_ATT_Q11"); h->r0 = param_find("EKF_ATT_R0"); h->r1 = param_find("EKF_ATT_R1"); h->r2 = param_find("EKF_ATT_R2"); h->r3 = param_find("EKF_ATT_R3"); - h->r4 = param_find("EKF_ATT_R4"); - h->r5 = param_find("EKF_ATT_R5"); - h->r6 = param_find("EKF_ATT_R6"); - h->r7 = param_find("EKF_ATT_R7"); - h->r8 = param_find("EKF_ATT_R8"); h->roll_off = param_find("ATT_ROLL_OFFS"); h->pitch_off = param_find("ATT_PITCH_OFFS"); @@ -117,23 +91,11 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->q2, &(p->q[2])); param_get(h->q3, &(p->q[3])); param_get(h->q4, &(p->q[4])); - param_get(h->q5, &(p->q[5])); - param_get(h->q6, &(p->q[6])); - param_get(h->q7, &(p->q[7])); - param_get(h->q8, &(p->q[8])); - param_get(h->q9, &(p->q[9])); - param_get(h->q10, &(p->q[10])); - param_get(h->q11, &(p->q[11])); param_get(h->r0, &(p->r[0])); param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); param_get(h->r3, &(p->r[3])); - param_get(h->r4, &(p->r[4])); - param_get(h->r5, &(p->r[5])); - param_get(h->r6, &(p->r[6])); - param_get(h->r7, &(p->r[7])); - param_get(h->r8, &(p->r[8])); param_get(h->roll_off, &(p->roll_off)); param_get(h->pitch_off, &(p->pitch_off)); diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h old mode 100644 new mode 100755 index b315d5fe83..09817d58e6 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -50,8 +50,8 @@ struct attitude_estimator_ekf_params { }; struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2, r3, r4, r5, r6, r7, r8; - param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11; + param_t r0, r1, r2, r3; + param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; }; diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c old mode 100644 new mode 100755 index 04f6ea267e..9e97ad11a8 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ @@ -15,8 +15,6 @@ #include "cross.h" #include "eye.h" #include "mrdivide.h" -#include "diag.h" -#include "power.h" /* Type Definitions */ @@ -50,7 +48,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) b_u1 = -1; } - y = (real32_T)atan2f((real32_T)b_u0, (real32_T)b_u1); + y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); } else if (u1 == 0.0F) { if (u0 > 0.0F) { y = RT_PIF / 2.0F; @@ -60,7 +58,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) y = 0.0F; } } else { - y = (real32_T)atan2f(u0, u1); + y = (real32_T)atan2(u0, u1); } return y; @@ -71,80 +69,88 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) */ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T - P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T + P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { - real32_T a[12]; - real32_T b_a[12]; - int32_T i; - real32_T Q[144]; + real32_T wak[3]; real32_T O[9]; real_T dv0[9]; - real32_T c_a[9]; - real32_T d_a[9]; + real32_T a[9]; + int32_T i; + real32_T b_a[9]; real32_T x_n_b[3]; real32_T b_x_aposteriori_k[3]; - real32_T m_n_b[3]; real32_T z_n_b[3]; - real32_T x_apriori[12]; + real32_T c_a[3]; + real32_T d_a[3]; int32_T i0; + real32_T x_apriori[12]; real_T dv1[144]; real32_T A_lin[144]; + static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + real32_T b_A_lin[144]; + real32_T b_q[144]; + real32_T c_A_lin[144]; + real32_T d_A_lin[144]; + real32_T e_A_lin[144]; int32_T i1; real32_T P_apriori[144]; - real32_T f0; - real32_T R[81]; real32_T b_P_apriori[108]; - static const int8_T iv0[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, + static const int8_T iv1[108] = { 1, 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, 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, 1, 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, 1, 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, 1 }; real32_T K_k[108]; - static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + real32_T fv0[81]; + static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 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, 0, 0, 0, 0, 0, 0, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - real32_T fv0[81]; + real32_T b_r[81]; + real32_T fv1[81]; + real32_T f0; real32_T c_P_apriori[36]; - static const int8_T iv2[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + static const int8_T iv3[36] = { 1, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - real32_T fv1[36]; - static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + real32_T fv2[36]; + static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 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 }; - real32_T S_k[36]; + real32_T c_r[9]; + real32_T b_K_k[36]; real32_T d_P_apriori[72]; - static const int8_T iv4[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, + static const int8_T iv5[72] = { 1, 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, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; - real32_T b_K_k[72]; - static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + real32_T c_K_k[72]; + static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - real32_T b_r[6]; - static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 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, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1 }; - + real32_T b_z[6]; static const int8_T iv7[72] = { 1, 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, 1, 0, 0, 0, 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, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; - real32_T fv2[6]; - real32_T b_z[6]; + static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 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, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1 }; + + real32_T fv3[6]; + real32_T c_z[6]; /* Extended Attitude Kalmanfilter */ /* */ @@ -160,44 +166,31 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const /* coder.varsize('udpIndVect', [9,1], [1,0]) */ /* udpIndVect=find(updVect); */ /* process and measurement noise covariance matrix */ - /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */ - power(q, a); - for (i = 0; i < 12; i++) { - b_a[i] = a[i] * dt; - } - - diag(b_a, Q); - - /* Q=[1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 1e-1, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 1e-10, 0, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ - /* 0, 0, 0, 0, 1e-10, 0, 1e-4, 1e-4, 0, 1e-4, 1e-4, 0; */ - /* 0, 0, 0, 0, 0, 1e-10, 0, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1, 0; */ - /* 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-1]; */ + /* Q = diag(q.^2*dt); */ /* observation matrix */ - /* 'attitudeKalmanfilter:44' wx= x_aposteriori_k(1); */ - /* 'attitudeKalmanfilter:45' wy= x_aposteriori_k(2); */ - /* 'attitudeKalmanfilter:46' wz= x_aposteriori_k(3); */ - /* 'attitudeKalmanfilter:48' wox= x_aposteriori_k(4); */ - /* 'attitudeKalmanfilter:49' woy= x_aposteriori_k(5); */ - /* 'attitudeKalmanfilter:50' woz= x_aposteriori_k(6); */ - /* 'attitudeKalmanfilter:52' zex= x_aposteriori_k(7); */ - /* 'attitudeKalmanfilter:53' zey= x_aposteriori_k(8); */ - /* 'attitudeKalmanfilter:54' zez= x_aposteriori_k(9); */ - /* 'attitudeKalmanfilter:56' mux= x_aposteriori_k(10); */ - /* 'attitudeKalmanfilter:57' muy= x_aposteriori_k(11); */ - /* 'attitudeKalmanfilter:58' muz= x_aposteriori_k(12); */ - /* 'attitudeKalmanfilter:61' wk =[wx; */ - /* 'attitudeKalmanfilter:62' wy; */ - /* 'attitudeKalmanfilter:63' wz]; */ - /* 'attitudeKalmanfilter:65' wok =[wox;woy;woz]; */ - /* 'attitudeKalmanfilter:66' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ + /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ + /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ + /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ + /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ + /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ + /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ + /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ + /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ + /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ + /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ + /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ + /* % prediction section */ + /* body angular accelerations */ + /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ + wak[0] = x_aposteriori_k[3]; + wak[1] = x_aposteriori_k[4]; + wak[2] = x_aposteriori_k[5]; + + /* body angular rates */ + /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ O[0] = 0.0F; O[1] = -x_aposteriori_k[2]; O[2] = x_aposteriori_k[1]; @@ -208,69 +201,73 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const O[7] = x_aposteriori_k[0]; O[8] = 0.0F; - /* 'attitudeKalmanfilter:67' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ + /* prediction of the earth z vector */ + /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); for (i = 0; i < 9; i++) { - c_a[i] = (real32_T)dv0[i] + O[i] * dt; + a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:68' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ + /* prediction of the magnetic vector */ + /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); for (i = 0; i < 9; i++) { - d_a[i] = (real32_T)dv0[i] + O[i] * dt; + b_a[i] = (real32_T)dv0[i] + O[i] * dt; } - /* 'attitudeKalmanfilter:70' EZ=[0,zez,-zey; */ - /* 'attitudeKalmanfilter:71' -zez,0,zex; */ - /* 'attitudeKalmanfilter:72' zey,-zex,0]'; */ - /* 'attitudeKalmanfilter:73' MA=[0,muz,-muy; */ - /* 'attitudeKalmanfilter:74' -muz,0,mux; */ - /* 'attitudeKalmanfilter:75' zey,-mux,0]'; */ - /* 'attitudeKalmanfilter:79' E=eye(3); */ - /* 'attitudeKalmanfilter:80' Es=[1,0,0; */ - /* 'attitudeKalmanfilter:81' 0,1,0; */ - /* 'attitudeKalmanfilter:82' 0,0,0]; */ - /* 'attitudeKalmanfilter:83' Z=zeros(3); */ - /* 'attitudeKalmanfilter:84' x_apriori=[wk;wok;zek;muk]; */ - x_n_b[0] = x_aposteriori_k[6]; - x_n_b[1] = x_aposteriori_k[7]; - x_n_b[2] = x_aposteriori_k[8]; - b_x_aposteriori_k[0] = x_aposteriori_k[9]; - b_x_aposteriori_k[1] = x_aposteriori_k[10]; - b_x_aposteriori_k[2] = x_aposteriori_k[11]; - x_apriori[0] = x_aposteriori_k[0]; - x_apriori[1] = x_aposteriori_k[1]; - x_apriori[2] = x_aposteriori_k[2]; - x_apriori[3] = x_aposteriori_k[3]; - x_apriori[4] = x_aposteriori_k[4]; - x_apriori[5] = x_aposteriori_k[5]; + /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ + /* 'attitudeKalmanfilter:66' -zez,0,zex; */ + /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ + /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ + /* 'attitudeKalmanfilter:69' -muz,0,mux; */ + /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ + /* 'attitudeKalmanfilter:74' E=eye(3); */ + /* 'attitudeKalmanfilter:76' Z=zeros(3); */ + /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ + x_n_b[0] = x_aposteriori_k[0]; + x_n_b[1] = x_aposteriori_k[1]; + x_n_b[2] = x_aposteriori_k[2]; + b_x_aposteriori_k[0] = x_aposteriori_k[6]; + b_x_aposteriori_k[1] = x_aposteriori_k[7]; + b_x_aposteriori_k[2] = x_aposteriori_k[8]; + z_n_b[0] = x_aposteriori_k[9]; + z_n_b[1] = x_aposteriori_k[10]; + z_n_b[2] = x_aposteriori_k[11]; for (i = 0; i < 3; i++) { - m_n_b[i] = 0.0F; + c_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - m_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; + c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; } - z_n_b[i] = 0.0F; + d_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { - z_n_b[i] += d_a[i + 3 * i0] * b_x_aposteriori_k[i0]; + d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; } - x_apriori[i + 6] = m_n_b[i]; + x_apriori[i] = x_n_b[i] + dt * wak[i]; } for (i = 0; i < 3; i++) { - x_apriori[i + 9] = z_n_b[i]; + x_apriori[i + 3] = wak[i]; } - /* 'attitudeKalmanfilter:86' A_lin=[ Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:87' Z, Z, Z, Z */ - /* 'attitudeKalmanfilter:88' EZ, Z, O, Z */ - /* 'attitudeKalmanfilter:89' MA, Z, Z, O]; */ - /* 'attitudeKalmanfilter:92' A_lin=eye(12)+A_lin*dt; */ + for (i = 0; i < 3; i++) { + x_apriori[i + 6] = c_a[i]; + } + + for (i = 0; i < 3; i++) { + x_apriori[i + 9] = d_a[i]; + } + + /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ + /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ + /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ + /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ + /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { - A_lin[i0 + 12 * i] = 0.0F; + A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; } for (i0 = 0; i0 < 3; i0++) { @@ -339,7 +336,164 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } - /* 'attitudeKalmanfilter:98' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ + /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ + /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ + /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ + /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ + /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ + /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ + b_q[0] = q[0]; + b_q[12] = 0.0F; + b_q[24] = 0.0F; + b_q[36] = 0.0F; + b_q[48] = 0.0F; + b_q[60] = 0.0F; + b_q[72] = 0.0F; + b_q[84] = 0.0F; + b_q[96] = 0.0F; + b_q[108] = 0.0F; + b_q[120] = 0.0F; + b_q[132] = 0.0F; + b_q[1] = 0.0F; + b_q[13] = q[0]; + b_q[25] = 0.0F; + b_q[37] = 0.0F; + b_q[49] = 0.0F; + b_q[61] = 0.0F; + b_q[73] = 0.0F; + b_q[85] = 0.0F; + b_q[97] = 0.0F; + b_q[109] = 0.0F; + b_q[121] = 0.0F; + b_q[133] = 0.0F; + b_q[2] = 0.0F; + b_q[14] = 0.0F; + b_q[26] = q[0]; + b_q[38] = 0.0F; + b_q[50] = 0.0F; + b_q[62] = 0.0F; + b_q[74] = 0.0F; + b_q[86] = 0.0F; + b_q[98] = 0.0F; + b_q[110] = 0.0F; + b_q[122] = 0.0F; + b_q[134] = 0.0F; + b_q[3] = 0.0F; + b_q[15] = 0.0F; + b_q[27] = 0.0F; + b_q[39] = q[1]; + b_q[51] = 0.0F; + b_q[63] = 0.0F; + b_q[75] = 0.0F; + b_q[87] = 0.0F; + b_q[99] = 0.0F; + b_q[111] = 0.0F; + b_q[123] = 0.0F; + b_q[135] = 0.0F; + b_q[4] = 0.0F; + b_q[16] = 0.0F; + b_q[28] = 0.0F; + b_q[40] = 0.0F; + b_q[52] = q[1]; + b_q[64] = 0.0F; + b_q[76] = 0.0F; + b_q[88] = 0.0F; + b_q[100] = 0.0F; + b_q[112] = 0.0F; + b_q[124] = 0.0F; + b_q[136] = 0.0F; + b_q[5] = 0.0F; + b_q[17] = 0.0F; + b_q[29] = 0.0F; + b_q[41] = 0.0F; + b_q[53] = 0.0F; + b_q[65] = q[1]; + b_q[77] = 0.0F; + b_q[89] = 0.0F; + b_q[101] = 0.0F; + b_q[113] = 0.0F; + b_q[125] = 0.0F; + b_q[137] = 0.0F; + b_q[6] = 0.0F; + b_q[18] = 0.0F; + b_q[30] = 0.0F; + b_q[42] = 0.0F; + b_q[54] = 0.0F; + b_q[66] = 0.0F; + b_q[78] = q[2]; + b_q[90] = 0.0F; + b_q[102] = 0.0F; + b_q[114] = 0.0F; + b_q[126] = 0.0F; + b_q[138] = 0.0F; + b_q[7] = 0.0F; + b_q[19] = 0.0F; + b_q[31] = 0.0F; + b_q[43] = 0.0F; + b_q[55] = 0.0F; + b_q[67] = 0.0F; + b_q[79] = 0.0F; + b_q[91] = q[2]; + b_q[103] = 0.0F; + b_q[115] = 0.0F; + b_q[127] = 0.0F; + b_q[139] = 0.0F; + b_q[8] = 0.0F; + b_q[20] = 0.0F; + b_q[32] = 0.0F; + b_q[44] = 0.0F; + b_q[56] = 0.0F; + b_q[68] = 0.0F; + b_q[80] = 0.0F; + b_q[92] = 0.0F; + b_q[104] = q[2]; + b_q[116] = 0.0F; + b_q[128] = 0.0F; + b_q[140] = 0.0F; + b_q[9] = 0.0F; + b_q[21] = 0.0F; + b_q[33] = 0.0F; + b_q[45] = 0.0F; + b_q[57] = 0.0F; + b_q[69] = 0.0F; + b_q[81] = 0.0F; + b_q[93] = 0.0F; + b_q[105] = 0.0F; + b_q[117] = q[3]; + b_q[129] = 0.0F; + b_q[141] = 0.0F; + b_q[10] = 0.0F; + b_q[22] = 0.0F; + b_q[34] = 0.0F; + b_q[46] = 0.0F; + b_q[58] = 0.0F; + b_q[70] = 0.0F; + b_q[82] = 0.0F; + b_q[94] = 0.0F; + b_q[106] = 0.0F; + b_q[118] = 0.0F; + b_q[130] = q[3]; + b_q[142] = 0.0F; + b_q[11] = 0.0F; + b_q[23] = 0.0F; + b_q[35] = 0.0F; + b_q[47] = 0.0F; + b_q[59] = 0.0F; + b_q[71] = 0.0F; + b_q[83] = 0.0F; + b_q[95] = 0.0F; + b_q[107] = 0.0F; + b_q[119] = 0.0F; + b_q[131] = 0.0F; + b_q[143] = q[3]; for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { A_lin[i + 12 * i0] = 0.0F; @@ -347,38 +501,63 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * i0]; } + + c_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; + } + } + + for (i0 = 0; i0 < 12; i0++) { + d_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + } + + e_A_lin[i + 12 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; + } } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - f0 += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; - } - - P_apriori[i + 12 * i0] = f0 + Q[i + 12 * i0]; + P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; } } - /* %update */ - /* 'attitudeKalmanfilter:102' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ + /* % update */ + /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:103' R=diag(r); */ - b_diag(r, R); + /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ + if ((z[5] < 4.0F) || (z[4] > 15.0F)) { + /* 'attitudeKalmanfilter:112' r(2)=10000; */ + r[1] = 10000.0F; + } + /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ + /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ + /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ + /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ + /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ + /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:106' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:107' Z, Z, E, Z; */ - /* 'attitudeKalmanfilter:108' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:110' y_k=z(1:9)-H_k*x_apriori; */ - /* 'attitudeKalmanfilter:112' S_k=H_k*P_apriori*H_k'+R; */ - /* 'attitudeKalmanfilter:113' K_k=(P_apriori*H_k'/(S_k)); */ + /* [zw;ze;zmk]; */ + /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ + /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ + /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ + /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 9; i0++) { b_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv0[i1 + b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + 12 * i0]; } } @@ -388,53 +567,136 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { K_k[i + 9 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - K_k[i + 9 * i0] += (real32_T)iv1[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; + } + } + + for (i0 = 0; i0 < 9; i0++) { + fv0[i + 9 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; } } } + b_r[0] = r[0]; + b_r[9] = 0.0F; + b_r[18] = 0.0F; + b_r[27] = 0.0F; + b_r[36] = 0.0F; + b_r[45] = 0.0F; + b_r[54] = 0.0F; + b_r[63] = 0.0F; + b_r[72] = 0.0F; + b_r[1] = 0.0F; + b_r[10] = r[0]; + b_r[19] = 0.0F; + b_r[28] = 0.0F; + b_r[37] = 0.0F; + b_r[46] = 0.0F; + b_r[55] = 0.0F; + b_r[64] = 0.0F; + b_r[73] = 0.0F; + b_r[2] = 0.0F; + b_r[11] = 0.0F; + b_r[20] = r[0]; + b_r[29] = 0.0F; + b_r[38] = 0.0F; + b_r[47] = 0.0F; + b_r[56] = 0.0F; + b_r[65] = 0.0F; + b_r[74] = 0.0F; + b_r[3] = 0.0F; + b_r[12] = 0.0F; + b_r[21] = 0.0F; + b_r[30] = r[1]; + b_r[39] = 0.0F; + b_r[48] = 0.0F; + b_r[57] = 0.0F; + b_r[66] = 0.0F; + b_r[75] = 0.0F; + b_r[4] = 0.0F; + b_r[13] = 0.0F; + b_r[22] = 0.0F; + b_r[31] = 0.0F; + b_r[40] = r[1]; + b_r[49] = 0.0F; + b_r[58] = 0.0F; + b_r[67] = 0.0F; + b_r[76] = 0.0F; + b_r[5] = 0.0F; + b_r[14] = 0.0F; + b_r[23] = 0.0F; + b_r[32] = 0.0F; + b_r[41] = 0.0F; + b_r[50] = r[1]; + b_r[59] = 0.0F; + b_r[68] = 0.0F; + b_r[77] = 0.0F; + b_r[6] = 0.0F; + b_r[15] = 0.0F; + b_r[24] = 0.0F; + b_r[33] = 0.0F; + b_r[42] = 0.0F; + b_r[51] = 0.0F; + b_r[60] = r[2]; + b_r[69] = 0.0F; + b_r[78] = 0.0F; + b_r[7] = 0.0F; + b_r[16] = 0.0F; + b_r[25] = 0.0F; + b_r[34] = 0.0F; + b_r[43] = 0.0F; + b_r[52] = 0.0F; + b_r[61] = 0.0F; + b_r[70] = r[2]; + b_r[79] = 0.0F; + b_r[8] = 0.0F; + b_r[17] = 0.0F; + b_r[26] = 0.0F; + b_r[35] = 0.0F; + b_r[44] = 0.0F; + b_r[53] = 0.0F; + b_r[62] = 0.0F; + b_r[71] = 0.0F; + b_r[80] = r[2]; for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 9; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - f0 += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; - } - - fv0[i + 9 * i0] = f0 + R[i + 9 * i0]; + fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; } } - mrdivide(b_P_apriori, fv0, K_k); + mrdivide(b_P_apriori, fv1, K_k); - /* 'attitudeKalmanfilter:116' x_aposteriori=x_apriori+K_k*y_k; */ + /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; + f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; } - c_a[i] = z[i] - f0; + O[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 9; i0++) { - f0 += K_k[i + 12 * i0] * c_a[i0]; + f0 += K_k[i + 12 * i0] * O[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:117' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ + /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 9; i1++) { - f0 += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; + f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -442,60 +704,72 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 + * i0]; } } } } else { - /* 'attitudeKalmanfilter:118' else */ - /* 'attitudeKalmanfilter:119' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:138' else */ + /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:120' R=diag(r(1:3)); */ - c_diag(*(real32_T (*)[3])&r[0], O); - + /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ + /* 'attitudeKalmanfilter:142' 0,r(1),0; */ + /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:123' H_k=[ E, Z, Z, Z]; */ - /* 'attitudeKalmanfilter:125' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:127' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ - /* 'attitudeKalmanfilter:128' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ + /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { c_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv2[i1 + 12 * i0]; + iv3[i1 + 12 * i0]; } } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { - fv1[i + 3 * i0] = 0.0F; + fv2[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * + fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } - } - for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { - f0 = 0.0F; + O[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; + O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; } - - c_a[i + 3 * i0] = f0 + O[i + 3 * i0]; } } - b_mrdivide(c_P_apriori, c_a, S_k); + c_r[0] = r[0]; + c_r[3] = 0.0F; + c_r[6] = 0.0F; + c_r[1] = 0.0F; + c_r[4] = r[0]; + c_r[7] = 0.0F; + c_r[2] = 0.0F; + c_r[5] = 0.0F; + c_r[8] = r[0]; + for (i = 0; i < 3; i++) { + for (i0 = 0; i0 < 3; i0++) { + a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; + } + } - /* 'attitudeKalmanfilter:131' x_aposteriori=x_apriori+K_k*y_k; */ + b_mrdivide(c_P_apriori, a, b_K_k); + + /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; + f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; } x_n_b[i] = z[i] - f0; @@ -504,22 +778,22 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 3; i0++) { - f0 += S_k[i + 12 * i0] * x_n_b[i0]; + f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:132' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 3; i1++) { - f0 += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; + f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -527,87 +801,134 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:133' else */ - /* 'attitudeKalmanfilter:134' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ + /* 'attitudeKalmanfilter:156' else */ + /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { - /* 'attitudeKalmanfilter:135' R=diag(r(1:6)); */ - d_diag(*(real32_T (*)[6])&r[0], S_k); + /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ + if ((z[5] < 4.0F) || (z[4] > 15.0F)) { + /* 'attitudeKalmanfilter:159' r(2)=10000; */ + r[1] = 10000.0F; + } + /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ + /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ + /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ + /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ + /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:138' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:139' Z, Z, E, Z]; */ - /* 'attitudeKalmanfilter:141' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:143' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - /* 'attitudeKalmanfilter:144' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ + /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) - iv4[i1 + 12 * i0]; + iv5[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { - b_K_k[i + 6 * i0] = 0.0F; + c_K_k[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - b_K_k[i + 6 * i0] += (real32_T)iv5[i + 6 * i1] * P_apriori[i1 + 12 + c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } - } - for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { - f0 = 0.0F; + fv2[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - f0 += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; + fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; } - - fv1[i + 6 * i0] = f0 + S_k[i + 6 * i0]; } } - c_mrdivide(d_P_apriori, fv1, b_K_k); + b_K_k[0] = r[0]; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r[0]; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r[0]; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r[1]; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r[1]; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r[1]; + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 6; i0++) { + c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; + } + } - /* 'attitudeKalmanfilter:147' x_aposteriori=x_apriori+K_k*y_k; */ + c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); + + /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { - f0 += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; + f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } - b_r[i] = z[i] - f0; + b_z[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - f0 += b_K_k[i + 12 * i0] * b_r[i0]; + f0 += c_K_k[i + 12 * i0] * b_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:148' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; + f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -615,49 +936,28 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * - i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:149' else */ - /* 'attitudeKalmanfilter:150' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ + /* 'attitudeKalmanfilter:181' else */ + /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) { - /* 'attitudeKalmanfilter:151' R=diag([r(1:3);r(7:9)]); */ + /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ + /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ + /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ + /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ + /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ + /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ /* observation matrix */ - /* 'attitudeKalmanfilter:154' H_k=[ E, Z, Z, Z; */ - /* 'attitudeKalmanfilter:155' Z, Z, Z, E]; */ - /* 'attitudeKalmanfilter:157' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ - /* 'attitudeKalmanfilter:159' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 12; i0++) { - b_K_k[i + 6 * i0] = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - b_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + - 12 * i0]; - } - } - } - - for (i = 0; i < 3; i++) { - b_r[i << 1] = r[i]; - b_r[1 + (i << 1)] = r[6 + i]; - } - - for (i = 0; i < 6; i++) { - for (i0 = 0; i0 < 6; i0++) { - f0 = 0.0F; - for (i1 = 0; i1 < 12; i1++) { - f0 += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; - } - - S_k[i + 6 * i0] = f0 + b_r[3 * (i + i0)]; - } - } - - /* 'attitudeKalmanfilter:160' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ + /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ + /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ + /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ + /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; @@ -668,45 +968,105 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } } - c_mrdivide(d_P_apriori, S_k, b_K_k); + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 12; i0++) { + c_K_k[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + + 12 * i0]; + } + } - /* 'attitudeKalmanfilter:163' x_aposteriori=x_apriori+K_k*y_k; */ + for (i0 = 0; i0 < 6; i0++) { + fv2[i + 6 * i0] = 0.0F; + for (i1 = 0; i1 < 12; i1++) { + fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * + i0]; + } + } + } + + b_K_k[0] = r[0]; + b_K_k[6] = 0.0F; + b_K_k[12] = 0.0F; + b_K_k[18] = 0.0F; + b_K_k[24] = 0.0F; + b_K_k[30] = 0.0F; + b_K_k[1] = 0.0F; + b_K_k[7] = r[0]; + b_K_k[13] = 0.0F; + b_K_k[19] = 0.0F; + b_K_k[25] = 0.0F; + b_K_k[31] = 0.0F; + b_K_k[2] = 0.0F; + b_K_k[8] = 0.0F; + b_K_k[14] = r[0]; + b_K_k[20] = 0.0F; + b_K_k[26] = 0.0F; + b_K_k[32] = 0.0F; + b_K_k[3] = 0.0F; + b_K_k[9] = 0.0F; + b_K_k[15] = 0.0F; + b_K_k[21] = r[2]; + b_K_k[27] = 0.0F; + b_K_k[33] = 0.0F; + b_K_k[4] = 0.0F; + b_K_k[10] = 0.0F; + b_K_k[16] = 0.0F; + b_K_k[22] = 0.0F; + b_K_k[28] = r[2]; + b_K_k[34] = 0.0F; + b_K_k[5] = 0.0F; + b_K_k[11] = 0.0F; + b_K_k[17] = 0.0F; + b_K_k[23] = 0.0F; + b_K_k[29] = 0.0F; + b_K_k[35] = r[2]; + for (i = 0; i < 6; i++) { + for (i0 = 0; i0 < 6; i0++) { + c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; + } + } + + c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); + + /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { - b_r[i] = z[i]; + b_z[i] = z[i]; } for (i = 0; i < 3; i++) { - b_r[i + 3] = z[i + 6]; + b_z[i + 3] = z[i + 6]; } for (i = 0; i < 6; i++) { - fv2[i] = 0.0F; + fv3[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { - fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; + fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; } - b_z[i] = b_r[i] - fv2[i]; + c_z[i] = b_z[i] - fv3[i]; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { - f0 += b_K_k[i + 12 * i0] * b_z[i0]; + f0 += c_K_k[i + 12 * i0] * c_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } - /* 'attitudeKalmanfilter:164' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ + /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { - f0 += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; + f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; } - Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; + b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } @@ -714,19 +1074,19 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { - P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 - * i0]; + P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * + P_apriori[i1 + 12 * i0]; } } } } else { - /* 'attitudeKalmanfilter:165' else */ - /* 'attitudeKalmanfilter:166' x_aposteriori=x_apriori; */ + /* 'attitudeKalmanfilter:202' else */ + /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ for (i = 0; i < 12; i++) { x_aposteriori[i] = x_apriori[i]; } - /* 'attitudeKalmanfilter:167' P_aposteriori=P_apriori; */ + /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); } } @@ -734,54 +1094,54 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const } /* % euler anglels extraction */ - /* 'attitudeKalmanfilter:176' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ + /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ for (i = 0; i < 3; i++) { x_n_b[i] = -x_aposteriori[i + 6]; } rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); - /* 'attitudeKalmanfilter:177' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ + /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& - x_aposteriori[9]), m_n_b); + x_aposteriori[9]), wak); - /* 'attitudeKalmanfilter:179' y_n_b=cross(z_n_b,m_n_b); */ + /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ for (i = 0; i < 3; i++) { - x_n_b[i] = m_n_b[i]; + x_n_b[i] = wak[i]; } - cross(z_n_b, x_n_b, m_n_b); + cross(z_n_b, x_n_b, wak); - /* 'attitudeKalmanfilter:180' y_n_b=y_n_b./norm(y_n_b); */ + /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ for (i = 0; i < 3; i++) { - x_n_b[i] = m_n_b[i]; + x_n_b[i] = wak[i]; } - rdivide(x_n_b, norm(m_n_b), m_n_b); + rdivide(x_n_b, norm(wak), wak); - /* 'attitudeKalmanfilter:182' x_n_b=(cross(y_n_b,z_n_b)); */ - cross(m_n_b, z_n_b, x_n_b); + /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ + cross(wak, z_n_b, x_n_b); - /* 'attitudeKalmanfilter:183' x_n_b=x_n_b./norm(x_n_b); */ + /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ for (i = 0; i < 3; i++) { b_x_aposteriori_k[i] = x_n_b[i]; } rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); - /* 'attitudeKalmanfilter:189' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ for (i = 0; i < 3; i++) { Rot_matrix[i] = x_n_b[i]; - Rot_matrix[3 + i] = m_n_b[i]; + Rot_matrix[3 + i] = wak[i]; Rot_matrix[6 + i] = z_n_b[i]; } - /* 'attitudeKalmanfilter:193' phi=atan2f(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:194' theta=-asinf(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:195' psi=atan2f(Rot_matrix(1,2),Rot_matrix(1,1)); */ - /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */ + /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h old mode 100644 new mode 100755 index c93d7f4bbc..afa63c1a91 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ @@ -29,6 +29,6 @@ /* Variable Definitions */ /* Function Declarations */ -extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); +extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]); #endif /* End of code generation (attitudeKalmanfilter.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c old mode 100644 new mode 100755 index 67b6fa4229..7d620d7fa1 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h old mode 100644 new mode 100755 index 610924d757..8b599be663 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_initialize' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c old mode 100644 new mode 100755 index c896027237..7f97274196 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h old mode 100644 new mode 100755 index a196307759..da84a50244 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter_terminate' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h old mode 100644 new mode 100755 index eba83d0d99..30fd1e75e6 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c old mode 100644 new mode 100755 index 42cd709715..84ada9002b --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h old mode 100644 new mode 100755 index c90d6b3a5c..e727f0684a --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -3,7 +3,7 @@ * * Code generation for function 'cross' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c old mode 100644 new mode 100755 index dbd44c6a82..b89ab58ef6 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h old mode 100644 new mode 100755 index 325fd23ec2..94fbe76717 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ * * Code generation for function 'eye' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c old mode 100644 new mode 100755 index 0a540775a0..a810f22e4f --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h old mode 100644 new mode 100755 index d286eda5ac..2d3b0d51ff --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'mrdivide' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c old mode 100644 new mode 100755 index deb71dc602..0c418cc7b6 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h old mode 100644 new mode 100755 index 3459cbdb5f..60cf77b571 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ * * Code generation for function 'norm' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.c b/apps/attitude_estimator_ekf/codegen/rdivide.c old mode 100644 new mode 100755 index 11e4e6f1f8..d035dae5e1 --- a/apps/attitude_estimator_ekf/codegen/rdivide.c +++ b/apps/attitude_estimator_ekf/codegen/rdivide.c @@ -3,7 +3,7 @@ * * Code generation for function 'rdivide' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rdivide.h b/apps/attitude_estimator_ekf/codegen/rdivide.h old mode 100644 new mode 100755 index 4e24303748..4bbebebe28 --- a/apps/attitude_estimator_ekf/codegen/rdivide.h +++ b/apps/attitude_estimator_ekf/codegen/rdivide.h @@ -3,7 +3,7 @@ * * Code generation for function 'rdivide' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c old mode 100644 new mode 100755 index d20fa13fca..34164d1044 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h old mode 100644 new mode 100755 index 83a3558735..145373cd02 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c old mode 100644 new mode 100755 index d357102afd..d84ca9573e --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h old mode 100644 new mode 100755 index 415927709b..65fdaa96f6 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h old mode 100644 new mode 100755 index 608391608f..3564983630 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c old mode 100644 new mode 100755 index 69069562b8..303d1d9d2e --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h old mode 100644 new mode 100755 index 9686964ae5..bd56b30d94 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h old mode 100644 new mode 100755 index 77fd3ec7ae..9a5c96267e --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ * * Code generation for function 'attitudeKalmanfilter' * - * C source code generated on: Tue Oct 16 15:27:58 2012 + * C source code generated on: Sat Jan 19 15:25:29 2013 * */ @@ -26,8 +26,8 @@ * Number of bits: char: 8 short: 16 int: 32 * long: 32 native word size: 32 * Byte ordering: LittleEndian - * Signed integer division rounds to: Undefined - * Shift right on a signed integer as arithmetic shift: off + * Signed integer division rounds to: Zero + * Shift right on a signed integer as arithmetic shift: on *=======================================================================*/ /*=======================================================================* diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 7e89dffb20..1d59f96778 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -290,7 +290,6 @@ void KalmanNav::updatePublications() _att.R_valid = true; _att.q_valid = true; - _att.counter = _navFrames; // selectively update publications, // do NOT call superblock do-all method diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 9491ce7e3b..ccca8bdc6d 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -511,7 +511,6 @@ handle_message(mavlink_message_t *msg) hil_attitude.yawspeed = hil_state.yawspeed; /* set timestamp and notify processes (broadcast) */ - hil_attitude.counter++; hil_attitude.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); } diff --git a/apps/uORB/topics/vehicle_attitude.h b/apps/uORB/topics/vehicle_attitude.h old mode 100644 new mode 100755 index 009379920a..c31c81d0ce --- a/apps/uORB/topics/vehicle_attitude.h +++ b/apps/uORB/topics/vehicle_attitude.h @@ -57,29 +57,26 @@ */ struct vehicle_attitude_s { - /* - * Actual data, this is specific to the type of data which is stored in this struct - * A line containing L0GME will be added by the Python logging code generator to the - * logged dataset. - */ - uint64_t timestamp; /**< in microseconds since system start */ + uint64_t timestamp; /**< in microseconds since system start */ /* This is similar to the mavlink message ATTITUDE, but for onboard use */ - /* @warning Roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */ + /** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */ - float roll; /**< Roll angle (rad, Tait-Bryan, NED) LOGME */ - float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) LOGME */ - float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) LOGME */ - float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) LOGME */ - float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) LOGME */ - float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) LOGME */ - float rate_offsets[3];/**< Offsets of the body angular rates from zero */ - float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ - float q[4]; /**< Quaternion (NED) */ - bool R_valid; /**< Rotation matrix valid */ - bool q_valid; /**< Quaternion valid */ - uint16_t counter; /**< Counter of all attitude messages (wraps) */ + float roll; /**< Roll angle (rad, Tait-Bryan, NED) */ + float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) */ + float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) */ + float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */ + float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */ + float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */ + float rollacc; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */ + float pitchacc; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */ + float yawacc; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */ + float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ + float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ + float q[4]; /**< Quaternion (NED) */ + bool R_valid; /**< Rotation matrix valid */ + bool q_valid; /**< Quaternion valid */ }; From 0e01f2b6fb843903126f43943cb0d283728786b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 23 Jan 2013 18:23:55 +0100 Subject: [PATCH 02/30] Removed unused files --- apps/attitude_estimator_ekf/codegen/diag.c | 78 ------------------- apps/attitude_estimator_ekf/codegen/diag.h | 37 --------- apps/attitude_estimator_ekf/codegen/power.c | 84 --------------------- apps/attitude_estimator_ekf/codegen/power.h | 34 --------- 4 files changed, 233 deletions(-) delete mode 100644 apps/attitude_estimator_ekf/codegen/diag.c delete mode 100644 apps/attitude_estimator_ekf/codegen/diag.h delete mode 100644 apps/attitude_estimator_ekf/codegen/power.c delete mode 100644 apps/attitude_estimator_ekf/codegen/power.h diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c deleted file mode 100644 index 7c28e873a3..0000000000 --- a/apps/attitude_estimator_ekf/codegen/diag.c +++ /dev/null @@ -1,78 +0,0 @@ -/* - * diag.c - * - * Code generation for function 'diag' - * - * C source code generated on: Tue Oct 16 15:27:58 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "diag.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ - -/* Function Definitions */ - -/* - * - */ -void b_diag(const real32_T v[9], real32_T d[81]) -{ - int32_T j; - memset(&d[0], 0, 81U * sizeof(real32_T)); - for (j = 0; j < 9; j++) { - d[j + 9 * j] = v[j]; - } -} - -/* - * - */ -void c_diag(const real32_T v[3], real32_T d[9]) -{ - int32_T j; - for (j = 0; j < 9; j++) { - d[j] = 0.0F; - } - - for (j = 0; j < 3; j++) { - d[j + 3 * j] = v[j]; - } -} - -/* - * - */ -void d_diag(const real32_T v[6], real32_T d[36]) -{ - int32_T j; - memset(&d[0], 0, 36U * sizeof(real32_T)); - for (j = 0; j < 6; j++) { - d[j + 6 * j] = v[j]; - } -} - -/* - * - */ -void diag(const real32_T v[12], real32_T d[144]) -{ - int32_T j; - memset(&d[0], 0, 144U * sizeof(real32_T)); - for (j = 0; j < 12; j++) { - d[j + 12 * j] = v[j]; - } -} - -/* End of code generation (diag.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h deleted file mode 100644 index 98b411c2d1..0000000000 --- a/apps/attitude_estimator_ekf/codegen/diag.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * diag.h - * - * Code generation for function 'diag' - * - * C source code generated on: Tue Oct 16 15:27:58 2012 - * - */ - -#ifndef __DIAG_H__ -#define __DIAG_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void b_diag(const real32_T v[9], real32_T d[81]); -extern void c_diag(const real32_T v[3], real32_T d[9]); -extern void d_diag(const real32_T v[6], real32_T d[36]); -extern void diag(const real32_T v[12], real32_T d[144]); -#endif -/* End of code generation (diag.h) */ diff --git a/apps/attitude_estimator_ekf/codegen/power.c b/apps/attitude_estimator_ekf/codegen/power.c deleted file mode 100644 index fc602fb5ce..0000000000 --- a/apps/attitude_estimator_ekf/codegen/power.c +++ /dev/null @@ -1,84 +0,0 @@ -/* - * power.c - * - * Code generation for function 'power' - * - * C source code generated on: Tue Oct 16 15:27:58 2012 - * - */ - -/* Include files */ -#include "rt_nonfinite.h" -#include "attitudeKalmanfilter.h" -#include "power.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1); - -/* Function Definitions */ -static real32_T rt_powf_snf(real32_T u0, real32_T u1) -{ - real32_T y; - real32_T f1; - real32_T f2; - if (rtIsNaNF(u0) || rtIsNaNF(u1)) { - y = ((real32_T)rtNaN); - } else { - f1 = (real32_T)fabs(u0); - f2 = (real32_T)fabs(u1); - if (rtIsInfF(u1)) { - if (f1 == 1.0F) { - y = ((real32_T)rtNaN); - } else if (f1 > 1.0F) { - if (u1 > 0.0F) { - y = ((real32_T)rtInf); - } else { - y = 0.0F; - } - } else if (u1 > 0.0F) { - y = 0.0F; - } else { - y = ((real32_T)rtInf); - } - } else if (f2 == 0.0F) { - y = 1.0F; - } else if (f2 == 1.0F) { - if (u1 > 0.0F) { - y = u0; - } else { - y = 1.0F / u0; - } - } else if (u1 == 2.0F) { - y = u0 * u0; - } else if ((u1 == 0.5F) && (u0 >= 0.0F)) { - y = (real32_T)sqrt(u0); - } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) { - y = ((real32_T)rtNaN); - } else { - y = (real32_T)pow(u0, u1); - } - } - - return y; -} - -/* - * - */ -void power(const real32_T a[12], real32_T y[12]) -{ - int32_T k; - for (k = 0; k < 12; k++) { - y[k] = rt_powf_snf(a[k], 2.0F); - } -} - -/* End of code generation (power.c) */ diff --git a/apps/attitude_estimator_ekf/codegen/power.h b/apps/attitude_estimator_ekf/codegen/power.h deleted file mode 100644 index 40a0d9353f..0000000000 --- a/apps/attitude_estimator_ekf/codegen/power.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * power.h - * - * Code generation for function 'power' - * - * C source code generated on: Tue Oct 16 15:27:58 2012 - * - */ - -#ifndef __POWER_H__ -#define __POWER_H__ -/* Include files */ -#include -#include -#include -#include -#include "rt_defines.h" -#include "rt_nonfinite.h" - -#include "rtwtypes.h" -#include "attitudeKalmanfilter_types.h" - -/* Type Definitions */ - -/* Named Constants */ - -/* Variable Declarations */ - -/* Variable Definitions */ - -/* Function Declarations */ -extern void power(const real32_T a[12], real32_T y[12]); -#endif -/* End of code generation (power.h) */ From 7ccc57f3c0c5616347f42a41ae06fc17486ac49e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Feb 2013 13:02:20 +0100 Subject: [PATCH 03/30] Fixed test outputs, decoupled tests from NuttX low-level output via message() macro --- apps/px4/tests/test_adc.c | 8 +++++--- apps/px4/tests/tests_main.c | 12 ++++++++---- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/apps/px4/tests/test_adc.c b/apps/px4/tests/test_adc.c index 4c021303f4..030ac6e23c 100644 --- a/apps/px4/tests/test_adc.c +++ b/apps/px4/tests/test_adc.c @@ -60,8 +60,10 @@ int test_adc(int argc, char *argv[]) { int fd = open(ADC_DEVICE_PATH, O_RDONLY); - if (fd < 0) - err(1, "can't open ADC device"); + if (fd < 0) { + warnx("ERROR: can't open ADC device"); + return 1; + } for (unsigned i = 0; i < 5; i++) { /* make space for a maximum of eight channels */ @@ -82,7 +84,7 @@ int test_adc(int argc, char *argv[]) usleep(150000); } - message("\t ADC test successful.\n"); + warnx("\t ADC test successful.\n"); errout_with_dev: diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c index ad6828f201..9f8c5c9eae 100644 --- a/apps/px4/tests/tests_main.c +++ b/apps/px4/tests/tests_main.c @@ -135,6 +135,7 @@ test_all(int argc, char *argv[]) unsigned i; char *args[2] = {"all", NULL}; unsigned int failcount = 0; + unsigned int testcount = 0; bool passed[NTESTS]; printf("\nRunning all tests...\n\n"); @@ -156,6 +157,7 @@ test_all(int argc, char *argv[]) fflush(stdout); passed[i] = true; } + testcount++; } } @@ -178,7 +180,7 @@ test_all(int argc, char *argv[]) printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n"); printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); printf("\n"); - printf(" All tests passed (%d of %d)\n", i, i); + printf(" All tests passed (%d of %d)\n", testcount, testcount); } else { printf(" ______ ______ __ __ \n"); @@ -187,7 +189,7 @@ test_all(int argc, char *argv[]) printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n"); printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n"); printf("\n"); - printf(" Some tests failed (%d of %d)\n", failcount, i); + printf(" Some tests failed (%d of %d)\n", failcount, testcount); } printf("\n"); @@ -245,6 +247,7 @@ int test_jig(int argc, char *argv[]) unsigned i; char *args[2] = {"jig", NULL}; unsigned int failcount = 0; + unsigned int testcount = 0; bool passed[NTESTS]; printf("\nRunning all tests...\n\n"); @@ -264,6 +267,7 @@ int test_jig(int argc, char *argv[]) fflush(stdout); passed[i] = true; } + testcount++; } } @@ -284,7 +288,7 @@ int test_jig(int argc, char *argv[]) printf(" \\ \\_\\ \\_\\ \\ \\_____\\ \\ \\_____\\ \\ \\_____\\ \\ \\_\\ \\_\\ \n"); printf(" \\/_/\\/_/ \\/_____/ \\/_____/ \\/_____/ \\/_/\\/_/ \n"); printf("\n"); - printf(" All tests passed (%d of %d)\n", i, i); + printf(" All tests passed (%d of %d)\n", testcount, testcount); } else { printf(" ______ ______ __ __ \n"); printf(" /\\ ___\\ /\\ __ \\ /\\ \\ /\\ \\ \n"); @@ -292,7 +296,7 @@ int test_jig(int argc, char *argv[]) printf(" \\ \\_\\ \\ \\_\\ \\_\\ \\ \\_\\ \\ \\_____\\ \n"); printf(" \\/_/ \\/_/\\/_/ \\/_/ \\/_____/ \n"); printf("\n"); - printf(" Some tests failed (%d of %d)\n", failcount, i); + printf(" Some tests failed (%d of %d)\n", failcount, testcount); } printf("\n"); From 50b736333f97761e4613354ee853071c652736ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 15:57:12 +0100 Subject: [PATCH 04/30] Reduced, but functional u-blox series driver --- apps/drivers/drv_gps.h | 80 ++++ apps/drivers/gps/Makefile | 42 ++ apps/drivers/gps/gps.cpp | 627 ++++++++++++++++++++++++++ apps/drivers/gps/gps_helper.h | 47 ++ apps/drivers/gps/ubx.cpp | 818 ++++++++++++++++++++++++++++++++++ apps/drivers/gps/ubx.h | 369 +++++++++++++++ 6 files changed, 1983 insertions(+) create mode 100644 apps/drivers/drv_gps.h create mode 100644 apps/drivers/gps/Makefile create mode 100644 apps/drivers/gps/gps.cpp create mode 100644 apps/drivers/gps/gps_helper.h create mode 100644 apps/drivers/gps/ubx.cpp create mode 100644 apps/drivers/gps/ubx.h diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h new file mode 100644 index 0000000000..89fc54b0d9 --- /dev/null +++ b/apps/drivers/drv_gps.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file GPS driver interface. + */ + +#ifndef _DRV_GPS_H +#define _DRV_GPS_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define GPS_DEVICE_PATH "/dev/gps" + +typedef enum { + GPS_DRIVER_MODE_UBX = 0, + GPS_DRIVER_MODE_MTK19, + GPS_DRIVER_MODE_MTK16, + GPS_DRIVER_MODE_NMEA, +} gps_driver_mode_t; + + + + +/* + * ObjDev tag for GPS data. + */ +ORB_DECLARE(gps); + +/* + * ioctl() definitions + */ +#define _GPSIOCBASE (0x2800) //TODO: arbitrary choice... +#define _GPSIOC(_n) (_IOC(_GPSIOCBASE, _n)) + +/** configure ubx mode */ +#define GPS_CONFIGURE_UBX _GPSIOC(0) + +/** configure mtk mode */ +#define GPS_CONFIGURE_MTK19 _GPSIOC(1) +#define GPS_CONFIGURE_MTK16 _GPSIOC(2) + +/** configure mtk mode */ +#define GPS_CONFIGURE_NMEA _GPSIOC(3) + +#endif /* _DRV_GPS_H */ diff --git a/apps/drivers/gps/Makefile b/apps/drivers/gps/Makefile new file mode 100644 index 0000000000..3859a88a5a --- /dev/null +++ b/apps/drivers/gps/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# GPS driver +# + +APPNAME = gps +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp new file mode 100644 index 0000000000..7d6f7144ef --- /dev/null +++ b/apps/drivers/gps/gps.cpp @@ -0,0 +1,627 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps.cpp + * Driver for the GPS on UART6 + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +#include + +#include + +#include "ubx.h" + +#define SEND_BUFFER_LENGTH 100 +#define TIMEOUT 1000000 //1s + +#define NUMBER_OF_BAUDRATES 4 +#define CONFIG_TIMEOUT 2000000 + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + + + +class GPS : public device::CDev +{ +public: + GPS(); + ~GPS(); + + virtual int init(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +private: + + int _task_should_exit; + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; + unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; + uint8_t _send_buffer[SEND_BUFFER_LENGTH]; + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + volatile int _task; ///< worker task + + bool _response_received; + bool _config_needed; + bool _baudrate_changed; + bool _mode_changed; + bool _healthy; + gps_driver_mode_t _mode; + unsigned _messages_received; + + GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + + struct vehicle_gps_position_s _report; + orb_advert_t _report_pub; + + orb_advert_t _gps_topic; + + void recv(); + + void config(); + + static void task_main_trampoline(void *arg); + + + /** + * worker task + */ + void task_main(void); + + int set_baudrate(unsigned baud); + + /** + * Send a reset command to the GPS + */ + void cmd_reset(); + +}; + + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int gps_main(int argc, char *argv[]); + +namespace +{ + +GPS *g_dev; + +} + + +GPS::GPS() : + CDev("gps", GPS_DEVICE_PATH), + _task_should_exit(false), + _baudrates_to_try({9600, 38400, 57600, 115200}), + _config_needed(true), + _baudrate_changed(false), + _mode_changed(true), + _healthy(false), + _mode(GPS_DRIVER_MODE_UBX), + _messages_received(0), + _Helper(nullptr) +{ + /* we need this potentially before it could be set in task_main */ + g_dev = this; + + _debug_enabled = true; + debug("[gps driver] instantiated"); +} + +GPS::~GPS() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) + task_delete(_task); + g_dev = nullptr; +} + +int +GPS::init() +{ + int ret = ERROR; + + /* do regular cdev init */ + if (CDev::init() != OK) + goto out; + + /* start the IO interface task */ + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 4096, (main_t)&GPS::task_main_trampoline, nullptr); + + if (_task < 0) { + debug("task start failed: %d", errno); + return -errno; + } + +// if (_gps_topic < 0) +// debug("failed to create GPS object"); + + ret = OK; +out: + return ret; +} + +int +GPS::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case GPS_CONFIGURE_UBX: + if (_mode != GPS_DRIVER_MODE_UBX) { + _mode = GPS_DRIVER_MODE_UBX; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_MTK19: + if (_mode != GPS_DRIVER_MODE_MTK19) { + _mode = GPS_DRIVER_MODE_MTK19; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_MTK16: + if (_mode != GPS_DRIVER_MODE_MTK16) { + _mode = GPS_DRIVER_MODE_MTK16; + _mode_changed = true; + } + break; + case GPS_CONFIGURE_NMEA: + if (_mode != GPS_DRIVER_MODE_NMEA) { + _mode = GPS_DRIVER_MODE_NMEA; + _mode_changed = true; + } + break; + case SENSORIOCRESET: + cmd_reset(); + break; + } + + return ret; +} + +void +GPS::config() +{ + int length = 0; + + _Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH); + + /* the message needs to be sent at the old baudrate */ + if (length > 0) { + if (length != ::write(_serial_fd, _send_buffer, length)) { + debug("write config failed"); + return; + } + } + + if (_baudrate_changed) { + set_baudrate(_baudrate); + _baudrate_changed = false; + } +} + +void +GPS::task_main_trampoline(void *arg) +{ + g_dev->task_main(); +} + +void +GPS::task_main() +{ + log("starting"); + + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + + memset(&_report, 0, sizeof(_report)); + + /* open the serial port */ + _serial_fd = ::open("/dev/ttyS3", O_RDWR); + + uint8_t buf[256]; + int baud_i = 0; + uint64_t time_before_configuration; + + if (_serial_fd < 0) { + log("failed to open serial port: %d", errno); + goto out; + } + + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + debug("ready"); + + /* lock against the ioctl handler */ + lock(); + + + _baudrate = _baudrates_to_try[baud_i]; + set_baudrate(_baudrate); + + time_before_configuration = hrt_absolute_time(); + + /* loop handling received serial bytes */ + while (!_task_should_exit) { + + if (_mode_changed) { + if (_Helper != nullptr) { + delete(_Helper); + _Helper = nullptr; // XXX is this needed? + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(); + break; + case GPS_DRIVER_MODE_MTK19: + //_Helper = new MTK19(); + break; + case GPS_DRIVER_MODE_MTK16: + //_Helper = new MTK16(); + break; + case GPS_DRIVER_MODE_NMEA: + //_Helper = new NMEA(); + break; + default: + break; + } + _mode_changed = false; + } + + if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { + baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; + _baudrate = _baudrates_to_try[baud_i]; + set_baudrate(_baudrate); + time_before_configuration = hrt_absolute_time(); + } + + + int poll_timeout; + if (_config_needed) { + poll_timeout = 50; + } else { + poll_timeout = 250; + } + /* sleep waiting for data, but no more than 1000ms */ + unlock(); + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); + lock(); + + + + + /* this would be bad... */ + if (ret < 0) { + log("poll error %d", errno); + } else if (ret == 0) { + config(); + if (_config_needed == false) { + _config_needed = true; + debug("Lost GPS module"); + } + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + int count; + + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_serial_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ + int j; + for (j = 0; j < count; j++) { + _messages_received += _Helper->parse(buf[j], &_report); + } + if (_messages_received > 0) { + if (_config_needed == true) { + _config_needed = false; + debug("Found GPS module"); + } + + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + _messages_received = 0; + } + } + } + } +out: + debug("exiting"); + + ::close(_serial_fd); + + /* tell the dtor that we are exiting */ + _task = -1; + _exit(0); +} + +int +GPS::set_baudrate(unsigned baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n", baud); + return -EINVAL; + } + + struct termios uart_config; + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_serial_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + /* set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state); + return -1; + } + + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate (tcsetattr)\n"); + return -1; + } +} + +void +GPS::cmd_reset() +{ + _healthy = false; +} + +void +GPS::print_info() +{ + +} + +/** + * Local functions in support of the shell command. + */ +namespace gps +{ + +GPS *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new GPS; + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(GPS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + printf("Could not open device path: %s\n", GPS_DEVICE_PATH); + goto fail; + } + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +void +stop() +{ + delete g_dev; + g_dev = nullptr; + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(GPS_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + exit(0); +} + +/** + * Print the status of the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + + +int +gps_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + gps::start(); + + if (!strcmp(argv[1], "stop")) + gps::stop(); + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + gps::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + gps::reset(); + + /* + * Print driver status. + */ + if (!strcmp(argv[1], "status")) + gps::info(); + + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'"); +} diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h new file mode 100644 index 0000000000..baacf7cb47 --- /dev/null +++ b/apps/drivers/gps/gps_helper.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol definitions */ + +#ifndef GPS_HELPER_H + +class GPS_Helper +{ +public: + virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0; + virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0; +}; + +#endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp new file mode 100644 index 0000000000..981b023e55 --- /dev/null +++ b/apps/drivers/gps/ubx.cpp @@ -0,0 +1,818 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol implementation */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ubx.h" + + +UBX::UBX() : +_waited(0), +_config_state(UBX_CONFIG_STATE_PRT), +_new_nav_posllh(false), +_new_nav_timeutc(false), +_new_nav_dop(false), +_new_nav_sol(false), +_new_nav_velned(false) +{ + decodeInit(); + _waiting_for_ack = false; +} + +UBX::~UBX() +{ +} + +void +UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) +{ + /* make sure the buffer to write the message is long enough */ + assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); + + /* try again after 10 times */ + if (_waited > 10) { + _waiting_for_ack = false; + } + + if (!_waiting_for_ack) { + _waiting_for_ack = true; + _waited = 0; + if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { + config_needed = false; + length = 0; + _config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */ + _waiting_for_ack = false; + return; + } else if (_config_state == UBX_CONFIG_STATE_PRT) { + /* send a CFT-PRT message to define set ubx protocol and leave the baudrate as is, we just want an ACK-ACK from this */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = baudrate; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + length = sizeof(cfg_prt_packet)+2; + + } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { + +// printf("Send change of baudrate now\n"); + + /* send a CFT-PRT message to define set ubx protocol and and baudrate, now let's try to switch the baudrate */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + length = sizeof(cfg_prt_packet)+2; + + /* detection when the baudrate has been changed in the first step */ + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + /* set a flag and exit */ + baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; + baudrate_changed = true; + _config_state = UBX_CONFIG_STATE_RATE; + _waiting_for_ack = false; + return; + } + + + + } else if (_config_state == UBX_CONFIG_STATE_RATE) { + + /* send a CFT-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + + addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet)); + length = sizeof(cfg_rate_packet)+2; + + } else if (_config_state == UBX_CONFIG_STATE_NAV5) { + /* send a NAV5 message to set the options for the internal estimator */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); //set everything to 0 + + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet)); + length = sizeof(cfg_nav5_packet)+2; + + } else { + /* catch the remaining config states here */ + + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); //set everything to 0 + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1; + + switch (_config_state) { + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + cfg_msg_packet.rate[1] = 5; + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + break; +// case UBX_CONFIG_STATE_MSG_RXM_SVSI: +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; +// break; + default: + break; + } + + addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + + buffer[0] = UBX_SYNC1; + buffer[1] = UBX_SYNC2; + memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); + length = sizeof(cfg_msg_packet)+2; + } + } else { + _waited++; + } +} + +int +UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) +{ + int ret = 0; + //printf("Received char: %c\n", b); + + switch (_decode_state) { + /* First, look for sync1 */ + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } + break; + /* Second, look for sync2 */ + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + } else { + /* Second start symbol was wrong, reset state machine */ + decodeInit(); + } + break; + /* Now look for class */ + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + addByteToChecksum(b); + /* check for known class */ + switch (b) { + case UBX_CLASS_ACK: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = ACK; + break; + + case UBX_CLASS_NAV: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = NAV; + break; + +// case UBX_CLASS_RXM: +// _decode_state = UBX_DECODE_GOT_CLASS; +// _message_class = RXM; +// break; + + case UBX_CLASS_CFG: + _decode_state = UBX_DECODE_GOT_CLASS; + _message_class = CFG; + break; + default: //unknown class: reset state machine + decodeInit(); + break; + } + break; + case UBX_DECODE_GOT_CLASS: + addByteToChecksum(b); + switch (_message_class) { + case NAV: + switch (b) { + case UBX_MESSAGE_NAV_POSLLH: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_POSLLH; + break; + + case UBX_MESSAGE_NAV_SOL: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SOL; + break; + + case UBX_MESSAGE_NAV_TIMEUTC: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_TIMEUTC; + break; + + case UBX_MESSAGE_NAV_DOP: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_DOP; + break; + + case UBX_MESSAGE_NAV_SVINFO: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_SVINFO; + break; + + case UBX_MESSAGE_NAV_VELNED: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = NAV_VELNED; + break; + + default: //unknown class: reset state machine, should not happen + decodeInit(); + break; + } + break; +// case RXM: +// switch (b) { +// case UBX_MESSAGE_RXM_SVSI: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = RXM_SVSI; +// break; +// +// default: //unknown class: reset state machine, should not happen +// decodeInit(); +// break; +// } +// break; + + case CFG: + switch (b) { + case UBX_MESSAGE_CFG_NAV5: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = CFG_NAV5; + break; + + default: //unknown class: reset state machine, should not happen + decodeInit(); + break; + } + break; + + case ACK: + switch (b) { + case UBX_MESSAGE_ACK_ACK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_ACK; + break; + case UBX_MESSAGE_ACK_NAK: + _decode_state = UBX_DECODE_GOT_MESSAGEID; + _message_id = ACK_NAK; + break; + default: //unknown class: reset state machine, should not happen + decodeInit(); + break; + } + break; + default: //should not happen because we set the class + printf("UBX Error, we set a class that we don't know\n"); + decodeInit(); + break; + } + break; + case UBX_DECODE_GOT_MESSAGEID: + addByteToChecksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; + case UBX_DECODE_GOT_LENGTH1: + addByteToChecksum(b); + _payload_size += b << 8; // here comes the second byte of length + _decode_state = UBX_DECODE_GOT_LENGTH2; + break; + case UBX_DECODE_GOT_LENGTH2: + /* Add to checksum if not yet at checksum byte */ + if (_rx_count < _payload_size) + addByteToChecksum(b); + _rx_buffer[_rx_count] = b; + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + switch (_message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + gps_position->lat = packet->lat; + gps_position->lon = packet->lon; + gps_position->alt = packet->height_msl; + + gps_position->counter_pos_valid++; + gps_position->counter++; + + _new_nav_posllh = true; + + } else { + printf("[gps] NAV_POSLLH: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->fix_type = packet->gpsFix; + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + gps_position->s_variance = packet->sAcc; + gps_position->p_variance = packet->pAcc; + + _new_nav_sol = true; + + } else { + printf("[gps] NAV_SOL: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_DOP: { +// printf("GOT NAV_DOP MESSAGE\n"); + gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->eph = packet->hDOP; + gps_position->epv = packet->vDOP; + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + _new_nav_dop = true; + + } else { + printf("[gps] NAV_DOP: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + + time_t epoch = mktime(&timeinfo); + + gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + _new_nav_timeutc = true; + + } else { + printf("\t[gps] NAV_TIMEUTC: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char _rx_buffer_part3[length_part3]; + memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3; + + //Check if checksum is valid and then store the gps information + if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) { + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; //for temporal storage + + uint8_t satellites_used = 0; + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + + /* Write satellite information in the global storage */ + gps_position->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + if ((packet_part2->flags) & 1) { //flags is a bitfield + gps_position->satellite_used[i] = 1; + satellites_used++; + + } else { + gps_position->satellite_used[i] = 0; + } + + gps_position->satellite_snr[i] = packet_part2->cno; + gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + + } else { + gps_position->satellite_used[i] = 0; + gps_position->satellite_snr[i] = 0; + gps_position->satellite_elevation[i] = 0; + gps_position->satellite_azimuth[i] = 0; + } + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + gps_position->satellite_prn[i] = 0; + gps_position->satellite_used[i] = 0; + gps_position->satellite_snr[i] = 0; + gps_position->satellite_elevation[i] = 0; + gps_position->satellite_azimuth[i] = 0; + } + + /* set flag if any sat info is available */ + if (!packet_part1->numCh > 0) { + gps_position->satellite_info_available = 1; + + } else { + gps_position->satellite_info_available = 0; + } + + gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report + + } else { + printf("\t[gps] NAV_SVINFO: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + //Check if checksum is valid and the store the gps information + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + gps_position->vel = (uint16_t)packet->speed; + gps_position->vel_n = packet->velN / 100.0f; + gps_position->vel_e = packet->velE / 100.0f; + gps_position->vel_d = packet->velD / 100.0f; + gps_position->vel_ned_valid = true; + gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); + + gps_position->timestamp = hrt_absolute_time(); + gps_position->counter++; + + + _new_nav_velned = true; + + } else { + printf("[gps] NAV_VELNED: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + +// case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); +// const int length_part1 = 7; +// char _rx_buffer_part1[length_part1]; +// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); +// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// +// //Check if checksum is valid and the store the gps information +// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { +// +// gps_position->satellites_visible = packet->numVis; +// gps_position->timestamp = hrt_absolute_time(); +// gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// ret = 1; +// +// } else { +// printf("[gps] RXM_SVSI: checksum invalid\n"); +// } +// +// // Reset state machine to decode next packet +// decodeInit(); +// return ret; +// +// break; +// } + + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + //Check if checksum is valid + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + _waiting_for_ack = false; + + switch (_config_state) { + case UBX_CONFIG_STATE_PRT: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + _config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE; + break; + case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) + _config_state = UBX_CONFIG_STATE_RATE; + break; + case UBX_CONFIG_STATE_RATE: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE) + _config_state = UBX_CONFIG_STATE_NAV5; + break; + case UBX_CONFIG_STATE_NAV5: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) + _config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH; + break; + case UBX_CONFIG_STATE_MSG_NAV_POSLLH: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; + break; + case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_DOP; + break; + case UBX_CONFIG_STATE_MSG_NAV_DOP: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; + break; + case UBX_CONFIG_STATE_MSG_NAV_SVINFO: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; + break; + case UBX_CONFIG_STATE_MSG_NAV_SOL: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED; + break; + case UBX_CONFIG_STATE_MSG_NAV_VELNED: + if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) + _config_state = UBX_CONFIG_STATE_CONFIGURED; + break; +// case UBX_CONFIG_STATE_MSG_RXM_SVSI: +// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) +// _config_state = UBX_CONFIG_STATE_CONFIGURED; +// break; + default: + break; + } + } else { + printf("[gps] ACK_ACK: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; + + //Check if checksum is valid + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + + printf("[gps] the ubx gps returned: not acknowledged\n"); + ret = 1; + + } else { + printf("[gps] ACK_NAK: checksum invalid\n"); + } + + // Reset state machine to decode next packet + decodeInit(); + return ret; + + break; + } + + default: //we don't know the message + printf("Unknown message received: %d-%d\n",_message_class,_message_id); + decodeInit(); + + break; + } + } // end if _rx_count high enough + if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + printf("Buffer full"); + decodeInit(); + } + break; + default: + break; + } + + /* return 1 when all needed messages have arrived */ + if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + + gps_position->timestamp = hrt_absolute_time(); + ret = 1; + _new_nav_posllh = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; + } + + return ret; +} + +void +UBX::decodeInit(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = UBX_DECODE_UNINIT; + _message_class = CLASS_UNKNOWN; + _message_id = ID_UNKNOWN; + _payload_size = 0; +} + +void +UBX::addByteToChecksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} + +void +UBX::addChecksumToMessage(uint8_t* message, const unsigned length) +{ + uint8_t ck_a = 0; + uint8_t ck_b = 0; + unsigned i; + + for (i = 0; i < length-2; i++) { + ck_a = ck_a + message[i]; + ck_b = ck_b + ck_a; + } + message[length-2] = ck_a; + message[length-1] = ck_b; +} diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h new file mode 100644 index 0000000000..dff25a518f --- /dev/null +++ b/apps/drivers/gps/ubx.h @@ -0,0 +1,369 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file U-Blox protocol definitions */ + +#ifndef UBX_H_ +#define UBX_H_ + +#include "gps_helper.h" + +#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ + +#define UBX_SYNC1 0xB5 +#define UBX_SYNC2 0x62 + +//UBX Protocol definitions (this is the subset of the messages that are parsed) +#define UBX_CLASS_NAV 0x01 +//#define UBX_CLASS_RXM 0x02 +#define UBX_CLASS_ACK 0x05 +#define UBX_CLASS_CFG 0x06 +#define UBX_MESSAGE_NAV_POSLLH 0x02 +#define UBX_MESSAGE_NAV_SOL 0x06 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +#define UBX_MESSAGE_NAV_DOP 0x04 +#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_VELNED 0x12 +//#define UBX_MESSAGE_RXM_SVSI 0x20 +#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_CFG_PRT 0x00 +#define UBX_MESSAGE_CFG_NAV5 0x24 +#define UBX_MESSAGE_CFG_MSG 0x01 +#define UBX_MESSAGE_CFG_RATE 0x08 + +#define UBX_CFG_PRT_LENGTH 20 +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ +#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */ + +#define UBX_CFG_RATE_LENGTH 6 +#define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ +#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */ +#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */ + + +#define UBX_CFG_NAV5_LENGTH 36 +#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0001 /**< only update dynamic model and fix mode */ +#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */ +#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ + +#define UBX_CFG_MSG_LENGTH 8 +#define UBX_CFG_MSG_PAYLOAD_RATE1 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} UART1 chosen */ + + +// ************ +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t lon; // Longitude * 1e-7, deg + int32_t lat; // Latitude * 1e-7, deg + int32_t height; // Height above Ellipsoid, mm + int32_t height_msl; // Height above mean sea level, mm + uint32_t hAcc; // Horizontal Accuracy Estimate, mm + uint32_t vAcc; // Vertical Accuracy Estimate, mm + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_posllh_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 + int16_t week; // GPS week (GPS time) + uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix + uint8_t flags; + int32_t ecefX; + int32_t ecefY; + int32_t ecefZ; + uint32_t pAcc; + int32_t ecefVX; + int32_t ecefVY; + int32_t ecefVZ; + uint32_t sAcc; + uint16_t pDOP; + uint8_t reserved1; + uint8_t numSV; + uint32_t reserved2; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_sol_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint32_t time_accuracy; //Time Accuracy Estimate, ns + int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC) + uint16_t year; //Year, range 1999..2099 (UTC) + uint8_t month; //Month, range 1..12 (UTC) + uint8_t day; //Day of Month, range 1..31 (UTC) + uint8_t hour; //Hour of Day, range 0..23 (UTC) + uint8_t min; //Minute of Hour, range 0..59 (UTC) + uint8_t sec; //Seconds of Minute, range 0..59 (UTC) + uint8_t valid_flag; //Validity Flags (see ubx documentation) + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_timeutc_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint16_t gDOP; //Geometric DOP (scaling 0.01) + uint16_t pDOP; //Position DOP (scaling 0.01) + uint16_t tDOP; //Time DOP (scaling 0.01) + uint16_t vDOP; //Vertical DOP (scaling 0.01) + uint16_t hDOP; //Horizontal DOP (scaling 0.01) + uint16_t nDOP; //Northing DOP (scaling 0.01) + uint16_t eDOP; //Easting DOP (scaling 0.01) + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_dop_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + uint8_t numCh; //Number of channels + uint8_t globalFlags; + uint16_t reserved2; + +} gps_bin_nav_svinfo_part1_packet_t; + +typedef struct { + uint8_t chn; //Channel number, 255 for SVs not assigned to a channel + uint8_t svid; //Satellite ID + uint8_t flags; + uint8_t quality; + uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz + int8_t elev; //Elevation in integer degrees + int16_t azim; //Azimuth in integer degrees + int32_t prRes; //Pseudo range residual in centimetres + +} gps_bin_nav_svinfo_part2_packet_t; + +typedef struct { + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_svinfo_part3_packet_t; + +typedef struct { + uint32_t time_milliseconds; // GPS Millisecond Time of Week + int32_t velN; //NED north velocity, cm/s + int32_t velE; //NED east velocity, cm/s + int32_t velD; //NED down velocity, cm/s + uint32_t speed; //Speed (3-D), cm/s + uint32_t gSpeed; //Ground Speed (2-D), cm/s + int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5 + uint32_t sAcc; //Speed Accuracy Estimate, cm/s + uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5 + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_nav_velned_packet_t; + +//typedef struct { +// int32_t time_milliseconds; // Measurement integer millisecond GPS time of week +// int16_t week; //Measurement GPS week number +// uint8_t numVis; //Number of visible satellites +// +// //... rest of package is not used in this implementation +// +//} gps_bin_rxm_svsi_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_ack_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + + uint8_t ck_a; + uint8_t ck_b; +} gps_bin_ack_nak_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t portID; + uint8_t res0; + uint16_t res1; + uint32_t mode; + uint32_t baudRate; + uint16_t inProtoMask; + uint16_t outProtoMask; + uint16_t flags; + uint16_t pad; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_prt_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t measRate; + uint16_t navRate; + uint16_t timeRef; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_rate_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint16_t mask; + uint8_t dynModel; + uint8_t fixMode; + int32_t fixedAlt; + uint32_t fixedAltVar; + int8_t minElev; + uint8_t drLimit; + uint16_t pDop; + uint16_t tDop; + uint16_t pAcc; + uint16_t tAcc; + uint8_t staticHoldThresh; + uint8_t dgpsTimeOut; + uint32_t reserved2; + uint32_t reserved3; + uint32_t reserved4; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_nav5_packet_t; + +typedef struct { + uint8_t clsID; + uint8_t msgID; + uint16_t length; + uint8_t msgClass_payload; + uint8_t msgID_payload; + uint8_t rate[6]; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_bin_cfg_msg_packet_t; + + +// END the structures of the binary packets +// ************ + +typedef enum { + UBX_CONFIG_STATE_PRT = 0, + UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, + UBX_CONFIG_STATE_RATE, + UBX_CONFIG_STATE_NAV5, + UBX_CONFIG_STATE_MSG_NAV_POSLLH, + UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, + UBX_CONFIG_STATE_MSG_NAV_DOP, + UBX_CONFIG_STATE_MSG_NAV_SVINFO, + UBX_CONFIG_STATE_MSG_NAV_SOL, + UBX_CONFIG_STATE_MSG_NAV_VELNED, +// UBX_CONFIG_STATE_MSG_RXM_SVSI, + UBX_CONFIG_STATE_CONFIGURED +} ubx_config_state_t; + +typedef enum { + CLASS_UNKNOWN = 0, + NAV = 1, + RXM = 2, + ACK = 3, + CFG = 4 +} ubx_message_class_t; + +typedef enum { + //these numbers do NOT correspond to the message id numbers of the ubx protocol + ID_UNKNOWN = 0, + NAV_POSLLH, + NAV_SOL, + NAV_TIMEUTC, + NAV_DOP, + NAV_SVINFO, + NAV_VELNED, +// RXM_SVSI, + CFG_NAV5, + ACK_ACK, + ACK_NAK, +} ubx_message_id_t; + +typedef enum { + UBX_DECODE_UNINIT = 0, + UBX_DECODE_GOT_SYNC1, + UBX_DECODE_GOT_SYNC2, + UBX_DECODE_GOT_CLASS, + UBX_DECODE_GOT_MESSAGEID, + UBX_DECODE_GOT_LENGTH1, + UBX_DECODE_GOT_LENGTH2 +} ubx_decode_state_t; + +//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t; +#pragma pack(pop) + +#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer + +class UBX : public GPS_Helper +{ +public: + UBX(); + ~UBX(); + + void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned); + int parse(uint8_t, struct vehicle_gps_position_s*); + +private: + void decodeInit(void); + void addByteToChecksum(uint8_t); + void addChecksumToMessage(uint8_t*, const unsigned); + unsigned _waited; + bool _waiting_for_ack; + ubx_config_state_t _config_state; + ubx_decode_state_t _decode_state; + uint8_t _rx_buffer[RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; + ubx_message_class_t _message_class; + ubx_message_id_t _message_id; + unsigned _payload_size; + bool _new_nav_posllh; + bool _new_nav_timeutc; + bool _new_nav_dop; + bool _new_nav_sol; + bool _new_nav_velned; +}; + +#endif /* UBX_H_ */ From 3fd8c73bfbdad1fdb9b5b31337cf6eb3fa46c34c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 15:58:53 +0100 Subject: [PATCH 05/30] Disabled old-style gps interface, enabled GPS driver --- apps/drivers/gps/gps.cpp | 2 ++ nuttx/configs/px4fmu/nsh/appconfig | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 7d6f7144ef..9fd679240f 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -471,6 +471,8 @@ GPS::set_baudrate(unsigned baud) warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } + + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ } void diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 653f97de11..710e2fb5f5 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -80,7 +80,6 @@ CONFIGURED_APPS += uORB CONFIGURED_APPS += mavlink CONFIGURED_APPS += mavlink_onboard -CONFIGURED_APPS += gps CONFIGURED_APPS += commander CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors @@ -114,6 +113,7 @@ CONFIGURED_APPS += drivers/stm32/tone_alarm CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil +CONFIGURED_APPS += drivers/gps # Testing stuff CONFIGURED_APPS += px4/sensors_bringup From 12f4cb2dc38303b58750a8f3b120738a291ac8d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 16:13:17 +0100 Subject: [PATCH 06/30] Tuned GPS update rates --- apps/mavlink/mavlink.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ceb7c25549..09abcf2693 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -625,7 +625,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 2 Hz */ + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 100); + /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { @@ -634,8 +636,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 100 ms */ + /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); @@ -651,6 +655,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ From 30f028908a69db058c05b9af7e31c8c52c3bc339 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 16:15:48 +0100 Subject: [PATCH 07/30] Fixed typo --- apps/mavlink/mavlink.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 09abcf2693..b958d5f96c 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -626,7 +626,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); @@ -639,7 +639,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 200); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); @@ -656,7 +656,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 500); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ From 13ec0675703ecc9c7bccb22ef3cd049888dd1abb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 17:55:58 +0100 Subject: [PATCH 08/30] Minor quick cleanups --- apps/drivers/gps/gps.cpp | 108 +++++++++++++++++++++++++++------------ apps/drivers/gps/ubx.cpp | 28 +++++----- 2 files changed, 89 insertions(+), 47 deletions(-) diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 9fd679240f..6f7310cc5c 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -107,30 +107,29 @@ public: private: - int _task_should_exit; - int _serial_fd; ///< serial interface to GPS + int _task_should_exit; + int _serial_fd; ///< serial interface to GPS unsigned _baudrate; unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; uint8_t _send_buffer[SEND_BUFFER_LENGTH]; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - volatile int _task; ///< worker task + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + volatile int _task; ///< worker task bool _response_received; bool _config_needed; bool _baudrate_changed; bool _mode_changed; bool _healthy; - gps_driver_mode_t _mode; + gps_driver_mode_t _mode; unsigned _messages_received; + float _rate; ///< position update rate - GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper - struct vehicle_gps_position_s _report; - orb_advert_t _report_pub; - - orb_advert_t _gps_topic; + struct vehicle_gps_position_s _report; ///< the current GPS report + orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message void recv(); @@ -140,16 +139,19 @@ private: /** - * worker task + * Worker task. */ - void task_main(void); + void task_main(void); - int set_baudrate(unsigned baud); + /** + * Set the module baud rate + */ + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS */ - void cmd_reset(); + void cmd_reset(); }; @@ -177,13 +179,15 @@ GPS::GPS() : _healthy(false), _mode(GPS_DRIVER_MODE_UBX), _messages_received(0), - _Helper(nullptr) + _Helper(nullptr), + _rate(0.0f), + _report_pub(-1) { /* we need this potentially before it could be set in task_main */ g_dev = this; + memset(&_report, 0, sizeof(_report)); _debug_enabled = true; - debug("[gps driver] instantiated"); } GPS::~GPS() @@ -213,16 +217,13 @@ GPS::init() goto out; /* start the IO interface task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 4096, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { - debug("task start failed: %d", errno); + warnx("task start failed: %d", errno); return -errno; } -// if (_gps_topic < 0) -// debug("failed to create GPS object"); - ret = OK; out: return ret; @@ -231,6 +232,8 @@ out: int GPS::ioctl(struct file *filp, int cmd, unsigned long arg) { + lock(); + int ret = OK; switch (cmd) { @@ -263,6 +266,8 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) break; } + unlock(); + return ret; } @@ -298,10 +303,6 @@ GPS::task_main() { log("starting"); - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - - memset(&_report, 0, sizeof(_report)); - /* open the serial port */ _serial_fd = ::open("/dev/ttyS3", O_RDWR); @@ -311,7 +312,9 @@ GPS::task_main() if (_serial_fd < 0) { log("failed to open serial port: %d", errno); - goto out; + /* tell the dtor that we are exiting, set error code */ + _task = -1; + _exit(1); } /* poll descriptor */ @@ -329,6 +332,9 @@ GPS::task_main() time_before_configuration = hrt_absolute_time(); + uint64_t last_rate_measurement = hrt_absolute_time(); + unsigned last_rate_count = 0; + /* loop handling received serial bytes */ while (!_task_should_exit) { @@ -369,9 +375,9 @@ GPS::task_main() if (_config_needed) { poll_timeout = 50; } else { - poll_timeout = 250; + poll_timeout = 400; } - /* sleep waiting for data, but no more than 1000ms */ + /* sleep waiting for data, but no more than the poll timeout */ unlock(); int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); lock(); @@ -386,7 +392,7 @@ GPS::task_main() config(); if (_config_needed == false) { _config_needed = true; - debug("Lost GPS module"); + warnx("lost GPS module"); } } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -411,8 +417,22 @@ GPS::task_main() debug("Found GPS module"); } - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + _messages_received = 0; + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > 5000000) { + _rate = last_rate_count / (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + } } } } @@ -484,7 +504,29 @@ GPS::cmd_reset() void GPS::print_info() { - + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + case GPS_DRIVER_MODE_MTK19: + warnx("protocol: MTK 1.9"); + break; + case GPS_DRIVER_MODE_MTK16: + warnx("protocol: MTK 1.6"); + break; + case GPS_DRIVER_MODE_NMEA: + warnx("protocol: NMEA"); + break; + default: + break; + } + warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); + if (_report.timestamp != 0) { + warnx("position lock: %dD, last update %d seconds ago", (int)_report.fix_type, + int((hrt_absolute_time() - _report.timestamp) / 1000000)); + warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); + warnx("update rate: %6.2f Hz", (double)_rate); + } } /** diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 981b023e55..7e95514ddc 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -371,7 +371,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } break; default: //should not happen because we set the class - printf("UBX Error, we set a class that we don't know\n"); + warnx("UBX Error, we set a class that we don't know\n"); decodeInit(); break; } @@ -410,7 +410,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_posllh = true; } else { - printf("[gps] NAV_POSLLH: checksum invalid\n"); + warnx("NAV_POSLLH: checksum invalid\n"); } // Reset state machine to decode next packet @@ -437,7 +437,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_sol = true; } else { - printf("[gps] NAV_SOL: checksum invalid\n"); + warnx("NAV_SOL: checksum invalid\n"); } // Reset state machine to decode next packet @@ -463,7 +463,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_dop = true; } else { - printf("[gps] NAV_DOP: checksum invalid\n"); + warnx("NAV_DOP: checksum invalid\n"); } // Reset state machine to decode next packet @@ -624,7 +624,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_velned = true; } else { - printf("[gps] NAV_VELNED: checksum invalid\n"); + warnx("NAV_VELNED: checksum invalid\n"); } // Reset state machine to decode next packet @@ -651,7 +651,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // ret = 1; // // } else { -// printf("[gps] RXM_SVSI: checksum invalid\n"); +// warnx("RXM_SVSI: checksum invalid\n"); // } // // // Reset state machine to decode next packet @@ -719,7 +719,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } } else { - printf("[gps] ACK_ACK: checksum invalid\n"); + warnx("ACK_ACK: checksum invalid\n"); } // Reset state machine to decode next packet @@ -736,11 +736,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - printf("[gps] the ubx gps returned: not acknowledged\n"); + warnx("the ubx gps returned: not acknowledged\n"); ret = 1; } else { - printf("[gps] ACK_NAK: checksum invalid\n"); + warnx("ACK_NAK: checksum invalid\n"); } // Reset state machine to decode next packet @@ -768,16 +768,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } - /* return 1 when all needed messages have arrived */ + /* return 1 when position updates and the remaining packets updated at least once */ if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { gps_position->timestamp = hrt_absolute_time(); ret = 1; _new_nav_posllh = false; - _new_nav_timeutc = false; - _new_nav_dop = false; - _new_nav_sol = false; - _new_nav_velned = false; + // _new_nav_timeutc = false; + // _new_nav_dop = false; + // _new_nav_sol = false; + // _new_nav_velned = false; } return ret; From d4bd7225baa8c46e6a93dcd063f66ce53fe88e69 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 18:00:10 +0100 Subject: [PATCH 09/30] More cleanup --- apps/drivers/gps/gps.cpp | 3 ++- apps/drivers/gps/ubx.cpp | 22 +++++++++++----------- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 6f7310cc5c..bd77e8969a 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -341,7 +341,8 @@ GPS::task_main() if (_mode_changed) { if (_Helper != nullptr) { delete(_Helper); - _Helper = nullptr; // XXX is this needed? + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; } switch (_mode) { diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 7e95514ddc..fe4758d8cf 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -371,7 +371,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } break; default: //should not happen because we set the class - warnx("UBX Error, we set a class that we don't know\n"); + warnx("UBX Error, we set a class that we don't know"); decodeInit(); break; } @@ -410,7 +410,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_posllh = true; } else { - warnx("NAV_POSLLH: checksum invalid\n"); + warnx("NAV_POSLLH: checksum invalid"); } // Reset state machine to decode next packet @@ -437,7 +437,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_sol = true; } else { - warnx("NAV_SOL: checksum invalid\n"); + warnx("NAV_SOL: checksum invalid"); } // Reset state machine to decode next packet @@ -463,7 +463,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_dop = true; } else { - warnx("NAV_DOP: checksum invalid\n"); + warnx("NAV_DOP: checksum invalid"); } // Reset state machine to decode next packet @@ -499,7 +499,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_timeutc = true; } else { - printf("\t[gps] NAV_TIMEUTC: checksum invalid\n"); + warnx("NAV_TIMEUTC: checksum invalid"); } // Reset state machine to decode next packet @@ -593,7 +593,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report } else { - printf("\t[gps] NAV_SVINFO: checksum invalid\n"); + warnx("NAV_SVINFO: checksum invalid"); } // Reset state machine to decode next packet @@ -624,7 +624,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) _new_nav_velned = true; } else { - warnx("NAV_VELNED: checksum invalid\n"); + warnx("NAV_VELNED: checksum invalid"); } // Reset state machine to decode next packet @@ -719,7 +719,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } } else { - warnx("ACK_ACK: checksum invalid\n"); + warnx("ACK_ACK: checksum invalid"); } // Reset state machine to decode next packet @@ -736,7 +736,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - warnx("the ubx gps returned: not acknowledged\n"); + warnx("UBX: NO ACK"); ret = 1; } else { @@ -751,7 +751,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } default: //we don't know the message - printf("Unknown message received: %d-%d\n",_message_class,_message_id); + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); decodeInit(); break; @@ -760,7 +760,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; } else { - printf("Buffer full"); + warnx("buffer overflow"); decodeInit(); } break; From cb0fd834ae15c45cf6993f8c9eea9c99b2b153dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 18:14:55 +0100 Subject: [PATCH 10/30] Minor polishing, fixed rate and last measurement indication --- apps/drivers/gps/gps.cpp | 6 +++--- apps/drivers/gps/ubx.cpp | 6 ------ apps/mavlink/orb_listener.c | 2 +- 3 files changed, 4 insertions(+), 10 deletions(-) diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index bd77e8969a..49b94fc2e7 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -430,7 +430,7 @@ GPS::task_main() /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > 5000000) { - _rate = last_rate_count / (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; } @@ -523,8 +523,8 @@ GPS::print_info() } warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { - warnx("position lock: %dD, last update %d seconds ago", (int)_report.fix_type, - int((hrt_absolute_time() - _report.timestamp) / 1000000)); + warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, + ((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index fe4758d8cf..1105e0da40 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -429,7 +429,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->fix_type = packet->gpsFix; - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; gps_position->s_variance = packet->sAcc; gps_position->p_variance = packet->pAcc; @@ -457,7 +456,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->eph = packet->hDOP; gps_position->epv = packet->vDOP; - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; _new_nav_dop = true; @@ -493,7 +491,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; _new_nav_timeutc = true; @@ -587,7 +584,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -617,7 +613,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->vel_ned_valid = true; gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; @@ -645,7 +640,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { // // gps_position->satellites_visible = packet->numVis; -// gps_position->timestamp = hrt_absolute_time(); // gps_position->counter++; // _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); // ret = 1; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index ec5149745c..18da70f610 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -698,7 +698,7 @@ uorb_receive_start(void) /* --- GPS VALUE --- */ mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ /* --- HOME POSITION --- */ mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); From 039d394c207830a2c9c3a22e03302c867bd57af6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 4 Feb 2013 16:27:01 -0800 Subject: [PATCH 11/30] Merged with newer, cleaned up code, fixed the checksum error --- apps/drivers/drv_gps.h | 2 - apps/drivers/gps/gps.cpp | 130 +++++++++++++++++++--------------- apps/drivers/gps/gps_helper.h | 5 +- apps/drivers/gps/ubx.cpp | 107 ++++++++++++++-------------- apps/drivers/gps/ubx.h | 114 +++++++++++++++-------------- 5 files changed, 191 insertions(+), 167 deletions(-) diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 89fc54b0d9..1ae27ed2f9 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -54,8 +54,6 @@ typedef enum { } gps_driver_mode_t; - - /* * ObjDev tag for GPS data. */ diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 49b94fc2e7..450b3091bd 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -33,7 +33,7 @@ /** * @file gps.cpp - * Driver for the GPS on UART6 + * Driver for the GPS on a serial port */ #include @@ -107,46 +107,41 @@ public: private: - int _task_should_exit; - int _serial_fd; ///< serial interface to GPS - unsigned _baudrate; - unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; - uint8_t _send_buffer[SEND_BUFFER_LENGTH]; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - volatile int _task; ///< worker task + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + volatile int _task; ///< worker task + bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + gps_driver_mode_t _mode; ///< current mode + GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + struct vehicle_gps_position_s _report; ///< uORB topic for gps position + orb_advert_t _report_pub; ///< uORB pub for gps position + float _rate; ///< position update rate - bool _response_received; - bool _config_needed; - bool _baudrate_changed; - bool _mode_changed; - bool _healthy; - gps_driver_mode_t _mode; - unsigned _messages_received; - float _rate; ///< position update rate - - GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper - - struct vehicle_gps_position_s _report; ///< the current GPS report - orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message - - void recv(); + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ void config(); + /** + * Trampoline to the worker task + */ static void task_main_trampoline(void *arg); /** - * Worker task. + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running */ void task_main(void); /** - * Set the module baud rate + * Set the baudrate of the UART to the GPS */ - int set_baudrate(unsigned baud); + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS @@ -176,12 +171,10 @@ GPS::GPS() : _config_needed(true), _baudrate_changed(false), _mode_changed(true), - _healthy(false), _mode(GPS_DRIVER_MODE_UBX), - _messages_received(0), _Helper(nullptr), - _rate(0.0f), - _report_pub(-1) + _report_pub(-1), + _rate(0.0f) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -216,7 +209,7 @@ GPS::init() if (CDev::init() != OK) goto out; - /* start the IO interface task */ + /* start the GPS driver worker task */ _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { @@ -275,17 +268,22 @@ void GPS::config() { int length = 0; + uint8_t send_buffer[SEND_BUFFER_LENGTH]; - _Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH); + _Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH); - /* the message needs to be sent at the old baudrate */ + /* The config message is sent sent at the old baudrate */ if (length > 0) { - if (length != ::write(_serial_fd, _send_buffer, length)) { + + if (length != ::write(_serial_fd, send_buffer, length)) { debug("write config failed"); return; } } + /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed + * from 9600 to 38400 + */ if (_baudrate_changed) { set_baudrate(_baudrate); _baudrate_changed = false; @@ -304,11 +302,10 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open("/dev/ttyS3", O_RDWR); + _serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters - uint8_t buf[256]; - int baud_i = 0; - uint64_t time_before_configuration; + /* buffer to read from the serial port */ + uint8_t buf[32]; if (_serial_fd < 0) { log("failed to open serial port: %d", errno); @@ -326,16 +323,16 @@ GPS::task_main() /* lock against the ioctl handler */ lock(); - + unsigned baud_i = 0; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); - time_before_configuration = hrt_absolute_time(); + uint64_t time_before_configuration = hrt_absolute_time(); uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - /* loop handling received serial bytes */ + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_mode_changed) { @@ -364,14 +361,17 @@ GPS::task_main() _mode_changed = false; } + /* If a configuration does not finish in the config timeout, change the baudrate */ if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); + _Helper->reset(); time_before_configuration = hrt_absolute_time(); } - + /* during configuration, the timeout should be small, so that we can send config messages in between parsing, + * but during normal operation, it should never timeout because updates should arrive with 5Hz */ int poll_timeout; if (_config_needed) { poll_timeout = 50; @@ -409,11 +409,21 @@ GPS::task_main() /* pass received bytes to the packet decoder */ int j; + int ret_parse = 0; for (j = 0; j < count; j++) { - _messages_received += _Helper->parse(buf[j], &_report); + ret_parse += _Helper->parse(buf[j], &_report); } - if (_messages_received > 0) { - if (_config_needed == true) { + + if (ret_parse < 0) { + /* This means something went wrong in the parser, let's reconfigure */ + if (!_config_needed) { + _config_needed = true; + debug("Lost GPS module"); + } + config(); + } else if (ret_parse > 0) { + /* Looks like we got a valid position update, stop configuring and publish it */ + if (_config_needed) { _config_needed = false; debug("Found GPS module"); } @@ -424,8 +434,6 @@ GPS::task_main() } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - - _messages_received = 0; last_rate_count++; /* measure update rate every 5 seconds */ @@ -435,10 +443,10 @@ GPS::task_main() last_rate_count = 0; } } + /* else if ret_parse == 0: just keep parsing */ } } } -out: debug("exiting"); ::close(_serial_fd); @@ -469,7 +477,6 @@ GPS::set_baudrate(unsigned baud) warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } - struct termios uart_config; int termios_state; @@ -482,24 +489,26 @@ GPS::set_baudrate(unsigned baud) uart_config.c_cflag &= ~(CSTOPB | PARENB); /* set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state); + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); return -1; } - - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; } void GPS::cmd_reset() { - _healthy = false; + _config_needed = true; } void @@ -524,10 +533,12 @@ GPS::print_info() warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - ((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } + + usleep(100000); } /** @@ -583,6 +594,9 @@ fail: errx(1, "driver start failed"); } +/** + * Stop the driver. + */ void stop() { diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index baacf7cb47..8a6b0148b7 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -40,8 +40,9 @@ class GPS_Helper { public: - virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0; - virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0; + virtual void reset() = 0; + virtual void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) = 0; + virtual int parse(uint8_t b, struct vehicle_gps_position_s *gps_position) = 0; }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 1105e0da40..66a891da4f 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -48,36 +48,41 @@ UBX::UBX() : -_waited(0), _config_state(UBX_CONFIG_STATE_PRT), +_waiting_for_ack(false), _new_nav_posllh(false), _new_nav_timeutc(false), _new_nav_dop(false), _new_nav_sol(false), _new_nav_velned(false) { - decodeInit(); - _waiting_for_ack = false; + reset(); } UBX::~UBX() { } +void +UBX::reset() +{ + decodeInit(); + _config_state = UBX_CONFIG_STATE_PRT; + _waiting_for_ack = false; +} + void UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) { - /* make sure the buffer to write the message is long enough */ + /* make sure the buffer, where the message is written to, is long enough */ assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); - /* try again after 10 times */ - if (_waited > 10) { - _waiting_for_ack = false; - } - + /* Only send a new config message when we got the ACK of the last one, + * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command + * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. + */ if (!_waiting_for_ack) { _waiting_for_ack = true; - _waited = 0; if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { config_needed = false; length = 0; @@ -85,10 +90,15 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, _waiting_for_ack = false; return; } else if (_config_state == UBX_CONFIG_STATE_PRT) { - /* send a CFT-PRT message to define set ubx protocol and leave the baudrate as is, we just want an ACK-ACK from this */ + + /* Send a CFG-PRT message to set the UBX protocol for in and out + * and leave the baudrate as it is, we just want an ACK-ACK from this + */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + /* Set everything else of the packet to 0, otherwise the module wont accept it */ memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; @@ -98,18 +108,20 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + /* Calculate the checksum now */ addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + /* Start with the two sync bytes */ buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; + /* Copy it to the buffer that will be written back in the main gps driver */ memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + /* Set the length of the packet (plus the 2 sync bytes) */ length = sizeof(cfg_prt_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { -// printf("Send change of baudrate now\n"); - - /* send a CFT-PRT message to define set ubx protocol and and baudrate, now let's try to switch the baudrate */ + /* Send a CFG-PRT message again, this time change the baudrate */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); @@ -129,18 +141,15 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); length = sizeof(cfg_prt_packet)+2; - /* detection when the baudrate has been changed in the first step */ + /* If the new baudrate will be different from the current one, we should report that back to the driver */ if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - /* set a flag and exit */ baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; baudrate_changed = true; - _config_state = UBX_CONFIG_STATE_RATE; - _waiting_for_ack = false; - return; + /* Don't wait for an ACK, we're switching baudrate and we might never get, + * after that, start fresh */ + reset(); } - - } else if (_config_state == UBX_CONFIG_STATE_RATE) { /* send a CFT-RATE message to define update rate */ @@ -162,9 +171,9 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, length = sizeof(cfg_rate_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_NAV5) { - /* send a NAV5 message to set the options for the internal estimator */ + /* send a NAV5 message to set the options for the internal filter */ type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); //set everything to 0 + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); cfg_nav5_packet.clsID = UBX_CLASS_CFG; cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; @@ -181,15 +190,16 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, length = sizeof(cfg_nav5_packet)+2; } else { - /* catch the remaining config states here */ + /* Catch the remaining config states here, they all need the same packet type */ type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); //set everything to 0 + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); cfg_msg_packet.clsID = UBX_CLASS_CFG; cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1; + /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; switch (_config_state) { case UBX_CONFIG_STATE_MSG_NAV_POSLLH: @@ -207,7 +217,8 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, case UBX_CONFIG_STATE_MSG_NAV_SVINFO: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - cfg_msg_packet.rate[1] = 5; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; break; case UBX_CONFIG_STATE_MSG_NAV_SOL: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; @@ -232,16 +243,14 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); length = sizeof(cfg_msg_packet)+2; } - } else { - _waited++; } } int -UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) +UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) { + /* if no error happens and no new report is ready yet, ret will stay 0 */ int ret = 0; - //printf("Received char: %c\n", b); switch (_decode_state) { /* First, look for sync1 */ @@ -373,6 +382,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); decodeInit(); + ret = -1; break; } break; @@ -415,8 +425,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -441,8 +449,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -466,8 +472,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -501,8 +505,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -594,8 +596,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -615,7 +615,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->counter++; - _new_nav_velned = true; } else { @@ -624,8 +623,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -718,7 +715,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; break; } @@ -730,7 +726,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - warnx("UBX: NO ACK"); + warnx("UBX: Received: Not Acknowledged"); ret = 1; } else { @@ -751,11 +747,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } } // end if _rx_count high enough - if (_rx_count < RECV_BUFFER_SIZE) { + else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; } else { - warnx("buffer overflow"); + warnx("buffer full, restarting"); decodeInit(); + ret = -1; } break; default: @@ -765,13 +762,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) /* return 1 when position updates and the remaining packets updated at least once */ if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + /* Add timestamp to finish the report */ gps_position->timestamp = hrt_absolute_time(); - ret = 1; + /* Reset the flags */ _new_nav_posllh = false; - // _new_nav_timeutc = false; - // _new_nav_dop = false; - // _new_nav_sol = false; - // _new_nav_velned = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; + + ret = 1; } return ret; @@ -807,6 +807,7 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } + /* The checksum is written to the last to bytes of a message */ message[length-2] = ck_a; message[length-1] = ck_b; } diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index dff25a518f..0cac10f0a3 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -40,16 +40,17 @@ #include "gps_helper.h" -#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 -//UBX Protocol definitions (this is the subset of the messages that are parsed) +/* ClassIDs (the ones that are used) */ #define UBX_CLASS_NAV 0x01 //#define UBX_CLASS_RXM 0x02 #define UBX_CLASS_ACK 0x05 #define UBX_CLASS_CFG 0x06 + +/* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 @@ -65,11 +66,11 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ #define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ #define UBX_CFG_RATE_LENGTH 6 #define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ @@ -83,30 +84,30 @@ #define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ #define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE1 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} UART1 chosen */ - +#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ // ************ /** the structures of the binary packets */ #pragma pack(push, 1) typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t lon; // Longitude * 1e-7, deg - int32_t lat; // Latitude * 1e-7, deg - int32_t height; // Height above Ellipsoid, mm - int32_t height_msl; // Height above mean sea level, mm - uint32_t hAcc; // Horizontal Accuracy Estimate, mm - uint32_t vAcc; // Vertical Accuracy Estimate, mm + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t lon; /**< Longitude * 1e-7, deg */ + int32_t lat; /**< Latitude * 1e-7, deg */ + int32_t height; /**< Height above Ellipsoid, mm */ + int32_t height_msl; /**< Height above mean sea level, mm */ + uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ + uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_posllh_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 - int16_t week; // GPS week (GPS time) - uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ + int16_t week; /**< GPS week (GPS time) */ + uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ uint8_t flags; int32_t ecefX; int32_t ecefY; @@ -125,50 +126,50 @@ typedef struct { } gps_bin_nav_sol_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint32_t time_accuracy; //Time Accuracy Estimate, ns - int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC) - uint16_t year; //Year, range 1999..2099 (UTC) - uint8_t month; //Month, range 1..12 (UTC) - uint8_t day; //Day of Month, range 1..31 (UTC) - uint8_t hour; //Hour of Day, range 0..23 (UTC) - uint8_t min; //Minute of Hour, range 0..59 (UTC) - uint8_t sec; //Seconds of Minute, range 0..59 (UTC) - uint8_t valid_flag; //Validity Flags (see ubx documentation) + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ + int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of Month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ + uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint16_t gDOP; //Geometric DOP (scaling 0.01) - uint16_t pDOP; //Position DOP (scaling 0.01) - uint16_t tDOP; //Time DOP (scaling 0.01) - uint16_t vDOP; //Vertical DOP (scaling 0.01) - uint16_t hDOP; //Horizontal DOP (scaling 0.01) - uint16_t nDOP; //Northing DOP (scaling 0.01) - uint16_t eDOP; //Easting DOP (scaling 0.01) + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ + uint16_t pDOP; /**< Position DOP (scaling 0.01) */ + uint16_t tDOP; /**< Time DOP (scaling 0.01) */ + uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ + uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ + uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ + uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_dop_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint8_t numCh; //Number of channels + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint8_t numCh; /**< Number of channels */ uint8_t globalFlags; uint16_t reserved2; } gps_bin_nav_svinfo_part1_packet_t; typedef struct { - uint8_t chn; //Channel number, 255 for SVs not assigned to a channel - uint8_t svid; //Satellite ID + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ uint8_t flags; uint8_t quality; - uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz - int8_t elev; //Elevation in integer degrees - int16_t azim; //Azimuth in integer degrees - int32_t prRes; //Pseudo range residual in centimetres + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ + int8_t elev; /**< Elevation in integer degrees */ + int16_t azim; /**< Azimuth in integer degrees */ + int32_t prRes; /**< Pseudo range residual in centimetres */ } gps_bin_nav_svinfo_part2_packet_t; @@ -192,9 +193,9 @@ typedef struct { } gps_bin_nav_velned_packet_t; //typedef struct { -// int32_t time_milliseconds; // Measurement integer millisecond GPS time of week -// int16_t week; //Measurement GPS week number -// uint8_t numVis; //Number of visible satellites +// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ // // //... rest of package is not used in this implementation // @@ -210,7 +211,6 @@ typedef struct { typedef struct { uint8_t clsID; uint8_t msgID; - uint8_t ck_a; uint8_t ck_b; } gps_bin_ack_nak_packet_t; @@ -340,17 +340,27 @@ class UBX : public GPS_Helper public: UBX(); ~UBX(); - - void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned); + void reset(void); + void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length); int parse(uint8_t, struct vehicle_gps_position_s*); private: + /** + * Reset the parse state machine for a fresh start + */ void decodeInit(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ void addByteToChecksum(uint8_t); + + /** + * Add the two checksum bytes to an outgoing message + */ void addChecksumToMessage(uint8_t*, const unsigned); - unsigned _waited; - bool _waiting_for_ack; ubx_config_state_t _config_state; + bool _waiting_for_ack; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; From 53c11f87cb9b231cfb9199ce797d983f4e2c6a40 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 4 Feb 2013 17:57:30 -0800 Subject: [PATCH 12/30] Small corrections --- apps/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 450b3091bd..f4fd7b88e1 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -110,13 +110,13 @@ private: bool _task_should_exit; ///< flag to make the main worker task exit int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate - unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to volatile int _task; ///< worker task bool _config_needed; ///< flag to signal that configuration of GPS is needed bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode - GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + GPS_Helper *_Helper; ///< Class for a GPS interface struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate From 368ba0056f4a4597c13781b6b5fd6e65930f9fee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 13:47:31 +0100 Subject: [PATCH 13/30] Added option to select port name, minor tweaks to status printing, sacrificied 20 bytes for better status / user debuggability --- apps/drivers/gps/gps.cpp | 76 +++++++++++++++++++++++++--------------- 1 file changed, 47 insertions(+), 29 deletions(-) diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index f4fd7b88e1..5905db6b82 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -93,7 +94,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(); + GPS(const char* uart_path); ~GPS(); virtual int init(); @@ -107,19 +108,20 @@ public: private: - bool _task_should_exit; ///< flag to make the main worker task exit - int _serial_fd; ///< serial interface to GPS - unsigned _baudrate; ///< current baudrate - const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to - volatile int _task; ///< worker task - bool _config_needed; ///< flag to signal that configuration of GPS is needed - bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed - bool _mode_changed; ///< flag that the GPS mode has changed - gps_driver_mode_t _mode; ///< current mode - GPS_Helper *_Helper; ///< Class for a GPS interface - struct vehicle_gps_position_s _report; ///< uORB topic for gps position - orb_advert_t _report_pub; ///< uORB pub for gps position - float _rate; ///< position update rate + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + char _port[20]; ///< device / serial port path + const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + volatile int _task; //< worker task + bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + gps_driver_mode_t _mode; ///< current mode + GPS_Helper *_Helper; ///< instance of GPS parser + struct vehicle_gps_position_s _report; ///< uORB topic for gps position + orb_advert_t _report_pub; ///< uORB pub for gps position + float _rate; ///< position update rate /** @@ -141,7 +143,7 @@ private: /** * Set the baudrate of the UART to the GPS */ - int set_baudrate(unsigned baud); + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS @@ -164,7 +166,7 @@ GPS *g_dev; } -GPS::GPS() : +GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _baudrates_to_try({9600, 38400, 57600, 115200}), @@ -176,6 +178,11 @@ GPS::GPS() : _report_pub(-1), _rate(0.0f) { + /* store port name */ + strncpy(_port, uart_path, sizeof(_port)); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; + /* we need this potentially before it could be set in task_main */ g_dev = this; memset(&_report, 0, sizeof(_report)); @@ -198,6 +205,7 @@ GPS::~GPS() if (_task != -1) task_delete(_task); g_dev = nullptr; + } int @@ -302,13 +310,13 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters + _serial_fd = ::open(_port, O_RDWR); //TODO make the device dynamic depending on startup parameters /* buffer to read from the serial port */ uint8_t buf[32]; if (_serial_fd < 0) { - log("failed to open serial port: %d", errno); + log("failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; _exit(1); @@ -318,7 +326,6 @@ GPS::task_main() pollfd fds[1]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; - debug("ready"); /* lock against the ioctl handler */ lock(); @@ -418,14 +425,12 @@ GPS::task_main() /* This means something went wrong in the parser, let's reconfigure */ if (!_config_needed) { _config_needed = true; - debug("Lost GPS module"); } config(); } else if (ret_parse > 0) { /* Looks like we got a valid position update, stop configuring and publish it */ if (_config_needed) { _config_needed = false; - debug("Found GPS module"); } /* opportunistic publishing - else invalid data would end up on the bus */ @@ -530,7 +535,7 @@ GPS::print_info() default: break; } - warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); @@ -549,7 +554,7 @@ namespace gps GPS *g_dev; -void start(); +void start(const char *uart_path); void stop(); void test(); void reset(); @@ -559,7 +564,7 @@ void info(); * Start the driver. */ void -start() +start(const char *uart_path) { int fd; @@ -567,7 +572,7 @@ start() errx(1, "already started"); /* create the driver */ - g_dev = new GPS; + g_dev = new GPS(uart_path); if (g_dev == nullptr) goto fail; @@ -644,7 +649,6 @@ info() if (g_dev == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); g_dev->print_info(); exit(0); @@ -656,11 +660,24 @@ info() int gps_main(int argc, char *argv[]) { + + /* set to default */ + char* device_name = "/dev/ttyS3"; + /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) - gps::start(); + if (!strcmp(argv[1], "start")) { + /* work around getopt unreliability */ + if (argc > 3) { + if (!strcmp(argv[2], "-d")) { + device_name = argv[3]; + } else { + goto out; + } + } + gps::start(device_name); + } if (!strcmp(argv[1], "stop")) gps::stop(); @@ -682,5 +699,6 @@ gps_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) gps::info(); - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'"); +out: + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); } From fbbeef7e29cc6aa82e653916b7d5e8005948b815 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 5 Feb 2013 18:54:06 +0100 Subject: [PATCH 14/30] Update on every position change, do not wait for other measurements --- apps/drivers/gps/ubx.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 66a891da4f..a6f181a735 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -765,11 +765,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) /* Add timestamp to finish the report */ gps_position->timestamp = hrt_absolute_time(); /* Reset the flags */ + + /* update on every position change, accept minor delay on other measurements */ _new_nav_posllh = false; - _new_nav_timeutc = false; - _new_nav_dop = false; - _new_nav_sol = false; - _new_nav_velned = false; + // _new_nav_timeutc = false; + // _new_nav_dop = false; + // _new_nav_sol = false; + // _new_nav_velned = false; ret = 1; } From a79ad17f09ac69bf2a19a4f617fa1df16bcaa6be Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 5 Feb 2013 23:16:32 -0800 Subject: [PATCH 15/30] Changed parse interface, differentiation between config needed and position updated, working but might be solved more elegant --- apps/drivers/drv_gps.h | 2 + apps/drivers/gps/gps.cpp | 73 +++++++++++++++++++---------------- apps/drivers/gps/gps_helper.h | 6 +-- apps/drivers/gps/ubx.cpp | 65 ++++++++++++++++--------------- apps/drivers/gps/ubx.h | 4 +- 5 files changed, 81 insertions(+), 69 deletions(-) diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 1ae27ed2f9..dfde115eff 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -44,6 +44,8 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" +#define GPS_DEFAULT_UART_PORT "/dev/ttyS3" + #define GPS_DEVICE_PATH "/dev/gps" typedef enum { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 5905db6b82..28dc949d4b 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -278,7 +278,7 @@ GPS::config() int length = 0; uint8_t send_buffer[SEND_BUFFER_LENGTH]; - _Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH); + _Helper->configure(send_buffer, length, SEND_BUFFER_LENGTH, _baudrate_changed, _baudrate); /* The config message is sent sent at the old baudrate */ if (length > 0) { @@ -339,6 +339,9 @@ GPS::task_main() uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; + bool pos_updated; + bool config_needed_res; + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { @@ -391,17 +394,17 @@ GPS::task_main() lock(); - - /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); } else if (ret == 0) { - config(); + /* no response from the GPS */ if (_config_needed == false) { _config_needed = true; - warnx("lost GPS module"); + warnx("GPS module timeout"); + _Helper->reset(); } + config(); } else if (ret > 0) { /* if we have new data from GPS, go handle it */ if (fds[0].revents & POLLIN) { @@ -416,39 +419,43 @@ GPS::task_main() /* pass received bytes to the packet decoder */ int j; - int ret_parse = 0; for (j = 0; j < count; j++) { - ret_parse += _Helper->parse(buf[j], &_report); - } + pos_updated = false; + config_needed_res = _config_needed; + _Helper->parse(buf[j], &_report, config_needed_res, pos_updated); + + if (pos_updated) { + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > 5000000) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + } - if (ret_parse < 0) { - /* This means something went wrong in the parser, let's reconfigure */ - if (!_config_needed) { - _config_needed = true; } - config(); - } else if (ret_parse > 0) { - /* Looks like we got a valid position update, stop configuring and publish it */ - if (_config_needed) { + if (config_needed_res == true && _config_needed == false) { + /* the parser told us that an error happened and reconfiguration is needed */ + _config_needed = true; + warnx("GPS module lost"); + _Helper->reset(); + config(); + } else if (config_needed_res == true && _config_needed == true) { + /* we are still configuring */ + config(); + } else if (config_needed_res == false && _config_needed == true) { + /* the parser is happy, stop configuring */ + warnx("GPS module found"); _config_needed = false; } - - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > 5000000) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - } } - /* else if ret_parse == 0: just keep parsing */ } } } @@ -662,7 +669,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char* device_name = "/dev/ttyS3"; + char* device_name = GPS_DEFAULT_UART_PORT; /* * Start/load the driver. diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 8a6b0148b7..176b7eba81 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -40,9 +40,9 @@ class GPS_Helper { public: - virtual void reset() = 0; - virtual void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) = 0; - virtual int parse(uint8_t b, struct vehicle_gps_position_s *gps_position) = 0; + virtual void reset(void) = 0; + virtual void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) = 0; + virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0; }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index a6f181a735..440ec74c58 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -72,7 +72,7 @@ UBX::reset() } void -UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) +UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) { /* make sure the buffer, where the message is written to, is long enough */ assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); @@ -84,9 +84,10 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, if (!_waiting_for_ack) { _waiting_for_ack = true; if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { - config_needed = false; - length = 0; - _config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */ + /* This should never happen, the parser should set the flag, + * if it should be reconfigured, reset() should be called first. + */ + warnx("ubx: already configured"); _waiting_for_ack = false; return; } else if (_config_state == UBX_CONFIG_STATE_PRT) { @@ -246,12 +247,9 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, } } -int -UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) +void +UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) { - /* if no error happens and no new report is ready yet, ret will stay 0 */ - int ret = 0; - switch (_decode_state) { /* First, look for sync1 */ case UBX_DECODE_UNINIT: @@ -382,7 +380,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); decodeInit(); - ret = -1; + config_needed = true; break; } break; @@ -417,7 +415,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->counter_pos_valid++; gps_position->counter++; + /* Add timestamp to finish the report */ + gps_position->timestamp = hrt_absolute_time(); + _new_nav_posllh = true; + /* set flag to trigger publishing of new position */ + pos_updated = true; } else { warnx("NAV_POSLLH: checksum invalid"); @@ -436,11 +439,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->fix_type = packet->gpsFix; - - gps_position->counter++; gps_position->s_variance = packet->sAcc; gps_position->p_variance = packet->pAcc; + gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); + + _new_nav_sol = true; } else { @@ -463,6 +468,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->epv = packet->vDOP; gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); _new_nav_dop = true; @@ -496,6 +502,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); _new_nav_timeutc = true; @@ -587,6 +594,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -614,6 +622,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); gps_position->counter++; + gps_position->timestamp = hrt_absolute_time(); _new_nav_velned = true; @@ -647,8 +656,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) // // // Reset state machine to decode next packet // decodeInit(); -// return ret; -// // break; // } @@ -701,6 +708,8 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) case UBX_CONFIG_STATE_MSG_NAV_VELNED: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_CONFIGURED; + /* set the flag to tell the driver that configuration was successful */ + config_needed = false; break; // case UBX_CONFIG_STATE_MSG_RXM_SVSI: // if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) @@ -715,7 +724,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) // Reset state machine to decode next packet decodeInit(); - break; } @@ -727,16 +735,14 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { warnx("UBX: Received: Not Acknowledged"); - ret = 1; - + /* configuration obviously not successful */ + config_needed = true; } else { warnx("ACK_NAK: checksum invalid\n"); } // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -752,7 +758,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) } else { warnx("buffer full, restarting"); decodeInit(); - ret = -1; + config_needed = true; } break; default: @@ -762,21 +768,18 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) /* return 1 when position updates and the remaining packets updated at least once */ if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { - /* Add timestamp to finish the report */ - gps_position->timestamp = hrt_absolute_time(); + /* we have received everything, this most probably means that the configuration is fine */ + config_needed = false; + /* Reset the flags */ - - /* update on every position change, accept minor delay on other measurements */ _new_nav_posllh = false; - // _new_nav_timeutc = false; - // _new_nav_dop = false; - // _new_nav_sol = false; - // _new_nav_velned = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; - ret = 1; } - - return ret; + return; } void diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index 0cac10f0a3..d3c6c6d4cf 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -341,8 +341,8 @@ public: UBX(); ~UBX(); void reset(void); - void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length); - int parse(uint8_t, struct vehicle_gps_position_s*); + void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate); + void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); private: /** From fc4be3e7280db480b67b7c6cec11e35481969bbb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 12:41:05 -0800 Subject: [PATCH 16/30] Changed gps position topic mostly to SI units and float, removed counters and added specifig timestamps --- apps/commander/commander.c | 14 +-- apps/drivers/gps/gps.cpp | 4 +- apps/drivers/gps/ubx.cpp | 110 ++++++++++++------------ apps/drivers/gps/ubx.h | 30 +++---- apps/examples/kalman_demo/KalmanNav.cpp | 23 ++--- apps/mavlink/mavlink_receiver.c | 24 +++--- apps/mavlink/orb_listener.c | 10 +-- apps/uORB/topics/home_position.h | 8 +- apps/uORB/topics/vehicle_gps_position.h | 55 ++++++------ 9 files changed, 139 insertions(+), 139 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 3a6fecf746..f19f1d0e6f 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1678,8 +1678,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph / 100.0f; - float vdop_m = gps_position.epv / 100.0f; + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; /* check if gps fix is ok */ // XXX magic number @@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m) && (vdop_m < dop_threshold_m) && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp < 2000000) + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { warnx("setting home position"); @@ -1706,11 +1706,11 @@ int commander_thread_main(int argc, char *argv[]) home.lon = gps_position.lon; home.alt = gps_position.alt; - home.eph = gps_position.eph; - home.epv = gps_position.epv; + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; - home.s_variance = gps_position.s_variance; - home.p_variance = gps_position.p_variance; + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; /* announce new home position */ if (home_pub > 0) { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 28dc949d4b..8c2775adbb 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -543,9 +543,9 @@ GPS::print_info() break; } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); - if (_report.timestamp != 0) { + if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 440ec74c58..eec1df9809 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -52,7 +53,7 @@ _config_state(UBX_CONFIG_STATE_PRT), _waiting_for_ack(false), _new_nav_posllh(false), _new_nav_timeutc(false), -_new_nav_dop(false), +//_new_nav_dop(false), _new_nav_sol(false), _new_nav_velned(false) { @@ -211,10 +212,10 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - break; +// case UBX_CONFIG_STATE_MSG_NAV_DOP: +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; +// break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; @@ -316,10 +317,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_id = NAV_TIMEUTC; break; - case UBX_MESSAGE_NAV_DOP: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_DOP; - break; +// case UBX_MESSAGE_NAV_DOP: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = NAV_DOP; +// break; case UBX_MESSAGE_NAV_SVINFO: _decode_state = UBX_DECODE_GOT_MESSAGEID; @@ -412,11 +413,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->lon = packet->lon; gps_position->alt = packet->height_msl; - gps_position->counter_pos_valid++; - gps_position->counter++; + gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m + gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m /* Add timestamp to finish the report */ - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_position = hrt_absolute_time(); _new_nav_posllh = true; /* set flag to trigger publishing of new position */ @@ -439,11 +440,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->fix_type = packet->gpsFix; - gps_position->s_variance = packet->sAcc; - gps_position->p_variance = packet->pAcc; + gps_position->s_variance_m_s = packet->sAcc; + gps_position->p_variance_m = packet->pAcc; - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_variance = hrt_absolute_time(); _new_nav_sol = true; @@ -457,29 +457,28 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; } - case NAV_DOP: { -// printf("GOT NAV_DOP MESSAGE\n"); - gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - gps_position->eph = packet->hDOP; - gps_position->epv = packet->vDOP; - - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); - - _new_nav_dop = true; - - } else { - warnx("NAV_DOP: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; +// +// //Check if checksum is valid and the store the gps information +// if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { +// +// gps_position->eph_m = packet->hDOP; +// gps_position->epv = packet->vDOP; +// +// gps_position->timestamp_posdilution = hrt_absolute_time(); +// +// _new_nav_dop = true; +// +// } else { +// warnx("NAV_DOP: checksum invalid"); +// } +// +// // Reset state machine to decode next packet +// decodeInit(); +// break; +// } case NAV_TIMEUTC: { // printf("GOT NAV_TIMEUTC MESSAGE\n"); @@ -501,8 +500,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_time = hrt_absolute_time(); _new_nav_timeutc = true; @@ -593,8 +591,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_satellites = hrt_absolute_time(); // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -614,18 +611,17 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ //Check if checksum is valid and the store the gps information if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->vel = (uint16_t)packet->speed; - gps_position->vel_n = packet->velN / 100.0f; - gps_position->vel_e = packet->velE / 100.0f; - gps_position->vel_d = packet->velD / 100.0f; + gps_position->vel_m_s = (float)packet->speed * 1e-2f; + gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; + gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; + gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; gps_position->vel_ned_valid = true; - gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); - - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_velocity = hrt_absolute_time(); _new_nav_velned = true; + } else { warnx("NAV_VELNED: checksum invalid"); } @@ -690,13 +686,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; break; case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_DOP; - break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; break; +// case UBX_CONFIG_STATE_MSG_NAV_DOP: +// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) +// _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; +// break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; @@ -766,7 +762,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } /* return 1 when position updates and the remaining packets updated at least once */ - if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) { /* we have received everything, this most probably means that the configuration is fine */ config_needed = false; @@ -774,7 +770,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ /* Reset the flags */ _new_nav_posllh = false; _new_nav_timeutc = false; - _new_nav_dop = false; +// _new_nav_dop = false; _new_nav_sol = false; _new_nav_velned = false; diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index d3c6c6d4cf..43cded02f7 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -54,7 +54,7 @@ #define UBX_MESSAGE_NAV_POSLLH 0x02 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 -#define UBX_MESSAGE_NAV_DOP 0x04 +//#define UBX_MESSAGE_NAV_DOP 0x04 #define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 @@ -140,18 +140,18 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ - uint16_t pDOP; /**< Position DOP (scaling 0.01) */ - uint16_t tDOP; /**< Time DOP (scaling 0.01) */ - uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ - uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ - uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ - uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_dop_packet_t; +//typedef struct { +// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ +// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ +// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ +// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ +// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ +// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ +// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ +// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ +// uint8_t ck_a; +// uint8_t ck_b; +//} gps_bin_nav_dop_packet_t; typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ @@ -311,7 +311,7 @@ typedef enum { NAV_POSLLH, NAV_SOL, NAV_TIMEUTC, - NAV_DOP, +// NAV_DOP, NAV_SVINFO, NAV_VELNED, // RXM_SVSI, @@ -371,7 +371,7 @@ private: unsigned _payload_size; bool _new_nav_posllh; bool _new_nav_timeutc; - bool _new_nav_dop; +// bool _new_nav_dop; bool _new_nav_sol; bool _new_nav_velned; }; diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 7e89dffb20..b7f651d8a6 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -190,11 +190,12 @@ void KalmanNav::update() if (!_positionInitialized && _attitudeInitialized && // wait for attitude first gpsUpdate && - _gps.fix_type > 2 && - _gps.counter_pos_valid > 10) { - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + _gps.fix_type > 2 + //&& _gps.counter_pos_valid > 10 + ) { + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); @@ -259,7 +260,7 @@ void KalmanNav::updatePublications() // position publication _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp; + _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); @@ -631,8 +632,8 @@ int KalmanNav::correctPos() // residual Vector y(5); - y(0) = _gps.vel_n - vN; - y(1) = _gps.vel_e - vE; + y(0) = _gps.vel_n_m_s - vN; + y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; y(4) = double(_gps.alt) / 1.0e3 - alt; @@ -651,9 +652,9 @@ int KalmanNav::correctPos() // abort correction and return printf("[kalman_demo] numerical failure in gps correction\n"); // fallback to GPS - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 86732d07c0..b3b4b1e0b2 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg) static uint64_t old_timestamp = 0; /* gps */ - hil_gps.timestamp = gps.time_usec; - hil_gps.counter = hil_counter++; + hil_gps.timestamp_position = gps.time_usec; +// hil_gps.counter = hil_counter++; hil_gps.time_gps_usec = gps.time_usec; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.counter_pos_valid = hil_counter++; - hil_gps.eph = gps.eph; - hil_gps.epv = gps.epv; - hil_gps.s_variance = 100; - hil_gps.p_variance = 100; - hil_gps.vel = gps.vel; - hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_d = 0.0f; - hil_gps.cog = gps.cog; +// hil_gps.counter_pos_valid = hil_counter++; + hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance? + hil_gps.p_variance_m = 100; // XXX 100 m variance? + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_d_m_s = 0.0f; + hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 18da70f610..9f85d5801f 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -231,15 +231,15 @@ l_vehicle_gps_position(struct listener *l) /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, - gps.timestamp, + gps.timestamp_position, gps.fix_type, gps.lat, gps.lon, gps.alt, - gps.eph, - gps.epv, - gps.vel, - gps.cog, + (uint16_t)(gps.eph_m * 1e2f), // from m to cm + (uint16_t)(gps.epv_m * 1e2f), // from m to cm + (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s + (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100 gps.satellites_visible); if (gps.satellite_info_available && (gps_counter % 4 == 0)) { diff --git a/apps/uORB/topics/home_position.h b/apps/uORB/topics/home_position.h index dec34b6ab4..7e1b82a0fb 100644 --- a/apps/uORB/topics/home_position.h +++ b/apps/uORB/topics/home_position.h @@ -61,10 +61,10 @@ struct home_position_s int32_t lat; /**< Latitude in 1E7 degrees */ int32_t lon; /**< Longitude in 1E7 degrees */ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - float s_variance; /**< speed accuracy estimate cm/s */ - float p_variance; /**< position accuracy estimate cm */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float s_variance_m_s; /**< speed accuracy estimate m/s */ + float p_variance_m; /**< position accuracy estimate m */ }; /** diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index db529da06d..aa75c22acb 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -55,35 +55,38 @@ */ struct vehicle_gps_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint32_t counter; /**< Count of GPS messages */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + uint64_t timestamp_position; /**< Timestamp for position information */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - int32_t lat; /**< Latitude in 1E7 degrees //LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees //LOGME */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL //LOGME */ - uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */ - uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */ - uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - float s_variance; /**< speed accuracy estimate cm/s */ - float p_variance; /**< position accuracy estimate cm */ - uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */ - float vel_n; /**< GPS ground speed in m/s */ - float vel_e; /**< GPS ground speed in m/s */ - float vel_d; /**< GPS ground speed in m/s */ - uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + uint64_t timestamp_variance; + float s_variance_m_s; /**< speed accuracy estimate m/s */ + float p_variance_m; /**< position accuracy estimate m */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - /* flags */ - float vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ + float vel_m_s; /**< GPS ground speed (m/s) */ + float vel_n_m_s; /**< GPS ground speed in m/s */ + float vel_e_m_s; /**< GPS ground speed in m/s */ + float vel_d_m_s; /**< GPS ground speed in m/s */ + float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */ + bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + + uint64_t timestamp_time; /**< Timestamp for time information */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + + uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ + uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + uint8_t satellite_prn[20]; /**< Global satellite ID */ + uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ + uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** From d962e6c403678e14a64a6b01be8773e98660bb24 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 13:50:32 -0800 Subject: [PATCH 17/30] Removed some unnecessairy flags, home position back working --- apps/commander/commander.c | 9 +++-- apps/drivers/gps/gps.cpp | 2 +- apps/drivers/gps/ubx.cpp | 49 ++++--------------------- apps/drivers/gps/ubx.h | 5 --- apps/uORB/topics/vehicle_gps_position.h | 2 +- 5 files changed, 16 insertions(+), 51 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f19f1d0e6f..6b1bc0f9b5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1683,7 +1683,8 @@ int commander_thread_main(int argc, char *argv[]) /* check if gps fix is ok */ // XXX magic number - float dop_threshold_m = 2.0f; + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; /* * If horizontal dilution of precision (hdop / eph) @@ -1694,8 +1695,10 @@ int commander_thread_main(int argc, char *argv[]) * the system is currently not armed, set home * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m) - && (vdop_m < dop_threshold_m) + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 8c2775adbb..45f18158fb 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -591,7 +591,7 @@ start(const char *uart_path) fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) { - printf("Could not open device path: %s\n", GPS_DEVICE_PATH); + errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } exit(0); diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index eec1df9809..a823271758 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -50,12 +50,7 @@ UBX::UBX() : _config_state(UBX_CONFIG_STATE_PRT), -_waiting_for_ack(false), -_new_nav_posllh(false), -_new_nav_timeutc(false), -//_new_nav_dop(false), -_new_nav_sol(false), -_new_nav_velned(false) +_waiting_for_ack(false) { reset(); } @@ -413,13 +408,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->lon = packet->lon; gps_position->alt = packet->height_msl; - gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m - gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m + gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m /* Add timestamp to finish the report */ gps_position->timestamp_position = hrt_absolute_time(); - _new_nav_posllh = true; /* set flag to trigger publishing of new position */ pos_updated = true; @@ -445,9 +439,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->timestamp_variance = hrt_absolute_time(); - - _new_nav_sol = true; - } else { warnx("NAV_SOL: checksum invalid"); } @@ -502,8 +493,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->timestamp_time = hrt_absolute_time(); - _new_nav_timeutc = true; - } else { warnx("NAV_TIMEUTC: checksum invalid"); } @@ -581,20 +570,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->satellite_elevation[i] = 0; gps_position->satellite_azimuth[i] = 0; } + gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - /* set flag if any sat info is available */ - if (!packet_part1->numCh > 0) { - gps_position->satellite_info_available = 1; - + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + gps_position->satellite_info_available = true; } else { - gps_position->satellite_info_available = 0; + gps_position->satellite_info_available = false; } - - gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones gps_position->timestamp_satellites = hrt_absolute_time(); - // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report - } else { warnx("NAV_SVINFO: checksum invalid"); } @@ -619,9 +604,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->vel_ned_valid = true; gps_position->timestamp_velocity = hrt_absolute_time(); - _new_nav_velned = true; - - } else { warnx("NAV_VELNED: checksum invalid"); } @@ -760,21 +742,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ default: break; } - - /* return 1 when position updates and the remaining packets updated at least once */ - if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) { - - /* we have received everything, this most probably means that the configuration is fine */ - config_needed = false; - - /* Reset the flags */ - _new_nav_posllh = false; - _new_nav_timeutc = false; -// _new_nav_dop = false; - _new_nav_sol = false; - _new_nav_velned = false; - - } return; } diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index 43cded02f7..a23bb55025 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -369,11 +369,6 @@ private: ubx_message_class_t _message_class; ubx_message_id_t _message_id; unsigned _payload_size; - bool _new_nav_posllh; - bool _new_nav_timeutc; -// bool _new_nav_dop; - bool _new_nav_sol; - bool _new_nav_velned; }; #endif /* UBX_H_ */ diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index aa75c22acb..5463a460da 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -86,7 +86,7 @@ struct vehicle_gps_position_s uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + bool satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** From 6ed5d97aea29a284015708a6089b7910afea8369 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 18:47:32 -0800 Subject: [PATCH 18/30] Merged mtk16 and mtk19 helper classes, configure() now writes directly instead of buffering --- apps/drivers/drv_gps.h | 13 +------- apps/drivers/gps/gps.cpp | 53 ++++-------------------------- apps/drivers/gps/gps_helper.h | 3 +- apps/drivers/gps/ubx.cpp | 62 ++++++++++++++--------------------- apps/drivers/gps/ubx.h | 11 +++++-- 5 files changed, 41 insertions(+), 101 deletions(-) diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index dfde115eff..67248efcd9 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -50,8 +50,7 @@ typedef enum { GPS_DRIVER_MODE_UBX = 0, - GPS_DRIVER_MODE_MTK19, - GPS_DRIVER_MODE_MTK16, + GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_NMEA, } gps_driver_mode_t; @@ -67,14 +66,4 @@ ORB_DECLARE(gps); #define _GPSIOCBASE (0x2800) //TODO: arbitrary choice... #define _GPSIOC(_n) (_IOC(_GPSIOCBASE, _n)) -/** configure ubx mode */ -#define GPS_CONFIGURE_UBX _GPSIOC(0) - -/** configure mtk mode */ -#define GPS_CONFIGURE_MTK19 _GPSIOC(1) -#define GPS_CONFIGURE_MTK16 _GPSIOC(2) - -/** configure mtk mode */ -#define GPS_CONFIGURE_NMEA _GPSIOC(3) - #endif /* _DRV_GPS_H */ diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 45f18158fb..3e1aca810c 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -72,6 +72,7 @@ #include #include "ubx.h" +#include "mtk.h" #define SEND_BUFFER_LENGTH 100 #define TIMEOUT 1000000 //1s @@ -238,30 +239,6 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case GPS_CONFIGURE_UBX: - if (_mode != GPS_DRIVER_MODE_UBX) { - _mode = GPS_DRIVER_MODE_UBX; - _mode_changed = true; - } - break; - case GPS_CONFIGURE_MTK19: - if (_mode != GPS_DRIVER_MODE_MTK19) { - _mode = GPS_DRIVER_MODE_MTK19; - _mode_changed = true; - } - break; - case GPS_CONFIGURE_MTK16: - if (_mode != GPS_DRIVER_MODE_MTK16) { - _mode = GPS_DRIVER_MODE_MTK16; - _mode_changed = true; - } - break; - case GPS_CONFIGURE_NMEA: - if (_mode != GPS_DRIVER_MODE_NMEA) { - _mode = GPS_DRIVER_MODE_NMEA; - _mode_changed = true; - } - break; case SENSORIOCRESET: cmd_reset(); break; @@ -275,19 +252,7 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) void GPS::config() { - int length = 0; - uint8_t send_buffer[SEND_BUFFER_LENGTH]; - - _Helper->configure(send_buffer, length, SEND_BUFFER_LENGTH, _baudrate_changed, _baudrate); - - /* The config message is sent sent at the old baudrate */ - if (length > 0) { - - if (length != ::write(_serial_fd, send_buffer, length)) { - debug("write config failed"); - return; - } - } + _Helper->configure(_serial_fd, _baudrate_changed, _baudrate); /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed * from 9600 to 38400 @@ -356,11 +321,8 @@ GPS::task_main() case GPS_DRIVER_MODE_UBX: _Helper = new UBX(); break; - case GPS_DRIVER_MODE_MTK19: - //_Helper = new MTK19(); - break; - case GPS_DRIVER_MODE_MTK16: - //_Helper = new MTK16(); + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(); break; case GPS_DRIVER_MODE_NMEA: //_Helper = new NMEA(); @@ -530,11 +492,8 @@ GPS::print_info() case GPS_DRIVER_MODE_UBX: warnx("protocol: UBX"); break; - case GPS_DRIVER_MODE_MTK19: - warnx("protocol: MTK 1.9"); - break; - case GPS_DRIVER_MODE_MTK16: - warnx("protocol: MTK 1.6"); + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); break; case GPS_DRIVER_MODE_NMEA: warnx("protocol: NMEA"); diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 176b7eba81..576692c2a4 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -36,12 +36,13 @@ /* @file U-Blox protocol definitions */ #ifndef GPS_HELPER_H +#define GPS_HELPER_H class GPS_Helper { public: virtual void reset(void) = 0; - virtual void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) = 0; + virtual void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) = 0; virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0; }; diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index a823271758..8e2396564d 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -68,11 +68,8 @@ UBX::reset() } void -UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) +UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) { - /* make sure the buffer, where the message is written to, is long enough */ - assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); - /* Only send a new config message when we got the ACK of the last one, * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. @@ -105,16 +102,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - /* Calculate the checksum now */ - addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - /* Start with the two sync bytes */ - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - /* Copy it to the buffer that will be written back in the main gps driver */ - memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); - /* Set the length of the packet (plus the 2 sync bytes) */ - length = sizeof(cfg_prt_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { @@ -131,12 +119,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); - length = sizeof(cfg_prt_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); /* If the new baudrate will be different from the current one, we should report that back to the driver */ if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { @@ -160,12 +143,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - addChecksumToMessage((uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_rate_packet, sizeof(cfg_rate_packet)); - length = sizeof(cfg_rate_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); } else if (_config_state == UBX_CONFIG_STATE_NAV5) { /* send a NAV5 message to set the options for the internal filter */ @@ -179,12 +157,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; - addChecksumToMessage((uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_nav5_packet, sizeof(cfg_nav5_packet)); - length = sizeof(cfg_nav5_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); } else { /* Catch the remaining config states here, they all need the same packet type */ @@ -233,12 +206,7 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba break; } - addChecksumToMessage((uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); - - buffer[0] = UBX_SYNC1; - buffer[1] = UBX_SYNC2; - memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); - length = sizeof(cfg_msg_packet)+2; + sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); } } } @@ -779,3 +747,21 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) message[length-2] = ck_a; message[length-1] = ck_b; } + +void +UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length) +{ + ssize_t ret = 0; + + /* Calculate the checksum now */ + addChecksumToMessage(packet, length); + + const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; + + /* Start with the two sync bytes */ + ret += write(fd, sync_bytes, sizeof(sync_bytes)); + ret += write(fd, packet, length); + + if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? + warnx("ubx: config write fail"); +} diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index a23bb55025..c420e83b9e 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -40,7 +40,6 @@ #include "gps_helper.h" - #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 @@ -341,7 +340,7 @@ public: UBX(); ~UBX(); void reset(void); - void configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate); + void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); private: @@ -358,7 +357,13 @@ private: /** * Add the two checksum bytes to an outgoing message */ - void addChecksumToMessage(uint8_t*, const unsigned); + void addChecksumToMessage(uint8_t* message, const unsigned length); + + /** + * Helper to send a config packet + */ + void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length); + ubx_config_state_t _config_state; bool _waiting_for_ack; ubx_decode_state_t _decode_state; From 0d54661ce90dfe2440daea2639a9853520d8366c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 20:04:49 -0800 Subject: [PATCH 19/30] Added MTK 1.6, works after some seconds, work in progress --- apps/drivers/gps/gps.cpp | 43 +++++---- apps/drivers/gps/mtk.cpp | 184 +++++++++++++++++++++++++++++++++++++++ apps/drivers/gps/mtk.h | 108 +++++++++++++++++++++++ 3 files changed, 320 insertions(+), 15 deletions(-) create mode 100644 apps/drivers/gps/mtk.cpp create mode 100644 apps/drivers/gps/mtk.h diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 3e1aca810c..c749e8b7f1 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -77,7 +77,7 @@ #define SEND_BUFFER_LENGTH 100 #define TIMEOUT 1000000 //1s -#define NUMBER_OF_BAUDRATES 4 +#define NUMBER_OF_TRIES 5 #define CONFIG_TIMEOUT 2000000 /* oddly, ERROR is not defined for c++ */ @@ -113,7 +113,8 @@ private: int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path - const unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + const unsigned _baudrates_to_try[NUMBER_OF_TRIES]; ///< try different baudrates that GPS could be set to + const gps_driver_mode_t _modes_to_try[NUMBER_OF_TRIES]; ///< try different modes volatile int _task; //< worker task bool _config_needed; ///< flag to signal that configuration of GPS is needed bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed @@ -170,11 +171,11 @@ GPS *g_dev; GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), - _baudrates_to_try({9600, 38400, 57600, 115200}), + _baudrates_to_try({9600, 38400, 57600, 115200, 38400}), + _modes_to_try({GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK}), _config_needed(true), _baudrate_changed(false), - _mode_changed(true), - _mode(GPS_DRIVER_MODE_UBX), + _mode_changed(false), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -295,8 +296,10 @@ GPS::task_main() /* lock against the ioctl handler */ lock(); - unsigned baud_i = 0; - _baudrate = _baudrates_to_try[baud_i]; + unsigned try_i = 0; + _baudrate = _baudrates_to_try[try_i]; + _mode = _modes_to_try[try_i]; + _mode_changed = true; set_baudrate(_baudrate); uint64_t time_before_configuration = hrt_absolute_time(); @@ -310,6 +313,23 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { + /* If a configuration does not finish in the config timeout, change the baudrate */ + if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { + /* loop through possible modes/baudrates */ + try_i = (try_i + 1) % NUMBER_OF_TRIES; + _baudrate = _baudrates_to_try[try_i]; + set_baudrate(_baudrate); + if (_mode != _modes_to_try[try_i]) { + _mode_changed = true; + } + _mode = _modes_to_try[try_i]; + + if (_Helper != nullptr) { + _Helper->reset(); + } + time_before_configuration = hrt_absolute_time(); + } + if (_mode_changed) { if (_Helper != nullptr) { delete(_Helper); @@ -333,14 +353,7 @@ GPS::task_main() _mode_changed = false; } - /* If a configuration does not finish in the config timeout, change the baudrate */ - if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { - baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; - _baudrate = _baudrates_to_try[baud_i]; - set_baudrate(_baudrate); - _Helper->reset(); - time_before_configuration = hrt_absolute_time(); - } + /* during configuration, the timeout should be small, so that we can send config messages in between parsing, * but during normal operation, it should never timeout because updates should arrive with 5Hz */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp new file mode 100644 index 0000000000..555fb7a515 --- /dev/null +++ b/apps/drivers/gps/mtk.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file mkt.cpp */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mtk.h" + + +MTK::MTK() +{ + decodeInit(); +} + +MTK::~MTK() +{ +} + +void +MTK::reset() +{ + +} + +void +MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) +{ + if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) + warnx("mtk: config write failed"); + + return; +} + +void +MTK::decodeInit(void) +{ + _rx_ck_a = 0; + _rx_ck_b = 0; + _rx_count = 0; + _decode_state = MTK_DECODE_UNINIT; +} + +void +MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +{ + if (_decode_state == MTK_DECODE_UNINIT) { + + if (b == 0xd0) { + _decode_state = MTK_DECODE_GOT_CK_A; + config_needed = false; + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_A) { + if (b == 0xdd) { + _decode_state = MTK_DECODE_GOT_CK_B; + + } else { + // Second start symbol was wrong, reset state machine + decodeInit(); + } + + } else if (_decode_state == MTK_DECODE_GOT_CK_B) { + // Add to checksum + if (_rx_count < 33) + addByteToChecksum(b); + + // Fill packet buffer + _rx_buffer[_rx_count] = b; + _rx_count++; + + /* Packet size minus checksum */ + if (_rx_count >= 35) { + type_gps_mtk_packet *packet = (type_gps_mtk_packet *) _rx_buffer;; + + /* Check if checksum is valid */ + if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->alt = (int32_t)(packet->msl_altitude * 10); // from cm to mm + gps_position->fix_type = packet->fix_type; + gps_position->eph_m = packet->hdop; + gps_position->epv_m = 0.0; //unknown in mtk custom mode + gps_position->vel_m_s = ((float)packet->ground_speed)*1e-2f; // from cm/s to m/s + gps_position->cog_rad = ((float)packet->heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + gps_position->satellites_visible = packet->satellites; + + /* convert time and date information to unix timestamp */ + struct tm timeinfo; //TODO: test this conversion + uint32_t timeinfo_conversion_temp; + + timeinfo.tm_mday = packet->date * 1e-4; + timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4; + timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; + timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; + + timeinfo.tm_hour = packet->utc_time * 1e-7; + timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7; + timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; + timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; + timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; + timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; + time_t epoch = mktime(&timeinfo); + + gps_position->time_gps_usec = epoch * 1e6; //TODO: test this + gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + gps_position->timestamp_position = gps_position->timestamp_time = hrt_absolute_time(); + + pos_updated = true; + + + } else { + warnx("mtk Checksum invalid, 0x%x, 0x%x instead of 0x%x, 0x%x", _rx_ck_a, packet->ck_a, _rx_ck_b, packet->ck_b); + } + + // Reset state machine to decode next packet + decodeInit(); + } + } + return; +} + +void +MTK::addByteToChecksum(uint8_t b) +{ + _rx_ck_a = _rx_ck_a + b; + _rx_ck_b = _rx_ck_b + _rx_ck_a; +} diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h new file mode 100644 index 0000000000..8929eb041c --- /dev/null +++ b/apps/drivers/gps/mtk.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* @file mtk.h */ + +#ifndef MTK_H_ +#define MTK_H_ + +#include "gps_helper.h" + +#define MTK_SYNC1 0xd0 +#define MTK_SYNC2 0xdd + +#define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" +#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" +#define SBAS_ON "$PMTK313,1*2E\r\n" +#define WAAS_ON "$PMTK301,2*2E\r\n" +#define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" + +typedef enum { + MTK_DECODE_UNINIT = 0, + MTK_DECODE_GOT_CK_A = 1, + MTK_DECODE_GOT_CK_B = 2 +} mtk_decode_state_t; + +/** the structures of the binary packets */ +#pragma pack(push, 1) + +typedef struct { + uint8_t payload; ///< Number of payload bytes + int32_t latitude; ///< Latitude in degrees * 10^7 + int32_t longitude; ///< Longitude in degrees * 10^7 + uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 + uint32_t ground_speed; ///< FIXME SPEC UNCLEAR + int32_t heading; + uint8_t satellites; + uint8_t fix_type; + uint32_t date; + uint32_t utc_time; + uint16_t hdop; + uint8_t ck_a; + uint8_t ck_b; +} type_gps_mtk_packet; + +#pragma pack(pop) + +#define MTK_RECV_BUFFER_SIZE 40 + +class MTK : public GPS_Helper +{ +public: + MTK(); + ~MTK(); + void reset(void); + void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); + void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + +private: + /** + * Reset the parse state machine for a fresh start + */ + void decodeInit(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ + void addByteToChecksum(uint8_t); + + mtk_decode_state_t _decode_state; + uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; + unsigned _rx_count; + uint8_t _rx_ck_a; + uint8_t _rx_ck_b; +}; + +#endif /* MTK_H_ */ From b620136af4f8de913fd12872a91a80f62861dc4c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 22:58:52 -0800 Subject: [PATCH 20/30] Added support for MTK revision 19, working condition but configuration of MTK is very slow and needs improvement --- apps/drivers/gps/mtk.cpp | 25 +++++++++++++++++++------ apps/drivers/gps/mtk.h | 4 +++- 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 555fb7a515..bed388c49b 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -48,7 +48,8 @@ #include "mtk.h" -MTK::MTK() +MTK::MTK() : +_mtk_revision(0) { decodeInit(); } @@ -102,13 +103,18 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ { if (_decode_state == MTK_DECODE_UNINIT) { - if (b == 0xd0) { + if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; config_needed = false; + _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { + _decode_state = MTK_DECODE_GOT_CK_A; + config_needed = false; + _mtk_revision = 19; } } else if (_decode_state == MTK_DECODE_GOT_CK_A) { - if (b == 0xdd) { + if (b == MTK_SYNC2) { _decode_state = MTK_DECODE_GOT_CK_B; } else { @@ -131,11 +137,18 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ /* Check if checksum is valid */ if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + if (_mtk_revision == 16) { + gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { + gps_position->lat = packet->latitude; // both degrees*1e7 + gps_position->lon = packet->longitude; // both degrees*1e7 + } else { + warnx("mtk: unknown revision"); + } gps_position->alt = (int32_t)(packet->msl_altitude * 10); // from cm to mm gps_position->fix_type = packet->fix_type; - gps_position->eph_m = packet->hdop; + gps_position->eph_m = packet->hdop; // XXX: Check this because eph_m is in m and hdop is without unit gps_position->epv_m = 0.0; //unknown in mtk custom mode gps_position->vel_m_s = ((float)packet->ground_speed)*1e-2f; // from cm/s to m/s gps_position->cog_rad = ((float)packet->heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 8929eb041c..7a4b4f197c 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -40,7 +40,8 @@ #include "gps_helper.h" -#define MTK_SYNC1 0xd0 +#define MTK_SYNC1_V16 0xd0 +#define MTK_SYNC1_V19 0xd1 #define MTK_SYNC2 0xdd #define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" @@ -99,6 +100,7 @@ private: void addByteToChecksum(uint8_t); mtk_decode_state_t _decode_state; + uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; From d36eb8a3fcce358409a7205bbd75576a447ac7b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 23:25:09 -0800 Subject: [PATCH 21/30] Sped up MTK configuration but the detection time can still be improved: timeouts/usleeps --- apps/drivers/gps/mtk.cpp | 34 ++++++++++++++++++++-------------- apps/drivers/gps/mtk.h | 3 ++- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index bed388c49b..026b0660b5 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -49,6 +49,7 @@ MTK::MTK() : +_config_sent(false), _mtk_revision(0) { decodeInit(); @@ -67,24 +68,29 @@ MTK::reset() void MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) { - if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) - warnx("mtk: config write failed"); - usleep(10000); + if (_config_sent == false) { - if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) + warnx("mtk: config write failed"); + usleep(10000); - if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) + warnx("mtk: config write failed"); + usleep(10000); - if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) - warnx("mtk: config write failed"); + if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) + warnx("mtk: config write failed"); + usleep(10000); + + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) + warnx("mtk: config write failed"); + + _config_sent = true; + } return; } diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 7a4b4f197c..e436371958 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -99,7 +99,8 @@ private: */ void addByteToChecksum(uint8_t); - mtk_decode_state_t _decode_state; + mtk_decode_state_t _decode_state; + bool _config_sent; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; From a88b9f4eefe8315cb692779dd300400d8052eb44 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 7 Feb 2013 14:48:00 -0800 Subject: [PATCH 22/30] Restructered the parsing/configuring, MTK working --- apps/drivers/gps/gps.cpp | 239 ++++++------------------------- apps/drivers/gps/gps_helper.cpp | 90 ++++++++++++ apps/drivers/gps/gps_helper.h | 11 +- apps/drivers/gps/mtk.cpp | 240 ++++++++++++++++++++------------ apps/drivers/gps/mtk.h | 35 +++-- 5 files changed, 313 insertions(+), 302 deletions(-) create mode 100644 apps/drivers/gps/gps_helper.cpp diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index c749e8b7f1..76b5f45336 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -53,7 +53,6 @@ #include #include #include -#include #include #include @@ -71,7 +70,7 @@ #include -#include "ubx.h" +//#include "ubx.h" #include "mtk.h" #define SEND_BUFFER_LENGTH 100 @@ -113,10 +112,8 @@ private: int _serial_fd; ///< serial interface to GPS unsigned _baudrate; ///< current baudrate char _port[20]; ///< device / serial port path - const unsigned _baudrates_to_try[NUMBER_OF_TRIES]; ///< try different baudrates that GPS could be set to - const gps_driver_mode_t _modes_to_try[NUMBER_OF_TRIES]; ///< try different modes volatile int _task; //< worker task - bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _healthy; ///< flag to signal if the GPS is ok bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed bool _mode_changed; ///< flag that the GPS mode has changed gps_driver_mode_t _mode; ///< current mode @@ -171,11 +168,9 @@ GPS *g_dev; GPS::GPS(const char* uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), - _baudrates_to_try({9600, 38400, 57600, 115200, 38400}), - _modes_to_try({GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK}), - _config_needed(true), - _baudrate_changed(false), + _healthy(false), _mode_changed(false), + _mode(GPS_DRIVER_MODE_MTK), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -250,20 +245,6 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } -void -GPS::config() -{ - _Helper->configure(_serial_fd, _baudrate_changed, _baudrate); - - /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed - * from 9600 to 38400 - */ - if (_baudrate_changed) { - set_baudrate(_baudrate); - _baudrate_changed = false; - } -} - void GPS::task_main_trampoline(void *arg) { @@ -276,10 +257,7 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR); //TODO make the device dynamic depending on startup parameters - - /* buffer to read from the serial port */ - uint8_t buf[32]; + _serial_fd = ::open(_port, O_RDWR); if (_serial_fd < 0) { log("failed to open serial port: %s err: %d", _port, errno); @@ -288,151 +266,65 @@ GPS::task_main() _exit(1); } - /* poll descriptor */ - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; - - /* lock against the ioctl handler */ - lock(); - - unsigned try_i = 0; - _baudrate = _baudrates_to_try[try_i]; - _mode = _modes_to_try[try_i]; - _mode_changed = true; - set_baudrate(_baudrate); - - uint64_t time_before_configuration = hrt_absolute_time(); - uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - bool pos_updated; - bool config_needed_res; - /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - /* If a configuration does not finish in the config timeout, change the baudrate */ - if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { - /* loop through possible modes/baudrates */ - try_i = (try_i + 1) % NUMBER_OF_TRIES; - _baudrate = _baudrates_to_try[try_i]; - set_baudrate(_baudrate); - if (_mode != _modes_to_try[try_i]) { - _mode_changed = true; - } - _mode = _modes_to_try[try_i]; - - if (_Helper != nullptr) { - _Helper->reset(); - } - time_before_configuration = hrt_absolute_time(); + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; } - if (_mode_changed) { - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(); - break; + switch (_mode) { +// case GPS_DRIVER_MODE_UBX: +// _Helper = new UBX(); +// break; case GPS_DRIVER_MODE_MTK: + printf("try new mtk\n"); _Helper = new MTK(); break; case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); + //_Helper = new NMEA(); //TODO: add NMEA break; default: break; - } - _mode_changed = false; } - - - - /* during configuration, the timeout should be small, so that we can send config messages in between parsing, - * but during normal operation, it should never timeout because updates should arrive with 5Hz */ - int poll_timeout; - if (_config_needed) { - poll_timeout = 50; - } else { - poll_timeout = 400; - } - /* sleep waiting for data, but no more than the poll timeout */ + // XXX unlock/lock makes sense? unlock(); - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout); - lock(); + if (_Helper->configure(_serial_fd, _baudrate) == 0) { + while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) { + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - /* this would be bad... */ - if (ret < 0) { - log("poll error %d", errno); - } else if (ret == 0) { - /* no response from the GPS */ - if (_config_needed == false) { - _config_needed = true; - warnx("GPS module timeout"); - _Helper->reset(); - } - config(); - } else if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - int count; + last_rate_count++; - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... - */ - count = ::read(_serial_fd, buf, sizeof(buf)); + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > 5000000) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + } - /* pass received bytes to the packet decoder */ - int j; - for (j = 0; j < count; j++) { - pos_updated = false; - config_needed_res = _config_needed; - _Helper->parse(buf[j], &_report, config_needed_res, pos_updated); - - if (pos_updated) { - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > 5000000) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - } - - } - if (config_needed_res == true && _config_needed == false) { - /* the parser told us that an error happened and reconfiguration is needed */ - _config_needed = true; - warnx("GPS module lost"); - _Helper->reset(); - config(); - } else if (config_needed_res == true && _config_needed == true) { - /* we are still configuring */ - config(); - } else if (config_needed_res == false && _config_needed == true) { - /* the parser is happy, stop configuring */ - warnx("GPS module found"); - _config_needed = false; - } + if (!_healthy) { + warnx("module found"); + _healthy = true; } } + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; + } } + lock(); } debug("exiting"); @@ -443,59 +335,12 @@ GPS::task_main() _exit(0); } -int -GPS::set_baudrate(unsigned baud) -{ - /* process baud rate */ - int speed; - switch (baud) { - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - default: - warnx("ERROR: Unsupported baudrate: %d\n", baud); - return -EINVAL; - } - struct termios uart_config; - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); - - /* clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - /* no parity, one stop bit */ - uart_config.c_cflag &= ~(CSTOPB | PARENB); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); - return -1; - } - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); - return -1; - } - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate (tcsetattr)\n"); - return -1; - } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ - return 0; -} void GPS::cmd_reset() { - _config_needed = true; + //XXX add reset? } void @@ -514,7 +359,7 @@ GPS::print_info() default: break; } - warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp new file mode 100644 index 0000000000..2caa82af15 --- /dev/null +++ b/apps/drivers/gps/gps_helper.cpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include +#include "gps_helper.h" + +/* @file gps_helper.cpp */ + +int +GPS_Helper::set_baudrate(const int &fd, unsigned baud) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n", baud); + return -EINVAL; + } + struct termios uart_config; + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); + return -1; + } + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate (tcsetattr)\n"); + return -1; + } + /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; +} diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 576692c2a4..537ca819ce 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -33,17 +33,20 @@ * ****************************************************************************/ -/* @file U-Blox protocol definitions */ +/* @file gps_helper.h */ #ifndef GPS_HELPER_H #define GPS_HELPER_H +#include +#include + class GPS_Helper { public: - virtual void reset(void) = 0; - virtual void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) = 0; - virtual void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0; + virtual int configure(const int &fd, unsigned &baud) = 0; + virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0; + int set_baudrate(const int &fd, unsigned baud); }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 026b0660b5..4ccbbfbe4a 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -37,85 +37,143 @@ #include #include +#include #include #include #include #include -#include -#include #include #include "mtk.h" MTK::MTK() : -_config_sent(false), _mtk_revision(0) { - decodeInit(); + decode_init(); } MTK::~MTK() { } -void -MTK::reset() +int +MTK::configure(const int &fd, unsigned &baudrate) { + /* set baudrate first */ + if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0) + return -1; -} + baudrate = MTK_BAUDRATE; -void -MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) -{ - if (_config_sent == false) { + /* Write config messages, don't wait for an answer */ + if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); + if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) { + warnx("mtk: config write failed"); + return -1; + } + usleep(10000); - if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) - warnx("mtk: config write failed"); - usleep(10000); - - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) - warnx("mtk: config write failed"); - - _config_sent = true; + if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + warnx("mtk: config write failed"); + return -1; } - return; + return 0; +} + +int +MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + gps_mtk_packet_t packet; + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* first read whatever is left */ + if (j < count) { + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j], packet) > 0) { + handle_message(packet, gps_position); + return 1; + } + j++; + } + /* everything is read */ + j = count = 0; + } + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(fd, buf, sizeof(buf)); + } + } + } } void -MTK::decodeInit(void) +MTK::decode_init(void) { _rx_ck_a = 0; _rx_ck_b = 0; _rx_count = 0; _decode_state = MTK_DECODE_UNINIT; } - -void -MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +int +MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) { + int ret = 0; + if (_decode_state == MTK_DECODE_UNINIT) { if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; - config_needed = false; _mtk_revision = 16; } else if (b == MTK_SYNC1_V19) { _decode_state = MTK_DECODE_GOT_CK_A; - config_needed = false; _mtk_revision = 19; } @@ -125,78 +183,82 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } else { // Second start symbol was wrong, reset state machine - decodeInit(); + decode_init(); } } else if (_decode_state == MTK_DECODE_GOT_CK_B) { // Add to checksum if (_rx_count < 33) - addByteToChecksum(b); + add_byte_to_checksum(b); // Fill packet buffer - _rx_buffer[_rx_count] = b; + ((uint8_t*)(&packet))[_rx_count] = b; _rx_count++; - /* Packet size minus checksum */ - if (_rx_count >= 35) { - type_gps_mtk_packet *packet = (type_gps_mtk_packet *) _rx_buffer;; - - /* Check if checksum is valid */ - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - if (_mtk_revision == 16) { - gps_position->lat = packet->latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position->lon = packet->longitude * 10; // from degrees*1e6 to degrees*1e7 - } else if (_mtk_revision == 19) { - gps_position->lat = packet->latitude; // both degrees*1e7 - gps_position->lon = packet->longitude; // both degrees*1e7 - } else { - warnx("mtk: unknown revision"); - } - gps_position->alt = (int32_t)(packet->msl_altitude * 10); // from cm to mm - gps_position->fix_type = packet->fix_type; - gps_position->eph_m = packet->hdop; // XXX: Check this because eph_m is in m and hdop is without unit - gps_position->epv_m = 0.0; //unknown in mtk custom mode - gps_position->vel_m_s = ((float)packet->ground_speed)*1e-2f; // from cm/s to m/s - gps_position->cog_rad = ((float)packet->heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - gps_position->satellites_visible = packet->satellites; - - /* convert time and date information to unix timestamp */ - struct tm timeinfo; //TODO: test this conversion - uint32_t timeinfo_conversion_temp; - - timeinfo.tm_mday = packet->date * 1e-4; - timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4; - timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; - timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; - - timeinfo.tm_hour = packet->utc_time * 1e-7; - timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7; - timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; - timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; - timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; - timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; - time_t epoch = mktime(&timeinfo); - - gps_position->time_gps_usec = epoch * 1e6; //TODO: test this - gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; - gps_position->timestamp_position = gps_position->timestamp_time = hrt_absolute_time(); - - pos_updated = true; - - + /* Packet size minus checksum, XXX ? */ + if (_rx_count >= sizeof(packet)) { + /* Compare checksum */ + if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { + ret = 1; } else { - warnx("mtk Checksum invalid, 0x%x, 0x%x instead of 0x%x, 0x%x", _rx_ck_a, packet->ck_a, _rx_ck_b, packet->ck_b); + warnx("MTK Checksum invalid"); + ret = -1; } - // Reset state machine to decode next packet - decodeInit(); + decode_init(); } } + return ret; +} + +void +MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position) +{ + if (_mtk_revision == 16) { + gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 + gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + } else if (_mtk_revision == 19) { + gps_position.lat = packet.latitude; // both degrees*1e7 + gps_position.lon = packet.longitude; // both degrees*1e7 + } else { + warnx("mtk: unknown revision"); + gps_position.lat = 0; + gps_position.lon = 0; + } + gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm + gps_position.fix_type = packet.fix_type; + gps_position.eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + gps_position.epv_m = 0.0; //unknown in mtk custom mode + gps_position.vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + gps_position.cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + gps_position.satellites_visible = packet.satellites; + + /* convert time and date information to unix timestamp */ + struct tm timeinfo; //TODO: test this conversion + uint32_t timeinfo_conversion_temp; + + timeinfo.tm_mday = packet.date * 1e-4; + timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4; + timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1; + timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100; + + timeinfo.tm_hour = packet.utc_time * 1e-7; + timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7; + timeinfo.tm_min = timeinfo_conversion_temp * 1e-5; + timeinfo_conversion_temp -= timeinfo.tm_min * 1e5; + timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3; + timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; + time_t epoch = mktime(&timeinfo); + + gps_position.time_gps_usec = epoch * 1e6; //TODO: test this + gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3; + gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time(); + return; } void -MTK::addByteToChecksum(uint8_t b) +MTK::add_byte_to_checksum(uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index e436371958..39063beabd 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -50,6 +50,9 @@ #define WAAS_ON "$PMTK301,2*2E\r\n" #define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" +#define MTK_TIMEOUT_5HZ 400 +#define MTK_BAUDRATE 38400 + typedef enum { MTK_DECODE_UNINIT = 0, MTK_DECODE_GOT_CK_A = 1, @@ -64,16 +67,16 @@ typedef struct { int32_t latitude; ///< Latitude in degrees * 10^7 int32_t longitude; ///< Longitude in degrees * 10^7 uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 - uint32_t ground_speed; ///< FIXME SPEC UNCLEAR - int32_t heading; - uint8_t satellites; - uint8_t fix_type; + uint32_t ground_speed; ///< velocity in m/s + int32_t heading; ///< heading in degrees * 10^2 + uint8_t satellites; ///< number of sattelites used + uint8_t fix_type; ///< fix type: XXX correct for that uint32_t date; uint32_t utc_time; - uint16_t hdop; + uint16_t hdop; ///< horizontal dilution of position (without unit) uint8_t ck_a; uint8_t ck_b; -} type_gps_mtk_packet; +} gps_mtk_packet_t; #pragma pack(pop) @@ -84,23 +87,31 @@ class MTK : public GPS_Helper public: MTK(); ~MTK(); - void reset(void); - void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); - void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + int receive(const int &fd, struct vehicle_gps_position_s &gps_position); + int configure(const int &fd, unsigned &baudrate); private: + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b, gps_mtk_packet_t &packet); + + /** + * Handle the package once it has arrived + */ + void handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position); + /** * Reset the parse state machine for a fresh start */ - void decodeInit(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void addByteToChecksum(uint8_t); + void add_byte_to_checksum(uint8_t); mtk_decode_state_t _decode_state; - bool _config_sent; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; unsigned _rx_count; From 508d6d2b4f86db5224b6201cb5a8bda3b6e2a2b8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 6 Feb 2013 08:37:58 +1100 Subject: [PATCH 23/30] drivers/mpu6000: add default product ID case --- apps/drivers/mpu6000/mpu6000.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 55b7cfa85c..5743006394 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -418,6 +418,9 @@ MPU6000::init() case MPU6000_REV_D8: case MPU6000_REV_D9: case MPU6000_REV_D10: + // default case to cope with new chip revisions, which + // presumably won't have the accel scaling bug + default: // Accel scale 8g (4096 LSB/g) write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); break; From 1fed72caf8679dae6748dffa597086cdb0644218 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Feb 2013 11:19:52 +1100 Subject: [PATCH 24/30] mpu6000: support setting the DLPF filter frequency APM uses this for different aircraft types --- apps/drivers/mpu6000/mpu6000.cpp | 46 ++++++++++++++++++++++++++++---- 1 file changed, 41 insertions(+), 5 deletions(-) diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 5743006394..27e200e40b 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -267,6 +267,11 @@ private: */ int self_test(); + /* + set low pass filter frequency + */ + void _set_dlpf_filter(uint16_t frequency_hz); + }; /** @@ -379,7 +384,7 @@ MPU6000::init() // FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter) // was 90 Hz, but this ruins quality and does not improve the // system response - write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_20HZ); + _set_dlpf_filter(20); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -488,6 +493,37 @@ MPU6000::probe() return -EIO; } +/* + set the DLPF filter frequency. This affects both accel and gyro. + */ +void +MPU6000::_set_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + /* + choose next highest filter frequency available + */ + if (frequency_hz <= 5) { + filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { + filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { + filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 42) { + filter = BITS_DLPF_CFG_42HZ; + } else if (frequency_hz <= 98) { + filter = BITS_DLPF_CFG_98HZ; + } else if (frequency_hz <= 188) { + filter = BITS_DLPF_CFG_188HZ; + } else if (frequency_hz <= 256) { + filter = BITS_DLPF_CFG_256HZ_NOLPF2; + } else { + filter = BITS_DLPF_CFG_2100HZ_NOLPF; + } + write_reg(MPUREG_CONFIG, filter); +} + ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { @@ -613,8 +649,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: - /* XXX not implemented */ - return -EINVAL; + _set_dlpf_filter((uint16_t)arg); + return OK; case ACCELIOCSSCALE: { @@ -671,8 +707,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSLOWPASS: case GYROIOCGLOWPASS: - /* XXX not implemented */ - return -EINVAL; + _set_dlpf_filter((uint16_t)arg); + return OK; case GYROIOCSSCALE: /* copy scale in */ From df6cf142e7d67fa8c868245ff144f4e1d4ebdfb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 8 Feb 2013 11:05:57 -0800 Subject: [PATCH 25/30] Another rewrite: most of the polling, reading and writing is now inside the GPS classes --- apps/drivers/drv_gps.h | 3 +- apps/drivers/gps/gps.cpp | 62 +- apps/drivers/gps/gps_helper.cpp | 2 + apps/drivers/gps/gps_helper.h | 4 +- apps/drivers/gps/mtk.cpp | 69 ++- apps/drivers/gps/mtk.h | 10 +- apps/drivers/gps/ubx.cpp | 968 ++++++++++++++++---------------- apps/drivers/gps/ubx.h | 32 +- 8 files changed, 583 insertions(+), 567 deletions(-) diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 67248efcd9..1dda8ef0b8 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -49,7 +49,8 @@ #define GPS_DEVICE_PATH "/dev/gps" typedef enum { - GPS_DRIVER_MODE_UBX = 0, + GPS_DRIVER_MODE_NONE = 0, + GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_NMEA, } gps_driver_mode_t; diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 76b5f45336..6d7cfcc688 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -36,10 +36,7 @@ * Driver for the GPS on a serial port */ -#include - -#include - +#include #include #include #include @@ -54,30 +51,24 @@ #include #include #include - +#include #include -#include - #include - #include - +#include #include #include #include - #include - +#include #include -//#include "ubx.h" +#include "ubx.h" #include "mtk.h" -#define SEND_BUFFER_LENGTH 100 -#define TIMEOUT 1000000 //1s -#define NUMBER_OF_TRIES 5 -#define CONFIG_TIMEOUT 2000000 +#define TIMEOUT_5HZ 400 +#define RATE_MEASUREMENT_PERIOD 5000000 /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -170,7 +161,7 @@ GPS::GPS(const char* uart_path) : _task_should_exit(false), _healthy(false), _mode_changed(false), - _mode(GPS_DRIVER_MODE_MTK), + _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -279,12 +270,11 @@ GPS::task_main() } switch (_mode) { -// case GPS_DRIVER_MODE_UBX: -// _Helper = new UBX(); -// break; + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; case GPS_DRIVER_MODE_MTK: - printf("try new mtk\n"); - _Helper = new MTK(); + _Helper = new MTK(_serial_fd, &_report); break; case GPS_DRIVER_MODE_NMEA: //_Helper = new NMEA(); //TODO: add NMEA @@ -292,11 +282,11 @@ GPS::task_main() default: break; } - // XXX unlock/lock makes sense? unlock(); - if (_Helper->configure(_serial_fd, _baudrate) == 0) { - - while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) { + if (_Helper->configure(_baudrate) == 0) { + unlock(); + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { +// lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); @@ -307,7 +297,7 @@ GPS::task_main() last_rate_count++; /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > 5000000) { + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; @@ -323,8 +313,26 @@ GPS::task_main() _healthy = false; _rate = 0.0f; } + + lock(); } lock(); + + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + // case GPS_DRIVER_MODE_NMEA: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + default: + break; + } + } debug("exiting"); diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp index 2caa82af15..9c1fad5691 100644 --- a/apps/drivers/gps/gps_helper.cpp +++ b/apps/drivers/gps/gps_helper.cpp @@ -57,6 +57,8 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; + warnx("try baudrate: %d\n", speed); + default: warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 537ca819ce..f3d3bc40b3 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -44,8 +44,8 @@ class GPS_Helper { public: - virtual int configure(const int &fd, unsigned &baud) = 0; - virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0; + virtual int configure(unsigned &baud) = 0; + virtual int receive(unsigned timeout) = 0; int set_baudrate(const int &fd, unsigned baud); }; diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 4ccbbfbe4a..4762bd503a 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -47,7 +47,9 @@ #include "mtk.h" -MTK::MTK() : +MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), _mtk_revision(0) { decode_init(); @@ -58,40 +60,40 @@ MTK::~MTK() } int -MTK::configure(const int &fd, unsigned &baudrate) +MTK::configure(unsigned &baudrate) { /* set baudrate first */ - if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0) + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) return -1; baudrate = MTK_BAUDRATE; /* Write config messages, don't wait for an answer */ - if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { + if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { + if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) { + if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) { + if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { warnx("mtk: config write failed"); return -1; } @@ -100,16 +102,19 @@ MTK::configure(const int &fd, unsigned &baudrate) } int -MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) +MTK::receive(unsigned timeout) { /* poll descriptor */ pollfd fds[1]; - fds[0].fd = fd; + fds[0].fd = _fd; fds[0].events = POLLIN; uint8_t buf[32]; gps_mtk_packet_t packet; + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + int j = 0; ssize_t count = 0; @@ -120,9 +125,13 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) /* pass received bytes to the packet decoder */ while (j < count) { if (parse_char(buf[j], packet) > 0) { - handle_message(packet, gps_position); + handle_message(packet); return 1; } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } j++; } /* everything is read */ @@ -130,7 +139,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) } /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ); + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (ret < 0) { /* something went wrong when polling */ @@ -148,7 +157,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) * won't block even on a blocking device. If more bytes are * available, we'll go back to poll() again... */ - count = ::read(fd, buf, sizeof(buf)); + count = ::read(_fd, buf, sizeof(buf)); } } } @@ -212,26 +221,26 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) } void -MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position) +MTK::handle_message(gps_mtk_packet_t &packet) { if (_mtk_revision == 16) { - gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 } else if (_mtk_revision == 19) { - gps_position.lat = packet.latitude; // both degrees*1e7 - gps_position.lon = packet.longitude; // both degrees*1e7 + _gps_position->lat = packet.latitude; // both degrees*1e7 + _gps_position->lon = packet.longitude; // both degrees*1e7 } else { warnx("mtk: unknown revision"); - gps_position.lat = 0; - gps_position.lon = 0; + _gps_position->lat = 0; + _gps_position->lon = 0; } - gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm - gps_position.fix_type = packet.fix_type; - gps_position.eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - gps_position.epv_m = 0.0; //unknown in mtk custom mode - gps_position.vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s - gps_position.cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - gps_position.satellites_visible = packet.satellites; + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm + _gps_position->fix_type = packet.fix_type; + _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + _gps_position->satellites_visible = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; //TODO: test this conversion @@ -250,9 +259,9 @@ MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; time_t epoch = mktime(&timeinfo); - gps_position.time_gps_usec = epoch * 1e6; //TODO: test this - gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3; - gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time(); + _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this + _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); return; } diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 39063beabd..d4e390b01b 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -85,10 +85,10 @@ typedef struct { class MTK : public GPS_Helper { public: - MTK(); + MTK(const int &fd, struct vehicle_gps_position_s *gps_position); ~MTK(); - int receive(const int &fd, struct vehicle_gps_position_s &gps_position); - int configure(const int &fd, unsigned &baudrate); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: /** @@ -99,7 +99,7 @@ private: /** * Handle the package once it has arrived */ - void handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position); + void handle_message(gps_mtk_packet_t &packet); /** * Reset the parse state machine for a fresh start @@ -111,6 +111,8 @@ private: */ void add_byte_to_checksum(uint8_t); + int _fd; + struct vehicle_gps_position_s *_gps_position; mtk_decode_state_t _decode_state; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 8e2396564d..74cbc5aaf3 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -47,172 +48,251 @@ #include "ubx.h" +#define UBX_CONFIG_TIMEOUT 100 -UBX::UBX() : -_config_state(UBX_CONFIG_STATE_PRT), +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), _waiting_for_ack(false) { - reset(); + decode_init(); } UBX::~UBX() { } -void -UBX::reset() +int +UBX::configure(unsigned &baudrate) { - decodeInit(); - _config_state = UBX_CONFIG_STATE_PRT; - _waiting_for_ack = false; + _waiting_for_ack = true; + + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < 5; baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + + /* Send a CFG-PRT message to set the UBX protocol for in and out + * and leave the baudrate as it is, we just want an ACK-ACK from this + */ + type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + /* Set everything else of the packet to 0, otherwise the module wont accept it */ + memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_PRT; + + /* Define the package contents, don't change the baudrate */ + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = baudrate; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + /* Send a CFG-PRT message again, this time change the baudrate */ + + cfg_prt_packet.clsID = UBX_CLASS_CFG; + cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; + cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); + baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + } + + /* no ack is ecpected here, keep going configuring */ + + /* send a CFT-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_RATE; + + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + + send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + /* send a NAV5 message to set the options for the internal filter */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_NAV5; + + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_MSG; + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + + _waiting_for_ack = false; + return 0; + } + return -1; } -void -UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) +int +UBX::receive(unsigned timeout) { - /* Only send a new config message when we got the ACK of the last one, - * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command - * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. - */ - if (!_waiting_for_ack) { - _waiting_for_ack = true; - if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { - /* This should never happen, the parser should set the flag, - * if it should be reconfigured, reset() should be called first. - */ - warnx("ubx: already configured"); - _waiting_for_ack = false; - return; - } else if (_config_state == UBX_CONFIG_STATE_PRT) { + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; - /* Send a CFG-PRT message to set the UBX protocol for in and out - * and leave the baudrate as it is, we just want an ACK-ACK from this - */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - /* Set everything else of the packet to 0, otherwise the module wont accept it */ - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + uint8_t buf[32]; - /* Define the package contents, don't change the baudrate */ - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = baudrate; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + int j = 0; + ssize_t count = 0; - } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { + while (true) { - /* Send a CFG-PRT message again, this time change the baudrate */ - type_gps_bin_cfg_prt_packet_t cfg_prt_packet; - memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - /* If the new baudrate will be different from the current one, we should report that back to the driver */ - if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; - baudrate_changed = true; - /* Don't wait for an ACK, we're switching baudrate and we might never get, - * after that, start fresh */ - reset(); + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j]) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message() > 0) + return 1; } - - } else if (_config_state == UBX_CONFIG_STATE_RATE) { - - /* send a CFT-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - - sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - - } else if (_config_state == UBX_CONFIG_STATE_NAV5) { - /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; - - sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - - } else { - /* Catch the remaining config states here, they all need the same packet type */ - - type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); - - cfg_msg_packet.clsID = UBX_CLASS_CFG; - cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; - cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; - - switch (_config_state) { - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; - break; -// case UBX_CONFIG_STATE_MSG_NAV_DOP: -// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; -// break; - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - /* For satelites info 1Hz is enough */ - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - break; -// case UBX_CONFIG_STATE_MSG_RXM_SVSI: -// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; -// break; - default: - break; + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; } + j++; + } - sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } } } } -void -UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +int +UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ @@ -227,13 +307,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _decode_state = UBX_DECODE_GOT_SYNC2; } else { /* Second start symbol was wrong, reset state machine */ - decodeInit(); + decode_init(); } break; /* Now look for class */ case UBX_DECODE_GOT_SYNC2: /* everything except sync1 and sync2 needs to be added to the checksum */ - addByteToChecksum(b); + add_byte_to_checksum(b); /* check for known class */ switch (b) { case UBX_CLASS_ACK: @@ -256,12 +336,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_class = CFG; break; default: //unknown class: reset state machine - decodeInit(); + decode_init(); break; } break; case UBX_DECODE_GOT_CLASS: - addByteToChecksum(b); + add_byte_to_checksum(b); switch (_message_class) { case NAV: switch (b) { @@ -296,7 +376,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -308,7 +388,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ // break; // // default: //unknown class: reset state machine, should not happen -// decodeInit(); +// decode_init(); // break; // } // break; @@ -321,7 +401,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -337,384 +417,282 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_id = ACK_NAK; break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); - decodeInit(); - config_needed = true; + decode_init(); +// config_needed = true; break; } break; case UBX_DECODE_GOT_MESSAGEID: - addByteToChecksum(b); + add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; break; case UBX_DECODE_GOT_LENGTH1: - addByteToChecksum(b); + add_byte_to_checksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; break; case UBX_DECODE_GOT_LENGTH2: /* Add to checksum if not yet at checksum byte */ if (_rx_count < _payload_size) - addByteToChecksum(b); + add_byte_to_checksum(b); _rx_buffer[_rx_count] = b; /* once the payload has arrived, we can process the information */ if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - switch (_message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->lat = packet->lat; - gps_position->lon = packet->lon; - gps_position->alt = packet->height_msl; - - gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m - - /* Add timestamp to finish the report */ - gps_position->timestamp_position = hrt_absolute_time(); - - /* set flag to trigger publishing of new position */ - pos_updated = true; - - } else { - warnx("NAV_POSLLH: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - gps_position->fix_type = packet->gpsFix; - gps_position->s_variance_m_s = packet->sAcc; - gps_position->p_variance_m = packet->pAcc; - - gps_position->timestamp_variance = hrt_absolute_time(); - - } else { - warnx("NAV_SOL: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - -// case NAV_DOP: { -//// printf("GOT NAV_DOP MESSAGE\n"); -// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; -// -// //Check if checksum is valid and the store the gps information -// if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { -// -// gps_position->eph_m = packet->hDOP; -// gps_position->epv = packet->vDOP; -// -// gps_position->timestamp_posdilution = hrt_absolute_time(); -// -// _new_nav_dop = true; -// -// } else { -// warnx("NAV_DOP: checksum invalid"); -// } -// -// // Reset state machine to decode next packet -// decodeInit(); -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - - time_t epoch = mktime(&timeinfo); - - gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - - gps_position->timestamp_time = hrt_absolute_time(); - - } else { - warnx("NAV_TIMEUTC: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); - - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; - - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); - gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3; - - //Check if checksum is valid and then store the gps information - if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) { - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; //for temporal storage - - uint8_t satellites_used = 0; - int i; - - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* Get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; - - - /* Write satellite information in the global storage */ - gps_position->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - - if (!unhealthy) { - if ((packet_part2->flags) & 1) { //flags is a bitfield - gps_position->satellite_used[i] = 1; - satellites_used++; - - } else { - gps_position->satellite_used[i] = 0; - } - - gps_position->satellite_snr[i] = packet_part2->cno; - gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - - } else { - gps_position->satellite_used[i] = 0; - gps_position->satellite_snr[i] = 0; - gps_position->satellite_elevation[i] = 0; - gps_position->satellite_azimuth[i] = 0; - } - - } - - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - gps_position->satellite_prn[i] = 0; - gps_position->satellite_used[i] = 0; - gps_position->satellite_snr[i] = 0; - gps_position->satellite_elevation[i] = 0; - gps_position->satellite_azimuth[i] = 0; - } - gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - /* set timestamp if any sat info is available */ - if (packet_part1->numCh > 0) { - gps_position->satellite_info_available = true; - } else { - gps_position->satellite_info_available = false; - } - gps_position->timestamp_satellites = hrt_absolute_time(); - - } else { - warnx("NAV_SVINFO: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - gps_position->vel_m_s = (float)packet->speed * 1e-2f; - gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; - gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; - gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; - gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - gps_position->vel_ned_valid = true; - gps_position->timestamp_velocity = hrt_absolute_time(); - - } else { - warnx("NAV_VELNED: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - -// case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); -// const int length_part1 = 7; -// char _rx_buffer_part1[length_part1]; -// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); -// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; -// -// //Check if checksum is valid and the store the gps information -// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { -// -// gps_position->satellites_visible = packet->numVis; -// gps_position->counter++; -// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); -// ret = 1; -// -// } else { -// warnx("RXM_SVSI: checksum invalid\n"); -// } -// -// // Reset state machine to decode next packet -// decodeInit(); -// break; -// } - - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - //Check if checksum is valid - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - _waiting_for_ack = false; - - switch (_config_state) { - case UBX_CONFIG_STATE_PRT: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) - _config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE; - break; - case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) - _config_state = UBX_CONFIG_STATE_RATE; - break; - case UBX_CONFIG_STATE_RATE: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE) - _config_state = UBX_CONFIG_STATE_NAV5; - break; - case UBX_CONFIG_STATE_NAV5: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) - _config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH; - break; - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; - break; -// case UBX_CONFIG_STATE_MSG_NAV_DOP: -// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) -// _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; -// break; - case UBX_CONFIG_STATE_MSG_NAV_SVINFO: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED; - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_CONFIGURED; - /* set the flag to tell the driver that configuration was successful */ - config_needed = false; - break; -// case UBX_CONFIG_STATE_MSG_RXM_SVSI: -// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) -// _config_state = UBX_CONFIG_STATE_CONFIGURED; -// break; - default: - break; - } - } else { - warnx("ACK_ACK: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { + return 1; + } else { + decode_init(); + return -1; + warnx("ubx: Checksum wrong"); } - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; - - //Check if checksum is valid - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - config_needed = true; - } else { - warnx("ACK_NAK: checksum invalid\n"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - decodeInit(); - - break; - } - } // end if _rx_count high enough - else if (_rx_count < RECV_BUFFER_SIZE) { + return 1; + } else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; } else { - warnx("buffer full, restarting"); - decodeInit(); - config_needed = true; + warnx("ubx: buffer full"); + decode_init(); + return -1; } break; default: break; } - return; + return 0; //XXX ? +} + + +int +UBX::handle_message() +{ + int ret = 0; + + switch (_message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; + + _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + + /* Add timestamp to finish the report */ + _gps_position->timestamp_position = hrt_absolute_time(); + /* only return 1 when new position is available */ + ret = 1; + } + break; + } + + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; + + _gps_position->timestamp_variance = hrt_absolute_time(); + } + break; + } + +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; +// +// _gps_position->eph_m = packet->hDOP; +// _gps_position->epv = packet->vDOP; +// +// _gps_position->timestamp_posdilution = hrt_absolute_time(); +// +// _new_nav_dop = true; +// +// break; +// } + + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); + + if (!_waiting_for_ack) { + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + + _gps_position->timestamp_time = hrt_absolute_time(); + } + break; + } + + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + if (!_waiting_for_ack) { + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char _rx_buffer_part3[length_part3]; + memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; //for temporal storage + + uint8_t satellites_used = 0; + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + + /* Write satellite information in the global storage */ + _gps_position->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + if ((packet_part2->flags) & 1) { //flags is a bitfield + _gps_position->satellite_used[i] = 1; + satellites_used++; + + } else { + _gps_position->satellite_used[i] = 0; + } + + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + + } else { + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + } else { + _gps_position->satellite_info_available = false; + } + _gps_position->timestamp_satellites = hrt_absolute_time(); + } + + break; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + + if (!_waiting_for_ack) { + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _gps_position->vel_m_s = (float)packet->speed * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + } + + break; + } + +// case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); +// const int length_part1 = 7; +// char _rx_buffer_part1[length_part1]; +// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); +// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// +// _gps_position->satellites_visible = packet->numVis; +// _gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// +// break; +// } + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { + ret = 1; + } + } + } + break; + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + warnx("UBX: Received: Not Acknowledged"); + /* configuration obviously not successful */ + ret = -1; + break; + } + + default: //we don't know the message + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + ret = -1; + break; + } + // end if _rx_count high enough + decode_init(); + return ret; //XXX? } void -UBX::decodeInit(void) +UBX::decode_init(void) { _rx_ck_a = 0; _rx_ck_b = 0; @@ -726,14 +704,14 @@ UBX::decodeInit(void) } void -UBX::addByteToChecksum(uint8_t b) +UBX::add_byte_to_checksum(uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; } void -UBX::addChecksumToMessage(uint8_t* message, const unsigned length) +UBX::add_checksum_to_message(uint8_t* message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; @@ -749,12 +727,12 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) } void -UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length) +UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; /* Calculate the checksum now */ - addChecksumToMessage(packet, length); + add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index c420e83b9e..e3dd1c7ea9 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -86,6 +86,8 @@ #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_MAX_PAYLOAD_LENGTH 500 + // ************ /** the structures of the binary packets */ #pragma pack(push, 1) @@ -337,35 +339,49 @@ typedef enum { class UBX : public GPS_Helper { public: - UBX(); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position); ~UBX(); - void reset(void); - void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); - void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: + + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b); + + /** + * Handle the package once it has arrived + */ + int handle_message(void); + /** * Reset the parse state machine for a fresh start */ - void decodeInit(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void addByteToChecksum(uint8_t); + void add_byte_to_checksum(uint8_t); /** * Add the two checksum bytes to an outgoing message */ - void addChecksumToMessage(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t* message, const unsigned length); /** * Helper to send a config packet */ - void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length); + void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + int _fd; + struct vehicle_gps_position_s *_gps_position; ubx_config_state_t _config_state; bool _waiting_for_ack; + uint8_t _clsID_needed; + uint8_t _msgID_needed; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; From ab44a64ca01f35d9f9777d18a01ff9f35996fbf2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Feb 2013 18:04:57 +0100 Subject: [PATCH 26/30] Updated mixers for Q and X5 --- ROMFS/Makefile | 3 +- ROMFS/mixers/FMU_Q.mix | 52 ++++++++++++++++++++++ ROMFS/mixers/{FMU_delta.mix => FMU_X5.mix} | 0 3 files changed, 54 insertions(+), 1 deletion(-) create mode 100644 ROMFS/mixers/FMU_Q.mix rename ROMFS/mixers/{FMU_delta.mix => FMU_X5.mix} (100%) diff --git a/ROMFS/Makefile b/ROMFS/Makefile index 3b024de06d..d827fa4915 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -23,7 +23,8 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \ $(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \ $(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \ - $(SRCROOT)/mixers/FMU_delta.mix~mixers/FMU_delta.mix \ + $(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \ + $(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \ $(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \ $(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \ $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \ diff --git a/ROMFS/mixers/FMU_Q.mix b/ROMFS/mixers/FMU_Q.mix new file mode 100644 index 0000000000..a35d299fda --- /dev/null +++ b/ROMFS/mixers/FMU_Q.mix @@ -0,0 +1,52 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Bormatec Camflyer Q + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -5000 -8000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -8000 -5000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 + diff --git a/ROMFS/mixers/FMU_delta.mix b/ROMFS/mixers/FMU_X5.mix similarity index 100% rename from ROMFS/mixers/FMU_delta.mix rename to ROMFS/mixers/FMU_X5.mix From 298b46ecfc184253d57a2bd4449ce4f64971502c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 12 Feb 2013 13:21:41 +0100 Subject: [PATCH 27/30] Improved log conversion MATLAB script --- ROMFS/logging/logconv.m | 163 +++++++++++++++++++++++++++++++++------- 1 file changed, 137 insertions(+), 26 deletions(-) mode change 100644 => 100755 ROMFS/logging/logconv.m diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m old mode 100644 new mode 100755 index b53d0ae399..c405410dee --- a/ROMFS/logging/logconv.m +++ b/ROMFS/logging/logconv.m @@ -45,29 +45,30 @@ end % float true_airspeed; % Definition of the logged values -logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); -logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); -logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); +logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); +logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{21} = struct('name', 'omnidirectional_flow', 'bytes', 4, 'array', 22,'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{22} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{23} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{24} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); % First get length of one line columns = length(logFormat); @@ -83,7 +84,7 @@ if exist(filePath, 'file') fileInfo = dir(filePath); fileSize = fileInfo.bytes; - elements = int64(fileSize./(lineLength)) + elements = int64(fileSize./(lineLength)); fid = fopen(filePath, 'r'); offset = 0; @@ -102,8 +103,8 @@ if exist(filePath, 'file') % shot the flight time time_us = sysvector.timestamp(end) - sysvector.timestamp(1); - time_s = time_us*1e-6 - time_m = time_s/60 + time_s = time_us*1e-6; + time_m = time_s/60; % close the logfile fclose(fid); @@ -112,3 +113,113 @@ if exist(filePath, 'file') else disp(['file: ' filePath ' does not exist' char(10)]); end + +%% Plot GPS RAW measurements + +% Only plot GPS data if available +if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0 + figure('units','normalized','outerposition',[0 0 1 1]) + plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3)); +end + + +%% Plot optical flow trajectory + +flow_sz = size(sysvector.timestamp); +flow_elements = flow_sz(1); + +xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms] + + +%calc dt +dt = zeros(flow_elements,1); +for i = 1:flow_elements-1 + dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s] +end +dt(1,1) = mean(dt); + + +global_speed = zeros(flow_elements,3); + +%calc global speed (with rot matrix) +for i = 1:flow_elements + rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]'; + speedX = sysvector.optical_flow(i,3); + speedY = sysvector.optical_flow(i,4); + + relSpeed = [-speedY,speedX,0]; + global_speed(i,:) = relSpeed * rotM; +end + + + +px = zeros(flow_elements,1); +py = zeros(flow_elements,1); +distance = 0; + +last_vx = 0; +last_vy = 0; +elem_cnt = 0; + +% Very basic accumulation, stops on bad flow quality +for i = 1:flow_elements + if sysvector.optical_flow(i,6) > 5 + px(i,1) = global_speed(i,1)*dt(i,1); + py(i,1) = global_speed(i,2)*dt(i,1); + distance = distance + norm([px(i,1) py(i,1)]); + last_vx = px(i,1); + last_vy = py(i,1); + else + px(i,1) = last_vx; + py(i,1) = last_vy; + last_vx = last_vx*0.95; + last_vy = last_vy*0.95; + end +end + +px_sum = cumsum(px); +py_sum = cumsum(py); +time = cumsum(dt); + +figure() +set(gca, 'Units','normal'); + +plot(py_sum, px_sum, '-blue', 'LineWidth',2); +axis equal; +% set title and axis captions +xlabel('X position (meters)','fontsize',14) +ylabel('Y position (meters)','fontsize',14) +% mark begin and end +hold on +plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,... +'MarkerEdgeColor','k',... +'MarkerFaceColor','g',... +'MarkerSize',10) +hold on +plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,... +'MarkerEdgeColor','k',... +'MarkerFaceColor','b',... +'MarkerSize',10) +% add total length as annotation +set(gca,'fontsize',13); +legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60)); +title('Optical Flow Position Integration', 'fontsize', 15); + +figure() +plot(time, sysvector.optical_flow(:,5), 'blue'); +axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]); +xlabel('seconds','fontsize',14); +ylabel('m','fontsize',14); +set(gca,'fontsize',13); +title('Ultrasound Altitude', 'fontsize', 15); + + +figure() +plot(time, global_speed(:,2), 'red'); +hold on; +plot(time, global_speed(:,1), 'blue'); +legend('y velocity (m/s)', 'x velocity (m/s)'); +xlabel('seconds','fontsize',14); +ylabel('m/s','fontsize',14); +set(gca,'fontsize',13); +title('Optical Flow Velocity', 'fontsize', 15); From 31cfec3dbd22dab59d609c6af6b1fd3146476975 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Feb 2013 09:11:14 +0100 Subject: [PATCH 28/30] Hotfix: Disable board detect, as it just creates noise, enable it once time permits to wrap it up --- ROMFS/scripts/rc.boarddetect | 66 +++++++++++++++++++++++++++++++++++ ROMFS/scripts/rcS | 67 ------------------------------------ 2 files changed, 66 insertions(+), 67 deletions(-) create mode 100644 ROMFS/scripts/rc.boarddetect diff --git a/ROMFS/scripts/rc.boarddetect b/ROMFS/scripts/rc.boarddetect new file mode 100644 index 0000000000..f233e51df4 --- /dev/null +++ b/ROMFS/scripts/rc.boarddetect @@ -0,0 +1,66 @@ +#!nsh +# +# If we are still in flight mode, work out what airframe +# configuration we have and start up accordingly. +# +if [ $MODE != autostart ] +then + echo "[init] automatic startup cancelled by user script" +else + echo "[init] detecting attached hardware..." + + # + # Assume that we are PX4FMU in standalone mode + # + set BOARD PX4FMU + + # + # Are we attached to a PX4IOAR (AR.Drone carrier board)? + # + if boardinfo test name PX4IOAR + then + set BOARD PX4IOAR + if [ -f /etc/init.d/rc.PX4IOAR ] + then + echo "[init] reading /etc/init.d/rc.PX4IOAR" + usleep 500 + sh /etc/init.d/rc.PX4IOAR + fi + else + echo "[init] PX4IOAR not detected" + fi + + # + # Are we attached to a PX4IO? + # + if boardinfo test name PX4IO + then + set BOARD PX4IO + if [ -f /etc/init.d/rc.PX4IO ] + then + echo "[init] reading /etc/init.d/rc.PX4IO" + usleep 500 + sh /etc/init.d/rc.PX4IO + fi + else + echo "[init] PX4IO not detected" + fi + + # + # Looks like we are stand-alone + # + if [ $BOARD == PX4FMU ] + then + echo "[init] no expansion board detected" + if [ -f /etc/init.d/rc.standalone ] + then + echo "[init] reading /etc/init.d/rc.standalone" + sh /etc/init.d/rc.standalone + fi + fi + + # + # We may not reach here if the airframe-specific script exits the shell. + # + echo "[init] startup done." +fi \ No newline at end of file diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 660bf61e9d..89a7678797 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -77,70 +77,3 @@ then # if APM startup is successful then nsh will exit sh /etc/init.d/rc.APM fi - - -# -# If we are still in flight mode, work out what airframe -# configuration we have and start up accordingly. -# -if [ $MODE != autostart ] -then - echo "[init] automatic startup cancelled by user script" -else - echo "[init] detecting attached hardware..." - - # - # Assume that we are PX4FMU in standalone mode - # - set BOARD PX4FMU - - # - # Are we attached to a PX4IOAR (AR.Drone carrier board)? - # - if boardinfo test name PX4IOAR - then - set BOARD PX4IOAR - if [ -f /etc/init.d/rc.PX4IOAR ] - then - echo "[init] reading /etc/init.d/rc.PX4IOAR" - usleep 500 - sh /etc/init.d/rc.PX4IOAR - fi - else - echo "[init] PX4IOAR not detected" - fi - - # - # Are we attached to a PX4IO? - # - if boardinfo test name PX4IO - then - set BOARD PX4IO - if [ -f /etc/init.d/rc.PX4IO ] - then - echo "[init] reading /etc/init.d/rc.PX4IO" - usleep 500 - sh /etc/init.d/rc.PX4IO - fi - else - echo "[init] PX4IO not detected" - fi - - # - # Looks like we are stand-alone - # - if [ $BOARD == PX4FMU ] - then - echo "[init] no expansion board detected" - if [ -f /etc/init.d/rc.standalone ] - then - echo "[init] reading /etc/init.d/rc.standalone" - sh /etc/init.d/rc.standalone - fi - fi - - # - # We may not reach here if the airframe-specific script exits the shell. - # - echo "[init] startup done." -fi From d129eff5b980f160f48a31af33ea2e8f074e61dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 13 Feb 2013 12:49:33 +0100 Subject: [PATCH 29/30] Turned all control outputs into NED frame moments, this is validated in real flight with a correct mixer setup. --- apps/controllib/fixedwing.cpp | 24 +++++++++++++++---- .../fixedwing_att_control_rate.c | 4 +--- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 03da35a0a5..d9637b4a7a 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -322,11 +322,24 @@ void BlockMultiModeBacksideAutopilot::update() _att.roll, _att.pitch, _att.yaw, _att.rollspeed, _att.pitchspeed, _att.yawspeed ); - _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); - _actuators.control[CH_ELV] = _backsideAutopilot.getElevator(); + _actuators.control[CH_AIL] = - _backsideAutopilot.getAileron(); + _actuators.control[CH_ELV] = - _backsideAutopilot.getElevator(); _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); + // XXX limit throttle to manual setting (safety) for now. + // If it turns out to be confusing, it can be removed later once + // a first binary release can be targeted. + // This is not a hack, but a design choice. + + /* do not limit in HIL */ + if (!_status.flag_hil_enabled) { + /* limit to value of manual throttle */ + _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + _actuators.control[CH_THR] : _manual.throttle; + } + + } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { @@ -337,11 +350,12 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] = _manual.throttle; } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - _stabilization.update( - _ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw, + + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_ELV] = - _stabilization.getElevator(); _actuators.control[CH_RDR] = _stabilization.getRudder(); _actuators.control[CH_THR] = _manual.throttle; } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_rate.c b/apps/fixedwing_att_control/fixedwing_att_control_rate.c index 87b6e925c1..ba5b593e28 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,9 +196,7 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); /* pitch rate (PI) */ - actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - /* set pitch minus feedforward throttle compensation (nose pitches up from throttle */ - actuators->control[1] += (-1.0f) * p.pitch_thr_ff * rate_sp->thrust; + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); /* yaw rate (PI) */ actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); From ae51810a8164dbac6b258c7d49efb28385202856 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 14 Feb 2013 20:17:18 -0800 Subject: [PATCH 30/30] Changed names and default values of attitude estimator parameters --- .../attitude_estimator_ekf_params.c | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 5b77566119..7d3812abdb 100755 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,19 +44,19 @@ /* Extended Kalman Filter covariances */ /* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_Q0, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q1, 1e1f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); /* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); -PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); -PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f); +PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); @@ -66,16 +66,16 @@ PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ - h->q0 = param_find("EKF_ATT_Q0"); - h->q1 = param_find("EKF_ATT_Q1"); - h->q2 = param_find("EKF_ATT_Q2"); - h->q3 = param_find("EKF_ATT_Q3"); - h->q4 = param_find("EKF_ATT_Q4"); + h->q0 = param_find("EKF_ATT_V2_Q0"); + h->q1 = param_find("EKF_ATT_V2_Q1"); + h->q2 = param_find("EKF_ATT_V2_Q2"); + h->q3 = param_find("EKF_ATT_V2_Q3"); + h->q4 = param_find("EKF_ATT_V2_Q4"); - h->r0 = param_find("EKF_ATT_R0"); - h->r1 = param_find("EKF_ATT_R1"); - h->r2 = param_find("EKF_ATT_R2"); - h->r3 = param_find("EKF_ATT_R3"); + h->r0 = param_find("EKF_ATT_V2_R0"); + h->r1 = param_find("EKF_ATT_V2_R1"); + h->r2 = param_find("EKF_ATT_V2_R2"); + h->r3 = param_find("EKF_ATT_V2_R3"); h->roll_off = param_find("ATT_ROLL_OFFS"); h->pitch_off = param_find("ATT_PITCH_OFFS");