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

Merge branch 'master' of github.com:PX4/Firmware

This commit is contained in:
Lorenz Meier 2013-02-16 18:01:15 +01:00
parent a0780a20b5 781845587c
commit 1c98343e7b
62 changed files with 3446 additions and 797 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ROMFS/Makefile
View File

@ -23,7 +23,8 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \
$(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \
$(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \
$(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \
$(SRCROOT)/mixers/FMU_delta.mix~mixers/FMU_delta.mix \
$(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \
$(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \
$(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \
$(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \
$(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \

163
ROMFS/logging/logconv.m Normal file → Executable file
View File

@ -45,29 +45,30 @@ end
% float true_airspeed;
% Definition of the logged values
logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{21} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{22} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{23} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{1} = struct('name', 'timestamp', 'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
logFormat{2} = struct('name', 'gyro', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{3} = struct('name', 'accel', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{4} = struct('name', 'mag', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{5} = struct('name', 'baro', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{6} = struct('name', 'baro_alt', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{7} = struct('name', 'baro_temp', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{8} = struct('name', 'control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{17} = struct('name', 'rot_matrix', 'bytes', 4, 'array', 9, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{18} = struct('name', 'vicon_position', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{20} = struct('name', 'optical_flow', 'bytes', 4, 'array', 6, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{21} = struct('name', 'omnidirectional_flow', 'bytes', 4, 'array', 22,'precision', 'float', 'machineformat', 'ieee-le');
logFormat{22} = struct('name', 'diff_pressure', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{23} = struct('name', 'ind_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{24} = struct('name', 'true_airspeed', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
% First get length of one line
columns = length(logFormat);
@ -83,7 +84,7 @@ if exist(filePath, 'file')
fileInfo = dir(filePath);
fileSize = fileInfo.bytes;
elements = int64(fileSize./(lineLength))
elements = int64(fileSize./(lineLength));
fid = fopen(filePath, 'r');
offset = 0;
@ -102,8 +103,8 @@ if exist(filePath, 'file')
% shot the flight time
time_us = sysvector.timestamp(end) - sysvector.timestamp(1);
time_s = time_us*1e-6
time_m = time_s/60
time_s = time_us*1e-6;
time_m = time_s/60;
% close the logfile
fclose(fid);
@ -112,3 +113,113 @@ if exist(filePath, 'file')
else
disp(['file: ' filePath ' does not exist' char(10)]);
end
%% Plot GPS RAW measurements
% Only plot GPS data if available
if cumsum(double(sysvector.gps_raw_position(200:end,1))) > 0
figure('units','normalized','outerposition',[0 0 1 1])
plot3(sysvector.gps_raw_position(200:end,1), sysvector.gps_raw_position(200:end,2), sysvector.gps_raw_position(200:end,3));
end
%% Plot optical flow trajectory
flow_sz = size(sysvector.timestamp);
flow_elements = flow_sz(1);
xt(1:flow_elements,1) = sysvector.timestamp(:,1); % time column [ms]
%calc dt
dt = zeros(flow_elements,1);
for i = 1:flow_elements-1
dt(i+1,1) = double(xt(i+1,1)-xt(i,1)) * 10^(-6); % timestep [s]
end
dt(1,1) = mean(dt);
global_speed = zeros(flow_elements,3);
%calc global speed (with rot matrix)
for i = 1:flow_elements
rotM = [sysvector.rot_matrix(i,1:3);sysvector.rot_matrix(i,4:6);sysvector.rot_matrix(i,7:9)]';
speedX = sysvector.optical_flow(i,3);
speedY = sysvector.optical_flow(i,4);
relSpeed = [-speedY,speedX,0];
global_speed(i,:) = relSpeed * rotM;
end
px = zeros(flow_elements,1);
py = zeros(flow_elements,1);
distance = 0;
last_vx = 0;
last_vy = 0;
elem_cnt = 0;
% Very basic accumulation, stops on bad flow quality
for i = 1:flow_elements
if sysvector.optical_flow(i,6) > 5
px(i,1) = global_speed(i,1)*dt(i,1);
py(i,1) = global_speed(i,2)*dt(i,1);
distance = distance + norm([px(i,1) py(i,1)]);
last_vx = px(i,1);
last_vy = py(i,1);
else
px(i,1) = last_vx;
py(i,1) = last_vy;
last_vx = last_vx*0.95;
last_vy = last_vy*0.95;
end
end
px_sum = cumsum(px);
py_sum = cumsum(py);
time = cumsum(dt);
figure()
set(gca, 'Units','normal');
plot(py_sum, px_sum, '-blue', 'LineWidth',2);
axis equal;
% set title and axis captions
xlabel('X position (meters)','fontsize',14)
ylabel('Y position (meters)','fontsize',14)
% mark begin and end
hold on
plot(py_sum(1,1),px_sum(1,1),'ks','LineWidth',2,...
'MarkerEdgeColor','k',...
'MarkerFaceColor','g',...
'MarkerSize',10)
hold on
plot(py_sum(end,1),px_sum(end,1),'kv','LineWidth',2,...
'MarkerEdgeColor','k',...
'MarkerFaceColor','b',...
'MarkerSize',10)
% add total length as annotation
set(gca,'fontsize',13);
legend('Trajectory', 'START', sprintf('END\n(%.2f m, %.0f:%.0f s)', distance, time_m, time_s - time_m*60));
title('Optical Flow Position Integration', 'fontsize', 15);
figure()
plot(time, sysvector.optical_flow(:,5), 'blue');
axis([time(1,1) time(end,1) 0 (max(sysvector.optical_flow(i,5))+0.2)]);
xlabel('seconds','fontsize',14);
ylabel('m','fontsize',14);
set(gca,'fontsize',13);
title('Ultrasound Altitude', 'fontsize', 15);
figure()
plot(time, global_speed(:,2), 'red');
hold on;
plot(time, global_speed(:,1), 'blue');
legend('y velocity (m/s)', 'x velocity (m/s)');
xlabel('seconds','fontsize',14);
ylabel('m/s','fontsize',14);
set(gca,'fontsize',13);
title('Optical Flow Velocity', 'fontsize', 15);

52
ROMFS/mixers/FMU_Q.mix Normal file
View File

@ -0,0 +1,52 @@
Delta-wing mixer for PX4FMU
===========================
Designed for Bormatec Camflyer Q
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -5000 -8000 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -8000 -5000 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000

0
ROMFS/mixers/FMU_delta.mix → ROMFS/mixers/FMU_X5.mix
View File

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