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.
Now that we are using a consistent 50Hz minimum update rate for the covariance prediction we do not need a different initial gyro bias uncertainty for plane and copter to maintain filter stability margins.
The default value of 0.1 rad/s was too high and gave excessive settling time of the filter attitude after startup.
The initial attitude uncertainty has been increased to allow for some movement during startup.
This commit adds the class Linux::GPIO_Sysfs. This class provides a generic
implementation of AP_HAL::GPIO on Linux by using GPIO Sysfs Interface
(https://www.kernel.org/doc/Documentation/gpio/sysfs.txt).
The channel() interface should be preferred in places that need to be
fast. Since it maintains the file descriptor open this is much faster
than opening and closing it.
We are using a microcontroller to read the PWM input from RC. The read
values are sent to our board using a simple serial protocol through the
UART interface.
This patch interprets these values and passes them forward to the APM.
If the baro data and magnetometer data are interleaved (arriving every 100 msec and offset by 50 msec), then the filter will go unstable during startup and fail to complete checks.
Since we are using uint8_t and uint16_t types we need to include the
correspondent system header. Otherwise it would depend on the include
order of who is including this particular header, causing failures as we
move headers around.
This sets the fusion of the synthetic position and velocity to occur at the same time as the barometer
This makes filter tuning more consistent between GPS and non-GPS useage
High measurement data rates can fill buffers with data that is always new and never fused because it is over-written before it falls behind the measurement time horizon.
new param: NAVL1_XTRACK_I
// @Description: Crosstrack error integrator gain. This gain is applied to the crosstrack error to ensure it converges to zero. Set to zero to disable. Smaller values converge slower, higher values will cause crosstrack error oscillation.
fixes https://github.com/diydrones/ardupilot/issues/2650
when param is changed the integrator is set to zero. This makes for easier tuning by seeing it converge to zero on each change.
This option now is passed when instantiating the code in ArduCopter, so
selecting the default value at compile time is not necessary anymore.
The motivation is to move vehicle specifc code out of the general
libraries. This patch shouldn't change behavior.
The AHRS_EKF_USE_ALWAYS define is used to force EKF to be always
used. It is defined only for building ArduCopter. Change it to be a
runtime flag. Keep its default value still based on the original define,
once the Copter uses it the define will be removed.
The motivation is to move vehicle specifc code out of the general
libraries. This patch shouldn't change behavior.
For all supported boards the maximum number of instances is 3. The
number of HIL_COMPASSES was already defined as 2 instead of 3, so this
is left as before.
We don't support HAL_CPU_CLASS <= HAL_CPU_CLASS_16 anymore. This makes
COMPASS_MAX_INSTANCES and COMPASS_MAX_BACKEND constant for all supported
boards.
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;
Add libraries that those boards depend on. This should be handled in the
future elsewhere (and once for each board), but for now let's make it
compile again.
Remove unnecessary includes, in particular the includes for specific
boards. The list of libraries for 'polygon' example was updated so that
the example compiles again.
This is going to be used by vehicles that already have an object with
setup/loop functions. The vehicle object will just implement the
HAL::Callbacks interface.
Move the macros to a single place and reduce the variations not based on
board, but based on
- The name of the entry-point function, specified by AP_MAIN;
- Whether it contains argc/argv arguments or not.
The goal here is that programs (vehicles and examples) don't need to
include all possible boards to define a main function. Further patches
will change the programs.
Add run method, that encapsulate any mainloop logic on behalf of the
client code. The setup/loop functions are passed via a HAL::Callbacks
interface. The AP_HAL_MAIN() macro should be kept as trivial as
possible.
This interface should be implemented by the existing vehicle objects. To
make easy for the examples (that don't have the equivalent of vehicle
objects), a FunCallbacks was added to bridge to the functions directly.
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'.
The legacy EKF switches GPs aiding on on arming, whereas the new EKF switches it on based on GPS data quality.
This means the decision to arm and therefore the predicted solution flags must now reflect the actual status of the navigation solution as it will no longer change when motor arming occurs.
If high vibration levels cause offsets between the two, it switches to the accelerometer with lower vibration levels. The default behaviour is to use the average of both accelerometers.
Don't fuse other measurements on the same frame that magnetometer measurements arrive if running at a high frame rate as there will be insufficient time to complete other operations.
This parameter is a compromise between numerical accuracy of the covariance prediction and sensor timing jitter
Further testing has shown that doing covariance prediction and sensor fusion every 10msec has no observable effect on fusion health and reduces timing hitter noise on magnetometer observations during high rate maneovures
The values chosen ensure that up to consistent 250 msec of sensor delay compensation is available for different platform types
The revised values also ensure that fusion occurs at different time to when the 10Hz magnetometer measurements are read
Adds fusion of the declination when there are no earth relative measurements so that the declination angle and therefore the copters yaw angle have an absolute reference.
This enables the length (but not the declination) of the earth field North/East states to change along with the magnetometer offsets.
Provide an option to always do learning
Make field learning decision logic clearer
Change defaults so that plane learns when airborne
Change defaults so that Rover does not learn (large external magnetic interference)
Magnetic interference whilst on the ground can adversely affect filter states. This patch ensures that the simpler and more robust magnetic heading observation method is used until the vehicle has cleared the ground.
Fixes code that didn't take into account fall-through behaviour of C++ switch statements
Makes get_rigin furnction more generic allowing the consumer to decide what to do with an invalid origin
If an external gyro calibration has been performed, we should assume that it has been done under static conditions
Otherwise it is pointless and we should allow the EKF to find its own gyro bias offsets.
This patch adds additional methods to the the AHRS library so that the AP_InertialNav library dow nto have to access the EKF directly. This enables Copter to fly using the EKF nominated by AHRS_EKF_TYPE.
It will also pave the way to elimination of the AP_InertialNav library.
This warning happens because of the difference of datatypes between
32 and 64 bits processors.
%% libraries/AP_HAL_Linux/RCInput_UDP.o
/home/zehortigoza/dev/ardupilot/libraries/AP_HAL_Linux/RCInput_UDP.cpp: In member function 'virtual void Linux::LinuxRCInput_UDP::_timer_tick()':
/home/zehortigoza/dev/ardupilot/libraries/AP_HAL_Linux/RCInput_UDP.cpp:42:72: warning: format '%llu' expects argument of type 'long long unsigned int', but argument 3 has type 'uint64_t {aka long unsigned int}' [-Wformat=]
hal.console->printf("no rc cmds received for %llu\n", delay);
As AVR2560 is not supported anymore and do integer operations is
usually faster than float-point the _calculate() implementation was
done using only integer operations and as more close to what
datasheet says.
offset parameter units are milligauss
User settable parameters should have a User category defined. Those that should never be set by a user should not have this.
The function rotate_field() can change the values axes and the function
correct_field() applies offsets (which are already in milligauss). Thus any
sensitivity adjustment must be done for two reasons:
(1) The offsets must be applied to the values already in milligauss;
(2) The factory sensitivity adjustment values are per axis, if any rotation
that switches axes is applied, that'll mess with the adjustment.
Experiments showed that before this patch the length of the mag field reported
quite different from the expected. After this patch, the same experiments
showed reasonable values.
This is part of the transition to make all mag field values be used in
milligauss. Additionally the value of _gain_multiple is adapted to the new way
we're using it and corrected accordingly to the datasheets.
The use of _gain_multiple is not necessary because the values of
expected_{x,yz} and _mag_{x,y,z} are both in sensor raw unit (i.e., lsbs).
That wasn't fixed before in order not to make APM users to recalibrate their
compasses.
The failure to initialise the magnetometer bias states to zero can result in a large jump in yaw gyro bias and heading when a heading reset is performed.
Fixes a potential error where changes to timing and arrival rate of magnetometer and baro data could block the fusion of synthetic position and velocity measurements, allowing unrestrained tilt errors during operation without GPS or optical flow.
Fusion of synthetic position or velocity measurements is now timed to coincide with fusion of barometer observations.
If a new barometer observation has not arrived after 200 msec then the synthetic position or velocity is fused anyway so that fusion of synthetic position or velocity observations cannot occur any slower than 5 Hz
Previous check default only checked the number of satellites and horizontal position accuracy.
Updated default value also checks HDoP and speed accuracy.
Fixes the issue of three unused variables, two of which were used in a
commented Debug() call.
To keep the convenient debug message (and the variable names for the
data bytes), this patch uncomment the debug call but wrap the variables
and the debug call around an ifdef for the local symbol
gsof_DEBUGGING. So by turning it on, the debug will already be in place.
The Debug() call was modified to actually compile and include the third
variable in the output.
It uses a heating resistor controlled by a pwm.
By changing the duty cycle of the pwm, we can control the temperature.
A simple PI algorithm is used in order to get to the correct temperature
fast enough and without too much overshoot
It is implemented as a member of the Util class in order not to make to much
modification to the current codebase
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.
Surround calls to rcout->write() with rcout->cork() and rcout->push().
If the RCOutput implementation allows the writes are grouped and only
sent together to the underlying hardware.
Fix warning and use htole16 instead of trying to implement it.
The current code does nothing on little endian platforms.
Moreover, the status variable was unused.
MPU6000 data sheet indicates that variation on gyro ZRO across temperature range from -40 to +85 is +-20 deg/sec.
The limits on the gyro bias states have been increased to allow for this.
To enable the EKF to accommodate such large gyro bias values in yaw without the yaw error wrapping, leading to continual heading drift, an unwrap function has been applied to the compass heading error.
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.
This was only used for supporting APM1. The removal was mostly automatic
with:
sed -i 's/pgm_read_byte(&_motor_to_channel_map\[\([^]]*\)\])/\1/g' libraries/AP_Motors/*.cpp
sed -i 's/_motor_to_channel_map\[\([^]]*\)\]/\1/g' libraries/AP_Motors/*.cpp
And then remove references to MOTOR_TO_CHANNEL_MAP and
_motor_to_channel_map and make sure the variable used in shifts is
unsigned
This method is not used anymore since the introduction of channel map and
allowing motors to be enabled/disabled in AP_Motors.
Later we may introduce a method to write multiple values with a default
implementation that supports the channel and enable maps rather than
requiring all subclasses to implement this method.
This is the only place where this variant of RCOutput::write() is
called. Remove it so to use the common interface. It can be added back
later when there's support for asynchronous write.
Now that most places in the code use the ARRAY_SIZE macro instead of
coding it by hand, let's use some type safety in its definition. This is
a C++ version of similar macros used in kmod, Linux kernel and the
source of them, ccan.
A C++ version like this is used in V8 (the JS engine) and other open
source projects.
The main benefit of this version is that you get a compile error if you
pass in a variable that's not an array. For example,
Bla y[10];
Bla *y_ptr = y;
void foo(Bla x[])
{
// build error since x[] decay to a pointer in function
// parameter
for (int i = 0; i < ARRAY_SIZE(x); i++) {
...
}
// build error since y_ptr is not an array
for (int i = 0; i < ARRAY_SIZE(y_ptr); i++) {
...
}
}
I added the additional specialization to allow arrays of size 0.
If there is a read error, reading from the adc will return 0 but moreover,
we need to re-initiate a read or else we are stuck forever.
From MS5611-01BA03 datasheet, p. 10, CONVERSION SEQUENCE:
"After the conversion, using ADC read command the result is clocked out with the MSB first.
If the conversion is not executed before the ADC read command, or the ADC read command is
repeated, it will give 0 as the output result."
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
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
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
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
we should use baro height not hgt_afe for the climb rate filter. This
makes the climb rate consistent with the one from the EKF. The lidar
correction comes in with the demanded height, not the observed height
the previous approach assumed a 1:1 mapping between compass backends
and compass instances, which isn't true on PX4.
It also only setup milligauss offsets on a set_and_save call, which is
not the only way offsets change
this adds a milligauss_ratio per instance, which is considerably
simpler
Due to the description string getting to long HDop was renamed as EPH with VDop as EPV (Which is the same terimnology used to describe the MAVLink side). Status was shortened to stat as well.
AK8963 is configured in 16-bit ADC mode which implies sensitivity of 0.15 uT/LSb. Knowing this fact we can convert the measurements to the proper units. The change will make users recalibrate their compasses.
From now on there's a pair get_field_milligauss() and
get_offsets_milligauss() that can make the transition to the common
units across all compasses easier.
compile time optimization
Functions like sq() are better moved to the header file as inline.
Compiler can then optimize these out when used in code, this saves cpu
cycles with stack push, pop during function calls.
- add get_prev_nav_cmd_with_wp_index(). This is different than get_prev_nav_cmd_index() in that it only stores the index if there is a valid lat/lng (+1 squashed commits)
- added mission item command to NAV_LAND which is the abort takeoff altitude. If 0 then use last takeoff if available, else use 50m
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.
Also fix bug in do-mount-control so that do-mount-control can switch mount into retract mode
Also removes ability to set which axis are stabilized through
ardupilotmega mount_configure message
