Newer esc firmware versions on bebop 1 and all the versions on bebop 2
have a different order for the motors in the i2c frame sent to the
esc contoller. This commit adds support for both versions by reading
the firmware version of the esc, using GET_INFO frame
- added lowest/highest_threat tracking. This is currently defined as 2D distance. Room for improvement to make it 3D and be flight vector based instead of distance
- when trying to add a vehicle but the buffer is full, overwrite the lowest_threat/furthest
- added basic THREAT enum of high/low which means in or our of the 200m radius. Room for improvement here.
Like was done for AP_HAL_Linux in 2ac96b9 ("AP_HAL_Linux: remove prefix
from AP_HAL_Linux classes"), remove the "Empty" prefix from class names
since we are already inside the Empty namespace.
This was previously used to allow to save a state in a SPIDriver so we
could synchronize the initialization of AP_Compass and
AP_InertialSensor.
It was only used by MPU9250 and is not used anymore since the move to
AuxiliaryBus initialization and it's not used anymore since c3dae6f
("AP_InertialSensor: MPU9250: Remove methods not used anymore")
This was previously used to allow to save a state in a SPIDriver so we
could synchronize the initialization of AP_Compass and
AP_InertialSensor.
It was only used by MPU9250 and is not used anymore since the move to
AuxiliaryBus initialization and it's not used anymore since c3dae6f
("AP_InertialSensor: MPU9250: Remove methods not used anymore")
This is the same approach as done for AP_HAL_Linux in e3d78b8 ("AP_HAL_Linux:
fix passing callback to member function"). It fixes the following warnings:
ardupilot/libraries/AP_HAL_PX4/Scheduler.cpp: In member function 'virtual void PX4::PX4Scheduler::init(void*)':
ardupilot/libraries/AP_HAL_PX4/Scheduler.cpp:55:95: warning: converting from 'void* (PX4::PX4Scheduler::*)()' to 'pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions]
pthread_create(&_timer_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_timer_thread, this);
^
ardupilot/libraries/AP_HAL_PX4/Scheduler.cpp:65:94: warning: converting from 'void* (PX4::PX4Scheduler::*)()' to 'pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions]
pthread_create(&_uart_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_uart_thread, this);
^
ardupilot/libraries/AP_HAL_PX4/Scheduler.cpp:75:92: warning: converting from 'void* (PX4::PX4Scheduler::*)()' to 'pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions]
pthread_create(&_io_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_io_thread, this);
^
ardupilot/libraries/AP_HAL_PX4/Scheduler.cpp:85:100: warning: converting from 'void* (PX4::PX4Scheduler::*)()' to 'pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions]
pthread_create(&_storage_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_storage_thread, this);
ardupilot/libraries/AP_HAL_PX4/NSHShellStream.cpp: In member function 'void PX4::NSHShellStream::start_shell()':
ardupilot/libraries/AP_HAL_PX4/NSHShellStream.cpp:83:99: warning: converting from 'void (PX4::NSHShellStream::*)()' to 'pthread_startroutine_t {aka void* (*)(void*)}' [-Wpmf-conversions]
pthread_create(&shell_thread_ctx, &thread_attr, (pthread_startroutine_t)&PX4::NSHShellStream::shell_thread, this);
^
The argument in init() is not used by any implementation. Add a second
method without it so the HAL implementation can used it instead. Later
the unused method will be removed.
We don't need to expose to other libraries how each backend is
implemented. AP_Baro.h is the main header, included by other libraries.
Instead of including each backend in the main header, move them to where
they are needed. Additionally standardize the order and how we include
the headers.
The advantages are:
- Internals of each backend is not exposed outside of the
library
- Faster incremental builds since we don't need to recompile
whoever includes AP_Baro.h because a backend changed
The configuration of MS5637 is different from MS5611 in 2 ways:
- The PROM is of 112 bytes rather than 128
- The CRC is located in the first MSB of the first word, not the
last one
For CRC calculation we also need to zero out the last (missing) word.
This renames _check_crc() to _read_prom(), which returns false when the
PROM doesn't contain valid data. It also makes it virtual so MS5637 can
override it. This also moves the PROM read to be all in the same place
rather than split between the CRC field and coefficient fields. Finally
calculate_crc() is renamed to crc4() to be shorter and add info on what
it does.
On MS5637 we will need to override the method to read and calculate the
PROM's crc. Thus we need a 2-phase init.
It also makes the constructor of AP_Baro_MS56XX protected since only the
derived classes should instantiate the base one.
The problem with the current MIN/MAX macros is that they evaluate twice
the arguments. For example, these cases provide unintended results:
// a is incremented twice
a = MIN(a++, b);
// foo() with side-effects
a = MIN(foo(), b);
The alternative implementation here was provided by Daniel Frenzel using
template function. It doesn't have type safety as std::min and std::max,
but adding type safety would mean to check case by case what would be a
reasonable type and add proper casts. Here the arguments for MIN and MAX
can have different types and the return type is deduced from the
expression in the function.
Inspecting the current callers no place was found with the unintended
results above, but some in which now we don't calculate twice the
parameters will benefit from this new version. Examples:
float velocity_max = MIN(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));
float acro_level_mix = constrain_float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch()
accel_x_cmss = (GRAVITY_MSS * 100) * (-(_ahrs.cos_yaw() * _ahrs.sin_pitch() / MAX(_ahrs.cos_pitch(),0.5f)) - _ahrs.sin_yaw() * _ahrs.sin_roll() / MAX(_ahrs.cos_roll(),0.5f));
track_leash_slack = MIN(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy);
RC_Channel_aux::move_servo(RC_Channel_aux::k_sprayer_pump, MIN(MAX(ground_speed * _pump_pct_1ms, 100 *_pump_min_pct),10000),0,10000);
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.
Only compiled on Bebop, the constructor will need to be modified to
pass the pwm chip number and to create a PWM_Sysfs instead of a PWM_Sysfs_Bebop
in case it is used on a mainline linux board
Currently, the default behaviour on linux boards tries to
write LED gpios with fixed values among them. There is no way
to declare that there are no LED GPIOs.
This commit moves the declaration of the LED Gpios in AP_HAL_Boards.h
and makes AP_Notify do nothing if no LED gpio was declared
- Make error path in constructor shorter and earlier. It's calling
panic() so there's no reason to do anything else
- We don't need to check variable for NULL when calling free()
- Change set/get_polarity to use a virtual function; this allows us
not to fail silently if _polarity_path is NULL for PWM_Sysfs.
PWM_Sysfs_Bebop just overrides this method and implement an empty
version.
Modify existing class to create a PWM_Sysfs_Base class and derive it for
Bebop and Pwm_Sysfs (mainline kernel)
use asprintf for path allocation since it doesn't cost so much and is done
only at startup
Note that the constructor of the 2 classes : PWM_Sysfs and PWM_Sysfs_Bebop
allocate the paths and the constructor and desctuctor of class PWM_Sysfs_Base
frees them.
only keep in memory the paths that are needed later, i.e free _export_path,
_duty_path. The remaining path are freed in the destructor
The airspeed observation buffer was only being checked when new data arrived instead of every frame which introduced some timing jitter. The buffer is now checked every filer update step.
The duplication and inconsistent naming of booleans used to indicate availability f data has been fixed.
- contains a list of ADS-B vehicles
- does threat analysis and sets a flag when threat is found
- dynamically allocated database, consumes 40*25 =1kB of RAM when param ADSB_ENABLE = 1
- The values are very aggressive, you'll come into contact with another aircraft very soon
- Credit goes to Tridge for this work
- This also allows hardware attached via UART to feed real aircraft into the simulator
This feature is enabled with the following command:
sim_vehicle -A --adsb
The values are very aggressive, you'll come into contact with another aircraft very soon
Credit goes to Tridge for this work
This feature is enabled with the following command:
sim_vehicle -A --adsb
These changes were pair coded an tested by Siddharth Purohit and Paul Riseborough
Fix indexing errors
Move buffer code into a separate file
Split observer and IMU/output buffers and remove duplicate sample time
Optimise observation buffer search
Reduce maximum allowed fusion age to 100 msec
this bypasses all attitude and position estimators and uses the SITL
state directly. It can be used for when the SITL backend cannot
provide perfect sensor data
These are "left-overs" from how things worked before commit
"AP_HAL: make code not depend on concrete HAL
implementations". The real declaration now lives inside get_HAL() function.
Use the opportunitiy to change the files to use "#pragma once".
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.