dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware into px4io-i2c

This commit is contained in:
Lorenz Meier 2013-02-16 16:46:57 +01:00
parent 6eb69b07a8 781845587c
commit 400b073aa3
58 changed files with 3255 additions and 770 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
ROMFS/scripts/rc.boarddetect Normal file
View File

@ -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

67
ROMFS/scripts/rcS
View File

@ -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

2
apps/attitude_estimator_ekf/Makefile Normal file → Executable file
View File

@ -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

4
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c Normal file → Executable file
View File

@ -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));

76
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c Normal file → Executable file
View File

@ -44,34 +44,20 @@
/* 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_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);
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_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);
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);
PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
@ -80,28 +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->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->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->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->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");
@ -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));

4
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h Normal file → Executable file
View File

@ -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;
};

898
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c Normal file → Executable file
View File

File diff suppressed because it is too large Load Diff

4
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h Normal file → Executable file
View File

@ -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) */

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.h Normal file → Executable file
View File

@ -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
*
*/

78
apps/attitude_estimator_ekf/codegen/diag.c
View File

@ -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) */

37
apps/attitude_estimator_ekf/codegen/diag.h
View File

@ -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 <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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) */

2
apps/attitude_estimator_ekf/codegen/eye.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/eye.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/mrdivide.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/mrdivide.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/norm.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/norm.h Normal file → Executable file
View File

@ -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
*
*/

84
apps/attitude_estimator_ekf/codegen/power.c
View File

@ -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) */

34
apps/attitude_estimator_ekf/codegen/power.h
View File

@ -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 <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#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) */

2
apps/attitude_estimator_ekf/codegen/rdivide.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rdivide.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_defines.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h Normal file → Executable file
View File

@ -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
*
*/

6
apps/attitude_estimator_ekf/codegen/rtwtypes.h Normal file → Executable file
View File

@ -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
*=======================================================================*/
/*=======================================================================*

23
apps/commander/commander.c
View File

@ -1678,12 +1678,13 @@ 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
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,10 +1695,12 @@ 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 < 2000000)
&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
&& !current_status.flag_system_armed) {
warnx("setting home position");
@ -1706,11 +1709,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) {

24
apps/controllib/fixedwing.cpp
View File

@ -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;
}

70
apps/drivers/drv_gps.h Normal file
View File

@ -0,0 +1,70 @@
/****************************************************************************
*
* 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 <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
#define GPS_DEVICE_PATH "/dev/gps"
typedef enum {
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
GPS_DRIVER_MODE_MTK,
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))
#endif /* _DRV_GPS_H */

42
apps/drivers/gps/Makefile Normal file
View File

@ -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

536
apps/drivers/gps/gps.cpp Normal file
View File

@ -0,0 +1,536 @@
/****************************************************************************
*
* 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 a serial port
*/
#include <nuttx/clock.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdio.h>
#include <stdbool.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h>
#include <systemlib/perf_counter.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h>
#include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include "ubx.h"
#include "mtk.h"
#define TIMEOUT_5HZ 400
#define RATE_MEASUREMENT_PERIOD 5000000
/* 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(const char* uart_path);
~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:
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
volatile int _task; //< worker task
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
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
/**
* 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: main GPS thread that configures the GPS and parses incoming data, always running
*/
void task_main(void);
/**
* Set the baudrate of the UART to the GPS
*/
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(const char* uart_path) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
_mode_changed(false),
_mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr),
_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));
_debug_enabled = true;
}
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 GPS driver worker task */
_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
return -errno;
}
ret = OK;
out:
return ret;
}
int
GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
{
lock();
int ret = OK;
switch (cmd) {
case SENSORIOCRESET:
cmd_reset();
break;
}
unlock();
return ret;
}
void
GPS::task_main_trampoline(void *arg)
{
g_dev->task_main();
}
void
GPS::task_main()
{
log("starting");
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR);
if (_serial_fd < 0) {
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);
}
uint64_t last_rate_measurement = hrt_absolute_time();
unsigned last_rate_count = 0;
/* loop handling received serial bytes and also configuring in between */
while (!_task_should_exit) {
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(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA
break;
default:
break;
}
unlock();
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);
} 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 > 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;
}
if (!_healthy) {
warnx("module found");
_healthy = true;
}
}
if (_healthy) {
warnx("module lost");
_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");
::close(_serial_fd);
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
void
GPS::cmd_reset()
{
//XXX add reset?
}
void
GPS::print_info()
{
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX");
break;
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
break;
default:
break;
}
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));
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("update rate: %6.2f Hz", (double)_rate);
}
usleep(100000);
}
/**
* Local functions in support of the shell command.
*/
namespace gps
{
GPS *g_dev;
void start(const char *uart_path);
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start(const char *uart_path)
{
int fd;
if (g_dev != nullptr)
errx(1, "already started");
/* create the driver */
g_dev = new GPS(uart_path);
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) {
errx(1, "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");
}
/**
* Stop the driver.
*/
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");
g_dev->print_info();
exit(0);
}
} // namespace
int
gps_main(int argc, char *argv[])
{
/* set to default */
char* device_name = GPS_DEFAULT_UART_PORT;
/*
* Start/load the driver.
*/
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();
/*
* 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();
out:
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
}

92
apps/drivers/gps/gps_helper.cpp Normal file
View File

@ -0,0 +1,92 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* 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 <termios.h>
#include <errno.h>
#include <systemlib/err.h>
#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;
warnx("try baudrate: %d\n", speed);
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;
}

52
apps/drivers/gps/gps_helper.h Normal file
View File

@ -0,0 +1,52 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* 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_helper.h */
#ifndef GPS_HELPER_H
#define GPS_HELPER_H
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
class GPS_Helper
{
public:
virtual int configure(unsigned &baud) = 0;
virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud);
};
#endif /* GPS_HELPER_H */

274
apps/drivers/gps/mtk.cpp Normal file
View File

@ -0,0 +1,274 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* 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 <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <assert.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include "mtk.h"
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_mtk_revision(0)
{
decode_init();
}
MTK::~MTK()
{
}
int
MTK::configure(unsigned &baudrate)
{
/* set baudrate first */
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))) {
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");
return -1;
}
usleep(10000);
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))) {
warnx("mtk: config write failed");
return -1;
}
usleep(10000);
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
warnx("mtk: config write failed");
return -1;
}
return 0;
}
int
MTK::receive(unsigned timeout)
{
/* poll descriptor */
pollfd fds[1];
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;
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);
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 */
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
MTK::decode_init(void)
{
_rx_ck_a = 0;
_rx_ck_b = 0;
_rx_count = 0;
_decode_state = MTK_DECODE_UNINIT;
}
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;
_mtk_revision = 16;
} else if (b == MTK_SYNC1_V19) {
_decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 19;
}
} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
if (b == MTK_SYNC2) {
_decode_state = MTK_DECODE_GOT_CK_B;
} else {
// Second start symbol was wrong, reset state machine
decode_init();
}
} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
// Add to checksum
if (_rx_count < 33)
add_byte_to_checksum(b);
// Fill packet buffer
((uint8_t*)(&packet))[_rx_count] = b;
_rx_count++;
/* 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");
ret = -1;
}
// Reset state machine to decode next packet
decode_init();
}
}
return ret;
}
void
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
} 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::add_byte_to_checksum(uint8_t b)
{
_rx_ck_a = _rx_ck_a + b;
_rx_ck_b = _rx_ck_b + _rx_ck_a;
}

124
apps/drivers/gps/mtk.h Normal file
View File

@ -0,0 +1,124 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* 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_V16 0xd0
#define MTK_SYNC1_V19 0xd1
#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"
#define MTK_TIMEOUT_5HZ 400
#define MTK_BAUDRATE 38400
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; ///< 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; ///< horizontal dilution of position (without unit)
uint8_t ck_a;
uint8_t ck_b;
} gps_mtk_packet_t;
#pragma pack(pop)
#define MTK_RECV_BUFFER_SIZE 40
class MTK : public GPS_Helper
{
public:
MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
~MTK();
int receive(unsigned timeout);
int configure(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);
/**
* Reset the parse state machine for a fresh start
*/
void decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
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];
unsigned _rx_count;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
};
#endif /* MTK_H_ */

745
apps/drivers/gps/ubx.cpp Normal file
View File

@ -0,0 +1,745 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* 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 <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <assert.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
#include "ubx.h"
#define UBX_CONFIG_TIMEOUT 100
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_waiting_for_ack(false)
{
decode_init();
}
UBX::~UBX()
{
}
int
UBX::configure(unsigned &baudrate)
{
_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;
}
int
UBX::receive(unsigned timeout)
{
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t count = 0;
while (true) {
/* 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;
}
/* 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 */
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));
}
}
}
}
int
UBX::parse_char(uint8_t 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 */
decode_init();
}
break;
/* Now look for class */
case UBX_DECODE_GOT_SYNC2:
/* everything except sync1 and sync2 needs to be added to the checksum */
add_byte_to_checksum(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
decode_init();
break;
}
break;
case UBX_DECODE_GOT_CLASS:
add_byte_to_checksum(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
decode_init();
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
// decode_init();
// 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
decode_init();
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
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");
decode_init();
// config_needed = true;
break;
}
break;
case UBX_DECODE_GOT_MESSAGEID:
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:
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)
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
/* 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");
}
return 1;
} else if (_rx_count < RECV_BUFFER_SIZE) {
_rx_count++;
} else {
warnx("ubx: buffer full");
decode_init();
return -1;
}
break;
default:
break;
}
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::decode_init(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::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::add_checksum_to_message(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;
}
/* The checksum is written to the last to bytes of a message */
message[length-2] = ck_a;
message[length-1] = ck_b;
}
void
UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{
ssize_t ret = 0;
/* Calculate the checksum now */
add_checksum_to_message(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");
}

395
apps/drivers/gps/ubx.h Normal file
View File

@ -0,0 +1,395 @@
/****************************************************************************
*
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* 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_SYNC1 0xB5
#define UBX_SYNC2 0x62
/* 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
//#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 /**< 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_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_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)
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(const int &fd, struct vehicle_gps_position_s *gps_position);
~UBX();
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 decode_init(void);
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void add_byte_to_checksum(uint8_t);
/**
* Add the two checksum bytes to an outgoing message
*/
void add_checksum_to_message(uint8_t* message, const unsigned length);
/**
* Helper to send a config packet
*/
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;
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;
};
#endif /* UBX_H_ */

49
apps/drivers/mpu6000/mpu6000.cpp
View File

@ -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);
@ -418,6 +423,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;
@ -485,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)
{
@ -610,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:
{
@ -668,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 */

24
apps/examples/kalman_demo/KalmanNav.cpp
View File

@ -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();
@ -290,7 +291,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
@ -631,8 +631,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 +651,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);

4
apps/fixedwing_att_control/fixedwing_att_control_rate.c
View File

@ -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);

10
apps/mavlink/mavlink.c
View File

@ -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_INT, 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_INT, 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_INT, 500);
} else {
/* very low baud rate, limit to 1 Hz / 1000 ms */

25
apps/mavlink/mavlink_receiver.c
View File

@ -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;
@ -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);
}

12
apps/mavlink/orb_listener.c
View File

@ -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)) {
@ -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));

8
apps/px4/tests/test_adc.c
View File

@ -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:

12
apps/px4/tests/tests_main.c
View File

@ -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");

8
apps/uORB/topics/home_position.h
View File

@ -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 */
};
/**

35
apps/uORB/topics/vehicle_attitude.h Normal file → Executable file
View File

@ -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 */
};

55
apps/uORB/topics/vehicle_gps_position.h
View File

@ -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 */
bool satellite_info_available; /**< 0 for no info, 1 for info available */
};
/**

2
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -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