Skip to content

Commit a54c2a4

Browse files
Merge branch 'PX4:main' into main
2 parents e0eaa9c + 256b329 commit a54c2a4

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

44 files changed

+525
-503
lines changed

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

Lines changed: 38 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1,51 +1,68 @@
11
#!/bin/sh
2-
# @name Gazebo lawnmower
2+
# @name Gazebo - Zero Turn Lawnmower
33
# @type Rover
44
# @class Rover
55

66
. ${R}etc/init.d/rc.rover_differential_defaults
77

88
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
9-
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
9+
PX4_GZ_WORLD=${PX4_GZ_WORLD:=lawn}
1010
PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
1111

1212
param set-default SIM_GZ_EN 1 # Gazebo bridge
1313

14+
param set-default NAV_ACC_RAD 0.5
15+
1416
# We can arm and drive in manual mode when it slides and GPS check fails:
1517
param set-default COM_ARM_WO_GPS 1
1618

17-
# Rover parameters
19+
# Differential Parameters
1820
param set-default RD_WHEEL_TRACK 0.9
19-
param set-default RD_YAW_RATE_I 0.1
20-
param set-default RD_YAW_RATE_P 5
21-
param set-default RD_MAX_ACCEL 1
22-
param set-default RD_MAX_JERK 3
23-
param set-default RD_MAX_SPEED 8
24-
param set-default RD_YAW_P 5
25-
param set-default RD_YAW_I 0.1
26-
param set-default RD_MAX_YAW_RATE 30
2721
param set-default RD_TRANS_DRV_TRN 0.349066
2822
param set-default RD_TRANS_TRN_DRV 0.174533
2923

24+
# Rover Control Parameters
25+
param set-default RO_ACCEL_LIM 1.5
26+
param set-default RO_DECEL_LIM 5
27+
param set-default RO_JERK_LIM 15
28+
param set-default RO_MAX_THR_SPEED 2.7
29+
30+
# Rover Rate Control Parameters
31+
param set-default RO_YAW_RATE_I 0.2
32+
param set-default RO_YAW_RATE_P 1.0
33+
param set-default RO_YAW_RATE_LIM 60
34+
param set-default RO_YAW_ACCEL_LIM 50
35+
param set-default RO_YAW_DECEL_LIM 1000
36+
param set-default RO_YAW_RATE_CORR 1.0
37+
38+
# Rover Attitude Control Parameters
39+
param set-default RO_YAW_P 5
40+
41+
# Rover Velocity Control Parameters
42+
param set-default RO_SPEED_LIM 2.1
43+
param set-default RO_SPEED_I 0.01
44+
param set-default RO_SPEED_P 0.1
45+
param set-defatul RO_SPEED_RED 1.0
46+
3047
# Pure pursuit parameters
31-
param set-default PP_LOOKAHD_MAX 30
32-
param set-default PP_LOOKAHD_MIN 2
3348
param set-default PP_LOOKAHD_GAIN 1
49+
param set-default PP_LOOKAHD_MAX 10
50+
param set-default PP_LOOKAHD_MIN 1
3451

3552
# Actuator mapping - set SITL motors/servos output parameters:
3653

3754
# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
3855
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
39-
#param set-default SIM_GZ_WH_MIN1 0
40-
#param set-default SIM_GZ_WH_MAX1 200
41-
#param set-default SIM_GZ_WH_DIS1 100
42-
#param set-default SIM_GZ_WH_FAIL1 100
56+
param set-default SIM_GZ_WH_MIN1 87
57+
param set-default SIM_GZ_WH_MAX1 113
58+
param set-default SIM_GZ_WH_DIS1 100
59+
param set-default SIM_GZ_WH_FAIL1 100
4360

4461
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
45-
#param set-default SIM_GZ_WH_MIN2 0
46-
#param set-default SIM_GZ_WH_MAX2 200
47-
#param set-default SIM_GZ_WH_DIS2 100
48-
#param set-default SIM_GZ_WH_FAIL2 100
62+
param set-default SIM_GZ_WH_MIN2 87
63+
param set-default SIM_GZ_WH_MAX2 113
64+
param set-default SIM_GZ_WH_DIS2 100
65+
param set-default SIM_GZ_WH_FAIL2 100
4966

5067
param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
5168

boards/cuav/7-nano/default.px4board

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ CONFIG_DRIVERS_GPS=y
2020
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
2121
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
2222
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
23+
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
2324
CONFIG_COMMON_INS=y
2425
CONFIG_COMMON_LIGHT=y
2526
CONFIG_COMMON_MAGNETOMETER=y
@@ -42,8 +43,8 @@ CONFIG_MODULES_EVENTS=y
4243
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
4344
CONFIG_MODULES_FW_ATT_CONTROL=y
4445
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
45-
CONFIG_MODULES_FW_MODE_MANAGER=y
4646
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
47+
CONFIG_MODULES_FW_MODE_MANAGER=y
4748
CONFIG_MODULES_FW_RATE_CONTROL=y
4849
CONFIG_MODULES_GIMBAL=y
4950
CONFIG_MODULES_GYRO_CALIBRATION=y

boards/cuav/7-nano/init/rc.board_sensors

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,10 +12,17 @@ else
1212
fi
1313

1414
iim42652 -s -R 22 start
15+
1516
bmi088 -A -R 29 -s start
16-
bmi088 -G -R 29 -s start
17+
if ! bmi088 -G -R 29 -s start
18+
then
19+
iim42653 -s -b 2 -R 22 start
20+
fi
1721

18-
ist8310 -I -R 18 start
22+
if ! ist8310 -I -R 18 start
23+
then
24+
iis2mdc -I -R 37 start
25+
fi
1926

2027
bmp581 -s start
2128
icp201xx -I start

boards/cuav/7-nano/src/spi.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
4242
initSPIBus(SPI::Bus::SPI2, {
4343
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin2}, SPI::DRDY{GPIO::PortG, GPIO::Pin3}),
4444
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
45+
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortD, GPIO::Pin12}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
4546
}, {GPIO::PortI, GPIO::Pin11}),
4647
// initSPIBus(SPI::Bus::SPI3,{
4748
// // no devices

docs/en/sensor_bus/i2c_general.md

Lines changed: 20 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -4,23 +4,23 @@
44

55
It is recommended for:
66

7-
* Connecting offboard components that require low bandwidth and low latency communication, e.g. [rangefinders](../sensor/rangefinders.md), [magnetometers](../gps_compass/magnetometer.md), [airspeed sensors](../sensor/airspeed.md) and [tachometers](../sensor/tachometers.md) .
7+
* Connecting offboard components that require low bandwidth and low latency communication, e.g. [rangefinders](../sensor/rangefinders.md), [magnetometers](../gps_compass/magnetometer.md), [airspeed sensors](../sensor/airspeed.md) and [tachometers](../sensor/tachometers.md).
88
* Compatibility with peripheral devices that only support I2C.
99
* Allowing multiple devices to attach to a single bus, which is useful for conserving ports.
1010

1111
I2C allows multiple master devices to connect to multiple slave devices using only 2 wires per connection (SDA, SCL).
12-
in theory a bus can support 128 devices, each accessed via its unique address.
12+
in theory, a bus can support 128 devices, each accessed via its unique address.
1313

1414
::: info
15-
UAVCAN would normally be preferred where higher data rates are required, and on larger vehicles where sensors are be mounted further from the flight controller.
15+
UAVCAN would normally be preferred where higher data rates are required, and on larger vehicles where sensors are mounted further from the flight controller.
1616
:::
1717

1818

1919
## Wiring
2020

2121
I2C uses a pair of wires: SDA (serial data) and SCL (serial clock).
2222
The bus is of open-drain type, meaning that devices ground the data line.
23-
It uses a pullup resistor to push it to `log.1` (idle state) - every wire has it usually located on the bus terminating devices.
23+
It uses a pull-up resistor to push it to `log.1` (idle state) - every wire has it usually located on the bus terminating devices.
2424
One bus can connect to multiple I2C devices.
2525
The individual devices are connected without any crossing.
2626

@@ -52,41 +52,44 @@ If two I2C devices on a bus have the same ID there will be a clash, and neither
5252
This usually occurs because a user needs to attach two sensors of the same type to the bus, but may also happen if devices use duplicate addresses by default.
5353

5454
Particular I2C devices may allow you to select a new address for one of the devices to avoid the clash.
55-
Some devices do not support this option, or do not have broad options for the addresses that can be used (i.e. cannot be used to avoid a clash).
55+
Some devices do not support this option or do not have broad options for the addresses that can be used (i.e. cannot be used to avoid a clash).
5656

5757
If you can't change the addresses, one option is to use an [I2C Address Translator](#i2c-address-translators).
5858

5959
### Insufficient Transfer Capacity
6060

61-
The bandwidth available for each individual device generally decreases as more devices are added. The exact decrease depends on the bandwidth used by each individual device. Therefore it is possible to connect many low bandwidth devices, like [tachometers](../sensor/tachometers.md).
61+
The bandwidth available for each device generally decreases as more devices are added. The exact decrease depends on the bandwidth used by each individual device. Therefore it is possible to connect many low-bandwidth devices, like [tachometers](../sensor/tachometers.md).
6262
If too many devices are added, it can cause transmission errors and network unreliability.
6363

6464
There are several ways to reduce the problem:
65-
* Dividing the devices into groups, each with approximately the same number of devices and connecting each group to one autopilot port
65+
* Dividing the devices into groups, each with approximately the same number of devices, and connecting each group to one autopilot port
6666
* Increase bus speed limit (usually set to 100kHz for external I2C bus)
6767

6868
### Excessive Wiring Capacitance
6969

70-
The electrical capacity of bus wiring increases as more devices/wires are added. The exact decrease depends on total length of bus wiring and wiring specific capacitance.
70+
The electrical capacity of bus wiring increases as more devices/wires are added. The exact decrease depends on the total length of bus wiring and wiring-specific capacitance.
7171
The problem can be analyzed using an oscilloscope, where we see that the edges of SDA/SCL signals are no longer sharp.
7272

7373
There are several ways to reduce the problem:
74-
* Dividing the devices into groups, each with approximately the same number of devices and connecting each group to one autopilot port
75-
* Using the shortest and the highest quality I2C cables possible
76-
* Separating the devices with a weak open-drain driver to smaller bus with lower capacitance
77-
* [I2C Bus Accelerators](#i2c-bus-accelerators)
74+
* Dividing the devices into groups, each with approximately the same number of devices, and connecting each group to one autopilot port
75+
* Using the shorter and higher quality I2C cables, see the [cable wiring page](../assembly/cable_wiring.md#i2c-cables) for details
76+
* Separating the devices with a weak open-drain driver to smaller buses with lower capacitance by using [I2C Bus Accelerators](#i2c-bus-accelerators)
7877

7978
## I2C Bus Accelerators
8079

81-
I2C bus accelerators are separate circuits that can be used to support longer wiring length on an I2C bus.
80+
I2C bus accelerators are separate circuits that can be used to support longer wiring lengths on an I2C bus.
8281
They work by physically dividing an I2C network into 2 parts and using their own transistors to amplify I2C signals.
8382

8483
Available accelerators include:
85-
- [Thunderfly TFI2CEXT01](https://github.com/ThunderFly-aerospace/TFI2CEXT01):
84+
- [Thunderfly TFI2CEXT01](https://docs.thunderfly.cz/avionics/TFI2CEXT01/):
8685
![I2C bus extender](../../assets/peripherals/i2c_tfi2cext/tfi2cext01a_bottom.jpg)
8786
- This has Dronecode connectors and is hence very easy to add to a Pixhawk I2C setup.
8887
- The module has no settings (it works out of the box).
8988

89+
### I2C Level Converter function
90+
91+
Some I2C devices have 5V on the data lines, while the Pixhawk connector standard port expects these lines to be 3.3 V.
92+
You can use the TFI2CEXT01 as a level converter to connect 5V devices to a Pixhawk I2C port. This feature is possible because the SCL and SDA lines of TFI2CEXT01 are 5V tolerant.
9093

9194
## I2C Address Translators
9295

@@ -96,21 +99,12 @@ The work by listening for I2C communication and transforming the address when a
9699
Supported I2C Address Translators include:
97100

98101
- [Thunderfly TFI2CADT01](../sensor_bus/translator_tfi2cadt.md)
99-
102+
- This has Dronecode connectors and is very easy to add to a Pixhawk I2C setup.
100103

101104
## I2C Bus Splitters
102105

103-
I2C Bus Splitters are circuit boards that split the I2C port on your flight controller into multiple ports.
104-
They are useful if you want to use multiple I2C peripherals on a flight controller that has only one I2C port (or too few), such as an airspeed sensor and a distance sensor.
105-
106-
You can find an appropriate board using an internet search.
107-
108-
## I2C Level Converter
109-
110-
Some I2C devices have 5V on the data lines, while the Pixhawk connector standard port expects these lines to be 3.3 V.
111-
You can use an I2C level converter to connect 5V devices to a Pixhawk I2C port.
112-
113-
You can find an appropriate covnerter using an internet search.
106+
I2C Bus Splitters are devices that split the I2C port on your flight controller into multiple connectors.
107+
They are useful if you want to use multiple I2C peripherals on a flight controller that has only one I2C port (or too few), such as an airspeed sensor and a distance sensor. Both devices [I2C Address Translator](../sensor_bus/translator_tfi2cadt.md) and [I2C Bus Accelerators](#i2c-bus-accelerators) could also be used as I2C splitters because they have multiple I2C connectors for connecting additional I2C devices.
114108

115109
## I2C Development
116110

docs/en/sensor_bus/translator_tfi2cadt.md

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,19 @@
11
# TFI2CADT01 - I²C Address Translator
22

3-
[TFI2CADT01](https://github.com/ThunderFly-aerospace/TFI2CADT01) is an address translator module that is compatible with Pixhawk and PX4.
3+
[TFI2CADT01](https://docs.thunderfly.cz/avionics/TFI2CADT01/) is an address translator module that is compatible with Pixhawk and PX4.
44

55
Address translation allows multiple I2C devices with the same address to coexist on an I2C network.
66
The module may be needed if using several devices that have the same hard-coded address.
77

8-
The module has an input and an output side.
9-
A sensor is connected to the master device on one side.
8+
The module has an input and an output side and a sensor is connected to the master device on one side.
109
On the output side sensors, whose addresses are to be translated, can be connected.
1110
The module contains two pairs of connectors, each pair responsible for different translations.
1211

1312
![TFI2CADT - i2c address translator](../../assets/peripherals/i2c_tfi2cadt/tfi2cadt01a_both_sides.jpg)
1413

1514
::: info
1615
[TFI2CADT01](https://github.com/ThunderFly-aerospace/TFI2CADT01) is designed as open-source hardware with GPLv3 license.
17-
It is commercially available from [ThunderFly](https://www.thunderfly.cz/) company or from [Tindie eshop](https://www.tindie.com/products/thunderfly/tfi2cadt01-i2c-address-translator/).
16+
It is commercially available from [ThunderFly](https://www.thunderfly.cz/) company or from [Tindie eshop](https://www.tindie.com/products/26353/).
1817
:::
1918

2019
## Address Translation Method
@@ -31,7 +30,7 @@ If you need your own value for address translation, changing the configuration r
3130
The tachometer sensor [TFRPM01](../sensor/thunderfly_tachometer.md) can be set to two different addresses using a solder jumper.
3231
If the autopilot has three buses, only 6 sensors can be connected and no bus remains free (2 available addresses * 3 I2C ports).
3332
In some multicopters or VTOL solutions, there is a need to measure the RPM of 8 or more elements.
34-
The [TFI2CADT01](https://www.tindie.com/products/thunderfly/tfi2cadt01-i2c-address-translator/) is highly recommended in this case.
33+
The [TFI2CADT01](https://www.tindie.com/products/26353/) is highly recommended in this case.
3534

3635
![Multiple sensors](../../assets/peripherals/i2c_tfi2cadt/tfi2cadt01_multi_tfrpm01.jpg)
3736

@@ -58,7 +57,7 @@ graph TD
5857

5958
::: info
6059
TFI2CADT01 does not contain any I2C buffer or accelerator.
61-
As it adds additional capacitance on the bus, we advise combining it with some bus booster, e.g. [TFI2CEXT01](https://github.com/ThunderFly-aerospace/TFI2CEXT01).
60+
As it adds additional capacitance on the bus, we advise combining it with some bus booster, e.g. [TFI2CEXT01](https://docs.thunderfly.cz/avionics/TFI2CEXT01/).
6261
:::
6362

6463
### Other Resources

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
5858
const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
5959
const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);
6060

61-
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.drag_ctrl == 0))) {
61+
if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.ekf2_drag_ctrl == 0))) {
6262
_control_status.flags.wind = false;
6363
}
6464

@@ -72,7 +72,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
7272
if (_control_status.flags.fixed_wing) {
7373
if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
7474
if (!_control_status.flags.fuse_aspd) {
75-
_yawEstimator.setTrueAirspeed(_params.EKFGSF_tas_default);
75+
_yawEstimator.setTrueAirspeed(_params.ekf2_gsf_tas);
7676
}
7777

7878
} else {
@@ -82,7 +82,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
8282

8383
#endif // CONFIG_EKF2_GNSS
8484

85-
if (_params.arsp_thr <= 0.f) {
85+
if (_params.ekf2_arsp_thr <= 0.f) {
8686
stopAirspeedFusion();
8787
return;
8888
}
@@ -99,7 +99,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
9999
const bool continuing_conditions_passing = _control_status.flags.in_air
100100
&& !_control_status.flags.fake_pos;
101101

102-
const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
102+
const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr;
103103
const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
104104
const bool starting_conditions_passing = continuing_conditions_passing
105105
&& is_airspeed_significant
@@ -151,7 +151,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
151151
void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const
152152
{
153153
// Variance for true airspeed measurement - (m/sec)^2
154-
const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
154+
const float R = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f) *
155155
math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
156156

157157
float innov = 0.f;
@@ -165,7 +165,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
165165
R, // observation variance
166166
innov, // innovation
167167
innov_var, // innovation variance
168-
math::max(_params.tas_innov_gate, 1.f)); // innovation gate
168+
math::max(_params.ekf2_tas_gate, 1.f)); // innovation gate
169169
}
170170

171171
void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
@@ -243,7 +243,7 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
243243
constexpr float sideslip_var = sq(math::radians(15.0f));
244244

245245
const float euler_yaw = getEulerYaw(_R_to_earth);
246-
const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f)
246+
const float airspeed_var = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f)
247247
* math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
248248

249249
matrix::SquareMatrix<float, State::wind_vel.dof> P_wind;

0 commit comments

Comments
 (0)