The new function can deal with a variable number of function parameters.
Additionally, I renamed the functions to norm(), because this is the
standard name used in several other projects.
Several coding style problems were already fixed by previous commit,
just finish the cleanup on this example:
- replace tabs with spaces
- remove unneeded \r
- remove extra spaces
We should return the number of bytes written/read, not 0 on success.
This number may be useful in some cases so return it.
While at it fix a simple wrong space in the header.
Due to the way the headers are organized changing a single change in an
inertial sensor driver would trigger a rebuild for most of the files in
the project. Time could be saved by using ccache (since most of the
things didn't change) but we can do better, i.e. re-organize the headers
so we don't have to re-build everything.
With this patch only AP_InertialSensor/AP_InertialSensor.h is exposed to
most users. There are some corner cases to integrate with some example
code, but most of the places now depend only on this header and this
header doesn't depend on the specific backends.
Now changing a single header, e.g. AP_InertialSensor_L3G4200D.h triggers
a rebuild only of these files:
$ waf copter
'copter' finished successfully (0.000s)
Waf: Entering directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
[ 80/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor.cpp
[ 84/370] Compiling libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
[310/370] Linking build/minlure/ArduCopter/libArduCopter_libs.a
[370/370] Linking build/minlure/bin/arducopter
Waf: Leaving directory `/home/lucas/p/dronecode/ardupilot/build/minlure'
This is not used by any board and has a lot of commented out code. For
example, the compass is not enabled. The comment in the beginning of
the driver says it should serve as an example, but we should rather use
a working driver as an example. If this was at least a bit simpler and
that worked in the past we could refactor it to the new I2CDevice API.
This is not the case.
new functions that get a filtered min/max accel peaks on each axis with fixed 500ms timeout:
Vector3f get_accel_peak_hold_pos()
Vector3f get_accel_peak_hold_neg()
This allows slower mechanisms, such as is_flying, to detect accel spikes which would indicate ground or object impacts. Vibe is too filtered. Independent positive and negative peaks are available
The problem with using min() and max() is that they conflict with some
C++ headers. Name the macros in uppercase instead. We may go case by
case later converting them to be typesafe.
Changes generated with:
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)max(/\1MAX(/g'
git ls-files '*.cpp' '*.h' -z | xargs -0 sed -i 's/\([^_[:alnum:]]\)min(/\1MIN(/g'
As commented in 8218140 ("AP_Common: add scanf format macro"), "FORMAT"
was a bad name for this macro since there's also the scanf. Rename to
FMT_PRINTF to follow the scanf name.
This method will be used to initialize and configure I2C backends that
have an auxiliary I2C bus that can be connected to the main I2C bus,
like MPU6000 and MPU9250.
Using MPU9250 over I2C we can connect the auxiliary bus where there is
a AK8963 and connect this bus to the main one, this way we don't need
any AuxiliaryBus infrastructure as we need with SPI and we can talk
with AK8963 as we would talk with a standalone AK8963.
Data-ready pin wasn't being used before due to a bug in the Kernel with
concurrent accesses to GPIO in Intel Baytrail platforms. That has been fixed in
Kernel version 4.2.
"%S" is used for wide string, but we are passing a char*. Use lowercase
in this case to remove warnings like this:
libraries/AP_InertialSensor/AP_InertialSensor.cpp: In member function
'bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract*, float&, float&)':
libraries/AP_InertialSensor/AP_InertialSensor.cpp:620:61: warning:
format '%S' expects argument of type 'wchar_t*', but argument 3 has type 'const char*' [-Wformat=]
"Place vehicle %S and press any key.\n", msg);
^
Most of AP_Progmem is already gone so we can stop including it in most
of the places. The only places that need it are the ones using
pgm_read_*() APIs.
In some cases the header needed to be added in the .cpp since it was
removed from the .h to reduce scope. In those cases the headers were
also reordered.
prog_char and prog_char_t are now the same as char on supported
platforms. So, just change all places that use them and prefer char
instead.
AVR-specific places were not changed.
Now variables don't have to be declared with PROGMEM anymore, so remove
them. This was automated with:
git grep -l -z PROGMEM | xargs -0 sed -i 's/ PROGMEM / /g'
git grep -l -z PROGMEM | xargs -0 sed -i 's/PROGMEM//g'
The 2 commands were done so we don't leave behind spurious spaces.
AVR-specific places were not changed.
The PSTR is already define as a NOP for all supported platforms. It's
only needed for AVR so here we remove all the uses throughout the
codebase.
This was automated with a simple python script so it also converts
places which spans to multiple lines, removing the matching parentheses.
AVR-specific places were not changed.
In order to avoid confusion between sample rate from sensor and sample rate
from the frontend class (AP_InertialSensor), use "raw sample rate" to refer to
the former.
The changes in the code were basically done with the following commands:
git grep -wl _accel_sample_rates | xargs sed -i "s,\<_accel_sample_rates\>,_accel_raw_sample_rates,g"
git grep -wl _set_accel_sample_rate | xargs sed -i "s,\<_set_accel_sample_rate\>,_set_accel_raw_sample_rate,g"
git grep -wl _accel_sample_rate | xargs sed -i "s,\<_accel_sample_rate\>,_accel_raw_sample_rate,g"
git grep -wl _gyro_sample_rates | xargs sed -i "s,\<_gyro_sample_rates\>,_gyro_raw_sample_rates,g"
git grep -wl _set_gyro_sample_rate | xargs sed -i "s,\<_set_gyro_sample_rate\>,_set_gyro_raw_sample_rate,g"
git grep -wl _gyro_sample_rate | xargs sed -i "s,\<_gyro_sample_rate\>,_gyro_raw_sample_rate,g"
And also with minor changes on indentation and comments.
Delta angle calculation is now unified, so there is no need for such a method.
That also avoids developers thinking they need that method being called
somewhere in their new drivers.
This commit basically moves delta angle calculation that was previously done in
AP_InertialSensor_PX4 to a common place. Instances must publish their gyro raw
sample rate to enable delta angle calculation.
The delta velocity calculation is now unified, so there is no need for such a
method. That also avoids delevopers thinking they need that method being called
somewhere in their new drivers.
This commit basically moves delta velocity calculation that was previously done
in AP_InertialSensor_PX4 to a common place. Instances must publish their accel
raw sample rate to enable delta velocity calculation.
We don't support HAL_CPU_CLASS <= HAL_CPU_CLASS_16 anymore. This makes
INS_MAX_INSTANCES, INS_MAX_BACKENDS and INS_VIBRATION_CHECK constant for
all supported boards.
The code with ifdef for !FAST_SAMPLING is bit rotting and the example
for AP_InertialSensor is currently broken for this case. Instead of
adding more ifdefs, remove the legacy implementation for !FAST_SAMPLING
since we don't support it anymore.
Reported by Grant:
/home/grant/3dr/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp:
In member function 'void AP_InertialSensor_MPU6000::_accumulate(uint8_t*,
uint8_t)':
/home/grant/3dr/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp:776:20:
error: no match for 'operator+=' (operand types are 'Vector3l {aka
Vector3<long int>}' and 'Vector3f {aka Vector3<float>}')
_accel_sum += accel;
/home/grant/3dr/ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp:777:19:
error: no match for 'operator+=' (operand types are 'Vector3l {aka
Vector3<long int>}' and 'Vector3f {aka Vector3<float>}')
_gyro_sum += gyro;
Instead of requiring every program to specify the HAL related modules,
let the build system do it (in practice everything we compiled depended
on HAL anyway). This allow including only the necessary files in the
compilation.
The switching between different AP_HAL was happening by giving different
definitions of AP_HAL_BOARD_DRIVER, and the programs would use it to
instantiate.
A program or library code would have to explicitly include (and depend)
on the concrete implementation of the HAL, even when using it only via
interface.
The proposed change move this dependency to be link time. There is a
AP_HAL::get_HAL() function that is used by the client code. Each
implementation of HAL provides its own definition of this function,
returning the appropriate concrete instance.
Since this replaces the job of AP_HAL_BOARD_DRIVER, the definition was
removed.
The static variables for PX4 and VRBRAIN were named differently to avoid
shadowing the extern symbol 'hal'.
Read temperature as part of the normal burst. This is not very costly since it
is part of the burst read in i2c and already read in spi.
It is meant to be used for imu heating.
The filter is set to 1Hz on temperature because of the inherent inertia of
heating systems.
In 294298e ("AP_InertialSensor: use method for downcast") I was too eager
to use "auto" and ended up using the implicit copy constructor instead
of actually getting a reference to the object.
Instead of just doing a static cast to the desired class, use a method
named "from". Pros:
- When we have data shared on the parent class, the code is cleaner in
child class when it needs to access this data. Almost all the data
we use in AP_HAL benefits from this
- There's a minimal type checking because now we are using a method
that can only receive the type of the parent class
This is a good way of letting each implementation easily calculate vibration
and clipping: all they need to do is publish their sample rate and they don't
need to worry about the call for calculation.
These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.
Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.
Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.
Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.
Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.
Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.
Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.
Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
In order to allow other libraries to use the InertialSensor we need a
way to let them to get the only instance of InertialSensor. The
conventional way to do a singleton would be to let the constructor
private and force it to be instantiated from the get_instance() method.
Here however we just call panic() on the constructor if there's already
an instance alive. This allows us to let the vehicles as is. Later we
can change it so they call the get_instance() method instead.
Add an AuxiliaryBus class that can be derived for specific
implementations in inertial sensor backends. It's an abstract
implementation so other libraries can use the auxiliary bus exported. In
order for this to succeed the backend implementation must split the
initialization of the sensor from the actual sample collecting, like is
done in MPU6000.
When AP_InertialSensor::get_auxiliary_bus() is called it will execute
following steps:
a) Force the backends to be detected if it's the first time it's
being called
b) Find the backend identified by the id
c) call get_auxiliary_bus() on the backend so other libraries can
that AuxiliaryBus to initialize a slave device
Slave devices can be used by calling AuxiliaryBus::request_next_slave()
and are owned by the caller until AuxiliaryBus::register_periodic_read()
is called. From that time on the AuxiliaryBus object takes its ownership.
This way it's possible to do the necessary cleanup later without
introducing refcounts, that we don't have support to.
Between these 2 functions the caller can configure the slave device by
doing its specific initializations by calling the passthrough_*
functions. After the initial configuration and register_periodic_read()
is called only read() can be called.
Identify backend with an id, allowing other libraries to connect to
them. This is different from the _product_id member because it
identifies the sensor, not the board the sensor is in, which is
meaningless for our use case.
This allows backends to have a separate detection and initialization
logic. It doesn't change any backend yet and with the current code
there's no change in behavior either. This only allows
AP_InertialSensor::_detect_backend() to be called earlier so
AP_InertialSensor object can be used by other libraries. If it's not
called, later on AP_InertialSensor::init() will detect and start all
backends.
We were able to read only the block of registers that are part of the
data output from accelerometer/gyroscope. In order to support reading
the external sensors we need support for reading a generic block of
registers.
very strict check that all axis are not vibrating much at all
new param: INS_STILL_THRESH used to be a vibration threshold for different platforms
// @Description: Threshold to tolerate vibration to determine if vehicle is motionless. This depends on the frame type and if there is a constant vibration due to motors before launch or after landing. Total motionless is about 0.05. Suggested values: Planes/rover use 0.1, multirotors use 1, tradHeli uses 5
This commit changes the way libraries headers are included in source files:
- If the header is in the same directory the source belongs to, so the
notation '#include ""' is used with the path relative to the directory
containing the source.
- If the header is outside the directory containing the source, then we use
the notation '#include <>' with the path relative to libraries folder.
Some of the advantages of such approach:
- Only one search path for libraries headers.
- OSs like Windows may have a better lookup time.
We were previously leaking the AP_MPU6000_BusDriver if the
~AP_InertialSensor_MPU6000::detect*() failed. In order to avoid the
leak move the repeated code in a single private _detect() member that
receives everything as argument. Then this method takes ownership of the
objects.
By a adding a destructor to AP_InertialSensor_MPU6000 it becomes easier to
free the objects it takes ownership of.
Different detect() function might need different arguments and passing a
pointer to function here is cumbersome. For example, it forces to have a
method like "detect_i2c2" rather than allowing hal.i2c2 to be passed as
parameter.
The methods actually use the enum from AP_HAL::SPIDeviceDriver, so don't
declare a new one. The I2C implementation is empty; if we actually start
to use it we'd better move the bus abstraction to HAL.
Now that the initialization of MPU9250 is shared between the
AP_InertialSensor and other drivers using it as a backend, we can reset
the MPU9250 in order to put it in a known state.
Now we have the initialization code split in 2 parts:
1) Making sure the MPU9250 chip is alive and working: this is now in a
static function that may be called by other drivers that use MPU9250 as
backend.
2) The configuration of gyro and accel. Once the first part is completed
successfully the AP_InertialSensor_MPU9250 finishes the configuration of
the sensors it uses.
The only change in behavior here is that before we would try 25 time (5x
inside _hardware_init time 5x inside _init_sensor() that calls the first
function) to "boot the chip" and now we are doing "only" 5.
Add static methods to do the SPI transactions and provide the wrapper
methods when we have an instance of the object. This is useful so these
methods can be called from other contexts when the AP_InertialSensor
hasn't been initialized yet.
The Compass library is initialized before the InertialSensor. AK8963 with
MPU9250 as backend already takes care of resetting MPU9250. The problem with
also resetting it in the MPU9250 initialization code is that if the reset
happens during an internal I2C transaction, the AK8963 may hang. So here we
remove the reset inside MPU9250. There still a possibility that the first
MPU9250 initialization is not successful and it resets the chip, but it's not
happening in tests.
As we intend to eventually get board related parameters from a configuration
file, this commit makes the GPIO numbers for data-ready pins be instance
variables instead of from C constant macros.
Another advantage of using instance variables in this context is the
possibility of using more than one LSM9DS0.
If the data-ready polling is done entirely on GPIO pins, it isn't necessary to
hold the semaphore before we now we have data to consume. In that case, only
take the SPI semaphore if there's new data available.
On the other hand, if at least one SPI transaction is done in order to check
for new data, then it makes sense to take the semaphore beforehand.
This commit makes accel and gyro initialization routines use bitfield macros
instead of hardcoding the literal value when wrinting on registers. That is
less prone to typos and a lot of times self-explanatory. Also, due to the
latter, the long comments explaining each register field were removed (any
detail can be checked on the datasheet).
This adds the backend driver for LSM9DS0. This implementation is based on the
legacy driver coded by Víctor Mayoral Vilches (under folder LSM9DS0) and makes
some necessary adaptations and fixes in order to work properly. The legacy
driver folder was removed.
The calibration on LSM9DS0 was giving offsets between 4.0 and 4.2 on x-axis and
around 3.6 on y-axis. It turned out that those offsets were actually right.
The maximum absolute values of calibration offset should be a sensor
characteristic rather than a constant value for all sensors.
The constant value previously used (3.5 m/s/s for all axes) is set here as a
default maximum absolute calibration offset for every instance to keep it
working.
The previous implementation made some boards apply two rotations to suit
their default orientation. That was happening because there was an
unconditional rotation being done (commented as "rotate for bbone
default").
This commit makes that unconditional rotation as a default rotation
instead and adjusts the former additional rotations to be single
rotations.
As the datasheet says: "To prevent switching into I2C mode when using
SPI, the I2C interface should be disabled by setting the I2C_IF_DIS
configuration bit."
We also reset the sensor like PX4Firmware does for initializing the
MPU6000. See: ee1d8cd770/src/drivers/mpu6000/mpu6000.cpp (L695)
The main thread would always be blocked on the semaphore to read the
data from accelerometer and gyroscope. Especially if we have a slow
update of these values in _accumulate() due to the I2C transfer function
taking too much time: the timer thread would never give up the CPU,
causing starvation on the main thread.
This fixes the issue by reducing the critical region using a flip-buffer
so _accumulate() can work on its own copy of the data. Now that the
critical region is smaller, also avoid the semaphore and use a spinlock
instead.
Gustavo Jose de Sousa
Grant Morphett
Lucas De Marchi
Ricardo de Almeida Gonzaga
dgrat
Andrew Tridgell
mirkix
AndersonRayner
ashwinvasudevan
raspilot
Staroselskii Georgii
Jonathan Challinger
Randy Mackay
Víctor Mayoral Vilches
Tom Pittenger
Don Gagne
Aaron Wang Shi
Julien BERAUD
Caio Marcelo de Oliveira Filho
José Roberto de Souza
Víctor Mayoral Vilches
squilter
ziltoid2
Peter Barker
Peter Barker