Compare commits

...

30 Commits

Author SHA1 Message Date
Silvan Fuhrer 6552468df1 VTOL: use the same logic for front transition complete in all vtol types
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-05-29 15:57:25 +02:00
Jacob Dahl a4650fd70d HealthCheck: added health check for logger to report if it's running (#22781) 2024-05-29 11:56:50 +02:00
Hamish Willee b5627f487f camera_trigger: module docs for camera trigger driver (#23104) 2024-05-29 11:37:27 +02:00
Matthias Grob d1db0addf9 CameraFeedback: shorten line length such that documentation parser works
This broke in 4f64acb352 and was also flagged by CI in the pr and since then.
2024-05-29 11:24:53 +02:00
Silvan Fuhrer 032ae69eee VTOL: remove _dt passing as it's no longer used (and was wrong)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-05-28 14:54:33 +02:00
Silvan Fuhrer f8fe7c7aa3 VTOL Standard: fix transition pusher motor slew rate
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-05-28 14:54:33 +02:00
Matthias Grob dfee9ca4c6 MAVLink: remove never used _mavlink_link_termination_allowed 2024-05-28 10:41:00 +02:00
Silvan Fuhrer 1206005ed2 RTL_status: improve comment
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-05-27 12:03:16 +02:00
Silvan Fuhrer 42bca65cbf RTL_mission_reverse: start from previous WP if RTL is triggered while in Mission
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-05-27 12:03:16 +02:00
Silvan Fuhrer b9d3b9f211 RTL_mission_fast: continue mission if RTL is triggered while in Mission
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2024-05-27 12:03:16 +02:00
bresch f9e2ab8d44 msg-rates-setpoint: fix frame name NED -> FRD 2024-05-27 09:35:34 +02:00
Jacob Dahl da35c4adce cdcacm_autostart: handle USB power only (#23183) 2024-05-25 17:16:34 -06:00
bresch ccbcbbe268 wind_est_replay: report scale instead of inverse_scale
The estimator internally estimates the scale inverse, but the interface
should be the scale as "airspeed_corrected = scale * airspeed"
2024-05-24 17:25:39 +02:00
Hamish Willee 4f64acb352 Docs for camera_feedback module (#23103)
* Docs for camera_feedback module

* Update src/modules/camera_feedback/CameraFeedback.cpp

* Update src/modules/camera_feedback/CameraFeedback.cpp
2024-05-23 08:44:34 +10:00
Beat Küng e1ffc2cdaa commander: add check for 5V overcurrent 2024-05-22 09:34:04 +02:00
Julian Oes a9962dc44d boards: update all bootloaders 2024-05-22 18:18:55 +12:00
Julian Oes 5bace785e0 px_uploader: catch serial exception correctly
Signed-off-by: Julian Oes <julian@oes.ch>
2024-05-22 18:18:55 +12:00
Julian Oes e4fd80b6ef bootloader: remove unused/duplicate defines 2024-05-22 18:18:55 +12:00
Julian Oes 6ebb2b33df bootloader: track ArduPilot protocol
Just so we don't conflict on these commands in the future.
2024-05-22 18:18:55 +12:00
Julian Oes 8fe8f2fcb3 px_uploader.py: clean up various tidbits
Includes:
- Remove some of the outdated Python2 checks and compatibility.
- Try not catch all exceptions but only the expected ones. Otherwise,
  this makes it really hard to debug if anything unexpected actually
  goes wrong.
- Make use of fstrings.
- Make output slightly prettier.

Signed-off-by: Julian Oes <julian@oes.ch>
2024-05-22 18:18:55 +12:00
Julian Oes 7c4507b6d6 bootloader: add bootloader version
This adds a new protocol extension which allows to get the bootloader
version.

The bootloader version is different from the bootloader protocol
revision which has stabilized at 5 and is not easy to update unless a
bootloader is actually breaking the protocol. The reason being that both
the Python script as well as the uploader used in QGC will not attempt
to load firmware if they don't know the bootloader version, so it could
basically be considered a "breaking" protocol revision.

Signed-off-by: Julian Oes <julian@oes.ch>
2024-05-22 18:18:55 +12:00
Julian Oes 21e550fdba tools/bootloader: add force-erase option
If the STM32H7 fails to program or erase a full chunk of 256 bytes, the
ECC check will trigger a busfault when trying to read from it.

To speed up erasing and optimize wear, we read before erasing to check
if it actually needs erasing. That's when a busfault happens and the
erase time outs.

The workaround is to add an option to do a full erase without check.

Credit goes to:
https://github.com/ArduPilot/ardupilot/pull/22090

And the protocol option added to the bootloader is the same as for
ArduPilot, so compatible.

Signed-off-by: Julian Oes <julian@oes.ch>
2024-05-22 18:18:55 +12:00
Jacob Dahl 4a13e495d7 boards: ark: pi6x: CONFIG_DRIVERS_CDCACM_AUTOSTART=y (#23163) 2024-05-21 19:49:40 -06:00
Konrad 664a0f2cda HomePosition: Add minimum position change needed to be recognised as new home position 2024-05-21 09:11:56 +02:00
alexklimaj 1c213fa760 boards: arkv6x ark_pi6x change mavlink dialect to development 2024-05-20 16:07:34 -04:00
Jacob Dahl e72ecdbefb drivers/imu: new Murata SCH16T IMU driver (#22914)
---------

Co-authored-by: alexklimaj <alex@arkelectron.com>
2024-05-20 14:38:19 -04:00
Jacob Dahl 70304fe715 [mavlink] Parameter to always start on USB (#22234)
* usb: Added parameter to enable always starting mavlink on USB.

    Refactored cdcacm_init into a module and added a paramter to allow always starting mavlink on
    USB, also added a paramter to choose the mode. The current default behavior is to wait and listen
    for data on USB and auto-detect the protocol (mavlink, nsh, ublox). This results in the mavlink
    stream not starting until something else on the mavlink network sends a packet first. The new
    default behavior is to always start mavlink.

    Added parameters
    MAV_USB_ENABLE -- default 1 (always start mavlink on USB)
    MAV_USE_MODE -- default 3 (onboard)

* added 3 retries for opening serial port in mavlink, removed sleep before sercon

* added DRIVERS_CDCACM_AUTOSTART to ark-v6x default.px4board

* added CONFIG_DRIVERS_CDCACM_AUTOSTART=y to default.px4board for boards with CONFIG_CDCACM in their nsh/defconfig

* format

* remove PGA460 from COMMON_DISTANCE_SENSOR to save flash

* remove LIS2MDL from COMMON_MAGNETOMETER to save flash

* disable CONFIG_DRIVERS_CDCACM_AUTOSTART for fmu-v5 protected.px4board

* moved and renamed parameters, removed mode logic in mavlink

* changed parameter names, added mode none

* remove parameters from mavlink
2024-05-20 12:35:29 -06:00
Peter van der Perk 6b0ac49daf hardfault_log: Add jump to 0x0 & write 0x0 faults 2024-05-17 14:43:23 -04:00
Peter van der Perk ebfa53286f dronecan: SocketCAN driver check size before copying
Avoids memory corruption if we get packets to big
2024-05-17 14:39:52 -04:00
Peter van der Perk 470bea9ba8 Update NuttX
Fixes imxrt1170 mpu config for extra checks
2024-05-17 14:32:43 -04:00
140 changed files with 2471 additions and 212 deletions
+4 -2
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2017 - 2022 PX4 Development Team. All rights reserved.
# Copyright (c) 2017 - 2024 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
@@ -132,7 +132,9 @@ list(GET VERSION_LIST 2 PX4_VERSION_PATCH)
string(REPLACE "-" ";" PX4_VERSION_PATCH ${PX4_VERSION_PATCH})
list(GET PX4_VERSION_PATCH 0 PX4_VERSION_PATCH)
message(STATUS "PX4 version: ${PX4_GIT_TAG} (${PX4_VERSION_MAJOR}.${PX4_VERSION_MINOR}.${PX4_VERSION_PATCH})")
# # Capture only the hash part after 'g'
string(REGEX MATCH "g([a-f0-9]+)$" GIT_HASH "${PX4_GIT_TAG}")
set(PX4_GIT_HASH ${CMAKE_MATCH_1})
define_property(GLOBAL PROPERTY PX4_MODULE_LIBRARIES
BRIEF_DOCS "PX4 module libs"
+6
View File
@@ -138,6 +138,12 @@ then
adis16507 -S start
fi
# SCH16T spi external IMU
if param compare -s SENS_EN_SCH16T 1
then
sch16t -S start
fi
# Eagle Tree airspeed sensor external I2C
if param compare -s SENS_EN_ETSASPD 1
then
+8
View File
@@ -439,6 +439,14 @@ else
# Must be started after the serial config is read
rc_input start $RC_INPUT_ARGS
# Manages USB interface
if ! cdcacm_autostart start
then
sercon
echo "Starting MAVLink on /dev/ttyACM0"
mavlink start -d /dev/ttyACM0
fi
#
# Play the startup tune (if not disabled or there is an error)
#
+1 -1
View File
@@ -15,7 +15,7 @@ class ModuleDocumentation(object):
# TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md
valid_categories = ['driver', 'estimator', 'controller', 'system',
'communication', 'command', 'template', 'simulation', 'autotune']
valid_subcategories = ['', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
valid_subcategories = ['', 'camera', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']
max_line_length = 80 # wrap lines that are longer than this
+76 -49
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3
############################################################################
#
# Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
# Copyright (c) 2012-2024 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
@@ -50,9 +50,6 @@
# Currently only used for informational purposes.
#
# for python2.7 compatibility
from __future__ import print_function
import sys
import argparse
import binascii
@@ -70,35 +67,32 @@ from sys import platform as _platform
try:
import serial
except ImportError as e:
print("Failed to import serial: " + str(e))
print(f"Failed to import serial: {e}")
print("")
print("You may need to install it using:")
print(" pip3 install --user pyserial")
print(" python -m pip install pyserial")
print("")
sys.exit(1)
# Define time to use time.time() by default
def _time():
return time.time()
# Detect python version
if sys.version_info[0] < 3:
runningPython3 = False
else:
runningPython3 = True
if sys.version_info[1] >=3:
# redefine to use monotonic time when available
def _time():
try:
return time.monotonic()
except Exception:
return time.time()
raise RuntimeError("Python 2 is not supported. Please try again using Python 3.")
sys.exit(1)
# Use monotonic time where available
def _time():
try:
return time.monotonic()
except Exception:
return time.time()
class FirmwareNotSuitableException(Exception):
def __init__(self, message):
super(FirmwareNotSuitableException, self).__init__(message)
class firmware(object):
'''Loads a firmware file'''
@@ -163,13 +157,13 @@ class firmware(object):
def crc(self, padlen):
state = self.__crc32(self.image, int(0))
for i in range(len(self.image), (padlen - 1), 4):
for _ in range(len(self.image), (padlen - 1), 4):
state = self.__crc32(self.crcpad, state)
return state
class uploader(object):
'''Uploads a firmware file to the PX FMU bootloader'''
class uploader:
'''Uploads a firmware file to the PX4 bootloader'''
# protocol bytes
INSYNC = b'\x12'
@@ -195,6 +189,8 @@ class uploader(object):
GET_CHIP = b'\x2c' # rev5+ , get chip version
SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay
GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII
GET_VERSION = b'\x2f' # rev5+ , get chip description in ASCII
CHIP_FULL_ERASE = b'\x40' # full erase of flash, rev6+
MAX_DES_LENGTH = 20
REBOOT = b'\x30'
@@ -205,6 +201,7 @@ class uploader(object):
INFO_BOARD_ID = b'\x02' # board type
INFO_BOARD_REV = b'\x03' # board revision
INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes
BL_VERSION = b'\x07' # get bootloader version, e.g. major.minor.patch.githash (up to 20 chars)
PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4
READ_MULTI_MAX = 252 # protocol max is 255
@@ -235,6 +232,7 @@ class uploader(object):
self.baudrate_bootloader = baudrate_bootloader
self.baudrate_flightstack = baudrate_flightstack
self.baudrate_flightstack_idx = -1
self.force_erase = False
def close(self):
if self.port is not None:
@@ -357,19 +355,22 @@ class uploader(object):
self.port.baudrate = self.baudrate_bootloader * 2.33
except NotImplementedError as e:
# This error can occur because pySerial on Windows does not support odd baudrates
print(str(e) + " -> could not check for FTDI device, assuming USB connection")
print(f"{e} -> could not check for FTDI device, assuming USB connection")
return
self.__send(uploader.GET_SYNC +
uploader.EOC)
try:
self.__getSync(False)
except:
except RuntimeError:
# if it fails we are on a real serial port - only leave this enabled on Windows
if sys.platform.startswith('win'):
self.ackWindowedMode = True
finally:
self.port.baudrate = self.baudrate_bootloader
try:
self.port.baudrate = self.baudrate_bootloader
except Exception:
pass
# send the GET_DEVICE command and wait for an info parameter
def __getInfo(self, param):
@@ -410,6 +411,17 @@ class uploader(object):
pieces = value.split(b",")
return pieces
def __getVersion(self):
self.__send(uploader.GET_VERSION + uploader.EOC)
try:
length = self.__recv_int()
value = self.__recv(length)
self.__getSync()
except RuntimeError:
# Bootloader doesn't support version call
return "unknown"
return value.decode()
def __drawProgressBar(self, label, progress, maxVal):
if maxVal < progress:
progress = maxVal
@@ -421,10 +433,16 @@ class uploader(object):
# send the CHIP_ERASE command and wait for the bootloader to become ready
def __erase(self, label):
print("Windowed mode: %s" % self.ackWindowedMode)
print(f"Windowed mode: {self.ackWindowedMode}")
print("\n", end='')
self.__send(uploader.CHIP_ERASE +
uploader.EOC)
if self.force_erase:
print("Trying force erase of full chip...\n")
self.__send(uploader.CHIP_FULL_ERASE +
uploader.EOC)
else:
self.__send(uploader.CHIP_ERASE +
uploader.EOC)
# erase is very slow, give it 30s
deadline = _time() + 30.0
@@ -441,9 +459,14 @@ class uploader(object):
if self.__trySync():
self.__drawProgressBar(label, 10.0, 10.0)
if self.force_erase:
print("\nForce erase done.\n")
return
raise RuntimeError("timed out waiting for erase")
if self.force_erase:
raise RuntimeError("timed out waiting for erase, force erase is likely not supported by bootloader!")
else:
raise RuntimeError("timed out waiting for erase")
# send a PROG_MULTI command to write a collection of bytes
def __program_multi(self, data, windowMode):
@@ -581,8 +604,11 @@ class uploader(object):
self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV)
self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
self.version = self.__getVersion()
# upload the firmware
def upload(self, fw_list, force=False, boot_delay=None, boot_check=False):
def upload(self, fw_list, force=False, boot_delay=None, boot_check=False, force_erase=False):
self.force_erase = force_erase
# select correct binary
found_suitable_firmware = False
for file in fw_list:
@@ -611,6 +637,8 @@ class uploader(object):
print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%) " % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
print()
print(f"Bootloader version: {self.version}")
# Make sure we are doing the right thing
start = _time()
if self.board_type != fw.property('board_id'):
@@ -640,14 +668,14 @@ class uploader(object):
self.otp_coa = self.otp[32:160]
# show user:
try:
print("sn: ", end='')
print("Sn: ", end='')
for byte in range(0, 12, 4):
x = self.__getSN(byte)
x = x[::-1] # reverse the bytes
self.sn = self.sn + x
print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
print('')
print("chip: %08x" % self.__getCHIP())
print("Chip: %08x" % self.__getCHIP())
otp_id = self.otp_id.decode('Latin-1')
if ("PX4" in otp_id):
@@ -657,17 +685,19 @@ class uploader(object):
print("OTP pid: " + binascii.hexlify(self.otp_pid).decode('Latin-1'))
print("OTP coa: " + binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
except Exception:
except Exception as e:
# ignore bad character encodings
print(f"Exception ignored: {e}")
pass
# Silicon errata check was added in v5
if (self.bl_rev >= 5):
des = self.__getCHIPDes()
if (len(des) == 2):
print("family: %s" % des[0])
print("revision: %s" % des[1])
print("flash: %d bytes" % self.fw_maxsize)
family, revision = des
print(f"Family: {family.decode()}")
print(f"Revision: {revision.decode()}")
print(f"Flash: {self.fw_maxsize} bytes")
# Prevent uploads where the maximum image size of the board config is smaller than the flash
# of the board. This is a hint the user chose the wrong config and will lack features
@@ -678,8 +708,7 @@ class uploader(object):
# https://github.com/PX4/Firmware/blob/master/src/drivers/boards/common/stm32/board_mcu_version.c#L125-L144
if self.fw_maxsize > fw.property('image_maxsize') and not force:
raise RuntimeError("Board can accept larger flash images (%u bytes) than board config (%u bytes). Please use the correct board configuration to avoid lacking critical functionality."
% (self.fw_maxsize, fw.property('image_maxsize')))
raise RuntimeError(f"Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes). Please use the correct board configuration to avoid lacking critical functionality.")
else:
# If we're still on bootloader v4 on a Pixhawk, we don't know if we
# have the silicon errata and therefore need to flash px4_fmu-v2
@@ -780,16 +809,13 @@ class uploader(object):
def main():
# Python2 is EOL
if not runningPython3:
raise RuntimeError("Python 2 is not supported. Please try again using Python 3.")
# Parse commandline arguments
parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
parser.add_argument('--port', action="store", required=True, help="Comma-separated list of serial port(s) to which the FMU may be attached")
parser.add_argument('--baud-bootloader', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200) when communicating with bootloader, only required for true serial ports.")
parser.add_argument('--baud-flightstack', action="store", default="57600", help="Comma-separated list of baud rate of the serial port (default is 57600) when communicating with flight stack (Mavlink or NSH), only required for true serial ports.")
parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading')
parser.add_argument('--force-erase', action="store_true", help="Do not perform the blank check, always erase every sector of the application space")
parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash')
parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot')
parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)")
@@ -867,9 +893,10 @@ def main():
# Windows, don't open POSIX ports
if "/" not in port:
up = uploader(port, args.baud_bootloader, baud_flightstack)
except Exception:
except Exception as e:
# open failed, rate-limit our attempts
time.sleep(0.05)
print(f"Exception ignored: {e}")
# and loop to the next port
continue
@@ -884,10 +911,10 @@ def main():
up.identify()
found_bootloader = True
print()
print("Found board id: %s,%s bootloader version: %s on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
print(f"Found board id: {up.board_type},{up.board_rev} bootloader protocol revision {up.bl_rev} on {port}")
break
except Exception:
except (RuntimeError, serial.SerialException):
if not up.send_reboot(args.use_protocol_splitter_format):
break
@@ -907,14 +934,14 @@ def main():
try:
# ok, we have a bootloader, try flashing it
up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay)
up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay, force_erase=args.force_erase)
# if we made this far without raising exceptions, the upload was successful
successful = True
except RuntimeError as ex:
except RuntimeError as e:
# print the error
print("\nERROR: %s" % ex.args)
print(f"\n\nError: {e}")
except FirmwareNotSuitableException:
unsuitable_board = True
@@ -12,6 +12,7 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
+3
View File
@@ -8,6 +8,7 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
@@ -18,6 +19,7 @@ CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_HEATER=y
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
@@ -57,6 +59,7 @@ CONFIG_MODULES_LOGGER=y
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
CONFIG_MODULES_MANUAL_CONTROL=y
CONFIG_MODULES_MAVLINK=y
CONFIG_MAVLINK_DIALECT="development"
CONFIG_MODULES_MC_ATT_CONTROL=y
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
Binary file not shown.
+2
View File
@@ -8,6 +8,7 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
@@ -39,6 +40,7 @@ CONFIG_MODULES_LOGGER=y
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
CONFIG_MODULES_MANUAL_CONTROL=y
CONFIG_MODULES_MAVLINK=y
CONFIG_MAVLINK_DIALECT="development"
CONFIG_MODULES_MC_ATT_CONTROL=y
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+1
View File
@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_MAIERTEK_MPC2520=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
@@ -3,6 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m4"
CONFIG_BOARD_CONSTRAINED_FLASH=y
CONFIG_BOARD_CONSTRAINED_MEMORY=y
CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing"
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_BAROMETER_LPS25H=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
CONFIG_DRIVERS_GPS=y
@@ -4,6 +4,7 @@ CONFIG_BOARD_CONSTRAINED_FLASH=y
CONFIG_BOARD_CONSTRAINED_MEMORY=y
CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing"
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_BOSCH_BMI088_I2C=y
+1
View File
@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
Binary file not shown.
+1
View File
@@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
Binary file not shown.
@@ -11,6 +11,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -11,6 +11,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -12,6 +12,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -8,6 +8,7 @@ CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP280=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+1
View File
@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP280=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
+1
View File
@@ -12,6 +12,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -9,6 +9,7 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP280=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y
+1
View File
@@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+1
View File
@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
+1
View File
@@ -8,6 +8,7 @@ CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
Binary file not shown.
+1
View File
@@ -12,6 +12,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -12,6 +12,7 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
@@ -12,6 +12,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -9,6 +9,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -9,6 +9,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -10,6 +10,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_DRIVERS_BAROMETER_MPL3115A2=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_DRIVERS_BAROMETER_MPL3115A2=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y
+1
View File
@@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP280=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
+1
View File
@@ -9,6 +9,7 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
CONFIG_DRIVERS_IMU_ST_L3GD20=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -12,6 +12,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -10,6 +10,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -6,6 +6,7 @@ CONFIG_COMMON_OPTICAL_FLOW=n
CONFIG_COMMON_TELEMETRY=n
CONFIG_DRIVERS_CAMERA_CAPTURE=n
CONFIG_DRIVERS_CAMERA_TRIGGER=n
CONFIG_DRIVERS_CDCACM_AUTOSTART=n
CONFIG_DRIVERS_IRLOCK=n
CONFIG_DRIVERS_PCA9685_PWM_OUT=n
CONFIG_DRIVERS_PWM_INPUT=n
+1
View File
@@ -14,6 +14,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
Binary file not shown.
+1
View File
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
Binary file not shown.
+1
View File
@@ -14,6 +14,7 @@ CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
Binary file not shown.
+1
View File
@@ -13,6 +13,7 @@ CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP280=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
+1
View File
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_HEATER=y
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
@@ -4,6 +4,7 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
+1
View File
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_DRIVERS_BATT_SMBUS=y
CONFIG_DRIVERS_CAMERA_CAPTURE=y
CONFIG_DRIVERS_CAMERA_TRIGGER=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DISTANCE_SENSOR=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
+2
View File
@@ -9,6 +9,8 @@ uint8 BACKEND_MAVLINK = 2
uint8 BACKEND_ALL = 3
uint8 backend
bool is_logging
float32 total_written_kb # total written to log in kiloBytes
float32 write_rate_kb_s # write rate in kiloBytes/s
+5 -5
View File
@@ -8,8 +8,8 @@ bool has_vtol_approach # flag if approaches are defined for current RTL_
uint8 rtl_type # Type of RTL chosen
uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
uint8 RTL_STATUS_TYPE_NONE=0 # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # RTL type is following the mission from closest point to mission landing
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position
uint8 RTL_STATUS_TYPE_NONE=0 # pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # chosen to directly go to a safe point or home position
uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # going straight to the beginning of the mission landing
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # Following the mission from start index to mission landing. Start index is current WP if in Mission mode, and closest WP otherwise.
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # Following the mission in reverse from start index to the beginning of the mission. Start index is previous WP if in Mission mode, and closest WP otherwise.
+1 -1
View File
@@ -1,6 +1,6 @@
uint64 timestamp # time since system start (microseconds)
# body angular rates in NED frame
# body angular rates in FRD frame
float32 roll # [rad/s] roll rate setpoint
float32 pitch # [rad/s] pitch rate setpoint
float32 yaw # [rad/s] yaw rate setpoint
@@ -37,6 +37,10 @@ add_library(bootloader
crypto.c
)
target_compile_definitions(bootloader
PRIVATE BOOTLOADER_VERSION="PX4BLv${PX4_VERSION_MAJOR}.${PX4_VERSION_MINOR}.${PX4_VERSION_PATCH}g${PX4_GIT_HASH}"
)
target_link_libraries(bootloader
PRIVATE
arch_bootloader
+54 -16
View File
@@ -80,7 +80,7 @@
// RESET finalise flash programming, reset chip and starts application
//
#define BL_PROTOCOL_VERSION 5 // The revision of the bootloader protocol
#define BL_PROTOCOL_REVISION 5 // The revision of the bootloader protocol
//* Next revision needs to update
// protocol bytes
@@ -106,14 +106,20 @@
#define PROTO_GET_CHIP 0x2c // read chip version (MCU IDCODE)
#define PROTO_SET_DELAY 0x2d // set minimum boot delay
#define PROTO_GET_CHIP_DES 0x2e // read chip version In ASCII
#define PROTO_GET_VERSION 0x2f // read version
#define PROTO_BOOT 0x30 // boot the application
#define PROTO_DEBUG 0x31 // emit debug information - format not defined
#define PROTO_SET_BAUD 0x33 // set baud rate on uart
#define PROTO_RESERVED_0X36 0x36 // Reserved
#define PROTO_RESERVED_0X37 0x37 // Reserved
// Reserved for external flash programming
// #define PROTO_EXTF_ERASE 0x34 // Erase sectors from external flash
// #define PROTO_EXTF_PROG_MULTI 0x35 // write bytes at external flash program address and increment
// #define PROTO_EXTF_READ_MULTI 0x36 // read bytes at address and increment
// #define PROTO_EXTF_GET_CRC 0x37 // compute & return a CRC of data in external flash
#define PROTO_RESERVED_0X38 0x38 // Reserved
#define PROTO_RESERVED_0X39 0x39 // Reserved
#define PROTO_CHIP_FULL_ERASE 0x40 // Full erase, without any flash wear optimization
#define PROTO_PROG_MULTI_MAX 64 // maximum PROG_MULTI size
#define PROTO_READ_MULTI_MAX 255 // size of the size field
@@ -125,13 +131,6 @@
#define PROTO_DEVICE_FW_SIZE 4 // size of flashable area
#define PROTO_DEVICE_VEC_AREA 5 // contents of reserved vectors 7-10
#define STATE_PROTO_OK 0x10 // INSYNC/OK - 'ok' response
#define STATE_PROTO_FAILED 0x11 // INSYNC/FAILED - 'fail' response
#define STATE_PROTO_INVALID 0x13 // INSYNC/INVALID - 'invalid' response for bad commands
#define STATE_PROTO_BAD_SILICON_REV 0x14 // On the F4 series there is an issue with < Rev 3 silicon
#define STATE_PROTO_RESERVED_0X15 0x15 // Reserved
// State
#define STATE_PROTO_GET_SYNC 0x1 // Have Seen NOP for re-establishing sync
#define STATE_PROTO_GET_DEVICE 0x2 // Have Seen get device ID bytes
@@ -142,7 +141,8 @@
#define STATE_PROTO_GET_SN 0x40 // Have Seen read a word from UDID area ( Serial) at the given address
#define STATE_PROTO_GET_CHIP 0x80 // Have Seen read chip version (MCU IDCODE)
#define STATE_PROTO_GET_CHIP_DES 0x100 // Have Seen read chip version In ASCII
#define STATE_PROTO_BOOT 0x200 // Have Seen boot the application
#define STATE_PROTO_GET_VERSION 0x200 // Have Seen get version
#define STATE_PROTO_BOOT 0x400 // Have Seen boot the application
#if defined(TARGET_HW_PX4_PIO_V1)
#define STATE_ALLOWS_ERASE (STATE_PROTO_GET_SYNC)
@@ -157,6 +157,18 @@
static uint8_t bl_type;
static uint8_t last_input;
int get_version(int n, uint8_t *version_str)
{
int len = strlen(BOOTLOADER_VERSION);
if (len > n) {
len = n;
}
strncpy((char *)version_str, BOOTLOADER_VERSION, n);
return len;
}
inline void cinit(void *config, uint8_t interface)
{
#if INTERFACE_USB
@@ -257,7 +269,7 @@ inline void cout(uint8_t *buf, unsigned len)
#endif
static const uint32_t bl_proto_rev = BL_PROTOCOL_VERSION; // value returned by PROTO_DEVICE_BL_REV
static const uint32_t bl_proto_rev = BL_PROTOCOL_REVISION; // value returned by PROTO_DEVICE_BL_REV
static unsigned head, tail;
static uint8_t rx_buf[256] USB_DATA_ALIGN;
@@ -649,6 +661,8 @@ bootloader(unsigned timeout)
led_on(LED_ACTIVITY);
bool full_erase = false;
// handle the command byte
switch (c) {
@@ -728,6 +742,10 @@ bootloader(unsigned timeout)
// success reply: INSYNC/OK
// erase failure: INSYNC/FAILURE
//
case PROTO_CHIP_FULL_ERASE:
full_erase = true;
// Fallthrough
case PROTO_CHIP_ERASE:
/* expect EOC */
@@ -755,17 +773,18 @@ bootloader(unsigned timeout)
arch_flash_unlock();
for (int i = 0; flash_func_sector_size(i) != 0; i++) {
flash_func_erase_sector(i);
flash_func_erase_sector(i, full_erase);
}
// disable the LED while verifying the erase
led_set(LED_OFF);
// verify the erase
for (address = 0; address < board_info.fw_size; address += 4)
for (address = 0; address < board_info.fw_size; address += 4) {
if (flash_func_read_word(address) != 0xffffffff) {
goto cmd_fail;
}
}
address = 0;
SET_BL_STATE(STATE_PROTO_CHIP_ERASE);
@@ -965,7 +984,7 @@ bootloader(unsigned timeout)
// read the chip description
//
// command: GET_CHIP_DES/EOC
// reply: <value:4>/INSYNC/OK
// reply: <length:4><buffer...>/INSYNC/OK
case PROTO_GET_CHIP_DES: {
uint8_t buffer[MAX_DES_LENGTH];
unsigned len = MAX_DES_LENGTH;
@@ -982,6 +1001,25 @@ bootloader(unsigned timeout)
}
break;
// read the bootloader version (not to be confused with protocol revision)
//
// command: GET_VERSION/EOC
// reply: <length:4><buffer...>/INSYNC/OK
case PROTO_GET_VERSION: {
uint8_t buffer[MAX_VERSION_LENGTH];
// expect EOC
if (!wait_for_eoc(2)) {
goto cmd_bad;
}
int len = get_version(sizeof(buffer), buffer);
cout_word(len);
cout(buffer, len);
SET_BL_STATE(STATE_PROTO_GET_VERSION);
}
break;
#ifdef BOOT_DELAY_ADDRESS
case PROTO_SET_DELAY: {
@@ -1099,4 +1137,4 @@ bad_silicon:
continue;
#endif
}
}
}
+7 -2
View File
@@ -39,6 +39,8 @@
#pragma once
#include <stdbool.h>
/*****************************************************************************
* Generic bootloader functions.
*/
@@ -94,6 +96,7 @@ extern int buf_get(void);
#endif
#define MAX_DES_LENGTH 20
#define MAX_VERSION_LENGTH 32
#define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
extern void led_on(unsigned led);
@@ -105,7 +108,7 @@ extern void board_deinit(void);
extern uint32_t board_get_devices(void);
extern void clock_deinit(void);
extern uint32_t flash_func_sector_size(unsigned sector);
extern void flash_func_erase_sector(unsigned sector);
extern void flash_func_erase_sector(unsigned sector, bool force);
extern void flash_func_write_word(uintptr_t address, uint32_t word);
extern uint32_t flash_func_read_word(uintptr_t address);
extern uint32_t flash_func_read_otp(uintptr_t address);
@@ -121,6 +124,8 @@ extern uint32_t get_mcu_id(void);
int get_mcu_desc(int max, uint8_t *revstr);
extern int check_silicon(void);
int get_version(int max, uint8_t *version_str);
/*****************************************************************************
* Interface in/output.
*/
@@ -133,4 +138,4 @@ extern void cout(uint8_t *buf, unsigned len);
#if !defined(APP_VECTOR_OFFSET)
# define APP_VECTOR_OFFSET 0
#endif
#define APP_VECTOR_OFFSET_WORDS (APP_VECTOR_OFFSET/sizeof(uint32_t))
#define APP_VECTOR_OFFSET_WORDS (APP_VECTOR_OFFSET/sizeof(uint32_t))
@@ -395,6 +395,24 @@ flash_func_sector_size(unsigned sector)
return 0;
}
/* imxRT uses Flash lib, not up_progmem so let's stub it here */
up_progmem_ispageerased(unsigned sector)
{
const uint32_t bytes_per_sector = flash_func_sector_size(sector);
uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector));
const uint32_t uint32_per_sector = bytes_per_sector / sizeof(*address);
int blank = 0; /* Assume it is Bank */
for (uint32_t i = 0; i < uint32_per_sector; i++) {
if (address[i] != 0xffffffff) {
blank = -1; /* It is not blank */
break;
}
}
return blank;
}
/*!
* @name Configuration Option
@@ -407,31 +425,15 @@ flash_func_sector_size(unsigned sector)
* */
locate_code(".ramfunc")
void
flash_func_erase_sector(unsigned sector)
flash_func_erase_sector(unsigned sector, bool force)
{
if (sector > BOARD_FLASH_SECTORS || (int)sector < BOARD_FIRST_FLASH_SECTOR_TO_ERASE) {
return;
}
/* blank-check the sector */
const uint32_t bytes_per_sector = flash_func_sector_size(sector);
uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector));
const uint32_t uint32_per_sector = bytes_per_sector / sizeof(*address);
bool blank = true;
if (force || flash_func_is_sector_blank(sector) != 0) {
struct flexspi_nor_config_s *pConfig = &g_bootConfig;
for (uint32_t i = 0; i < uint32_per_sector; i++) {
if (address[i] != 0xffffffff) {
blank = false;
break;
}
}
struct flexspi_nor_config_s *pConfig = &g_bootConfig;
/* erase the sector if it failed the blank check */
if (!blank) {
uintptr_t offset = ((uintptr_t) address) - IMXRT_FLEXSPI1_CIPHER_BASE;
irqstate_t flags;
flags = enter_critical_section();
@@ -439,8 +441,6 @@ flash_func_erase_sector(unsigned sector)
leave_critical_section(flags);
UNUSED(status);
}
}
void
@@ -466,18 +466,13 @@ flash_func_sector_size(unsigned sector)
}
void
flash_func_erase_sector(unsigned sector)
flash_func_erase_sector(unsigned sector, bool force)
{
if (sector > BOARD_FLASH_SECTORS || (int)sector < BOARD_FIRST_FLASH_SECTOR_TO_ERASE) {
return;
}
/* blank-check the sector */
bool blank = up_progmem_ispageerased(sector) == 0;
/* erase the sector if it failed the blank check */
if (!blank) {
if (force || (up_progmem_ispageerased(sector) != 0)) {
up_progmem_eraseblock(sector);
}
}
@@ -38,7 +38,6 @@ if(NOT PX4_BOARD MATCHES "io-v2")
board_crashdump.c
board_dma_alloc.c
board_fat_dma_alloc.c
cdc_acm_check.cpp
console_buffer.cpp
cpuload.cpp
gpio.c
@@ -84,4 +83,4 @@ add_subdirectory(srgbled)
if (DEFINED PX4_CRYPTO)
add_library(px4_random nuttx_random.c)
target_link_libraries(px4_random PRIVATE nuttx_crypto)
endif()
endif()
@@ -62,8 +62,6 @@
#include <px4_platform/board_ctrl.h>
#endif
extern void cdcacm_init(void);
#if !defined(CONFIG_BUILD_FLAT)
typedef CODE void (*initializer_t)(void);
extern initializer_t _sinit;
@@ -196,10 +194,6 @@ int px4_platform_init()
px4_log_initialize();
#if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_FLAT)
cdcacm_init();
#endif
return PX4_OK;
}
@@ -2,7 +2,6 @@
add_library(px4_layer
${KERNEL_SRCS}
cdc_acm_check.cpp
${PX4_SOURCE_DIR}/platforms/common/Serial.cpp
SerialImpl.cpp
)
@@ -6,7 +6,6 @@ add_library(px4_layer
board_fat_dma_alloc.c
tasks.cpp
console_buffer_usr.cpp
cdc_acm_check.cpp
${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp
${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp
px4_userspace_init.cpp
@@ -44,8 +44,6 @@
#include <uORB/uORB.h>
#include <sys/boardctl.h>
extern void cdcacm_init(void);
extern "C" void px4_userspace_init(void)
{
hrt_init();
@@ -55,8 +53,4 @@ extern "C" void px4_userspace_init(void)
px4::WorkQueueManagerStart();
uorb_start();
#if defined(CONFIG_SYSTEM_CDCACM)
cdcacm_init();
#endif
}
+40 -1
View File
@@ -53,6 +53,7 @@
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/module.h>
#include <systemlib/err.h>
#include <parameters/param.h>
@@ -924,7 +925,45 @@ CameraTrigger::status()
static int usage()
{
PX4_INFO("usage: camera_trigger {start|stop|status|test|test_power}\n");
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Camera trigger driver.
This module triggers cameras that are connected to the flight-controller outputs,
or simple MAVLink cameras that implement the MAVLink trigger protocol.
The driver responds to the following MAVLink trigger commands being found in missions or recieved over MAVLink:
- `MAV_CMD_DO_TRIGGER_CONTROL`
- `MAV_CMD_DO_DIGICAM_CONTROL`
- `MAV_CMD_DO_SET_CAM_TRIGG_DIST`
- `MAV_CMD_OBLIQUE_SURVEY`
The commands cause the driver to trigger camera image capture based on time or distance.
Each time an image capture is triggered, the `CAMERA_TRIGGER` MAVLink message is emitted.
A "simple MAVLink camera" is one that supports the above command set.
When configured for this kind of camera, all the driver does is emit the `CAMERA_TRIGGER` MAVLink message as expected.
The incoming commands must be forwarded to the MAVLink camera, and are automatically emitted to MAVLink channels
when found in missions.
The driver is configured using [Camera Trigger parameters](../advanced_config/parameter_reference.md#camera-trigger).
In particular:
- `TRIG_INTERFACE` - How the camera is connected to flight controller (PWM, GPIO, Seagull, MAVLink)
- `TRIG_MODE` - Distance or time based triggering, with values set by `TRIG_DISTANCE` and `TRIG_INTERVAL`.
[Setup/usage information](../camera/index.md).
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("camera_trigger", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("camera");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information");
PRINT_MODULE_USAGE_COMMAND_DESCR("test","Trigger one image (not logged or forwarded to GCS)");
PRINT_MODULE_USAGE_COMMAND_DESCR("test_power","Toggle power");
return 1;
}
@@ -39,8 +39,7 @@
#include <parameters/param.h>
#include <px4_platform_common/log.h>
#define arraySize(a) (sizeof((a))/sizeof(((a)[0])))
#include <systemlib/px4_macros.h>
class CameraInterface
{
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2023 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.
#
############################################################################
px4_add_module(
MODULE drivers__cdcacm_autostart
MAIN cdcacm_autostart
COMPILE_FLAGS
# -DDEBUG_BUILD
SRCS
cdcacm_autostart.cpp
)
+6
View File
@@ -0,0 +1,6 @@
menuconfig DRIVERS_CDCACM_AUTOSTART
bool "cdcacm_autostart"
default n
depends on MODULES_MAVLINK
---help---
Enable support for cdcacm_autostart
@@ -0,0 +1,664 @@
/****************************************************************************
*
* Copyright (c) 2023 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 defined(CONFIG_SYSTEM_CDCACM)
#include "cdcacm_autostart.h"
__BEGIN_DECLS
#include <arch/board/board.h>
#include <builtin/builtin.h>
extern int sercon_main(int c, char **argv);
extern int serdis_main(int c, char **argv);
__END_DECLS
#include <px4_platform_common/shutdown.h>
#define USB_DEVICE_PATH "/dev/ttyACM0"
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
# undef SERIAL_PASSTHRU_UBLOX_DEV
# if defined(CONFIG_SERIAL_PASSTHRU_GPS1) && defined(CONFIG_BOARD_SERIAL_GPS1)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS1
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS2)&& defined(CONFIG_BOARD_SERIAL_GPS2)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS2
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS3)&& defined(CONFIG_BOARD_SERIAL_GPS3)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS3
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS4)&& defined(CONFIG_BOARD_SERIAL_GPS4)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS4
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS5) && defined(CONFIG_BOARD_SERIAL_GPS5)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS5
# endif
# if !defined(SERIAL_PASSTHRU_UBLOX_DEV)
# error "CONFIG_SERIAL_PASSTHRU_GPSn and CONFIG_BOARD_SERIAL_GPSn must be defined"
# endif
#endif
CdcAcmAutostart::CdcAcmAutostart() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{}
CdcAcmAutostart::~CdcAcmAutostart()
{
PX4_INFO("Stopping CDC/ACM autostart");
if (_ttyacm_fd >= 0) {
px4_close(_ttyacm_fd);
}
ScheduleClear();
}
int CdcAcmAutostart::Start()
{
PX4_INFO("Starting CDC/ACM autostart");
UpdateParams(true);
ScheduleNow();
return PX4_OK;
}
void CdcAcmAutostart::Run()
{
if (should_exit()) {
exit_and_cleanup();
return;
}
UpdateParams();
run_state_machine();
}
void CdcAcmAutostart::run_state_machine()
{
_reschedule_time = 500_ms;
_vbus_present = (board_read_VBUS_state() == PX4_OK);
// If the hardware supports RESET lockout that has nArmed ANDed with VBUS
// vbus_sense may drop during a param save which uses
// BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE to prevent external resets
// while writing the params. If we are not armed and nARMRED is low
// we are in such a lock out so ignore changes on VBUS_SENSE during this
// time.
#if defined(BOARD_GET_EXTERNAL_LOCKOUT_STATE)
if (BOARD_GET_EXTERNAL_LOCKOUT_STATE() == 0) {
_vbus_present = _vbus_present_prev;
ScheduleDelayed(500_ms);
return;
}
#endif
// Do not reconfigure USB while flying
actuator_armed_s report;
_actuator_armed_sub.copy(&report);
if (report.armed) {
_vbus_present_prev = _vbus_present;
} else {
switch (_state) {
case UsbAutoStartState::disconnected:
state_disconnected();
break;
case UsbAutoStartState::connecting:
state_connecting();
break;
case UsbAutoStartState::connected:
state_connected();
break;
case UsbAutoStartState::disconnecting:
state_disconnecting();
break;
}
}
_vbus_present_prev = _vbus_present;
ScheduleDelayed(_reschedule_time);
}
void CdcAcmAutostart::state_connected()
{
if (!_vbus_present && !_vbus_present_prev && (_active_protocol == UsbProtocol::mavlink)) {
PX4_DEBUG("lost vbus!");
sched_lock();
static const char app[] {"mavlink"};
static const char *stop_argv[] {"mavlink", "stop", "-d", USB_DEVICE_PATH, NULL};
exec_builtin(app, (char **)stop_argv, NULL, 0);
sched_unlock();
_state = UsbAutoStartState::disconnecting;
}
}
void CdcAcmAutostart::state_disconnected()
{
if (_vbus_present && _vbus_present_prev) {
PX4_DEBUG("starting sercon");
if (sercon_main(0, nullptr) == EXIT_SUCCESS) {
_state = UsbAutoStartState::connecting;
PX4_DEBUG("state connecting");
_reschedule_time = 1_s;
}
} else if (_vbus_present && !_vbus_present_prev) {
// USB just connected, try again soon
_reschedule_time = 100_ms;
}
}
void CdcAcmAutostart::state_connecting()
{
int bytes_available = 0;
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
struct termios uart_config;
speed_t baudrate;
#endif
if (!_vbus_present) {
PX4_DEBUG("No VBUS");
goto fail;
}
if (_ttyacm_fd < 0) {
PX4_DEBUG("opening port");
_ttyacm_fd = px4_open(USB_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
}
if (_ttyacm_fd < 0) {
PX4_DEBUG("can't open port");
// fail silently and keep trying to open the port
return;
}
if (_sys_usb_auto.get() == 2) {
PX4_INFO("Starting mavlink on %s (SYS_USB_AUTO=2)", USB_DEVICE_PATH);
if (start_mavlink()) {
_state = UsbAutoStartState::connected;
_active_protocol = UsbProtocol::mavlink;
} else {
_state = UsbAutoStartState::disconnecting;
_reschedule_time = 100_ms;
}
return;
} else if (_sys_usb_auto.get() == 0) {
// Do nothing
_state = UsbAutoStartState::connected;
_active_protocol = UsbProtocol::none;
return;
}
// Otherwise autodetect
if ((px4_ioctl(_ttyacm_fd, FIONREAD, &bytes_available) != PX4_OK) ||
(bytes_available < 3)) {
PX4_DEBUG("bytes_available: %d", bytes_available);
// Return back to connecting state to check again soon
return;
}
// Non-blocking read
_bytes_read = px4_read(_ttyacm_fd, _buffer, sizeof(_buffer));
#if defined(DEBUG_BUILD)
if (_bytes_read > 0) {
fprintf(stderr, "%d bytes\n", _bytes_read);
for (int i = 0; i < _bytes_read; i++) {
fprintf(stderr, "|%X", _buffer[i]);
}
fprintf(stderr, "\n");
}
#endif // DEBUG_BUILD
if (_bytes_read <= 0) {
PX4_DEBUG("no _bytes_read");
// Return back to connecting state to check again soon
return;
}
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
// Get the baudrate for serial passthru before closing the port.
tcgetattr(_ttyacm_fd, &uart_config);
baudrate = cfgetspeed(&uart_config);
#endif
PX4_DEBUG("_bytes_read %d", _bytes_read);
px4_close(_ttyacm_fd);
_ttyacm_fd = -1;
// Parse for mavlink reboot command
if (scan_buffer_for_mavlink_reboot()) {
// Reboot incoming. Return without rescheduling.
return;
}
// Parse for mavlink heartbeats (v1 and v2).
if (scan_buffer_for_mavlink_heartbeat()) {
if (start_mavlink()) {
_state = UsbAutoStartState::connected;
_active_protocol = UsbProtocol::mavlink;
} else {
_state = UsbAutoStartState::disconnecting;
_reschedule_time = 100_ms;
}
return;
}
// Parse for carriage returns indicating someone is trying to use the nsh.
if (scan_buffer_for_carriage_returns()) {
if (start_nsh()) {
_state = UsbAutoStartState::connected;
_active_protocol = UsbProtocol::nsh;
} else {
_state = UsbAutoStartState::disconnecting;
_reschedule_time = 100_ms;
}
return;
}
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
// Parse for ublox start of packet byte sequence.
if (scan_buffer_for_ublox_bytes()) {
if (start_ublox_serial_passthru(baudrate)) {
_state = UsbAutoStartState::connected;
_active_protocol = UsbProtocol::ublox;
} else {
_state = UsbAutoStartState::disconnecting;
_reschedule_time = 100_ms;
}
return;
}
#endif
return;
fail:
PX4_DEBUG("fail...");
// VBUS not present, open failed
if (_ttyacm_fd >= 0) {
px4_close(_ttyacm_fd);
_ttyacm_fd = -1;
}
_state = UsbAutoStartState::disconnecting;
}
void CdcAcmAutostart::state_disconnecting()
{
PX4_DEBUG("state_disconnecting");
if (_ttyacm_fd > 0) {
px4_close(_ttyacm_fd);
}
// Disconnect serial
serdis_main(0, NULL);
_state = UsbAutoStartState::disconnected;
_active_protocol = UsbProtocol::none;
}
bool CdcAcmAutostart::scan_buffer_for_mavlink_reboot()
{
bool rebooting = false;
// Mavlink reboot/shutdown command
// COMMAND_LONG (#76) with command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246)
static constexpr int MAVLINK_COMMAND_LONG_MIN_LENGTH = 41;
if (_bytes_read < MAVLINK_COMMAND_LONG_MIN_LENGTH) {
return rebooting;
}
// scan buffer for mavlink COMMAND_LONG
for (int i = 0; i < _bytes_read - MAVLINK_COMMAND_LONG_MIN_LENGTH; i++) {
if ((_buffer[i] == 0xFE) // Mavlink v1 start byte
&& (_buffer[i + 5] == 76) // 76=0x4C COMMAND_LONG
&& (_buffer[i + 34] == 246) // 246=0xF6 MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN
) {
// mavlink v1 COMMAND_LONG
// buffer[0]: start byte (0xFE for mavlink v1)
// buffer[3]: SYSID
// buffer[4]: COMPID
// buffer[5]: message id (COMMAND_LONG 76=0x4C)
// buffer[6-10]: COMMAND_LONG param 1 (little endian float)
// buffer[34]: COMMAND_LONG command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246/0xF6)
float param1_raw = 0;
memcpy(&param1_raw, &_buffer[i + 6], 4);
int param1 = roundf(param1_raw);
PX4_INFO("%s: Mavlink MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN param 1: %d (SYSID:%d COMPID:%d)",
USB_DEVICE_PATH, param1, _buffer[i + 3], _buffer[i + 4]);
if (param1 == 1) {
// 1: Reboot autopilot
rebooting = true;
px4_reboot_request(REBOOT_REQUEST, 0);
} else if (param1 == 2) {
// 2: Shutdown autopilot
#if defined(BOARD_HAS_POWER_CONTROL)
rebooting = true;
px4_shutdown_request(0);
#endif // BOARD_HAS_POWER_CONTROL
} else if (param1 == 3) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
rebooting = true;
px4_reboot_request(REBOOT_TO_BOOTLOADER, 0);
}
}
}
return rebooting;
}
bool CdcAcmAutostart::scan_buffer_for_mavlink_heartbeat()
{
static constexpr int MAVLINK_HEARTBEAT_MIN_LENGTH = 9;
bool start_mavlink = false;
if (_bytes_read < MAVLINK_HEARTBEAT_MIN_LENGTH) {
return start_mavlink;
}
// scan buffer for mavlink HEARTBEAT (v1 & v2)
for (int i = 0; i < _bytes_read - MAVLINK_HEARTBEAT_MIN_LENGTH; i++) {
if ((_buffer[i] == 0xFE) && (_buffer[i + 1] == 9) && (_buffer[i + 5] == 0)) {
// mavlink v1 HEARTBEAT
// buffer[0]: start byte (0xFE for mavlink v1)
// buffer[1]: length (9 for HEARTBEAT)
// buffer[3]: SYSID
// buffer[4]: COMPID
// buffer[5]: mavlink message id (0 for HEARTBEAT)
PX4_INFO("%s: launching mavlink (HEARTBEAT v1 from SYSID:%d COMPID:%d)",
USB_DEVICE_PATH, _buffer[i + 3], _buffer[i + 4]);
start_mavlink = true;
} else if ((_buffer[i] == 0xFD) && (_buffer[i + 1] == 9)
&& (_buffer[i + 7] == 0) && (_buffer[i + 8] == 0) && (_buffer[i + 9] == 0)) {
// mavlink v2 HEARTBEAT
// buffer[0]: start byte (0xFD for mavlink v2)
// buffer[1]: length (9 for HEARTBEAT)
// buffer[5]: SYSID
// buffer[6]: COMPID
// buffer[7:9]: mavlink message id (0 for HEARTBEAT)
PX4_INFO("%s: launching mavlink (HEARTBEAT v2 from SYSID:%d COMPID:%d)",
USB_DEVICE_PATH, _buffer[i + 5], _buffer[i + 6]);
start_mavlink = true;
}
}
return start_mavlink;
}
bool CdcAcmAutostart::scan_buffer_for_carriage_returns()
{
bool start_nsh = false;
if (_bytes_read < 3) {
return start_nsh;
}
// nshterm (3 carriage returns)
// scan buffer looking for 3 consecutive carriage returns (0xD)
for (int i = 1; i < _bytes_read - 1; i++) {
if (_buffer[i - 1] == 0xD && _buffer[i] == 0xD && _buffer[i + 1] == 0xD) {
PX4_INFO("%s: launching nshterm", USB_DEVICE_PATH);
start_nsh = true;
break;
}
}
return start_nsh;
}
bool CdcAcmAutostart::scan_buffer_for_ublox_bytes()
{
bool success = false;
if (_bytes_read < 4) {
return success;
}
// scan buffer looking for 0xb5 0x62 which indicates the start of a packet
for (int i = 0; i < _bytes_read; i++) {
bool ub = _buffer[i] == 0xb5 && _buffer[i + 1] == 0x62;
if (ub && ((_buffer[i + 2 ] == 0x6 && (_buffer[i + 3 ] == 0xb8 || _buffer[i + 3 ] == 0x13)) ||
(_buffer[i + 2 ] == 0xa && _buffer[i + 3 ] == 0x4))) {
PX4_INFO("%s: launching ublox serial passthru", USB_DEVICE_PATH);
success = true;
break;
}
}
return success;
}
bool CdcAcmAutostart::start_mavlink()
{
bool success = false;
char mavlink_mode_string[3];
snprintf(mavlink_mode_string, sizeof(mavlink_mode_string), "%ld", _usb_mav_mode.get());
static const char *argv[] {"mavlink", "start", "-d", USB_DEVICE_PATH, "-m", mavlink_mode_string, nullptr};
if (execute_process((char **)argv) > 0) {
success = true;
}
return success;
}
bool CdcAcmAutostart::start_nsh()
{
bool success = false;
static const char *argv[] {"nshterm", USB_DEVICE_PATH, nullptr};
if (execute_process((char **)argv) > 0) {
success = true;
}
return success;
}
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
bool CdcAcmAutostart::start_ublox_serial_passthru(speed_t baudrate)
{
bool success = false;
char baudstring[16];
snprintf(baudstring, sizeof(baudstring), "%ld", baudrate);
// Stop the GPS driver first
static const char *gps_argv[] {"gps", "stop", nullptr};
static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV, nullptr};
if (execute_process((char **)gps_argv) > 0) {
if (execute_process((char **)passthru_argv) > 0) {
success = true;
}
}
return success;
}
#endif
int CdcAcmAutostart::execute_process(char **argv)
{
int pid = -1;
sched_lock();
pid = exec_builtin(argv[0], argv, nullptr, 0);
sched_unlock();
return pid;
}
int CdcAcmAutostart::task_spawn(int argc, char *argv[])
{
CdcAcmAutostart *instance = new CdcAcmAutostart();
if (!instance) {
PX4_ERR("alloc failed");
return -1;
}
int ret = instance->Start();
if (ret != PX4_OK) {
delete instance;
return ret;
}
_object.store(instance);
_task_id = task_id_is_work_queue;
return ret;
}
void CdcAcmAutostart::UpdateParams(const bool force)
{
if (_parameter_update_sub.updated() || force) {
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
ModuleParams::updateParams();
}
}
int CdcAcmAutostart::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int CdcAcmAutostart::print_status()
{
const char *state = "";
const char *protocol = "";
switch (_state) {
case UsbAutoStartState::disconnected:
state = "disconnected";
break;
case UsbAutoStartState::connecting:
state = "connecting";
break;
case UsbAutoStartState::connected:
state = "connected";
break;
case UsbAutoStartState::disconnecting:
state = "disconnecting";
break;
}
switch (_active_protocol) {
case UsbProtocol::none:
protocol = "none";
break;
case UsbProtocol::mavlink:
protocol = "mavlink";
break;
case UsbProtocol::nsh:
protocol = "nsh";
break;
case UsbProtocol::ublox:
protocol = "ublox";
break;
}
PX4_INFO("Running");
PX4_INFO("State: %s", state);
PX4_INFO("Protocol: %s", protocol);
return PX4_OK;
}
int CdcAcmAutostart::print_usage(const char *reason)
{
if (reason) {
printf("%s\n\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module listens on USB and auto-configures the protocol depending on the bytes received.
The supported protocols are: MAVLink, nsh, and ublox serial passthrough. If the parameter SYS_USB_AUTO=2
the module will only try to start mavlink as long as the USB VBUS is detected. Otherwise it will spin
and continue to check for VBUS and start mavlink once it is detected.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("cdcacm_autostart", "system");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
#endif
extern "C" __EXPORT int cdcacm_autostart_main(int argc, char *argv[])
{
#if defined(CONFIG_SYSTEM_CDCACM)
return CdcAcmAutostart::main(argc, argv);
#endif
return 1;
}
@@ -0,0 +1,124 @@
/****************************************************************************
*
* Copyright (c) 2023 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
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_armed.h>
#include <termios.h>
using namespace time_literals;
class CdcAcmAutostart : public ModuleBase<CdcAcmAutostart>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
CdcAcmAutostart();
~CdcAcmAutostart() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase */
int print_status() override;
int Start();
private:
enum class UsbAutoStartState {
disconnected,
connecting,
connected,
disconnecting,
};
enum class UsbProtocol {
none,
mavlink,
nsh,
ublox,
};
void Run() override;
void UpdateParams(const bool force = false);
void run_state_machine();
void state_disconnected();
void state_connecting();
void state_connected();
void state_disconnecting();
bool scan_buffer_for_mavlink_reboot();
bool scan_buffer_for_mavlink_heartbeat();
bool scan_buffer_for_carriage_returns();
bool scan_buffer_for_ublox_bytes();
bool start_mavlink();
bool start_nsh();
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
bool start_ublox_serial_passthru(speed_t baudrate);
#endif
int execute_process(char **argv);
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 500_ms};
UsbAutoStartState _state{UsbAutoStartState::disconnected};
UsbProtocol _active_protocol{UsbProtocol::none};
bool _vbus_present = false;
bool _vbus_present_prev = false;
int _ttyacm_fd = -1;
char _buffer[80] = {};
int _bytes_read = 0;
uint32_t _reschedule_time = 0;
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_USB_AUTO>) _sys_usb_auto,
(ParamInt<px4::params::USB_MAV_MODE>) _usb_mav_mode
)
};
@@ -0,0 +1,68 @@
/****************************************************************************
*
* Copyright (c) 2024 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.
*
****************************************************************************/
/**
* Enable USB autostart
*
* @value 0 Disabled
* @value 1 Auto-detect
* @value 2 MAVLink
*
* @reboot_required true
*
* @group CDCACM
*/
PARAM_DEFINE_INT32(SYS_USB_AUTO, 2);
/**
* Specify USB MAVLink mode
*
* @value 0 normal
* @value 1 custom
* @value 2 onboard
* @value 3 osd
* @value 4 magic
* @value 5 config
* @value 6 iridium
* @value 7 minimal
* @value 8 extvision
* @value 9 extvisionmin
* @value 10 gimbal
* @value 11 onboard_low_bandwidth
* @value 12 uavionix
*
* @reboot_required true
*
* @group CDCACM
*/
PARAM_DEFINE_INT32(USB_MAV_MODE, 2);
+2
View File
@@ -133,6 +133,8 @@
#define DRV_IMU_DEVTYPE_ADIS16477 0x59
#define DRV_IMU_DEVTYPE_ADIS16507 0x5A
#define DRV_IMU_DEVTYPE_SCH16T 0x5B
#define DRV_BARO_DEVTYPE_MPC2520 0x5F
#define DRV_BARO_DEVTYPE_LPS22HB 0x60
+1
View File
@@ -8,6 +8,7 @@ menu "IMU"
select DRIVERS_IMU_ANALOG_DEVICES_ADIS16470
select DRIVERS_IMU_BOSCH_BMI055
select DRIVERS_IMU_BOSCH_BMI088
select DRIVERS_IMU_MURATA_SCH16T
select DRIVERS_IMU_NXP_FXAS21002C
select DRIVERS_IMU_NXP_FXOS8701CQ
select DRIVERS_IMU_INVENSENSE_ICM20602

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