Compare commits

...

181 Commits

Author SHA1 Message Date
Matthias Grob b5f6699f2e mixer_module: send a last sample out after all outputs were disabled
This matters for PWM when the last output gets disabled on either FMU or IO
it would just keep on running.

Also when rebooting with a parameters reset or new airframe with no mapped outputs
it would previously keep outputting PWM with the disarmed value of the new airframe
e.g. 1000us which is a safety hazard because servos could break the physical limit of the
model or miscalibrated ESCs spinning motors.
2024-03-25 19:21:54 +01:00
Matthias Grob 1096384a38 px4iofirmware: don't switch to disarmed or failsafe value on disabled PWM outputs
If the output is set to 0 then the FMU had this channel disabled/no function mapped
to it. In that case we do not want to suddenly start outputing failsafe or disarmed
signals.
2024-03-25 19:21:54 +01:00
Matthias Grob 999a71c4dd px4io: don't output on disabled PWM pins
Same logic as on the FMU PWM updateOutputs() in PWMOut.cpp
2024-03-25 19:21:54 +01:00
Thomas Frans bcbae86b9f code: add more style options in `.editorconfig` 2024-03-25 09:48:09 -04:00
Eric Katzfey 4a553938fb
VOXL2: HRT updates for synchronization with Qurt time (#22881)
- Added offset to Posix hrt time to account for synchronization with Qurt hrt time
- Added new Kconfig to configure synchronization of HRT timestamps on VOXL2
- Moved voxl2 libfc sensor library submodule from muorb module to boards directory
- Added check to make sure hrt_elapsed_time can never be negative
2024-03-22 15:24:51 -04:00
Daniel Agar c024ea396a boards/px4/fmu-v5x: remove legacy rover_pos_control to reduce flash usage 2024-03-22 15:17:03 -04:00
Eric Katzfey 69028f37a9
New platform independent Serial interface (#21723) 2024-03-21 21:00:23 -04:00
Thomas Frans bb9f4d42f3 gps: fix incorrect task id in module startup 2024-03-21 20:58:59 -04:00
Beat Küng 2e12e14a23 boards/px4/fmu-v5x: remove sd_stress & reflect to reduce flash usage 2024-03-21 20:58:21 -04:00
Thomas Frans d0251b8688
add `.editorconfig` for consistent code style across editors (#22916)
EditorConfig is a well-known convention to share style settings across
different editors. Adding one will make it easier for new contributors
or people who like to use a different editor to contribute.
2024-03-21 20:56:20 -04:00
Eric Katzfey 82a1aa37db
uORB: fix for uORB communicator, only send most recent data for new subscription (#22893) 2024-03-21 20:54:43 -04:00
Eric Katzfey 5f6dc1c5d0
uORB: SubscriptionBlocking purged the broken attempt to set the mutex protocol in constructor 2024-03-21 20:53:34 -04:00
Øyvind Taksdal Stubhaug 710286da72
uavcan: publish new can interface status as uorb topic (#22873) 2024-03-20 12:38:47 -04:00
Daniel Agar 34c19b2e5a
boards/px4/fmu-v5x: default remove systemcmds/sd_stress to save flash 2024-03-20 12:35:34 -04:00
alexklimaj af16544809 boards: ark septentrio update flash size and enable ekf2 2024-03-20 11:17:05 -04:00
Daniel Agar 35532609c9
mathlib: utilities refactor float to function template (for optional double precision usage)
Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>
2024-03-20 11:10:37 -04:00
bresch 62b8db153b mpc: fix PositionControl unit test
The unit test assumes the position controller is in "decoupled" mode
2024-03-20 14:16:12 +01:00
bresch 638e17d551 ekf: update change indicator 2024-03-20 14:13:49 +01:00
bresch 6d819343aa ekf2: fix direct state measurement update for suboptimal K case
The duration of a unit test had to be increased because the incorrect
covariance matrix update, was making the unit test passing faster
(over-optimistic variance).
2024-03-20 14:13:49 +01:00
bresch cb2bb2e098 ekf2: add no gyro bias estimate test case
This makes the ekf unstable and creates NANs during initialization
2024-03-20 14:13:49 +01:00
bresch c9221b91ad ekf2: fix gnss yaw unit test 2024-03-20 14:04:19 +01:00
Drone-Lab 37caddedbb
navigator: update mission after changing home position (#22834) 2024-03-20 08:37:19 +01:00
muramura 63850873eb sd_bench: Display maximum time for maximum write time 2024-03-20 08:35:33 +01:00
Hamish Willee 95627ea098
SMART_BATTERY_INFO to BATTERY_INFO (#22875)
* Update submodule mavlink to latest Wed Mar 13 01:02:16 UTC 2024

    - mavlink in PX4/Firmware (497327e916103ef05ff8f08f47d33b9a19bc28d7): c4a5c49737
    - mavlink current upstream: a3558d6b33
    - Changes: c4a5c49737...a3558d6b33

    a3558d6b 2024-03-07 Hamish Willee - common - DO_FENCE_ENABLE/PARACHUTE fix (#2090)
b9730e0f 2024-03-06 olliw42 - update RADIO_RC_CHANNELS to latest, remove all mlrs from storm32.xml (#1919)
7fed0268 2024-03-06 Patrick José Pereira - common: MAV_CMD_DO_SET_SYS_CMP_ID: Add first version (#2082)
2909b481 2024-03-06 Hamish Willee - Update Pymavlink (#2089)
e9b532a9 2024-03-05 Randy Mackay - common: add set-camera-source command (#2079)
bcdbeb7f 2024-03-01 auturgy - Allow individual fences to be enabled and disabled (#2085)
2f8403d1 2024-02-29 Hamish Willee - MAV_CMD_ODID_SET_EMERGENCY -  (#2086)
daa59c02 2024-02-22 Peter Barker - common.xml: add a command to deal with safety switch (#2081)
977332e2 2024-02-14 Hamish Willee - COMPONENT_INFORMATION_BASIC - add manufacturer date (#2078)
4fef7de2 2024-02-07 Randy Mackay - Common: rename SMART_BATTERY_INFO to BATTERY_INFO and add SOH (#2070)
3865b311 2024-02-01 Hamish Willee - FLIGHT_INFORMATION - description to match PX4 (#2067)
f80e6818 2024-01-31 KonradRudin - development.xml: merge both MAV_CMD enums together (#2074)

* SMART_BATTERY_INFO to BATTERY_INFO on new mavlink module

* Update src/modules/mavlink/streams/BATTERY_INFO.hpp

* fix trivial whitespace

---------

Co-authored-by: PX4 BuildBot <bot@px4.io>
Co-authored-by: Daniel Agar <daniel@agar.ca>
2024-03-20 11:33:37 +13:00
bresch 2e6dd243af mpc: add possibility to generate tilt using full 3D accel
Using full 3D acceleration provides better horizontal acceleration
tracking but also creates a sometimes unwanted behavior because the tilt
is directly coupled with the vertical acceleration setpoint.
2024-03-19 12:29:34 -04:00
muramura 32aa3263a6 EKF: Change a typo 2024-03-18 10:29:44 +01:00
Beat Küng 70346a5b2f failsafe: set cause to generic when fallback mode is activated
Previously when triggering low battery RTL and then losing GPS, the fallback
to Descend would still have low battery as cause.
2024-03-15 14:06:58 +01:00
Silvan Fuhrer da39d075ac Commander enums: shorten failsafe event messages
Such that the focus is on the important keywords.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-15 10:25:18 +01:00
Silvan Fuhrer f6430a27d6 Commander enums: capitalize battery level key words
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-15 10:25:18 +01:00
Silvan Fuhrer 4ae2fbd171 Commander enums: capitalize flight mode names
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-15 10:25:18 +01:00
Silvan Fuhrer 6e15dd5328 Commander: trigger warning when arming denied due to check failure
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-15 09:34:11 +01:00
Silvan Fuhrer d330d47495 EstimatorCheck: GNSS data fusion stopped as INFO if local position is already invalid
Helps to reduce spamming of less important warnings.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-15 09:33:56 +01:00
Silvan Fuhrer 6d8273483c Commander: set vehicle_status.failsafe flag only if action for failed check is more than warning
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-15 09:33:41 +01:00
bresch 7c6ecd95a8 mc_wind_estimator_tuning: optionally use GNSS velocity
Sometimes GNSS is logged but not used
2024-03-14 21:06:24 +01:00
bresch b2f1122372 ekf2: remove old yaw 321 and 312 derivations 2024-03-14 11:33:45 -04:00
bresch ee63f3e664 update change indicator 2024-03-14 11:33:45 -04:00
bresch e3f67d5c1a ekf2: new yaw derivation
Instead of euler angles, compute measurement jacobian using a small
global perturbation around the vertical axis
2024-03-14 11:33:45 -04:00
Silvan Fuhrer 6373d8d243
commander: low flight time failsafe: set UserTakeoverAllowed::Auto to enter Hold first (#22887)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-14 16:30:06 +01:00
Silvan Fuhrer 68fcfc43ef
mro-zero classic: remove gyro fft module to save flash (#22878)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-14 14:20:51 +01:00
Eric Katzfey ae947513d7
add load_mon support for Qurt platform (#22883)
- Added check in commander to see if CPU load monitoring has been disabled before signalling overload
2024-03-13 21:33:58 -04:00
Alexander Lampalzer ad50afda10
update msg_files to PARENT_SCOPE (#22800) 2024-03-13 09:34:47 +01:00
Niklas Hauser 23c5c0b12d dataman: Add client sync perf counter and increase default timeout to 5s 2024-03-13 09:22:38 +01:00
Daniel Agar a1cce7e961 uxrce_dds_client: optimizations and instrumentation
- skip ping session if data flowing bidirectionally
 - add perf counters for loop time and interval
 - skip blocking poll if there's input data to read
2024-03-12 16:22:26 -04:00
Daniel Agar b115d3cd44 uxrce_dds_client: refactor init to retry indefinitely
- move init from UxrceddsClient to init() method so that retry is
   possible for both serial and UDP init
2024-03-12 16:22:26 -04:00
Silvan Fuhrer 9f4ae0a85d
vtol: only publish generic warning through mavlink to safe flash (#22870)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-12 13:27:13 +01:00
Silvan Fuhrer 00cc68baa1
Commander: make low remaining flight time configurable and fix clearing condition (#22863)
* Commander: make low remaining flight time configurable and do not clear

- add _ACT param to disable/warning/RTL this feature
- publish rtl flight time estimate also in RTL, and thus fix re-validation
- make failure message clearer, distinguish from battery low

* battery check: add hysteresis for declaring battery_low_remaining_time false again


---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: KonradRudin <98741601+KonradRudin@users.noreply.github.com>
2024-03-12 12:56:01 +01:00
Silvan Fuhrer 7fe5ee64fe rtl_direct: fix on_inactive()
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-12 11:27:49 +01:00
Silvan Fuhrer 7f370ac6df Tiltrotor: disable MC yaw fade out during front transition blending
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-12 11:26:07 +01:00
Thomas Stauber 1ad83a8002
mavlink: OPEN_DRONE_ID_SYSTEM stream publish operator altitude in geodetic frame (#22866) 2024-03-11 19:49:11 -04:00
Eric Katzfey 18d53c3bfd
boards/modalai/voxl2: Add new capabilities to Qurt platform HITL driver
* Added new sensor control options and test capability in dsp_hitl
* HITL working in VIO mode only
* Fixed units on GPS HIL input
2024-03-11 19:47:15 -04:00
Eric Katzfey f4ebfa6130
parameters: support for an optional remote parameter database (#22836)
The voxl2 has a split architecture. PX4 runs on a posix platform and a Qurt platform. The two communicate uorb topics back and forth with the muorb module. But each has it's own parameters database and they need to stay in sync with each other. This PR adds support to keep the 2 parameter databases in sync. The main parameters database running on Linux has file system support while the Qurt one does not. The Linux side is considered the primary and the Qurt side is considered the remote.
2024-03-11 13:52:22 -04:00
Konrad c5fde63440 mission: The mission check on activation should only be performed for a mission, not RTL.
We need to make sure that when the RTL is triggered, it should not reevaluate it, as when it was valid but evaluated to false on activation, it can't do a RTL.
2024-03-11 17:08:56 +01:00
Silvan Fuhrer cb8520427c rtl direct: fix setting of previous altitude (abs vs rel)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-11 14:04:02 +01:00
Silvan Fuhrer 86c074378f rtl_direct_mission_land: fix abs/rel usage of item.altitude
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-11 14:04:02 +01:00
Konrad e9fda548fa rtl_direct: Rtl estimate only needs valid destination, not home position 2024-03-11 14:04:02 +01:00
Konrad 91d1342f20 rtl_direct_mission: rtl_estimate for fixedwing land is diagonal 2024-03-11 14:04:02 +01:00
Konrad 89844625b4 rtl: reduce time estimate calculations 2024-03-11 14:04:02 +01:00
Konrad fde71cd15e rtl_direct_mission_land: add time estimation for RTL mission land 2024-03-11 14:04:02 +01:00
Konrad 14e4169473 rtl_direct: Move the time estimation calculation into a separate helper class 2024-03-11 14:04:02 +01:00
KonradRudin 8dcfcf5b9e
mission_base: land_start_item invalid only when negative. (#22856)
rtl: land_start_item invalid only when negative.

Update src/modules/navigator/rtl.cpp
2024-03-11 09:46:16 +01:00
alexklimaj a80a5a92f4 boards: ARK Flow fix typo 2024-03-09 16:40:59 -05:00
alexklimaj b81ad8841e drivers: broadcom AFBR update to API 1.5.6 2024-03-09 16:40:59 -05:00
Eric Katzfey 57df7e35b2
uORB: make queue size (ORB_QUEUE_LENGTH) completely static (#22815)
Previously uORB queue size was an awkward mix of runtime configurable (at advertise or IOCTL before allocate), but effectively static with all queue size settings (outside of test code) actually coming from the topic declaration (presently ORB_QUEUE_LENGTH in the .msg). This change finally resolves the inconsistency making the queue size fully static.

Additionally there were some corner cases that the muorb and orb communicator implementation were not correctly handling. This PR provides fixes for those issues. Also correctly sets remote queue lengths now based on the topic definitions.

* Made setting of uORB topic queue size in based on topic definition only
* Fixes to the ModalAI muorb implementation
* Removed libfc sensor from format checks
* msg/TransponderReport.msg ORB_QUEUE_LENGTH 8->16 (was set to higher in AdsbConflict.h

---------

Co-authored-by: Eric Katzfey <eric.katzfey@modalai.com>
Co-authored-by: Daniel Agar <daniel@agar.ca>
2024-03-08 16:28:24 -05:00
Alexis Guijarro 006dcfafb7
boards/mro/ctrl-zero-classic: corrections for mRo Control Zero Classic Board (#22745)
- Build target changed from STM32H743II to STM32H743ZI
- Missing external SPI interface added
- Nonexistent  I2C3 interface removed
- I2C4 pins changed
- Red and Green LED lights remapped
- Missing ADC inputs added and already present ones corrected
- CAN Silent interfaces corrected
- Power pins corrected and Level Shifter pin added to enable ICM20948
- Buzzer pin remapped
- HRT channel and PPM pin changed
- RSSI input remapped
- ICM20602 and BMI088 pins corrected
- Serial ports remapped
2024-03-08 14:50:53 -05:00
Silvan Fuhrer 85a882e1ce
FW Position Control: control_backtransition(): always track line from start (#22853)
Remove option to track from previous wp to reduce complexity and fix
case where prev=current point and the line following broke down.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-08 19:11:08 +01:00
Konrad 04099ed483 mission_base: Force mission validity check on activation 2024-03-08 17:26:04 +01:00
Konrad 1aa26a5a91 missionFeasibilityChecker: Fix tests 2024-03-08 17:26:04 +01:00
Konrad acd750e033 mission_base: Run feasibility checker only after first global position has been published 2024-03-08 17:26:04 +01:00
Konrad 6c6142ba79 MissionFeasibiltyChecker: Do not delete uorb data on reset. 2024-03-08 17:26:04 +01:00
Konrad 7fb584adbe MissionResult uorb: fix wrong int types 2024-03-08 17:26:04 +01:00
Konrad fb3aab1fb0 mission_base: check mission feasibility again, if geofence has changed. 2024-03-08 17:26:04 +01:00
Konrad 1b03ac4d2b mission_base: Only run mission feasibility if the geofence module is ready 2024-03-08 17:26:04 +01:00
Konrad 815cea2abb geofence: publish status of loaded geofence 2024-03-08 17:26:04 +01:00
Konrad 51321c605e mission_base: clean up mission check evaluation 2024-03-08 17:26:04 +01:00
Konrad a0ae073d8c mission_base: Do not initialize mission from dataman. only listen on mission topic 2024-03-08 17:26:04 +01:00
Silvan Fuhrer 7884e0a3f7
Navigator: remove vtol_takeoff special handling for RTL (#22844)
We had a special handling for RTL triggered in vtol_takeoff state.
The idea is to wait until the VTOL Takeoff is completed and only
then switch to RTL. On a second thought this special handling isn't
really necessary and for the sake of simplicity should be removed.
This also removes the side effect of the indicated flight mode
after RTL being set to VTOL_Takeoff again.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-08 11:40:14 +01:00
Silvan Fuhrer f799141a19
FW Pos Controller: do not publish roll angle constrained warning if landed (#22850)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-08 09:40:43 +01:00
Eric Katzfey e20215087f Moving from Qurt specific icm4266p driver to mainline version 2024-03-07 21:14:49 -05:00
bresch 0d0978b3b9 ekf2: update change indicator 2024-03-07 11:06:31 -05:00
bresch 0639f5370c ekf2: fix mag and wind covariance prediction 2024-03-07 11:06:31 -05:00
bresch 2bacb4b65d ekf2: update change indicator 2024-03-07 15:11:47 +01:00
bresch 421f13e4b5 ekf2: fix joseph covariance update for Schmidt-Kalman filter
If part of the Kalman gain is zeroed, the first step of the joseph
update does not produce a symmetrical matrix.
2024-03-07 15:11:47 +01:00
Silvan Fuhrer 1e253a9626
VTOL: treat Descend mode as Land (#22843)
* vtol_type: enable pusher assist also in Descend mode

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

* vtol_type: treat Descend as Land for pusher assist

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

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-07 10:22:25 +01:00
cuav-liu1 bb5efa5577 ICP201: increase config delay 2024-03-06 21:20:51 -05:00
Daniel Agar 1c741836c0 sensors/vehicle_imu: sensor update loop limit iterations 2024-03-06 21:19:38 -05:00
Daniel Agar 8b6c70e0f2 sensors/vehicle_angular_velocity: sensor update loop limit iterations 2024-03-06 21:19:38 -05:00
Daniel Agar 1fc38aab92 sensors/vehicle_air_data: sensor update loop limit iterations 2024-03-06 21:19:38 -05:00
Daniel Agar 2bf1eeb003 sensors/vehicle_acceleration: sensor update loop limit iterations 2024-03-06 21:19:38 -05:00
Daniel Agar 87960c04d8 mag_bias_estimator: sensor update loop limit iterations 2024-03-06 21:19:38 -05:00
Daniel Agar d96970a2b9 sensor/vehicle_magnetometer: sensor update loop limit iterations
- place upper bound to prevent looping indefinitely (high publish rate, etc)
2024-03-06 21:19:38 -05:00
Silvan Fuhrer c5835a48de
FW Position Controller: do not publish roll angle constrain warning in VTOL transition (#22842)
* FW Position Control: some cosmetical changes

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

* FW Position Control: disable roll constraining warning in VTOL transition

In transitions it is expected that the roll is constrained, and
instead of defining an aribitrary threshold let's rather disable
the user warning in that case.

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

* FW Pos C: define magic numbers for roll constraining warning as constants

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

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-03-06 15:51:54 +01:00
bresch 6f9a378247 yaw_est: force set gyro bias when at rest
The gyro bias estimate from EFK2 is really good when at rest and should
be used by the yaw estimator to prevent heading drifts due to poor
heading observability.
2024-03-05 14:00:06 -05:00
PX4 BuildBot 67e68783cf Update submodule gz to latest Tue Mar 5 12:39:22 UTC 2024
- gz in PX4/Firmware (5f8f0213a807d327a30a7df05e58f7887cf936ab): 2228336568
    - gz current upstream: 6b4ed09d1b
    - Changes: 2228336568...6b4ed09d1b

    6b4ed09 2024-02-23 Sergei Grichine - Added IMU sensor noise to the model, to avoid STALE messages (#34)
953e02b 2024-02-22 frede791 - add imu sensor model noise
2024-03-05 13:59:22 -05:00
Peter van der Perk d1ae242a91 v6x-rt: fix rover build regression 2024-03-05 10:21:41 -05:00
Peter van der Perk 9cef834624 fmu-v6xrt: update px4board enables vtol 2024-03-05 08:18:44 -05:00
Sihyun Noh 23a41299fa
mag calibration: minor cleanup (#22830) 2024-03-05 09:00:08 +01:00
Niklas Hauser 0186d687b2 Add minimal Skynode RC13 config to the PAB manifest 2024-03-04 13:33:20 -05:00
Peter van der Perk d28653b605 nuttx: update apps 2024-03-04 13:32:36 -05:00
Peter van der Perk 87d79aeb75 netman: generate default config if file doesn't exist
ENOENT returns if the file doesn't exist yet, when using mtd /fs/mtd_net always exist.
On a filesystem you've to generate the file so if ENOENT returns we've to regenerate the default config as well.
2024-03-04 13:32:36 -05:00
Matthias Grob 1bd65f8beb mantis-edu: remove duplicate RC define 2024-03-04 13:30:28 -05:00
Peter van der Perk e0b49afe81 bmp388: Driver print out start BMP390 identifier when detected 2024-03-04 02:05:37 -05:00
Don Gagne f02b44bec5 Update to latest sitl gazebo camera 2024-03-03 12:18:02 +13:00
bresch 28db3e1c8c ekf2: update change indicator 2024-02-27 12:33:43 -05:00
bresch e9d43015ce ekf2: fix unit tests failing due to mag fusion changes 2024-02-27 12:33:43 -05:00
bresch b46fc9a67d ekf2 sensor_sim: set correct world mag field 2024-02-27 12:33:43 -05:00
bresch b80f15f7b5 ekf2-mag_auto: always use mag 3D after takeoff 2024-02-27 12:33:43 -05:00
Silvan Fuhrer 086656dc7f
FW Attitude Controller: fix manual yaw rate setpoint limit (#22812)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-02-27 17:23:13 +01:00
bresch 051baec9c4 ekf2: allow wind dead-reckoning after manual position reset
Reset velocity using airspeed and start navigating
2024-02-27 09:34:05 -05:00
Matthias Grob 2491548a0f Jenkinsfile: correct typo, missing comma
Introduced in
2c81c9fdea
2024-02-27 13:32:46 +01:00
DanielePettenuzzo 18f96c16ce fix gimbal driver for mavlink gimbal v2 input and AUX output
The main problem was that during initial negotiation the client would
request the gimbal_manager_information from px4 but px4 would never send
it because in this configuration the device_compid was set to 0.
2024-02-27 09:53:25 +01:00
GuillaumeLaine 63495ddac3 geo: correct unit test 2024-02-27 09:05:39 +01:00
Peter van der Perk efbbd64ec0 fmu-v6xrt: Increase lpwork stack size 2024-02-26 14:01:56 -05:00
Daniel Agar 8001132d33 ekf2: ZeroGyroUpdate move to fuseDirectStateMeasurement 2024-02-26 12:32:59 -05:00
bresch 08a2a6c836 update EKF2 change indicator 2024-02-26 12:32:59 -05:00
bresch d501d8e1d4 ekf2: use Joseph stabilized update in direct state observations 2024-02-26 12:32:59 -05:00
bresch 9d9766c6cf ekf2: use Joseph stabilized covariance update 2024-02-26 12:32:59 -05:00
makekam d988005216 Update injectxmlparams.py
Add extraction of Boolean attributes in injectxmlparams.py.
2024-02-23 11:06:05 -05:00
Silvan Fuhrer 5dfdf8c071 matrix: remove bold printing of diagonal elements
As this was not working in NSH.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-02-23 11:05:09 -05:00
Daniel Agar b2b7439060
ROMFS: respect kconfig for including romfs files (airframes, etc) (#22571)
* ROMFS: respect kconfig for including romfs files (airframes, etc)

* ROMFS: only add R1 airframe with differential drive control

* ROMFS: adapt to differential drive module renaming

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
2024-02-23 15:40:00 +01:00
bresch 37a40d3fc2 baro static pressure compensation tuning: remove dependency to baro bias
`estimator_baro_bias` requires to have GNSS and baro hgt active and GNSS as the reference. This is quite restrictive. Instead, we can simply use a high-passed version of the baro error.
2024-02-23 10:07:13 +01:00
Sergei Grichine b405d75553
Added Zero Turn Lawnmower model (#22717)
* Added Lawnmower airframe

* Update 5005_gz_lawnmower

Works all right

* Update 5005_gz_lawnmower

RDD_WHL_SPEED has new name: RDD_WHEEL_SPEED

* Update ROMFS/px4fmu_common/init.d-posix/airframes/5005_gz_lawnmower

Co-authored-by: Per Frivik <94360401+PerFrivik@users.noreply.github.com>

* Update ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

Co-authored-by: Per Frivik <94360401+PerFrivik@users.noreply.github.com>

* Renamed 5005_gz_lawnmower to 4011_gz_lawnmower

also pulled latest GZ models hash

---------

Co-authored-by: Per Frivik <94360401+PerFrivik@users.noreply.github.com>
2024-02-22 15:30:12 +01:00
Matthias Grob 4e3bd4f196 MAVSDK tests: shorten Position, Altitude control flights
We get more than 5 meter away much quicker.
2024-02-22 15:29:13 +01:00
Matthias Grob 0cc4b41a51 MAVSDK test: Fix fly_forward_in_altctl() timing 2024-02-22 15:29:13 +01:00
Matthias Grob f602228048 MAVSDK test: increase offboard position threshold
This is a workaround to hotfix CI but the root cause is #22792
(MAVSDK test failing after EKF change, accelerometer simulation issues not learned anymore?)
2024-02-22 15:29:13 +01:00
PerFrivik 9b122adae4 Fix fly_forward_in_posctl() timing 2024-02-22 15:29:13 +01:00
Eric Katzfey 1ec0ba4736 Added param system command to voxl2 slpi build 2024-02-21 11:54:02 -05:00
Eric Katzfey 8da8b88a54 Fixed and added Qurt platform dsp_hitl driver 2024-02-21 11:54:02 -05:00
Eric Katzfey be08c57a0a Changed order of service startup in SLPI DSP muorb since parameter library now needs work queues 2024-02-21 11:54:02 -05:00
Eric Katzfey a436a8f3b8 Fixed unresolved symbol error for qurt platform due to missing sbus library 2024-02-21 11:54:02 -05:00
Eric Katzfey 5ad0e68d8e Fix build error for Qurt platform in pab_manifest.c 2024-02-21 11:54:02 -05:00
Eric Katzfey f07eeaa776 Added special muorb startup ordering in px4_init for posix platform 2024-02-21 11:52:24 -05:00
Daniel Agar 506c60c471
ekf2: declination fusion don't use uninitialized parameter (EKF2_MAG_DECL) 2024-02-21 09:45:44 -05:00
Niklas Hauser 643d3e3bf3 Navigator: Prevent busy-looping if Dataman read/write times out
MissionBase did not initialize its mission data, thus could enter an
infinite loop in updateDatamanCache() if the initMission() failed to
read the mission off, for example, due to the SDCard storage task taking
longer than the timeout to respond.

This change constrains the loading loop and resets the mission data even
if the data write failed.
2024-02-21 13:31:43 +01:00
Daniel Agar 8243b4f474 ekf2: move vel/pos reset helpers 2024-02-20 13:16:24 -05:00
Daniel Agar 22b957696d ekf2: velocity/position fusion helper minor consistency cleanup 2024-02-20 13:16:24 -05:00
Daniel Agar c338891677 ekf2: split vel_pos_fusion.cpp 2024-02-20 13:16:24 -05:00
Daniel Agar c4c41c49e5 ekf2: move fuseVelPosHeight() -> fuseDirectStateMeasurement()
- don't bother keeping bad_vel_{N,E,D} and bad_pos_{N,E,D} fault status bits
2024-02-20 13:16:24 -05:00
Daniel Agar 021dd0d0af ekf2: fix EV height bias predict call
- needs to be called every iteration
2024-02-20 11:47:53 -05:00
bresch c221da27a7 ekf2: set attitude validity flag using centralized function 2024-02-20 11:33:30 -05:00
Matthias Grob 51fe4351c6 StickTiltXY: Fix too high maximum tilt problem
And add unit tests.
2024-02-20 14:27:49 +01:00
Silvan Fuhrer 8a75733511 Navigator: fix VTOL land waypoint calculation
The setpoint.yaw can be NAN, and this made the calculated land point NAN
as well. Looking at the current yaw is anyway a better way to approximate
the course over ground that fundamentally should be used.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-02-19 14:54:28 +01:00
Daniel Agar 1032dd3470 ekf: fix measurementUpdate comment typo 2024-02-19 09:41:49 +01:00
Konrad 424c3cd2cb FeasibilityChecker: Add new TakeoffLandAvailable option
ADd a new misison feasiblity checker option to check if a proper landing approach is defined when in air. There must be at least a mission landing or a VTOL approach defined in order for the mission to be accepted. Else, use the same logic as in MIS_TKO_LAND_REQ=4
2024-02-16 10:27:22 +01:00
Konrad 68100650da RTL: publish a status message on currently chosen RTL point 2024-02-16 10:27:22 +01:00
Cyril C 74303a79e1
drivers/batt_smbus: fix BQ40Z80 timeout problem (#22751)
Co-authored-by: cyril.calvez <c.calvez@elistair.com>
2024-02-15 13:24:40 -05:00
Daniel Agar 8dc3975456
ekf2: only populate gnss pos aid src status if ref initialized
- this is a minor logging improvement when plotting the position from the beginning of the log (often a replay session)
2024-02-15 13:13:10 -05:00
Matthias Grob 84a7d42566 rover build: correct differential drive kconfig name 2024-02-15 10:08:51 -05:00
Matthias Grob f26df8492f Update GPS drivers to contain the astyle fix 2024-02-15 15:23:06 +01:00
Konrad cb09dde606 FixedwingPositionControl: Used corrected npfg roll output in path mode 2024-02-13 17:17:44 +01:00
Konrad 1a1891073e FixedwingPositionControl: Only warn user when roll is reduced for a longer period of time 2024-02-13 17:17:44 +01:00
Daniel Agar b8714f8980 ROMFS: rc.simulator EKF2 setup specific to gazebo classic 2024-02-13 11:14:44 -05:00
PX4 BuildBot 0c099f2b56 Update submodule gz to latest Tue Feb 13 12:39:17 UTC 2024
- gz in PX4/Firmware (c9ad60e3cc): c78f7f0141
    - gz current upstream: f1c461fffb
    - Changes: c78f7f0141...f1c461fffb

    f1c461f 2024-02-08 frederik - increase monocam clipping distance
6d5db73 2024-02-07 Sergei Grichine - Added Zero Turn Lawnmower model (#27)
5332071 2024-02-06 Frederik Markus - add navsat plugin to worlds and navsat sensor to models (#26)
2024-02-13 11:13:17 -05:00
Frederik Markus bb53781b8f
simulation/gz_bridge: enable navsat plugin for accurate positioning of real life maps in Gazebo (#22638)
* publish the global groundtruth from the navsat callback and rearrange the local groundtruth as the altitude reference now has a dependency on the global groundtruth being initialized

---------

Signed-off-by: frederik <frederik.anilmarkus@gmail.com>
2024-02-13 11:09:35 -05:00
Silvan Fuhrer c9ad60e3cc Update src/modules/navigator/mission_block.cpp
Co-authored-by: Matthias Grob <maetugr@gmail.com>
2024-02-13 10:34:57 +01:00
Silvan Fuhrer a6ef7b6da9 RTL: write out weather vane in comments (instead of WV)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-02-13 10:34:57 +01:00
Silvan Fuhrer 6957818603 RTL: clean up naming of function arguments
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-02-13 10:34:57 +01:00
Matthias Grob cb03835124 RTL: use dest.yaw instead of a separate heading_sp 2024-02-13 10:34:57 +01:00
Silvan Fuhrer b19e35ec7c RTL: change when to set a heading setpoint, generally leave it up to the executer
-remove RTL_HDG_MD
-only set heading setpoint in Navigator::RTL once above landing point,
or when RTL is triggered close to it
-never set a heading during RTL if weather vane is enabled

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-02-13 10:34:57 +01:00
PX4 BuildBot dce53a626e boards: update all NuttX defconfigs 2024-02-12 08:58:49 -05:00
Daniel Agar 5f589bdda3
Update submodule GPSDrivers to latest Mon Feb 12 12:39:19 UTC 2024
- GPSDrivers in PX4/Firmware (17ff40898c683e1fe96ff9e2d2790594d188f872): 3393191fbb
    - GPSDrivers current upstream: f48cc01d31
    - Changes: 3393191fbb...f48cc01d31

    f48cc01 2024-02-08 Julian Oes - ubx: separate config for jamming monitor
bc72f55 2024-02-08 Julian Oes - sbf: simplify odd define

Co-authored-by: PX4 BuildBot <bot@px4.io>
2024-02-12 08:58:21 -05:00
Matthias Grob 1998f54ea6 DifferentialDrive: move spoolup consideration to the main module 2024-02-12 14:29:10 +01:00
PerFrivik bef694f9ba Added spoolup and removed temporary timeout for EKF 2024-02-12 14:29:10 +01:00
PerFrivik 560d6a9d4b cleanup + updated acro 2024-02-12 14:29:10 +01:00
PerFrivik f996caa5bd Fixed bug in the guidance logic
After smoothing the linera velocity setpoint, the EKF has trouble initializing, becuase the acceleration is too smooth, to combat this issue, there is a 1 second delay when initializing the mission mode
2024-02-12 14:29:10 +01:00
PerFrivik bb0dfba4e6 added acro mode
Acro mode is manual mode, but with rate control
2024-02-12 14:29:10 +01:00
PerFrivik d197d94889 Fixed guidance logic and added feedforward term to compute the angular velocity 2024-02-12 14:29:10 +01:00
Matthias Grob 396ef222ee DifferentialDrive: Rework structure
3 Components Guidance - Control - Allocation
with their corresponding uORB interface.
2024-02-12 14:29:10 +01:00
Matthias Grob f85144ca76 DifferentialDrive: remove trailing zeros from prameter metadata 2024-02-12 14:29:10 +01:00
Matthias Grob b54b4f7dce Rename module differential_drive_control -> differential_drive 2024-02-12 14:29:10 +01:00
Matthias Grob fc90e235f1 Rename differential drive setpoint topics 2024-02-12 14:29:10 +01:00
Matthias Grob f7baeae1a0 DifferentialDriveControl: only save required parts of uORB message 2024-02-12 14:29:10 +01:00
PerFrivik e457a5baed Differential Drive Guidance: Add guidance
also add dependency on control allocation parameter CA_R_REV

Differential Drive Guidance: Added mission logic

Differential Drive Guidance

Differential Drive Guidance

Differential Guidance: Inlcude library

Differential Guidance: Compiles, does not work though

Differential Guidance: Works somewhat

Differential Guidance: Temp

Differential Guidance: Tuning

Differeital Drive Guidance: Remove waypoint mover

Differential Guidance: Fixed accuracy issue by converting from float to double

Differential Guidance: rebased on differentialdrive and improved waypoint accuracy

Temp

Differential Guidance: cleanup

temp
2024-02-12 14:29:10 +01:00
PX4 BuildBot 0f3925b21d update all px4board kconfig 2024-02-09 10:48:26 -05:00
muramura f636414ca7 tuning_tools: Change 1G to a more accurate value 2024-02-09 10:26:57 -05:00
muramura 00a9e4c76b Auto: Change 1G to a more accurate value 2024-02-09 10:26:57 -05:00
Alex Klimaj 31bbda0b58
boards: new ARK Septentrio GPS CAN node(ark_septentrio-gps)
* update gps submodule with sbf fix
* ARK Septentrio GPS initial commit
2024-02-09 10:26:09 -05:00
Matthias Grob b355c16141 Lanbao driver: correct rangefinder type to IR 2024-02-09 06:19:12 +01:00
Matthias Grob 7fdb5ef3cb PWMOut/px4io: correct automatic servo/motor configuration messages 2024-02-09 06:19:12 +01:00
fury1895 3de7d83e5f v6x board_sensors: publish system_power if ADC_ADS1115_EN is enabled 2024-02-08 16:11:34 +01:00
Matthias Grob 97cb933cff FLightTaskAuto: limit nudging speed based on distance sensor 2024-02-07 11:23:55 +01:00
Silvan Fuhrer 584d8abe1e params: change return type of param_modify_on_import to enum
Return early in param_import_callback() with 1 if we do a param_set in the param translation.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-02-07 08:08:37 +01:00
361 changed files with 11069 additions and 5923 deletions

View File

@ -120,7 +120,7 @@ pipeline {
"px4_fmu-v6xrt_rover",
"px4_io-v2_default",
"raspberrypi_pico_default",
"siyi_n7_default"
"siyi_n7_default",
"sky-drones_smartap-airlink_default",
"spracing_h7extreme_default",
"thepeach_k1_default",

14
.editorconfig Normal file
View File

@ -0,0 +1,14 @@
root = true
[*]
insert_final_newline = false
[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
indent_style = tab
tab_width = 8
# Not in the official standard, but supported by many editors
max_line_length = 120
[*.yaml]
indent_style = space
indent_size = 2

View File

@ -25,6 +25,7 @@ jobs:
ark_can-rtk-gps,
ark_cannode,
ark_fmu-v6x,
ark_septentrio-gps,
atl_mantis-edu,
av_x-v1,
bitcraze_crazyflie,

2
.gitmodules vendored
View File

@ -81,5 +81,5 @@
url = https://github.com/PX4/PX4-gazebo-models.git
branch = main
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
path = src/modules/muorb/apps/libfc-sensor-api
path = boards/modalai/voxl2/libfc-sensor-api
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

View File

@ -151,6 +151,16 @@ CONFIG:
buildType: MinSizeRel
settings:
CONFIG: ark_can-rtk-gps_canbootloader
ark_septentrio_gps_default:
short: ark_septentrio_gps_default
buildType: MinSizeRel
settings:
CONFIG: ark_septentrio_gps_default
ark_septentrio_gps_canbootloader:
short: ark_septentrio_gps_canbootloader
buildType: MinSizeRel
settings:
CONFIG: ark_septentrio_gps_canbootloader
ark_cannode_default:
short: ark_cannode_default
buildType: MinSizeRel

View File

@ -0,0 +1,80 @@
#!/bin/sh
# @name Gazebo lawnmower
# @type Rover
# @class Rover
. ${R}etc/init.d/rc.rover_differential_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
param set-default SIM_GZ_EN 1 # Gazebo bridge
# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 1
# We can arm and drive in manual mode when it slides and GPS check fails:
param set-default COM_ARM_WO_GPS 1
# Set Differential Drive Kinematics Library parameters:
param set RDD_WHEEL_BASE 0.9
param set RDD_WHEEL_RADIUS 0.22
param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h
# Actuator mapping - set SITL motors/servos output parameters:
# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
#param set-default SIM_GZ_WH_MIN1 0
#param set-default SIM_GZ_WH_MAX1 200
#param set-default SIM_GZ_WH_DIS1 100
#param set-default SIM_GZ_WH_FAIL1 100
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
#param set-default SIM_GZ_WH_MIN2 0
#param set-default SIM_GZ_WH_MAX2 200
#aram set-default SIM_GZ_WH_DIS2 100
#param set-default SIM_GZ_WH_FAIL2 100
param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation.
# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate
# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator
# controls in practical scenarios.
# Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
param set-default SIM_GZ_SV_FUNC3 203
param set-default SIM_GZ_SV_MIN3 0
param set-default SIM_GZ_SV_MAX3 1000
param set-default SIM_GZ_SV_DIS3 500
param set-default SIM_GZ_SV_FAIL3 500
# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
# - on minimum when disarmed or failed:
param set-default SIM_GZ_SV_FUNC4 204
param set-default SIM_GZ_SV_MIN4 0
param set-default SIM_GZ_SV_MAX4 1000
param set-default SIM_GZ_SV_DIS4 500
param set-default SIM_GZ_SV_FAIL4 500
# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode:
#param set-default SIM_GZ_SV_FUNC5 205
#param set-default SIM_GZ_SV_MIN5 1000
#param set-default SIM_GZ_SV_MAX5 2000
#param set-default SIM_GZ_SV_DIS5 1000
#param set-default SIM_GZ_SV_FAIL5 1000
# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure:
#param set-default SIM_GZ_SV_FUNC6 206
# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207):
#param set-default SIM_GZ_SV_FUNC7 207
# Spare PCA9685 servo channel 8 - "Servo 8" (208):
#param set-default SIM_GZ_SV_FUNC8 208

View File

@ -82,6 +82,7 @@ px4_add_romfs_files(
4008_gz_advanced_plane
4009_gz_r1_rover
4010_gz_x500_mono_cam
4011_gz_lawnmower
6011_gazebo-classic_typhoon_h480
6011_gazebo-classic_typhoon_h480.post

View File

@ -41,19 +41,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
echo "INFO [init] Gazebo simulator"
# set local coordinate frame reference
if [ -n "${PX4_HOME_LAT}" ]; then
param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
fi
if [ -n "${PX4_HOME_LON}" ]; then
param set SIM_GZ_HOME_LON ${PX4_HOME_LON}
fi
if [ -n "${PX4_HOME_ALT}" ]; then
param set SIM_GZ_HOME_ALT ${PX4_HOME_ALT}
fi
# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
if [ -z "${PX4_GZ_STANDALONE}" ]; then
@ -180,6 +167,12 @@ elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)"
else
# otherwise start simulator (mavlink) module
# EKF2 specifics
param set-default EKF2_GPS_DELAY 10
param set-default EKF2_MULTI_IMU 3
param set-default SENS_IMU_MODE 0
simulator_tcp_port=$((4560+px4_instance))
# Check if PX4_SIM_HOSTNAME environment variable is empty

View File

@ -164,10 +164,6 @@ param set-default COM_RC_IN_MODE 1
# Speedup SITL startup
param set-default EKF2_REQ_GPS_H 0.5
# Multi-EKF
param set-default EKF2_MULTI_IMU 3
param set-default SENS_IMU_MODE 0
param set-default IMU_GYRO_FFT_EN 1
param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets

View File

@ -34,28 +34,79 @@
add_subdirectory(airframes)
px4_add_romfs_files(
rc.airship_apps
rc.airship_defaults
rc.autostart_ext
rc.balloon_apps
rc.balloon_defaults
rc.boat_defaults
rc.fw_apps
rc.fw_defaults
rc.heli_defaults
rc.logging
rc.mc_apps
rc.mc_defaults
rcS
rc.sensors
rc.thermal_cal
rc.rover_apps
rc.rover_defaults
rc.rover_differential_apps
rc.rover_differential_defaults
rc.uuv_apps
rc.uuv_defaults
rc.vehicle_setup
rc.vtol_apps
rc.vtol_defaults
# TODO
rc.balloon_apps
rc.balloon_defaults
)
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
px4_add_romfs_files(
rc.airship_apps
rc.airship_defaults
)
endif()
if(CONFIG_MODULES_FW_RATE_CONTROL)
px4_add_romfs_files(
rc.fw_apps
rc.fw_defaults
)
endif()
if(CONFIG_MODULES_MC_RATE_CONTROL)
px4_add_romfs_files(
rc.heli_defaults
rc.mc_apps
rc.mc_defaults
)
endif()
if(CONFIG_MODULES_ROVER_POS_CONTROL)
px4_add_romfs_files(
rc.rover_apps
rc.rover_defaults
rc.boat_defaults # hack
)
endif()
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
px4_add_romfs_files(
rc.rover_differential_apps
rc.rover_differential_defaults
)
endif()
if(CONFIG_MODULES_UUV_ATT_CONTROL)
px4_add_romfs_files(
rc.uuv_apps
rc.uuv_defaults
)
endif()
if(CONFIG_MODULES_VTOL_ATT_CONTROL)
px4_add_romfs_files(
rc.vtol_apps
rc.vtol_defaults
)
endif()
if(CONFIG_MODULES_LOGGER)
px4_add_romfs_files(
rc.logging
)
endif()
if(CONFIG_MODULES_TEMPERATURE_COMPENSATION)
px4_add_romfs_files(
rc.thermal_cal
)
endif()

View File

@ -32,94 +32,127 @@
############################################################################
px4_add_romfs_files(
# [0-999] Reserved (historical)"
# [1000, 1999] Simulation setups"
1001_rc_quad_x.hil
1002_standard_vtol.hil
1100_rc_quad_x_sih.hil
1101_rc_plane_sih.hil
1102_tailsitter_duo_sih.hil
# [2000, 2999] Standard planes"
2100_standard_plane
2106_albatross
2507_cloudship
# [3000, 3999] Flying wing"
3000_generic_wing
# [4000, 4999] Quadrotor x"
4001_quad_x
4014_s500
4015_holybro_s500
4016_holybro_px4vision
4017_nxp_hovergames
4019_x500_v2
4020_holybro_px4vision_v1_5
4041_beta75x
4050_generic_250
4052_holybro_qav250
4053_holybro_kopis2
4061_atl_mantis_edu
4071_ifo
4073_ifo-s
4500_clover4
4901_crazyflie21
# [5000, 5999] Quadrotor +"
5001_quad_+
# [6000, 6999] Hexarotor x"
6001_hexa_x
6002_draco_r
# [7000, 7999] Hexarotor +"
7001_hexa_+
# [8000, 8999] Octorotor +"
8001_octo_x
# [9000, 9999] Octorotor +"
9001_octo_+
# [11000, 11999] Hexa Cox
11001_hexa_cox
# [12000, 12999] Octo Cox
12001_octo_cox
# [13000, 13999] VTOL
13000_generic_vtol_standard
13100_generic_vtol_tiltrotor
13013_deltaquad
13014_vtol_babyshark
13030_generic_vtol_quad_tiltrotor
13200_generic_vtol_tailsitter
# [14000, 14999] MC with tilt
14001_generic_mc_with_tilt
16001_helicopter
# [17000, 17999] Autogyro
17002_TF-AutoG2
17003_TF-G2
# [0-999] Reserved (historical)
# [18000, 18999] High-altitude balloons
18001_TF-B1
# [22000, 22999] Reserve for custom models
24001_dodeca_cox
50000_generic_ground_vehicle
50004_nxpcup_car_dfrobot_gpx
50003_aion_robotics_r1_rover
# [60000, 61000] (Unmanned) Underwater Robots
60000_uuv_generic
60001_uuv_hippocampus
60002_uuv_bluerov2_heavy
)
if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
px4_add_romfs_files(
# [1000, 1999] Simulation setups
1001_rc_quad_x.hil
1002_standard_vtol.hil
1100_rc_quad_x_sih.hil
1101_rc_plane_sih.hil
1102_tailsitter_duo_sih.hil
)
endif()
if(CONFIG_MODULES_MC_RATE_CONTROL)
px4_add_romfs_files(
# [4000, 4999] Quadrotor x
4001_quad_x
4014_s500
4015_holybro_s500
4016_holybro_px4vision
4017_nxp_hovergames
4019_x500_v2
4020_holybro_px4vision_v1_5
4041_beta75x
4050_generic_250
4052_holybro_qav250
4053_holybro_kopis2
4061_atl_mantis_edu
4071_ifo
4073_ifo-s
4500_clover4
4901_crazyflie21
# [5000, 5999] Quadrotor +
5001_quad_+
# [6000, 6999] Hexarotor x
6001_hexa_x
6002_draco_r
# [7000, 7999] Hexarotor +
7001_hexa_+
# [8000, 8999] Octorotor +
8001_octo_x
# [9000, 9999] Octorotor +
9001_octo_+
# [11000, 11999] Hexa Cox
11001_hexa_cox
# [12000, 12999] Octo Cox
12001_octo_cox
# [14000, 14999] MC with tilt
14001_generic_mc_with_tilt
16001_helicopter
24001_dodeca_cox
)
endif()
if(CONFIG_MODULES_FW_RATE_CONTROL)
px4_add_romfs_files(
# [2000, 2999] Standard planes
2100_standard_plane
2106_albatross
# [3000, 3999] Flying wing
3000_generic_wing
# [17000, 17999] Autogyro
17002_TF-AutoG2
17003_TF-G2
)
endif()
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
px4_add_romfs_files(
2507_cloudship
)
endif()
if(CONFIG_MODULES_VTOL_ATT_CONTROL)
px4_add_romfs_files(
# [13000, 13999] VTOL
13000_generic_vtol_standard
13100_generic_vtol_tiltrotor
13013_deltaquad
13014_vtol_babyshark
13030_generic_vtol_quad_tiltrotor
13200_generic_vtol_tailsitter
)
endif()
if(CONFIG_MODULES_ROVER_POS_CONTROL)
px4_add_romfs_files(
50000_generic_ground_vehicle
50004_nxpcup_car_dfrobot_gpx
)
endif()
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
px4_add_romfs_files(
50003_aion_robotics_r1_rover
)
endif()
if(CONFIG_MODULES_UUV_ATT_CONTROL)
px4_add_romfs_files(
# [60000, 61000] (Unmanned) Underwater Robots
60000_uuv_generic
60001_uuv_hippocampus
60002_uuv_bluerov2_heavy
)
endif()

View File

@ -28,7 +28,6 @@ param set-default EKF2_ARSP_THR 8
param set-default EKF2_FUSE_BETA 1
param set-default EKF2_GPS_CHECK 21
param set-default EKF2_MAG_ACCLIM 0
param set-default EKF2_MAG_YAWLIM 0
param set-default EKF2_REQ_EPH 10
param set-default EKF2_REQ_EPV 10
param set-default EKF2_REQ_HDRIFT 0.5

View File

@ -5,7 +5,7 @@
ekf2 start &
# Start rover differential drive controller.
differential_drive_control start
differential_drive start
# Start Land Detector.
land_detector start rover

View File

@ -439,7 +439,12 @@ else
#
# Start a thermal calibration if required.
#
. ${R}etc/init.d/rc.thermal_cal
set RC_THERMAL_CAL ${R}etc/init.d/rc.thermal_cal
if [ -f ${RC_THERMAL_CAL} ]
then
. ${RC_THERMAL_CAL}
fi
unset RC_THERMAL_CAL
#
# Start gimbal to control mounts such as gimbals, disabled by default.
@ -500,7 +505,12 @@ else
#
# Start the logger.
#
. ${R}etc/init.d/rc.logging
set RC_LOGGING ${R}etc/init.d/rc.logging
if [ -f ${RC_LOGGING} ]
then
. ${RC_LOGGING}
fi
unset RC_LOGGING
#
# Set additional parameters and env variables for selected AUTOSTART.

View File

@ -30,4 +30,5 @@ exec find boards msg src platforms test \
-path src/lib/cdrstream/cyclonedds -prune -o \
-path src/lib/cdrstream/rosidl -prune -o \
-path src/modules/zenoh/zenoh-pico -prune -o \
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

View File

@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
@{
queue_length = 1
for constant in spec.constants:
if constant.name == 'ORB_QUEUE_LENGTH':
queue_length = constant.val
}@
@[for topic in topics]@
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic), @queue_length);
@[end for]
void print_message(const orb_metadata *meta, const @uorb_struct& message)

@ -1 +1 @@
Subproject commit 33ac87a37676fb597de110e926bbf0a080169ffe
Subproject commit da7206e057703cc645770f02437013358b71e1c0

@ -1 +1 @@
Subproject commit c78f7f01417168e8faab7a83ade2129c0d26b39d
Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd

View File

@ -4,7 +4,7 @@
"description": "Firmware for the ARK flow board",
"image": "",
"build_time": 0,
"summary": "ARFFLOW",
"summary": "ARKFLOW",
"version": "0.1",
"image_size": 0,
"image_maxsize": 2080768,

View File

@ -0,0 +1,5 @@
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m4"
CONFIG_BOARD_ROMFSROOT=""
CONFIG_BOARD_CONSTRAINED_MEMORY=y
CONFIG_DRIVERS_BOOTLOADERS=y

View File

@ -0,0 +1,38 @@
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m4"
CONFIG_BOARD_ROMFSROOT="cannode"
CONFIG_BOARD_NO_HELP=y
CONFIG_BOARD_CONSTRAINED_MEMORY=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_BOOTLOADERS=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_BOARD_UAVCAN_INTERFACES=1
CONFIG_DRIVERS_UAVCANNODE=y
CONFIG_UAVCANNODE_BEEP_COMMAND=y
CONFIG_UAVCANNODE_GNSS_FIX=y
CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
CONFIG_UAVCANNODE_RTK_DATA=y
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_REBOOT=y
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
CONFIG_SYSTEMCMDS_TOP=y
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_SYSTEMCMDS_WORK_QUEUE=y

View File

@ -0,0 +1,13 @@
{
"board_id": 84,
"magic": "PX4FWv1",
"description": "Firmware for the ARK Septentrio GPS board",
"image": "",
"build_time": 0,
"summary": "ARKSEPTENTRIOGPS",
"version": "0.1",
"image_size": 0,
"image_maxsize": 2080768,
"git_identity": "",
"board_revision": 0
}

View File

@ -0,0 +1,15 @@
#!/bin/sh
#
# board specific defaults
#------------------------------------------------------------------------------
param set-default CBRK_IO_SAFETY 0
param set-default CANNODE_SUB_MBD 1
param set-default CANNODE_SUB_RTCM 1
param set-default MBE_ENABLE 1
param set-default SENS_IMU_CLPNOTI 0
safety_button start
tone_alarm start
ekf2 start

View File

@ -0,0 +1,11 @@
#!/bin/sh
#
# board sensors init
#------------------------------------------------------------------------------
gps start -d /dev/ttyS0 -p sbf
icm42688p -R 0 -s start
bmp388 -I -b 1 start
bmm150 -I -b 1 start

View File

@ -0,0 +1,57 @@
#
# This file is autogenerated: PLEASE DO NOT EDIT IT.
#
# You can use "make menuconfig" to make any modifications to the installed .config file.
# You can then do "make savedefconfig" to generate a new defconfig file that includes your
# modifications.
#
CONFIG_ARCH="arm"
CONFIG_ARCH_BOARD_CUSTOM=y
CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/septentrio-gps/nuttx-config"
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
CONFIG_ARCH_CHIP="stm32"
CONFIG_ARCH_CHIP_STM32=y
CONFIG_ARCH_CHIP_STM32F412CE=y
CONFIG_ARCH_INTERRUPTSTACK=4096
CONFIG_ARMV7M_MEMCPY=y
CONFIG_ARMV7M_USEBASEPRI=y
CONFIG_BINFMT_DISABLE=y
CONFIG_BOARDCTL=y
CONFIG_BOARD_LOOPSPERMSEC=16717
CONFIG_DEBUG_FULLOPT=y
CONFIG_DEBUG_SYMBOLS=y
CONFIG_DEBUG_TCBINFO=y
CONFIG_DEFAULT_SMALL=y
CONFIG_DISABLE_MOUNTPOINT=y
CONFIG_EXPERIMENTAL=y
CONFIG_FDCLONE_DISABLE=y
CONFIG_FDCLONE_STDIO=y
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_IDLETHREAD_STACKSIZE=4096
CONFIG_INIT_STACKSIZE=4096
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_LIBC_LONG_LONG=y
CONFIG_LIBC_STRERROR=y
CONFIG_MM_REGIONS=2
CONFIG_NAME_MAX=0
CONFIG_NUNGET_CHARS=0
CONFIG_PREALLOC_TIMERS=0
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_RAM_SIZE=262144
CONFIG_RAM_START=0x20010000
CONFIG_RAW_BINARY=y
CONFIG_SIG_DEFAULT=y
CONFIG_SIG_SIGALRM_ACTION=y
CONFIG_SIG_SIGUSR1_ACTION=y
CONFIG_SIG_SIGUSR2_ACTION=y
CONFIG_STACK_COLORATION=y
CONFIG_START_DAY=30
CONFIG_START_MONTH=11
CONFIG_STDIO_DISABLE_BUFFERING=y
CONFIG_STM32_FLASH_CONFIG_G=y
CONFIG_STM32_NOEXT_VECTORS=y
CONFIG_STM32_TIM8=y
CONFIG_TASK_NAME_SIZE=0
CONFIG_USEC_PER_TICK=1000

View File

@ -0,0 +1,152 @@
/************************************************************************************
* configs/px4fmu/include/board.h
* include/arch/board/board.h
*
* Copyright (C) 2009 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
************************************************************************************/
#include "board_dma_map.h"
#ifndef __ARCH_BOARD_BOARD_H
#define __ARCH_BOARD_BOARD_H
#include <nuttx/config.h>
#ifndef __ASSEMBLY__
# include <stdint.h>
#endif
#include <stm32.h>
/* HSI - 8 MHz RC factory-trimmed
* LSI - 32 KHz RC
* HSE - 8 MHz Crystal
* LSE - not installed
*/
#define STM32_BOARD_USEHSE 1
#define STM32_BOARD_XTAL 8000000
#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL
#define STM32_HSI_FREQUENCY 16000000ul
#define STM32_LSI_FREQUENCY 32000
/* Main PLL Configuration */
#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8)
#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384)
#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4
#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8)
#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2)
#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16)
#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192)
#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2)
#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2)
#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/
#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL
#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB
#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ
#define STM32_SYSCLK_FREQUENCY 96000000ul
/* AHB clock (HCLK) is SYSCLK (96MHz) */
#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */
#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY
#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */
/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */
#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */
#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2)
/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */
#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY)
#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY)
#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY)
#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY)
#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY)
#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY)
#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY)
/* APB2 clock (PCLK2) is HCLK (96MHz) */
#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */
#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY)
/* Timers driven from APB2 will be PCLK2 since no prescale division */
#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY)
#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY)
#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY)
#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY)
#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY)
/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */
#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY)
/* Alternate function pin selections ************************************************/
/* UARTs */
#define GPIO_USART1_RX GPIO_USART1_RX_2
#define GPIO_USART1_TX GPIO_USART1_TX_3
#define GPIO_USART2_RX GPIO_USART2_RX_1
#define GPIO_USART2_TX GPIO_USART2_TX_1
/* CAN */
#define GPIO_CAN1_RX GPIO_CAN1_RX_1
#define GPIO_CAN1_TX GPIO_CAN1_TX_1
/* I2C */
#define GPIO_MCU_I2C1_SCL
#define GPIO_MCU_I2C1_SDA
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1
#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6)
#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7)
#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1
#define GPIO_I2C2_SDA GPIO_I2C2_SDA_4
#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9)
#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10)
/* SPI */
#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1
#endif /* __ARCH_BOARD_BOARD_H */

View File

@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
// DMA1 Channel/Stream Selections
//--------------------------------------------//---------------------------//----------------
// DMA2 Channel/Stream Selections
//--------------------------------------------//---------------------------//----------------
#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 // DMA2, Stream 0, Channel 3
#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3
#define DMACHAN_USART1_RX DMAMAP_USART1_RX_1 // DMA2, Stream 2, Channel 4
#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1
//#define DMACHAN_USART1_TX DMAMAP_USART1_TX // DMA2, Stream 7, Channel 4

View File

@ -0,0 +1,153 @@
#
# This file is autogenerated: PLEASE DO NOT EDIT IT.
#
# You can use "make menuconfig" to make any modifications to the installed .config file.
# You can then do "make savedefconfig" to generate a new defconfig file that includes your
# modifications.
#
# CONFIG_DISABLE_ENVIRON is not set
# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
# CONFIG_DISABLE_PTHREAD is not set
# CONFIG_NSH_DISABLEBG is not set
# CONFIG_NSH_DISABLESCRIPT is not set
# CONFIG_NSH_DISABLE_CAT is not set
# CONFIG_NSH_DISABLE_CD is not set
# CONFIG_NSH_DISABLE_CP is not set
# CONFIG_NSH_DISABLE_DATE is not set
# CONFIG_NSH_DISABLE_DF is not set
# CONFIG_NSH_DISABLE_ECHO is not set
# CONFIG_NSH_DISABLE_ENV is not set
# CONFIG_NSH_DISABLE_EXEC is not set
# CONFIG_NSH_DISABLE_EXPORT is not set
# CONFIG_NSH_DISABLE_FREE is not set
# CONFIG_NSH_DISABLE_GET is not set
# CONFIG_NSH_DISABLE_HELP is not set
# CONFIG_NSH_DISABLE_ITEF is not set
# CONFIG_NSH_DISABLE_KILL is not set
# CONFIG_NSH_DISABLE_LOOPS is not set
# CONFIG_NSH_DISABLE_LS is not set
# CONFIG_NSH_DISABLE_MKDIR is not set
# CONFIG_NSH_DISABLE_MOUNT is not set
# CONFIG_NSH_DISABLE_MV is not set
# CONFIG_NSH_DISABLE_PS is not set
# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
# CONFIG_NSH_DISABLE_PWD is not set
# CONFIG_NSH_DISABLE_RM is not set
# CONFIG_NSH_DISABLE_RMDIR is not set
# CONFIG_NSH_DISABLE_SEMICOLON is not set
# CONFIG_NSH_DISABLE_SET is not set
# CONFIG_NSH_DISABLE_SLEEP is not set
# CONFIG_NSH_DISABLE_SOURCE is not set
# CONFIG_NSH_DISABLE_TEST is not set
# CONFIG_NSH_DISABLE_TIME is not set
# CONFIG_NSH_DISABLE_UMOUNT is not set
# CONFIG_NSH_DISABLE_UNSET is not set
# CONFIG_NSH_DISABLE_USLEEP is not set
CONFIG_ARCH="arm"
CONFIG_ARCH_BOARD_CUSTOM=y
CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/septentrio-gps/nuttx-config"
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
CONFIG_ARCH_CHIP="stm32"
CONFIG_ARCH_CHIP_STM32=y
CONFIG_ARCH_CHIP_STM32F412CE=y
CONFIG_ARCH_INTERRUPTSTACK=768
CONFIG_ARCH_STACKDUMP=y
CONFIG_ARMV7M_MEMCPY=y
CONFIG_ARMV7M_USEBASEPRI=y
CONFIG_BOARDCTL_RESET=y
CONFIG_BOARD_ASSERT_RESET_VALUE=0
CONFIG_BOARD_LOOPSPERMSEC=16717
CONFIG_BOARD_RESET_ON_ASSERT=2
CONFIG_BUILTIN=y
CONFIG_DEBUG_FULLOPT=y
CONFIG_DEBUG_HARDFAULT_ALERT=y
CONFIG_DEBUG_SYMBOLS=y
CONFIG_DEBUG_TCBINFO=y
CONFIG_DEFAULT_SMALL=y
CONFIG_FDCLONE_STDIO=y
CONFIG_FS_CROMFS=y
CONFIG_FS_ROMFS=y
CONFIG_GRAN=y
CONFIG_GRAN_INTR=y
CONFIG_HAVE_CXX=y
CONFIG_HAVE_CXXINITIALIZE=y
CONFIG_I2C=y
CONFIG_I2C_RESET=y
CONFIG_IDLETHREAD_STACKSIZE=750
CONFIG_INIT_ENTRYPOINT="nsh_main"
CONFIG_INIT_STACKSIZE=2624
CONFIG_LIBC_FLOATINGPOINT=y
CONFIG_LIBC_LONG_LONG=y
CONFIG_LIBC_MAX_EXITFUNS=1
CONFIG_MEMSET_64BIT=y
CONFIG_MEMSET_OPTSPEED=y
CONFIG_MM_REGIONS=2
CONFIG_NAME_MAX=40
CONFIG_NSH_ARCHINIT=y
CONFIG_NSH_ARGCAT=y
CONFIG_NSH_BUILTIN_APPS=y
CONFIG_NSH_CMDPARMS=y
CONFIG_NSH_CROMFSETC=y
CONFIG_NSH_LINELEN=128
CONFIG_NSH_MAXARGUMENTS=15
CONFIG_NSH_NESTDEPTH=8
CONFIG_NSH_QUOTE=y
CONFIG_NSH_ROMFSETC=y
CONFIG_NSH_ROMFSSECTSIZE=128
CONFIG_NSH_VARS=y
CONFIG_PREALLOC_TIMERS=50
CONFIG_PTHREAD_MUTEX_ROBUST=y
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_RAM_SIZE=262144
CONFIG_RAM_START=0x20000000
CONFIG_RAW_BINARY=y
CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_HPWORKPRIORITY=249
CONFIG_SCHED_HPWORKSTACKSIZE=1280
CONFIG_SCHED_INSTRUMENTATION=y
CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
CONFIG_SCHED_WAITPID=y
CONFIG_SERIAL_TERMIOS=y
CONFIG_SIG_DEFAULT=y
CONFIG_SIG_SIGALRM_ACTION=y
CONFIG_SIG_SIGUSR1_ACTION=y
CONFIG_SIG_SIGUSR2_ACTION=y
CONFIG_SIG_SIGWORK=4
CONFIG_STACK_COLORATION=y
CONFIG_START_DAY=30
CONFIG_START_MONTH=11
CONFIG_STDIO_BUFFER_SIZE=32
CONFIG_STM32_ADC1=y
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
CONFIG_STM32_DMA1=y
CONFIG_STM32_DMA2=y
CONFIG_STM32_FLASH_CONFIG_G=y
CONFIG_STM32_FLASH_PREFETCH=y
CONFIG_STM32_FLOWCONTROL_BROKEN=y
CONFIG_STM32_I2C1=y
CONFIG_STM32_I2C2=y
CONFIG_STM32_JTAG_SW_ENABLE=y
CONFIG_STM32_PWR=y
CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32_SPI1=y
CONFIG_STM32_SPI1_DMA=y
CONFIG_STM32_SPI1_DMA_BUFFER=1024
CONFIG_STM32_TIM8=y
CONFIG_STM32_USART1=y
CONFIG_STM32_USART2=y
CONFIG_STM32_USART_BREAKS=y
CONFIG_STM32_WWDG=y
CONFIG_SYSTEM_NSH=y
CONFIG_TASK_NAME_SIZE=24
CONFIG_USART1_BAUD=57600
CONFIG_USART1_RXBUFSIZE=2000
CONFIG_USART1_RXDMA=y
CONFIG_USART1_TXBUFSIZE=2000
CONFIG_USART2_BAUD=57600
CONFIG_USART2_RXBUFSIZE=600
CONFIG_USART2_SERIAL_CONSOLE=y
CONFIG_USART2_TXBUFSIZE=1100
CONFIG_USEC_PER_TICK=1000

View File

@ -0,0 +1,134 @@
/****************************************************************************
* nuttx-config/scripts/canbootloader_script.ld
*
* Copyright (C) 2015 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
* 256Kb of SRAM. SRAM is split up into three blocks:
*
* 1) 112Kb of SRAM beginning at address 0x2000:0000
* 2) 16Kb of SRAM beginning at address 0x2001:c000
* 3) 64Kb of SRAM beginning at address 0x2002:0000
* 4) 64Kb of TCM SRAM beginning at address 0x1000:0000
*
* When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
* where the code expects to begin execution by jumping to the entry point in
* the 0x0800:0000 address range.
*
* The first 0x10000 of flash is reserved for the bootloader.
*/
MEMORY
{
flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
}
OUTPUT_ARCH(arm)
ENTRY(__start) /* treat __start as the anchor for dead code stripping */
EXTERN(_vectors) /* force the vectors to be included in the output */
/*
* Ensure that abort() is present in the final object. The exception handling
* code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
*/
EXTERN(abort)
SECTIONS
{
.text : {
_stext = ABSOLUTE(.);
*(.vectors)
*(.text .text.*)
*(.fixup)
*(.gnu.warning)
*(.rodata .rodata.*)
*(.gnu.linkonce.t.*)
*(.got)
*(.gcc_except_table)
*(.gnu.linkonce.r.*)
_etext = ABSOLUTE(.);
} > flash
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
.ARM.extab : {
*(.ARM.extab*)
} > flash
__exidx_start = ABSOLUTE(.);
.ARM.exidx : {
*(.ARM.exidx*)
} > flash
__exidx_end = ABSOLUTE(.);
_eronly = ABSOLUTE(.);
.data : {
_sdata = ABSOLUTE(.);
*(.data .data.*)
*(.gnu.linkonce.d.*)
CONSTRUCTORS
_edata = ABSOLUTE(.);
} > sram AT > flash
.bss : {
_sbss = ABSOLUTE(.);
*(.bss .bss.*)
*(.gnu.linkonce.b.*)
*(COMMON)
. = ALIGN(4);
_ebss = ABSOLUTE(.);
} > sram
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_info 0 : { *(.debug_info) }
.debug_line 0 : { *(.debug_line) }
.debug_pubnames 0 : { *(.debug_pubnames) }
.debug_aranges 0 : { *(.debug_aranges) }
}

View File

@ -0,0 +1,146 @@
/****************************************************************************
* scripts/ld.script
*
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.org>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name NuttX nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
* 256Kb of SRAM. SRAM is split up into three blocks:
*
* 1) 112Kb of SRAM beginning at address 0x2000:0000
* 2) 16Kb of SRAM beginning at address 0x2001:c000
* 3) 64Kb of SRAM beginning at address 0x2002:0000
* 4) 64Kb of TCM SRAM beginning at address 0x1000:0000
*
* When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
* where the code expects to begin execution by jumping to the entry point in
* the 0x0800:0000 address range.
*
* The first 0x10000 of flash is reserved for the bootloader.
*/
MEMORY
{
flash (rx) : ORIGIN = 0x08010000, LENGTH = 928K
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
}
OUTPUT_ARCH(arm)
ENTRY(__start) /* treat __start as the anchor for dead code stripping */
EXTERN(_vectors) /* force the vectors to be included in the output */
/*
* Ensure that abort() is present in the final object. The exception handling
* code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
*/
EXTERN(abort)
EXTERN(_bootdelay_signature)
SECTIONS
{
.text : {
_stext = ABSOLUTE(.);
*(.vectors)
. = ALIGN(8);
/*
* This section positions the app_descriptor_t used
* by the make_can_boot_descriptor.py tool to set
* the application image's descriptor so that the
* uavcan bootloader has the ability to validate the
* image crc, size etc
*/
KEEP(*(.app_descriptor))
*(.text .text.*)
*(.fixup)
*(.gnu.warning)
*(.rodata .rodata.*)
*(.gnu.linkonce.t.*)
*(.got)
*(.gcc_except_table)
*(.gnu.linkonce.r.*)
_etext = ABSOLUTE(.);
} > flash
/*
* Init functions (static constructors and the like)
*/
.init_section : {
_sinit = ABSOLUTE(.);
KEEP(*(.init_array .init_array.*))
_einit = ABSOLUTE(.);
} > flash
.ARM.extab : {
*(.ARM.extab*)
} > flash
__exidx_start = ABSOLUTE(.);
.ARM.exidx : {
*(.ARM.exidx*)
} > flash
__exidx_end = ABSOLUTE(.);
_eronly = ABSOLUTE(.);
.data : {
_sdata = ABSOLUTE(.);
*(.data .data.*)
*(.gnu.linkonce.d.*)
CONSTRUCTORS
_edata = ABSOLUTE(.);
} > sram AT > flash
.bss : {
_sbss = ABSOLUTE(.);
*(.bss .bss.*)
*(.gnu.linkonce.b.*)
*(COMMON)
. = ALIGN(4);
_ebss = ABSOLUTE(.);
} > sram
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_info 0 : { *(.debug_info) }
.debug_line 0 : { *(.debug_line) }
.debug_pubnames 0 : { *(.debug_pubnames) }
.debug_aranges 0 : { *(.debug_aranges) }
}

View File

@ -0,0 +1,67 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader")
add_library(drivers_board
boot_config.h
boot.c
led.c
led.h
)
target_link_libraries(drivers_board
PRIVATE
nuttx_arch
nuttx_drivers
canbootloader
)
target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader)
else()
add_library(drivers_board
can.c
i2c.cpp
init.c
led.c
spi.cpp
)
target_link_libraries(drivers_board
PRIVATE
arch_spi
drivers__led # drv_led_start
nuttx_arch
nuttx_drivers
px4_layer
)
endif()

View File

@ -0,0 +1,125 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file board_config.h
*
* board internal definitions
*/
#pragma once
#include <px4_platform_common/px4_config.h>
#include <nuttx/compiler.h>
#include <stdint.h>
/* BUTTON */
#define GPIO_BTN_SAFETY /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15)
/* Safety LED */
#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)
/* Tone alarm output. */
#define TONE_ALARM_TIMER 2 /* timer 2 */
#define TONE_ALARM_CHANNEL 1 /* channel 1 */
#define GPIO_TONE_ALARM_IDLE /* PA0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
#define GPIO_TONE_ALARM /* PA0 */ (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
/* CAN Silent mode control */
#define GPIO_CAN1_SILENT_S0 /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
/* CAN termination software control */
#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION
/* ICM42688p FSYNC */
#define GPIO_42688P_FSYNC /* PB8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
/* Boot config */
#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI)
/* LEDs are driven with open drain to support Anode to 5V or 3.3V */
#define GPIO_TIM1_CH1 /* PA8 */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
#define GPIO_TIM1_CH2 /* PA9 */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
#define GPIO_TIM1_CH3 /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
#define GPIO_I2C1_SCL_RESET /* PB6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6)
#define GPIO_I2C1_SDA_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7)
#define GPIO_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define GPIO_I2C2_SDA_RESET /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
#define GPIO_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
#define GPIO_I2C2_SDA_RESET /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
#define GPIO_USART1_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN3)
#define GPIO_USART1_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN15)
#define GPIO_USART2_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN3)
#define GPIO_USART2_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN2)
#define FLASH_BASED_PARAMS
/* High-resolution timer */
#define HRT_TIMER 3 /* use timer 3 for the HRT */
#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */
#define PX4_GPIO_INIT_LIST { \
GPIO_BTN_SAFETY, \
GPIO_LED_SAFETY, \
GPIO_I2C1_SCL_RESET, \
GPIO_I2C1_SDA_RESET, \
GPIO_I2C2_SCL_RESET, \
GPIO_I2C2_SDA_RESET, \
GPIO_42688P_FSYNC, \
GPIO_BOOT_CONFIG, \
GPIO_CAN1_TX, \
GPIO_CAN1_RX, \
GPIO_CAN1_SILENT_S0, \
GPIO_CAN1_TERMINATION, \
}
__BEGIN_DECLS
#define BOARD_HAS_N_S_RGB_LED 1
#define BOARD_MAX_LEDS BOARD_HAS_N_S_RGB_LED
#ifndef __ASSEMBLY__
extern void stm32_spiinitialize(void);
#include <px4_platform_common/board_common.h>
#endif /* __ASSEMBLY__ */
__END_DECLS

View File

@ -0,0 +1,188 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Author: Ben Dyer <ben_dyer@mac.com>
* Pavel Kirienko <pavel.kirienko@zubax.com>
* David Sidrane <david_s5@nscdg.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <px4_config.h>
#include <stdint.h>
#include "boot_config.h"
#include "board.h"
#include <debug.h>
#include <string.h>
#include <arch/board/board.h>
#include <nuttx/board.h>
#include "led.h"
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the initialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
__EXPORT void stm32_boardinitialize(void)
{
putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR);
stm32_configgpio(GPIO_CAN1_RX);
stm32_configgpio(GPIO_CAN1_TX);
stm32_configgpio(GPIO_CAN1_SILENT_S0);
stm32_configgpio(GPIO_CAN1_TERMINATION);
putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
stm32_configgpio(GPIO_GETNODEINFO_JUMPER);
#endif
}
/************************************************************************************
* Name: board_deinitialize
*
* Description:
* This function is called by the bootloader code prior to booting
* the application. Is should place the HW into an benign initialized state.
*
************************************************************************************/
void board_deinitialize(void)
{
putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
}
/****************************************************************************
* Name: board_get_product_name
*
* Description:
* Called to retrieve the product name. The returned value is a assumed
* to be written to a pascal style string that will be length prefixed
* and not null terminated
*
* Input Parameters:
* product_name - A pointer to a buffer to write the name.
* maxlen - The maximum number of charter that can be written
*
* Returned Value:
* The length of characters written to the buffer.
*
****************************************************************************/
uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen)
{
DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME));
memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME));
return UAVCAN_STRLEN(HW_UAVCAN_NAME);
}
/****************************************************************************
* Name: board_get_hardware_version
*
* Description:
* Called to retrieve the hardware version information. The function
* will first initialize the the callers struct to all zeros.
*
* Input Parameters:
* hw_version - A pointer to a uavcan_hardwareversion_t.
*
* Returned Value:
* Length of the unique_id
*
****************************************************************************/
size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version)
{
memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t));
hw_version->major = HW_VERSION_MAJOR;
hw_version->minor = HW_VERSION_MINOR;
return board_get_mfguid(*(mfguid_t *) hw_version->unique_id);
}
/****************************************************************************
* Name: board_indicate
*
* Description:
* Provides User feedback to indicate the state of the bootloader
* on board specific hardware.
*
* Input Parameters:
* indication - A member of the uiindication_t
*
* Returned Value:
* None
*
****************************************************************************/
#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)}
typedef begin_packed_struct struct led_t {
uint8_t red;
uint8_t green;
uint8_t blue;
uint8_t hz;
} end_packed_struct led_t;
static const led_t i2l[] = {
led(0, off, 0, 0, 0, 0),
led(1, reset, 128, 128, 128, 30),
led(2, autobaud_start, 0, 128, 0, 1),
led(3, autobaud_end, 0, 128, 0, 2),
led(4, allocation_start, 0, 0, 64, 2),
led(5, allocation_end, 0, 128, 64, 3),
led(6, fw_update_start, 32, 128, 64, 3),
led(7, fw_update_erase_fail, 32, 128, 32, 3),
led(8, fw_update_invalid_response, 64, 0, 0, 1),
led(9, fw_update_timeout, 64, 0, 0, 2),
led(a, fw_update_invalid_crc, 64, 0, 0, 4),
led(b, jump_to_app, 0, 128, 0, 10),
};
void board_indicate(uiindication_t indication)
{
rgb_led(i2l[indication].red,
i2l[indication].green,
i2l[indication].blue,
i2l[indication].hz);
}

View File

@ -0,0 +1,130 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file boot_config.h
*
* bootloader definitions that configures the behavior and options
* of the Boot loader
* This file is relies on the parent folder's boot_config.h file and defines
* different usages of the hardware for bootloading
*/
#pragma once
/************************************************************************************
* Included Files
************************************************************************************/
/* Bring in the board_config.h definitions
* todo:make this be pulled in from a targed's build
* files in nuttx*/
#include "board_config.h"
#include "uavcan.h"
#include <nuttx/compiler.h>
#include <stdint.h>
#include <stm32_flash.h>
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define OPT_PREFERRED_NODE_ID ANY_NODE_ID
//todo:wrap OPT_x in in ifdefs for command line definitions
#define OPT_TBOOT_MS 3000
#define OPT_NODE_STATUS_RATE_MS 800
#define OPT_NODE_INFO_RATE_MS 50
#define OPT_BL_NUMBER_TIMERS 7
/*
* This Option set is set to 1 ensure a provider of firmware has an
* opportunity update the node's firmware.
* This Option is the default policy and can be overridden by
* a jumper
* When this Policy is set, the node will ignore tboot and
* wait indefinitely for a GetNodeInfo request before booting.
*
* OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow
* the polarity of the jumper to be True Active
*
* wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO
* Jumper
* yes 1 0 x
* yes 1 1 Active
* no 1 1 Not Active
* no 0 0 X
* yes 0 1 Active
* no 0 1 Not Active
*
*/
#define OPT_WAIT_FOR_GETNODEINFO 0
#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1
#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1
#define OPT_ENABLE_WD 1
#define OPT_RESTART_TIMEOUT_MS 20000
/* Reserved for the Booloader */
#define OPT_BOOTLOADER_SIZE_IN_K (1024*64)
/* Reserved for the application out of the total
* system flash minus the BOOTLOADER_SIZE_IN_K
*/
#define OPT_APPLICATION_RESERVER_IN_K 0
#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K
#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K))
#define FLASH_BASE STM32_FLASH_BASE
#define FLASH_SIZE STM32_FLASH_SIZE
#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET)
#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET)
#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t)))
#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t)))
#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t)))
/* If this board uses big flash that have large sectors */
#define OPT_USE_YIELD
/* Bootloader Option*****************************************************************
*
*/
#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI)

View File

@ -0,0 +1,130 @@
/****************************************************************************
*
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file can.c
*
* Board-specific CAN functions.
*/
/************************************************************************************
* Included Files
************************************************************************************/
#include <px4_platform_common/px4_config.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/can/can.h>
#include <arch/board/board.h>
#include "chip.h"
#include "arm_internal.h"
#include "stm32.h"
#include "stm32_can.h"
#include "board_config.h"
#ifdef CONFIG_CAN
/************************************************************************************
* Pre-processor Definitions
************************************************************************************/
/* Configuration ********************************************************************/
#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
# undef CONFIG_STM32_CAN2
#endif
#ifdef CONFIG_STM32_CAN1
# define CAN_PORT 1
#else
# define CAN_PORT 2
#endif
/************************************************************************************
* Private Functions
************************************************************************************/
/************************************************************************************
* Public Functions
************************************************************************************/
int can_devinit(void);
/************************************************************************************
* Name: can_devinit
*
* Description:
* All STM32 architectures must provide the following interface to work with
* examples/can.
*
************************************************************************************/
int can_devinit(void)
{
static bool initialized = false;
struct can_dev_s *can;
int ret;
/* Check if we have already initialized */
if (!initialized) {
/* Call stm32_caninitialize() to get an instance of the CAN interface */
can = stm32_caninitialize(CAN_PORT);
if (can == NULL) {
canerr("ERROR: Failed to get CAN interface\n");
return -ENODEV;
}
/* Register the CAN driver at "/dev/can0" */
ret = can_register("/dev/can0", can);
if (ret < 0) {
canerr("ERROR: can_register failed: %d\n", ret);
return ret;
}
/* Now we are initialized */
initialized = true;
}
return OK;
}
#endif

View File

@ -0,0 +1,39 @@
/****************************************************************************
*
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <px4_arch/i2c_hw_description.h>
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
initI2CBusInternal(1),
initI2CBusInternal(2),
};

View File

@ -0,0 +1,168 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file init.c
*
* board specific early startup code. This file implements the
* board_app_initialize() function that is called early by nsh during startup.
*
* Code here is run before the rcS script is invoked; it should start required
* subsystems and perform board-specific initialization.
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <debug.h>
#include <errno.h>
#include <syslog.h>
#include <nuttx/board.h>
#include <stm32.h>
#include "board_config.h"
#include "led.h"
#include <stm32_uart.h>
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_board_led.h>
#include <drivers/drv_watchdog.h>
#include <systemlib/px4_macros.h>
#include <px4_platform_common/init.h>
#include <px4_platform/gpio.h>
# if defined(FLASH_BASED_PARAMS)
# include <parameters/flashparams/flashfs.h>
#endif
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the initialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
__EXPORT void stm32_boardinitialize(void)
{
watchdog_init();
/* configure pins */
const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
px4_gpio_init(gpio, arraySize(gpio));
// Configure SPI all interfaces GPIO & enable power.
stm32_spiinitialize();
// Check if button is held. If so go into gps passthrough mode
if (stm32_gpioread(GPIO_BTN_SAFETY)) {
rgb_led(128, 128, 128, 10);
stm32_configgpio(GPIO_USART1_TX_GPIO);
stm32_configgpio(GPIO_USART1_RX_GPIO);
stm32_configgpio(GPIO_USART2_TX_GPIO);
stm32_configgpio(GPIO_USART2_RX_GPIO);
while (1) {
watchdog_pet();
stm32_gpiowrite(GPIO_USART2_TX_GPIO, stm32_gpioread(GPIO_USART1_RX_GPIO));
stm32_gpiowrite(GPIO_USART1_TX_GPIO, stm32_gpioread(GPIO_USART2_RX_GPIO));
}
}
}
/****************************************************************************
* Name: board_app_initialize
*
* Description:
* Perform application specific initialization. This function is never
* called directly from application code, but only indirectly via the
* (non-standard) boardctl() interface using the command BOARDIOC_INIT.
*
* Input Parameters:
* arg - The boardctl() argument is passed to the board_app_initialize()
* implementation without modification. The argument has no
* meaning to NuttX; the meaning of the argument is a contract
* between the board-specific initalization logic and the the
* matching application logic. The value cold be such things as a
* mode enumeration value, a set of DIP switch switch settings, a
* pointer to configuration data read from a file or serial FLASH,
* or whatever you would like to do with it. Every implementation
* should accept zero/NULL as a default configuration.
*
* Returned Value:
* Zero (OK) is returned on success; a negated errno value is returned on
* any failure to indicate the nature of the failure.
*
****************************************************************************/
__EXPORT int board_app_initialize(uintptr_t arg)
{
px4_platform_init();
#if defined(SERIAL_HAVE_RXDMA)
// set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event.
static struct hrt_call serial_dma_call;
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
#if defined(FLASH_BASED_PARAMS)
static sector_descriptor_t params_sector_map[] = {
{2, 16 * 1024, 0x08008000},
{3, 16 * 1024, 0x0800C000},
{0, 0, 0},
};
/* Initialize the flashfs layer to use heap allocated memory */
int result = parameter_flashfs_init(params_sector_map, NULL, 0);
if (result != OK) {
syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
}
#endif // FLASH_BASED_PARAMS
/* Configure the HW based on the manifest */
//px4_platform_configure();
return OK;
}

View File

@ -0,0 +1,124 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file led.c
*
* LED backend.
*/
#include <px4_platform_common/px4_config.h>
#include <stdbool.h>
#include "chip.h"
#include "stm32_gpio.h"
#include "board_config.h"
#include <nuttx/board.h>
#include <arch/board/board.h>
#include "led.h"
#define TMR_BASE STM32_TIM1_BASE
#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN
#define TMR_REG(o) (TMR_BASE+(o))
void rgb_led(int r, int g, int b, int freqs)
{
long fosc = TMR_FREQUENCY;
long prescale = 2048;
long p1s = fosc / prescale;
long p0p5s = p1s / 2;
uint16_t val;
static bool once = 0;
if (!once) {
once = 1;
/* Enabel Clock to Block */
modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);
/* Reload */
val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET));
val |= ATIM_EGR_UG;
putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET));
/* Set Prescaler STM32_TIM_SETCLOCK */
putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET));
/* Enable STM32_TIM_SETMODE*/
putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET));
putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE |
(ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET));
putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET));
putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P |
ATIM_CCER_CC2E | ATIM_CCER_CC2P |
ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET));
stm32_configgpio(GPIO_TIM1_CH1);
stm32_configgpio(GPIO_TIM1_CH2);
stm32_configgpio(GPIO_TIM1_CH3);
/* master output enable = on */
putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET)));
}
long p = freqs == 0 ? p1s : p1s / freqs;
putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET));
p = freqs == 0 ? p1s + 1 : p0p5s / freqs;
putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET));
putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET));
putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET));
val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET));
if (freqs == 0) {
val &= ~ATIM_CR1_CEN;
} else {
val |= ATIM_CR1_CEN;
}
putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET));
}

View File

@ -0,0 +1,37 @@
/****************************************************************************
*
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
* Author: David Sidrane<david_s5@nscdg.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
__BEGIN_DECLS
void rgb_led(int r, int g, int b, int freqs);
__END_DECLS

View File

@ -0,0 +1,44 @@
/****************************************************************************
*
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <px4_arch/spi_hw_description.h>
#include <drivers/drv_sensor.h>
#include <nuttx/spi/spi.h>
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}),
}),
};
static constexpr bool unused = validateSPIConfig(px4_spi_buses);

View File

@ -0,0 +1,17 @@
# UAVCAN boot loadable Module ID
set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
add_definitions(
-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
)
set(uavcanblid_hw_version_major 0)
set(uavcanblid_hw_version_minor 84)
set(uavcanblid_name "\"org.ark.septentrio-gps\"")
add_definitions(
-DHW_UAVCAN_NAME=${uavcanblid_name}
-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
)

View File

@ -64,9 +64,6 @@
// Hacks for MAVLink RC button input
#define ATL_MANTIS_RC_INPUT_HACKS
// Hacks for MAVLink RC button input
#define ATL_MANTIS_RC_INPUT_HACKS
/*
* ADC channels
*

View File

@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y

View File

@ -4,13 +4,14 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
CONFIG_DRIVERS_QSHELL_QURT=y
CONFIG_DRIVERS_VOXL2_IO=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_EKF2=y
@ -28,4 +29,6 @@ CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_ORB_COMMUNICATOR=y
CONFIG_PARAM_REMOTE=y

View File

@ -44,11 +44,10 @@ add_library(drivers_board
)
# Add custom drivers for SLPI
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)

View File

@ -62,6 +62,8 @@
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/sensor_optical_flow.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@ -88,21 +90,22 @@ static bool _is_running = false;
volatile bool _task_should_exit = false;
static px4_task_t _task_handle = -1;
int _uart_fd = -1;
bool debug = false;
bool _debug = false;
std::string port = "2";
int baudrate = 921600;
const unsigned mode_flag_custom = 1;
const unsigned mode_flag_armed = 128;
bool _send_gps = false;
bool _send_mag = false;
bool _send_distance = false;
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::Publication<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
@ -128,13 +131,42 @@ float x_gyro = 0;
float y_gyro = 0;
float z_gyro = 0;
uint64_t gyro_accel_time = 0;
bool _use_software_mav_throttling{false};
int heartbeat_counter = 0;
int imu_counter = 0;
int hil_sensor_counter = 0;
int vision_msg_counter = 0;
int gps_counter = 0;
// Status counters
uint32_t heartbeat_received_counter = 0;
uint32_t heartbeat_sent_counter = 0;
uint32_t imu_counter = 0;
uint32_t hil_sensor_counter = 0;
uint32_t mag_counter = 0;
uint32_t baro_counter = 0;
uint32_t actuator_sent_counter = 0;
uint32_t odometry_received_counter = 0;
uint32_t odometry_sent_counter = 0;
uint32_t gps_received_counter = 0;
uint32_t gps_sent_counter = 0;
uint32_t distance_received_counter = 0;
uint32_t distance_sent_counter = 0;
uint32_t flow_received_counter = 0;
uint32_t flow_sent_counter = 0;
uint32_t unknown_msg_received_counter = 0;
enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES};
struct position_source_data_s {
char label[8];
bool send;
bool fail;
uint32_t failure_duration;
uint64_t failure_duration_start;
} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = {
{"GPS", false, false, 0, 0},
{"VIO", false, false, 0, 0},
{"FLOW", false, false, 0, 0}
};
uint64_t first_sensor_msg_timestamp = 0;
uint64_t first_sensor_report_timestamp = 0;
uint64_t last_sensor_report_timestamp = 0;
vehicle_status_s _vehicle_status{};
vehicle_control_mode_s _control_mode{};
@ -144,7 +176,6 @@ battery_status_s _battery_status{};
sensor_accel_fifo_s accel_fifo{};
sensor_gyro_fifo_s gyro_fifo{};
int openPort(const char *dev, speed_t speed);
int closePort();
@ -153,7 +184,8 @@ int writeResponse(void *buf, size_t len);
int start(int argc, char *argv[]);
int stop();
int get_status();
void print_status();
void clear_status_counters();
bool isOpen() { return _uart_fd >= 0; };
void usage();
@ -163,50 +195,65 @@ void *send_actuator(void *);
void send_actuator_data();
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void handle_message_hil_gps_dsp(mavlink_message_t *msg);
void handle_message_odometry_dsp(mavlink_message_t *msg);
void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg);
void handle_message_command_long_dsp(mavlink_message_t *msg);
void handle_message_dsp(mavlink_message_t *msg);
void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
void
handle_message_dsp(mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_SENSOR:
hil_sensor_counter++;
handle_message_hil_sensor_dsp(msg);
break;
case MAVLINK_MSG_ID_HIL_GPS:
if (_send_gps) { handle_message_hil_gps_dsp(msg); }
gps_received_counter++;
break;
if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); }
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_message_vision_position_estimate_dsp(msg);
break;
case MAVLINK_MSG_ID_ODOMETRY:
handle_message_odometry_dsp(msg);
break;
odometry_received_counter++;
if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); }
case MAVLINK_MSG_ID_COMMAND_LONG:
handle_message_command_long_dsp(msg);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
PX4_DEBUG("Heartbeat msg received");
heartbeat_received_counter++;
if (_debug) { PX4_INFO("Heartbeat msg received"); }
break;
case MAVLINK_MSG_ID_SYSTEM_TIME:
PX4_DEBUG("MAVLINK SYSTEM TIME");
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
flow_received_counter++;
if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); }
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
distance_received_counter++;
if (_send_distance) { handle_message_distance_sensor(msg); }
break;
default:
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
unknown_msg_received_counter++;
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
break;
}
}
@ -228,7 +275,6 @@ void send_actuator_data()
bool first_sent = false;
while (true) {
bool controls_updated = false;
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
@ -239,45 +285,50 @@ void send_actuator_data()
bool actuator_updated = false;
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
uint8_t newBuf[512];
uint16_t newBufLen = 0;
mavlink_hil_actuator_controls_t hil_act_control;
actuator_controls_from_outputs_dsp(&hil_act_control);
mavlink_message_t message{};
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
if (actuator_updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
px4_lockstep_wait_for_components();
if (_actuator_outputs.timestamp > 0) {
mavlink_hil_actuator_controls_t hil_act_control;
actuator_controls_from_outputs_dsp(&hil_act_control);
mavlink_message_t message{};
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
previous_timestamp = _actuator_outputs.timestamp;
previous_uorb_timestamp = _actuator_outputs.timestamp;
uint8_t newBuf[512];
uint16_t newBufLen = 0;
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
int writeRetval = writeResponse(&newBuf, newBufLen);
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
actuator_sent_counter++;
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
first_sent = true;
send_esc_telemetry_dsp(hil_act_control);
send_esc_status(hil_act_control);
}
} else if (!actuator_updated && first_sent && differential > 4000) {
mavlink_hil_actuator_controls_t hil_act_control;
actuator_controls_from_outputs_dsp(&hil_act_control);
} else if (! actuator_updated && first_sent && differential > 4000) {
previous_timestamp = hrt_absolute_time();
mavlink_message_t message{};
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
uint8_t newBuf[512];
uint16_t newBufLen = 0;
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
int writeRetval = writeResponse(&newBuf, newBufLen);
//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
send_esc_telemetry_dsp(hil_act_control);
actuator_sent_counter++;
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
send_esc_status(hil_act_control);
}
differential = hrt_absolute_time() - previous_timestamp;
px4_usleep(1000);
}
}
@ -287,14 +338,10 @@ void task_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 's':
_use_software_mav_throttling = true;
break;
case 'd':
debug = true;
_debug = true;
break;
case 'p':
@ -310,7 +357,19 @@ void task_main(int argc, char *argv[])
break;
case 'g':
_send_gps = true;
position_source_data[(int) position_source::GPS].send = true;
break;
case 'o':
position_source_data[(int) position_source::VIO].send = true;
break;
case 'h':
_send_distance = true;
break;
case 'f':
position_source_data[(int) position_source::FLOW].send = true;
break;
default:
@ -319,11 +378,13 @@ void task_main(int argc, char *argv[])
}
const char *charport = port.c_str();
int openRetval = openPort(charport, (speed_t) baudrate);
int open = isOpen();
(void) openPort(charport, (speed_t) baudrate);
if (open) {
PX4_ERR("Port is open: %d", openRetval);
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
if (! isOpen()) {
PX4_ERR("DSP HITL failed to open serial port");
return;
}
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
@ -342,14 +403,11 @@ void task_main(int argc, char *argv[])
pthread_attr_destroy(&sender_thread_attr);
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
_is_running = true;
while (!_task_should_exit) {
uint8_t rx_buf[1024];
//rx_buf[511] = '\0';
uint64_t timestamp = hrt_absolute_time();
@ -357,8 +415,8 @@ void task_main(int argc, char *argv[])
if (got_first_sensor_msg) {
uint64_t delta_time = timestamp - last_imu_update_timestamp;
if (delta_time > 15000) {
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
if ((imu_counter) && (delta_time > 15000)) {
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
}
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
@ -396,7 +454,7 @@ void task_main(int argc, char *argv[])
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
(void) writeResponse(&hb_newBuf, hb_newBufLen);
last_heartbeat_timestamp = timestamp;
heartbeat_counter++;
heartbeat_sent_counter++;
}
bool vehicle_updated = false;
@ -416,7 +474,7 @@ void task_main(int argc, char *argv[])
_is_running = false;
}
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
{
esc_status_s esc_status{};
esc_status.timestamp = hrt_absolute_time();
@ -448,17 +506,13 @@ void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
_esc_status_pub.publish(esc_status);
}
void
handle_message_command_long_dsp(mavlink_message_t *msg)
{
/* command */
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (debug) {
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
}
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
mavlink_command_ack_t ack = {};
ack.result = MAV_RESULT_UNSUPPORTED;
@ -470,46 +524,140 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
uint16_t acknewBufLen = 0;
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time());
if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); }
}
int flow_debug_counter = 0;
void
handle_message_vision_position_estimate_dsp(mavlink_message_t *msg)
handle_message_hil_optical_flow(mavlink_message_t *msg)
{
mavlink_vision_position_estimate_t vpe;
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
mavlink_hil_optical_flow_t flow;
mavlink_msg_hil_optical_flow_decode(msg, &flow);
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
vehicle_odometry_s odom{};
uint64_t timestamp = hrt_absolute_time();
odom.timestamp_sample = timestamp;
if ((_debug) && (!(flow_debug_counter % 10))) {
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
}
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = vpe.x;
odom.position[1] = vpe.y;
odom.position[2] = vpe.z;
flow_debug_counter++;
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
q.copyTo(odom.q);
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = 1;
device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
// VISION_POSITION_ESTIMATE covariance
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
// If unknown, assign NaN value to first element in the array.
odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0
odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1
odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2
sensor_optical_flow_s sensor_optical_flow{};
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
sensor_optical_flow.device_id = device_id.devid;
odom.reset_counter = vpe.reset_counter;
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
odom.timestamp = hrt_absolute_time();
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
sensor_optical_flow.quality = flow.quality;
_visual_odometry_pub.publish(odom);
vision_msg_counter++;
int index = (int) position_source::FLOW;
if (position_source_data[index].fail) {
uint32_t duration = position_source_data[index].failure_duration;
hrt_abstime start = position_source_data[index].failure_duration_start;
if (duration) {
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
PX4_INFO("Optical flow failure ending");
position_source_data[index].fail = false;
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
sensor_optical_flow.quality = 0;
}
} else {
sensor_optical_flow.quality = 0;
}
}
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
if (integrated_gyro.isAllFinite()) {
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
sensor_optical_flow.delta_angle_available = true;
}
sensor_optical_flow.max_flow_rate = NAN;
sensor_optical_flow.min_ground_distance = NAN;
sensor_optical_flow.max_ground_distance = NAN;
// Use distance value for distance sensor topic
// if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
// // Positive value (including zero): distance known. Negative value: Unknown distance.
// sensor_optical_flow.distance_m = flow.distance;
// sensor_optical_flow.distance_available = true;
// }
// Emulate voxl-flow-server where distance comes in a separate
// distance sensor topic message
sensor_optical_flow.distance_m = 0.0f;
sensor_optical_flow.distance_available = false;
sensor_optical_flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(sensor_optical_flow);
flow_sent_counter++;
}
int distance_debug_counter = 0;
void handle_message_distance_sensor(mavlink_message_t *msg)
{
mavlink_distance_sensor_t dist_sensor;
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
if ((_debug) && (!(distance_debug_counter % 10))) {
PX4_INFO("distance: time: %u, quality: %u, height: %u",
dist_sensor.time_boot_ms, dist_sensor.signal_quality,
dist_sensor.current_distance);
}
distance_debug_counter++;
distance_sensor_s ds{};
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = 1;
device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
ds.max_distance = static_cast<float>(dist_sensor.max_distance) * 1e-2f; /* cm to m */
ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */
ds.h_fov = dist_sensor.horizontal_fov;
ds.v_fov = dist_sensor.vertical_fov;
ds.q[0] = dist_sensor.quaternion[0];
ds.q[1] = dist_sensor.quaternion[1];
ds.q[2] = dist_sensor.quaternion[2];
ds.q[3] = dist_sensor.quaternion[3];
ds.type = dist_sensor.type;
ds.device_id = device_id.devid;
ds.orientation = dist_sensor.orientation;
// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
// quality value. Also it comes normalised between 1 and 100 while the uORB
// signal quality is normalised between 0 and 100.
ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
_distance_sensor_pub.publish(ds);
distance_sent_counter++;
}
void
@ -518,6 +666,8 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
mavlink_odometry_t odom_in;
mavlink_msg_odometry_decode(msg, &odom_in);
odometry_sent_counter++;
// fill vehicle_odometry from Mavlink ODOMETRY
vehicle_odometry_s odom{};
uint64_t timestamp = hrt_absolute_time();
@ -699,6 +849,28 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
odom.reset_counter = odom_in.reset_counter;
odom.quality = odom_in.quality;
int index = (int) position_source::VIO;
if (position_source_data[index].fail) {
uint32_t duration = position_source_data[index].failure_duration;
hrt_abstime start = position_source_data[index].failure_duration_start;
if (duration) {
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
PX4_INFO("VIO failure ending");
position_source_data[index].fail = false;
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
odom.quality = 0;
}
} else {
odom.quality = 0;
}
}
switch (odom_in.estimator_type) {
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
case MAV_ESTIMATOR_TYPE_NAIVE:
@ -745,10 +917,6 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
msg->mode = mode_flag_custom;
msg->mode |= (armed) ? mode_flag_armed : 0;
msg->flags = 0;
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
msg->flags |= 1;
#endif
}
int openPort(const char *dev, speed_t speed)
@ -759,7 +927,8 @@ int openPort(const char *dev, speed_t speed)
}
_uart_fd = qurt_uart_open(dev, speed);
PX4_DEBUG("qurt_uart_opened");
if (_debug) { PX4_INFO("qurt_uart_opened"); }
if (_uart_fd < 0) {
PX4_ERR("Error opening port: %s (%i)", dev, errno);
@ -840,25 +1009,50 @@ int stop()
void usage()
{
PX4_INFO("Usage: dsp_hitl {start|info|status|stop}");
PX4_INFO("Usage: dsp_hitl {start|status|clear|failure|stop}");
PX4_INFO(" failure <source> <duration>");
PX4_INFO(" source: gps, vio, flow");
PX4_INFO(" duration: 0 (toggle state), else seconds");
}
int get_status()
void print_status()
{
PX4_INFO("Running: %s", _is_running ? "yes" : "no");
PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
return 0;
PX4_INFO("HIL Sensor received: %i", hil_sensor_counter);
PX4_INFO("IMU updates: %i", imu_counter);
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
PX4_INFO("Magnetometer sent: %i", mag_counter);
PX4_INFO("Barometer sent: %i", baro_counter);
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter);
PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter);
PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter);
PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter);
PX4_INFO("Actuator updates sent: %i", actuator_sent_counter);
PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter);
}
uint64_t first_sensor_msg_timestamp = 0;
uint64_t first_sensor_report_timestamp = 0;
uint64_t last_sensor_report_timestamp = 0;
void
clear_status_counters()
{
heartbeat_received_counter = 0;
heartbeat_sent_counter = 0;
imu_counter = 0;
hil_sensor_counter = 0;
mag_counter = 0;
baro_counter = 0;
actuator_sent_counter = 0;
odometry_received_counter = 0;
odometry_sent_counter = 0;
gps_received_counter = 0;
gps_sent_counter = 0;
distance_received_counter = 0;
distance_sent_counter = 0;
flow_received_counter = 0;
flow_sent_counter = 0;
unknown_msg_received_counter = 0;
}
void
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
@ -928,6 +1122,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
}
_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
mag_counter++;
}
}
@ -942,17 +1138,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
sensor_baro.error_count = 0;
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
}
// differential pressure
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
differential_pressure_s report{};
report.timestamp_sample = gyro_accel_time;
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
report.temperature = hil_sensor.temperature;
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
report.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(report);
baro_counter++;
}
// battery status
@ -970,7 +1157,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
_battery_pub.publish(hil_battery_status);
}
hil_sensor_counter++;
}
void
@ -989,15 +1175,41 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
gps.device_id = device_id.devid;
gps.lat = hil_gps.lat;
gps.lon = hil_gps.lon;
gps.alt = hil_gps.alt;
gps.alt_ellipsoid = hil_gps.alt;
gps.latitude_deg = hil_gps.lat * 1e-7;
gps.longitude_deg = hil_gps.lon * 1e-7;
gps.altitude_msl_m = hil_gps.alt * 1e-3;
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
gps.s_variance_m_s = 0.25f;
gps.c_variance_rad = 0.5f;
gps.satellites_used = hil_gps.satellites_visible;
gps.fix_type = hil_gps.fix_type;
int index = (int) position_source::GPS;
if (position_source_data[index].fail) {
uint32_t duration = position_source_data[index].failure_duration;
hrt_abstime start = position_source_data[index].failure_duration_start;
if (duration) {
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
PX4_INFO("GPS failure ending");
position_source_data[index].fail = false;
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
gps.satellites_used = 1;
gps.fix_type = 0;
}
} else {
gps.satellites_used = 1;
gps.fix_type = 0;
}
}
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
@ -1021,7 +1233,6 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
gps.timestamp_time_relative = 0;
gps.time_utc_usec = hil_gps.time_usec;
gps.satellites_used = hil_gps.satellites_visible;
gps.heading = NAN;
gps.heading_offset = NAN;
@ -1029,10 +1240,51 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
gps.timestamp = hrt_absolute_time();
_sensor_gps_pub.publish(gps);
gps_counter++;
gps_sent_counter++;
}
int
process_failure(dsp_hitl::position_source src, int duration)
{
if (src >= position_source::NUM_POSITION_SOURCES) {
return 1;
}
int index = (int) src;
if (position_source_data[index].send) {
if (duration <= 0) {
// Toggle state
if (position_source_data[index].fail) {
PX4_INFO("Ending indefinite %s failure", position_source_data[index].label);
position_source_data[index].fail = false;
} else {
PX4_INFO("Starting indefinite %s failure", position_source_data[index].label);
position_source_data[index].fail = true;
}
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
PX4_INFO("%s failure for %d seconds", position_source_data[index].label, duration);
position_source_data[index].fail = true;
position_source_data[index].failure_duration = duration;
position_source_data[index].failure_duration_start = hrt_absolute_time();
}
} else {
PX4_ERR("%s not active, cannot create failure", position_source_data[index].label);
return 1;
}
return 0;
}
} // End dsp_hitl namespace
int dsp_hitl_main(int argc, char *argv[])
{
int myoptind = 1;
@ -1044,20 +1296,47 @@ int dsp_hitl_main(int argc, char *argv[])
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
return dsp_hitl::start(argc - 1, argv + 1);
}
else if (!strcmp(verb, "stop")) {
} else if (!strcmp(verb, "stop")) {
return dsp_hitl::stop();
}
else if (!strcmp(verb, "status")) {
return dsp_hitl::get_status();
}
} else if (!strcmp(verb, "status")) {
dsp_hitl::print_status();
return 0;
else {
} else if (!strcmp(verb, "clear")) {
dsp_hitl::clear_status_counters();
return 0;
} else if (!strcmp(verb, "failure")) {
if (argc != 4) {
dsp_hitl::usage();
return 1;
}
const char *source = argv[myoptind + 1];
int duration = atoi(argv[myoptind + 2]);
if (!strcmp(source, "gps")) {
return dsp_hitl::process_failure(dsp_hitl::position_source::GPS, duration);
} else if (!strcmp(source, "vio")) {
return dsp_hitl::process_failure(dsp_hitl::position_source::VIO, duration);
} else if (!strcmp(source, "flow")) {
return dsp_hitl::process_failure(dsp_hitl::position_source::FLOW, duration);
} else {
PX4_ERR("Unknown failure source %s, duration %d", source, duration);
dsp_hitl::usage();
return 1;
}
return 0;
} else {
dsp_hitl::usage();
return 1;
}

View File

@ -1,991 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ICM42688P.hpp"
bool hitl_mode = false;
using namespace time_literals;
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
{
return (msb << 8u) | lsb;
}
ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
// SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency),
// I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
// _drdy_gpio(drdy_gpio)
SPI(config),
I2CSPIDriver(config),
_drdy_gpio(config.drdy_gpio),
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
if (config.drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
if (!hitl_mode) {
// _px4_accel = std::make_shared<PX4Accelerometer>(get_device_id(), rotation);
// _px4_gyro = std::make_shared<PX4Gyroscope>(get_device_id(), rotation);
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
// _imu_server_pub.advertise();
} else {
ConfigureSampleRate(0);
}
}
ICM42688P::~ICM42688P()
{
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_missed_perf);
// if (!hitl_mode){
// _imu_server_pub.unadvertise();
// }
}
int ICM42688P::init()
{
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
return Reset() ? 0 : -1;
}
bool ICM42688P::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
ScheduleClear();
ScheduleNow();
return true;
}
void ICM42688P::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void ICM42688P::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_missed_perf);
}
int ICM42688P::probe()
{
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami == WHOAMI) {
PX4_INFO("ICM42688P::probe successful!");
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_ERROR;
}
void ICM42688P::RunImpl()
{
PX4_INFO(">>> ICM42688P this: %p", this);
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// DEVICE_CONFIG: Software reset configuration
RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(2_ms); // to be safe wait 2 ms for soft reset to be effective
break;
case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
_state = STATE::CONFIGURE;
ScheduleDelayed(10_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
ScheduleDelayed(10_ms);
}
}
break;
case STATE::CONFIGURE:
if (Configure()) {
// Wakeup accel and gyro after configuring registers
ScheduleDelayed(1_ms); // add a delay here to be safe
RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
// if configure succeeded then start reading from FIFO
_state = STATE::FIFO_READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(100_ms);
} else {
PX4_ERR("ICM42688P::RunImpl interrupt configuration failed");
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
}
FIFOReset();
} else {
PX4_ERR("ICM42688P::RunImpl configuration failed");
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::FIFO_READ: {
#ifndef __PX4_QURT
uint32_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
} else {
samples = _fifo_gyro_samples;
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
if (samples == 0) {
// check current FIFO count
const uint16_t fifo_count = FIFOReadCount();
if (fifo_count >= FIFO::SIZE) {
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
} else {
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
samples = 0;
}
}
}
bool success = false;
if (samples >= 1) {
if (FIFORead(now, samples)) {
success = true;
if (_failure_count > 0) {
_failure_count--;
}
}
}
if (!success) {
_failure_count++;
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
return;
}
}
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
&& RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
) {
_last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
} else {
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
#endif
}
break;
}
}
void ICM42688P::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 800; // default to 800 Hz
}
// round down to nearest FIFO sample dt
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
ConfigureFIFOWatermark(_fifo_gyro_samples);
}
void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO watermark threshold in number of bytes
const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
for (auto &r : _register_bank0_cfg) {
if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
// FIFO_WM[7:0] FIFO_CONFIG2
r.set_bits = fifo_watermark_threshold & 0xFF;
} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
// FIFO_WM[11:8] FIFO_CONFIG3
r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
}
}
}
void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
cmd_bank_sel[1] = bank;
transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
_last_register_bank = bank;
}
}
bool ICM42688P::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_bank0_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
for (const auto &reg_cfg : _register_bank1_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
for (const auto &reg_cfg : _register_bank2_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg_cfg : _register_bank0_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
for (const auto &reg_cfg : _register_bank1_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
for (const auto &reg_cfg : _register_bank2_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
// // 20-bits data format used
// // the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
if (!hitl_mode) {
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
_px4_gyro.set_range(math::radians(2000.f));
_px4_gyro.set_scale(math::radians(1.f / 131.f));
}
return success;
}
static bool interrupt_debug = false;
static uint32_t interrupt_debug_count = 0;
static const uint32_t interrupt_debug_trigger = 800;
static hrt_abstime last_interrupt_time = 0;
static hrt_abstime avg_interrupt_delta = 0;
static hrt_abstime max_interrupt_delta = 0;
static hrt_abstime min_interrupt_delta = 60 * 1000 * 1000;
static hrt_abstime cumulative_interrupt_delta = 0;
int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
{
hrt_abstime current_interrupt_time = hrt_absolute_time();
if (interrupt_debug) {
if (last_interrupt_time) {
hrt_abstime interrupt_delta_time = current_interrupt_time - last_interrupt_time;
if (interrupt_delta_time > max_interrupt_delta) { max_interrupt_delta = interrupt_delta_time; }
if (interrupt_delta_time < min_interrupt_delta) { min_interrupt_delta = interrupt_delta_time; }
cumulative_interrupt_delta += interrupt_delta_time;
}
last_interrupt_time = current_interrupt_time;
interrupt_debug_count++;
if (interrupt_debug_count == interrupt_debug_trigger) {
avg_interrupt_delta = cumulative_interrupt_delta / interrupt_debug_trigger;
PX4_INFO(">>> Max: %llu, Min: %llu, Avg: %llu", max_interrupt_delta,
min_interrupt_delta, avg_interrupt_delta);
interrupt_debug_count = 0;
cumulative_interrupt_delta = 0;
}
}
static_cast<ICM42688P *>(arg)->DataReady();
return 0;
}
void ICM42688P::DataReady()
{
#ifndef __PX4_QURT
uint32_t expected = 0;
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
ScheduleNow();
}
#else
uint16_t fifo_byte_count = FIFOReadCount();
FIFORead(hrt_absolute_time(), fifo_byte_count / sizeof(FIFO::DATA));
#endif
}
bool ICM42688P::DataReadyInterruptConfigure()
{
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
}
bool ICM42688P::DataReadyInterruptDisable()
{
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
template <typename T>
bool ICM42688P::RegisterCheck(const T &reg_cfg)
{
bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
return success;
}
template <typename T>
uint8_t ICM42688P::RegisterRead(T reg)
{
uint8_t cmd[2] {};
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
SelectRegisterBank(reg);
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
template <typename T>
void ICM42688P::RegisterWrite(T reg, uint8_t value)
{
uint8_t cmd[2] { (uint8_t)reg, value };
SelectRegisterBank(reg);
transfer(cmd, cmd, sizeof(cmd));
}
template <typename T>
void ICM42688P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
}
uint16_t ICM42688P::FIFOReadCount()
{
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
return 0;
}
return combine(fifo_count_buf[1], fifo_count_buf[2]);
}
// static uint32_t debug_decimator = 0;
// static hrt_abstime last_sample_time = 0;
// static bool imu_debug = true;
bool ICM42688P::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
{
FIFOTransferBuffer buffer{};
const size_t max_transfer_size = 10 * sizeof(FIFO::DATA) + 4;
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, max_transfer_size);
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_count(_bad_transfer_perf);
return false;
}
if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
perf_count(_fifo_overflow_perf);
FIFOReset();
return false;
}
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
FIFOReset();
return false;
}
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_samples == 0) {
perf_count(_fifo_empty_perf);
return false;
}
// check FIFO header in every sample
uint16_t valid_samples = 0;
// for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
for (int i = 0; i < math::min(samples, (uint16_t) 10); i++) {
bool valid = true;
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8b_0110_10xx
const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
// FIFO sample empty if HEADER_MSG set
valid = false;
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
// accel bit not set
valid = false;
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
// gyro bit not set
valid = false;
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
// Packet does not contain a new and valid extended 20-bit data
valid = false;
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
// accel ODR changed
valid = false;
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
// gyro ODR changed
valid = false;
}
if (valid) {
valid_samples++;
} else {
perf_count(_bad_transfer_perf);
break;
}
}
// if (imu_debug) {
// debug_decimator++;
// if (debug_decimator == 801) {
// debug_decimator = 0;
// PX4_INFO("Initial: %u Next: %u Valid: %u Delta: %llu", samples, fifo_count_samples, valid_samples, timestamp_sample - last_sample_time);
// }
// last_sample_time = timestamp_sample;
// }
if (valid_samples > 0) {
if (ProcessTemperature(buffer.f, valid_samples)) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
ProcessIMU(timestamp_sample, buffer.f, valid_samples);
return true;
}
}
return false;
}
void ICM42688P::FIFOReset()
{
perf_count(_fifo_reset_perf);
// SIGNAL_PATH_RESET: FIFO flush
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
// reset while FIFO is disabled
_drdy_fifo_read_samples.store(0);
}
static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
{
// 0xXXXAABBC
uint32_t high = ((a << 12) & 0x000FF000);
uint32_t low = ((b << 4) & 0x00000FF0);
uint32_t lowest = (c & 0x0000000F);
uint32_t x = high | low | lowest;
if (a & Bit7) {
// sign extend
x |= 0xFFF00000u;
}
return static_cast<int32_t>(x);
}
void ICM42688P::ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
float gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0;
for (int i = 0; i < samples; i++) {
_imu_server_decimator++;
if (_imu_server_decimator == 8) {
_imu_server_decimator = 0;
// 20 bit hires mode
// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
// Accel data is 18 bit
int32_t temp_accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
int32_t temp_accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
int32_t temp_accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
int32_t temp_gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0,
fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
int32_t temp_gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0,
fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
int32_t temp_gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0,
fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
// accel samples invalid if -524288
if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
// shift accel by 2 (2 least significant bits are always 0)
accel_x = (float) temp_accel_x / 4.f;
accel_y = (float) temp_accel_y / 4.f;
accel_z = (float) temp_accel_z / 4.f;
// shift gyro by 1 (least significant bit is always 0)
gyro_x = (float) temp_gyro_x / 2.f;
gyro_y = (float) temp_gyro_y / 2.f;
gyro_z = (float) temp_gyro_z / 2.f;
// correct frame for publication
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
accel_y = -accel_y;
accel_z = -accel_z;
gyro_y = -gyro_y;
gyro_z = -gyro_z;
// Scale everything appropriately
float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
accel_x *= accel_scale_factor;
accel_y *= accel_scale_factor;
accel_z *= accel_scale_factor;
float gyro_scale_factor = math::radians(1.f / 131.f);
gyro_x *= gyro_scale_factor;
gyro_y *= gyro_scale_factor;
gyro_z *= gyro_scale_factor;
// Store the data in our array
_imu_server_data.accel_x[_imu_server_index] = accel_x;
_imu_server_data.accel_y[_imu_server_index] = accel_y;
_imu_server_data.accel_z[_imu_server_index] = accel_z;
_imu_server_data.gyro_x[_imu_server_index] = gyro_x;
_imu_server_data.gyro_y[_imu_server_index] = gyro_y;
_imu_server_data.gyro_z[_imu_server_index] = gyro_z;
_imu_server_data.ts[_imu_server_index] = timestamp_sample - (125 * (samples - 1 - i));
_imu_server_index++;
// If array is full, publish the data
if (_imu_server_index == 10) {
_imu_server_index = 0;
_imu_server_data.timestamp = hrt_absolute_time();
_imu_server_data.temperature = 0; // Not used right now
_imu_server_pub.publish(_imu_server_data);
}
}
}
}
}
void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT;
// 18-bits of accelerometer data
bool scale_20bit = false;
// first pass
for (int i = 0; i < samples; i++) {
// 20 bit hires mode
// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
// Accel data is 18 bit ()
int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
// sample invalid if -524288
if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
// check if any values are going to exceed int16 limits
static constexpr int16_t max_accel = INT16_MAX;
static constexpr int16_t min_accel = INT16_MIN;
if (accel_x >= max_accel || accel_x <= min_accel) {
scale_20bit = true;
}
if (accel_y >= max_accel || accel_y <= min_accel) {
scale_20bit = true;
}
if (accel_z >= max_accel || accel_z <= min_accel) {
scale_20bit = true;
}
// shift by 2 (2 least significant bits are always 0)
accel.x[accel.samples] = accel_x / 4;
accel.y[accel.samples] = accel_y / 4;
accel.z[accel.samples] = accel_z / 4;
accel.samples++;
}
}
if (!scale_20bit) {
// if highres enabled accel data is always 8192 LSB/g
if (!hitl_mode) {
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
}
} else {
// 20 bit data scaled to 16 bit (2^4)
for (int i = 0; i < samples; i++) {
// 20 bit hires mode
// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
// Accel data is 18 bit ()
int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
accel.x[i] = accel_x;
accel.y[i] = accel_y;
accel.z[i] = accel_z;
}
if (!hitl_mode) {
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
}
}
// correct frame for publication
for (int i = 0; i < accel.samples; i++) {
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[i] = accel.x[i];
accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
}
if (!hitl_mode) {
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
}
if (accel.samples > 0) {
if (!hitl_mode) {
_px4_accel.updateFIFO(accel);
}
}
}
void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_gyro_fifo_s gyro{};
gyro.timestamp_sample = timestamp_sample;
gyro.samples = 0;
gyro.dt = FIFO_SAMPLE_DT;
// 20-bits of gyroscope data
bool scale_20bit = false;
// first pass
for (int i = 0; i < samples; i++) {
// 20 bit hires mode
// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
// check if any values are going to exceed int16 limits
static constexpr int16_t max_gyro = INT16_MAX;
static constexpr int16_t min_gyro = INT16_MIN;
if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
scale_20bit = true;
}
if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
scale_20bit = true;
}
if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
scale_20bit = true;
}
gyro.x[gyro.samples] = gyro_x / 2;
gyro.y[gyro.samples] = gyro_y / 2;
gyro.z[gyro.samples] = gyro_z / 2;
gyro.samples++;
}
if (!scale_20bit) {
// if highres enabled gyro data is always 131 LSB/dps
if (!hitl_mode) {
_px4_gyro.set_scale(math::radians(1.f / 131.f));
}
} else {
// 20 bit data scaled to 16 bit (2^4)
for (int i = 0; i < samples; i++) {
gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
}
if (!hitl_mode) {
_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
}
}
// correct frame for publication
for (int i = 0; i < gyro.samples; i++) {
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
gyro.x[i] = gyro.x[i];
gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
}
if (!hitl_mode) {
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
}
if (gyro.samples > 0) {
if (!hitl_mode) {
_px4_gyro.updateFIFO(gyro);
}
}
}
bool ICM42688P::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
{
int16_t temperature[FIFO_MAX_SAMPLES];
float temperature_sum{0};
int valid_samples = 0;
for (int i = 0; i < samples; i++) {
const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
// sample invalid if -32768
if (t != -32768) {
temperature_sum += t;
temperature[valid_samples] = t;
valid_samples++;
}
}
if (valid_samples > 0) {
const float temperature_avg = temperature_sum / valid_samples;
for (int i = 0; i < valid_samples; i++) {
// temperature changing wildly is an indication of a transfer error
if (fabsf(temperature[i] - temperature_avg) > 1000) {
perf_count(_bad_transfer_perf);
return false;
}
}
// use average temperature reading
const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
if (PX4_ISFINITE(TEMP_degC)) {
if (!hitl_mode) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
return true;
}
} else {
perf_count(_bad_transfer_perf);
}
}
return false;
}

View File

@ -1,235 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ICM42688P.hpp
*
* Driver for the Invensense ICM42688P connected via SPI.
*
*/
#pragma once
#include "InvenSense_ICM42688P_registers.hpp"
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/topics/imu_server.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <memory>
using namespace InvenSense_ICM42688P;
extern bool hitl_mode;
class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
{
public:
// ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
// spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
ICM42688P(const I2CSPIDriverConfig &config);
~ICM42688P() override;
// static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
// int runtime_instance);
static void print_usage();
void RunImpl();
int init() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured
static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR};
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
// static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr uint32_t FIFO_MAX_SAMPLES{10};
// Transfer data
struct FIFOTransferBuffer {
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
uint8_t INT_STATUS{0};
uint8_t FIFO_COUNTH{0};
uint8_t FIFO_COUNTL{0};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// ensure no struct padding
static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)),
"Invalid FIFOTransferBuffer size");
struct register_bank0_config_t {
Register::BANK_0 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
struct register_bank1_config_t {
Register::BANK_1 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
struct register_bank2_config_t {
Register::BANK_2 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
int probe() override;
bool Reset();
bool Configure();
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
template <typename T> bool RegisterCheck(const T &reg_cfg);
template <typename T> uint8_t RegisterRead(T reg);
template <typename T> void RegisterWrite(T reg, uint8_t value);
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
void FIFOReset();
void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
const spi_drdy_gpio_t _drdy_gpio;
// std::shared_ptr<PX4Accelerometer> _px4_accel;
// std::shared_ptr<PX4Gyroscope> _px4_gyro;
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
perf_counter_t _drdy_missed_perf{nullptr};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
CONFIGURE,
FIFO_READ,
} _state{STATE::RESET};
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{12};
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
{ Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
{ Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
{ Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime
{ Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime
{ Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
};
uint8_t _checked_register_bank1{0};
static constexpr uint8_t size_register_bank1_cfg{4};
register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
{ Register::BANK_1::GYRO_CONFIG_STATIC3, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_CLEAR},
{ Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_CLEAR},
{ Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_CLEAR},
};
uint8_t _checked_register_bank2{0};
static constexpr uint8_t size_register_bank2_cfg{3};
register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_CLEAR },
{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_CLEAR },
};
uint32_t _temperature_samples{0};
// Support for the IMU server
uint32_t _imu_server_index{0};
uint32_t _imu_server_decimator{0};
imu_server_s _imu_server_data;
uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};
};

View File

@ -1,430 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file InvenSense_ICM42688P_registers.hpp
*
* Invensense ICM-42688-P registers.
*
*/
#pragma once
#include <cstdint>
namespace InvenSense_ICM42688P
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t WHOAMI = 0x47;
static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
static constexpr float TEMPERATURE_OFFSET = 25.f; // C
namespace Register
{
enum class BANK_0 : uint8_t {
DEVICE_CONFIG = 0x11,
INT_CONFIG = 0x14,
FIFO_CONFIG = 0x16,
TEMP_DATA1 = 0x1D,
TEMP_DATA0 = 0x1E,
INT_STATUS = 0x2D,
FIFO_COUNTH = 0x2E,
FIFO_COUNTL = 0x2F,
FIFO_DATA = 0x30,
SIGNAL_PATH_RESET = 0x4B,
INTF_CONFIG0 = 0x4C,
INTF_CONFIG1 = 0x4D,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
GYRO_CONFIG1 = 0x51,
GYRO_ACCEL_CONFIG0 = 0x52,
ACCEL_CONFIG1 = 0x53,
FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63,
INT_SOURCE0 = 0x65,
SELF_TEST_CONFIG = 0x70,
WHO_AM_I = 0x75,
REG_BANK_SEL = 0x76,
};
enum class BANK_1 : uint8_t {
GYRO_CONFIG_STATIC2 = 0x0B,
GYRO_CONFIG_STATIC3 = 0x0C,
GYRO_CONFIG_STATIC4 = 0x0D,
GYRO_CONFIG_STATIC5 = 0x0E,
INTF_CONFIG5 = 0x7B,
};
enum class BANK_2 : uint8_t {
ACCEL_CONFIG_STATIC2 = 0x03,
ACCEL_CONFIG_STATIC3 = 0x04,
ACCEL_CONFIG_STATIC4 = 0x05,
};
};
//---------------- BANK0 Register bits
// DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, //
};
// INT_CONFIG
enum INT_CONFIG_BIT : uint8_t {
INT1_MODE = Bit2,
INT1_DRIVE_CIRCUIT = Bit1,
INT1_POLARITY = Bit0,
};
// FIFO_CONFIG
enum FIFO_CONFIG_BIT : uint8_t {
// 7:6 FIFO_MODE
FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
};
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4,
DATA_RDY_INT = Bit3,
FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1,
};
// SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t {
ABORT_AND_RESET = Bit3,
FIFO_FLUSH = Bit1,
};
// PWR_MGMT0
enum PWR_MGMT0_BIT : uint8_t {
GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
};
// GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5,
GYRO_FS_SEL_500_DPS = Bit6,
GYRO_FS_SEL_250_DPS = Bit6 | Bit5,
GYRO_FS_SEL_125_DPS = Bit7,
// 3:0 GYRO_ODR
// 0001: 32kHz
GYRO_ODR_32KHZ_SET = Bit0,
GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz
GYRO_ODR_16KHZ_SET = Bit1,
GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
ACCEL_FS_SEL_8G = Bit5,
ACCEL_FS_SEL_4G = Bit6,
ACCEL_FS_SEL_2G = Bit6 | Bit5,
// 3:0 ACCEL_ODR
// 0001: 32kHz
ACCEL_ODR_32KHZ_SET = Bit0,
ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz
ACCEL_ODR_16KHZ_SET = Bit1,
ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default)
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
};
// GYRO_ACCEL_CONFIG0
enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
// 7:4 ACCEL_UI_FILT_BW
ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
// 3:0 GYRO_UI_FILT_BW
GYRO_UI_FILT_BW = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
};
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
};
// FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_HIRES_EN = Bit4,
FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1,
FIFO_ACCEL_EN = Bit0,
};
// INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR
CLEAR_ON_FIFO_READ = Bit3,
};
// INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t {
UI_FSYNC_INT1_EN = Bit6,
PLL_RDY_INT1_EN = Bit5,
RESET_DONE_INT1_EN = Bit4,
UI_DRDY_INT1_EN = Bit3,
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
FIFO_FULL_INT1_EN = Bit1,
UI_AGC_RDY_INT1_EN = Bit0,
};
// REG_BANK_SEL
enum REG_BANK_SEL_BIT : uint8_t {
USER_BANK_0 = 0, // 0: Select USER BANK 0.
USER_BANK_1 = Bit0, // 1: Select USER BANK 1.
USER_BANK_2 = Bit1, // 2: Select USER BANK 2.
USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3.
};
//---------------- BANK1 Register bits
// GYRO_CONFIG_STATIC2
enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
GYRO_AAF_DIS = Bit1,
GYRO_NF_DIS = Bit0,
};
// GYRO_CONFIG_STATIC3
enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
// 585 Hz
GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13
GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1,
// 213 Hz
// GYRO_AAF_DELT_SET = Bit2 | Bit0, //5
// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1,
// 126 Hz
//GYRO_AAF_DELT_SET = Bit1 | Bit0, //3
//GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2,
// 42 Hz
// GYRO_AAF_DELT_SET = Bit0, //1
// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
};
// GYRO_CONFIG_STATIC4
enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
// 585 Hz
GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170
GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
// 213 Hz
// GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
// 126 Hz
//GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9
//GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1,
// 42 Hz
// GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1
// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
};
// GYRO_CONFIG_STATIC5
enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
// 585 Hz
GYRO_AAF_DELTSQR_HIGH_SET = 0,
GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4
GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4,
// 213 Hz
// GYRO_AAF_DELTSQR_HIGH_SET = 0,
// GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
// GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
// 126 Hz
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12
// GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4,
// 42 Hz
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
// GYRO_AAF_BITSHIFT_CLEAR = 0,
};
//---------------- BANK2 Register bits
// ACCEL_CONFIG_STATIC2
enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
ACCEL_AAF_DIS = Bit0,
ACCEL_AAF_DELT = Bit3 | Bit1,
// 213 Hz
ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5
ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2,
// 42 Hz
// ACCEL_AAF_DELT_SET = Bit1, //1
// ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2,
};
// ACCEL_CONFIG_STATIC3
enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0,
// 213 Hz
ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
// 42 Hz
// ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1
// ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
};
// ACCEL_CONFIG_STATIC4
enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
ACCEL_AAF_BITSHIFT = Bit7 | Bit5,
ACCEL_AAF_DELTSQR_HIGH = 0,
// 213 Hz
ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
// 42 Hz
// ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
// ACCEL_AAF_BITSHIFT_CLEAR = 0,
ACCEL_AAF_DELTSQR_HIGH_SET = 0,
ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
};
namespace FIFO
{
static constexpr size_t SIZE = 2048;
// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
// Packet 4
struct DATA {
uint8_t FIFO_Header;
uint8_t ACCEL_DATA_X1; // Accel X [19:12]
uint8_t ACCEL_DATA_X0; // Accel X [11:4]
uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
uint8_t GYRO_DATA_X1; // Gyro X [19:12]
uint8_t GYRO_DATA_X0; // Gyro X [11:4]
uint8_t GYRO_DATA_Y1; // Gyro Y [19:12]
uint8_t GYRO_DATA_Y0; // Gyro Y [11:4]
uint8_t GYRO_DATA_Z1; // Gyro Z [19:12]
uint8_t GYRO_DATA_Z0; // Gyro Z [11:4]
uint8_t TEMP_DATA1; // Temperature[15:8]
uint8_t TEMP_DATA0; // Temperature[7:0]
uint8_t TimeStamp_h; // TimeStamp[15:8]
uint8_t TimeStamp_l; // TimeStamp[7:0]
uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
};
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8b_0110_10xx
enum FIFO_HEADER_BIT : uint8_t {
HEADER_MSG = Bit7, // 1: FIFO is empty
HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
};
}
} // namespace InvenSense_ICM42688P

View File

@ -1,116 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ICM42688P.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <string>
void ICM42688P::print_usage()
{
PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
// int runtime_instance)
// {
// ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
// cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
//
// if (!instance) {
// PX4_ERR("alloc failed");
// return nullptr;
// }
//
// if (OK != instance->init()) {
// delete instance;
// return nullptr;
// }
//
// return instance;
// }
extern "C" int icm42688p_main(int argc, char *argv[])
{
for (int i = 0; i <= argc - 1; i++) {
if (std::string(argv[i]) == "-h") {
argv[i] = 0;
hitl_mode = true;
break;
}
}
int ch;
using ThisDriver = ICM42688P;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = SPI_SPEED;
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optArg());
break;
}
}
const char *verb = cli.optArg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

View File

@ -0,0 +1,34 @@
############################################################################
#
# Copyright (c) 2024 ModalAI, Inc. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
include_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)

View File

@ -1,7 +1,7 @@
# Link against the public stub version of the proprietary fc sensor library
target_link_libraries(px4 PRIVATE
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
px4_layer
${module_libraries}
)

View File

@ -11,7 +11,6 @@ CONFIG_DRIVERS_VOXL2_IO=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_LOAD_MON=y
CONFIG_MODULES_LOGGER=y
CONFIG_MODULES_MAVLINK=y
CONFIG_MODULES_MUORB_APPS=y
@ -26,3 +25,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_ORB_COMMUNICATOR=y
CONFIG_PARAM_PRIMARY=y

View File

@ -1,10 +1,9 @@
#!/bin/bash
cd src/modules/muorb/apps/libfc-sensor-api
cd boards/modalai/voxl2/libfc-sensor-api
rm -fR build
mkdir build
cd build
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
make
cd ../../../../../..
cd ../../../../..

View File

@ -76,12 +76,13 @@ qshell flight_mode_manager start
# Start all of the processing modules on the applications processor
dataman start
navigator start
load_mon start
# Start microdds_client for ros2 offboard messages from agent over localhost
microdds_client start -t udp -h 127.0.0.1 -p 8888
qshell pwm_out_sim start -m hil
# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
# qshell dsp_hitl start -g -m -o -h -f
qshell dsp_hitl start -g -m
# start the onboard fast link to connect to voxl-mavlink-server

View File

@ -207,7 +207,6 @@ qshell flight_mode_manager start
# Start all of the processing modules on the applications processor
dataman start
navigator start
load_mon start
# This bridge allows raw data packets to be sent over UART to the ESC
voxl2_io_bridge start

View File

@ -1,9 +1,11 @@
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_DPS310=y
@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
@ -46,7 +49,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
CONFIG_MODULES_LOAD_MON=y
@ -96,4 +98,3 @@ CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
CONFIG_EXAMPLES_FAKE_GPS=y

View File

@ -6,4 +6,4 @@
param set-default BAT1_V_DIV 10.1
param set-default BAT1_A_PER_V 17
safety_button start
param set-default TEL_FRSKY_CONFIG 103

View File

@ -16,3 +16,5 @@ icm20948 -s -b 1 -R 8 -M start
# Interal DPS310 (barometer)
dps310 -s -b 2 start
safety_button start

View File

@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
CONFIG_ARCH_CHIP="stm32h7"
CONFIG_ARCH_CHIP_STM32H743II=y
CONFIG_ARCH_CHIP_STM32H743ZI=y
CONFIG_ARCH_CHIP_STM32H7=y
CONFIG_ARCH_INTERRUPTSTACK=768
CONFIG_ARMV7M_BASEPRI_WAR=y

View File

@ -222,6 +222,9 @@
/* UART/USART */
#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */
#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */
#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */
#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */
#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */
@ -235,9 +238,6 @@
#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */
#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */
#define GPIO_USART6_TX 0 /* USART6 is RX-only */
#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */
#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */
#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */
@ -268,13 +268,14 @@
#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */
#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */
#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */
#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */
#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */
/* I2C */
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */
#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */
#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */

View File

@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
CONFIG_ARCH_CHIP="stm32h7"
CONFIG_ARCH_CHIP_STM32H743II=y
CONFIG_ARCH_CHIP_STM32H743ZI=y
CONFIG_ARCH_CHIP_STM32H7=y
CONFIG_ARCH_INTERRUPTSTACK=768
CONFIG_ARCH_STACKDUMP=y
@ -192,7 +192,6 @@ CONFIG_STM32H7_DMA2=y
CONFIG_STM32H7_DMACAPABLE=y
CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
CONFIG_STM32H7_I2C1=y
CONFIG_STM32H7_I2C3=y
CONFIG_STM32H7_I2C4=y
CONFIG_STM32H7_I2C_DYNTIMEO=y
CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
@ -212,17 +211,20 @@ CONFIG_STM32H7_SPI2=y
CONFIG_STM32H7_SPI5=y
CONFIG_STM32H7_SPI5_DMA=y
CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
CONFIG_STM32H7_SPI6=y
CONFIG_STM32H7_TIM15=y
CONFIG_STM32H7_TIM16=y
CONFIG_STM32H7_TIM1=y
CONFIG_STM32H7_TIM2=y
CONFIG_STM32H7_TIM3=y
CONFIG_STM32H7_TIM4=y
CONFIG_STM32H7_TIM8=y
CONFIG_STM32H7_USART1=y
CONFIG_STM32H7_USART2=y
CONFIG_STM32H7_USART3=y
CONFIG_STM32H7_UART4=y
CONFIG_STM32H7_UART7=y
CONFIG_STM32H7_UART8=y
CONFIG_STM32H7_USART2=y
CONFIG_STM32H7_USART3=y
CONFIG_STM32H7_USART6=y
CONFIG_STM32H7_USART_BREAKS=y
CONFIG_STM32H7_USART_INVERT=y
CONFIG_STM32H7_USART_SINGLEWIRE=y
@ -252,9 +254,6 @@ CONFIG_USART3_IFLOWCONTROL=y
CONFIG_USART3_OFLOWCONTROL=y
CONFIG_USART3_RXBUFSIZE=600
CONFIG_USART3_TXBUFSIZE=3000
CONFIG_USART6_BAUD=57600
CONFIG_USART6_RXBUFSIZE=600
CONFIG_USART6_TXBUFSIZE=1500
CONFIG_USBDEV=y
CONFIG_USBDEV_BUSPOWERED=y
CONFIG_USBDEV_MAXPOWER=500

View File

@ -45,95 +45,111 @@
#include <stm32_gpio.h>
/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
#define BOARD_HAS_CONTROL_STATUS_LEDS 1
#define BOARD_OVERLOAD_LED LED_RED
#define BOARD_ARMED_STATE_LED LED_BLUE
/* ADC channels */
#define PX4_ADC_GPIO \
/* PA2 */ GPIO_ADC12_INP14, \
/* PC4 */ GPIO_ADC12_INP4, \
/* PA3 */ GPIO_ADC12_INP15, \
/* PA4 */ GPIO_ADC12_INP18, \
/* PC1 */ GPIO_ADC123_INP11
/* PC0 */ GPIO_ADC123_INP10, \
/* PC5 */ GPIO_ADC12_INP8, \
/* PB0 */ GPIO_ADC12_INP9, \
/* PB1 */ GPIO_ADC12_INP5
/* Define Channel numbers must match above GPIO pins */
#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */
#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */
#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */
#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */
#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */
#define ADC_RC_RSSI_CHANNEL 10 /* PC0 */
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 8 /* PC5 */
#define ADC_AUX1_VOLTAGE_CHANNEL 9 /* PB0 */
#define ADC_AUX2_VOLTAGE_CHANNEL 5 /* PB1 */
#define ADC_CHANNELS \
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
(1 << ADC_BATTERY_CURRENT_CHANNEL) | \
(1 << ADC_SCALED_V5_CHANNEL) | \
(1 << ADC_RC_RSSI_CHANNEL))
(1 << ADC_RC_RSSI_CHANNEL) | \
(1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \
(1 << ADC_AUX1_VOLTAGE_CHANNEL) | \
(1 << ADC_AUX2_VOLTAGE_CHANNEL))
/* HW has to large of R termination on ADC todo:change when HW value is chosen */
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
/* CAN Silence: Silent mode control */
#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
#define GPIO_CAN1_SILENT_S0 /* PF11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11)
#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
/* PWM */
#define DIRECT_PWM_OUTPUT_CHANNELS 12
/* Power supply control and monitoring GPIOs */
#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
#define BOARD_NUMBER_BRICKS 1
#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
/* Define True logic Power Control in arch agnostic form */
#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
/* Tone alarm output */
#define TONE_ALARM_TIMER 2 /* timer 2 */
#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */
#define TONE_ALARM_TIMER 16 /* timer 16 */
#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */
#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)
#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1
#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2
#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2
/* USB OTG FS */
#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
/* High-resolution timer */
#define HRT_TIMER 3 /* use timer3 for the HRT */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */
#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */
#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */
#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
/* RC Serial port */
#define RC_SERIAL_PORT "/dev/ttyS3"
#define RC_SERIAL_PORT "/dev/ttyS5"
#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)
/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
/* No PX4IO processor present */
#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
/* Power switch controls ******************************************************/
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
/*
* Board has a separate RC_IN
*
* GPIO PPM_IN on PB0 T3CH3
* GPIO PPM_IN on PC7 T3CH2
* SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
* Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
*/
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
@ -156,7 +172,7 @@
#define BOARD_HAS_STATIC_MANIFEST 1
#define BOARD_NUM_IO_TIMERS 5
#define BOARD_NUM_IO_TIMERS 6
#define BOARD_ENABLE_CONSOLE_BUFFER
@ -171,9 +187,12 @@
GPIO_CAN2_SILENT_S0, \
GPIO_nPOWER_IN_A, \
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
GPIO_LEVEL_SHIFTER_OE, \
GPIO_TONE_ALARM_IDLE, \
GPIO_SAFETY_SWITCH_IN, \
GPIO_OTGFS_VBUS, \
GPIO_BTN_SAFETY, \
GPIO_LED_SAFETY, \
}
__BEGIN_DECLS

View File

@ -35,6 +35,5 @@
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
initI2CBusExternal(1),
initI2CBusExternal(3),
initI2CBusExternal(4),
};

View File

@ -37,7 +37,7 @@
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
}, {GPIO::PortE, GPIO::Pin3}),
initSPIBus(SPI::Bus::SPI2, {
@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
}),
};

View File

@ -33,6 +33,28 @@
#include <px4_arch/io_timer_hw_description.h>
/* Timer allocation
*
* TIM1_CH4 T FMU_CH1
* TIM1_CH3 T FMU_CH2
* TIM1_CH2 T FMU_CH3
* TIM1_CH1 T FMU_CH4
*
* TIM4_CH2 T FMU_CH5
* TIM4_CH3 T FMU_CH6
* TIM2_CH3 T FMU_CH7
* TIM2_CH1 T FMU_CH8
*
* TIM2_CH4 T FMU_CH9
* TIM15_CH1 T FMU_CH10
*
* TIM8_CH1 T FMU_CH11
*
* TIM4_CH4 T FMU_CH12
*
* TIM16_CH1 T BUZZER - Driven by other driver
*/
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),

View File

@ -18,4 +18,4 @@ CONFIG_MODULES_UUV_POS_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

View File

@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@ -102,9 +101,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_SYSTEMCMDS_PERF=y
CONFIG_SYSTEMCMDS_REBOOT=y
CONFIG_SYSTEMCMDS_REFLECT=y
CONFIG_SYSTEMCMDS_SD_BENCH=y
CONFIG_SYSTEMCMDS_SD_STRESS=y
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
CONFIG_SYSTEMCMDS_TOP=y
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

View File

@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0
param set-default SENS_EN_INA228 0
param set-default SENS_EN_INA226 1
if ver hwbasecmp 008 009 00a 010
if ver hwbasecmp 008 009 00a 010 011
then
# Skynode: use the "custom participant" config for uxrce_dds_client
param set-default UXRCE_DDS_PTCFG 2

View File

@ -3,7 +3,7 @@
# board specific MAVLink startup script.
#------------------------------------------------------------------------------
if ver hwbasecmp 008 009 00a 010
if ver hwbasecmp 008 009 00a 010 011
then
# Start MAVLink on the UART connected to the mission computer
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

View File

@ -49,7 +49,7 @@ then
fi
fi
if ver hwbasecmp 008 009 00a 010
if ver hwbasecmp 008 009 00a 010 011
then
#SKYNODE base fmu board orientation

View File

@ -1,21 +1,16 @@
CONFIG_DRIVERS_IRLOCK=n
CONFIG_MODULES_AIRSPEED_SELECTOR=n
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
CONFIG_MODULES_FW_ATT_CONTROL=n
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_FW_POS_CONTROL=n
CONFIG_MODULES_FW_RATE_CONTROL=n
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
CONFIG_MODULES_MC_ATT_CONTROL=n
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_UUV_ATT_CONTROL=n
CONFIG_MODULES_UUV_POS_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y

View File

@ -1,21 +1,16 @@
CONFIG_DRIVERS_IRLOCK=n
CONFIG_MODULES_AIRSPEED_SELECTOR=n
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
CONFIG_MODULES_FW_ATT_CONTROL=n
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_FW_POS_CONTROL=n
CONFIG_MODULES_FW_RATE_CONTROL=n
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
CONFIG_MODULES_MC_ATT_CONTROL=n
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_UUV_ATT_CONTROL=n
CONFIG_MODULES_UUV_POS_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

View File

@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

View File

@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0
param set-default SENS_EN_INA228 0
param set-default SENS_EN_INA226 0
if ver hwbasecmp 009 010
if ver hwbasecmp 009 010 011
then
# Skynode: use the "custom participant" config for uxrce_dds_client
param set-default UXRCE_DDS_PTCFG 2

View File

@ -4,7 +4,7 @@
#------------------------------------------------------------------------------
# if skynode base board is detected start Mavlink on Telem2
if ver hwbasecmp 009 010
if ver hwbasecmp 009 010 011
then
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

View File

@ -12,6 +12,7 @@ fi
if param compare -s ADC_ADS1115_EN 1
then
ads1115 start -X
board_adc start -n
else
board_adc start
fi
@ -55,7 +56,7 @@ then
fi
#Start Auterion Power Module selector for Skynode boards
if ver hwbasecmp 009 010
if ver hwbasecmp 009 010 011
then
pm_selector_auterion start
else
@ -92,7 +93,7 @@ else
icm20649 -s -R 6 start
else
# Internal SPI BMI088
if ver hwbasecmp 009 010
if ver hwbasecmp 009 010 011
then
bmi088 -A -R 6 -s start
bmi088 -G -R 6 -s start
@ -109,7 +110,7 @@ else
fi
# Internal SPI bus ICM42688p
if ver hwbasecmp 009 010
if ver hwbasecmp 009 010 011
then
icm42688p -R 12 -s start
else
@ -126,7 +127,7 @@ else
# Internal SPI bus ICM-42670-P (hard-mounted)
icm42670p -R 10 -s start
else
if ver hwbasecmp 009 010
if ver hwbasecmp 009 010 011
then
icm20602 -R 6 -s start
else

View File

@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

View File

@ -6,21 +6,30 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
CONFIG_COMMON_INS=y
CONFIG_COMMON_LIGHT=y
CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_DRIVERS_OSD_MSP_OSD=y
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
@ -32,14 +41,20 @@ CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_COMMON_UWB=y
CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
@ -57,11 +72,15 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
CONFIG_MODULES_VTOL_ATT_CONTROL=y
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
CONFIG_SYSTEMCMDS_BSONDUMP=y
CONFIG_SYSTEMCMDS_DMESG=y
CONFIG_SYSTEMCMDS_DUMPFILE=y
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y

View File

@ -4,7 +4,7 @@
#------------------------------------------------------------------------------
# if skynode base board is detected start Mavlink on Telem2
if ver hwbasecmp 009 010
if ver hwbasecmp 009 010 011
then
mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z

View File

@ -257,7 +257,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKSTACKSIZE=1632
CONFIG_SCHED_LPWORKSTACKSIZE=2032
CONFIG_SCHED_WAITPID=y
CONFIG_SDIO_BLOCKSETUP=y
CONFIG_SEM_PREALLOCHOLDERS=32

View File

@ -1,4 +1,9 @@
CONFIG_MODULES_AIRSPEED_SELECTOR=n
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
CONFIG_MODULES_FW_ATT_CONTROL=n
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_FW_POS_CONTROL=n
CONFIG_MODULES_FW_RATE_CONTROL=n
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
CONFIG_MODULES_MC_ATT_CONTROL=n
@ -6,6 +11,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y

View File

@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_EKF2=y
CONFIG_EKF2_VERBOSE_STATUS=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y

View File

@ -58,6 +58,7 @@ set(msg_files
CameraCapture.msg
CameraStatus.msg
CameraTrigger.msg
CanInterfaceStatus.msg
CellularStatus.msg
CollisionConstraints.msg
CollisionReport.msg
@ -99,6 +100,7 @@ set(msg_files
FollowTargetStatus.msg
GeneratorStatus.msg
GeofenceResult.msg
GeofenceStatus.msg
GimbalControls.msg
GimbalDeviceAttitudeStatus.msg
GimbalDeviceInformation.msg
@ -153,6 +155,10 @@ set(msg_files
OrbTest.msg
OrbTestLarge.msg
OrbTestMedium.msg
ParameterResetRequest.msg
ParameterSetUsedRequest.msg
ParameterSetValueRequest.msg
ParameterSetValueResponse.msg
ParameterUpdate.msg
Ping.msg
PositionControllerLandingStatus.msg
@ -173,6 +179,7 @@ set(msg_files
RegisterExtComponentReply.msg
RegisterExtComponentRequest.msg
Rpm.msg
RtlStatus.msg
RtlTimeEstimate.msg
SatelliteInfo.msg
SensorAccel.msg
@ -283,6 +290,9 @@ foreach(msg_file ${msg_files})
list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
endforeach()
# set parent scope msg_files for ROS
set(msg_files ${msg_files} PARENT_SCOPE)
# Generate uORB headers
add_custom_command(
OUTPUT

View File

@ -0,0 +1,6 @@
uint64 timestamp # time since system start (microseconds)
uint8 interface
uint64 io_errors
uint64 frames_tx
uint64 frames_rx

Some files were not shown because too many files have changed in this diff Show More