mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-21 21:55:34 +08:00
Merged master into beta
This commit is contained in:
Lorenz Meier
@@ -0,0 +1,39 @@
|
||||
#!nsh
|
||||
#
|
||||
# UNTESTED UNTESTED!
|
||||
#
|
||||
# Generic 10” Hexa coaxial geometry
|
||||
#
|
||||
# Lorenz Meier <lm@inf.ethz.ch>
|
||||
#
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
#
|
||||
# Default parameters for this platform
|
||||
#
|
||||
param set MC_ATT_P 7.0
|
||||
param set MC_ATT_I 0.0
|
||||
param set MC_ATT_D 0.0
|
||||
param set MC_ATTRATE_P 0.12
|
||||
param set MC_ATTRATE_I 0.0
|
||||
param set MC_ATTRATE_D 0.004
|
||||
param set MC_YAWPOS_P 2.0
|
||||
param set MC_YAWPOS_I 0.0
|
||||
param set MC_YAWPOS_D 0.0
|
||||
param set MC_YAWRATE_P 0.3
|
||||
param set MC_YAWRATE_I 0.2
|
||||
param set MC_YAWRATE_D 0.005
|
||||
|
||||
# TODO add default MPC parameters
|
||||
fi
|
||||
|
||||
set VEHICLE_TYPE mc
|
||||
set MIXER hexa_cox
|
||||
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_RATE 400
|
||||
# DJI ESC range
|
||||
set PWM_DISARMED 900
|
||||
set PWM_MIN 1200
|
||||
set PWM_MAX 1900
|
||||
@@ -2,11 +2,36 @@
|
||||
#
|
||||
# Generic 10” Octo coaxial geometry
|
||||
#
|
||||
# Lorenz Meier <lm@inf.ethz.ch>, Anton Babushkin <anton.babushkin@me.com>
|
||||
# Lorenz Meier <lm@inf.ethz.ch>
|
||||
#
|
||||
|
||||
sh /etc/init.d/rc.mc_defaults
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
#
|
||||
# Default parameters for this platform
|
||||
#
|
||||
param set MC_ATT_P 7.0
|
||||
param set MC_ATT_I 0.0
|
||||
param set MC_ATT_D 0.0
|
||||
param set MC_ATTRATE_P 0.12
|
||||
param set MC_ATTRATE_I 0.0
|
||||
param set MC_ATTRATE_D 0.004
|
||||
param set MC_YAWPOS_P 2.0
|
||||
param set MC_YAWPOS_I 0.0
|
||||
param set MC_YAWPOS_D 0.0
|
||||
param set MC_YAWRATE_P 0.3
|
||||
param set MC_YAWRATE_I 0.2
|
||||
param set MC_YAWRATE_D 0.005
|
||||
|
||||
# TODO add default MPC parameters
|
||||
fi
|
||||
|
||||
set VEHICLE_TYPE mc
|
||||
set MIXER FMU_octo_cox
|
||||
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_OUTPUTS 12345678
|
||||
set PWM_RATE 400
|
||||
# DJI ESC range
|
||||
set PWM_DISARMED 900
|
||||
set PWM_MIN 1200
|
||||
set PWM_MAX 1900
|
||||
|
||||
@@ -180,6 +180,15 @@ then
|
||||
sh /etc/init.d/10016_3dr_iris
|
||||
fi
|
||||
|
||||
#
|
||||
# Hexa Coaxial
|
||||
#
|
||||
|
||||
if param compare SYS_AUTOSTART 11001
|
||||
then
|
||||
sh /etc/init.d/11001_hexa_cox
|
||||
fi
|
||||
|
||||
#
|
||||
# Octo Coaxial
|
||||
#
|
||||
|
||||
@@ -479,6 +479,9 @@ then
|
||||
set MAV_TYPE 2
|
||||
fi
|
||||
if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ]
|
||||
set MAV_TYPE 13
|
||||
fi
|
||||
if [ $MIXER == hexa_cox ]
|
||||
then
|
||||
set MAV_TYPE 13
|
||||
fi
|
||||
|
||||
@@ -23,13 +23,13 @@ for the elevons.
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 -3000 -5000 0 -10000 10000
|
||||
S: 0 1 -5000 -5000 0 -10000 10000
|
||||
S: 0 0 -8000 -8000 0 -10000 10000
|
||||
S: 0 1 6000 6000 0 -10000 10000
|
||||
|
||||
M: 2
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 0 -5000 -3000 0 -10000 10000
|
||||
S: 0 1 5000 5000 0 -10000 10000
|
||||
S: 0 0 -8000 -8000 0 -10000 10000
|
||||
S: 0 1 -6000 -6000 0 -10000 10000
|
||||
|
||||
Output 2
|
||||
--------
|
||||
@@ -48,7 +48,7 @@ M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 3 0 20000 -10000 -10000 10000
|
||||
|
||||
Gimbal / payload mixer for last four channels
|
||||
Gimbal / flaps / payload mixer for last four channels
|
||||
-----------------------------------------------------
|
||||
|
||||
M: 1
|
||||
@@ -66,4 +66,3 @@ S: 0 6 10000 10000 0 -10000 10000
|
||||
M: 1
|
||||
O: 10000 10000 0 -10000 10000
|
||||
S: 0 7 10000 10000 0 -10000 10000
|
||||
|
||||
|
||||
@@ -1,7 +1,3 @@
|
||||
Multirotor mixer for PX4FMU
|
||||
===========================
|
||||
|
||||
This file defines a single mixer for a hexacopter in the + configuration. All controls
|
||||
are mixed 100%.
|
||||
# Hexa +
|
||||
|
||||
R: 6+ 10000 10000 10000 0
|
||||
|
||||
@@ -1,7 +1,3 @@
|
||||
Multirotor mixer for PX4FMU
|
||||
===========================
|
||||
|
||||
This file defines a single mixer for a hexacopter in the X configuration. All controls
|
||||
are mixed 100%.
|
||||
# Hexa X
|
||||
|
||||
R: 6x 10000 10000 10000 0
|
||||
|
||||
@@ -1,7 +1,3 @@
|
||||
Multirotor mixer for PX4FMU
|
||||
===========================
|
||||
|
||||
This file defines a single mixer for a octocopter in the + configuration. All controls
|
||||
are mixed 100%.
|
||||
# Octo +
|
||||
|
||||
R: 8+ 10000 10000 10000 0
|
||||
|
||||
@@ -1,7 +1,3 @@
|
||||
Multirotor mixer for PX4FMU
|
||||
===========================
|
||||
|
||||
This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls
|
||||
are mixed 100%.
|
||||
# Octo coaxial
|
||||
|
||||
R: 8c 10000 10000 10000 0
|
||||
|
||||
@@ -1,7 +1,3 @@
|
||||
Multirotor mixer for PX4FMU
|
||||
===========================
|
||||
|
||||
This file defines a single mixer for a octocopter in the X configuration. All controls
|
||||
are mixed 100%.
|
||||
# Octo X
|
||||
|
||||
R: 8x 10000 10000 10000 0
|
||||
|
||||
@@ -1,154 +0,0 @@
|
||||
PX4 mixer definitions
|
||||
=====================
|
||||
|
||||
Files in this directory implement example mixers that can be used as a basis
|
||||
for customisation, or for general testing purposes.
|
||||
|
||||
Mixer basics
|
||||
------------
|
||||
|
||||
Mixers combine control values from various sources (control tasks, user inputs,
|
||||
etc.) and produce output values suitable for controlling actuators; servos,
|
||||
motors, switches and so on.
|
||||
|
||||
An actuator derives its value from the combination of one or more control
|
||||
values. Each of the control values is scaled according to the actuator's
|
||||
configuration and then combined to produce the actuator value, which may then be
|
||||
further scaled to suit the specific output type.
|
||||
|
||||
Internally, all scaling is performed using floating point values. Inputs and
|
||||
outputs are clamped to the range -1.0 to 1.0.
|
||||
|
||||
control control control
|
||||
| | |
|
||||
v v v
|
||||
scale scale scale
|
||||
| | |
|
||||
| v |
|
||||
+-------> mix <------+
|
||||
|
|
||||
scale
|
||||
|
|
||||
v
|
||||
out
|
||||
|
||||
Scaling
|
||||
-------
|
||||
|
||||
Basic scalers provide linear scaling of the input to the output.
|
||||
|
||||
Each scaler allows the input value to be scaled independently for inputs
|
||||
greater/less than zero. An offset can be applied to the output, and lower and
|
||||
upper boundary constraints can be applied. Negative scaling factors cause the
|
||||
output to be inverted (negative input produces positive output).
|
||||
|
||||
Scaler pseudocode:
|
||||
|
||||
if (input < 0)
|
||||
output = (input * NEGATIVE_SCALE) + OFFSET
|
||||
else
|
||||
output = (input * POSITIVE_SCALE) + OFFSET
|
||||
|
||||
if (output < LOWER_LIMIT)
|
||||
output = LOWER_LIMIT
|
||||
if (output > UPPER_LIMIT)
|
||||
output = UPPER_LIMIT
|
||||
|
||||
Syntax
|
||||
------
|
||||
|
||||
Mixer definitions are text files; lines beginning with a single capital letter
|
||||
followed by a colon are significant. All other lines are ignored, meaning that
|
||||
explanatory text can be freely mixed with the definitions.
|
||||
|
||||
Each file may define more than one mixer; the allocation of mixers to actuators
|
||||
is specific to the device reading the mixer definition, and the number of
|
||||
actuator outputs generated by a mixer is specific to the mixer.
|
||||
|
||||
A mixer begins with a line of the form
|
||||
|
||||
<tag>: <mixer arguments>
|
||||
|
||||
The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a
|
||||
multirotor mixer, etc.
|
||||
|
||||
Null Mixer
|
||||
..........
|
||||
|
||||
A null mixer consumes no controls and generates a single actuator output whose
|
||||
value is always zero. Typically a null mixer is used as a placeholder in a
|
||||
collection of mixers in order to achieve a specific pattern of actuator outputs.
|
||||
|
||||
The null mixer definition has the form:
|
||||
|
||||
Z:
|
||||
|
||||
Simple Mixer
|
||||
............
|
||||
|
||||
A simple mixer combines zero or more control inputs into a single actuator
|
||||
output. Inputs are scaled, and the mixing function sums the result before
|
||||
applying an output scaler.
|
||||
|
||||
A simple mixer definition begins with:
|
||||
|
||||
M: <control count>
|
||||
O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
|
||||
|
||||
If <control count> is zero, the sum is effectively zero and the mixer will
|
||||
output a fixed value that is <offset> constrained by <lower limit> and <upper
|
||||
limit>.
|
||||
|
||||
The second line defines the output scaler with scaler parameters as discussed
|
||||
above. Whilst the calculations are performed as floating-point operations, the
|
||||
values stored in the definition file are scaled by a factor of 10000; i.e. an
|
||||
offset of -0.5 is encoded as -5000.
|
||||
|
||||
The definition continues with <control count> entries describing the control
|
||||
inputs and their scaling, in the form:
|
||||
|
||||
S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
|
||||
|
||||
The <group> value identifies the control group from which the scaler will read,
|
||||
and the <index> value an offset within that group. These values are specific to
|
||||
the device reading the mixer definition.
|
||||
|
||||
When used to mix vehicle controls, mixer group zero is the vehicle attitude
|
||||
control group, and index values zero through three are normally roll, pitch,
|
||||
yaw and thrust respectively.
|
||||
|
||||
The remaining fields on the line configure the control scaler with parameters as
|
||||
discussed above. Whilst the calculations are performed as floating-point
|
||||
operations, the values stored in the definition file are scaled by a factor of
|
||||
10000; i.e. an offset of -0.5 is encoded as -5000.
|
||||
|
||||
Multirotor Mixer
|
||||
................
|
||||
|
||||
The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
|
||||
into a set of actuator outputs intended to drive motor speed controllers.
|
||||
|
||||
The mixer definition is a single line of the form:
|
||||
|
||||
R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
|
||||
|
||||
The supported geometries include:
|
||||
|
||||
4x - quadrotor in X configuration
|
||||
4+ - quadrotor in + configuration
|
||||
6x - hexcopter in X configuration
|
||||
6+ - hexcopter in + configuration
|
||||
8x - octocopter in X configuration
|
||||
8+ - octocopter in + configuration
|
||||
|
||||
Each of the roll, pitch and yaw scale values determine scaling of the roll,
|
||||
pitch and yaw controls relative to the thrust control. Whilst the calculations
|
||||
are performed as floating-point operations, the values stored in the definition
|
||||
file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
|
||||
|
||||
Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
|
||||
thrust input ranges from 0.0 to 1.0. Output for each actuator is in the
|
||||
range -1.0 to 1.0.
|
||||
|
||||
In the case where an actuator saturates, all actuator values are rescaled so that
|
||||
the saturating actuator is limited to 1.0.
|
||||
@@ -0,0 +1,3 @@
|
||||
# Hexa coaxial
|
||||
|
||||
R: 6c 10000 10000 10000 0
|
||||
@@ -50,6 +50,9 @@
|
||||
# Currently only used for informational purposes.
|
||||
#
|
||||
|
||||
# for python2.7 compatibility
|
||||
from __future__ import print_function
|
||||
|
||||
import sys
|
||||
import argparse
|
||||
import binascii
|
||||
@@ -154,6 +157,8 @@ class uploader(object):
|
||||
PROG_MULTI = b'\x27'
|
||||
READ_MULTI = b'\x28' # rev2 only
|
||||
GET_CRC = b'\x29' # rev3+
|
||||
GET_OTP = b'\x2a' # rev4+ , get a word from OTP area
|
||||
GET_SN = b'\x2b' # rev4+ , get a word from SN area
|
||||
REBOOT = b'\x30'
|
||||
|
||||
INFO_BL_REV = b'\x01' # bootloader protocol revision
|
||||
@@ -175,6 +180,8 @@ class uploader(object):
|
||||
def __init__(self, portname, baudrate):
|
||||
# open the port, keep the default timeout short so we can poll quickly
|
||||
self.port = serial.Serial(portname, baudrate, timeout=0.5)
|
||||
self.otp = b''
|
||||
self.sn = b''
|
||||
|
||||
def close(self):
|
||||
if self.port is not None:
|
||||
@@ -237,6 +244,22 @@ class uploader(object):
|
||||
self.__getSync()
|
||||
return value
|
||||
|
||||
# send the GET_OTP command and wait for an info parameter
|
||||
def __getOTP(self, param):
|
||||
t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array.
|
||||
self.__send(uploader.GET_OTP + t + uploader.EOC)
|
||||
value = self.__recv(4)
|
||||
self.__getSync()
|
||||
return value
|
||||
|
||||
# send the GET_OTP command and wait for an info parameter
|
||||
def __getSN(self, param):
|
||||
t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array.
|
||||
self.__send(uploader.GET_SN + t + uploader.EOC)
|
||||
value = self.__recv(4)
|
||||
self.__getSync()
|
||||
return value
|
||||
|
||||
# send the CHIP_ERASE command and wait for the bootloader to become ready
|
||||
def __erase(self):
|
||||
self.__send(uploader.CHIP_ERASE
|
||||
@@ -353,6 +376,31 @@ class uploader(object):
|
||||
if self.fw_maxsize < fw.property('image_size'):
|
||||
raise RuntimeError("Firmware image is too large for this board")
|
||||
|
||||
# OTP added in v4:
|
||||
if self.bl_rev > 3:
|
||||
for byte in range(0,32*6,4):
|
||||
x = self.__getOTP(byte)
|
||||
self.otp = self.otp + x
|
||||
print(binascii.hexlify(x).decode('Latin-1') + ' ', end='')
|
||||
# see src/modules/systemlib/otp.h in px4 code:
|
||||
self.otp_id = self.otp[0:4]
|
||||
self.otp_idtype = self.otp[4:5]
|
||||
self.otp_vid = self.otp[8:4:-1]
|
||||
self.otp_pid = self.otp[12:8:-1]
|
||||
self.otp_coa = self.otp[32:160]
|
||||
# show user:
|
||||
print("type: " + self.otp_id.decode('Latin-1'))
|
||||
print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
|
||||
print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
|
||||
print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
|
||||
print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
|
||||
print("sn: ", end='')
|
||||
for byte in range(0,12,4):
|
||||
x = self.__getSN(byte)
|
||||
x = x[::-1] # reverse the bytes
|
||||
self.sn = self.sn + x
|
||||
print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
|
||||
print('')
|
||||
print("erase...")
|
||||
self.__erase()
|
||||
|
||||
|
||||
@@ -38,6 +38,8 @@ MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
MODULES += drivers/frsky_telemetry
|
||||
MODULES += modules/sensors
|
||||
MODULES += drivers/mkblctrl
|
||||
|
||||
|
||||
# Needs to be burned to the ground and re-written; for now,
|
||||
# just don't build it.
|
||||
|
||||
@@ -18,6 +18,12 @@ MODULES += drivers/stm32/tone_alarm
|
||||
MODULES += drivers/led
|
||||
MODULES += drivers/boards/px4fmu-v2
|
||||
MODULES += drivers/px4io
|
||||
MODULES += drivers/rgbled
|
||||
MODULES += drivers/mpu6000
|
||||
MODULES += drivers/lsm303d
|
||||
MODULES += drivers/l3gd20
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/tests
|
||||
@@ -31,6 +37,9 @@ MODULES += systemcmds/hw_ver
|
||||
MODULES += modules/systemlib
|
||||
MODULES += modules/systemlib/mixer
|
||||
MODULES += modules/uORB
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
MODULES += lib/mathlib
|
||||
MODULES += lib/mathlib/math/filter
|
||||
|
||||
#
|
||||
# Libraries
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
@@ -86,6 +86,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
|
||||
_collect_phase(false),
|
||||
_diff_pres_offset(0.0f),
|
||||
_airspeed_pub(-1),
|
||||
_class_instance(-1),
|
||||
_conversion_interval(conversion_interval),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
|
||||
@@ -102,6 +103,9 @@ Airspeed::~Airspeed()
|
||||
/* make sure we are truly inactive */
|
||||
stop();
|
||||
|
||||
if (_class_instance != -1)
|
||||
unregister_class_devname(AIRSPEED_DEVICE_PATH, _class_instance);
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
delete _reports;
|
||||
@@ -126,13 +130,23 @@ Airspeed::init()
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
/* get a publish handle on the airspeed topic */
|
||||
differential_pressure_s zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &zero_report);
|
||||
/* register alternate interfaces if we have to */
|
||||
_class_instance = register_class_devname(AIRSPEED_DEVICE_PATH);
|
||||
|
||||
if (_airspeed_pub < 0)
|
||||
warnx("failed to create airspeed sensor object. Did you start uOrb?");
|
||||
/* publication init */
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
struct differential_pressure_s arp;
|
||||
measure();
|
||||
_reports->get(&arp);
|
||||
|
||||
/* measurement will have generated a report, publish */
|
||||
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
|
||||
|
||||
if (_airspeed_pub < 0)
|
||||
warnx("failed to create airspeed sensor object. uORB started?");
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
@@ -127,6 +127,8 @@ protected:
|
||||
|
||||
orb_advert_t _airspeed_pub;
|
||||
|
||||
int _class_instance;
|
||||
|
||||
unsigned _conversion_interval;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -153,6 +153,7 @@ private:
|
||||
float _accel_range_scale;
|
||||
float _accel_range_m_s2;
|
||||
orb_advert_t _accel_topic;
|
||||
int _class_instance;
|
||||
|
||||
unsigned _current_lowpass;
|
||||
unsigned _current_range;
|
||||
@@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) :
|
||||
_accel_range_scale(0.0f),
|
||||
_accel_range_m_s2(0.0f),
|
||||
_accel_topic(-1),
|
||||
_class_instance(-1),
|
||||
_current_lowpass(0),
|
||||
_current_range(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
|
||||
@@ -282,11 +284,6 @@ BMA180::init()
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
/* advertise sensor topic */
|
||||
struct accel_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
|
||||
|
||||
/* perform soft reset (p48) */
|
||||
write_reg(ADDR_RESET, SOFT_RESET);
|
||||
|
||||
@@ -322,6 +319,19 @@ BMA180::init()
|
||||
ret = ERROR;
|
||||
}
|
||||
|
||||
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
measure();
|
||||
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
struct accel_report arp;
|
||||
_reports->get(&arp);
|
||||
|
||||
/* measurement will have generated a report, publish */
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
@@ -723,7 +733,8 @@ BMA180::measure()
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
|
||||
if (_accel_topic > 0 && !(_pub_blocked))
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
|
||||
|
||||
/* stop the perf counter */
|
||||
perf_end(_sample_perf);
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -38,6 +38,7 @@
|
||||
*/
|
||||
|
||||
#include "device.h"
|
||||
#include "drivers/drv_device.h"
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <arch/irq.h>
|
||||
@@ -93,6 +94,7 @@ CDev::CDev(const char *name,
|
||||
Device(name, irq),
|
||||
// public
|
||||
// protected
|
||||
_pub_blocked(false),
|
||||
// private
|
||||
_devname(devname),
|
||||
_registered(false),
|
||||
@@ -256,6 +258,14 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
case DIOC_GETPRIV:
|
||||
*(void **)(uintptr_t)arg = (void *)this;
|
||||
return OK;
|
||||
break;
|
||||
case DEVIOCSPUBBLOCK:
|
||||
_pub_blocked = (arg != 0);
|
||||
return OK;
|
||||
break;
|
||||
case DEVIOCGPUBBLOCK:
|
||||
return _pub_blocked;
|
||||
break;
|
||||
}
|
||||
|
||||
return -ENOTTY;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -415,6 +415,8 @@ protected:
|
||||
*/
|
||||
virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
|
||||
|
||||
bool _pub_blocked; /**< true if publishing should be blocked */
|
||||
|
||||
private:
|
||||
static const unsigned _max_pollwaiters = 8;
|
||||
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 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 drv_device.h
|
||||
*
|
||||
* Generic device / sensor interface.
|
||||
*/
|
||||
|
||||
#ifndef _DRV_DEVICE_H
|
||||
#define _DRV_DEVICE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*/
|
||||
|
||||
#define _DEVICEIOCBASE (0x100)
|
||||
#define _DEVICEIOC(_n) (_IOC(_DEVICEIOCBASE, _n))
|
||||
|
||||
/** ask device to stop publishing */
|
||||
#define DEVIOCSPUBBLOCK _DEVICEIOC(0)
|
||||
|
||||
/** check publication block status */
|
||||
#define DEVIOCGPUBBLOCK _DEVICEIOC(1)
|
||||
|
||||
#endif /* _DRV_DEVICE_H */
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
@@ -184,8 +184,10 @@ ETSAirspeed::collect()
|
||||
report.voltage = 0;
|
||||
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
|
||||
if (_airspeed_pub > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
|
||||
}
|
||||
|
||||
new_report(report);
|
||||
|
||||
|
||||
+13
-8
@@ -289,11 +289,13 @@ GPS::task_main()
|
||||
|
||||
//no time and satellite information simulated
|
||||
|
||||
if (_report_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||
if (!(_pub_blocked)) {
|
||||
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);
|
||||
} else {
|
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||
}
|
||||
}
|
||||
|
||||
usleep(2e5);
|
||||
@@ -330,11 +332,14 @@ GPS::task_main()
|
||||
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);
|
||||
if (!(_pub_blocked)) {
|
||||
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++;
|
||||
|
||||
@@ -1,8 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -41,6 +39,9 @@
|
||||
|
||||
/**
|
||||
* @file gps_helper.cpp
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
float
|
||||
|
||||
@@ -1,8 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -35,6 +33,8 @@
|
||||
|
||||
/**
|
||||
* @file gps_helper.h
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef GPS_HELPER_H
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2012-2014 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
|
||||
|
||||
@@ -1,8 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Copyright (c) 2012, 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
|
||||
@@ -33,7 +31,12 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file mkt.cpp */
|
||||
/**
|
||||
* @file mtk.cpp
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
|
||||
@@ -1,8 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Copyright (c) 2012, 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
|
||||
@@ -33,7 +31,12 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file mtk.h */
|
||||
/**
|
||||
* @file mtk.cpp
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef MTK_H_
|
||||
#define MTK_H_
|
||||
|
||||
@@ -1,9 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
* Copyright (c) 2012, 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
|
||||
@@ -40,8 +37,13 @@
|
||||
* U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
|
||||
* including Prototol Specification.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
|
||||
+12
-5
@@ -1,9 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
* Anton Babushkin <anton.babushkin@me.com>
|
||||
* Copyright (c) 2012, 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
|
||||
@@ -34,7 +31,17 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* @file U-Blox protocol definitions */
|
||||
/**
|
||||
* @file ubx.h
|
||||
*
|
||||
* U-Blox protocol definition. Following u-blox 6/7 Receiver Description
|
||||
* including Prototol Specification.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef UBX_H_
|
||||
#define UBX_H_
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -381,16 +381,6 @@ HMC5883::init()
|
||||
reset();
|
||||
|
||||
_class_instance = register_class_devname(MAG_DEVICE_PATH);
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
/* get a publish handle on the mag topic if we are
|
||||
* the primary mag */
|
||||
struct mag_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
|
||||
|
||||
if (_mag_topic < 0)
|
||||
debug("failed to create sensor_mag object");
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but not calibrated */
|
||||
@@ -867,9 +857,17 @@ HMC5883::collect()
|
||||
/* z remains z */
|
||||
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
|
||||
|
||||
if (_mag_topic != -1) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
|
||||
|
||||
if (_mag_topic != -1) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
|
||||
} else {
|
||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &new_report);
|
||||
|
||||
if (_mag_topic < 0)
|
||||
debug("failed to create sensor_mag publication");
|
||||
}
|
||||
}
|
||||
|
||||
/* post a report to the ring */
|
||||
@@ -1140,10 +1138,12 @@ int HMC5883::check_calibration()
|
||||
SUBSYSTEM_TYPE_MAG};
|
||||
static orb_advert_t pub = -1;
|
||||
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
if (!(_pub_blocked)) {
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -379,15 +379,24 @@ L3GD20::init()
|
||||
goto out;
|
||||
|
||||
_class_instance = register_class_devname(GYRO_DEVICE_PATH);
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
/* advertise sensor topic */
|
||||
struct gyro_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &zero_report);
|
||||
}
|
||||
|
||||
reset();
|
||||
|
||||
measure();
|
||||
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
struct gyro_report grp;
|
||||
_reports->get(&grp);
|
||||
|
||||
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
|
||||
|
||||
if (_gyro_topic < 0)
|
||||
debug("failed to create sensor_gyro publication");
|
||||
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
out:
|
||||
return ret;
|
||||
@@ -894,8 +903,10 @@ L3GD20::measure()
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
if (_gyro_topic > 0)
|
||||
if (_gyro_topic > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
|
||||
}
|
||||
|
||||
_read++;
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
@@ -277,15 +277,15 @@ private:
|
||||
unsigned _mag_samplerate;
|
||||
|
||||
orb_advert_t _accel_topic;
|
||||
int _class_instance;
|
||||
int _accel_class_instance;
|
||||
|
||||
unsigned _accel_read;
|
||||
unsigned _mag_read;
|
||||
|
||||
perf_counter_t _accel_sample_perf;
|
||||
perf_counter_t _mag_sample_perf;
|
||||
perf_counter_t _reg7_resets;
|
||||
perf_counter_t _reg1_resets;
|
||||
perf_counter_t _reg7_resets;
|
||||
perf_counter_t _extreme_values;
|
||||
perf_counter_t _accel_reschedules;
|
||||
|
||||
@@ -295,8 +295,8 @@ private:
|
||||
|
||||
// expceted values of reg1 and reg7 to catch in-flight
|
||||
// brownouts of the sensor
|
||||
uint8_t _reg7_expected;
|
||||
uint8_t _reg1_expected;
|
||||
uint8_t _reg7_expected;
|
||||
|
||||
// accel logging
|
||||
int _accel_log_fd;
|
||||
@@ -500,7 +500,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
|
||||
_mag_range_scale(0.0f),
|
||||
_mag_samplerate(0),
|
||||
_accel_topic(-1),
|
||||
_class_instance(-1),
|
||||
_accel_class_instance(-1),
|
||||
_accel_read(0),
|
||||
_mag_read(0),
|
||||
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
|
||||
@@ -551,8 +551,8 @@ LSM303D::~LSM303D()
|
||||
if (_mag_reports != nullptr)
|
||||
delete _mag_reports;
|
||||
|
||||
if (_class_instance != -1)
|
||||
unregister_class_devname(ACCEL_DEVICE_PATH, _class_instance);
|
||||
if (_accel_class_instance != -1)
|
||||
unregister_class_devname(ACCEL_DEVICE_PATH, _accel_class_instance);
|
||||
|
||||
delete _mag;
|
||||
|
||||
@@ -562,13 +562,13 @@ LSM303D::~LSM303D()
|
||||
perf_free(_reg1_resets);
|
||||
perf_free(_reg7_resets);
|
||||
perf_free(_extreme_values);
|
||||
perf_free(_accel_reschedules);
|
||||
}
|
||||
|
||||
int
|
||||
LSM303D::init()
|
||||
{
|
||||
int ret = ERROR;
|
||||
int mag_ret;
|
||||
|
||||
/* do SPI init (and probe) first */
|
||||
if (SPI::init() != OK) {
|
||||
@@ -597,13 +597,37 @@ LSM303D::init()
|
||||
goto out;
|
||||
}
|
||||
|
||||
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
// we are the primary accel device, so advertise to
|
||||
// the ORB
|
||||
struct accel_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
|
||||
/* fill report structures */
|
||||
measure();
|
||||
|
||||
if (_mag->_mag_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
struct mag_report mrp;
|
||||
_mag_reports->get(&mrp);
|
||||
|
||||
/* measurement will have generated a report, publish */
|
||||
_mag->_mag_topic = orb_advertise(ORB_ID(sensor_mag), &mrp);
|
||||
|
||||
if (_mag->_mag_topic < 0)
|
||||
debug("failed to create sensor_mag publication");
|
||||
|
||||
}
|
||||
|
||||
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
|
||||
|
||||
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
struct accel_report arp;
|
||||
_accel_reports->get(&arp);
|
||||
|
||||
/* measurement will have generated a report, publish */
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
|
||||
|
||||
if (_accel_topic < 0)
|
||||
debug("failed to create sensor_accel publication");
|
||||
|
||||
}
|
||||
|
||||
out:
|
||||
@@ -727,7 +751,7 @@ LSM303D::check_extremes(const accel_report *arb)
|
||||
_last_log_us = now;
|
||||
::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n",
|
||||
(unsigned long long)arb->timestamp,
|
||||
arb->x, arb->y, arb->z,
|
||||
(double)arb->x, (double)arb->y, (double)arb->z,
|
||||
(int)arb->x_raw,
|
||||
(int)arb->y_raw,
|
||||
(int)arb->z_raw,
|
||||
@@ -1517,8 +1541,8 @@ LSM303D::measure()
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
if (_accel_topic != -1) {
|
||||
/* publish for subscribers */
|
||||
if (_accel_topic > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
}
|
||||
|
||||
@@ -1591,8 +1615,8 @@ LSM303D::mag_measure()
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
if (_mag->_mag_topic != -1) {
|
||||
/* publish for subscribers */
|
||||
if (_mag->_mag_topic > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
|
||||
}
|
||||
|
||||
@@ -1707,13 +1731,6 @@ LSM303D_mag::init()
|
||||
goto out;
|
||||
|
||||
_mag_class_instance = register_class_devname(MAG_DEVICE_PATH);
|
||||
if (_mag_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
// we are the primary mag device, so advertise to
|
||||
// the ORB
|
||||
struct mag_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_mag_topic = orb_advertise(ORB_ID(sensor_mag), &zero_report);
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013, 2014 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
|
||||
@@ -208,8 +208,10 @@ MEASAirspeed::collect()
|
||||
report.voltage = 0;
|
||||
report.max_differential_pressure_pa = _max_differential_pressure_pa;
|
||||
|
||||
/* announce the airspeed if needed, just publish else */
|
||||
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
|
||||
if (_airspeed_pub > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
|
||||
}
|
||||
|
||||
new_report(report);
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -443,7 +443,6 @@ int
|
||||
MPU6000::init()
|
||||
{
|
||||
int ret;
|
||||
int gyro_ret;
|
||||
|
||||
/* do SPI init (and probe) first */
|
||||
ret = SPI::init();
|
||||
@@ -488,16 +487,36 @@ MPU6000::init()
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* fetch an initial set of measurements for advertisement */
|
||||
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
|
||||
|
||||
measure();
|
||||
|
||||
_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
|
||||
if (_accel_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
/* advertise accel topic */
|
||||
accel_report ar;
|
||||
_accel_reports->get(&ar);
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
|
||||
}
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
struct accel_report arp;
|
||||
_accel_reports->get(&arp);
|
||||
|
||||
/* measurement will have generated a report, publish */
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
|
||||
|
||||
if (_accel_topic < 0)
|
||||
debug("failed to create sensor_accel publication");
|
||||
|
||||
}
|
||||
|
||||
if (_gyro->_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
struct gyro_report grp;
|
||||
_gyro_reports->get(&grp);
|
||||
|
||||
_gyro->_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &grp);
|
||||
|
||||
if (_gyro->_gyro_topic < 0)
|
||||
debug("failed to create sensor_gyro publication");
|
||||
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
@@ -1307,10 +1326,13 @@ MPU6000::measure()
|
||||
poll_notify(POLLIN);
|
||||
_gyro->parent_poll_notify();
|
||||
|
||||
if (_accel_topic != -1) {
|
||||
if (_accel_topic > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
|
||||
}
|
||||
if (_gyro->_gyro_topic != -1) {
|
||||
|
||||
if (_gyro->_gyro_topic > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
|
||||
}
|
||||
|
||||
@@ -1356,11 +1378,6 @@ MPU6000_gyro::init()
|
||||
}
|
||||
|
||||
_gyro_class_instance = register_class_devname(GYRO_DEVICE_PATH);
|
||||
if (_gyro_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
gyro_report gr;
|
||||
memset(&gr, 0, sizeof(gr));
|
||||
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2014 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
|
||||
@@ -90,6 +90,7 @@ static const int ERROR = -1;
|
||||
/* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */
|
||||
#define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */
|
||||
#define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */
|
||||
#define MS5611_BARO_DEVICE_PATH "/dev/ms5611"
|
||||
|
||||
class MS5611 : public device::CDev
|
||||
{
|
||||
@@ -132,6 +133,8 @@ protected:
|
||||
|
||||
orb_advert_t _baro_topic;
|
||||
|
||||
int _class_instance;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _measure_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
@@ -192,7 +195,7 @@ protected:
|
||||
extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
|
||||
|
||||
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
|
||||
CDev("MS5611", BARO_DEVICE_PATH),
|
||||
CDev("MS5611", MS5611_BARO_DEVICE_PATH),
|
||||
_interface(interface),
|
||||
_prom(prom_buf.s),
|
||||
_measure_ticks(0),
|
||||
@@ -204,6 +207,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
|
||||
_SENS(0),
|
||||
_msl_pressure(101325),
|
||||
_baro_topic(-1),
|
||||
_class_instance(-1),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
|
||||
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
|
||||
@@ -218,6 +222,9 @@ MS5611::~MS5611()
|
||||
/* make sure we are truly inactive */
|
||||
stop_cycle();
|
||||
|
||||
if (_class_instance != -1)
|
||||
unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance);
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
delete _reports;
|
||||
@@ -251,18 +258,57 @@ MS5611::init()
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* get a publish handle on the baro topic */
|
||||
struct baro_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &zero_report);
|
||||
/* register alternate interfaces if we have to */
|
||||
_class_instance = register_class_devname(BARO_DEVICE_PATH);
|
||||
|
||||
if (_baro_topic < 0) {
|
||||
debug("failed to create sensor_baro object");
|
||||
ret = -ENOSPC;
|
||||
goto out;
|
||||
}
|
||||
struct baro_report brp;
|
||||
/* do a first measurement cycle to populate reports with valid data */
|
||||
_measure_phase = 0;
|
||||
_reports->flush();
|
||||
|
||||
/* this do..while is goto without goto */
|
||||
do {
|
||||
/* do temperature first */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* now do a pressure measurement */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(MS5611_CONVERSION_INTERVAL);
|
||||
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
_reports->get(&brp);
|
||||
|
||||
ret = OK;
|
||||
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
|
||||
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &brp);
|
||||
|
||||
if (_baro_topic < 0)
|
||||
debug("failed to create sensor_baro publication");
|
||||
}
|
||||
|
||||
} while (0);
|
||||
|
||||
ret = OK;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
@@ -670,7 +716,10 @@ MS5611::collect()
|
||||
report.altitude = (((pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
|
||||
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
|
||||
if (_baro_topic > 0 && !(_pub_blocked)) {
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
|
||||
}
|
||||
|
||||
if (_reports->force(&report)) {
|
||||
perf_count(_buffer_overflows);
|
||||
@@ -812,7 +861,7 @@ start()
|
||||
goto fail;
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||
fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
|
||||
if (fd < 0) {
|
||||
warnx("can't open baro device");
|
||||
goto fail;
|
||||
@@ -846,10 +895,10 @@ test()
|
||||
ssize_t sz;
|
||||
int ret;
|
||||
|
||||
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
|
||||
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
@@ -905,7 +954,7 @@ test()
|
||||
void
|
||||
reset()
|
||||
{
|
||||
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "failed ");
|
||||
@@ -944,10 +993,10 @@ calibrate(unsigned altitude)
|
||||
float pressure;
|
||||
float p1;
|
||||
|
||||
int fd = open(BARO_DEVICE_PATH, O_RDONLY);
|
||||
int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", BARO_DEVICE_PATH);
|
||||
err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH);
|
||||
|
||||
/* start the sensor polling at max */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX))
|
||||
|
||||
@@ -42,6 +42,8 @@
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <dirent.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
@@ -50,6 +52,7 @@
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_device.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "state_machine_helper.h"
|
||||
@@ -332,6 +335,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
|
||||
|
||||
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
|
||||
valid_transition = true;
|
||||
|
||||
// Disable publication of all attached sensors
|
||||
|
||||
/* list directory */
|
||||
DIR *d;
|
||||
struct dirent *direntry;
|
||||
d = opendir("/dev");
|
||||
if (d) {
|
||||
|
||||
while ((direntry = readdir(d)) != NULL) {
|
||||
|
||||
int sensfd = ::open(direntry->d_name, 0);
|
||||
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
|
||||
close(sensfd);
|
||||
|
||||
printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
|
||||
}
|
||||
|
||||
closedir(d);
|
||||
|
||||
warnx("directory listing ok (FS mounted and readable)");
|
||||
|
||||
} else {
|
||||
/* failed opening dir */
|
||||
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
@@ -56,6 +56,7 @@
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include "systemlib/systemlib.h"
|
||||
#include "systemlib/err.h"
|
||||
@@ -65,6 +66,7 @@ __EXPORT int config_main(int argc, char *argv[]);
|
||||
static void do_gyro(int argc, char *argv[]);
|
||||
static void do_accel(int argc, char *argv[]);
|
||||
static void do_mag(int argc, char *argv[]);
|
||||
static void do_device(int argc, char *argv[]);
|
||||
|
||||
int
|
||||
config_main(int argc, char *argv[])
|
||||
@@ -72,20 +74,60 @@ config_main(int argc, char *argv[])
|
||||
if (argc >= 2) {
|
||||
if (!strcmp(argv[1], "gyro")) {
|
||||
do_gyro(argc - 2, argv + 2);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "accel")) {
|
||||
} else if (!strcmp(argv[1], "accel")) {
|
||||
do_accel(argc - 2, argv + 2);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "mag")) {
|
||||
} else if (!strcmp(argv[1], "mag")) {
|
||||
do_mag(argc - 2, argv + 2);
|
||||
} else {
|
||||
do_device(argc - 1, argv + 1);
|
||||
}
|
||||
}
|
||||
|
||||
errx(1, "expected a command, try 'gyro', 'accel', 'mag'");
|
||||
}
|
||||
|
||||
static void
|
||||
do_device(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
errx(1, "no device path provided and command provided.");
|
||||
}
|
||||
|
||||
int fd;
|
||||
int ret;
|
||||
|
||||
fd = open(argv[0], 0);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("%s", argv[0]);
|
||||
errx(1, "FATAL: no device found");
|
||||
|
||||
} else {
|
||||
|
||||
if (argc == 2 && !strcmp(argv[1], "block")) {
|
||||
|
||||
/* disable the device publications */
|
||||
ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
|
||||
|
||||
if (ret)
|
||||
errx(ret,"uORB publications could not be blocked");
|
||||
|
||||
} else if (argc == 2 && !strcmp(argv[1], "unblock")) {
|
||||
|
||||
/* enable the device publications */
|
||||
ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
|
||||
|
||||
if (ret)
|
||||
errx(ret,"uORB publications could not be unblocked");
|
||||
|
||||
} else {
|
||||
errx("no valid command: %s", argv[1]);
|
||||
}
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
do_gyro(int argc, char *argv[])
|
||||
{
|
||||
@@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[])
|
||||
if (ret)
|
||||
errx(ret,"range could not be set");
|
||||
|
||||
} else if(argc == 1 && !strcmp(argv[0], "check")) {
|
||||
} else if (argc == 1 && !strcmp(argv[0], "check")) {
|
||||
ret = ioctl(fd, GYROIOCSELFTEST, 0);
|
||||
|
||||
if (ret) {
|
||||
|
||||
Reference in New Issue
Block a user