Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/scripts/rc.boarddetect
+66
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
ROMFS/scripts/rcS
-67
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
apps/attitude_estimator_ekf/Makefile
Regular → Executable
-2
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
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
Regular → Executable
+4
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));
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
Regular → Executable
+19 -57
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));
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
Regular → Executable
+2 -2
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;
};
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
Regular → Executable
+629 -269
View File
File diff suppressed because it is too large Load Diff
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
Regular → Executable
+2 -2
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) */
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/cross.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/cross.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/diag.c
-78
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) */
apps/attitude_estimator_ekf/codegen/diag.h
-37
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) */
apps/attitude_estimator_ekf/codegen/eye.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/eye.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/mrdivide.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/mrdivide.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/norm.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/norm.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/power.c
-84
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) */
apps/attitude_estimator_ekf/codegen/power.h
-34
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) */
apps/attitude_estimator_ekf/codegen/rdivide.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rdivide.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rtGetInf.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rt_defines.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
Regular → Executable
+1 -1
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
*
*/
apps/attitude_estimator_ekf/codegen/rtwtypes.h
Regular → Executable
+3 -3
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
*=======================================================================*/
/*=======================================================================*
apps/commander/commander.c
+13 -10
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) {
apps/controllib/fixedwing.cpp
+19 -5
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;
}
apps/drivers/drv_gps.h
+70
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 */
apps/drivers/gps/Makefile
+42
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
apps/drivers/gps/gps.cpp
+536
View File
File diff suppressed because it is too large Load Diff
apps/drivers/gps/gps_helper.cpp
+92
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;
}
apps/drivers/gps/gps_helper.h
+52
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 */
apps/drivers/gps/mtk.cpp
+274
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;
}
apps/drivers/gps/mtk.h
+124
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_ */
apps/drivers/gps/ubx.cpp
+745
View File
File diff suppressed because it is too large Load Diff
apps/drivers/gps/ubx.h
+395
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_ */
apps/drivers/mpu6000/mpu6000.cpp
+44 -5
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 */
apps/examples/kalman_demo/KalmanNav.cpp
+12 -12
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);
apps/fixedwing_att_control/fixedwing_att_control_rate.c
+1 -3
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);
apps/mavlink/mavlink.c
+8 -2
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 */

Some files were not shown because too many files have changed in this diff Show More