Implement the new AP_HAL functions and use them in the Scheduler when
possible.
Because the functions are in a namespace, there's no need to do the
define/undef trick and avoid the globals millis() and micros() provided
by libmaple.
Implement the new AP_HAL functions and use them in the Scheduler when
possible.
The '_sketch_start_time' was renamed and moved as a detail of
implementation of the functions code. It allows the code to return time
starting from zero.
The 'stopped_clock_usec' was renamed to follow convention in the file
and add a getter so that AP_HAL functions can reach it. It's not a
problem this getter is public because in practice, regular code
shouldn't even access the Linux::Scheduler directly -- only code that
should is from Linux implementation.
Implement the new AP_HAL functions and use them in the Scheduler when
possible.
The '_sketch_start_time' was renamed and moved as a detail of
implementation of the functions code. It allows the code to return time
starting from zero.
The 'stopped_clock_usec' was renamed to follow convention in the file
and add a getter so that AP_HAL functions can reach it. It's not a
problem this getter is public because in practice, regular code
shouldn't even access the SITLScheduler directly -- only code that
should is from SITL itself.
For certain basic functionality, there aren't much benefit to be able to
vary the implementation easily at runtime. So instead of using virtual
functions, use regular functions that are "resolved" at link time. The
implementation of such functions is provided per board/platform.
Examples of functions that fit this include: getting the current
time (since boot), panic'ing, getting system information, rebooting.
These functions are less likely to benefit from the indirection provided
by virtual interfaces. For more complex hardware access APIs the
indirection makes more sense and ease the testing (when we have it!).
The idea is that instead of calling
hal.scheduler->panic("on the streets of london");
now use
AP_HAL::panic("on the streets of london");
A less important side-effect is that call-site code gets
smaller. Currently the compiler needs to get the hal, get the scheduler
pointer, get the right function pointer in the vtable for that
scheduler. And the call must include an extra parameter ("this"). Now it
will be just a function call, with the address resolved at link time.
This patch introduces the first functions that will be in the namespace,
further patches will implementations for each board and then switch the
call-sites. The extra init() function allow any initial setup needed for
the functions to work.
This revised threshold value is still double the maximum that has been observed in flight logs so far with healthy sensors
The previous value was too slow to switch for sudden IMU gyro faults
We can afford an ocasional false trigger becasue the front end will only select another instance if it is healthy and has lower errors
The ad-hoc scaling of error growth has been replaced with a consistent method that uses the main nav filters published vertical velocity uncertainty and the terrain gradient assumption.
GPS height has been added as a measurement option along with range finder and baro
Selection of the height measurement source has been moved into a separate function
Each height source is assigned its own measurement noise
If GPS or baro alt is not able to be used, it reverts to baro
When baro is not being used, an offset is continually calculated which enables a switch to baro without a height step.
The copter method was being used for plane and the plane method was not being run due to the change in flight status not being detected.
The plane reset method did not trigger if the EKF had already dragged the velocity states along with the GPS or could align to an incorrect heading.
The method has been reworked so that it resets to the GPS course, but only if there are inconsistent angles and large innovations.
To stop a failed magnetometer causing a loss of yaw reference later in flight, if all available sensors have been tried in flight and timed out, then no further magnetoemter data will be used
When there is already a driver registered on an i2c bus, the I2C_SLAVE ioctl
returns an error.
When it happens, it is better to display a warning and try to force the address.
It is especially useful on the bebop when killing the regular autopilot that uses
iio drivers to access the imu because else we would need to manually unbind the
driver in an init procedure.
I have added a warning because this error can also be resulting of another cause.
If the error is not EBUSY, then panic
If the I2C_SLAVE_FORCE ioctl fails then we panic because one of the i2c devices
won't be working properly.
when a GCS sends a command to a system ID that isn't our system ID,
the GCS may use a non-advertised component ID such as
MAV_COMP_ID_SYSTEM_CONTROL. Those packets should be fowarded to the
target system even though the target system has not specifically
advertised that target sysid/compid tuple.
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.
This is the same change as done in PX4:
This reduces self-heating of the sensor which reduces the amount
of altitude change when warming up. Apparently some individual
sensors are severely affected by this.
Unfortunately it raises the noise level, but Paul is confident
it won't be a significant issue.
This is a new method which will return true if an RC_Channel has a PWM
value that is at its TRIM value plus or minus the allowed dead zone
around the TRIM.
Add a macro to annotate functions that act like scanf. Calling the
printf format macro as FORMAT was bad as can be seen now. Later we need
to rename it to FMT_PRINTF.
This include some minor changes on all methods of PWM_Sysfs:
- Sort headers
- Add code inside Linux namespace rather than just use the namespace
- Declare a union pwm_params, that's only used to calculate at compile
time the maximum stack space we need in our methods: this is a bit
safer for future extensions
- Standardize error messages to include the useful params first and
then the error message
- Remove log message from hot path
- Don't abuse macros for checking error - convert the SNPRINTF_CHECK
macro into proper code, ignoring errors for not enough space since
they can't happen
- Fix call to read_file() passing uint8_t for "%u" in get_period()
- Fix passing char** instead of char* to write_file() in set_polarity()
- Use strncmp() instead of strncasecmp() since the kernel API uses
lowercase.
- Add comments on the 2 main methods of this class
Down-sample the IMU and output observer state data to 100Hz for storage in the buffer.
This reduces storage requirements for Copter by 75% or 6KB
It does not affect memory required by plane which already uses short buffers due to its 50Hz execution rate.
This means that the EKF filter operations operate at a maximum rate of 100Hz.
The output observer continues to operate at 400Hz and coning and sculling corrections are applied during the down-sampling so there is no loss of accuracy.
We are currently not using LowPassFilter2p<double> and it just generates
a lot of warnings on PX4 while instantiating it due to implicitly
promoting float to double:
libraries/Filter/LowPassFilter2p.cpp: In instantiation of
'T DigitalBiquadFilter<T>::apply(const T&, const DigitalBiquadFilter<T>::biquad_params&) [with T = double]':
libraries/Filter/LowPassFilter2p.cpp:86:41: required from 'T LowPassFilter2p<T>::apply(const T&) [with T = double]'
libraries/Filter/LowPassFilter2p.cpp:98:16: required from here
libraries/Filter/LowPassFilter2p.cpp:20:82: warning: implicit conversion from 'float' to 'double' to match other
operand of binary expression [-Wdouble-promotion]
T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
^
Large magnetometer innovations on the ground could be caused by factors that will disappear when flying, eg:
a) Bad initial gyro bias
b) External magnetic field disturbances (adjacent metal structures, placement of hatches with magnets, etc)
To avoid unnecessary switches, we inhibit switching until off-ground and when sufficient time has lapsed from power on to learn gyro bias offsets.
If the magnetometer fails innovation consistency checks for too long (currently 10 sec), then the next available sensor approved for yaw measurement will be used.
The original design intent was to require all axes to pass because severe errors are rarely constrained to a single axis.
This was not achieved with the previous implementation.
These changes move the innovation consistency checks for all three axes to the top before any axes are fused.
Unnecessary performance timers have been removed.
This was problematic to implement with magnetometer switching. It is likely that slow magnetometer learning can still be performed externally (eg plane) but this will need to be monitored to see if it causes issues.
instead of computing the terrain status on-demand, assign it in update() and cache the result. Then external tasks that check the status won't be doing terrain intensive calculations in their thread. All the calculations needed for the status were being performed in update already so this is an optimization.
Ensures that the latest GPS data is used to reset the states.
Separates the logic used to set the origin from the logic used to determine when to reset states and commence GPS aiding
The setting of the EKF origin and the entry into GPS aiding mode have been separated to make the logic clear.
The order of operations has been changed to ensure that when a reset to GPS is performed, a valid GPS measurement is available in the buffer
Declaration of GPS availability is not made unless the GPS data has been entered into the buffer
Only applied to interfaces required for data logging.
If an invalid instance is requested, the data for the primary instance is returned. This allows the primary data to be returned by calling with a -1 instance value.
Apply filtering to baro innovation check and and don't apply innovation checks once aiding has commenced because GPS and baro disturbances on the ground and during launch could generate a false positive
Prevents frame over-runs due to simultaneous fusion of measurements on each instance.
The offset is only applied if less than 5msec available between frames
this allows uavcan to be enabled/disabled at boot. When it is disabled
we save about 25k of memory, allowing for more options for things like
multiple EKF
num_errors should be used to detect bad bus transfers, not if we
actually read something. Since we are using i2c_sem->take_nonblocking()
failing here is more likely if the bus is shared.
We pass "sizeof(i2c_integral_frame)" to hal.i2c->readRegisters(). Since
we have a padding in i2c_integral_frame we actually read 3 bytes more
than we should. Add PACKED to the struct so this is fixed.
i2c_frame doesn't have a padding (or hole) so there isn't this problem,
but since it's also used to calculate the frame size, use PACKED there
too.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
Remove the checks for HAL_CPU_CLASS > HAL_CPU_CLASS_16 and
HAL_CPU_CLASS >= HAL_CPU_CLASS_75. Corresponding dead code will be
removed on separate commits.
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.
With commit 24f4153 ("AP_HAL_Linux: RCOutput_PCA9685: group writes") a
log was introduced when we can't get the bus semaphore. However since we
are calling the non blocking method, failing there is not that unlikely
if the bus is shared. Return back to the previous behavior of not
logging.
This removes errors in the in-flight reset of the earth field states by:
1) Using a state vector and magnetometer measurement from the same time coordinate
2) Not using the AHRS trim offsets in the calculation
dtIMUactual has been spit into a separate dtDelAng and dtDelVel and dtDelVel1 and dtDelVel2 delta time in recognition of the amount of timing jitter and different update rates for the IMU's
Vibration in the 400Hz delta angles could cause the angular rate condition check for in-flight magnetic field alignment to fail.
The symptons were failure to start magnetic field learning as expected when EK2_MAG_CAL=3 was set.
The calculation of a delta rotation between consecutive magnetometer samples has been introduced instead of the most recent IMU delta angle as this is less affected by noise and give an upper bound on the angular error.
the check has been moved into the magnetometer fusion control function so that any reset will be performed using fresh magnetometer data
Explicitly set Plane parameters rather than rely on use of the default
If no type defined, default to Copter parameters (most common platform type
Enable different platform types to use different initial accel bias uncertainty
Reduce initial accel bias uncertainty for copter to prevent initial oscillation in bias and height estimate
Large baro data errors when flying without GPS could cause total failure of the EKF.
This patch provides protection against this happening in-flight but allows for large innovations during preflight alignment.
Vibration in the 400Hz delta angles could cause the angular rate condition check for in-flight magnetic field alignment to fail.
The symptons were failure to start magnetic field learning as expected when EK2_MAG_CAL=3 was set.
Use the more robust, but less accurate compass heading fusion up to 5m altitude
Wait for the magnetometer data fusion time offset to be correct before using data to reset states
Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors
When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset.
Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m
Always re-do the yaw and mag reset when leaving the ground.
Not being able to leave the instantiation in ther header is not because
of PSTR issues, but basically because the instantiation needs to be in a
compilation unit, not in the header itself.
The only thing from AP_Progmem that's still used are the pgm_read_*
function and there's no support for AVR anymore. So remove the dead code
and use a single header to contain that inline functions.
"%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);
^
"%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);
^
"%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);
^
"%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.
This allows turning on/off desired velocity feedforward without setting desired_vel.z to zero. Setting desired_vel.z to zero has the side effect of disrupting the landing detection which needs to know if we are trying to descend
This increase in assumed altimeter noise and reduction in accel noise has been flight tested by L.Hall with noticeable reduction in the immediate response to Baro errors during moving flight.
This increases the time constant of response to baro errors such that the pilot can more easily compensate.
The previous gain from rate to magnetometer error was excessive. The revised value is equivalent to a magnetic field length of 0.5 with a timing uncertainty of 0.01 sec
Follow the following order for includes:
- Corresponding header file (if exists)
- System headers
- Other ArduPilot library headers
- "Local" headers (from the same library)
Testing on different platforms has shown that the new EKF has smaller innovations enabling innovation consistency checks that reject GPS and baro errors to be tightened.
The position and velocity thresholds for plane have been left the same because planes are less sensitive to GPS glitches as they fly higher and with more separation to surrounding objects. They are also more prone to bad inertial data due to the installation practices.
The altitude noise has been increased on plane to allow for the larger baro disturbances that result from the higher speeds and lack of a proper static pressure source. The innovation consistency gate has been adjusted to provide the same baro error limit of ~20m before baro is rejected.
Extended GPS loss can result in the earth field states becoming rotated and making it difficult for the EKF to recover its heading when GPS is regained.
During prolonged GPS outages, the position covariance can become large enough to cause the reset function to continually activate. This is fixed by ensuring that position covariances are always reset when the position is reset.
The innovation variance was being used incorrectly instead of the state variance to trigger the glitch reset.
Because we have changed the yaw angle and have taken a point sample on the magnetic field, covariances associated with the magnetic field states will be invalid and subsequent innovations could cause an unwanted disturbance in roll and pitch.
The reset of the Euler angles to a new yaw orientation was being done using roll and pitch from the output observer states, not the EKF state vector which meant that when roll and pitch were changing, the reset to a new yaw angle would also cause a roll and pitch disturbance.
This was introduced with the HAL rework:
In file included from /p/ardupilot/libraries/AP_HAL/AP_HAL.h:11:0,
from /p/ardupilot/ArduCopter/Copter.h:35,
from /p/ardupilot/ArduCopter/ArduCopter.cpp:76:
/p/ardupilot/ArduCopter/ArduCopter.cpp: In function 'int ArduPilot_main(int, char* const*)':
/p/ardupilot/libraries/AP_HAL/AP_HAL_Main.h:11:26: warning: no previous declaration for 'int ArduPilot_main(int, char* const*)' [-Wmissing-declarations]
#define AP_MAIN __EXPORT ArduPilot_main
^
It's due to PX4 using that warning as opposed to Linux. Since it harmless, add
the prototype for everybody.
ardupilot/libraries/AP_HAL_Linux/Storage_FRAM.cpp: In member
function 'int32_t Linux::Storage_FRAM::read(uint16_t, uint8_t*, uint16_t)':
/home/lucas/p/dronecode/ardupilot/libraries/AP_HAL_Linux/Storage_FRAM.cpp:183:24: war
ning: comparison is always false due to limited range of data type [-Wtype-limits]
if(Buff[i-fptr]==-1){
^
For the general case, pragma once is better replacement for of include
guards. One line instead of three, less scopes to close in the end of
the file, no chance to having the outdated names in the define symbol.
Remove unnecessary includes, reorder them in blocks separated by a blank
line
- Corresponding header file (if exists)
- System headers
- Other ArduPilot library headers
- "Local" headers (from the same library)
Now that SITL is compiled only when it's needed (i.e. using the SITL
board), there's no need to ifdef its files based on the
CONFIG_HAL_BOARD. So remove them.
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.
Caio Marcelo de Oliveira Filho
Jonathan Challinger
Paul Riseborough
Randy Mackay
Andrew Tridgell
Michael Oborne
Julien BERAUD
José Roberto de Souza
mirkix
Robert Lefebvre
Fredrik Hedberg
Jolyon Saunders
Peter Barker
Lucas De Marchi
Grant Morphett
Siddharth Bharat Purohit
lvale
Tom Pittenger
Víctor Mayoral Vilches
Víctor Mayoral Vilches
Gustavo Jose de Sousa
Leonard Hall
squilter