Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

drivers/rc_input: RC_INPUT_PROTO parameter minimal implementation #19539

Merged
merged 1 commit into from Apr 28, 2022

Conversation

dagar
Copy link
Member

@dagar dagar commented Apr 26, 2022

This is a minimal version of #19285 that fixes the drivers/rc_input problem where CRSF and GHST protocols can be falsely detected due to a nearly identical wire format. Following the next release the common drivers/rc_input module will be split into dedicated drivers per protocol.

By default the existing drivers/rc_input behavior is maintained, but with a new parameterRC_INPUT_PROTO that always manually specifying the protocol. Additionally the parameter is set to the RC protocol on the first successful decode, so future sessions will skip the scanning entirely.

Co-authored-by: chris1seto chris12892@gmail.com

@dagar dagar force-pushed the pr-rc_input_param_minimal branch 4 times, most recently from d4295e6 to b3e8e75 Compare April 26, 2022 05:32
@dagar dagar added the bug label Apr 26, 2022
Co-authored-by: chris1seto <chris12892@gmail.com>
@dagar dagar force-pushed the pr-rc_input_param_minimal branch from b3e8e75 to c8cd100 Compare April 26, 2022 12:39
Copy link
Contributor

@mcsauder mcsauder left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These changes look good to me. Flight tested with a Spektrum DX9 and SPM4647, no issues after multiple power cycles with RC connectivity: https://review.px4.io/plot_app?log=7903acf6-d8fe-428c-9962-0abbe9d800a3

@mcsauder mcsauder merged commit a15432f into master Apr 28, 2022
@mcsauder mcsauder deleted the pr-rc_input_param_minimal branch April 28, 2022 03:06
@junwoo091400
Copy link
Contributor

This is great, thank you @chris1seto and @dagar !

@dagar
Copy link
Member Author

dagar commented Apr 28, 2022

Thanks Mark.

dagar added a commit that referenced this pull request Apr 28, 2022
…9539)

Co-authored-by: chris1seto <chris12892@gmail.com>

Co-authored-by: chris1seto <chris12892@gmail.com>
@dagar
Copy link
Member Author

dagar commented Apr 28, 2022

Cherry-picked for release/1.13 4119176

lmarzen added a commit to Open-UAS/PX4-Autopilot that referenced this pull request Oct 13, 2022
…PX4-Autopilot:stable (#12)

* px4io: only publish valid input_rc

* bords: omnibus_f4sd move ekf2 from default to new ekf2 board variant

 - temporary solution to flash overflow

* Jenkinsfile-compile: skip px4_sitl_rtps for now

* boards: px4_fmu-v2_rover disable unused drivers to save flash

* sensors: add baro calibration and cleanup

 - sensor_baro.msg use SI (pressure in Pascals)
 - update all barometer drivers to publish directly and remove PX4Barometer helper
 - introduce baro cal (offset) mainly as a mechanism to adjust
relative priority
 - commander: add simple baro cal that sets baro offsets to align with
GPS altitude (if available)
 - create new sensors_status.msg to generalize sensor reporting

* sensors/vehicle_magnetometer: publish sensors_status_mag and other minor updates to stay in sync with air data

* FlightTaskOrbit: don't start Orbit if radius is not in range

* FlightTaskOrbit: increase radius limit

* boards: px4_fmu-v5x_rtps disable unused systemcmds and examples to save flash

* boards: px4_fmu-v2_multicopter disable distance_sensor/tfmini to save flash

* drives/gps: add new sensor_gnsss_relative msg

 - for ublox this corresponds to NAV_RELPOSNED

* uavcannode: publish RelPosHeading (from sensor_gnss_relative)

* simulator: fix conversion from hPa to Pa

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FlightModeManager: switch to failsafe task if orbit is rejected

* FlightModeManager: don't ack with result failed when parameters are invalid

Command denied is defined as "supported but has invalid parameters"
which matches the case.

* FlightModeManager: refactor flight task switch result condition for vehicle commands

* FlightModeManager: remove needless space at the end of invalid task error string

* FlightTaskOrbit: alert user about exceeded radius (events)

* FlightTaskOrbit: alert user about exceeded radius (mavlink_log)

* fmu-v5: Add support for ICM-42688-P

* spi: get the correct version revision

* fmu-v5: add macro definitions for different version revisions

* mavlink: Use round instead of ceil on BATTERY_STATUS percentage

Co-authored-by: Alex Mikhalev <alex@corvus-robotics.com>

* tunes: Fixed unspecified behaviour

This fixes unspecified behaviour due to evaluation order of args.
It's up to the compiler whether next_number() or next_dots() is executed
first which means that the behaviour is not properly specified.

* tunes: Print warning if there is a tune error

* boards: px4_fmu-v5_stackcheck disable common telemetry modules to save flash

* px_update_git_header:Extract latest release tag

* commander_params: add precision land option for mode switch

* boards: px4_fmu-v5_uavcanv0periph disable modules to save flash

* boards: px4_fmu-v2_fixedwing disable drivers/camera_trigger to save flash

* boards: px4_fmu_v5x_rtps disable common telemetry modules to save flash

* 1011_iris_irlock: require precland (PX4#19431)

* metadata.cmake: enable ethernet parameters

* battery: improve flight time remaining improvements

- introduce BAT_AVRG_CURRENT param that is used for init of average current estimate
- increase filtering of average current estimation
- only update average current filter when armed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* battery: use mechanism to keep an up to date armed state

* battery: update average current also when no capacity is configured

* holybro/kakuteh7: fix BOARD_FLASH_SIZE

The BL was reporting a flash size of 1703936, whereas it should be 1835008.

* Support for NXP UWB position sensor

uwb_sr150 driver for the sensor, and some
modifications in precision landing to allow
landing on a platform using the UWB system.

* precland: save flash space

* commander: silence GPS no longer validity PX4_WARN

* uavcan: update safety button

updated uavcannode/Publishers/SafetyButton.hpp
tested successfully
make format
revert cannode publishing

* safety and safety button: refactoring PX4#19413

* mathlib: add second order reference model filter with optional rate feedback (PX4#19246)

mathlib: add second order reference model filter with optional rate feedback (PX4#19246)

Reference models can be used as filters which exhibit a particular, chosen (reference) dynamic behavior. This PR implements a simple second order transfer function which can be used as such a reference model, additionally with rate feedback. The system is parameterized by explicitly set natural frequency and damping ratio. Another nice externality is that the output state and rate are kinematically consistent. Forward-euler and bilinear transform discretizations for the state space integration step are available.

* boards: omnibus_f4sd_ekf2 disable frsky telemetry to save flash (and fix build)

 - this isn't great, but our options are limited at the moment and this
can be fixed later once the old mixing system is deleted

* Commander: separate out arm state machine to class

Pure refactoring and just the first step to avoid conflicts on the way.

* ArmStateMachine: port arming_state_names into the class

* ArmStateMachine: port over unit tests to functional gtests

* mavlink: set correct param capability

PX4 supports int32 parameter types by interpreting the 32 bits in
the float field as an int32 field. To signal this behaviour, it should
set the bit which is described as PARAM_ENCODE_BYTEWISE.

PX4 had always handled parameters this way but never actually sent the
capability (which back then was called PARAM_UNION), however, it should
have. This came up recently in the MAVLink devcall when these flags were
discussed. The takeaway was to remove the flags to make it clearer and
to make sure the projects (like PX4) send them out correctly.

* update mavlink submodule to latest

 - update MAV_TYPE VTOL usage for current mavlink

* rc_update: further tighten timing requirements for valid data

 - any real RC data input will be much faster than 3 Hz, so this is an
easy way to minimize bogus decoded data from propagating

* ci: build and deploy kakuteh7

* modeCheck: allow arming in land mode for MAVSDK compatibility

ideally we can remove it again when the workflow is changed to
first changing mode then arming.

* use new safety_button topic for uavcannode Button publishing

* boards/matek/gnss-m9n-f4: IMU orientation update

 - the icm20602 needs to be rolled 180º and yaw 90º for this board.

* boards/matek/gnss-m9n-f4: RM3100 orientation fix

 - the RM3100 needs to be pitched 180º for correction orientation on this board.

* Add I2C retries in INA226 to prevent publishing 0's on a single read failure

* px4io: input_rc only publish new successful decodes

 - previously an invalid decode would continue to be transferred to the FMU (at 50 Hz) and published to the rest of the system as successfully decoded new RC data
 - by only publishing new successful decodes we can more effectively discard invalid data in downstream consumers

* commander: fix enable_failsafe reason (PX4#19391)

 - In this case, no action is configured for datalink lost. Action is configured for RC lost.
 - In case of no data link and no rc failsafe is enabled but reporting a "no_datalink" reason but "no_rc_and_no_datalink" seems more explicit.

* Increase NAV_LOITER_RAD and RTL_LOITER_RAD to 80m each

For many VTOLs/fixed-wing drones a 50m loiter radius is too tight, and
going to 80m is a better and safer default.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* ROMFS: remove duplicate setting of NAV_LOITER_RAD to 80

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* Update submodule mavlink to latest Sun Apr 10 12:39:00 UTC 2022

    - mavlink in PX4/Firmware (a1530591764f0c694560e4bb6ae41c15d3e35c9b): mavlink/mavlink@0133e5d
    - mavlink current upstream: mavlink/mavlink@56a5110
    - Changes: mavlink/mavlink@0133e5d...56a5110

    56a5110d 2022-04-09 Tom Pittenger - Add radius to DO_REPOSITION (PX4#1825)
3b5959bd 2022-04-07 Thomas Debrunner - Option to not reset non-configurable params in preflight storage (PX4#1826)

* ekf2: fix IMU missed perf count when not using multi-EKF

* Tools: add drag fusion tuning script

* boards: px4_fmu-v2_multicopter disable load_mon to save flash

* ekf2: properly reset IMU biases on calibration change (non-multi-EKF)

 - this was working in the multi-EKF case using vehicle_imu, but missing
in sensor_combined

* Update world_magnetic_model to latest Mon Apr 11 11:14:11 UTC 2022 (PX4#19475)

* Update submodule sitl_gazebo to latest Tue Apr 12 12:38:53 UTC 2022

    - sitl_gazebo in PX4/Firmware (bb2ea57): PX4/PX4-SITL_gazebo-classic@25138e8
    - sitl_gazebo current upstream: PX4/PX4-SITL_gazebo-classic@2cf56d0
    - Changes: PX4/PX4-SITL_gazebo-classic@25138e8...2cf56d0

    2cf56d0 2022-03-23 Julian Oes - Revert "models: Add model for standard_vtol_ctrlalloc"
b3ab8de 2022-03-18 JamesAnawati - Update cloudship.sdf.jinja

* Update submodule GPSDrivers to latest Tue Apr 12 12:38:56 UTC 2022

    - GPSDrivers in PX4/Firmware (c04e66a): PX4/PX4-GPSDrivers@ad1094a
    - GPSDrivers current upstream: PX4/PX4-GPSDrivers@ddb1825
    - Changes: PX4/PX4-GPSDrivers@ad1094a...ddb1825

    ddb1825 2022-03-29 Daniel Agar - ubx: print relevent UBX-MON-VER output

Co-authored-by: PX4 BuildBot <bot@px4.io>

* MulticopterPositionControl: remove unused return value parameters_update()

* posix: HRT hrt_lock() sem_wait try again if error returned

* ROMFS: move px4flow start to rc.sensors

* Add ARK CANnode board config

* uORB : Don't automatically include message name as default topic name in uORBTopics source files, to handle case where user doesn't use default messgae name for multi topic definition in .msg file

* navigator: stop handling speed changes via reposition triplet

- the mc and fw controllers are handling the speed changes directly

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FixedWingPositionControl: handle VEHICLE_CMD_DO_CHANGE_SPEED

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FlightModeManager: handle MAV_CMD_DO_CHANGE_SPEED

- support setting the cruise speed of the auto flight task via command

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* vehicle_command: added enum for speed types

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* addressed review comments

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* vehicle_command: specify what SPEED_TYPEs are for

* FlightModeManager/FixedwingPositionControl: robustify vehicle command parameter casting

* FlightTask: rename and move setCruisingSpeed() -> overrideCruiseSpeed()

* FlightTaskAuto: refactor _commanded_speed_ts -> _time_last_cruise_speed_override

* ekf: extract range finder noise computation

* ekf: add range finder kinematic consistency check

At each new valid range measurement, the time derivative of the distance
to the ground is computed and compared with the estimated velocity.
The average of a normalized innovation squared statistic check is used
to detect a bias in the derivative of distance measurement,
indicating that the distance measurements are kinematically inconsistent
with the filter.

* ekf: access member variable without getter

* ekf: use same gate for innov and innov sequence monitoring

* ekf: improve rng consistency check

To pass from invalid to valid:
- time hysteresis
- some vertical velocity
- test ratio < 1
- low-passed signed test ratio < 1

To pass from valid to invalid:
- low-passed signed test ratio >= 1

* ekf: requires kinematically consistent range finder data to continue terrain aiding

* ekf: use uint64_t for time variables

* ekf: add logging of range finder consistency check

* ekf: add logging for rng kinematic consistency check

* ekf: run rng consistency check only when not horizontally moving

The check assumes a non-moving terrain height

* ekf: make range finder kin consistency gate tunable by parameter

* ekf rng finder consistency: simplify class member names

* ekf rng kin: increase default gate size

The user needs to tune the range finder noise parameters properly and we
shouldn't need such a small gate

* ekf rng kin: reduce minimum rng variance

* sensors/vehicle_imu: replace coning metric with actual integrator coning correction (averaged)

 - this saves a relatively expensive higih rate cross product and gives
better visibility into what's actually happening internally

* sensors/vehicle_imu: Integrator use 1 microsecond for minimum DT

 - this is a more realistic minimum for the system

* Tools/ecl_ekf: fix vibe_metrics usage (moved to vehicle_imu_status instances)

* Update submodule mavlink to latest Wed Apr 13 12:39:05 UTC 2022

    - mavlink in PX4/Firmware (16fd85d): mavlink/mavlink@56a5110
    - mavlink current upstream: mavlink/mavlink@3b52eac
    - Changes: mavlink/mavlink@56a5110...3b52eac

    3b52eac0 2022-04-13 Beat Küng - add COMPONENT_METADATA, deprecate COMPONENT_INFORMATION (PX4#1823)

* lib/rc: Fix DSM2/DSMX guessing routine and DSM range checking (PX4#18270)

* Add Orangerx test case

Co-authored-by: Chris Seto <chris.seto@bossanova.com>

* dronecan beeper: remove unneded var

* px4io cleanup LED and heater handling

 - most px4_io-v2 boards have a blue LED that breathes for status
 - the pixhawk 2.1 (hex) re-used this blue LED for as an IMU heater (active low), but kept the same board id (so we have to detect at runtime)
 - the new cubepilot boards (yellow, orange) inverted the polarity of this heater pin
 - untangle the mess slightly so that things we know statically (eg cubepilot cubeorange LEDs and heater polarity) are handled at build time.

* param-reset: Add option to reset all configurable params, but not the ones that store vehicle information

* commander/safety: set early safety_button_available

* mavlink: add COMPONENT_METADATA message

And still support the previous message COMPONENT_INFORMATION for now.

* drivers/optical_flow: new PixArt PAA3905 optical flow driver

* boards: px4_fmu-v6x_default disable systemcmds/sd_bench to save flash

* boards: px4_fmu_v5x_rtps disable several modules to save flash

* boards: px4_fmu-v5_uacanv0periph disable systecmds/sd_bench to save flash

* boards: px4_fmu-v2_multicopter disable lightware_laser_serial to save flash

* px4iofirmware: convert most files to c++

* boards: px4_fmu-v5/v5x run default & rtps boards through kconfiglib to cleanup

* boards: px4_fmu-v5_stackcheck disable unused systemcmds to save flash

* small cleanup for  FlightTask::_evaluateDistanceToGround if-else

* output modules simplify locking for mixer reset and load

 - fixes the deadlock in px4io ioctl mixer reset
   - px4io Run() locks (CDev semaphore)
   - mixer load goes through px4io ioctl MIXERIOCRESET which calls MixingOutput::resetMixerThreadSafe()
   - MixingOutput::resetMixerThreadSafe() stores a Command::Type::resetMixer command in an atomic variable, schedules a work queue cycle, then sleep spins until the command is cleared
   - the execution of the cycle eventually calls back into PX4IO::updateOutputs(), which tries to lock (and waits forever)

* mission block: fix incorrectly calculated ccw loiter exit (PX4#19487)

* mission block: fix incorrectly calculated ccw loiter exit

* mission block: update comment on orbit exit location

* mission_block: fix vehicle not exiting loiter after reaching exit heading

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* commander: refactor home position setter

- always try to set local or global home position when possible
- set global home with GNSS position if global pos from EKF isn't
  available
- reset home when significantly moved from home before takeoff (checking
  lpos or gpos or GNSS)
- reset home on takeoff transition
- reset home on mavlink arm command
- remove "home required accuracy" parameters, rely on validity flags

* navigator: home_positionvalid -> global_home_position_valid

This is to make clear that the relevant part of the home position
message for navigator is the global one. Local home position isn't
required as everything is done in global coordinates.
The specific home_alt_valid is used when lat/lon are not used

* geofence: remove unused function parameters

* commander: improve set_in_air_position

- set local home using global pos and global home
- set local home using GNSS pos and global home
- set global home using global ref of local frame and local home

* mavsdk_tests: update MAVSDK dependency

This should fix the CI issue where the test just hangs trying to
connect.

* mc wind: rename MC wind estimor tuning script

* Tools: add baro pressure coefficient tuning script

* EKF: move python tuning tools to EKF module

* Update submodule GPSDrivers to latest Thu Apr 21 12:38:20 UTC 2022

    - GPSDrivers in PX4/Firmware (e9c07fa): PX4/PX4-GPSDrivers@ddb1825
    - GPSDrivers current upstream: PX4/PX4-GPSDrivers@6534b05
    - Changes: PX4/PX4-GPSDrivers@ddb1825...6534b05

    6534b05 2022-04-19 Jonas Perolini - ubx: disable gps heading for in RTK float fix type (PX4#104)

Co-authored-by: PX4 BuildBot <bot@px4.io>

* 13004_quad+_tailsitter - outputs mixed up

* Add gyro and accel range register values to the icm42688p driver.

* px4flow allow delayed background startup

* platforms/nuttx: cdc_acm_check implement mavlink reboot directly

* Mission: don't do anything in set_current_mission_index() when index=current already

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* ROMFS: fix typo in convergence and clair configs

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* FlightTaskAuto: fix Weather Vane during landing

Weather vane should only set a yawrate setpoint, but no yaw setpoint.
Setting it to NAN when WV is active makes sure that whatever _yaw_setpoint
is set previously (e.g. through Waypoint) is not used.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* logger: add default ground truth logging for HITL/SITL

* boards: new Sky-Drones AIRLink board support

* icm20948: disable debug output (_debug_enabled=true)

* Set Tune's Volume for Power-Off in Commander to default volume of tune_control message

* mavlink: shell expand locking (PX4#19308)

 - on some H7 boards (cuav x7pro tested) there's an occasional hard fault when starting the mavlink shell that is no longer reproducible with the slightly expanded locking
 - this is likely just changing the timing (holding the sched lock for longer), but this should be harmless for now until we can identify the root cause

* commander: Add prearm check for flight termination

* mavlink: delete Mavlink instance if early startup fails

* commander: mag calibration tolerate fit failure if sensor disabled

* ekf: robustify bad_acc_vertical check

when the vertical pos or vel innov ratio is above the threshold, the
other one needs to be significant too and not just positive to trigger
the failure

* [AUTO COMMIT] update change indication

* systemcmds/param: set-default should mark parameter active to avoid race conditions

* mc_att_control: only apply quat reset if estimate is newer than setpoint

* boards: add new mRo Control Zero Classic

* boards: add new Diatone Mamba F405 mk2

* boards/diatone/mamba-f405-mk2: fix code style

* boards/mro/ctrl-zero-classic: fix code style

* boards: update bootloaders to latest

* boards: px4_fmu-v2 restore systemcmds/ver needed for board init

* boards/diatone/mamba-f405-mk2: disable modules to save flash

* mc_pos_control: silence invalid setpoint warning when disarmed

* boards: px4_fmu-v6x_default disable common barometers to save flash

* Separate takeoff and landing to individual fixed wing states for FW pos control (PX4#19495)

* apply differential pressure calibration (SENS_DPRES_OFF) centrally

 - remove drv_airspeed.h and ioctls

* differential pressure remove filters from drivers and average in sensors/airspeed

* drivers/differential_pressure: remove lib/drivers/airspeed dependency and cleanup

 - split ms4525_airspeed into separate ms4515 and ms4525 drivers

* boards: cubepilot_cubeorange_test disable examples/fake_gps to save flash

* boards: px4_fmu-v5_uavcanv0periph disable modules to save flash

* drivers/rc_input: RC_INPUT_PROTO parameter minimal implementation (PX4#19539)

Co-authored-by: chris1seto <chris12892@gmail.com>

Co-authored-by: chris1seto <chris12892@gmail.com>

* fix dshot: remove setAllFailsafeValues

Fixes a regression from c1e5e66,
where with static mixers the dshot outputs would go to max instead of 0
in a failsafe case.

* dshot: avoid using pwm failsafe params when dynamic mixing is enabled

* ekf2: optimize KHP computation

Calculating K(HP) takes less operations than (KH)P because K and H are
vectors.

Without considering the sparsity optimization:
- KH (24*24 operations) is then a 24x24 matrix an it takes
24^3 operations to multiply it with P. Total: 14400 op

- HP (24*(24+24-1) operations) is a row vector
and it takes 24 operations to left-multiply it by K. Total:1152 op

* commander: fix incorrect return in set_link_loss_nav_state()

If both local position and altitude were not valid, then both RC loss and
datalink loss would not trigger any failsafe at all, independently from
the configured action.

* Commander: ensure diconnected battery is cleared from bit field

* fw pos ctrl: fix state switching logic for takeoff and landing

* fw pos ctrl: add missing guidance control interval setting to control_manual_position()

* fw pos ctrl: turn back to takeoff point with npfg

* uavcan: use timer 6 by default on stm32f7

* log_writer_file: protect access to _should_run, use px4::atomicbool for _exit_thread

* log_writer_file: fix corner case when mission log is enabled

Normally _should_run for mission is only ever true if _should_run for the
normal log is. There are exceptions though:
- the log buffer fails to allocate
- there was a write failure (e.g. due to SD card removal)
In that situation, the writer would not wait anymore but busy-loop.

* drivers/rc_input: ensure RC inversion is disabled initially and latch RC_INPUT_PROTO conservatively

 - this allows jumping straight to a non-SBUS RC protocol
 - increased the scan time per protocol 300->500 ms, which the newer DSM parser seems to need in some cases.
 - only set RC_INPUT_PROTO if we've had a successful RC lock for > 3 seconds

* Fix uavcan battery causing immediate RTL time remaining low

* output drivers: init SmartLock after exit_and_cleanup

This fixes an invalid memory access when exiting the module:
exit_and_cleanup destroys the object, but lock_guard is destructed after
and accesses the lock.

* .vscode/.gitignore ignore all log files

* Commander: ignore MAV_CMD_REQUEST_MSG

This commit adds the MAV_CMD_REQUEST_MESSAGE to the list of vehicle
commands which are ignored without generating a warning sound.

* HITL: undefined time_remaining_s should be NAN

* ROMFS: disable UAVCAN in HITL

Without this, uavcan creates MixingOutput classes which then create
empty actuator_outputs publications. This then prevents the motor
output in HITL to be forwarded to the simulator via mavlink.

* sensors/vehicle_magnetometer: fix multi_mode check

Signed-off-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>

* ekf2: use explicit flags instead of bitmask position

This prevents bitmask mismatch when a new flag is inserted

* icm42688p: only check configured registers periodically (as intended)

* commander: lockdown is not termination

We use lockdown to prevent outputs like motors and servos from being
active in HITL simulation. This means that we can't treat the lockdown
flag as a flight_terminated, otherwise we can't arm in HITL at all.

* sitl_gazebo: update submodule

This fixes the issue where HITL doesn't connect over USB.

* ekf2_post-processing: use estimator_status_flags instead of bitmasks

* boards: new px4_fmu-v6c board support (PX4#19544)

* px4_fmu-v6c:Fix mag rotation

* boards: STM32H7 pad to 256 bit - 32 bytes (PX4#19724)

* flash_cache:Flush complete cache line

* Update all H7 Bootloders

* boards: pixhawk 2 cube skip starting low quality l3gd20 gyro to save memory and cpu

 - free memory is getting tight on these older boards (depending on
configuratoin) and the pixhawk 2 cube still has 2 other superior IMUs, so this is just
dropping dead weight

* boards: reduce SPI DMA buffers on older STM32F4 boards

 - on common IMUs like the mpu6000, mpu9250, icm20602, etc each FIFO
sample is only 12 bytes so this is still more than large enough for the
worst case transfer

* update NuttX and apps to latest with sem holder fixes and updated ostest

* boards: NuttX update all boards to preallocated sem holder list

 - CONFIG_SEM_PREALLOCHOLDERS=32
 - CONFIG_SEM_NNESTPRIO=16 (default)

* sensors/vehicle_imu: don't bother checking IMU_GYRO_RATEMAX

* sensors/vehicle_angular_velocity: add IMU_GYRO_RATEMAX constraints

* uavcannode: Fix dronecan baro units

* px4_fmu-v6x:Add Rev 4 Sensors

* px4_fmu-v5X:Added Holybro mini base board

* uc_stm32h7_can:Correct initalization of the mumber of interfaces

   H7 Only supports 2 not 3 CAN interfaces.

   CanInitHelper passes in in the run time configuration of
   the number of interfaces. The code was ignoring these!

* px4_fmu-v6X:Added Holybro mini base board

* differential_pressure/sdp3x: sdp3x_main fix 'keep running' regression

* uavcan: GNSS optionally publish RTCMStream or MovingBaselineData

* ekf2: do not run rng kinematic consistency check for fixed-wings

As they are always moving horizontally, the check doesn't make sense
for fixed-wings.
Also don't run the check while on ground to prevent getting a failed
check during pre-takeoff manipulation.

* gps_inject_data: fixed integer overflow
- array length of data was increased without changing the data type of
the variable holding the length

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* adis16470: fix accel and gyro scaling

* drivers/imu/invensense/icm42670p: cleanup and small fixes

* icm42670p run at full speed

* px4_fmu-v6x:HB Mini add Ver 3, Ver 4 init

* standard: fixed pusher assist in hover

- in hover mode the pusher assist is already set in update_mc_state()

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* boards: V1.13.0 ARKV6X Backport (PX4#20253)

* fmu-v6c: internal baro and mag on external bus

This swaps the internal baro and mag back to the bus which is both
internal an external but configured as external for this case.

* px4_fmu-v6c: Move I2C 4 back to External

This is a revert of the revert.
This reverts commit 1080855.

* [DO NOT MERGE] px4_i2c_device_external hacks

* Add PX4 vision v1.5 Airframe

* boards: save some flash on CubeOrange test config

* Add BMP390 to BMP388 driver

* boards/platform: remove confusing override

This removes the odd px4_i2c_bus_external override which was confusing
me and lead to odd and inconsistent results.

The function is now only available with an int as the argument.

* cuav_x7pro: save flash on test target

* Add Holybro Pixhawk Pi CM4 Baseboard Support

* Fix Error on board_config.h Define on FMUv5X & FMUv6X

* Fix Error on manifest.c

* Correct BOARD_NUM_SPI_CFG_HW_VERSIONS at board_config.h

* update v5x rc.board_sensors

add V5X004002

* update fmuv5x rc.board_sensors with V5X004000

* update fmu-v6x rc.board_sensors, add V6X004003

* platforms: decrease flash usage by type for bus id

My assumption is that the bus are numbered < 127.
This saves about 100 bytes of flash.

* fmu-v5x: disable rev0 for HB Mini and CM4 base

As I understand it, only Rev 1 and Rev 2 were shipped by HB, and likely
to be used for the Mini and CM4.

* fmu-v6: disable Rev 0, Rev 1 for HB Mini, and CM4

As I understand it, only Rev 3 and Rev 4 were shipped by HB for Mini and
CM4, and are likely to be used for it.

It would be nice to have all combinations but it requires quite some
flash in the current implementation.

* fmu-v5x: alias for duplicates, remove commented

- Removed commented out config data.
- hw_mft_list_v0540 was the same as hw_mft_list_v0500

* fmu-v6x: alias, add VX43

- alias from hw_mft_list_v0650 to hw_mft_list_v0600 as it is the same
- add V6X50 again

* fmu-v3: make optional sensor startup quiet

This fixes the errors showing up at startup for me with a Black Cube.

* fmu-v5x: support for mini base board

This was forgotton with all the merges and shuffling previously.

* libuavcan: update submodule

This fixes a Python 3.10 issue for me.

* uavcan: fix RTCM corrections publication

Before this fix, this function would stall and somehow never return.

* Mag: fix estimated bias save to calibration

Clear the calibration at the end only otherwise everything gets erased
at the end of the first iteration of the outer loop

* Fix ARKV6X control allocator with base boards

* ms5611: ignore reading 0

This prevents publishing a negative pressure which leads to a NAN
altitude estimate further down the line.

* vehicle_imu: only reset raw accel/gyro Welford mean periodically

 - vehicle_imu_status can publish immediately on any measured sample
rate change or sensor error increment, but the windowed mean/variance
shouldn't necessarily reset that often

* welford mean: convert to matrix only template

* welford mean: protect against negative variances

* resolved merge conflicts

* resolved merge conflicts

* updated sitl_gazebo submodule

* updated sitl_gazebo submodule

* removed cudepilot files that were removed from PX4

* added whitespace to match PX4:stable

* updated sitl_gazebo submodule tto master

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>
Co-authored-by: Daniel Agar <daniel@agar.ca>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
Co-authored-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: CUAVmengxiao <mengxiao@cuav.net>
Co-authored-by: Kabir Mohammed <kabir@corvus-robotics.com>
Co-authored-by: Alex Mikhalev <alex@corvus-robotics.com>
Co-authored-by: Alex Mikhalev <alexmikhalevalex@gmail.com>
Co-authored-by: David Sidrane <David.Sidrane@NscDg.com>
Co-authored-by: stmoon <munhoney@gmail.com>
Co-authored-by: alessandro <3762382+potaito@users.noreply.github.com>
Co-authored-by: Beat Küng <beat-kueng@gmx.net>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Hovergames <loic.fernau@googlemail.com>
Co-authored-by: Alessandro Simovic <alessandro@auterion.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
Co-authored-by: Igor Misic <igy1000mb@gmail.com>
Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com>
Co-authored-by: Julian Oes <julian@oes.ch>
Co-authored-by: Ramon Roche <mrpollo@gmail.com>
Co-authored-by: Ryan Johnston <31726584+ryanjAA@users.noreply.github.com>
Co-authored-by: alexklimaj <alex@arkelectron.com>
Co-authored-by: Nicolas MARTIN <59083163+NicolasM0@users.noreply.github.com>
Co-authored-by: PX4 BuildBot <bot@px4.io>
Co-authored-by: bresch <brescianimathieu@gmail.com>
Co-authored-by: Junwoo Hwang <junwoo@auterion.com>
Co-authored-by: chris1seto <chris12892@gmail.com>
Co-authored-by: Chris Seto <chris.seto@bossanova.com>
Co-authored-by: Thomas Debrunner <thomas.debrunner@auterion.com>
Co-authored-by: bazooka joe <bazookajoe1900@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: mcsauder <mcsauder@gmail.com>
Co-authored-by: Kirill Shilov <aviaks.kirill@gmail.com>
Co-authored-by: bresch <bresch@users.noreply.github.com>
Co-authored-by: achim <lumserei@web.de>
Co-authored-by: JaeyoungLim <jalim@ethz.ch>
Co-authored-by: Nico van Duijn <nico@auterion.com>
Co-authored-by: Nicolas MARTIN <nicolas1.martin1@gmail.com>
Co-authored-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>
Co-authored-by: vincentpoont2 <vincentpoont2@gmail.com>
lmarzen added a commit to Open-UAS/PX4-Autopilot that referenced this pull request Jan 24, 2023
* Jenkinsfile-compile: skip px4_sitl_rtps for now

* boards: px4_fmu-v2_rover disable unused drivers to save flash

* sensors: add baro calibration and cleanup

 - sensor_baro.msg use SI (pressure in Pascals)
 - update all barometer drivers to publish directly and remove PX4Barometer helper
 - introduce baro cal (offset) mainly as a mechanism to adjust
relative priority
 - commander: add simple baro cal that sets baro offsets to align with
GPS altitude (if available)
 - create new sensors_status.msg to generalize sensor reporting

* sensors/vehicle_magnetometer: publish sensors_status_mag and other minor updates to stay in sync with air data

* FlightTaskOrbit: don't start Orbit if radius is not in range

* FlightTaskOrbit: increase radius limit

* boards: px4_fmu-v5x_rtps disable unused systemcmds and examples to save flash

* boards: px4_fmu-v2_multicopter disable distance_sensor/tfmini to save flash

* drives/gps: add new sensor_gnsss_relative msg

 - for ublox this corresponds to NAV_RELPOSNED

* uavcannode: publish RelPosHeading (from sensor_gnss_relative)

* simulator: fix conversion from hPa to Pa

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FlightModeManager: switch to failsafe task if orbit is rejected

* FlightModeManager: don't ack with result failed when parameters are invalid

Command denied is defined as "supported but has invalid parameters"
which matches the case.

* FlightModeManager: refactor flight task switch result condition for vehicle commands

* FlightModeManager: remove needless space at the end of invalid task error string

* FlightTaskOrbit: alert user about exceeded radius (events)

* FlightTaskOrbit: alert user about exceeded radius (mavlink_log)

* fmu-v5: Add support for ICM-42688-P

* spi: get the correct version revision

* fmu-v5: add macro definitions for different version revisions

* mavlink: Use round instead of ceil on BATTERY_STATUS percentage

Co-authored-by: Alex Mikhalev <alex@corvus-robotics.com>

* tunes: Fixed unspecified behaviour

This fixes unspecified behaviour due to evaluation order of args.
It's up to the compiler whether next_number() or next_dots() is executed
first which means that the behaviour is not properly specified.

* tunes: Print warning if there is a tune error

* boards: px4_fmu-v5_stackcheck disable common telemetry modules to save flash

* px_update_git_header:Extract latest release tag

* commander_params: add precision land option for mode switch

* boards: px4_fmu-v5_uavcanv0periph disable modules to save flash

* boards: px4_fmu-v2_fixedwing disable drivers/camera_trigger to save flash

* boards: px4_fmu_v5x_rtps disable common telemetry modules to save flash

* 1011_iris_irlock: require precland (PX4#19431)

* metadata.cmake: enable ethernet parameters

* battery: improve flight time remaining improvements

- introduce BAT_AVRG_CURRENT param that is used for init of average current estimate
- increase filtering of average current estimation
- only update average current filter when armed

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* battery: use mechanism to keep an up to date armed state

* battery: update average current also when no capacity is configured

* holybro/kakuteh7: fix BOARD_FLASH_SIZE

The BL was reporting a flash size of 1703936, whereas it should be 1835008.

* Support for NXP UWB position sensor

uwb_sr150 driver for the sensor, and some
modifications in precision landing to allow
landing on a platform using the UWB system.

* precland: save flash space

* commander: silence GPS no longer validity PX4_WARN

* uavcan: update safety button

updated uavcannode/Publishers/SafetyButton.hpp
tested successfully
make format
revert cannode publishing

* safety and safety button: refactoring PX4#19413

* mathlib: add second order reference model filter with optional rate feedback (PX4#19246)

mathlib: add second order reference model filter with optional rate feedback (PX4#19246)

Reference models can be used as filters which exhibit a particular, chosen (reference) dynamic behavior. This PR implements a simple second order transfer function which can be used as such a reference model, additionally with rate feedback. The system is parameterized by explicitly set natural frequency and damping ratio. Another nice externality is that the output state and rate are kinematically consistent. Forward-euler and bilinear transform discretizations for the state space integration step are available.

* boards: omnibus_f4sd_ekf2 disable frsky telemetry to save flash (and fix build)

 - this isn't great, but our options are limited at the moment and this
can be fixed later once the old mixing system is deleted

* Commander: separate out arm state machine to class

Pure refactoring and just the first step to avoid conflicts on the way.

* ArmStateMachine: port arming_state_names into the class

* ArmStateMachine: port over unit tests to functional gtests

* mavlink: set correct param capability

PX4 supports int32 parameter types by interpreting the 32 bits in
the float field as an int32 field. To signal this behaviour, it should
set the bit which is described as PARAM_ENCODE_BYTEWISE.

PX4 had always handled parameters this way but never actually sent the
capability (which back then was called PARAM_UNION), however, it should
have. This came up recently in the MAVLink devcall when these flags were
discussed. The takeaway was to remove the flags to make it clearer and
to make sure the projects (like PX4) send them out correctly.

* update mavlink submodule to latest

 - update MAV_TYPE VTOL usage for current mavlink

* rc_update: further tighten timing requirements for valid data

 - any real RC data input will be much faster than 3 Hz, so this is an
easy way to minimize bogus decoded data from propagating

* ci: build and deploy kakuteh7

* modeCheck: allow arming in land mode for MAVSDK compatibility

ideally we can remove it again when the workflow is changed to
first changing mode then arming.

* use new safety_button topic for uavcannode Button publishing

* boards/matek/gnss-m9n-f4: IMU orientation update

 - the icm20602 needs to be rolled 180º and yaw 90º for this board.

* boards/matek/gnss-m9n-f4: RM3100 orientation fix

 - the RM3100 needs to be pitched 180º for correction orientation on this board.

* Add I2C retries in INA226 to prevent publishing 0's on a single read failure

* px4io: input_rc only publish new successful decodes

 - previously an invalid decode would continue to be transferred to the FMU (at 50 Hz) and published to the rest of the system as successfully decoded new RC data
 - by only publishing new successful decodes we can more effectively discard invalid data in downstream consumers

* commander: fix enable_failsafe reason (PX4#19391)

 - In this case, no action is configured for datalink lost. Action is configured for RC lost.
 - In case of no data link and no rc failsafe is enabled but reporting a "no_datalink" reason but "no_rc_and_no_datalink" seems more explicit.

* Increase NAV_LOITER_RAD and RTL_LOITER_RAD to 80m each

For many VTOLs/fixed-wing drones a 50m loiter radius is too tight, and
going to 80m is a better and safer default.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* ROMFS: remove duplicate setting of NAV_LOITER_RAD to 80

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* Update submodule mavlink to latest Sun Apr 10 12:39:00 UTC 2022

    - mavlink in PX4/Firmware (a1530591764f0c694560e4bb6ae41c15d3e35c9b): mavlink/mavlink@0133e5d
    - mavlink current upstream: mavlink/mavlink@56a5110
    - Changes: mavlink/mavlink@0133e5d...56a5110

    56a5110d 2022-04-09 Tom Pittenger - Add radius to DO_REPOSITION (PX4#1825)
3b5959bd 2022-04-07 Thomas Debrunner - Option to not reset non-configurable params in preflight storage (PX4#1826)

* ekf2: fix IMU missed perf count when not using multi-EKF

* Tools: add drag fusion tuning script

* boards: px4_fmu-v2_multicopter disable load_mon to save flash

* ekf2: properly reset IMU biases on calibration change (non-multi-EKF)

 - this was working in the multi-EKF case using vehicle_imu, but missing
in sensor_combined

* Update world_magnetic_model to latest Mon Apr 11 11:14:11 UTC 2022 (PX4#19475)

* Update submodule sitl_gazebo to latest Tue Apr 12 12:38:53 UTC 2022

    - sitl_gazebo in PX4/Firmware (bb2ea57): PX4/PX4-SITL_gazebo-classic@25138e8
    - sitl_gazebo current upstream: PX4/PX4-SITL_gazebo-classic@2cf56d0
    - Changes: PX4/PX4-SITL_gazebo-classic@25138e8...2cf56d0

    2cf56d0 2022-03-23 Julian Oes - Revert "models: Add model for standard_vtol_ctrlalloc"
b3ab8de 2022-03-18 JamesAnawati - Update cloudship.sdf.jinja

* Update submodule GPSDrivers to latest Tue Apr 12 12:38:56 UTC 2022

    - GPSDrivers in PX4/Firmware (c04e66a): PX4/PX4-GPSDrivers@ad1094a
    - GPSDrivers current upstream: PX4/PX4-GPSDrivers@ddb1825
    - Changes: PX4/PX4-GPSDrivers@ad1094a...ddb1825

    ddb1825 2022-03-29 Daniel Agar - ubx: print relevent UBX-MON-VER output

Co-authored-by: PX4 BuildBot <bot@px4.io>

* MulticopterPositionControl: remove unused return value parameters_update()

* posix: HRT hrt_lock() sem_wait try again if error returned

* ROMFS: move px4flow start to rc.sensors

* Add ARK CANnode board config

* uORB : Don't automatically include message name as default topic name in uORBTopics source files, to handle case where user doesn't use default messgae name for multi topic definition in .msg file

* navigator: stop handling speed changes via reposition triplet

- the mc and fw controllers are handling the speed changes directly

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FixedWingPositionControl: handle VEHICLE_CMD_DO_CHANGE_SPEED

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* FlightModeManager: handle MAV_CMD_DO_CHANGE_SPEED

- support setting the cruise speed of the auto flight task via command

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* vehicle_command: added enum for speed types

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* addressed review comments

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* vehicle_command: specify what SPEED_TYPEs are for

* FlightModeManager/FixedwingPositionControl: robustify vehicle command parameter casting

* FlightTask: rename and move setCruisingSpeed() -> overrideCruiseSpeed()

* FlightTaskAuto: refactor _commanded_speed_ts -> _time_last_cruise_speed_override

* ekf: extract range finder noise computation

* ekf: add range finder kinematic consistency check

At each new valid range measurement, the time derivative of the distance
to the ground is computed and compared with the estimated velocity.
The average of a normalized innovation squared statistic check is used
to detect a bias in the derivative of distance measurement,
indicating that the distance measurements are kinematically inconsistent
with the filter.

* ekf: access member variable without getter

* ekf: use same gate for innov and innov sequence monitoring

* ekf: improve rng consistency check

To pass from invalid to valid:
- time hysteresis
- some vertical velocity
- test ratio < 1
- low-passed signed test ratio < 1

To pass from valid to invalid:
- low-passed signed test ratio >= 1

* ekf: requires kinematically consistent range finder data to continue terrain aiding

* ekf: use uint64_t for time variables

* ekf: add logging of range finder consistency check

* ekf: add logging for rng kinematic consistency check

* ekf: run rng consistency check only when not horizontally moving

The check assumes a non-moving terrain height

* ekf: make range finder kin consistency gate tunable by parameter

* ekf rng finder consistency: simplify class member names

* ekf rng kin: increase default gate size

The user needs to tune the range finder noise parameters properly and we
shouldn't need such a small gate

* ekf rng kin: reduce minimum rng variance

* sensors/vehicle_imu: replace coning metric with actual integrator coning correction (averaged)

 - this saves a relatively expensive higih rate cross product and gives
better visibility into what's actually happening internally

* sensors/vehicle_imu: Integrator use 1 microsecond for minimum DT

 - this is a more realistic minimum for the system

* Tools/ecl_ekf: fix vibe_metrics usage (moved to vehicle_imu_status instances)

* Update submodule mavlink to latest Wed Apr 13 12:39:05 UTC 2022

    - mavlink in PX4/Firmware (16fd85d): mavlink/mavlink@56a5110
    - mavlink current upstream: mavlink/mavlink@3b52eac
    - Changes: mavlink/mavlink@56a5110...3b52eac

    3b52eac0 2022-04-13 Beat Küng - add COMPONENT_METADATA, deprecate COMPONENT_INFORMATION (PX4#1823)

* lib/rc: Fix DSM2/DSMX guessing routine and DSM range checking (PX4#18270)

* Add Orangerx test case

Co-authored-by: Chris Seto <chris.seto@bossanova.com>

* dronecan beeper: remove unneded var

* px4io cleanup LED and heater handling

 - most px4_io-v2 boards have a blue LED that breathes for status
 - the pixhawk 2.1 (hex) re-used this blue LED for as an IMU heater (active low), but kept the same board id (so we have to detect at runtime)
 - the new cubepilot boards (yellow, orange) inverted the polarity of this heater pin
 - untangle the mess slightly so that things we know statically (eg cubepilot cubeorange LEDs and heater polarity) are handled at build time.

* param-reset: Add option to reset all configurable params, but not the ones that store vehicle information

* commander/safety: set early safety_button_available

* mavlink: add COMPONENT_METADATA message

And still support the previous message COMPONENT_INFORMATION for now.

* drivers/optical_flow: new PixArt PAA3905 optical flow driver

* boards: px4_fmu-v6x_default disable systemcmds/sd_bench to save flash

* boards: px4_fmu_v5x_rtps disable several modules to save flash

* boards: px4_fmu-v5_uacanv0periph disable systecmds/sd_bench to save flash

* boards: px4_fmu-v2_multicopter disable lightware_laser_serial to save flash

* px4iofirmware: convert most files to c++

* boards: px4_fmu-v5/v5x run default & rtps boards through kconfiglib to cleanup

* boards: px4_fmu-v5_stackcheck disable unused systemcmds to save flash

* small cleanup for  FlightTask::_evaluateDistanceToGround if-else

* output modules simplify locking for mixer reset and load

 - fixes the deadlock in px4io ioctl mixer reset
   - px4io Run() locks (CDev semaphore)
   - mixer load goes through px4io ioctl MIXERIOCRESET which calls MixingOutput::resetMixerThreadSafe()
   - MixingOutput::resetMixerThreadSafe() stores a Command::Type::resetMixer command in an atomic variable, schedules a work queue cycle, then sleep spins until the command is cleared
   - the execution of the cycle eventually calls back into PX4IO::updateOutputs(), which tries to lock (and waits forever)

* mission block: fix incorrectly calculated ccw loiter exit (PX4#19487)

* mission block: fix incorrectly calculated ccw loiter exit

* mission block: update comment on orbit exit location

* mission_block: fix vehicle not exiting loiter after reaching exit heading

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* commander: refactor home position setter

- always try to set local or global home position when possible
- set global home with GNSS position if global pos from EKF isn't
  available
- reset home when significantly moved from home before takeoff (checking
  lpos or gpos or GNSS)
- reset home on takeoff transition
- reset home on mavlink arm command
- remove "home required accuracy" parameters, rely on validity flags

* navigator: home_positionvalid -> global_home_position_valid

This is to make clear that the relevant part of the home position
message for navigator is the global one. Local home position isn't
required as everything is done in global coordinates.
The specific home_alt_valid is used when lat/lon are not used

* geofence: remove unused function parameters

* commander: improve set_in_air_position

- set local home using global pos and global home
- set local home using GNSS pos and global home
- set global home using global ref of local frame and local home

* mavsdk_tests: update MAVSDK dependency

This should fix the CI issue where the test just hangs trying to
connect.

* mc wind: rename MC wind estimor tuning script

* Tools: add baro pressure coefficient tuning script

* EKF: move python tuning tools to EKF module

* Update submodule GPSDrivers to latest Thu Apr 21 12:38:20 UTC 2022

    - GPSDrivers in PX4/Firmware (e9c07fa): PX4/PX4-GPSDrivers@ddb1825
    - GPSDrivers current upstream: PX4/PX4-GPSDrivers@6534b05
    - Changes: PX4/PX4-GPSDrivers@ddb1825...6534b05

    6534b05 2022-04-19 Jonas Perolini - ubx: disable gps heading for in RTK float fix type (PX4#104)

Co-authored-by: PX4 BuildBot <bot@px4.io>

* 13004_quad+_tailsitter - outputs mixed up

* Add gyro and accel range register values to the icm42688p driver.

* px4flow allow delayed background startup

* platforms/nuttx: cdc_acm_check implement mavlink reboot directly

* Mission: don't do anything in set_current_mission_index() when index=current already

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* ROMFS: fix typo in convergence and clair configs

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* FlightTaskAuto: fix Weather Vane during landing

Weather vane should only set a yawrate setpoint, but no yaw setpoint.
Setting it to NAN when WV is active makes sure that whatever _yaw_setpoint
is set previously (e.g. through Waypoint) is not used.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* logger: add default ground truth logging for HITL/SITL

* boards: new Sky-Drones AIRLink board support

* icm20948: disable debug output (_debug_enabled=true)

* Set Tune's Volume for Power-Off in Commander to default volume of tune_control message

* mavlink: shell expand locking (PX4#19308)

 - on some H7 boards (cuav x7pro tested) there's an occasional hard fault when starting the mavlink shell that is no longer reproducible with the slightly expanded locking
 - this is likely just changing the timing (holding the sched lock for longer), but this should be harmless for now until we can identify the root cause

* commander: Add prearm check for flight termination

* mavlink: delete Mavlink instance if early startup fails

* commander: mag calibration tolerate fit failure if sensor disabled

* ekf: robustify bad_acc_vertical check

when the vertical pos or vel innov ratio is above the threshold, the
other one needs to be significant too and not just positive to trigger
the failure

* [AUTO COMMIT] update change indication

* systemcmds/param: set-default should mark parameter active to avoid race conditions

* mc_att_control: only apply quat reset if estimate is newer than setpoint

* boards: add new mRo Control Zero Classic

* boards: add new Diatone Mamba F405 mk2

* boards/diatone/mamba-f405-mk2: fix code style

* boards/mro/ctrl-zero-classic: fix code style

* boards: update bootloaders to latest

* boards: px4_fmu-v2 restore systemcmds/ver needed for board init

* boards/diatone/mamba-f405-mk2: disable modules to save flash

* mc_pos_control: silence invalid setpoint warning when disarmed

* boards: px4_fmu-v6x_default disable common barometers to save flash

* Separate takeoff and landing to individual fixed wing states for FW pos control (PX4#19495)

* apply differential pressure calibration (SENS_DPRES_OFF) centrally

 - remove drv_airspeed.h and ioctls

* differential pressure remove filters from drivers and average in sensors/airspeed

* drivers/differential_pressure: remove lib/drivers/airspeed dependency and cleanup

 - split ms4525_airspeed into separate ms4515 and ms4525 drivers

* boards: cubepilot_cubeorange_test disable examples/fake_gps to save flash

* boards: px4_fmu-v5_uavcanv0periph disable modules to save flash

* drivers/rc_input: RC_INPUT_PROTO parameter minimal implementation (PX4#19539)

Co-authored-by: chris1seto <chris12892@gmail.com>

Co-authored-by: chris1seto <chris12892@gmail.com>

* fix dshot: remove setAllFailsafeValues

Fixes a regression from c1e5e66,
where with static mixers the dshot outputs would go to max instead of 0
in a failsafe case.

* dshot: avoid using pwm failsafe params when dynamic mixing is enabled

* ekf2: optimize KHP computation

Calculating K(HP) takes less operations than (KH)P because K and H are
vectors.

Without considering the sparsity optimization:
- KH (24*24 operations) is then a 24x24 matrix an it takes
24^3 operations to multiply it with P. Total: 14400 op

- HP (24*(24+24-1) operations) is a row vector
and it takes 24 operations to left-multiply it by K. Total:1152 op

* commander: fix incorrect return in set_link_loss_nav_state()

If both local position and altitude were not valid, then both RC loss and
datalink loss would not trigger any failsafe at all, independently from
the configured action.

* Commander: ensure diconnected battery is cleared from bit field

* fw pos ctrl: fix state switching logic for takeoff and landing

* fw pos ctrl: add missing guidance control interval setting to control_manual_position()

* fw pos ctrl: turn back to takeoff point with npfg

* uavcan: use timer 6 by default on stm32f7

* log_writer_file: protect access to _should_run, use px4::atomicbool for _exit_thread

* log_writer_file: fix corner case when mission log is enabled

Normally _should_run for mission is only ever true if _should_run for the
normal log is. There are exceptions though:
- the log buffer fails to allocate
- there was a write failure (e.g. due to SD card removal)
In that situation, the writer would not wait anymore but busy-loop.

* drivers/rc_input: ensure RC inversion is disabled initially and latch RC_INPUT_PROTO conservatively

 - this allows jumping straight to a non-SBUS RC protocol
 - increased the scan time per protocol 300->500 ms, which the newer DSM parser seems to need in some cases.
 - only set RC_INPUT_PROTO if we've had a successful RC lock for > 3 seconds

* Fix uavcan battery causing immediate RTL time remaining low

* output drivers: init SmartLock after exit_and_cleanup

This fixes an invalid memory access when exiting the module:
exit_and_cleanup destroys the object, but lock_guard is destructed after
and accesses the lock.

* .vscode/.gitignore ignore all log files

* Commander: ignore MAV_CMD_REQUEST_MSG

This commit adds the MAV_CMD_REQUEST_MESSAGE to the list of vehicle
commands which are ignored without generating a warning sound.

* HITL: undefined time_remaining_s should be NAN

* ROMFS: disable UAVCAN in HITL

Without this, uavcan creates MixingOutput classes which then create
empty actuator_outputs publications. This then prevents the motor
output in HITL to be forwarded to the simulator via mavlink.

* sensors/vehicle_magnetometer: fix multi_mode check

Signed-off-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>

* ekf2: use explicit flags instead of bitmask position

This prevents bitmask mismatch when a new flag is inserted

* icm42688p: only check configured registers periodically (as intended)

* commander: lockdown is not termination

We use lockdown to prevent outputs like motors and servos from being
active in HITL simulation. This means that we can't treat the lockdown
flag as a flight_terminated, otherwise we can't arm in HITL at all.

* sitl_gazebo: update submodule

This fixes the issue where HITL doesn't connect over USB.

* ekf2_post-processing: use estimator_status_flags instead of bitmasks

* boards: new px4_fmu-v6c board support (PX4#19544)

* px4_fmu-v6c:Fix mag rotation

* boards: STM32H7 pad to 256 bit - 32 bytes (PX4#19724)

* flash_cache:Flush complete cache line

* Update all H7 Bootloders

* boards: pixhawk 2 cube skip starting low quality l3gd20 gyro to save memory and cpu

 - free memory is getting tight on these older boards (depending on
configuratoin) and the pixhawk 2 cube still has 2 other superior IMUs, so this is just
dropping dead weight

* boards: reduce SPI DMA buffers on older STM32F4 boards

 - on common IMUs like the mpu6000, mpu9250, icm20602, etc each FIFO
sample is only 12 bytes so this is still more than large enough for the
worst case transfer

* update NuttX and apps to latest with sem holder fixes and updated ostest

* boards: NuttX update all boards to preallocated sem holder list

 - CONFIG_SEM_PREALLOCHOLDERS=32
 - CONFIG_SEM_NNESTPRIO=16 (default)

* sensors/vehicle_imu: don't bother checking IMU_GYRO_RATEMAX

* sensors/vehicle_angular_velocity: add IMU_GYRO_RATEMAX constraints

* uavcannode: Fix dronecan baro units

* px4_fmu-v6x:Add Rev 4 Sensors

* px4_fmu-v5X:Added Holybro mini base board

* uc_stm32h7_can:Correct initalization of the mumber of interfaces

   H7 Only supports 2 not 3 CAN interfaces.

   CanInitHelper passes in in the run time configuration of
   the number of interfaces. The code was ignoring these!

* px4_fmu-v6X:Added Holybro mini base board

* differential_pressure/sdp3x: sdp3x_main fix 'keep running' regression

* uavcan: GNSS optionally publish RTCMStream or MovingBaselineData

* ekf2: do not run rng kinematic consistency check for fixed-wings

As they are always moving horizontally, the check doesn't make sense
for fixed-wings.
Also don't run the check while on ground to prevent getting a failed
check during pre-takeoff manipulation.

* gps_inject_data: fixed integer overflow
- array length of data was increased without changing the data type of
the variable holding the length

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* adis16470: fix accel and gyro scaling

* drivers/imu/invensense/icm42670p: cleanup and small fixes

* icm42670p run at full speed

* px4_fmu-v6x:HB Mini add Ver 3, Ver 4 init

* standard: fixed pusher assist in hover

- in hover mode the pusher assist is already set in update_mc_state()

Signed-off-by: RomanBapst <bapstroman@gmail.com>

* boards: V1.13.0 ARKV6X Backport (PX4#20253)

* fmu-v6c: internal baro and mag on external bus

This swaps the internal baro and mag back to the bus which is both
internal an external but configured as external for this case.

* px4_fmu-v6c: Move I2C 4 back to External

This is a revert of the revert.
This reverts commit 1080855.

* [DO NOT MERGE] px4_i2c_device_external hacks

* Add PX4 vision v1.5 Airframe

* boards: save some flash on CubeOrange test config

* Add BMP390 to BMP388 driver

* boards/platform: remove confusing override

This removes the odd px4_i2c_bus_external override which was confusing
me and lead to odd and inconsistent results.

The function is now only available with an int as the argument.

* cuav_x7pro: save flash on test target

* Add Holybro Pixhawk Pi CM4 Baseboard Support

* Fix Error on board_config.h Define on FMUv5X & FMUv6X

* Fix Error on manifest.c

* Correct BOARD_NUM_SPI_CFG_HW_VERSIONS at board_config.h

* update v5x rc.board_sensors

add V5X004002

* update fmuv5x rc.board_sensors with V5X004000

* update fmu-v6x rc.board_sensors, add V6X004003

* platforms: decrease flash usage by type for bus id

My assumption is that the bus are numbered < 127.
This saves about 100 bytes of flash.

* fmu-v5x: disable rev0 for HB Mini and CM4 base

As I understand it, only Rev 1 and Rev 2 were shipped by HB, and likely
to be used for the Mini and CM4.

* fmu-v6: disable Rev 0, Rev 1 for HB Mini, and CM4

As I understand it, only Rev 3 and Rev 4 were shipped by HB for Mini and
CM4, and are likely to be used for it.

It would be nice to have all combinations but it requires quite some
flash in the current implementation.

* fmu-v5x: alias for duplicates, remove commented

- Removed commented out config data.
- hw_mft_list_v0540 was the same as hw_mft_list_v0500

* fmu-v6x: alias, add VX43

- alias from hw_mft_list_v0650 to hw_mft_list_v0600 as it is the same
- add V6X50 again

* fmu-v3: make optional sensor startup quiet

This fixes the errors showing up at startup for me with a Black Cube.

* fmu-v5x: support for mini base board

This was forgotton with all the merges and shuffling previously.

* libuavcan: update submodule

This fixes a Python 3.10 issue for me.

* uavcan: fix RTCM corrections publication

Before this fix, this function would stall and somehow never return.

* Mag: fix estimated bias save to calibration

Clear the calibration at the end only otherwise everything gets erased
at the end of the first iteration of the outer loop

* Fix ARKV6X control allocator with base boards

* ms5611: ignore reading 0

This prevents publishing a negative pressure which leads to a NAN
altitude estimate further down the line.

* vehicle_imu: only reset raw accel/gyro Welford mean periodically

 - vehicle_imu_status can publish immediately on any measured sample
rate change or sensor error increment, but the windowed mean/variance
shouldn't necessarily reset that often

* welford mean: convert to matrix only template

* welford mean: protect against negative variances

* Navigator: initialize _mission_item for all navigation modes in Navigator::Navigator()

This fixes the issue where the init happended in the initializer list, at which point
the params were not yet initialized and thus resulted in random values for the init
values of _mission_item.loiter_radius and .acceptance_radius.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

* Holybro PX4Vision: Don't use IO

* px4_fmu-v6:Add Revision 1 to manifest to note I2C4 is only internal

* vehicle_magnetometer: fix standalone mag bias est factored into in flight mag cal

 - preflight mag bias estimate is in the vehicle body frame and removed from the raw mag data after it's rotated, calibrated, etc
 - in flight mag bias (from ekf2) is in the vehicle body frame, but stored as a sensor frame offset immediately

* boards: px4_fmu-v6x_default include systemcmds/gpio

* mission_block: minimal acceptance radius of 1mm

to avoid float rounding errors leading to tiny acceptance radii
getting considered

* mission_block: explicitly (re)set the acceptance radius to default for takeoff items

otherwise a previously adjusted or uninitialized radius from the last flight
can cause problems during the new takeoff

* ekf2: do not fuse ZVU if other velocity source is active

* fmu-v2: make optional sensor startup quiet

Signed-off-by: Julian Oes <julian@oes.ch>

Signed-off-by: RomanBapst <bapstroman@gmail.com>
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Signed-off-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>
Signed-off-by: Julian Oes <julian@oes.ch>
Co-authored-by: Daniel Agar <daniel@agar.ca>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
Co-authored-by: RomanBapst <bapstroman@gmail.com>
Co-authored-by: CUAVmengxiao <mengxiao@cuav.net>
Co-authored-by: Kabir Mohammed <kabir@corvus-robotics.com>
Co-authored-by: Alex Mikhalev <alex@corvus-robotics.com>
Co-authored-by: Alex Mikhalev <alexmikhalevalex@gmail.com>
Co-authored-by: David Sidrane <David.Sidrane@NscDg.com>
Co-authored-by: stmoon <munhoney@gmail.com>
Co-authored-by: alessandro <3762382+potaito@users.noreply.github.com>
Co-authored-by: Beat Küng <beat-kueng@gmx.net>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Hovergames <loic.fernau@googlemail.com>
Co-authored-by: Alessandro Simovic <alessandro@auterion.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
Co-authored-by: Igor Misic <igy1000mb@gmail.com>
Co-authored-by: Thomas Stastny <thomas.stastny@auterion.com>
Co-authored-by: Julian Oes <julian@oes.ch>
Co-authored-by: Ramon Roche <mrpollo@gmail.com>
Co-authored-by: Ryan Johnston <31726584+ryanjAA@users.noreply.github.com>
Co-authored-by: alexklimaj <alex@arkelectron.com>
Co-authored-by: Nicolas MARTIN <59083163+NicolasM0@users.noreply.github.com>
Co-authored-by: PX4 BuildBot <bot@px4.io>
Co-authored-by: bresch <brescianimathieu@gmail.com>
Co-authored-by: Junwoo Hwang <junwoo@auterion.com>
Co-authored-by: chris1seto <chris12892@gmail.com>
Co-authored-by: Chris Seto <chris.seto@bossanova.com>
Co-authored-by: Thomas Debrunner <thomas.debrunner@auterion.com>
Co-authored-by: bazooka joe <bazookajoe1900@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
Co-authored-by: mcsauder <mcsauder@gmail.com>
Co-authored-by: Kirill Shilov <aviaks.kirill@gmail.com>
Co-authored-by: bresch <bresch@users.noreply.github.com>
Co-authored-by: achim <lumserei@web.de>
Co-authored-by: JaeyoungLim <jalim@ethz.ch>
Co-authored-by: Nico van Duijn <nico@auterion.com>
Co-authored-by: Nicolas MARTIN <nicolas1.martin1@gmail.com>
Co-authored-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>
Co-authored-by: vincentpoont2 <vincentpoont2@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants