- reorder and document members.
- remove tentative of vertical alignement
- like was done for accumulated values, move the calibration
values to a struct
This converts MS56XX to use the thread started by SPI/I2C instead of
using the timer thread. This also fixes a possible starvation of the
main thread:
1) INS driver registers itself to be sampled on timer thread
2) MS56XX registers itself to be sampled on timer thread
3) Main thread waits for a sample from INS with
ins.wait_for_sample()
4) timer thread is waiting on update from MS56XX and consequently
the main thread is waiting on an I2C/SPI transfer
Besides this starvation there's another one due to reuse of the timer
lock in order to pump values from the timer thread to the main thread. A
call to the update() method when we have a sample available would need
to wait on any other driver holding the timer lock.
Now there's a lock just to pass the new values from the bus thread to
the main thread with a very tiny critical region, not waiting on any
bus transfers and/or syscalls.
We aren't going to use all the poller infra for now and we need it's
behavior a little bit different for what we are going to use:
- Do not use any "fair" time for each ready fd since we don't want
to set a timeout
- Allow to set the fd on Pollable after constructing it since we are
likely to embed Pollable inside other structs and just later be
able to open an fd.
- Let caller use the epoll flags directly - this is not in AP_HAL,
so there's no need to abstract them
Add system's polling infrastructure to be notified whenever a
file descriptor is ready to be read from or written to.
Adds a few classes:
* Poller, as an interface to epoll()
* Pollable, as an interface to a file descriptor
Replace the previous not-implemented interface with a set of new methods
that can be resonably implemented:
- register_periodic_callback() now receives a functor returning bool
to easily allow "oneshot" timers
- adjust_periodic_callback() allows the caller to change the timer
for a specific handle. This way drivers like MS5611 can adjust the
timer depending on its state machine: the time to sample
temperature is smaller than the time to get a pressure sample
- add unregister_callback(): since we have an opaque pointer, we
can't tell the user to just delete it in order to unregister the
callback
../../libraries/AP_HAL_Linux/SPIDevice.cpp: In member function ‘virtual AP_HAL::OwnPtr<AP_HAL::SPIDevice> Linux::SPIDeviceManager::get_device(const char*)’:
../../libraries/AP_HAL_Linux/SPIDevice.cpp:337:27: warning: comparison is always false due to limited range of data type [-Wtype-limits]
for (uint8_t i = 0; i < _n_device_desc; i++) {
~~^~~~~~~~~~~~~~~~
Define a dummy device to remove warning.
This centralized private header encourages centralizing things on
umbrella headers that are a pain to maintain. Force each part of
AP_HAL_Linux to include what is used.
Make some member variables protected to follow what we do in other
places (and there's no reason to be private).
Move defines to .cpp to reduce their visibility.
This centralized namespace header encourages centralizing things on
umbrella headers that are a pain to maintain. Force each part of
AP_HAL_Linux to include what is used.
While at it, do some whitespace cleanups and minor changes to adhere to
coding style.
This is the only user of ADS7844 - we don't have it actually used in our
boards. Remove the example since we can later add a more generic one or
at least one that reuses a driver from our boards.
This method is needed when we want to transfer both tx and rx at the
same time, as opposed to common cases for sensors in which they are like
in the I2C interface: half-duplex.
This is selectable by a define and is never changed. Just remove
everything referencing it: we can come up with a better solution if it
is actually used later.
There's little point in having the Linux::AnalogIn just to implement and
empty interface. All implementations inside AP_HAL_Linux are already
inheriting directly from AP_HAL, so just remove it.
In a case ArduPilot is launched as a background process without
detaching with *nohup* like this ./arduplane -C /dev/ttyAMA0 ConsoleDevice
is created and an attempt to read from it is made. This yields in a stopped
process. This is an endeavour to overcome this problem.
This should fix CID 91386.
Before removing the 'if', I checked the log to confirm that both branch
didn't end-up being equal by mistake in some commit. But it looks like
the file was added in the project this way.
We should actually modify this function to scale back the z axis in order to avoid breaching the vertical fence. Currently breaching the vertical fence is handled within the position controller which is inconsistent.
- Most of the boards use bus 1 for first I2C
- If the bus doesn't exist, let the detect() method fail when it
call start_reading(), because _dev would be invalid
- Use "INS_" prefix for the name in order to limit the scope for that macro.
- Don't define it in the code and check if it is defined instead of checking
the value. With that, there's no need to touch the code for enabling debug,
only a reconfiguration is necessary (e.g., `CXXFLAGS='-DINS_TIMING_DEBUG' waf
configure ...`).
When building for px4-v2 we have an warning because we are checking for
the value of this undefined macro. Just change both checks to use
"defined()".
../../libraries/AP_HAL_PX4/Util.cpp:115:7: warning: "CONFIG_ARCH_BOARD_PX4FMU_V4" is not defined [-Wundef]
#elif CONFIG_ARCH_BOARD_PX4FMU_V4
^
when this is set the board RGB LED will be controlled by MAVLink
instead of internally. This is useful for cases where the LED patterns
and colours needed are specified by an external authority (such as the
OBC organisers)
When switching over to a back up magnetometer, ensure that the earth field estimate are reset. other wise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected.
This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset.
If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding.
This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed.
An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS.
Switching in and out of aiding modes was being performed in more than one place and was using two variables.
The reversion out of GPS mode due to prolonged loss of GPS was not working.
This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function.
The filter status logic calculations were being repeated every time the get function was called.
The logic is now updated once per filter update step and a separate get function added
Split publishing of local position into horiz and vert components with separate validity checks
Split status reporting into separate update and get functions and only update once after each loop update. This removes unnecessary re-calculation of the status logic and ensures all consumers get the same data (avoid possible race conditions).
If we start GPS aiding before the gyro bias variances have reduced, glitches on the GPS can cause attitude disturbances that degrade flight accuracy during early flight.
The minimum version for gcc was supposed to be 4.9 for any platform.
However our build instructions are outdated. Remove the problematic
parts that use the sparse-endian.h header while we don't fix the setup
for windows.
When building for px4-v2 we have an warning because we are checking for
the value of this undefined macro. Just change both checks to use
"defined()".
../../libraries/AP_BoardConfig/AP_BoardConfig.cpp:36:7: warning: "CONFIG_ARCH_BOARD_PX4FMU_V4" is not defined [-Wundef]
#elif CONFIG_ARCH_BOARD_PX4FMU_V4
^
during the first part of a takeoff when we have not yet reached the
target airspeed this forces the throttle to maximum. This fixes a case
where the throttle may drop too low during the first part of takeoff
and lead to a stall.
The reason of defining BMI160_MAX_FIFO_SAMPLES as 8 can be found on the
following histogram of the number of samples in the FIFO on each read while
performing the accelerometer calibration process:
Samples Count Freq Acc. Freq
------------------------------
1 3842 0.1201 0.120111
2 13172 0.4118 0.531904
3 9065 0.2834 0.815300
4 2710 0.0847 0.900022
5 2231 0.0697 0.969769
6 816 0.0255 0.995279
7 137 0.0043 0.999562
8 13 0.0004 0.999969
13 1 0.0000 1.000000
Using factory method maked it easier to grasp the lifetime of all object
that get created and destroyed. Instead of spanning this thoughout whole
source file we have it nicely encapsulated in this a little horrendeous
_parseDevicePath that is of course to improve more.
Otherwise we're going to leak memory without any need.
Before this fix we've created ConsoleDevice 4 times in case -A switch hadn't been supplied,
but we hadn't ever deleted those. Now there's no memory leak here.
Minor changes to follow coding style and improve readability:
- sort headers
- move struct definition to compilation unit rather than header
- Add braces to if, for, etc
- Initialize device on hw_init() method, allowing it not to be
present
- Add missing lock
- Add packed attribute to structs
- Move defines to source file
- Add missing semaphore take on bus
- Initialize device on init function rather than constructor: the
constructor may run before I2CDeviceManager is initialized since our
AP_Notify objects are static so it can't be used.
In _start_conversion(), the check for return code of _dev->transfer() was
inverted. The structure also needs to be PACKED, otherwise there will be
a hole in the middle. Fix these issues and use be16_t where it makes
sense.
In read() we need to check for the second byte of config register, so
either make it an array of uint8_t or convert from big endian to host
endianness. It's simpler to leave it as it was, accessing just the
first byte. Also the conversion value is in be16 type an needs to be
converted to host endiannes, not the opposite.
Fix bus number: all boards that use it expect it to be on bus 1, not 0.
this allows for external modules to be called at defined hook
locations in ArduPilot. The initial example is a module that consumes
the AHRS state, but this can be generalised to a wide variety of hooks