diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index cad2b8df3a7f..2c036211558b 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -96,8 +96,10 @@ pipeline { "nxp_fmuk66-v3_default", "nxp_fmuk66-v3_socketcan", "nxp_mr-canhubk3_default", + "nxp_mr-canhubk3_fmu", "nxp_ucans32k146_canbootloader", "nxp_ucans32k146_default", + "nxp_tropic-community_default", "omnibus_f4sd_default", "px4_fmu-v2_default", "px4_fmu-v2_fixedwing", diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml index dd912872fd30..2db64ecb4f3b 100644 --- a/.github/workflows/build_all_targets.yml +++ b/.github/workflows/build_all_targets.yml @@ -75,6 +75,7 @@ jobs: needs: group_targets strategy: matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} + fail-fast: false container: image: ${{ matrix.container }} steps: diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 96331bb145b2..bf42ea74f463 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -356,6 +356,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: nxp_mr-canhubk3_fmu + nxp_tropic-community_default: + short: nxp_tropic-community_default + buildType: MinSizeRel + settings: + CONFIG: nxp_tropic-community_default raspberrypi_pico_default: short: raspberrypi_pico buildType: MinSizeRel diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad deleted file mode 100644 index ace0e397c6c7..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ /dev/null @@ -1,131 +0,0 @@ -#!/bin/sh -# -# @name Vertical Technologies DeltaQuad -# -# @type Standard VTOL -# @class VTOL -# -# @maintainer Sander Smeets -# -# @output Motor1 motor 1 -# @output Motor2 motor 2 -# @output Motor3 motor 3 -# @output Motor4 motor 4 -# @output Servo1 Right elevon -# @output Servo2 Left elevon -# @output Servo3 Pusher motor -# @output Servo4 Pusher reverse channel -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# - -. ${R}etc/init.d/rc.vtol_defaults - -param set-default MAV_TYPE 22 - -param set-default BAT1_CAPACITY 23000 -param set-default BAT1_N_CELLS 4 -param set-default BAT1_R_INTERNAL 0.0025 - -param set-default SYS_HAS_NUM_ASPD 0 - -param set-default EKF2_GPS_POS_X -0.12 -param set-default EKF2_IMU_POS_X -0.12 - -param set-default FW_USE_AIRSPD 0 -param set-default NPFG_PERIOD 25 -param set-default FW_PR_FF 0.7 -param set-default FW_PR_I 0.18 -param set-default FW_PR_P 0.15 -param set-default FW_P_TC 0.5 -param set-default FW_PSP_OFF 5 -param set-default FW_R_LIM 35 -param set-default FW_RR_FF 0.9 -param set-default FW_RR_I 0.08 -param set-default FW_RR_P 0.18 -param set-default FW_T_CLMB_MAX 3 -param set-default FW_T_SINK_MAX 3 -param set-default FW_T_SINK_MIN 1 -param set-default FW_THR_TRIM 0.70 -param set-default FW_THR_SLEW_MAX 1 -param set-default FW_P_LIM_MAX 15 -param set-default FW_P_LIM_MIN -25 -param set-default FW_P_RMAX_NEG 45 -param set-default FW_P_RMAX_POS 45 -param set-default FW_R_RMAX 50 -param set-default FW_THR_MIN 0.55 -param set-default FW_BAT_SCALE_EN 1 -param set-default FW_T_RLL2THR 20 - -param set-default MC_ROLLRATE_P 0.16 -param set-default MC_ROLLRATE_I 0.01 -param set-default MC_PITCHRATE_I 0.05 - -param set-default MC_YAWRATE_MAX 20 -param set-default MC_AIRMODE 1 - -param set-default MIS_TAKEOFF_ALT 15 - -param set-default MPC_XY_P 0.8 -param set-default MPC_XY_VEL_MAX 5 -param set-default MPC_LAND_SPEED 1.2 -param set-default MPC_TILTMAX_LND 35 -param set-default MPC_Z_VEL_MAX_UP 1.5 -param set-default MPC_TKO_RAMP_T 0.8 -param set-default MPC_TILTMAX_AIR 25 -param set-default MPC_TILTMAX_LND 25 -param set-default MPC_YAWRAUTO_MAX 20 - -param set-default NAV_LOITER_RAD 100 - -param set-default PWM_MAIN_DIS5 1500 -param set-default PWM_MAIN_DIS6 1500 -param set-default PWM_MAIN_DIS7 900 -param set-default PWM_MAIN_DIS8 900 - - -param set-default SENS_BOARD_ROT 18 - -# TELEM2 config -param set-default MAV_1_CONFIG 102 -param set-default MAV_1_RATE 5000 -param set-default MAV_1_FORWARD 1 -param set-default SER_TEL2_BAUD 57600 - -param set-default VT_TYPE 2 -param set-default VT_PITCH_MIN 8 -param set-default VT_FW_QC_P 55 -param set-default VT_FW_QC_R 55 -param set-default VT_TRANS_MIN_TM 15 -param set-default VT_FWD_THRUST_SC 4 -param set-default VT_TRANS_TIMEOUT 22 - -param set-default COM_RC_OVERRIDE 0 - -param set-default CA_AIRFRAME 2 - -param set-default CA_ROTOR_COUNT 5 -param set-default CA_ROTOR0_PX 0.1515 -param set-default CA_ROTOR0_PY 0.245 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.1515 -param set-default CA_ROTOR1_PY -0.1875 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.1515 -param set-default CA_ROTOR2_PY -0.245 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.1515 -param set-default CA_ROTOR3_PY 0.1875 -param set-default CA_ROTOR3_KM -0.05 -param set-default CA_ROTOR4_AX 1 -param set-default CA_ROTOR4_AZ 0 -param set-default CA_ROTOR4_PX 0.2 - -param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TYPE 1 -param set-default CA_SV_CS0_TRQ_R -0.5 -param set-default CA_SV_CS1_TYPE 2 -param set-default CA_SV_CS1_TRQ_R 0.5 -param set-default CA_SV_CS2_TYPE 3 -param set-default CA_SV_CS2_TRQ_P 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark deleted file mode 100644 index ee98e2c889ea..000000000000 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ /dev/null @@ -1,121 +0,0 @@ -#!/bin/sh -# -# @name BabyShark VTOL -# -# @type Standard VTOL -# @class VTOL -# -# @maintainer Silvan Fuhrer -# -# @output Motor1 motor 1 -# @output Motor2 motor 2 -# @output Motor3 motor 3 -# @output Motor4 motor 4 -# @output Motor5 Pusher motor -# @output Servo1 Ailerons -# @output Servo2 A-tail left -# @output Servo3 A-tail right -# -# @board px4_fmu-v2 exclude -# @board bitcraze_crazyflie exclude -# @board holybro_kakutef7 exclude -# - -. ${R}etc/init.d/rc.vtol_defaults - -param set-default MAV_TYPE 22 - -param set-default BAT1_N_CELLS 6 - -param set-default FW_AIRSPD_MAX 30 -param set-default FW_AIRSPD_MIN 19 -param set-default FW_AIRSPD_TRIM 23 -param set-default FW_PN_R_SLEW_MAX 40 -param set-default FW_PSP_OFF 3 -param set-default FW_P_LIM_MAX 18 -param set-default FW_P_LIM_MIN -25 -param set-default FW_RLL_TO_YAW_FF 0.1 -param set-default FW_RR_P 0.08 -param set-default FW_R_LIM 45 -param set-default FW_R_RMAX 50 -param set-default FW_THR_TRIM 0.65 -param set-default FW_THR_MIN 0.3 -param set-default FW_THR_SLEW_MAX 0.6 -param set-default FW_T_SINK_MAX 15 -param set-default FW_T_SINK_MIN 3 -param set-default FW_YR_P 0.15 - -param set-default IMU_DGYRO_CUTOFF 15 -param set-default MC_PITCHRATE_MAX 60 -param set-default MC_ROLLRATE_MAX 60 -param set-default MC_YAWRATE_I 0.15 -param set-default MC_YAWRATE_MAX 40 -param set-default MC_YAWRATE_P 0.3 - -param set-default MC_AIRMODE 1 -param set-default MPC_LAND_SPEED 1 -param set-default MPC_MAN_TILT_MAX 25 -param set-default MPC_MAN_Y_MAX 40 -param set-default MPC_THR_HOVER 0.45 -param set-default MPC_TILTMAX_AIR 25 -param set-default MPC_YAWRAUTO_MAX 40 -param set-default MPC_Z_VEL_MAX_UP 2 - -param set-default NAV_ACC_RAD 3 - -param set-default PWM_MAIN_DIS3 1000 -param set-default PWM_MAIN_MIN3 1120 - -param set-default SENS_BOARD_ROT 4 - -param set-default VT_ARSP_BLEND 10 -param set-default VT_ARSP_TRANS 21 -param set-default VT_B_DEC_MSS 1.5 -param set-default VT_ELEV_MC_LOCK 0 -param set-default VT_FWD_THRUST_SC 1.2 -param set-default VT_F_TR_OL_TM 8 -param set-default VT_TRANS_MIN_TM 4 -param set-default VT_TYPE 2 - - -param set-default CA_AIRFRAME 2 -param set-default CA_ROTOR_COUNT 5 -param set-default CA_ROTOR4_AX 1 -param set-default CA_ROTOR4_AZ 0 - -# Square quadrotor X PX4 numbering -param set-default CA_ROTOR0_PX 1 -param set-default CA_ROTOR0_PY 1 -param set-default CA_ROTOR1_PX -1 -param set-default CA_ROTOR1_PY -1 -param set-default CA_ROTOR2_PX 1 -param set-default CA_ROTOR2_PY -1 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -1 -param set-default CA_ROTOR3_PY 1 -param set-default CA_ROTOR3_KM -0.05 - -param set-default CA_SV_CS_COUNT 3 -param set-default CA_SV_CS0_TYPE 15 -param set-default CA_SV_CS0_TRQ_R 1 -param set-default CA_SV_CS1_TRQ_P 0.5 -param set-default CA_SV_CS1_TRQ_R 0 -param set-default CA_SV_CS1_TRQ_Y -0.5 -param set-default CA_SV_CS1_TYPE 13 -param set-default CA_SV_CS2_TRQ_P 0.5 -param set-default CA_SV_CS2_TRQ_Y 0.5 -param set-default CA_SV_CS2_TYPE 14 - -param set-default PWM_MAIN_FUNC1 201 -param set-default PWM_MAIN_FUNC2 202 -param set-default PWM_MAIN_FUNC3 105 -param set-default PWM_MAIN_FUNC4 203 -param set-default PWM_MAIN_FUNC5 101 -param set-default PWM_MAIN_FUNC6 102 -param set-default PWM_MAIN_FUNC7 103 -param set-default PWM_MAIN_FUNC8 104 - -param set-default PWM_MAIN_TIM0 50 -param set-default PWM_MAIN_DIS1 1500 -param set-default PWM_MAIN_DIS2 1500 -param set-default PWM_MAIN_DIS4 1500 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f8823bf657ac..06d123d8576b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -129,8 +129,6 @@ if(CONFIG_MODULES_VTOL_ATT_CONTROL) # [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 ) diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 45ceff41092f..6a96e5585906 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -32,4 +32,5 @@ exec find boards msg src platforms test \ -path src/lib/cdrstream/rosidl -prune -o \ -path src/modules/zenoh/zenoh-pico -prune -o \ -path boards/modalai/voxl2/libfc-sensor-api -prune -o \ + -path src/drivers/actuators/vertiq_io/iq-module-communication-cpp -prune -o \ \( -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) -print \) | grep $PATTERN diff --git a/Tools/ci/generate_board_targets_json.py b/Tools/ci/generate_board_targets_json.py index f453d62c0797..c5c86f782fb4 100755 --- a/Tools/ci/generate_board_targets_json.py +++ b/Tools/ci/generate_board_targets_json.py @@ -25,10 +25,16 @@ help='Pretty output instead of a single line') parser.add_argument('-g', '--groups', dest='group', action='store_true', help='Groups targets') +parser.add_argument('-f', '--filter', dest='filter', help='comma separated list of board names to use instead of all') args = parser.parse_args() verbose = args.verbose +board_filter = [] +if args.filter: + for board in args.filter.split(','): + board_filter.append(board) + build_configs = [] grouped_targets = {} excluded_boards = ['modalai_voxl2', 'px4_ros2'] # TODO: fix and enable @@ -141,6 +147,10 @@ def process_target(px4board_file, target_name): label = files.name[:-9] target_name = manufacturer.name + '_' + board.name + '_' + label + if board_filter and not board_name in board_filter: + if verbose: print(f'excluding board {board_name} ({target_name})') + continue + if board_name in excluded_boards: if verbose: print(f'excluding board {board_name} ({target_name})') continue @@ -189,8 +199,6 @@ def process_target(px4board_file, target_name): # nuttx-0 # nuttx-1 final_groups = [] - temp_group = [] - group_number = {} last_man = '' last_arch = '' SPLIT_LIMIT = 10 @@ -199,88 +207,80 @@ def process_target(px4board_file, target_name): print(f'=:Architectures: [{grouped_targets.keys()}]') for arch in grouped_targets: if(verbose): - print(f'=:Processing: [{arch}] Last: [{last_arch}]') - - if(last_arch == ''): - last_arch = arch - if(arch not in group_number): - group_number[arch] = 0 - - if(last_arch != arch and len(temp_group) > 0): + print(f'=:Processing: [{arch}]') + temp_group = [] + for man in grouped_targets[arch]['manufacturers']: + if(verbose): + print(f'=:Processing: [{arch}][{man}]') + man_len = len(grouped_targets[arch]['manufacturers'][man]) + if(man_len > LOWER_LIMIT and man_len < (SPLIT_LIMIT + 1)): + # Manufacturers can have their own group + if(verbose): + print(f'=:Processing: [{arch}][{man}][{man_len}]==Manufacturers can have their own group') + group_name = arch + "-" + man + targets = comma_targets(grouped_targets[arch]['manufacturers'][man]) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(grouped_targets[arch]['manufacturers'][man]) + }) + elif(man_len >= (SPLIT_LIMIT + 1)): + # Split big man groups into subgroups + # example: Pixhawk + if(verbose): + print(f'=:Processing: [{arch}][{man}][{man_len}]==Manufacturers has multiple own groups') + chunk_limit = SPLIT_LIMIT + chunk_counter = 0 + for chunk in chunks(grouped_targets[arch]['manufacturers'][man], chunk_limit): + group_name = arch + "-" + man + "-" + str(chunk_counter) + targets = comma_targets(chunk) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(chunk), + }) + chunk_counter += 1 + else: + if(verbose): + print(f'=:Processing: [{arch}][{man}][{man_len}]==Manufacturers too small group with others') + temp_group.extend(grouped_targets[arch]['manufacturers'][man]) - group_name = last_arch + "-" + str(group_number[last_arch]) - group_number[last_arch] += 1 - targets = comma_targets(temp_group) + temp_len = len(temp_group) + chunk_counter = 0 + if(temp_len > 0 and temp_len < (SPLIT_LIMIT + 1)): if(verbose): - print(f'=:Orphan: [{arch}][{last_arch}][{targets}]') + print(f'=:Processing: [{arch}][orphan][{temp_len}]==Leftover arch can have their own group') + group_name = arch + "-" + str(chunk_counter) + targets = comma_targets(temp_group) final_groups.append({ - "container": grouped_targets[last_arch]['container'], + "container": grouped_targets[arch]['container'], "targets": targets, - "arch": last_arch, + "arch": arch, "group": group_name, - "len": len(temp_group) + "len": temp_len }) - last_arch = arch - temp_group = [] - - for man in grouped_targets[arch]['manufacturers']: + elif(temp_len >= (SPLIT_LIMIT + 1)): + # Split big man groups into subgroups + # example: Pixhawk if(verbose): - print(f'=:Processing: [{arch}][{man}]') - for tar in grouped_targets[arch]['manufacturers'][man]: - man_len = len(grouped_targets[arch]['manufacturers'][man]) - if(verbose): - print(f'=:Processing: [{arch}][{man}][{man_len}][{tar}]') - if(last_man != man): - # if(verbose): - # print(f'=:Processing: [{arch}][{man}][{tar}][{man_len}]') - if(man_len > LOWER_LIMIT and man_len < (SPLIT_LIMIT + 1)): - # Manufacturers can have their own group - if(verbose): - print(f'=:Processing: ==Manufacturers can have their own group') - print(f'=:Processing: Limits[{LOWER_LIMIT}][{SPLIT_LIMIT}]') - group_name = arch + "-" + man - targets = comma_targets(grouped_targets[arch]['manufacturers'][man]) - last_man = man - final_groups.append({ - "container": grouped_targets[arch]['container'], - "targets": targets, - "arch": arch, - "group": group_name, - "len": len(grouped_targets[arch]['manufacturers'][man]) - }) - elif(man_len >= (SPLIT_LIMIT + 1)): - # Split big man groups into subgroups - # example: Pixhawk - chunk_limit = SPLIT_LIMIT - chunk_counter = 0 - for chunk in chunks(grouped_targets[arch]['manufacturers'][man], chunk_limit): - group_name = arch + "-" + man + "-" + str(chunk_counter) - targets = comma_targets(chunk) - last_man = man - final_groups.append({ - "container": grouped_targets[arch]['container'], - "targets": targets, - "arch": arch, - "group": group_name, - "len": len(chunk), - }) - chunk_counter += 1 - else: - temp_group.append(tar) - - if(len(temp_group) > (LOWER_LIMIT - 1)): - group_name = arch + "-" + str(group_number[arch]) - last_arch = arch - group_number[arch] += 1 - targets = comma_targets(temp_group) + print(f'=:Processing: [{arch}][orphan][{temp_len}]==Leftover arch can has multpile group') + chunk_limit = SPLIT_LIMIT + chunk_counter = 0 + for chunk in chunks(temp_group, chunk_limit): + group_name = arch + "-" + str(chunk_counter) + targets = comma_targets(chunk) final_groups.append({ "container": grouped_targets[arch]['container'], "targets": targets, "arch": arch, "group": group_name, - "len": len(temp_group) + "len": len(chunk), }) - temp_group = [] + chunk_counter += 1 if(verbose): import pprint print("================") diff --git a/Tools/teensy_uploader.py b/Tools/teensy_uploader.py new file mode 100644 index 000000000000..aad97135c3df --- /dev/null +++ b/Tools/teensy_uploader.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python3 +############################################################################ +# +# 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. +# +############################################################################ + +# +# Serial firmware uploader for the Teensy + +import sys +import argparse +import time +import sys +import usb.core +import subprocess + +from sys import platform as _platform +from pymavlink import mavutil + +try: + import serial +except ImportError as e: + print("Failed to import serial: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyserial") + print("") + sys.exit(1) + +# Define time to use time.time() by default +def _time(): + return time.time() + +class uploader(object): + + def __init__(self, portname): + self.mavlink = mavutil.mavlink_connection(portname) + + def send_reboot(self): + try: + self.mavlink.wait_heartbeat() + self.mavlink.reboot_autopilot(True) + except: + pass + return True + +TEENSY_BL_VENDORID = 0x16c0 +TEENSY_BL_PRODUCTID = 0x0478 + +def main(): + # 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('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading') + parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') + parser.add_argument('--vendor-id', type=lambda x: int(x,0), default=None, help='PX4 USB vendorid') + parser.add_argument('--product-id', type=lambda x: int(x,0), default=None, help='PX4 USB productid') + parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)") + args = parser.parse_args() + + found_bootloader = False + + # find USB devices + dev = usb.core.find(idVendor=TEENSY_BL_VENDORID, idProduct=TEENSY_BL_PRODUCTID) + # loop through devices, printing vendor and product ids in decimal and hex + if dev is not None: + print("Found teensy bootloader") + found_bootloader = True + else: + dev = usb.core.find(idVendor=args.vendor_id, idProduct=args.product_id) + if dev is None: + print("No PX4 Device found try to press the button program push button") + print("Attempting to reboot into Teensy bootloader...", end="", flush=True) + + try: + while True: + portlist = [] + patterns = args.port.split(",") + # on unix-like platforms use glob to support wildcard ports. This allows + # the use of /dev/serial/by-id/usb-3D_Robotics on Linux, which prevents the upload from + # causing modem hangups etc + if "linux" in _platform or "darwin" in _platform or "cygwin" in _platform: + import glob + for pattern in patterns: + portlist += glob.glob(pattern) + else: + portlist = patterns + + for port in portlist: + try: + if "linux" in _platform: + # Linux, don't open Mac OS and Win ports + if "COM" not in port and "tty.usb" not in port: + up = uploader(port) + elif "darwin" in _platform: + # OS X, don't open Windows and Linux ports + if "COM" not in port and "ACM" not in port: + up = uploader(port) + elif "cygwin" in _platform: + # Cygwin, don't open native Windows COM and Linux ports + if "COM" not in port and "ACM" not in port: + up = uploader(port) + elif "win" in _platform: + # Windows, don't open POSIX ports + if "/" not in port: + up = uploader(port) + except Exception: + # open failed, rate-limit our attempts + time.sleep(0.05) + + # and loop to the next port + continue + + while True: + up.send_reboot() + + # wait for the reboot, without we might run into Serial I/O Error 5 + time.sleep(0.25) + + # wait for the close, without we might run into Serial I/O Error 6 + time.sleep(0.3) + + dev = usb.core.find(idVendor=TEENSY_BL_VENDORID, idProduct=TEENSY_BL_PRODUCTID) + # loop through devices, printing vendor and product ids in decimal and hex + if dev is not None: + print("") + print("Found teensy bootloader") + found_bootloader = True + break + + print('.', end="", flush=True) + + if not found_bootloader: + # Go to the next port + continue + + if(found_bootloader): + while True: + result = subprocess.Popen("teensy_loader_cli -v --mcu=TEENSY41 " + args.firmware[0], shell=True) + text = result.communicate()[0] + if(result.returncode == 0): + sys.exit(0) + + # Delay retries to < 20 Hz to prevent spin-lock from hogging the CPU + time.sleep(0.05) + + # CTRL+C aborts the upload/spin-lock by interrupt mechanics + except KeyboardInterrupt: + print("\n Upload aborted by user.") + sys.exit(0) + +if __name__ == '__main__': + main() diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board index af69fd3691c7..6206a797565b 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -16,6 +16,7 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y +CONFIG_DRIVERS_IMU_MURATA_SCH16T=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin index 73a4ffe8fb7b..d4fc803a0074 100755 Binary files a/boards/ark/fpv/extras/ark_fpv_bootloader.bin and b/boards/ark/fpv/extras/ark_fpv_bootloader.bin differ diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h index 26bf670ac83d..081dbb56698f 100644 --- a/boards/ark/fpv/src/board_config.h +++ b/boards/ark/fpv/src/board_config.h @@ -105,6 +105,11 @@ # define GPIO_nARMED_INIT GPIO_TRACED3 #endif +/* SPI */ + +#define SPI6_nRESET_EXTERNAL1 /* PF10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN10) +#define SPI6_RESET(on_true) px4_arch_gpiowrite(SPI6_nRESET_EXTERNAL1, !(on_true)) + /* I2C busses */ /* Devices on the onboard buses. @@ -154,7 +159,7 @@ /* Define Channel numbers must match above GPIO pin IN(n)*/ #define ADC_BATTERY_VOLTAGE_CHANNEL /* PB0 */ ADC1_CH(9) -#define ADC_BATTERY_CURRENT_CHANNEL /* PC2 */ ADC3_CH(0) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC2 */ ADC3_CH(12) #define ADC_SCALED_12V_CHANNEL /* PA4 */ ADC1_CH(18) #define ADC_SCALED_VDD_3V3_SENSORS_CHANNEL /* PA0 */ ADC1_CH(16) #define ADC_SCALED_V5_CHANNEL /* PB1 */ ADC1_CH(5) @@ -327,6 +332,7 @@ GPIO_5V_ON_BATTERY, \ GPIO_VDD_3V3_SD_CARD_EN, \ GPIO_nARMED_INIT, \ + SPI6_nRESET_EXTERNAL1, \ GPIO_FMU_CH1, \ GPIO_FMU_CH2, \ GPIO_FMU_CH3, \ diff --git a/boards/ark/fpv/src/hw_config.h b/boards/ark/fpv/src/hw_config.h index 8ee75507955a..d047226a9364 100644 --- a/boards/ark/fpv/src/hw_config.h +++ b/boards/ark/fpv/src/hw_config.h @@ -69,7 +69,7 @@ //#define USE_VBUS_PULL_DOWN #define INTERFACE_USART 1 -#define INTERFACE_USART_CONFIG "/dev/ttyS5,921600" +#define INTERFACE_USART_CONFIG "/dev/ttyS6,921600" #define BOOT_DELAY_ADDRESS 0x000001a0 #define BOARD_TYPE 59 #define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) diff --git a/boards/ark/fpv/src/init.c b/boards/ark/fpv/src/init.c index f97fb7d8aa70..5e321b3b52a8 100644 --- a/boards/ark/fpv/src/init.c +++ b/boards/ark/fpv/src/init.c @@ -112,6 +112,7 @@ __EXPORT void board_peripheral_reset(int ms) PAYLOAD_POWER_EN(false); board_control_spi_sensors_power(false, 0xffff); + SPI6_RESET(true); /* wait for the peripheral rail to reach GND */ usleep(ms * 1000); @@ -122,6 +123,9 @@ __EXPORT void board_peripheral_reset(int ms) /* switch the peripheral rail back on */ board_control_spi_sensors_power(true, 0xffff); PAYLOAD_POWER_EN(true); + + /* Release SPI6 Reset */ + SPI6_RESET(false); } /************************************************************************************ @@ -206,6 +210,8 @@ __EXPORT int board_app_initialize(uintptr_t arg) /* Power on Interfaces */ PAYLOAD_POWER_EN(true); + SPI6_RESET(false); + /* Need hrt running before using the ADC */ px4_platform_init(); diff --git a/boards/ark/fpv/src/spi.cpp b/boards/ark/fpv/src/spi.cpp index 424eafedd627..fb922fc95d69 100644 --- a/boards/ark/fpv/src/spi.cpp +++ b/boards/ark/fpv/src/spi.cpp @@ -40,16 +40,16 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI6, { - initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) }), }), initSPIFmumID(ARKFPV_1, { // Placeholder initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), - initSPIBus(SPI::Bus::SPI6, { - initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) }), }), }; diff --git a/boards/nxp/tropic-community/cmake/upload.cmake b/boards/nxp/tropic-community/cmake/upload.cmake new file mode 100644 index 000000000000..79da5c60fdc7 --- /dev/null +++ b/boards/nxp/tropic-community/cmake/upload.cmake @@ -0,0 +1,44 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.hex) + +add_custom_target(upload_teensy + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/teensy_uploader.py --port ${serial_ports} ${PX4_FW_NAME} --vendor-id 0x1FC9 --product-id 0x0024 + DEPENDS ${PX4_FW_NAME} + COMMENT "uploading px4" + VERBATIM + USES_TERMINAL + WORKING_DIRECTORY ${PX4_BINARY_DIR} +) diff --git a/boards/nxp/tropic-community/default.px4board b/boards/nxp/tropic-community/default.px4board new file mode 100644 index 000000000000..314ff5528be7 --- /dev/null +++ b/boards/nxp/tropic-community/default.px4board @@ -0,0 +1,99 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_PARAM_FILE="/fs/microsd/mtd_params" +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_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_INS=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=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 +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +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 +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +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_I2C_LAUNCHER=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_NETMAN=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/nxp/tropic-community/firmware.prototype b/boards/nxp/tropic-community/firmware.prototype new file mode 100644 index 000000000000..3e156fbd4885 --- /dev/null +++ b/boards/nxp/tropic-community/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 36, + "magic": "PX4FWv1", + "description": "Firmware for the TROPIC Community board", + "image": "", + "build_time": 0, + "summary": "TROPIC Community", + "version": "0.1", + "image_size": 0, + "image_maxsize": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/nxp/tropic-community/init/rc.board_defaults b/boards/nxp/tropic-community/init/rc.board_defaults new file mode 100644 index 000000000000..8a49eca594a9 --- /dev/null +++ b/boards/nxp/tropic-community/init/rc.board_defaults @@ -0,0 +1,34 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# Mavlink ethernet (CFG 1000) +param set-default MAV_2_CONFIG 1000 +param set-default MAV_2_BROADCAST 1 +param set-default MAV_2_MODE 0 +param set-default MAV_2_RADIO_CTL 0 +param set-default MAV_2_RATE 100000 +param set-default MAV_2_REMOTE_PRT 14550 +param set-default MAV_2_UDP_PRT 14550 + +param set-default SENS_EN_INA238 0 +param set-default SENS_EN_INA228 0 +param set-default SENS_EN_INA226 0 + +param set-default BAT1_V_DIV 18.000000000 +param set-default BAT1_A_PER_V 38.462030303 + +if [ -f "/fs/microsd/ipcfg-eth0" ] +then +else + netman update -i eth0 +fi + +rgbled_pwm start +safety_button start + +if param greater -s UAVCAN_ENABLE 0 +then + ifup can0 +fi diff --git a/boards/nxp/tropic-community/init/rc.board_mavlink b/boards/nxp/tropic-community/init/rc.board_mavlink new file mode 100644 index 000000000000..d789d908e8f2 --- /dev/null +++ b/boards/nxp/tropic-community/init/rc.board_mavlink @@ -0,0 +1,4 @@ +#!/bin/sh +# +# Tropic Community specific board MAVLink startup script. +#------------------------------------------------------------------------------ diff --git a/boards/nxp/tropic-community/init/rc.board_sensors b/boards/nxp/tropic-community/init/rc.board_sensors new file mode 100644 index 000000000000..f6bcb1a8fac3 --- /dev/null +++ b/boards/nxp/tropic-community/init/rc.board_sensors @@ -0,0 +1,74 @@ +#!/bin/sh +# +# PX4 board sensors init +#------------------------------------------------------------------------------ +# +# UART mapping on Tropic Community: +# +# LPUART5 /dev/ttyS0 CONSOLE +# LPUART3 /dev/ttyS1 GPS +# LPUART2 /dev/ttyS2 TELEM1 +# LPUART4 /dev/ttyS3 TELEM2 +# LPUART8 /dev/ttyS4 RC +# +#------------------------------------------------------------------------------ + +set INA_CONFIGURED no + +if param compare -s ADC_ADS1115_EN 1 +then + ads1115 start -X +else + board_adc start +fi + + +if param compare SENS_EN_INA226 1 +then + # Start Digital power monitors + ina226 -X -b 1 -t 1 -k start + set INA_CONFIGURED yes +fi + +if param compare SENS_EN_INA228 1 +then + # Start Digital power monitors + ina228 -X -b 1 -t 1 -k start + set INA_CONFIGURED yes +fi + +if param compare SENS_EN_INA238 1 +then + # Start Digital power monitors + ina238 -X -b 1 -t 1 -k start + set INA_CONFIGURED yes +fi + +if [ $INA_CONFIGURED = no ] +then + # INA226, INA228, INA238 auto-start + i2c_launcher start -b 1 +fi + +# Internal SPI bus ICM42688p +icm42688p -R 2 -b 3 -s start + +# Internal on IMU SPI BMI088 +bmi088 -A -R 2 -b 4 -s start +bmi088 -G -R 2 -b 4 -s start + +# Internal magnetometer on I2c +bmm150 -I -b 4 -R 6 -a 18 start + +# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 1 -R 10 start + +# Possible internal Baro + +# Disable startup of internal baros if param is set to false +if param compare SENS_INT_BARO_EN 1 +then + bmp388 -I -b 4 start +fi + +unset INA_CONFIGURED diff --git a/boards/nxp/tropic-community/nuttx-config/Kconfig b/boards/nxp/tropic-community/nuttx-config/Kconfig new file mode 100644 index 000000000000..f72f3c094ce4 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/Kconfig @@ -0,0 +1,4 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# diff --git a/boards/nxp/tropic-community/nuttx-config/include/board.h b/boards/nxp/tropic-community/nuttx-config/include/board.h new file mode 100644 index 000000000000..21ded1ea38b7 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/include/board.h @@ -0,0 +1,391 @@ +/************************************************************************************ + * nuttx-configs/nxp/nxp_tropic-community/include/board.h + * + * Copyright (C) 2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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. + * + ************************************************************************************/ + +#ifndef __NUTTX_CONFIG_NXP_TROPIC_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_NXP_TROPIC_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ + +/* Set VDD_SOC to 1.25V */ + +#define IMXRT_VDD_SOC (0x12) + +/* Set Arm PLL (PLL1) to fOut = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR + * 576Mhz = (24Mhz * ARM_PLL_DIV_SELECT/2) / ARM_PODF_DIVISOR + * ARM_PLL_DIV_SELECT = 96 + * ARM_PODF_DIVISOR = 2 + * 576Mhz = (24Mhz * 96/2) / 2 + * + * AHB_CLOCK_ROOT = PLL1fOut / IMXRT_AHB_PODF_DIVIDER + * 1Hz to 600 Mhz = 576Mhz / IMXRT_ARM_CLOCK_DIVIDER + * IMXRT_ARM_CLOCK_DIVIDER = 1 + * 576Mhz = 576Mhz / 1 + * + * PRE_PERIPH_CLK_SEL = PRE_PERIPH_CLK_SEL_PLL1 + * PERIPH_CLK_SEL = 1 (0 select PERIPH_CLK2_PODF, 1 select PRE_PERIPH_CLK_SEL_PLL1) + * PERIPH_CLK = 576Mhz + * + * IPG_CLOCK_ROOT = AHB_CLOCK_ROOT / IMXRT_IPG_PODF_DIVIDER + * IMXRT_IPG_PODF_DIVIDER = 4 + * 144Mhz = 576Mhz / 4 + * + * PRECLK_CLOCK_ROOT = IPG_CLOCK_ROOT / IMXRT_PERCLK_PODF_DIVIDER + * IMXRT_PERCLK_PODF_DIVIDER = 1 + * 16Mhz = 144Mhz / 9 + * + * SEMC_CLK_ROOT = 576Mhz / IMXRT_SEMC_PODF_DIVIDER (labeled AIX_PODF in 18.2) + * IMXRT_SEMC_PODF_DIVIDER = 8 + * 72Mhz = 576Mhz / 8 + * + * Set Sys PLL (PLL2) to fOut = (24Mhz * (20+(2*(DIV_SELECT))) + * 528Mhz = (24Mhz * (20+(2*(1))) + * + * Set USB1 PLL (PLL3) to fOut = (24Mhz * 20) + * 480Mhz = (24Mhz * 20) + * + * Set LPSPI PLL3 PFD0 to fOut = (480Mhz / 12 * 18) + * 720Mhz = (480Mhz / 12 * 18) + * 90Mhz = (720Mhz / LSPI_PODF_DIVIDER) + * + * Set LPI2C PLL3 / 8 to fOut = (480Mhz / 8) + * 60Mhz = (480Mhz / 8) + * 12Mhz = (60Mhz / LSPI_PODF_DIVIDER) + * + * Set USDHC1 PLL2 PFD2 to fOut = (528Mhz / 24 * 18) + * 396Mhz = (528Mhz / 24 * 18) + * 198Mhz = (396Mhz / IMXRT_USDHC1_PODF_DIVIDER) + */ + +#define BOARD_XTAL_FREQUENCY 24000000 +#define IMXRT_PRE_PERIPH_CLK_SEL CCM_CBCMR_PRE_PERIPH_CLK_SEL_PLL1 +#define IMXRT_PERIPH_CLK_SEL CCM_CBCDR_PERIPH_CLK_SEL_PRE_PERIPH +#define IMXRT_ARM_PLL_DIV_SELECT 96 +#define IMXRT_ARM_PODF_DIVIDER 2 +#define IMXRT_AHB_PODF_DIVIDER 1 +#define IMXRT_IPG_PODF_DIVIDER 4 +#define IMXRT_PERCLK_CLK_SEL CCM_CSCMR1_PERCLK_CLK_SEL_IPG_CLK_ROOT +#define IMXRT_PERCLK_PODF_DIVIDER 9 +#define IMXRT_SEMC_PODF_DIVIDER 8 +#define IMXRT_CAN_CLK_SELECT CCM_CSCMR2_CAN_CLK_SEL_PLL3_SW_80 +#define IMXRT_CAN_PODF_DIVIDER 1 + +#define IMXRT_LPSPI_CLK_SELECT CCM_CBCMR_LPSPI_CLK_SEL_PLL3_PFD0 +#define IMXRT_LSPI_PODF_DIVIDER 8 + +#define IMXRT_LPI2C_CLK_SELECT CCM_CSCDR2_LPI2C_CLK_SEL_PLL3_60M +#define IMXRT_LSI2C_PODF_DIVIDER 5 + +#define IMXRT_USDHC1_CLK_SELECT CCM_CSCMR1_USDHC1_CLK_SEL_PLL2_PFD0 +#define IMXRT_USDHC1_PODF_DIVIDER 2 + +#define IMXRT_USB1_PLL_DIV_SELECT CCM_ANALOG_PLL_USB1_DIV_SELECT_20 + +#define IMXRT_SYS_PLL_SELECT CCM_ANALOG_PLL_SYS_DIV_SELECT_22 + +#define IMXRT_USB1_PLL_DIV_SELECT CCM_ANALOG_PLL_USB1_DIV_SELECT_20 + +#define BOARD_CPU_FREQUENCY \ + (BOARD_XTAL_FREQUENCY * (IMXRT_ARM_PLL_DIV_SELECT / 2)) / IMXRT_ARM_PODF_DIVIDER + +#define BOARD_GPT_FREQUENCY \ + (BOARD_CPU_FREQUENCY / IMXRT_IPG_PODF_DIVIDER) / IMXRT_PERCLK_PODF_DIVIDER + + +/* 108Mhz clock for FlexIO using PLL3 PFD2 @ 520 */ +#define CONFIG_FLEXIO1_CLK 1 +#define CONFIG_FLEXIO1_PRED_DIVIDER 5 +#define CONFIG_FLEXIO1_PODF_DIVIDER 1 +#define CONFIG_PLL3_PFD2_FRAC 16 +#define BOARD_FLEXIO_PREQ 108000000 + +/* Define this to enable tracing */ +#if CONFIG_USE_TRACE +# define IMXRT_TRACE_PODF_DIVIDER 1 +# define IMXRT_TRACE_CLK_SELECT CCM_CBCMR_TRACE_CLK_SEL_PLL2_PFD0 +#endif + +/* SDIO *****************************************************************************/ + +/* Pin drive characteristics */ + +#define USDHC1_DATAX_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) +#define USDHC1_CMD_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) +#define USDHC1_CLK_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_SPEED_MAX) +#define USDHC1_CD_IOMUX (IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) + +#define PIN_USDHC1_D0 (GPIO_USDHC1_DATA0_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_02 */ +#define PIN_USDHC1_D1 (GPIO_USDHC1_DATA1_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_03 */ +#define PIN_USDHC1_D2 (GPIO_USDHC1_DATA2_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_04 */ +#define PIN_USDHC1_D3 (GPIO_USDHC1_DATA3_1 | USDHC1_DATAX_IOMUX) /* GPIO_SD_B0_05 */ +#define PIN_USDHC1_DCLK (GPIO_USDHC1_CLK_1 | USDHC1_CLK_IOMUX) /* GPIO_SD_B0_01 */ +#define PIN_USDHC1_CMD (GPIO_USDHC1_CMD_1 | USDHC1_CMD_IOMUX) /* GPIO_SD_B0_00 */ +#define PIN_USDHC1_CD (PIN_USDHC1_D3) + +/* Ideal 400Khz for initial inquiry. + * Given input clock 198 Mhz. + * 386.71875 KHz = 198 Mhz / (256 * 2) + */ + +#define BOARD_USDHC_IDMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV256 +#define BOARD_USDHC_IDMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(2) + +/* Ideal 25 Mhz for other modes + * Given input clock 198 Mhz. + * 24.75 MHz = 198 Mhz / (8 * 1) + */ + +#define BOARD_USDHC_MMCMODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_MMCMODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD1MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD1MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +#define BOARD_USDHC_SD4MODE_PRESCALER USDHC_SYSCTL_SDCLKFS_DIV8 +#define BOARD_USDHC_SD4MODE_DIVISOR USDHC_SYSCTL_DVS_DIV(1) + +/* ETH Disambiguation *******************************************************/ + +/* Ethernet Interrupt: GPIO_B0_15 + * + * This pin has a week pull-up within the PHY, is open-drain, and requires + * an external 1k ohm pull-up resistor (present on the EVK). A falling + * edge then indicates a change in state of the PHY. + */ + +#define GPIO_ENET_INT (IOMUX_ENET_INT_DEFAULT | \ + GPIO_PORT2 | GPIO_PIN15) /* B0_15 */ +#define GPIO_ENET_IRQ IMXRT_IRQ_GPIO2_15 + +/* Ethernet Reset: GPIO_B0_14 + * + * The #RST uses inverted logic. The initial value of zero will put the + * PHY into the reset state. + */ + +#define GPIO_ENET_RST (GPIO_OUTPUT | GPIO_OUTPUT_ZERO | \ + GPIO_PORT2 | GPIO_PIN14 | IOMUX_ENET_RST_DEFAULT) /* B0_14 */ + +#define GPIO_ENET_TX_DATA00 (GPIO_ENET_TX_DATA00_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_07 */ +#define GPIO_ENET_TX_DATA01 (GPIO_ENET_TX_DATA01_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_08 */ +#define GPIO_ENET_RX_DATA00 (GPIO_ENET_RX_DATA00_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_04 */ +#define GPIO_ENET_RX_DATA01 (GPIO_ENET_RX_DATA01_1| \ + IOMUX_ENET_DATA_DEFAULT) /* GPIO_B1_05 */ +#define GPIO_ENET_MDIO (GPIO_ENET_MDIO_1|IOMUX_ENET_MDIO_DEFAULT) /* GPIO_B1_15 */ +#define GPIO_ENET_MDC (GPIO_ENET_MDC_1|IOMUX_ENET_MDC_DEFAULT) /* GPIO_B1_14 */ +#define GPIO_ENET_RX_EN (GPIO_ENET_RX_EN_1|IOMUX_ENET_EN_DEFAULT) /* GPIO_B1_06 */ +#define GPIO_ENET_RX_ER (GPIO_ENET_RX_ER_1|IOMUX_ENET_RXERR_DEFAULT) /* GPIO_B1_11 */ +#define GPIO_ENET_TX_CLK (GPIO_ENET_REF_CLK_2|\ + IOMUX_ENET_TX_CLK_DEFAULT) /* GPIO_B1_10 */ +#define GPIO_ENET_TX_EN (GPIO_ENET_TX_EN_1|IOMUX_ENET_EN_DEFAULT) /* GPIO_B1_09 */ + +/* LED definitions ******************************************************************/ +/* The nxp fmutr1062 board has numerous LEDs but only three, LED_GREEN a Green LED, + * LED_BLUE a Blue LED and LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* PIO Disambiguation ***************************************************************/ +/* LPUARTs + */ +#define LPUART_IOMUX (IOMUX_PULL_UP_22K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_SLOW | IOMUX_SPEED_LOW | IOMUX_SCHMITT_TRIGGER) +#define IOMUX_UART_CTS_DEFAULT (IOMUX_PULL_DOWN_100K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_SLOW) + +/* Debug */ + +#define GPIO_LPUART5_RX (GPIO_LPUART5_RX_1 | LPUART_IOMUX) /* GPIO_B1_13 */ +#define GPIO_LPUART5_TX (GPIO_LPUART5_TX_1 | LPUART_IOMUX) /* GPIO_B1_12 */ + +/* AUX */ + +#define GPIO_LPUART4_RX (GPIO_LPUART4_RX_3 | LPUART_IOMUX) /* GPIO_B1_01 */ +#define GPIO_LPUART4_TX (GPIO_LPUART4_TX_3 | LPUART_IOMUX) /* GPIO_B1_00 */ + +/* GPS 1 */ + +#define GPIO_LPUART2_RX (GPIO_LPUART2_RX_1 | LPUART_IOMUX) /* GPIO_AD_B1_03 */ +#define GPIO_LPUART2_TX (GPIO_LPUART2_TX_1 | LPUART_IOMUX) /* GPIO_AD_B1_02 */ + +/* Telem 1 */ + +#define HS_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_SLEW_SLOW | IOMUX_DRIVE_HIZ | IOMUX_SPEED_MEDIUM | IOMUX_PULL_UP_47K) +#define HS_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_PULL_KEEP) + +#define GPIO_LPUART3_RX (GPIO_LPUART3_RX_1 | LPUART_IOMUX) /* GPIO_AD_B1_07 */ +#define GPIO_LPUART3_TX (GPIO_LPUART3_TX_1 | LPUART_IOMUX) /* GPIO_AD_B1_06 */ +#define GPIO_LPUART3_CTS (GPIO_LPUART3_CTS_1 | IOMUX_UART_CTS_DEFAULT | PADMUX_SION) /* GPIO_AD_B1_04 GPIO1_IO20 (GPIO only, no HW Flow control) */ +#define GPIO_LPUART3_RTS (GPIO_PORT1 | GPIO_PIN21 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | HS_OUTPUT_IOMUX) /* GPIO_AD_B1_05 GPIO1_IO21 (GPIO only, no HW Flow control) */ +//TODO check RT7 partial HW handshake + +/* RC INPUT single wire mode on TX, RX is not used */ + +#define GPIO_LPUART8_RX (GPIO_LPUART8_RX_2 | LPUART_IOMUX) /* WRONG!! */ +#define GPIO_LPUART8_TX (GPIO_LPUART8_TX_1 | LPUART_IOMUX) /* GPIO_AD_B1_10 */ + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + * CAN3 is routed to transceiver. + */ +#define FLEXCAN_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_40OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MEDIUM) + +#define GPIO_FLEXCAN3_RX (GPIO_FLEXCAN3_RX_3 | FLEXCAN_IOMUX) /* GPIO_EMC_37 */ +#define GPIO_FLEXCAN3_TX (GPIO_FLEXCAN3_TX_3 | FLEXCAN_IOMUX) /* GPIO_EMC_36 */ + +/* LPSPI */ +#define LPSPI_IOMUX (IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SLEW_FAST | IOMUX_SPEED_MAX) + + +#define GPIO_LPSPI3_SCK (GPIO_LPSPI3_SCK_1 | LPSPI_IOMUX) /* GPIO_AD_B1_15 */ +#define GPIO_LPSPI3_MISO (GPIO_LPSPI3_SDI_1 | LPSPI_IOMUX) /* GPIO_AD_B1_13 */ +#define GPIO_LPSPI3_MOSI (GPIO_LPSPI3_SDO_1 | LPSPI_IOMUX) /* GPIO_AD_B1_14 */ + +#define GPIO_LPSPI4_SCK (GPIO_LPSPI4_SCK_2 | LPSPI_IOMUX) /* GPIO_B0_03 */ +#define GPIO_LPSPI4_MISO (GPIO_LPSPI4_SDI_2 | LPSPI_IOMUX) /* GPIO_B0_01 */ +#define GPIO_LPSPI4_MOSI (GPIO_LPSPI4_SDO_2 | LPSPI_IOMUX) /* GPIO_B0_02 */ + +#define BOARD_SPI_BUS_MAX_BUS_ITEMS 2 + +/* LPI2Cs */ + +#define LPI2C_IOMUX (IOMUX_SPEED_MEDIUM | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | GPIO_SION_ENABLE) +#define LPI2C_IO_IOMUX (IOMUX_SPEED_MAX | IOMUX_SLEW_FAST | IOMUX_DRIVE_33OHM | IOMUX_OPENDRAIN | IOMUX_PULL_NONE) + +#define GPIO_LPI2C1_SDA (GPIO_LPI2C1_SDA_2 | LPI2C_IOMUX) /* EVK J24-9 R276 */ /* GPIO_AD_B1_01 */ +#define GPIO_LPI2C1_SCL (GPIO_LPI2C1_SCL_2 | LPI2C_IOMUX) /* EVK J24-10 R277 */ /* GPIO_AD_B1_00 */ + +#define GPIO_LPI2C1_SDA_RESET (GPIO_PORT1 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_01 GPIO1_IO17 */ +#define GPIO_LPI2C1_SCL_RESET (GPIO_PORT1 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B1_00 GPIO1_IO16 */ + +#define GPIO_LPI2C4_SDA (GPIO_LPI2C4_SDA_1 | LPI2C_IOMUX) /* GPIO_AD_B0_13 */ +#define GPIO_LPI2C4_SCL (GPIO_LPI2C4_SCL_1 | LPI2C_IOMUX) /* GPIO_AD_B0_12 */ + +#define GPIO_LPI2C4_SDA_RESET (GPIO_PORT1 | GPIO_PIN13 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B0_13 */ +#define GPIO_LPI2C4_SCL_RESET (GPIO_PORT1 | GPIO_PIN12 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LPI2C_IO_IOMUX) /* GPIO_AD_B0_12 */ + +/* Board doesn't provide GPIO or other Hardware for signaling to timing analyzer */ + +#define PROBE_INIT(mask) +#define PROBE(n,s) +#define PROBE_MARK(n) + +/************************************************************************************ + * Public Types + ************************************************************************************/ + +/************************************************************************************ + * Public Data + ************************************************************************************/ + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __NUTTX_CONFIG_NXP_TROPIC_INCLUDE_BOARD_H */ diff --git a/boards/nxp/tropic-community/nuttx-config/nsh/defconfig b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..db4942dd9af0 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig @@ -0,0 +1,236 @@ +# +# 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_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_SPI_CALLBACK is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/nxp/tropic-community/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="imxrt" +CONFIG_ARCH_CHIP_IMXRT=y +CONFIG_ARCH_CHIP_MIMXRT1062DVL6A=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RAMVECTORS=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_ITCM=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU=y +CONFIG_ARM_MPU_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=114325 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_PRODUCTID=0x0024 +CONFIG_CDCACM_PRODUCTSTR="PX4 TROPIC Community" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1FC9 +CONFIG_CDCACM_VENDORSTR="NXP SEMICONDUCTORS" +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_ETH0_PHY_DP83825I=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FSUTILS_IPCFG=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=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=2048 +CONFIG_IMXRT_EDMA=y +CONFIG_IMXRT_EDMA_EDBG=y +CONFIG_IMXRT_EDMA_ELINK=y +CONFIG_IMXRT_EDMA_NTCD=64 +CONFIG_IMXRT_ENET=y +CONFIG_IMXRT_ENET_NTXBUFFERS=8 +CONFIG_IMXRT_ENET_PHYINIT=y +CONFIG_IMXRT_FLEXCAN3=y +CONFIG_IMXRT_FLEXCAN_TXMB=1 +CONFIG_IMXRT_GPIO1_0_15_IRQ=y +CONFIG_IMXRT_GPIO1_16_31_IRQ=y +CONFIG_IMXRT_GPIO2_0_15_IRQ=y +CONFIG_IMXRT_GPIO2_16_31_IRQ=y +CONFIG_IMXRT_GPIO_IRQ=y +CONFIG_IMXRT_INIT_FLEXRAM=y +CONFIG_IMXRT_ITCM=384 +CONFIG_IMXRT_LPI2C1=y +CONFIG_IMXRT_LPI2C4=y +CONFIG_IMXRT_LPI2C_DMA=y +CONFIG_IMXRT_LPI2C_DMA_MAXMSG=16 +CONFIG_IMXRT_LPI2C_DYNTIMEO=y +CONFIG_IMXRT_LPI2C_DYNTIMEO_STARTSTOP=10 +CONFIG_IMXRT_LPSPI3=y +CONFIG_IMXRT_LPSPI3_DMA=y +CONFIG_IMXRT_LPSPI4=y +CONFIG_IMXRT_LPSPI4_DMA=y +CONFIG_IMXRT_LPSPI_DMA=y +CONFIG_IMXRT_LPUART2=y +CONFIG_IMXRT_LPUART3=y +CONFIG_IMXRT_LPUART4=y +CONFIG_IMXRT_LPUART5=y +CONFIG_IMXRT_LPUART8=y +CONFIG_IMXRT_LPUART_INVERT=y +CONFIG_IMXRT_LPUART_SINGLEWIRE=y +CONFIG_IMXRT_SNVS_LPSRTC=y +CONFIG_IMXRT_USBDEV=y +CONFIG_IMXRT_USDHC1=y +CONFIG_IMXRT_USDHC1_INVERT_CD=y +CONFIG_IMXRT_USDHC1_WIDTH_D1_D4=y +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_INTELHEX_BINARY=y +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_IPCFG_BINARY=y +CONFIG_IPCFG_PATH="/fs/microsd" +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_LPI2C1_DMA=y +CONFIG_LPI2C4_DMA=y +CONFIG_LPUART2_RXDMA=y +CONFIG_LPUART2_TXDMA=y +CONFIG_LPUART3_BAUD=57600 +CONFIG_LPUART3_IFLOWCONTROL=y +CONFIG_LPUART3_OFLOWCONTROL=y +CONFIG_LPUART3_RXBUFSIZE=600 +CONFIG_LPUART3_RXDMA=y +CONFIG_LPUART3_TXBUFSIZE=3000 +CONFIG_LPUART3_TXDMA=y +CONFIG_LPUART5_BAUD=57600 +CONFIG_LPUART5_SERIAL_CONSOLE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_NAME_MAX=40 +CONFIG_NET=y +CONFIG_NETDB_DNSCLIENT=y +CONFIG_NETDB_DNSSERVER_NOADDR=y +CONFIG_NETDEV_CAN_BITRATE_IOCTL=y +CONFIG_NETDEV_CAN_FILTER_IOCTL=y +CONFIG_NETDEV_LATEINIT=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DHCPC=y +CONFIG_NETINIT_DNS=y +CONFIG_NETINIT_DNSIPADDR=0XC0A800FE +CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_RETRY_MOUNTPATH=10 +CONFIG_NETINIT_THREAD=y +CONFIG_NETINIT_THREAD_PRIORITY=49 +CONFIG_NETUTILS_TELNETD=y +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_CAN=y +CONFIG_NET_CAN_EXTID=y +CONFIG_NET_CAN_NOTIFIER=y +CONFIG_NET_CAN_RAW_TX_DEADLINE=y +CONFIG_NET_CAN_SOCK_OPTS=y +CONFIG_NET_ETH_PKTSIZE=1518 +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOLINGER=y +CONFIG_NET_TCP=y +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_TIMESTAMP=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_TELNET_LOGIN=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=1048576 +CONFIG_RAM_START=0x20200000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1800 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=2032 +CONFIG_SCHED_WAITPID=y +CONFIG_SDIO_BLOCKSETUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=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_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_CLE=y +CONFIG_SYSTEM_DHCPC_RENEW=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_SYSTEM_SYSTEM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld new file mode 100644 index 000000000000..60a76f97e1b9 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld @@ -0,0 +1,767 @@ +/* Auto-generated */ +*(.text._ZN4uORB7Manager27orb_add_internal_subscriberE6ORB_IDhPj) +*(.text._ZN13MavlinkStream6updateERKy) +*(.text._ZN7Mavlink16update_rate_multEv) +*(.text._ZN3sym17PredictCovarianceIfEEN6matrix6MatrixIT_Lj23ELj23EEERKNS2_IS3_Lj24ELj1EEERKS4_RKNS2_IS3_Lj3ELj1EEES3_SC_SC_S3_S3_) +*(.text._ZN13MavlinkStream12get_size_avgEv) +*(.text._ZN16ControlAllocator3RunEv) +*(.text._ZN22MulticopterRateControl3RunEv.part.0) +*(.text._ZN7Mavlink9task_mainEiPPc) +*(.text._ZN7sensors22VehicleAngularVelocity3RunEv) +*(.text._ZN4uORB12Subscription9subscribeEv.part.0) +*(.text._ZN4uORB7Manager13orb_data_copyEPvS1_Rjb) +*(.text._ZN4uORB10DeviceNode5writeEP4filePKcj) +*(.text._ZN4uORB10DeviceNode7publishEPK12orb_metadataPvPKv) +*(.text._ZN4uORB12DeviceMaster19getDeviceNodeLockedEPK12orb_metadatah) +*(.text._Z12get_orb_meta6ORB_ID) +*(.text._ZN9ICM42688P12ProcessAccelERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN3px49WorkQueue3RunEv) +*(.text._ZN9ICM42688P11ProcessGyroERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN4EKF23RunEv) +*(.text._ZN7sensors10VehicleIMU7PublishEv) +*(.text._ZN4math17WelfordMeanVectorIfLj3EE6updateERKN6matrix6VectorIfLj3EEE) +*(.text._ZN7sensors10VehicleIMU10UpdateGyroEv) +*(.text._ZN9ICM42688P8FIFOReadERKyh) +*(.text._ZN3Ekf20controlGravityFusionERKN9estimator9imuSampleE) +*(.text._ZN16PX4Accelerometer10updateFIFOER19sensor_accel_fifo_s) +*(.text._ZN7sensors22VehicleAngularVelocity19CalibrateAndPublishERKyRKN6matrix7Vector3IfEES7_) +*(.text._ZN4uORB12Subscription10advertisedEv) +*(.text._ZNK15AttitudeControl6updateERKN6matrix10QuaternionIfEE) +*(.text._ZN7sensors10VehicleIMU11UpdateAccelEv) +*(.text.perf_set_elapsed.part.0) +*(.text._ZN4uORB12Subscription6updateEPv) +*(.text._ZN12PX4Gyroscope10updateFIFOER18sensor_gyro_fifo_s) +*(.text._ZN7sensors10VehicleIMU3RunEv) +*(.text.__aeabi_l2f) +*(.text._ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4_) +*(.text.pthread_mutex_timedlock) +*(.text._ZN7sensors22VehicleAngularVelocity21FilterAngularVelocityEiPfi) +*(.text._ZN26MulticopterAttitudeControl3RunEv.part.0) +*(.text._ZN6device3SPI9_transferEPhS1_j) +*(.text._ZN15OutputPredictor21calculateOutputStatesEyRKN6matrix7Vector3IfEEfS4_f) +*(.text._ZN7sensors18VotedSensorsUpdate7imuPollER17sensor_combined_s) +*(.text._Z9rotate_3i8RotationRsS0_S0_) +*(.text.fs_getfilep) +*(.text.MEM_DataCopy0_1) +*(.text._ZN7sensors19VehicleAcceleration3RunEv) +*(.text.uart_ioctl) +*(.text._ZN26MulticopterPositionControl3RunEv.part.0) +*(.text.pthread_mutex_take) +*(.text._ZN14ImuDownSampler6updateERKN9estimator9imuSampleE) +*(.text._ZN39ControlAllocationSequentialDesaturation6mixYawEv) +*(.text._ZN16ControlAllocator25publish_actuator_controlsEv.part.0) +*(.text._ZN9ICM42688P7RunImplEv) +*(.text._ZN4uORB12Subscription9subscribeEv) +*(.text.param_get) +*(.text._ZN7sensors22VehicleAngularVelocity21SensorSelectionUpdateERKyb) +*(.text._ZN3px49WorkQueue3AddEPNS_8WorkItemE) +*(.text._ZN4EKF220PublishLocalPositionERKy) +*(.text._mav_finalize_message_chan_send) +*(.text._ZN3Ekf19fixCovarianceErrorsEb) +*(.text._ZN7sensors22VehicleAngularVelocity16ParametersUpdateEb) +*(.text._ZN6events12SendProtocol6updateERKy) +*(.text._ZN6device3SPI8transferEPhS1_j) +*(.text._ZN27MavlinkStreamDistanceSensor4sendEv) +*(.text.hrt_call_internal) +*(.text._ZN39ControlAllocationSequentialDesaturation18mixAirmodeDisabledEv) +*(.text._ZN7Mavlink15get_free_tx_bufEv) +*(.text.nx_poll) +*(.text._ZN15MavlinkReceiver3runEv) +*(.text._ZN9ICM42688P18ProcessTemperatureEPKN20InvenSense_ICM42688P4FIFO4DATAEh) +*(.text._ZN15OutputPredictor19correctOutputStatesEyRKN6matrix10QuaternionIfEERKNS0_7Vector3IfEES8_S8_S8_) +*(.text._ZN3Ekf12predictStateERKN9estimator9imuSampleE) +*(.text._ZN3px46logger6Logger3runEv) +*(.text._ZN4uORB20SubscriptionInterval7updatedEv) +*(.text._ZN24MavlinkStreamCommandLong4sendEv) +*(.text._ZN9Commander3runEv) +*(.text._ZN3Ekf17predictCovarianceERKN9estimator9imuSampleE) +*(.text.wd_cancel) +*(.text._ZN7Sensors3RunEv) +*(.text.perf_end) +*(.text._ZN4uORB12Subscription7updatedEv) +*(.text._ZN13land_detector12LandDetector3RunEv) +*(.text.sched_idletask) +*(.text.atanf) +*(.text.uart_write) +*(.text.pthread_mutex_unlock) +*(.text.__ieee754_asinf) +*(.text.MEM_DataCopy0_2) +*(.text._ZN20MavlinkCommandSender13check_timeoutE17mavlink_channel_t) +*(.text._ZN16ControlAllocator32publish_control_allocator_statusEi) +*(.text.__ieee754_atan2f) +*(.text._ZNK18DynamicSparseLayer3getEt) +*(.text.__udivmoddi4) +*(.text._ZN8Failsafe17checkStateAndModeERKyRKN12FailsafeBase5StateERK16failsafe_flags_s) +*(.text._ZN29MavlinkStreamHygrometerSensor4sendEv) +*(.text.pthread_mutex_give) +*(.text._ZN3Ekf18controlFusionModesERKN9estimator9imuSampleE) +*(.text._ZN4cdev4CDev11poll_notifyEm) +*(.text.file_vioctl) +*(.text._ZN7sensors18VotedSensorsUpdate11sensorsPollER17sensor_combined_s) +*(.text.nxsig_nanosleep) +*(.text.imxrt_lpspi1select) +*(.text.sem_wait) +*(.text.perf_count_interval.part.0) +*(.text._ZN16ControlAllocator37update_effectiveness_matrix_if_neededE25EffectivenessUpdateReason) +*(.text.MEM_LongCopyJump) +*(.text.px4_arch_adc_sample) +*(.text._ZN31MulticopterHoverThrustEstimator3RunEv) +*(.text._ZNK17ControlAllocation20clipActuatorSetpointERN6matrix6VectorIfLj16EEE) +*(.text._ZN4uORB7Manager17get_device_masterEv) +*(.text._ZN13DataValidator3putEyPKfmh) +*(.text.cdcuart_ioctl) +*(.text.cdcacm_sndpacket) +*(.text._ZN7sensors22VehicleAngularVelocity16SensorBiasUpdateEb) +*(.text._ZN13MavlinkStream11update_dataEv) +*(.text._ZN7sensors18VotedSensorsUpdate21calcGyroInconsistencyEv) +*(.text.param_set_used) +*(.text._ZN18EstimatorInterface10setIMUDataERKN9estimator9imuSampleE) +*(.text._ZN18DataValidatorGroup8get_bestEyPi) +*(.text._ZN4EKF218PublishInnovationsERKy) +*(.text._ZN21MavlinkMissionManager4sendEv) +*(.text._ZN22MulticopterRateControl28updateActuatorControlsStatusERK25vehicle_torque_setpoint_sf) +*(.text._ZN11RateControl6updateERKN6matrix7Vector3IfEES4_S4_fb) +*(.text._ZN39ControlAllocationSequentialDesaturation19desaturateActuatorsERN6matrix6VectorIfLj16EEERKS2_b) +*(.text._ZN22MavlinkStreamCollision4sendEv) +*(.text.imxrt_lpi2c_transfer) +*(.text.uart_putxmitchar) +*(.text.clock_nanosleep) +*(.text.up_release_pending) +*(.text.MEM_DataCopy0) +*(.text._ZN22MavlinkStreamGPSRawInt4sendEv) +*(.text.dq_rem) +*(.text._ZN15GyroCalibration3RunEv.part.0) +*(.text._ZN7sensors18VotedSensorsUpdate22calcAccelInconsistencyEv) +*(.text._ZN24MavlinkStreamADSBVehicle4sendEv) +*(.text.sinf) +*(.text.hrt_call_after) +*(.text._ZN39ControlAllocationSequentialDesaturation8allocateEv) +*(.text.up_invalidate_dcache) +*(.text._ZN15PositionControl16_velocityControlEf) +*(.text._ZN4EKF222PublishAidSourceStatusERKy) +*(.text._ZN4ListIP13MavlinkStreamE8IteratorppEv) +*(.text._ZN20MavlinkStreamESCInfo4sendEv) +*(.text.sem_post) +*(.text._ZN3px417ScheduledWorkItem15ScheduleDelayedEm) +*(.text._ZN10FlightTaskC1Ev) +*(.text.usleep) +*(.text._ZN14FlightTaskAutoC1Ev) +*(.text.sem_getvalue) +*(.text._ZN23MavlinkStreamHighresIMU4sendEv) +*(.text.imxrt_gpio_write) +*(.text._ZN3Ekf6updateEv) +*(.text.__ieee754_acosf) +*(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE) +*(.text._ZN9Commander13dataLinkCheckEv) +*(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex) +*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_) +*(.text._ZN12PX4Gyroscope9set_scaleEf) +*(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s) +*(.text._ZN18MavlinkStreamDebug4sendEv) +*(.text._ZN27MavlinkStreamServoOutputRawILi0EE4sendEv) +*(.text.asinf) +*(.text._ZN6matrix5EulerIfEC1ERKNS_3DcmIfEE) +*(.text._ZN4EKF227PublishInnovationTestRatiosERKy) +*(.text._ZN4EKF213PublishStatusERKy) +*(.text._ZN4EKF226PublishInnovationVariancesERKy) +*(.text._ZN13land_detector23MulticopterLandDetector25_get_ground_contact_stateEv) +*(.text.imxrt_dmach_start) +*(.text._ZN3ADC19update_system_powerEy) +*(.text._ZNK3Ekf19get_ekf_soln_statusEPt) +*(.text._ZN3px46logger15watchdog_updateERNS0_15watchdog_data_tEb) +*(.text.imxrt_gpio_read) +*(.text._ZN32MavlinkStreamNavControllerOutput4sendEv) +*(.text._ZN15ArchPX4IOSerial13_bus_exchangeEP8IOPacket) +*(.text._ZN39MavlinkStreamGimbalDeviceAttitudeStatus4sendEv) +*(.text._ZNK10ConstLayer3getEt) +*(.text.__aeabi_uldivmod) +*(.text.up_udelay) +*(.text.up_idle) +*(.text._ZN20MavlinkStreamGPS2Raw4sendEv) +*(.text._ZN4EKF217UpdateCalibrationERKyRNS_19InFlightCalibrationERKN6matrix7Vector3IfEES8_fbb) +*(.text._ZN28MavlinkStreamGpsGlobalOrigin4sendEv) +*(.text._ZN11ControlMath15bodyzToAttitudeEN6matrix7Vector3IfEEfR27vehicle_attitude_setpoint_s) +*(.text._ZN4EKF217UpdateRangeSampleER17ekf2_timestamps_s) +*(.text._ZN3Ekf24controlOpticalFlowFusionERKN9estimator9imuSampleE) +*(.text._ZN19MavlinkStreamRawRpm4sendEv) +*(.text._ZN13MavlinkStream10const_rateEv) +*(.text._ZN4EKF215PublishOdometryERKyRKN9estimator9imuSampleE) +*(.text._ZN15FailureDetector20updateAttitudeStatusERK16vehicle_status_s) +*(.text._ZN7Mavlink19configure_sik_radioEv) +*(.text._ZN13BatteryStatus8adc_pollEv) +*(.text.getpid) +*(.text._ZN13DataValidator10confidenceEy) +*(.text._ZN22MavlinkStreamGPSStatus4sendEv) +*(.text._ZN4EKF220UpdateAirspeedSampleER17ekf2_timestamps_s) +*(.text._ZN23MavlinkStreamStatustext4sendEv) +*(.text._ZN3Ekf15constrainStatesEv) +*(.text._ZN12PX4IO_serial4readEjPvj) +*(.text.uart_poll) +*(.text._ZN24MavlinkParametersManager4sendEv) +*(.text._ZN26MulticopterPositionControl18set_vehicle_statesERK24vehicle_local_position_s) +*(.text.file_poll) +*(.text.hrt_elapsed_time) +*(.text._ZN7Mavlink11send_finishEv) +*(.text._ZNK3Ekf36estimateInertialNavFallingLikelihoodEv) +*(.text._ZN15PositionControl16_positionControlEv) +*(.text._ZN28MavlinkStreamDebugFloatArray4sendEv) +*(.text._ZN11ControlMath9limitTiltERN6matrix7Vector3IfEERKS2_f) +*(.text.pthread_mutex_lock) +*(.text._ZN21MavlinkStreamAltitude8get_sizeEv) +*(.text._ZN7Mavlink29check_requested_subscriptionsEv) +*(.text.imxrt_lpspi_setmode) +*(.text._ZN3Ekf34controlZeroInnovationHeadingUpdateEv) +*(.text.perf_begin) +*(.text.imxrt_lpspi_setfrequency) +*(.text._ZN17FlightModeManager9_initTaskE15FlightTaskIndex) +*(.text._ZN22MulticopterRateControl3RunEv) +*(.text.cosf) +*(.text._ZN22MavlinkStreamESCStatus4sendEv) +*(.text._ZN26MavlinkStreamCameraTrigger4sendEv) +*(.text._ZN36MavlinkStreamPositionTargetGlobalInt4sendEv) +*(.text._ZN4uORB12Subscription4copyEPv) +*(.text._ZN7sensors19VehicleAcceleration21SensorSelectionUpdateEb) +*(.text.crc_accumulate) +*(.text._ZN3px46logger6Logger13update_paramsEv) +*(.text._ZN11calibration14DeviceExternalEm) +*(.text._ZN25MavlinkStreamHomePosition8get_sizeEv) +*(.text.imxrt_lpspi_modifyreg32) +*(.text._ZN7sensors19VehicleAcceleration16SensorBiasUpdateEb) +*(.text.modifyreg32) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EEmlEf) +*(.text._ZN6matrix5EulerIfEC1ERKNS_10QuaternionIfEE) +*(.text.imxrt_queuedtd) +*(.text._ZN27MavlinkStreamDistanceSensor8get_sizeEv) +*(.text._ZN3Ekf16fuseVelPosHeightEffi) +*(.text._ZN3Ekf23controlBaroHeightFusionEv) +*(.text._ZN16PX4Accelerometer9set_scaleEf) +*(.text._ZN11ControlMath11constrainXYERKN6matrix7Vector2IfEES4_RKf) +*(.text._ZN22MavlinkStreamEfiStatus4sendEv) +*(.text._ZN22MavlinkStreamDebugVect4sendEv) +*(.text._ZN4EKF217PublishSensorBiasERKy) +*(.text._ZN17FlightModeManager3RunEv) +*(.text._ZN15PositionControl11_inputValidEv) +*(.text._ZN7sensors14VehicleAirData3RunEv) +*(.text.perf_count) +*(.text._ZN3Ekf16controlMagFusionEv) +*(.text.pthread_sem_give) +*(.text._ZN7sensors10VehicleIMU16ParametersUpdateEb) +*(.text._ZN30MavlinkStreamUTMGlobalPosition4sendEv) +*(.text._ZN4uORB20SubscriptionInterval4copyEPv) +*(.text._ZN12I2CSPIDriverI9ICM42688PE3RunEv) +*(.text._ZN17ObstacleAvoidanceC1EP12ModuleParams) +*(.text.imxrt_epcomplete.constprop.0) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EEmiERKS1_) +*(.text._ZN9Commander30handleModeIntentionAndFailsafeEv) +*(.text.perf_event_count) +*(.text._ZN4EKF215PublishAttitudeERKy) +*(.text._ZN19MavlinkStreamRawRpm8get_sizeEv) +*(.text._ZNK3px46atomicIbE4loadEv) +*(.text._ZN29MavlinkStreamHygrometerSensor8get_sizeEv) +*(.text.pthread_mutex_add) +*(.text._ZN12HomePosition6updateEbb) +*(.text._ZN5PX4IO3RunEv) +*(.text.poll_fdsetup) +*(.text._ZN15PositionControl20_accelerationControlEv) +*(.text._ZN3Ekf19controlHeightFusionERKN9estimator9imuSampleE) +*(.text._ZN9Commander19control_status_ledsEbh) +*(.text._ZN6device3I2C8transferEPKhjPhj) +*(.text.orb_publish) +*(.text._ZN7sensors19VehicleAcceleration16ParametersUpdateEb) +*(.text._ZN22MavlinkStreamVibration8get_sizeEv) +*(.text._ZN15MavlinkReceiver15CheckHeartbeatsERKyb) +*(.text._ZNK6matrix7Vector3IfEmiES1_) +*(.text.__aeabi_f2ulz) +*(.text._ZN9ICM42688P26DataReadyInterruptCallbackEiPvS0_) +*(.text._ZN13land_detector23MulticopterLandDetector23_get_maybe_landed_stateEv) +*(.text.acosf) +*(.text._ZN14ImuDownSampler5resetEv) +*(.text._ZN3Ekf31checkVerticalAccelerationHealthERKN9estimator9imuSampleE) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1ERKS1_) +*(.text.udp_pollsetup) +*(.text._ZL14timer_callbackPv) +*(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj23EEEf) +*(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi) +*(.text.nxsem_wait_irq) +*(.text._ZN20MavlinkCommandSender4lockEv) +*(.text.MEM_LongCopyEnd) +*(.text._ZThn24_N16ControlAllocator3RunEv) +*(.text._ZN15TimestampedListIN20MavlinkCommandSender14command_item_sELi3EE8get_nextEv) +*(.text._ZNK3Ekf21get_ekf_lpos_accuracyEPfS0_) +*(.text._ZN17FlightModeManager17start_flight_taskEv) +*(.text.MEM_DataCopyBytes) +*(.text._ZN29MavlinkStreamLocalPositionNED8get_sizeEv) +*(.text._ZN6SticksC1EP12ModuleParams) +*(.text._ZN27MavlinkStreamServoOutputRawILi1EE4sendEv) +*(.text._ZN3Ekf35updateHorizontalDeadReckoningstatusEv) +*(.text._ZN3Ekf20controlAirDataFusionERKN9estimator9imuSampleE) +*(.text._ZN24FlightTaskManualAltitudeC1Ev) +*(.text._ZN25MavlinkStreamHomePosition4sendEv) +*(.text._ZN24MavlinkParametersManager8send_oneEv) +*(.text._ZN15OutputPredictor29applyCorrectionToOutputBufferERKN6matrix7Vector3IfEES4_) +*(.text._ZN21HealthAndArmingChecks6updateEb) +*(.text._ZThn24_N22MulticopterRateControl3RunEv) +*(.text._ZN26MavlinkStreamManualControl4sendEv) +*(.text._ZN27MavlinkStreamOpticalFlowRad4sendEv) +*(.text._ZN18mag_bias_estimator16MagBiasEstimator3RunEv) +*(.text._ZN4uORB7Manager11orb_publishEPK12orb_metadataPvPKv) +*(.text._ZN24MavlinkParametersManager18send_untransmittedEv) +*(.text._ZN10MavlinkFTP4sendEv) +*(.text._ZN15ArchPX4IOSerial13_do_interruptEv) +*(.text._ZN3Ekf27controlExternalVisionFusionEv) +*(.text.clock_gettime) +*(.text._ZN3ADC17update_adc_reportEy) +*(.text._ZN3sym25ComputeYaw312InnovVarAndHIfEEvRKN6matrix6MatrixIT_Lj24ELj1EEERKNS2_IS3_Lj23ELj23EEES3_S3_PS3_PNS2_IS3_Lj23ELj1EEE) +*(.text._ZN3Ekf19runTerrainEstimatorERKN9estimator9imuSampleE) +*(.text._ZN32MavlinkStreamGimbalManagerStatus4sendEv) +*(.text._ZN9LockGuardD1Ev) +*(.text._ZN4EKF213PublishStatesERKy) +*(.text._ZN3ADC3RunEv) +*(.text._ZN6BMP38815compensate_dataEhPK16bmp3_uncomp_dataP9bmp3_data) +*(.text._ZN3Ekf20controlFakePosFusionEv) +*(.text._ZN11calibration9Gyroscope13set_device_idEm) +*(.text._ZN24MavlinkStreamOrbitStatus8get_sizeEv) +*(.text.imxrt_progressep) +*(.text.imxrt_gpio_configinput) +*(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) +*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN21MavlinkStreamAttitude4sendEv) +*(.text._ZN4EKF220UpdateMagCalibrationERKy) +*(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv) +*(.text._ZN9ICM42688P9DataReadyEv) +*(.text._ZN21MavlinkMissionManager20check_active_missionEv) +*(.text._ZNK3Ekf20get_ekf_vel_accuracyEPfS0_) +*(.text._ZN4EKF216UpdateBaroSampleER17ekf2_timestamps_s) +*(.text._ZN4EKF223UpdateSystemFlagsSampleER17ekf2_timestamps_s) +*(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv) +*(.text._ZN29MavlinkStreamObstacleDistance4sendEv) +*(.text._ZN24MavlinkStreamOrbitStatus4sendEv) +*(.text._ZN16PreFlightChecker26preFlightCheckHeightFailedERK23estimator_innovations_sf) +*(.text._ZN9Navigator3runEv) +*(.text._ZN24MavlinkParametersManager11send_paramsEv) +*(.text._ZN17MavlinkLogHandler4sendEv) +*(.text._ZN7control10SuperBlock5setDtEf) +*(.text._ZN29MavlinkStreamMountOrientation8get_sizeEv) +*(.text._ZN5PX4IO13io_get_statusEv) +*(.text._ZN26MulticopterAttitudeControl3RunEv) +*(.text._ZThn16_N31ActuatorEffectivenessMultirotor22getEffectivenessMatrixERN21ActuatorEffectiveness13ConfigurationE25EffectivenessUpdateReason) +*(.text._ZN4EKF218PublishStatusFlagsERKy) +*(.text._ZN11WeatherVaneC1EP12ModuleParams) +*(.text._ZN15FailureDetector6updateERK16vehicle_status_sRK22vehicle_control_mode_s) +*(.text._ZN7Mavlink10send_startEi) +*(.text.imxrt_lpspi_setbits) +*(.text._ZN15OutputPredictor37applyCorrectionToVerticalOutputBufferEf) +*(.text._ZN4EKF222UpdateAccelCalibrationERKy) +*(.text._ZN7sensors19VehicleMagnetometer3RunEv) +*(.text._ZN29MavlinkStreamMountOrientation4sendEv) +*(.text._ZN13land_detector12LandDetector19UpdateVehicleAtRestEv) +*(.text._ZN10FlightTask29_evaluateVehicleLocalPositionEv) +*(.text.board_autoled_off) +*(.text.__aeabi_f2lz) +*(.text._ZN32MavlinkStreamCameraImageCaptured4sendEv) +*(.text._ZN21MavlinkStreamOdometry8get_sizeEv) +*(.text._ZN28MavlinkStreamNamedValueFloat4sendEv) +*(.text.__aeabi_ul2f) +*(.text.poll) +*(.text._ZN14FlightTaskAutoD1Ev) +*(.text._ZN4uORB10DeviceNode22get_initial_generationEv) +*(.text._ZN3Ekf23controlGnssHeightFusionERKN9estimator9gpsSampleE) +*(.text._ZN3Ekf40updateOnGroundMotionForOpticalFlowChecksEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1Ev) +*(.text._ZN14ZeroGyroUpdate6updateER3EkfRKN9estimator9imuSampleE) +*(.text._ZN30MavlinkStreamOpenDroneIdSystem4sendEv) +*(.text._ZN22MavlinkStreamScaledIMU4sendEv) +*(.text._ZN46MavlinkStreamTrajectoryRepresentationWaypoints4sendEv) +*(.text.imxrt_ioctl) +*(.text._ZN3Ekf25checkMagBiasObservabilityEv) +*(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv) +*(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s) +*(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0) +*(.text._ZN19StickAccelerationXYC1EP12ModuleParams) +*(.text.imxrt_epsubmit) +*(.text._ZN15PositionControl6updateEf) +*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE) +*(.text._ZN23MavlinkStreamScaledIMU24sendEv) +*(.text._ZN5PX4IO10io_reg_getEhhPtj) +*(.text.imxrt_dma_send) +*(.text._ZN20MavlinkStreamWindCov4sendEv) +*(.text._ZN7sensors18VotedSensorsUpdate13checkFailoverERNS0_10SensorDataEPKcN6events3px45enums13sensor_type_tE) +*(.text._ZN21MavlinkStreamOdometry4sendEv) +*(.text.vsprintf_internal.constprop.0) +*(.text.udp_pollteardown) +*(.text._ZN12MixingOutput6updateEv) +*(.text.clock_abstime2ticks) +*(.text._ZN13BatteryStatus3RunEv) +*(.text._ZN32MavlinkStreamGimbalManagerStatus8get_sizeEv) +*(.text._ZN10FlightTask15_resetSetpointsEv) +*(.text._ZN9systemlib10Hysteresis20set_state_and_updateEbRKy) +*(.text.devif_callback_free.part.0) +*(.text._ZN6Sticks25checkAndUpdateStickInputsEv) +*(.text.atan2f) +*(.text._ZN23MavlinkStreamRCChannels4sendEv) +*(.text._ZN4EKF221UpdateExtVisionSampleER17ekf2_timestamps_s) +*(.text.imxrt_dmach_stop) +*(.text._ZN9Commander16handleAutoDisarmEv) +*(.text._ZN9Commander11updateTunesEv) +*(.text._ZN4EKF215UpdateMagSampleER17ekf2_timestamps_s) +*(.text._ZN18DataValidatorGroup3putEjyPKfmh) +*(.text._ZNK3Ekf19get_ekf_ctrl_limitsEPfS0_S0_S0_) +*(.text._ZN12FailsafeBase13checkFailsafeEibbRKNS_13ActionOptionsE) +*(.text._ZN17FlightTaskDescendD1Ev) +*(.text._ZN30MavlinkStreamOpenDroneIdSystem8get_sizeEv) +*(.text._ZNK3px46logger9LogWriter10is_startedENS0_7LogTypeE) +*(.text._ZN24FlightTaskManualAltitudeD1Ev) +*(.text._Z35px4_indicate_external_reset_lockout16LockoutComponentb) +*(.text.uart_pollnotify) +*(.text._ZN3Ekf11predictHaglERKN9estimator9imuSampleE) +*(.text._ZN4EKF215PublishBaroBiasERKy) +*(.text._ZN4EKF221UpdateGyroCalibrationERKy) +*(.text._ZN6matrix9constrainIfLj3ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) +*(.text._ZN4uORB22SubscriptionMultiArrayI16battery_status_sLh4EE16advertised_countEv) +*(.text._ZN23MavlinkStreamScaledIMU34sendEv) +*(.text.__aeabi_ldivmod) +*(.text._ZN15PositionControl16setInputSetpointERK21trajectory_setpoint_s) +*(.text.nxsig_pendingset) +*(.text.psock_poll) +*(.text._ZN15FailureInjector6updateEv) +*(.text.imxrt_writedtd) +*(.text.cdcacm_wrcomplete) +*(.text.cdcuart_txint) +*(.text._ZN3Ekf33updateVerticalDeadReckoningStatusEv) +*(.text._ZN33FlightTaskManualAltitudeSmoothVelC1Ev) +*(.text.powf) +*(.text._ZN4EKF217PublishEventFlagsERKy) +*(.text._ZN17FlightTaskDescend6updateEv) +*(.text.imxrt_iomux_configure) +*(.text.hrt_store_absolute_time) +*(.text.nxsem_set_protocol) +*(.text.write) +*(.text._ZN22MavlinkStreamSysStatus4sendEv) +*(.text._ZN4EKF216UpdateFlowSampleER17ekf2_timestamps_s) +*(.text._ZN4cdevL10cdev_ioctlEP4fileim) +*(.text._ZN7Mavlink10send_bytesEPKhj) +*(.text._ZNK8Failsafe17checkModeFallbackERK16failsafe_flags_sh) +*(.text.clock_systime_timespec) +*(.text._ZN4uORB10DeviceNode26remove_internal_subscriberEv) +*(.text._ZThn16_N4EKF23RunEv) +*(.text._ZNK3Ekf22computeYawInnovVarAndHEfRfRN6matrix6VectorIfLj23EEE) +*(.text._ZN12ActuatorTest6updateEif) +*(.text._ZN17VelocitySmoothingC1Efff) +*(.text._ZN13AnalogBattery19get_voltage_channelEv) +*(.text._ZN10FlightTask37_evaluateVehicleLocalPositionSetpointEv) +*(.text._ZN4uORB12Subscription11unsubscribeEv) +*(.text.net_lock) +*(.text.clock_time2ticks) +*(.text._ZN12FailsafeBase16updateStartDelayERKyb) +*(.text._ZN23MavlinkStreamStatustext8get_sizeEv) +*(.text._ZN11calibration13Accelerometer13set_device_idEm) +*(.text._ZN3px46logger6Logger18start_stop_loggingEv) +*(.text._ZN14FlightTaskAuto17_evaluateTripletsEv) +*(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb) +*(.text._ZN25MavlinkStreamMagCalReport4sendEv) +*(.text._ZN16PreFlightChecker27preFlightCheckHeadingFailedERK23estimator_innovations_sf) +*(.text.imxrt_config_gpio) +*(.text.nxsig_timeout) +*(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_) +*(.text._ZN3Ekf17measurementUpdateERN6matrix6VectorIfLj23EEEff) +*(.text.dq_addlast) +*(.text._ZN19MavlinkStreamVFRHUD4sendEv) +*(.text.hrt_call_reschedule) +*(.text.nxsem_boost_priority) +*(.text._ZN4EKF215UpdateGpsSampleER17ekf2_timestamps_s) +*(.text._ZN8StickYawC1EP12ModuleParams) +*(.text._ZN7control12BlockLowPass6updateEf) +*(.text._ZN15FailureDetector26updateImbalancedPropStatusEv) +*(.text._ZN9systemlib10Hysteresis6updateERKy) +*(.text.nxsem_tickwait_uninterruptible) +*(.text._ZN12HomePosition31hasMovedFromCurrentHomeLocationEv) +*(.text.devif_callback_alloc) +*(.text._ZN28MavlinkStreamNamedValueFloat8get_sizeEv) +*(.text._ZN24MavlinkStreamADSBVehicle8get_sizeEv) +*(.text._ZN26MavlinkStreamBatteryStatus8get_sizeEv) +*(.text._ZN26MulticopterPositionControl17parameters_updateEb) +*(.text._ZN3px46logger6Logger29handle_vehicle_command_updateEv) +*(.text.imxrt_lpspi_send) +*(.text._ZN4uORB10DeviceNode23add_internal_subscriberEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEaSERKS1_) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EE5emultERKS1_) +*(.text.mallinfo_handler) +*(.text._ZN13land_detector23MulticopterLandDetector14_update_topicsEv) +*(.text._ZN24ManualVelocitySmoothingZC1Ev) +*(.text._ZN3ADC6sampleEj) +*(.text._ZNK3Ekf22isTerrainEstimateValidEv) +*(.text._ZN15EstimatorChecks23setModeRequirementFlagsERK7ContextbbRK24vehicle_local_position_sRK12sensor_gps_sR16failsafe_flags_sR6Report) +*(.text._ZN11ControlMath11addIfNotNanERff) +*(.text._ZN9Commander21checkForMissionUpdateEv) +*(.text._Z8set_tunei) +*(.text._ZN3Ekf13stopGpsFusionEv) +*(.text._ZNK6matrix7Vector3IfE5crossERKNS_6MatrixIfLj3ELj1EEE) +*(.text._ZN10FlightTask22_checkEkfResetCountersEv) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EE11isAllFiniteEv) +*(.text._ZN14FlightTaskAuto24_evaluateGlobalReferenceEv) +*(.text._ZN6matrix9constrainIfLj2ELj1EEENS_6MatrixIT_XT0_EXT1_EEERKS3_S2_S2_) +*(.text._ZN3px46logger6Logger23handle_file_write_errorEv) +*(.text._ZN10FlightTask16updateInitializeEv) +*(.text._ZN11calibration9Gyroscope10set_offsetERKN6matrix7Vector3IfEE) +*(.text._ZNK6matrix6VectorIfLj3EE4normEv) +*(.text._ZN14FlightTaskAuto16updateInitializeEv) +*(.text.fabsf) +*(.text._ZN27MavlinkStreamAttitudeTarget8get_sizeEv) +*(.text.nxsem_get_value) +*(.text._ZN13AnalogBattery8is_validEv) +*(.text._ZN4uORB16SubscriptionDataI15home_position_sEC1EPK12orb_metadatah) +*(.text._ZN22MavlinkStreamGPSStatus8get_sizeEv) +*(.text.nxsem_destroyholder) +*(.text.psock_udp_cansend) +*(.text.MEM_DataCopy2_2) +*(.text._ZN10FlightTask8activateERK21trajectory_setpoint_s) +*(.text.sock_file_poll) +*(.text._ZN3Ekf20controlHaglRngFusionEv) +*(.text._ZN10Ringbuffer9pop_frontEPhj) +*(.text.nx_write) +*(.text._ZN9Commander18manualControlCheckEv) +*(.text._ZN31MavlinkStreamAttitudeQuaternion4sendEv) +*(.text.nxsem_canceled) +*(.text._ZN10FlightTask21getTrajectorySetpointEv) +*(.text.imxrt_dmach_getcount) +*(.text.sem_clockwait) +*(.text.inet_poll) +*(.text._ZN6BMP3887collectEv) +*(.text._ZN15ArchPX4IOSerial19_do_rx_dma_callbackEbi) +*(.text._ZN15ArchPX4IOSerial10_abort_dmaEv) +*(.text._ZNK15PositionControl24getLocalPositionSetpointER33vehicle_local_position_setpoint_s) +*(.text._ZN3Ekf16controlGpsFusionERKN9estimator9imuSampleE) +*(.text._ZN23MavlinkStreamRCChannels8get_sizeEv) +*(.text._ZN20MavlinkStreamESCInfo8get_sizeEv) +*(.text._ZNK6matrix6VectorIfLj2EE4normEv) +*(.text._Z15arm_auth_updateyb) +*(.text._ZN3LED5ioctlEP4fileim) +*(.text._ZNK3px46logger9LogWriter20had_file_write_errorEv) +*(.text._ZN29MavlinkStreamLocalPositionNED4sendEv) +*(.text._ZN6matrix6MatrixIfLj2ELj1EEC1ILj3ELj1EEERKNS_5SliceIfLj2ELj1EXT_EXT0_EEE) +*(.text._ZNK6matrix6VectorIfLj3EE3dotERKNS_6MatrixIfLj3ELj1EEE) +*(.text.imxrt_lpi2c_setclock) +*(.text._ZN6matrix12typeFunction9constrainIfEET_S2_S2_S2_.part.0) +*(.text._ZN13MapProjection13initReferenceEddy) +*(.text._ZN11calibration13Accelerometer23SensorCorrectionsUpdateEb) +*(.text._ZN31MavlinkStreamAttitudeQuaternion8get_sizeEv) +*(.text._ZN30MavlinkStreamGlobalPositionInt4sendEv) +*(.text._ZN6SticksD1Ev) +*(.text._ZN13NavigatorMode3runEb) +*(.text._ZL19param_find_internalPKcb) +*(.text.uart_datasent) +*(.text._ZN4EKF221PublishOpticalFlowVelERKy) +*(.text.nxsem_destroy) +*(.text.file_write) +*(.text._ZN21MavlinkStreamAltitude4sendEv) +*(.text._ZN7sensors14VehicleAirData12UpdateStatusEv) +*(.text.imxrt_padmux_map) +*(.text._ZN6BMP38815get_sensor_dataEhP9bmp3_data) +*(.text._ZN18MavlinkRateLimiter5checkERKy) +*(.text._ZThn24_N26MulticopterAttitudeControl3RunEv) +*(.text._ZN15ArchPX4IOSerial10_interruptEiPvS0_) +*(.text.imxrt_periphclk_configure) +*(.text._ZN3Ekf8initHaglEv) +*(.text._ZN4EKF218UpdateAuxVelSampleER17ekf2_timestamps_s) +*(.text._ZN3RTL11on_inactiveEv) +*(.text._ZN12FailsafeBase10modeCanRunERK16failsafe_flags_sh) +*(.text._ZN4EKF216PublishEvPosBiasERKy) +*(.text._ZN21MavlinkStreamAttitude8get_sizeEv) +*(.text._ZThn16_N7sensors19VehicleAcceleration3RunEv) +*(.text._ZN3Ekf24controlRangeHeightFusionEv) +*(.text._ZN33MavlinkStreamTimeEstimateToTarget4sendEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EE6setAllEf) +*(.text._ZN12ModuleParamsD1Ev) +*(.text._ZN3Ekf20controlFakeHgtFusionEv) +*(.text.imxrt_reqcomplete) +*(.text._ZNK6matrix7Vector3IfEmlEf) +*(.text._ZN18ZeroVelocityUpdate6updateER3EkfRKN9estimator9imuSampleE) +*(.text._ZN11ControlMath19addIfNotNanVector3fERN6matrix7Vector3IfEERKS2_) +*(.text._ZN11ControlMath16thrustToAttitudeERKN6matrix7Vector3IfEEfR27vehicle_attitude_setpoint_s) +*(.text.cos) +*(.text.net_unlock) +*(.text._ZN7sensors18VotedSensorsUpdate21setRelativeTimestampsER17sensor_combined_s) +*(.text._ZN12FailsafeBase13ActionOptionsC1ENS_6ActionE) +*(.text._ZN24FlightTaskManualAltitude16updateInitializeEv) +*(.text._ZN26MulticopterPositionControl3RunEv) +*(.text._ZN8Failsafe21fromQuadchuteActParamEi) +*(.text._ZN24VariableLengthRingbuffer9pop_frontEPhj) +*(.text._ZN7control15BlockDerivative6updateEf) +*(.text._ZN5PX4IO10io_reg_getEhh) +*(.text._ZN9Commander18safetyButtonUpdateEv) +*(.text._ZN13BatteryChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN18DataValidatorGroup16get_sensor_stateEj) +*(.text.uart_xmitchars_done) +*(.text._ZN4EKF225PublishYawEstimatorStatusERKy) +*(.text.sin) +*(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf) +*(.text._ZN6Safety19safetyButtonHandlerEv) +*(.text._ZN3Ekf19controlAuxVelFusionEv) +*(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_) +*(.text._ZThn24_N7Sensors3RunEv) +*(.text._ZN6matrix6MatrixIfLj2ELj1EEC1ERKS1_) +*(.text._ZN10FlightTask10reActivateEv) +*(.text._ZN5PX4IO17io_publish_raw_rcEv) +*(.text._ZNK15PositionControl19getAttitudeSetpointER27vehicle_attitude_setpoint_s) +*(.text._ZN4cdev4CDev4pollEP4fileP6pollfdb) +*(.text._ZN9Commander20offboardControlCheckEv) +*(.text._ZN4EKF216PublishGpsStatusERKy) +*(.text._ZN4uORB12SubscriptionaSEOS0_) +*(.text._ZN15TakeoffHandling18updateTakeoffStateEbbbfbRKy) +*(.text._ZN10ModeChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN14FlightTaskAuto24_updateInternalWaypointsEv) +*(.text._ZN8Failsafe17updateArmingStateERKybRK16failsafe_flags_s) +*(.text.imxrt_lpi2c_modifyreg) +*(.text.up_flush_dcache) +*(.text._ZN5PX4IO16io_handle_statusEt) +*(.text._ZN15GyroCalibration3RunEv) +*(.text.mavlink_start_uart_send) +*(.text.MEM_DataCopy2) +*(.text._ZNK9Commander14getPrearmStateEv) +*(.text._ZN15EstimatorChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN28FlightTaskManualAccelerationD1Ev) +*(.text._ZN11RateControl20getRateControlStatusER18rate_ctrl_status_s) +*(.text._ZN4uORB10DeviceNode15poll_notify_oneEP6pollfdm) +*(.text._ZN3GPS21handleInjectDataTopicEv.part.0) +*(.text._ZN9Commander17systemPowerUpdateEv) +*(.text._ZN4EKF221PublishGlobalPositionERKy) +*(.text._ZNK12FailsafeBase17getSelectedActionERKNS_5StateERK16failsafe_flags_sbbRNS_19SelectedActionStateE) +*(.text.imxrt_padctl_address) +*(.text._ZNK6matrix6VectorIfLj2EE4unitEv) +*(.text._ZN19RcCalibrationChecks14checkAndReportERK7ContextR6Report) +*(.text.devif_conn_callback_free) +*(.text._ZN13InnovationLpf6updateEfff.isra.0) +*(.text._ZN9Commander18landDetectorUpdateEv) +*(.text._ZN3Ekf18updateGroundEffectEv) +*(.text.nxsem_init) +*(.text._ZN9Commander16vtolStatusUpdateEv) +*(.text._ZN6matrix6MatrixIfLj4ELj1EEC1EPKf) +*(.text._ZN11ControlMath20setZeroIfNanVector3fERN6matrix7Vector3IfEE) +*(.text._ZThn8_N3ADC3RunEv) +*(.text._ZN11StickTiltXYC1EP12ModuleParams) +*(.text._ZN12SafetyButton3RunEv) +*(.text._ZN6BMP38811set_op_modeEh) +*(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_) +*(.text._ZN13AnalogBattery19get_current_channelEv) +*(.text._ZN15EstimatorChecks20checkEstimatorStatusERK7ContextR6ReportRK18estimator_status_s8NavModes) +*(.text._ZN12FailsafeBase11updateDelayERKy) +*(.text._ZN10FlightTask25_evaluateDistanceToGroundEv) +*(.text._ZN4EKF218PublishGnssHgtBiasERKy) +*(.text._ZN3Ekf21controlHaglFlowFusionEv) +*(.text._ZN6matrix6VectorIfLj3EE9normalizeEv) +*(.text._ZThn16_N7sensors10VehicleIMU3RunEv) +*(.text.__kernel_cos) +*(.text._ZN12SafetyButton19CheckPairingRequestEb) +*(.text.imxrt_dma_txavailable) +*(.text.perf_set_elapsed) +*(.text.pthread_sem_take) +*(.text._ZN8StickYawD1Ev) +*(.text._Z15blink_msg_statev) +*(.text._ZN19AccelerometerChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN8Failsafe14fromGfActParamEi) +*(.text._ZN3Ekf27checkAndFixCovarianceUpdateERKN6matrix12SquareMatrixIfLj23EEE) +*(.text._ZN3Ekf17controlBetaFusionERKN9estimator9imuSampleE) +*(.text._ZN36do_not_explicitly_use_this_namespace5ParamIfLN3px46paramsE919EEC1Ev) +*(.text._ZN22MavlinkStreamHeartbeat8get_sizeEv) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEdVEf) +*(.text._ZN17FlightTaskDescendC1Ev) +*(.text._ZN26MavlinkStreamCameraTrigger8get_sizeEv) +*(.text.iob_navail) +*(.text._ZN12FailsafeBase25removeNonActivatedActionsEv) +*(.text._ZN16PreFlightChecker28preFlightCheckHorizVelFailedERK23estimator_innovations_sf) +*(.text._ZN15TakeoffHandling10updateRampEff) +*(.text._Z7led_offi) +*(.text._ZN16PreFlightChecker22selectHeadingTestLimitEv) +*(.text.led_off) +*(.text.udp_wrbuffer_test) +*(.text._ZNK3Ekf34updateVerticalPositionAidSrcStatusERKyfffR24estimator_aid_source1d_s) +*(.text._ZNK4math17WelfordMeanVectorIfLj3EE8varianceEv) +*(.text._ZN27MavlinkStreamAttitudeTarget4sendEv) +*(.text._ZN12MixingOutput19updateSubscriptionsEb) +*(.text._ZN10FlightTaskD1Ev) +*(.text._ZThn24_N13land_detector12LandDetector3RunEv) +*(.text._ZN18MavlinkStreamDebug8get_sizeEv) +*(.text._ZN12GPSDriverUBX7receiveEj) +*(.text._ZN13BatteryStatus21parameter_update_pollEb) +*(.text._ZN3Ekf26checkYawAngleObservabilityEv) +*(.text._ZN3RTL18updateDatamanCacheEv) +*(.text.__ieee754_sqrtf) +*(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv) +*(.text.__kernel_sin) +*(.text._ZN11MissionBase17parameters_updateEv) +*(.text.nx_start) +*(.text._ZN3Ekf17controlDragFusionERKN9estimator9imuSampleE) +*(.text._ZNK8Failsafe22modifyUserIntendedModeEN12FailsafeBase6ActionES1_h) +*(.text._ZN3px417ScheduledWorkItem19schedule_trampolineEPv) +*(.text.uart_xmitchars_dma) +*(.text._ZN13land_detector23MulticopterLandDetector19_get_freefall_stateEv) +*(.text._ZThn24_N31MulticopterHoverThrustEstimator3RunEv) +*(.text._ZN11MissionBase11on_inactiveEv) +*(.text._ZN21FailureDetectorChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN12SystemChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN6matrix6MatrixIfLj3ELj1EEC1EPKf) +*(.text.imxrt_padmux_address) +*(.text._ZN3Ekf15setVelPosStatusEib) +*(.text._ZN19MavlinkStreamVFRHUD8get_sizeEv) +*(.text._ZN15EstimatorChecks15checkSensorBiasERK7ContextR6Report8NavModes) +*(.text._ZN20ImuConsistencyChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN17ObstacleAvoidanceD1Ev) +*(.text._ZN28MavlinkStreamGpsGlobalOrigin8get_sizeEv) +*(.text.MEM_DataCopy2_1) +*(.text._ZN6BMP3887measureEv) +*(.text._ZN4EKF217PublishRngHgtBiasERKy) +*(.text._ZN36MavlinkStreamPositionTargetGlobalInt8get_sizeEv) +*(.text._ZN28MavlinkStreamEstimatorStatus8get_sizeEv) +*(.text.up_clean_dcache) +*(.text._ZThn56_N26MulticopterPositionControl3RunEv) +*(.text._ZN16FlightTimeChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN13ManualControl12processInputEy) +*(.text._ZN17CpuResourceChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN10GyroChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN8Failsafe26fromImbalancedPropActParamEi) +*(.text._ZThn24_N13BatteryStatus3RunEv) +*(.text.mm_foreach) +*(.text._ZN35MavlinkStreamPositionTargetLocalNed8get_sizeEv) +*(.text._ZN32MavlinkStreamNavControllerOutput8get_sizeEv) +*(.text._ZN6matrix8wrap_2piIfEET_S1_) +*(.text._ZN4uORB7Manager30orb_remove_internal_subscriberEPv) +*(.text._ZN10BMP388_I2C7get_regEh) +*(.text._ZN4math17WelfordMeanVectorIfLj3EE5resetEv) +*(.text._ZN30MavlinkStreamUTMGlobalPosition10const_rateEv) +*(.text._ZN27MavlinkStreamScaledPressure8get_sizeEv) +*(.text._ZN3RTL17parameters_updateEv) +*(.text._ZN18EstimatorInterface11setBaroDataERKN9estimator10baroSampleE.part.0) +*(.text._ZN32MavlinkStreamOpenDroneIdLocation8get_sizeEv) +*(.text._ZN21MavlinkStreamTimesync4sendEv) +*(.text._ZN9Navigator23reset_position_setpointER19position_setpoint_s) +*(.text._ZN19RcAndDataLinkChecks14checkAndReportERK7ContextR6Report) +*(.text.imxrt_dma_txcallback) +*(.text._ZN24MavlinkParametersManager11send_uavcanEv) +*(.text._ZN4uORB10DeviceNode4readEP4filePcj) +*(.text._ZN4uORB10DeviceNode10poll_stateEP4file) +*(.text._ZN4uORB7Manager8orb_copyEPK12orb_metadataiPv) +*(.text._ZN27MavlinkStreamServoOutputRawILi0EE8get_sizeEv) +*(.text._ZN30MavlinkStreamUTMGlobalPosition8get_sizeEv) +*(.text._ZN8Geofence3runEv) +*(.text._ZN15EstimatorChecks25checkEstimatorStatusFlagsERK7ContextR6ReportRK18estimator_status_sRK24vehicle_local_position_s) +*(.text._ZN18MagnetometerChecks14checkAndReportERK7ContextR6Report) +*(.text._ZN6events9SendEvent3RunEv) +*(.text._ZN30MavlinkStreamGlobalPositionInt8get_sizeEv) +*(.text._ZN22MavlinkStreamESCStatus8get_sizeEv) +*(.text._Z20px4_spi_bus_externalRK13px4_spi_bus_t) +*(.text.read) +*(.text._ZN4uORB15PublicationBaseD1Ev) +*(.text._ZN22MavlinkStreamDebugVect8get_sizeEv) +*(.text._ZN22MavlinkStreamCollision8get_sizeEv) +*(.text._ZN7Mission11on_inactiveEv) +*(.text._ZN7sensors19VehicleMagnetometer20UpdateMagCalibrationEv) +*(.text._ZN11calibration27FindCurrentCalibrationIndexEPKcm) +*(.text._ZN4cdevL9cdev_readEP4filePcj) +*(.text.sem_timedwait) +*(.text.snprintf) +*(.text._ZN27MavlinkStreamOpticalFlowRad8get_sizeEv) +*(.text._ZNK6matrix6MatrixIfLj3ELj1EE6copyToEPf) +*(.text._ZN6Report13healthFailureIJhEEEv8NavModes20HealthComponentIndexmRKN6events9LogLevelsEPKcDpT_.isra.0.constprop.0) +*(.text._ZN13BatteryChecks16rtlEstimateCheckERK7ContextR6Reportf) +*(.text.sigemptyset) +*(.text.nx_read) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld new file mode 100644 index 000000000000..8b228a22f8b7 --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld @@ -0,0 +1,119 @@ +/* Static */ +*(.text.arm_ack_irq) +*(.text.arm_doirq) +*(.text.arm_svcall) +*(.text.arm_switchcontext) +*(.text.board_autoled_on) +*(.text.clock_timer) +*(.text.exception_common) +*(.text.flexio_irq_handler) +*(.text.hrt_absolute_time) +*(.text.hrt_call_enter) +*(.text.hrt_tim_isr) +*(.text.imxrt_configwaitints) +*(.text.imxrt_dma_callback) +*(.text.imxrt_dmach_interrupt) +*(.text.imxrt_dmach_xfrsetup) +*(.text.imxrt_dmaterminate) +*(.text.imxrt_dispatch) +*(.text.imxrt_edma_interrupt) +*(.text.imxrt_endwait) +*(.text.imxrt_enet_interrupt) +*(.text.imxrt_enet_interrupt_work) +*(.text.imxrt_gpio3_16_31_interrupt) +*(.text.imxrt_interrupt) +*(.text.imxrt_lpi2c_isr) +*(.text.imxrt_lpspi_exchange) +*(.text.imxrt_recvdma) +*(.text.imxrt_tcd_free) +*(.text.imxrt_timerisr) +*(.text.imxrt_transmit) +*(.text.imxrt_txdone) +*(.text.imxrt_txtimeout_work) +*(.text.imxrt_txtimeout_expiry) +*(.text.imxrt_txpoll) +*(.text.imxrt_txringfull) +*(.text.imxrt_txavail_work) +*(.text.imxrt_txavail) +*(.text.imxrt_usbinterrupt) +*(.text.irq_dispatch) +*(.text.ioctl) +*(.text.memcpy) +*(.text.memset) +*(.text.nxsched_add_blocked) +*(.text.nxsched_add_prioritized) +*(.text.nxsched_add_readytorun) +*(.text.nxsched_get_files) +*(.text.nxsched_get_tcb) +*(.text.nxsched_merge_pending) +*(.text.nxsched_process_timer) +*(.text.nxsched_remove_blocked) +*(.text.nxsched_remove_readytorun) +*(.text.nxsched_resume_scheduler) +*(.text.nxsched_suspend_scheduler) +*(.text.nxsem_add_holder) +*(.text.nxsem_add_holder_tcb) +*(.text.nxsem_clockwait) +*(.text.nxsem_foreachholder) +*(.text.nxsem_freecount0holder) +*(.text.nxsem_freeholder) +*(.text.nxsem_post) +*(.text.nxsem_release_holder) +*(.text.nxsem_restore_baseprio) +*(.text.nxsem_tickwait) +*(.text.nxsem_timeout) +*(.text.nxsem_trywait) +*(.text.nxsem_wait) +*(.text.nxsem_wait_uninterruptible) +*(.text.nxsig_timedwait) +*(.text.sched_lock) +*(.text.sched_note_resume) +*(.text.sched_note_suspend) +*(.text.sched_unlock) +*(.text.strcmp) +*(.text.sq_addafter) +*(.text.sq_addlast) +*(.text.sq_rem) +*(.text.sq_remafter) +*(.text.sq_remfirst) +*(.text.uart_connected) +*(.text.up_block_task) +*(.text.up_unblock_task) +*(.text.wd_timer) +*(.text.wd_start) +*(.text.work_thread) +*(.text.work_queue) +*(.text._do_memcpy) + +/* Tropic Eth tune */ +*(.text.devif_poll) +*(.text.devif_poll_tcp_connections) +*(.text.tcp_poll) +*(.text.devif_poll_udp_connections) +*(.text.udp_nextconn) +*(.text.udp_poll) +*(.text.udp_ipv4_select) +*(.text.udp_callback) +*(.text.udp_datahandler) +*(.text.udp_send) +*(.text.udp_active) +*(.text.udp_ipv4_active) +*(.text.psock_udp_sendto) +*(.text.sendto_eventhandler) +*(.text.net_dataevent) +*(.text.devif_conn_event) +*(.text.devif_event_trigger) +*(.text.devif_poll_icmp) +*(.text.icmp_poll) +*(.text.devif_packet_conversion) +*(.text.arp_out) +*(.text.arp_find) +*(.text.arp_format) +*(.text.net_ipv4addr_hdrcmp) +*(.text.net_ipv4addr_copy) +*(.text.net_ipv4addr_broadcast) +*(.text.wd_start) +*(.text.arp_arpin) +*(.text.ipv4_input) +*(.text.work_thread) +*(.text.work_queue) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/script.ld b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..a715059912ce --- /dev/null +++ b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld @@ -0,0 +1,179 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/scripts/flash.ld + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/* Specify the memory areas */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x60000000, LENGTH = 7936K + sram (rwx) : ORIGIN = 0x20200000, LENGTH = 512K + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 384K + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(g_flash_config) +EXTERN(g_image_vector_table) +EXTERN(g_boot_data) + +ENTRY(_stext) + +SECTIONS +{ + /* Image Vector Table and Boot Data for booting from external flash */ + + .boot_hdr : ALIGN(4) + { + FILL(0xff) + __boot_hdr_start__ = ABSOLUTE(.) ; + KEEP(*(.boot_hdr.conf)) + . = 0x1000 ; + KEEP(*(.boot_hdr.ivt)) + . = 0x1020 ; + KEEP(*(.boot_hdr.boot_data)) + . = 0x1030 ; + KEEP(*(.boot_hdr.dcd_data)) + __boot_hdr_end__ = ABSOLUTE(.) ; + . = 0x2000 ; + } >flash + + .vectors : + { + KEEP(*(.vectors)) + *(.text .text.__start) + } >flash + + .itcmfunc : + { + . = ALIGN(8); + _sitcmfuncs = ABSOLUTE(.); + FILL(0xFF) + . = 0x40 ; + INCLUDE "itcm_static_functions.ld" + INCLUDE "itcm_functions_includes.ld" + . = ALIGN(8); + _eitcmfuncs = ABSOLUTE(.); + } > itcm AT > flash + + _fitcmfuncs = LOADADDR(.itcmfunc); + + /* The RAM vector table (if present) should lie at the beginning of SRAM */ + + .ram_vectors (COPY) : { + *(.ram_vectors) + } > dtcm + + + /* Workaround for ethernet issue, by placing g_desc_pool into DTCM, + which effectively puts it into a no-cache region */ + .dtcm : { + *(.bss.g_desc_pool) + } > dtcm + + .text : + { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + . = ALIGN(4096); + _etext = ABSOLUTE(.); + _srodata = ABSOLUTE(.); + *(.rodata .rodata.*) + . = ALIGN(4096); + _erodata = ABSOLUTE(.); + } > flash + + .init_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(SORT_BY_INIT_PRIORITY(.init_array.*) SORT_BY_INIT_PRIORITY(.ctors.*))) + KEEP(*(.init_array .ctors)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } > flash + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } > flash + + _eronly = ABSOLUTE(.); + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > flash + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + .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) } + _sdtcm = ORIGIN(dtcm); + _edtcm = ORIGIN(dtcm) + LENGTH(dtcm); +} diff --git a/boards/nxp/tropic-community/src/CMakeLists.txt b/boards/nxp/tropic-community/src/CMakeLists.txt new file mode 100644 index 000000000000..586c60841e14 --- /dev/null +++ b/boards/nxp/tropic-community/src/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_library(drivers_board + i2c.cpp + init.c + sdhc.c + spi.cpp + timer_config.cpp + tropic_led_pwm.cpp + usb.c + imxrt_flexspi_nor_boot.c + imxrt_flexspi_nor_flash.c +) + + +target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer +) diff --git a/boards/nxp/tropic-community/src/board_config.h b/boards/nxp/tropic-community/src/board_config.h new file mode 100644 index 000000000000..cef895ab6cd3 --- /dev/null +++ b/boards/nxp/tropic-community/src/board_config.h @@ -0,0 +1,331 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Tropic Community internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include +#include + +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" +#include "hardware/imxrt_usb_analog.h" + +#include +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Configuration ************************************************************************************/ + +/* FMURT1062 GPIOs ***********************************************************************************/ +/* LEDs */ +/* An RGB LED is connected through GPIO as shown below: + */ +#define LED_IOMUX (IOMUX_OPENDRAIN | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW) +#define GPIO_nLED_RED /* GPIO_AD_B0_02 GPIO1_IO02 */ (GPIO_PORT1 | GPIO_PIN2 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) +#define GPIO_nLED_GREEN /* GPIO_B1_03 GPIO2_IO19 */ (GPIO_PORT2 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) +#define GPIO_nLED_BLUE /* GPIO_AD_B0_03 GPIO1_IO03 */ (GPIO_PORT1 | GPIO_PIN3 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | LED_IOMUX) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* + * Define the ability to shut off off the sensor signals + * by changing the signals to inputs + */ + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN_100K | IOMUX_CMOS_INPUT)) + +/* Define the Chip Selects, Data Ready and Control signals per SPI bus */ + +#define CS_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_33OHM | IOMUX_SPEED_LOW | IOMUX_SLEW_FAST) +#define OUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_UP_100K | IOMUX_DRIVE_50OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + + +/* SPI1 off */ + +#define _GPIO_LPSPI1_SCK /* GPIO_EMC_27 GPIO4_IO27 */ (GPIO_PORT4 | GPIO_PIN27 | CS_IOMUX) +#define _GPIO_LPSPI1_MISO /* GPIO_EMC_29 GPIO4_IO29 */ (GPIO_PORT4 | GPIO_PIN29 | CS_IOMUX) +#define _GPIO_LPSPI1_MOSI /* GPIO_EMC_28 GPIO4_IO28 */ (GPIO_PORT4 | GPIO_PIN28 | CS_IOMUX) + +#define GPIO_SPI1_SCK_OFF _PIN_OFF(_GPIO_LPSPI1_SCK) +#define GPIO_SPI1_MISO_OFF _PIN_OFF(_GPIO_LPSPI1_MISO) +#define GPIO_SPI1_MOSI_OFF _PIN_OFF(_GPIO_LPSPI1_MOSI) + +#define _GPIO_LPSPI3_SCK /* GPIO_AD_B1_15 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN31 | CS_IOMUX) +#define _GPIO_LPSPI3_MISO /* GPIO_AD_B1_13 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN29 | CS_IOMUX) +#define _GPIO_LPSPI3_MOSI /* GPIO_AD_B1_14 GPIO1_IO27 */ (GPIO_PORT1 | GPIO_PIN30 | CS_IOMUX) + +#define GPIO_SPI3_SCK_OFF _PIN_OFF(_GPIO_LPSPI3_SCK) +#define GPIO_SPI3_MISO_OFF _PIN_OFF(_GPIO_LPSPI3_MISO) +#define GPIO_SPI3_MOSI_OFF _PIN_OFF(_GPIO_LPSPI3_MOSI) + +/* Define the SPI4 Data Ready and Control signals */ + +#define GPIO_SPI4_DRDY7_EXTERNAL1 /* GPIO_EMC_35 GPIO3_IO21*/ (GPIO_PORT3 | GPIO_PIN21 | GPIO_INPUT | DRDY_IOMUX) + +#define GPIO_DRDY_OFF_SPI4_DRDY7_EXTERNAL1 _PIN_OFF(GPIO_SPI4_DRDY7_EXTERNAL1) + + +#define ADC_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) + +#define ADC1_CH(n) (n) +#define ADC1_GPIO(n, p) (GPIO_PORT1 | GPIO_PIN##p | ADC_IOMUX) // + +/* Define GPIO pins used as ADC N.B. Channel numbers are for reference, */ + +#define PX4_ADC_GPIO \ + /* BATTERY1_VOLTAGE GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_GPIO(0, 27), \ + /* BATTERY1_CURRENT GPIO_AD_B1_11 GPIO1 Pin 24 */ ADC1_GPIO(0, 24) + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_BATTERY_VOLTAGE_CHANNEL /* GPIO_AD_B1_08 GPIO1 Pin 27 */ ADC1_CH(13) +#define ADC_BATTERY_CURRENT_CHANNEL /* GPIO_AD_B1_11 GPIO1 Pin 12 */ ADC1_CH(0) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_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) + +/* Power supply control and monitoring GPIOs */ + +#define GENERAL_INPUT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_47K | IOMUX_DRIVE_HIZ) +#define GENERAL_OUTPUT_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_KEEP | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_FAST) + + +/* PWM + */ + +#define DIRECT_PWM_OUTPUT_CHANNELS 6 +#define BOARD_NUM_IO_TIMERS 4 + +// Input Capture not supported on MVP + +#define BOARD_HAS_NO_CAPTURE + +//#define BOARD_HAS_UI_LED_PWM 1 Not ported yet (Still Kinetis driver) +#define BOARD_HAS_LED_PWM 1 +#define BOARD_LED_PWM_DRIVE_ACTIVE_LOW 1 +#define BOARD_HAS_CUSTOM_LED_PWM 1 + +#define PWM_LED_RED /* GPIO_AD_B0_02 */ (GPIO_XBAR1_INOUT16_1 | GENERAL_OUTPUT_IOMUX) +#define PWM_LED_GREEN /* GPIO_B1_03 */ (GPIO_FLEXPWM2_PWMB03_4 | GENERAL_OUTPUT_IOMUX) +#define PWM_LED_BLUE /* GPIO_AD_B0_03 */ (GPIO_XBAR1_INOUT17_1 | GENERAL_OUTPUT_IOMUX) + + + +/* Tone alarm output */ + +//FIXME FlexPWM TONE DRIVER +#define TONE_ALARM_FLEXPWM PWMA_VAL +#define TONE_ALARM_TIMER 3 /* FlexPWM 3 */ +#define TONE_ALARM_CHANNEL 1 /* GPIO_EMC_31 PWM3_PWMA01 */ + +#define GPIO_BUZZER_1 /* GPIO_EMC_31 GPIO4_IO31 */ (GPIO_PORT1 | GPIO_PIN23 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO | GENERAL_OUTPUT_IOMUX) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM (GPIO_FLEXPWM3_PWMA01_1 | GENERAL_OUTPUT_IOMUX) + +/* USB OTG FS + * + * VBUS_VALID is detected in USB_ANALOG_USB1_VBUS_DETECT_STAT + */ + +/* High-resolution timer */ +#define HRT_TIMER 1 /* use GPT1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define RC_SERIAL_PORT "/dev/ttyS4" +#define RC_SERIAL_SINGLEWIRE 1 // Suport Single wire wiring +#define RC_SERIAL_SWAP_RXTX 1 // Set Swap (but not supported in HW) to use Single wire +#define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* Safety Switch is HW version dependent on having an PX4IO + * So we init to a benign state with the _INIT definition + * and provide the the non _INIT one for the driver to make a run time + * decision to use it. + */ +#define SAFETY_INIT_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_HIZ) +#define SAFETY_IOMUX (IOMUX_CMOS_OUTPUT | IOMUX_PULL_NONE | IOMUX_DRIVE_33OHM | IOMUX_SPEED_MEDIUM | IOMUX_SLEW_SLOW) +#define SAFETY_SW_IOMUX (IOMUX_CMOS_INPUT | IOMUX_PULL_UP_22K | IOMUX_DRIVE_HIZ) + +#define GPIO_SAFETY_SWITCH_IN /* GPIO_B0_12 GPIO2_IO12 */ (GPIO_PORT2 | GPIO_PIN12 | GPIO_INPUT | SAFETY_SW_IOMUX) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ + +#define BOARD_NUMBER_BRICKS 1 + +/* Use USB2 VBUS 4.7V comparator for valid check */ +static inline int board_read_usb2_vbus_state(void) +{ + return (getreg32(IMXRT_USB_ANALOG_USB2_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_VALID) ? 0 : 1; +} + +#define BOARD_ADC_BRICK_VALID (board_read_usb2_vbus_state() == 0) + + + + +#define BOARD_ADC_USB_CONNECTED (board_read_VBUS_state() == 0) + +/* FMUv5 never powers odd the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +// FIXME LETS NOT RESET FOR NOW +//#define BOARD_HAS_ON_RESET 1 +#define BOARD_HAS_TEENSY_BOOTLOADER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_FLEXCAN3_TX, \ + GPIO_FLEXCAN3_RX, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: fmurt1062_usdhc_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int fmurt1062_usdhc_initialize(void); + +/**************************************************************************************************** + * Name: imxrt_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void imxrt_spidev_initialize(void); + +/************************************************************************************ + * Name: imxrt_spi_bus_initialize + * + * Description: + * Called to configure SPI Buses. + * + ************************************************************************************/ + +extern int imxrt1062_spi_bus_initialize(void); + +/************************************************************************************ + * Name: imxrt_usb_initialize + * + * Description: + * Called to configure USB. + * + ************************************************************************************/ + +extern int imxrt_usb_initialize(void); + +extern void imxrt_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +extern void fmurt1062_timer_initialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/nxp/tropic-community/src/i2c.cpp b/boards/nxp/tropic-community/src/i2c.cpp new file mode 100644 index 000000000000..6bab2dee4a01 --- /dev/null +++ b/boards/nxp/tropic-community/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(4), +}; diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.c b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.c new file mode 100644 index 000000000000..8b8384545e3b --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_boot.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "imxrt_flexspi_nor_boot.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +locate_data(".boot_hdr.ivt") +const struct ivt_s g_image_vector_table = { + IVT_HEADER, /* IVT Header */ + IMAGE_ENTRY_ADDRESS, /* Image Entry Function */ + IVT_RSVD, /* Reserved = 0 */ + (uint32_t) DCD_ADDRESS, /* Address where DCD information is + * stored */ + (uint32_t) BOOT_DATA_ADDRESS, /* Address where BOOT Data Structure + * is stored */ + (uint32_t) IMAG_VECTOR_TABLE, /* Pointer to IVT Self (absolute + * address */ + (uint32_t) CSF_ADDRESS, /* Address where CSF file is stored */ + IVT_RSVD /* Reserved = 0 */ +}; + +locate_data(".boot_hdr.boot_data") +const struct boot_data_s g_boot_data = { + IMAGE_DEST, /* boot start location */ + (IMAGE_DEST_END - IMAGE_DEST), /* size */ + PLUGIN_FLAG, /* Plugin flag */ + 0xffffffff /* empty - extra data word */ +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ \ No newline at end of file diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.h b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.h new file mode 100644 index 000000000000..75e6d9c463c1 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_boot.h @@ -0,0 +1,155 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_boot.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_BOOT_H +#define __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_BOOT_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* IVT Data */ + +#define IVT_MAJOR_VERSION 0x4 +#define IVT_MAJOR_VERSION_SHIFT 0x4 +#define IVT_MAJOR_VERSION_MASK 0xf +#define IVT_MINOR_VERSION 0x1 +#define IVT_MINOR_VERSION_SHIFT 0x0 +#define IVT_MINOR_VERSION_MASK 0xf + +#define IVT_VERSION(major, minor) \ + ((((major) & IVT_MAJOR_VERSION_MASK) << IVT_MAJOR_VERSION_SHIFT) | \ + (((minor) & IVT_MINOR_VERSION_MASK) << IVT_MINOR_VERSION_SHIFT)) + +#define IVT_TAG_HEADER (0xd1) /* Image Vector Table */ +#define IVT_SIZE 0x2000 +#define IVT_PAR IVT_VERSION(IVT_MAJOR_VERSION, IVT_MINOR_VERSION) + +#define IVT_HEADER (IVT_TAG_HEADER | (IVT_SIZE << 8) | (IVT_PAR << 24)) +#define IVT_RSVD (uint32_t)(0x00000000) + +/* DCD Data */ + +#define DCD_TAG_HEADER (0xd2) +#define DCD_TAG_HEADER_SHIFT (24) +#define DCD_VERSION (0x41) +#define DCD_ARRAY_SIZE 1 + +#define FLASH_BASE 0x60000000 +#define FLASH_END 0x7f7fffff + +/* This needs to take into account the memory configuration at + * boot bootloader + */ + +#define ROM_BOOTLOADER_OCRAM_RES 0x8000 +#define OCRAM_BASE (0x20200000 + ROM_BOOTLOADER_OCRAM_RES) +#define OCRAM_END (OCRAM_BASE + (512 * 1024) + (256 * 1024) \ + - ROM_BOOTLOADER_OCRAM_RES) + +#define SCLK 1 +#if defined(CONFIG_BOOT_RUNFROMFLASH) +# define IMAGE_DEST FLASH_BASE +# define IMAGE_DEST_END FLASH_END +# define IMAGE_DEST_OFFSET 0 +#else +# define IMAGE_DEST OCRAM_BASE +# define IMAGE_DEST_END OCRAM_END +# define IMAGE_DEST_OFFSET IVT_SIZE +#endif + +#define LOCATE_IN_DEST(x) (((uint32_t)(x)) - FLASH_BASE + IMAGE_DEST) +#define LOCATE_IN_SRC(x) (((uint32_t)(x)) - IMAGE_DEST + FLASH_BASE) + +#define DCD_ADDRESS 0 +#define BOOT_DATA_ADDRESS LOCATE_IN_DEST(&g_boot_data) +#define CSF_ADDRESS 0 +#define PLUGIN_FLAG (uint32_t)0 + +/* Located in Destination Memory */ + +#define IMAGE_ENTRY_ADDRESS ((uint32_t)&_vectors) +#define IMAG_VECTOR_TABLE LOCATE_IN_DEST(&g_image_vector_table) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* IVT Data */ + +struct ivt_s { + /* Header with tag #HAB_TAG_IVT, length and HAB version fields */ + + uint32_t hdr; + + /* Absolute address of the first instr. to execute from the image */ + + uint32_t entry; + + /* Reserved in this version of HAB: should be NULL. */ + + uint32_t reserved1; + + /* Absolute address of the image DCD: may be NULL. */ + + uint32_t dcd; + + /* Absolute address of the Boot Data: may be NULL */ + + uint32_t boot_data; + + /* Absolute address of the IVT. */ + + uint32_t self; + + /* Absolute address of the image CSF. */ + + uint32_t csf; + + /* Reserved in this version of HAB: should be zero. */ + + uint32_t reserved2; +}; + +/* Boot Data */ + +struct boot_data_s { + uint32_t start; /* boot start location */ + uint32_t size; /* size */ + uint32_t plugin; /* plugin flag - 1 if downloaded application is + * plugin */ + uint32_t placeholder; /* placeholder to make even 0x10 size */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +extern const struct boot_data_s g_boot_data; +extern const uint8_t g_dcd_data[]; +extern const uint32_t _vectors[]; + +#endif /* __BOARDS_ARM_IMXRT_TEENSY_4_SRC_IMXRT_FLEXSPI_NOR_BOOT_H */ diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.c b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.c new file mode 100644 index 000000000000..f7ffe4e46db7 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.c @@ -0,0 +1,91 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_flash.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "imxrt_flexspi_nor_flash.h" + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +locate_data(".boot_hdr.conf") +const struct flexspi_nor_config_s g_flash_config = { + .mem_config = + { + .tag = FLEXSPI_CFG_BLK_TAG, + .version = FLEXSPI_CFG_BLK_VERSION, + .read_sample_clksrc = FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD, + .cs_hold_time = 1u, + .cs_setup_time = 1u, + .column_address_width = 0u, + .device_type = FLEXSPI_DEVICE_TYPE_SERIAL_NOR, + .sflash_pad_type = SERIAL_FLASH_4PADS, + .serial_clk_freq = FLEXSPI_SERIAL_CLKFREQ_133MHz, + .sflash_a1size = 8u * 1024u * 1024u, + .data_valid_time = + { + 0u, 0u + }, + .lookup_table = + { + /* Fast Read Quad I/O */ + [0 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xeb, + RADDR_SDR, FLEXSPI_4PAD, 0x18), + [0 + 1] = FLEXSPI_LUT_SEQ(DUMMY_SDR, FLEXSPI_4PAD, 0x06, + READ_SDR, FLEXSPI_4PAD, 0x04), + + /* Read Status Register-1 */ + [4 * 1 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x05, + READ_SDR, FLEXSPI_1PAD, 0x04), + + /* Write Status Register-1 */ + [4 * 3 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x01, STOP, FLEXSPI_1PAD, 0x0), + + /* Sector Erase (4KB) */ + [4 * 5 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x20, + RADDR_SDR, FLEXSPI_1PAD, 0x18), + + /* Block Erase (64KB) */ + [4 * 8 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0xd8, + RADDR_SDR, FLEXSPI_1PAD, 0x18), + + /* Page Program */ + [4 * 9 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x02, + RADDR_SDR, FLEXSPI_1PAD, 0x18), + [4 * 9 + 1] = FLEXSPI_LUT_SEQ(WRITE_SDR, FLEXSPI_1PAD, 0x04, + STOP, FLEXSPI_1PAD, 0x0), + + /* Chip Erase */ + [4 * 11 + 0] = FLEXSPI_LUT_SEQ(CMD_SDR, FLEXSPI_1PAD, 0x60, STOP, FLEXSPI_1PAD, 0x0), + }, + }, + + .page_size = 256u, + .sector_size = 4u * 1024u, + .blocksize = 64u * 1024u, + .is_uniform_blocksize = false, +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.h b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.h new file mode 100644 index 000000000000..11904fae65d0 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_nor_flash.h @@ -0,0 +1,354 @@ +/**************************************************************************** + * boards/arm/imxrt/teensy-4.x/src/imxrt_flexspi_nor_flash.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ +#ifndef __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_FLASH_H +#define __BOARDS_ARM_IMXRT_TEENSY_4X_SRC_IMXRT_FLEXSPI_NOR_FLASH_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* FLEXSPI memory config block related definitions */ + +#define FLEXSPI_CFG_BLK_TAG (0x42464346ul) +#define FLEXSPI_CFG_BLK_VERSION (0x56010100ul) +#define FLEXSPI_CFG_BLK_SIZE (512) + +/* FLEXSPI Feature related definitions */ + +#define FLEXSPI_FEATURE_HAS_PARALLEL_MODE 1 + +/* Lookup table related definitions */ + +#define CMD_INDEX_READ 0 +#define CMD_INDEX_READSTATUS 1 +#define CMD_INDEX_WRITEENABLE 2 +#define CMD_INDEX_WRITE 4 + +#define CMD_LUT_SEQ_IDX_READ 0 +#define CMD_LUT_SEQ_IDX_READSTATUS 1 +#define CMD_LUT_SEQ_IDX_WRITEENABLE 3 +#define CMD_LUT_SEQ_IDX_WRITE 9 + +#define CMD_SDR 0x01 +#define CMD_DDR 0x21 +#define RADDR_SDR 0x02 +#define RADDR_DDR 0x22 +#define CADDR_SDR 0x03 +#define CADDR_DDR 0x23 +#define MODE1_SDR 0x04 +#define MODE1_DDR 0x24 +#define MODE2_SDR 0x05 +#define MODE2_DDR 0x25 +#define MODE4_SDR 0x06 +#define MODE4_DDR 0x26 +#define MODE8_SDR 0x07 +#define MODE8_DDR 0x27 +#define WRITE_SDR 0x08 +#define WRITE_DDR 0x28 +#define READ_SDR 0x09 +#define READ_DDR 0x29 +#define LEARN_SDR 0x0a +#define LEARN_DDR 0x2a +#define DATSZ_SDR 0x0b +#define DATSZ_DDR 0x2b +#define DUMMY_SDR 0x0c +#define DUMMY_DDR 0x2c +#define DUMMY_RWDS_SDR 0x0d +#define DUMMY_RWDS_DDR 0x2d +#define JMP_ON_CS 0x1f +#define STOP 0 + +#define FLEXSPI_1PAD 0 +#define FLEXSPI_2PAD 1 +#define FLEXSPI_4PAD 2 +#define FLEXSPI_8PAD 3 + +#define FLEXSPI_LUT_OPERAND0_MASK (0xffu) +#define FLEXSPI_LUT_OPERAND0_SHIFT (0U) +#define FLEXSPI_LUT_OPERAND0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPERAND0_SHIFT)) & \ + FLEXSPI_LUT_OPERAND0_MASK) +#define FLEXSPI_LUT_NUM_PADS0_MASK (0x300u) +#define FLEXSPI_LUT_NUM_PADS0_SHIFT (8u) +#define FLEXSPI_LUT_NUM_PADS0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS0_SHIFT)) & \ + FLEXSPI_LUT_NUM_PADS0_MASK) +#define FLEXSPI_LUT_OPCODE0_MASK (0xfc00u) +#define FLEXSPI_LUT_OPCODE0_SHIFT (10u) +#define FLEXSPI_LUT_OPCODE0(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPCODE0_SHIFT)) & \ + FLEXSPI_LUT_OPCODE0_MASK) +#define FLEXSPI_LUT_OPERAND1_MASK (0xff0000u) +#define FLEXSPI_LUT_OPERAND1_SHIFT (16U) +#define FLEXSPI_LUT_OPERAND1(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_OPERAND1_SHIFT)) & \ + FLEXSPI_LUT_OPERAND1_MASK) +#define FLEXSPI_LUT_NUM_PADS1_MASK (0x3000000u) +#define FLEXSPI_LUT_NUM_PADS1_SHIFT (24u) +#define FLEXSPI_LUT_NUM_PADS1(x) (((uint32_t) \ + (((uint32_t)(x)) << FLEXSPI_LUT_NUM_PADS1_SHIFT)) & \ + FLEXSPI_LUT_NUM_PADS1_MASK) +#define FLEXSPI_LUT_OPCODE1_MASK (0xfc000000u) +#define FLEXSPI_LUT_OPCODE1_SHIFT (26u) +#define FLEXSPI_LUT_OPCODE1(x) (((uint32_t)(((uint32_t)(x)) << FLEXSPI_LUT_OPCODE1_SHIFT)) & \ + FLEXSPI_LUT_OPCODE1_MASK) + +#define FLEXSPI_LUT_SEQ(cmd0, pad0, op0, cmd1, pad1, op1) \ + (FLEXSPI_LUT_OPERAND0(op0) | FLEXSPI_LUT_NUM_PADS0(pad0) | \ + FLEXSPI_LUT_OPCODE0(cmd0) | FLEXSPI_LUT_OPERAND1(op1) | \ + FLEXSPI_LUT_NUM_PADS1(pad1) | FLEXSPI_LUT_OPCODE1(cmd1)) + +/* */ + +#define NOR_CMD_INDEX_READ CMD_INDEX_READ +#define NOR_CMD_INDEX_READSTATUS CMD_INDEX_READSTATUS +#define NOR_CMD_INDEX_WRITEENABLE CMD_INDEX_WRITEENABLE +#define NOR_CMD_INDEX_ERASESECTOR 3 +#define NOR_CMD_INDEX_PAGEPROGRAM CMD_INDEX_WRITE +#define NOR_CMD_INDEX_CHIPERASE 5 +#define NOR_CMD_INDEX_DUMMY 6 +#define NOR_CMD_INDEX_ERASEBLOCK 7 + +/* READ LUT sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READ CMD_LUT_SEQ_IDX_READ + +/* Read Status LUT sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS CMD_LUT_SEQ_IDX_READSTATUS + +/* 2 Read status DPI/QPI/OPI sequence id in LUT stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READSTATUS_XPI 2 + +/* 3 Write Enable sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE CMD_LUT_SEQ_IDX_WRITEENABLE + +/* 4 Write Enable DPI/QPI/OPI sequence id in LUT stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_WRITEENABLE_XPI 4 + +/* 5 Erase Sector sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_ERASESECTOR 5 + +/* 8 Erase Block sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_ERASEBLOCK 8 + +/* 9 Program sequence id in lookupTable stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_PAGEPROGRAM CMD_LUT_SEQ_IDX_WRITE + +/* 11 Chip Erase sequence in lookupTable id stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_CHIPERASE 11 + +/* 13 Read SFDP sequence in lookupTable id stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_READ_SFDP 13 + +/* 14 Restore 0-4-4/0-8-8 mode sequence id in LUT stored in config block */ + +#define NOR_CMD_LUT_SEQ_IDX_RESTORE_NOCMD 14 + +/* 15 Exit 0-4-4/0-8-8 mode sequence id in LUT stored in config blobk */ + +#define NOR_CMD_LUT_SEQ_IDX_EXIT_NOCMD 15 + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/* Definitions for FlexSPI Serial Clock Frequency */ + +enum flexspi_serial_clkfreq_e { + FLEXSPI_SERIAL_CLKFREQ_30MHz = 1, + FLEXSPI_SERIAL_CLKFREQ_50MHz = 2, + FLEXSPI_SERIAL_CLKFREQ_60MHz = 3, + FLEXSPI_SERIAL_CLKFREQ_75MHz = 4, + FLEXSPI_SERIAL_CLKFREQ_80MHz = 5, + FLEXSPI_SERIAL_CLKFREQ_100MHz = 6, + FLEXSPI_SERIAL_CLKFREQ_120MHz = 7, + FLEXSPI_SERIAL_CLKFREQ_133MHz = 8, + FLEXSPI_SERIAL_CLKFREQ_166MHz = 9, +}; + +/* FlexSPI clock configuration type */ + +enum flexspi_serial_clockmode_e { + FLEXSPI_CLKMODE_SDR, + FLEXSPI_CLKMODE_DDR, +}; + +/* FlexSPI Read Sample Clock Source definition */ + +enum flash_read_sample_clk_e { + FLASH_READ_SAMPLE_CLK_LOOPBACK_INTERNELLY = 0, + FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_DQSPAD = 1, + FLASH_READ_SAMPLE_CLK_LOOPBACK_FROM_SCKPAD = 2, + FLASH_READ_SAMPLE_CLK_EXTERNALINPUT_FROM_DQSPAD = 3, +}; + +/* Misc feature bit definitions */ + +enum flash_misc_feature_e { + FLEXSPIMISC_OFFSET_DIFFCLKEN = 0, /* Bit for Differential clock enable */ + FLEXSPIMISC_OFFSET_CK2EN = 1, /* Bit for CK2 enable */ + FLEXSPIMISC_OFFSET_PARALLELEN = 2, /* Bit for Parallel mode enable */ + + FLEXSPIMISC_OFFSET_WORD_ADDRESSABLE_EN = 3, /* Bit for Word Addressable + * enable */ + FLEXSPIMISC_OFFSET_SAFECONFIG_FREQ_EN = 4, /* Bit for Safe Configuration + * Frequency enable */ + + FLEXSPIMISC_OFFSET_PAD_SETTING_OVERRIDE_EN = 5, /* Bit for Pad setting + * override enable */ + + FLEXSPIMISC_OFFSET_DDR_MODE_EN = 6, /* Bit for DDR clock confiuration + * indication. */ +}; + +/* Flash Type Definition */ + +enum flash_flash_type_e { + FLEXSPI_DEVICE_TYPE_SERIAL_NOR = 1, /* Flash devices are Serial NOR */ + FLEXSPI_DEVICE_TYPE_SERIAL_NAND = 2, /* Flash devices are Serial + * NAND */ + FLEXSPI_DEVICE_TYPE_SERIAL_RAM = 3, /* Flash devices are Serial + * RAM/HyperFLASH */ + + FLEXSPI_DEVICE_TYPE_MCP_NOR_NAND = 0x12, /* Flash device is MCP device, + * A1 is Serial NOR, A2 is + * Serial NAND */ + FLEXSPI_DEVICE_TYPE_MCP_NOR_RAM = 0x13, /* Flash device is MCP device, + * A1 is Serial NOR, A2 is + * Serial RAMs */ +}; + +/* Flash Pad Definitions */ + +enum flash_flash_pad_e { + SERIAL_FLASH_1PAD = 1, + SERIAL_FLASH_2PADS = 2, + SERIAL_FLASH_4PADS = 4, + SERIAL_FLASH_8PADS = 8, +}; + +/* Flash Configuration Command Type */ + +enum flash_config_cmd_e { + DEVICE_CONFIG_CMD_TYPE_GENERIC, /* Generic command, for example: + * configure dummy cycles, drive + * strength, etc */ + DEVICE_CONFIG_CMD_TYPE_QUADENABLE, /* Quad Enable command */ + DEVICE_CONFIG_CMD_TYPE_SPI2XPI, /* Switch from SPI to DPI/QPI/OPI mode */ + DEVICE_CONFIG_CMD_TYPE_XPI2SPI, /* Switch from DPI/QPI/OPI to SPI mode */ + DEVICE_CONFIG_CMD_TYPE_SPI2NO_CMD, /* Switch to 0-4-4/0-8-8 mode */ + DEVICE_CONFIG_CMD_TYPE_RESET, /* Reset device command */ +}; + +/* FlexSPI LUT Sequence structure */ + +struct flexspi_lut_seq_s { + uint8_t seq_num; /* Sequence Number, valid number: 1-16 */ + uint8_t seq_id; /* Sequence Index, valid number: 0-15 */ + uint16_t reserved; +}; + +/* FlexSPI Memory Configuration Block */ + +struct flexspi_mem_config_s { + uint32_t tag; + uint32_t version; + uint32_t reserved0; + uint8_t read_sample_clksrc; + uint8_t cs_hold_time; + uint8_t cs_setup_time; + uint8_t column_address_width; /* [0x00f-0x00f] Column Address with, + * for HyperBus protocol, it is fixed + * to 3, For Serial NAND, need to refer + * to datasheet */ + uint8_t device_mode_cfg_enable; + uint8_t device_mode_type; + uint16_t wait_time_cfg_commands; + struct flexspi_lut_seq_s device_mode_seq; + uint32_t device_mode_arg; + uint8_t config_cmd_enable; + uint8_t config_mode_type[3]; + struct flexspi_lut_seq_s config_cmd_seqs[3]; + uint32_t reserved1; + uint32_t config_cmd_args[3]; + uint32_t reserved2; + uint32_t controller_misc_option; + uint8_t device_type; + uint8_t sflash_pad_type; + uint8_t serial_clk_freq; + uint8_t lut_custom_seq_enable; + uint32_t reserved3[2]; + uint32_t sflash_a1size; + uint32_t sflash_a2size; + uint32_t sflash_b1size; + uint32_t sflash_b2size; + uint32_t cspad_setting_override; + uint32_t sclkpad_setting_override; + uint32_t datapad_setting_override; + uint32_t dqspad_setting_override; + uint32_t timeout_in_ms; + uint32_t command_interval; + uint16_t data_valid_time[2]; + uint16_t busy_offset; + uint16_t busybit_polarity; + uint32_t lookup_table[64]; + struct flexspi_lut_seq_s lut_customseq[12]; + uint32_t reserved4[4]; +}; + +/* Serial NOR configuration block */ + +struct flexspi_nor_config_s { + struct flexspi_mem_config_s mem_config; /* Common memory configuration + * info via FlexSPI */ + + uint32_t page_size; /* Page size of Serial NOR */ + uint32_t sector_size; /* Sector size of Serial NOR */ + uint8_t ipcmd_serial_clkfreq; /* Clock frequency for IP command */ + uint8_t is_uniform_blocksize; /* Sector/Block size is the same */ + uint8_t reserved0[2]; /* Reserved for future use */ + uint8_t serial_nor_type; /* Serial NOR Flash type: 0/1/2/3 */ + uint8_t need_exit_nocmdmode; /* Need to exit NoCmd mode before other + * IP command */ + uint8_t halfclk_for_nonreadcmd; /* Half the Serial Clock for non-read + * command: true/false */ + uint8_t need_restore_nocmdmode; /* Need to Restore NoCmd mode after IP + * command execution */ + uint32_t blocksize; /* Block size */ + uint32_t reserve2[11]; /* Reserved for future use */ +}; + +#endif /* __BOARDS_ARM_IMXRT_TEENSY_4_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */ diff --git a/boards/nxp/tropic-community/src/init.c b/boards/nxp/tropic-community/src/init.c new file mode 100644 index 000000000000..f3b43fc4d52d --- /dev/null +++ b/boards/nxp/tropic-community/src/init.c @@ -0,0 +1,415 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * NXP imxrt1062-v1 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. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arm_internal.h" +#include "arm_internal.h" +#include "imxrt_flexspi_nor_boot.h" +#include "imxrt_iomuxc.h" +#include "imxrt_flexcan.h" +#include "imxrt_enet.h" +#include +#include "board_config.h" + +#include +#undef FLEXSPI_LUT_COUNT +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); + +extern uint32_t _srodata; /* Start of .rodata */ +extern uint32_t _erodata; /* End of .rodata */ +extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */ +extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */ +extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */ +extern uint64_t _sdtcm; /* Copy destination start address in DTCM */ +extern uint64_t _edtcm; /* Copy destination end address in DTCM */ +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + +} +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ + +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_gpio_output(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + + +locate_code(".ramfunc") +void imxrt_flash_setup_prefetch_partition(void) +{ +//Prefetch tuning to be determined +#if 0 + putreg32((uint32_t)&_srodata, IMXRT_FLEXSPI1_AHBBUFREGIONSTART0); + putreg32((uint32_t)&_erodata, IMXRT_FLEXSPI1_AHBBUFREGIONEND0); + /*putreg32((uint32_t)&_stext, IMXRT_FLEXSPI1_AHBBUFREGIONSTART1); + putreg32((uint32_t)&_etext, IMXRT_FLEXSPI1_AHBBUFREGIONEND1); + putreg32((uint32_t)&_stext, IMXRT_FLEXSPI1_AHBBUFREGIONSTART2); + putreg32((uint32_t)&_etext, IMXRT_FLEXSPI1_AHBBUFREGIONEND2); + putreg32((uint32_t)&_stext, IMXRT_FLEXSPI1_AHBBUFREGIONSTART3); + putreg32((uint32_t)&_etext, IMXRT_FLEXSPI1_AHBBUFREGIONEND3);*/ +#endif +#if 0 + struct flexspi_type_s *g_flexspi = (struct flexspi_type_s *)IMXRT_FLEXSPIC_BASE; + /* RODATA */ + g_flexspi->AHBRXBUFCR0[0] = FLEXSPI_AHBRXBUFCR0_BUFSZ(48) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(1); + + + /* All Text */ + g_flexspi->AHBRXBUFCR0[1] = FLEXSPI_AHBRXBUFCR0_BUFSZ(80) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(1); + + /* Disable 2 */ + g_flexspi->AHBRXBUFCR0[2] = FLEXSPI_AHBRXBUFCR0_BUFSZ(0) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(0); + + /* Disable 3 */ + g_flexspi->AHBRXBUFCR0[3] = FLEXSPI_AHBRXBUFCR0_BUFSZ(0) | + FLEXSPI_AHBRXBUFCR0_MSTRID(0) | + FLEXSPI_AHBRXBUFCR0_PREFETCHEN(1) | + FLEXSPI_AHBRXBUFCR0_REGIONEN(0); +#endif + + ARM_DSB(); + ARM_ISB(); + ARM_DMB(); +} + +/**************************************************************************** + * Name: imxrt_ocram_initialize + * + * Description: + * Called off reset vector to reconfigure the flexRAM + * and finish the FLASH to RAM Copy. + * + ****************************************************************************/ + +__EXPORT void imxrt_ocram_initialize(void) +{ + uint32_t regval; + register uint64_t *src; + register uint64_t *dest; + + /* FlexRAM Configuration + * F = 64K ITCM + * A = 64K DTCM + * 5 = 64K OCRAM + * So 0xFFFFFFAA is + * 384K FlexRAM ITCM + * 128K FlexRAM DTCM + * */ + + putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17); + regval = getreg32(IMXRT_IOMUXC_GPR_GPR16); + putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16); + + /* Copy any necessary code sections from FLASH to ITCM. The process is the + * same as the code copying from FLASH to RAM above. */ + for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs; + dest < (uint64_t *)&_eitcmfuncs;) { + *dest++ = *src++; + } + + /* Clear .dtcm. We'll do this inline (vs. calling memset) just to be + * certain that there are no issues with the state of global variables. + */ + + for (dest = &_sdtcm; dest < &_edtcm;) { + *dest++ = 0; + } +} + +/**************************************************************************** + * Name: imxrt_boardinitialize + * + * Description: + * All i.MX RT architectures must provide the following entry point. This + * entry point is called early in the initialization -- after clocking and + * memory have been configured but before caches have been enabled and + * before any devices have been initialized. + * + ****************************************************************************/ + +__EXPORT void imxrt_boardinitialize(void) +{ + imxrt_flash_setup_prefetch_partition(); + + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + + imxrt_spidev_initialize(); + + imxrt_usb_initialize(); + + fmurt1062_timer_initialize(); +} + +/**************************************************************************** + * Function: imxrt_phy_boardinitialize + * + * Description: + * Some boards require specialized initialization of the PHY before it can + * be used. This may include such things as configuring GPIOs, resetting + * the PHY, etc. + * If CONFIG_IMXRT_ENET_PHYINIT is defined in the configuration then the + * board specific logic must provide imxrt_phyinitialize(); + * The i.MX RT Ethernet driver will call this function one time before it + * first uses the PHY. + * + * Input Parameters: + * intf - Always zero for now. + * + * Returned Value: + * OK on success; Negated errno on failure. + * + ****************************************************************************/ + +int imxrt_phy_boardinitialize(int intf) +{ +#ifdef CONFIG_IMXRT_GPIO1_0_15_IRQ + /* Configure the PHY interrupt pin */ + + printf("Configuring interrupt: %08x\n", GPIO_ENET_INT); + imxrt_config_gpio(GPIO_ENET_INT); +#endif + + /* Configure the PHY reset pin. + * + * The #RST uses inverted logic. The initial value of zero will put the + * PHY into the reset state. + */ + + printf("Configuring reset: %08x\n", GPIO_ENET_RST); + imxrt_config_gpio(GPIO_ENET_RST); + + /* Take the PHY out of reset. */ + + imxrt_gpio_write(GPIO_ENET_RST, true); + return OK; +} + +void imxrt_flexio_clocking(void) +{ + uint32_t reg; + + /* Init USB PLL3 PFD2 */ + + reg = getreg32(IMXRT_CCM_ANALOG_PFD_480); + + while ((getreg32(IMXRT_CCM_ANALOG_PLL_USB1) & + CCM_ANALOG_PLL_USB1_LOCK) == 0) { + } + + reg &= ~CCM_ANALOG_PFD_480_PFD2_FRAC_MASK; + + /* Set PLL3 PFD2 to 480 * 18 / CONFIG_PLL3_PFD2_FRAC */ + + reg |= ((uint32_t)(CONFIG_PLL3_PFD2_FRAC) << CCM_ANALOG_PFD_480_PFD3_FRAC_SHIFT); + + putreg32(reg, IMXRT_CCM_ANALOG_PFD_480); + + reg = getreg32(IMXRT_CCM_CDCDR); + reg &= ~(CCM_CDCDR_FLEXIO1_CLK_SEL_MASK | + CCM_CDCDR_FLEXIO1_CLK_PODF_MASK | + CCM_CDCDR_FLEXIO1_CLK_PRED_MASK); + reg |= CCM_CDCDR_FLEXIO1_CLK_SEL(CONFIG_FLEXIO1_CLK); + reg |= CCM_CDCDR_FLEXIO1_CLK_PODF + (CCM_PODF_FROM_DIVISOR(CONFIG_FLEXIO1_PODF_DIVIDER)); + reg |= CCM_CDCDR_FLEXIO1_CLK_PRED + (CCM_PRED_FROM_DIVISOR(CONFIG_FLEXIO1_PRED_DIVIDER)); + putreg32(reg, IMXRT_CCM_CDCDR); +} + + +/**************************************************************************** + * 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) +{ + imxrt_flexio_clocking(); + + /* Power on Interfaces */ + + board_spi_reset(10, 0xffff); + + px4_platform_init(); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#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)imxrt_serial_dma_poll, NULL); +#endif + + int ret = OK; +#if defined(CONFIG_IMXRT_USDHC) + ret = fmurt1062_usdhc_initialize(); +#endif + + /* Configure SPI-based devices */ + + ret = imxrt1062_spi_bus_initialize(); + +#ifdef CONFIG_IMXRT_ENET + imxrt_netinitialize(0); +#endif + +#ifdef CONFIG_IMXRT_FLEXCAN3 + imxrt_caninitialize(3); +#endif + + return ret; +} diff --git a/boards/nxp/tropic-community/src/sdhc.c b/boards/nxp/tropic-community/src/sdhc.c new file mode 100644 index 000000000000..df2f92cc1c31 --- /dev/null +++ b/boards/nxp/tropic-community/src/sdhc.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * Copyright (C) 2016-2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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. + * + ****************************************************************************/ +/* A micro Secure Digital (SD) card slot is available on the board connected to + * the SD Host Controller (USDHC1) signals of the MCU. This slot will accept + * micro format SD memory cards. + * + * ------------ ------------- -------- + * SD Card Slot Board Signal IMXRT Pin + * ------------ ------------- -------- + * DAT0 USDHC1_DATA0 GPIO_SD_B0_02 + * DAT1 USDHC1_DATA1 GPIO_SD_B0_03 + * DAT2 USDHC1_DATA2 GPIO_SD_B0_04 + * CD/DAT3 USDHC1_DATA3 GPIO_SD_B0_05 + * CMD USDHC1_CMD GPIO_SD_B0_00 + * CLK USDHC1_CLK GPIO_SD_B0_01 + * CD USDHC1_CD GPIO_B1_12 + * ------------ ------------- -------- + * + * There are no Write Protect available to the IMXRT. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "imxrt_usdhc.h" + +#include "board_config.h" + +#ifdef CONFIG_IMXRT_USDHC +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ +/**************************************************************************** + * Private Data + ****************************************************************************/ +/**************************************************************************** + * Private Functions + ****************************************************************************/ +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: fmurt1062_usdhc_initialize + * + * Description: + * Inititialize the SDHC SD card slot + * + ****************************************************************************/ + +int fmurt1062_usdhc_initialize(void) +{ + int ret; + + /* Mount the SDHC-based MMC/SD block driver */ + /* First, get an instance of the SDHC interface */ + + struct sdio_dev_s *sdhc = imxrt_usdhc_initialize(CONFIG_NSH_MMCSDSLOTNO); + + if (!sdhc) { + PX4_ERR("ERROR: Failed to initialize SDHC slot %d\n", CONFIG_NSH_MMCSDSLOTNO); + return -ENODEV; + } + + /* Now bind the SDHC interface to the MMC/SD driver */ + + ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdhc); + + if (ret != OK) { + PX4_ERR("ERROR: Failed to bind SDHC to the MMC/SD driver: %d\n", ret); + return ret; + } + + syslog(LOG_INFO, "Successfully bound SDHC to the MMC/SD driver\n"); + + return OK; +} +#endif /* CONFIG_IMXRT_USDHC */ diff --git a/boards/nxp/tropic-community/src/spi.cpp b/boards/nxp/tropic-community/src/spi.cpp new file mode 100644 index 000000000000..b267b18d0626 --- /dev/null +++ b/boards/nxp/tropic-community/src/spi.cpp @@ -0,0 +1,315 @@ +/************************************************************************************ + * + * Copyright (C) 2016, 2018 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt + * David Sidrane + * + * 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 +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include "imxrt_lpspi.h" +#include "imxrt_gpio.h" +#include "board_config.h" +#include + +#if defined(CONFIG_IMXRT_LPSPI3) || defined(CONFIG_IMXRT_LPSPI4) + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::LPSPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port1, GPIO::Pin28}), /* GPIO_AD_B1_12 GPIO1_IO28 */ + }), + initSPIBus(SPI::Bus::LPSPI4, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin18}), /* GPIO_B1_02 GPIO2_IO18 */ + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::Port2, GPIO::Pin0}), /* GPIO_B0_00 GPIO2_IO00 */ + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); + +#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT | IOMUX_PULL_DOWN_100K | IOMUX_CMOS_INPUT)) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +/************************************************************************************ + * Name: fmurt1062_spidev_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board. + * + ************************************************************************************/ + +void imxrt_spidev_initialize(void) +{ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } +} + +/************************************************************************************ + * Name: imxrt_spi_bus_initialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the NXP FMUKRT1062-V1 board. + * + ************************************************************************************/ + +static const px4_spi_bus_t *_spi_bus3; +static const px4_spi_bus_t *_spi_bus4; + +__EXPORT int imxrt1062_spi_bus_initialize(void) +{ + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case 3: _spi_bus3 = &px4_spi_buses[i]; break; + + case 4: _spi_bus4 = &px4_spi_buses[i]; break; + } + } + + /* Configure SPI-based devices */ + + struct spi_dev_s *spi_icm = px4_spibus_initialize(3); + + if (!spi_icm) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 1); + return -ENODEV; + } + + /* Default bus 1 to 8MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_icm, 8 * 1000 * 1000); + SPI_SETBITS(spi_icm, 8); + SPI_SETMODE(spi_icm, SPIDEV_MODE3); + + /* Get the SPI port for the BMI088 */ + + struct spi_dev_s *spi_bmi = px4_spibus_initialize(4); + + if (!spi_bmi) { + PX4_ERR("[boot] FAILED to initialize SPI port %d\n", 4); + return -ENODEV; + } + + /* Default ext bus to 8MHz and de-assert the known chip selects. + */ + + SPI_SETFREQUENCY(spi_bmi, 8 * 1000 * 1000); + SPI_SETBITS(spi_bmi, 8); + SPI_SETMODE(spi_bmi, SPIDEV_MODE3); + + /* deselect all */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + SPI_SELECT(spi_bmi, px4_spi_buses[bus].devices[i].devid, false); + } + } + } + + return OK; + +} + +/**************************************************************************** + * Name: imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status + * + * Description: + * The external functions, imxrt_lpspi1/2/3select and imxrt_lpspi1/2/3status must be + * provided by board-specific logic. They are implementations of the select + * and status methods of the SPI interface defined by struct spi_ops_s (see + * include/nuttx/spi/spi.h). All other methods (including imxrt_lpspibus_initialize()) + * are provided by common STM32 logic. To use this common SPI logic on your + * board: + * + * 1. Provide logic in imxrt_boardinitialize() to configure SPI chip select + * pins. + * 2. Provide imxrt_lpspi1/2/3select() and imxrt_lpspi1/2/3status() functions in your + * board-specific logic. These functions will perform chip selection and + * status operations using GPIOs in the way your board is configured. + * 3. Add a calls to imxrt_lpspibus_initialize() in your low level application + * initialization logic + * 4. The handle returned by imxrt_lpspibus_initialize() may then be used to bind the + * SPI driver to higher level logic (e.g., calling + * mmcsd_spislotinitialize(), for example, will bind the SPI driver to + * the SPI MMC/SD driver). + * + ****************************************************************************/ + +static inline void imxrt_spixselect(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio == 0) { + break; + } + + if (devid == bus->devices[i].devid) { + // SPI select is active low, so write !selected to select the device + imxrt_gpio_write(bus->devices[i].cs_gpio, !selected); + } + } +} + +#if defined(CONFIG_IMXRT_LPSPI3) +__EXPORT void imxrt_lpspi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_spixselect(_spi_bus3, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi3status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#if defined(CONFIG_IMXRT_LPSPI4) +__EXPORT void imxrt_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + imxrt_spixselect(_spi_bus4, dev, devid, selected); +} + +__EXPORT uint8_t imxrt_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +/************************************************************************************ + * Name: board_spi_reset + * + * Description: + * + * + ************************************************************************************/ + +__EXPORT void board_spi_reset(int ms, int bus_mask) +{ +#ifdef CONFIG_IMXRT_LPSPI1 + + /* Goal not to back feed the chips on the bus via IO lines */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == 1 || px4_spi_buses[bus].bus == 3) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].cs_gpio)); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + imxrt_config_gpio(_PIN_OFF(px4_spi_buses[bus].devices[i].drdy_gpio)); + } + } + } + } + + imxrt_config_gpio(GPIO_SPI1_SCK_OFF); + imxrt_config_gpio(GPIO_SPI1_MISO_OFF); + imxrt_config_gpio(GPIO_SPI1_MOSI_OFF); + + imxrt_config_gpio(GPIO_SPI3_SCK_OFF); + imxrt_config_gpio(GPIO_SPI3_MISO_OFF); + imxrt_config_gpio(GPIO_SPI3_MOSI_OFF); + + + imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SDA_RESET)); + imxrt_config_gpio(_PIN_OFF(GPIO_LPI2C3_SCL_RESET)); + + /* set the sensor rail off */ + imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 0); + + /* wait for the sensor rail to reach GND */ + usleep(ms * 1000); + warnx("reset done, %d ms", ms); + + /* re-enable power */ + + /* switch the sensor rail back on */ + imxrt_gpio_write(GPIO_VDD_3V3_SENSORS_EN, 1); + + /* wait a bit before starting SPI, different times didn't influence results */ + usleep(100); + + /* reconfigure the SPI pins */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + if (px4_spi_buses[bus].bus == 1 || px4_spi_buses[bus].bus == 3) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + imxrt_config_gpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + + if (px4_spi_buses[bus].devices[i].drdy_gpio != 0) { + imxrt_config_gpio(px4_spi_buses[bus].devices[i].drdy_gpio); + } + } + } + } + + imxrt_config_gpio(GPIO_LPSPI1_SCK); + imxrt_config_gpio(GPIO_LPSPI1_MISO); + imxrt_config_gpio(GPIO_LPSPI1_MOSI); + + imxrt_config_gpio(GPIO_LPSPI3_SCK); + imxrt_config_gpio(GPIO_LPSPI3_MISO); + imxrt_config_gpio(GPIO_LPSPI3_MOSI); + + imxrt_config_gpio(GPIO_LPI2C3_SDA); + imxrt_config_gpio(GPIO_LPI2C3_SCL); + +#endif /* CONFIG_IMXRT_LPSPI1 */ + +} + +#endif /* CONFIG_IMXRT_LPSPI3 || CONFIG_IMXRT_LPSPI4 */ diff --git a/boards/nxp/tropic-community/src/timer_config.cpp b/boards/nxp/tropic-community/src/timer_config.cpp new file mode 100644 index 000000000000..1955fcc26d91 --- /dev/null +++ b/boards/nxp/tropic-community/src/timer_config.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +#include + +#include +#include "hardware/imxrt_tmr.h" +#include "hardware/imxrt_flexpwm.h" +#include "imxrt_gpio.h" +#include "imxrt_iomuxc.h" +#include "hardware/imxrt_pinmux.h" +#include "imxrt_xbar.h" +#include "imxrt_periphclks.h" + +#include +#include + +#include "board_config.h" + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* Register accessors */ + +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) + +/* QTimer3 register accessors */ + +#define REG(_reg) _REG(IMXRT_QTIMER3_BASE + IMXRT_TMR_OFFSET(IMXRT_TMR_CH0,(_reg))) + +#define rCOMP1 REG(IMXRT_TMR_COMP1_OFFSET) +#define rCOMP2 REG(IMXRT_TMR_COMP2_OFFSET) +#define rCAPT REG(IMXRT_TMR_CAPT_OFFSET) +#define rLOAD REG(IMXRT_TMR_LOAD_OFFSET) +#define rHOLD REG(IMXRT_TMR_HOLD_OFFSET) +#define rCNTR REG(IMXRT_TMR_CNTR_OFFSET) +#define rCTRL REG(IMXRT_TMR_CTRL_OFFSET) +#define rSCTRL REG(IMXRT_TMR_SCTRL_OFFSET) +#define rCMPLD1 REG(IMXRT_TMR_CMPLD1_OFFSET) +#define rCMPLD2 REG(IMXRT_TMR_CMPLD2_OFFSET) +#define rCSCTRL REG(IMXRT_TMR_CSCTRL_OFFSET) +#define rFILT REG(IMXRT_TMR_FILT_OFFSET) +#define rDMA REG(IMXRT_TMR_DMA_OFFSET) +#define rENBL REG(IMXRT_TMR_ENBL_OFFSET) + + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0), // PWM_1, PMW_5 + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1), // PWM_0 + initIOPWM(PWM::FlexPWM2, PWM::Submodule2), // PWM_4 + initIOPWMDshot(PWM::FlexPWM4, PWM::Submodule2), // PWM_2, PWM_3 +}; + +#define FXIO_IOMUX (IOMUX_SLEW_FAST | IOMUX_DRIVE_130OHM | IOMUX_PULL_UP_47K | IOMUX_SCHMITT_TRIGGER) + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_06, GPIO_FLEXIO1_FLEXIO06_1 | FXIO_IOMUX, 6), /* RevA. PWM_1 RevB. PWM1 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_07, GPIO_FLEXIO1_FLEXIO07_1 | FXIO_IOMUX, 7), /* RevA. PWM_5 RevB. PWM2 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_08, GPIO_FLEXIO1_FLEXIO08_1 | FXIO_IOMUX, 8), /* RevA. PWM_0 RevB. PWM3 */ + initIOTimerChannel(io_timers, {PWM::PWM2_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_B0_11), /* RevA. PWM_4 RevB. PWM4 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_04, GPIO_FLEXIO1_FLEXIO04_1 | FXIO_IOMUX, 4), /* RevA. PWM_3 RevB. PWM5 */ + initIOTimerChannelDshot(io_timers, {PWM::PWM4_PWM_B, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_05, GPIO_FLEXIO1_FLEXIO05_1 | FXIO_IOMUX, 5), /* RevA. PWM_2 RevB. PWM6 */ +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); + +constexpr io_timers_t led_pwm_timers[MAX_LED_TIMERS] = { +}; + +constexpr timer_io_channels_t led_pwm_channels[MAX_TIMER_LED_CHANNELS] = { +}; + +#include +void fmurt1062_timer_initialize(void) +{ + /* We must configure Qtimer 3 as the IPG divide by to yield 16 Mhz + * and deliver that clock to the eFlexPWM234 via XBAR + * + * IPG = 144 Mhz + * 16Mhz = 144 / 9 + * COMP 1 = 5, COMP2 = 4 + * + * */ + + /* Enable Block Clocks for Qtimer and XBAR1 */ + + imxrt_clockall_timer3(); + imxrt_clockall_xbar1(); + + /* Disable Timer */ + + rCTRL = 0; + rCOMP1 = 5 - 1; // N - 1 + rCOMP2 = 4 - 1; + + rCAPT = 0; + rLOAD = 0; + rCNTR = 0; + + rSCTRL = TMR_SCTRL_OEN; + + rCMPLD1 = 0; + rCMPLD2 = 0; + rCSCTRL = 0; + rFILT = 0; + rDMA = 0; + + /* Count rising edges of primary source, + * Prescaler is /1 + * Count UP until compare, then re-initialize. a successful compare occurs when the counter reaches a COMP1 value. + * Toggle OFLAG output using alternating compare registers + */ + rCTRL = (TMR_CTRL_CM_MODE1 | TMR_CTRL_PCS_DIV1 | TMR_CTRL_LENGTH | TMR_CTRL_OUTMODE_TOG_ALT); + + /* QTIMER3_TIMER0 -> Flexpwm234ExtClk */ + + imxrt_xbar_connect(IMXRT_XBARA1_OUT_FLEXPWM234_EXT_CLK_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER3_TMR0_OUT); +} diff --git a/boards/nxp/tropic-community/src/tropic_led_pwm.cpp b/boards/nxp/tropic-community/src/tropic_led_pwm.cpp new file mode 100644 index 000000000000..22ef3a357541 --- /dev/null +++ b/boards/nxp/tropic-community/src/tropic_led_pwm.cpp @@ -0,0 +1,218 @@ +/**************************************************************************** + * + * 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 Airmind 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 tropic_led_pwm.cpp +*/ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include "imxrt_xbar.h" +#include "imxrt_periphclks.h" +#include "hardware/imxrt_tmr.h" +#include "hardware/imxrt_flexpwm.h" + +#define LED_PWM_FREQ 1000 +#define FLEXPWM_FREQ 1000000 +#define QTMR_FREQ (144000000/128) + +#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET) + +#define FLEXPWM_TIMER_BASE IMXRT_FLEXPWM2_BASE + +/* Register accessors */ +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) +#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg)) +#define FLEXPWMREG(_tmr, _sm, _reg) _REG16(FLEXPWM_TIMER_BASE + ((_sm) * SM_SPACING), (_reg)) +#define QTMRREG(_reg, _chn) _REG16(IMXRT_QTIMER4_BASE + ((_chn) * IMXRT_TMR_CHANNEL_SPACING),(_reg)) + +/* FlexPWM Registers for LED_G */ +#define rINIT(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0INIT_OFFSET) /* Initial Count Register */ +#define rCTRL(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL_OFFSET) /* Control Register */ +#define rCTRL2(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL2_OFFSET) /* Control 2 Register */ +#define rFSTS0(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_FSTS0_OFFSET) /* Fault Status Register */ +#define rVAL0(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL0_OFFSET) /* Value Register 0 */ +#define rVAL1(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL1_OFFSET) /* Value Register 1 */ +#define rVAL2(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL2_OFFSET) /* Value Register 2 */ +#define rVAL3(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL3_OFFSET) /* Value Register 3 */ +#define rVAL4(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL4_OFFSET) /* Value Register 4 */ +#define rVAL5(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL5_OFFSET) /* Value Register 5 */ +#define rFFILT0(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_FFILT0_OFFSET) /* Fault Filter Register */ +#define rDISMAP0(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP0_OFFSET) /* Fault Disable Mapping Register 0 */ +#define rDISMAP1(_tim, _sm) FLEXPWMREG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP1_OFFSET) /* Fault Disable Mapping Register 1 */ +#define rOUTEN(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_OUTEN_OFFSET) /* Output Enable Register */ +#define rDTSRCSEL(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_DTSRCSEL_OFFSET) /* PWM Source Select Register */ +#define rMCTRL(_tim) FLEXPWMREG(_tim, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) /* Master Control Register */ + +#define QTMR_rCOMP1(_chn) QTMRREG(IMXRT_TMR_COMP1_OFFSET, _chn) +#define QTMR_rCOMP2(_chn) QTMRREG(IMXRT_TMR_COMP2_OFFSET, _chn) +#define QTMR_rCAPT(_chn) QTMRREG(IMXRT_TMR_CAPT_OFFSET, _chn) +#define QTMR_rLOAD(_chn) QTMRREG(IMXRT_TMR_LOAD_OFFSET, _chn) +#define QTMR_rHOLD(_chn) QTMRREG(IMXRT_TMR_HOLD_OFFSET, _chn) +#define QTMR_rCNTR(_chn) QTMRREG(IMXRT_TMR_CNTR_OFFSET, _chn) +#define QTMR_rCTRL(_chn) QTMRREG(IMXRT_TMR_CTRL_OFFSET, _chn) +#define QTMR_rSCTRL(_chn) QTMRREG(IMXRT_TMR_SCTRL_OFFSET, _chn) +#define QTMR_rCMPLD1(_chn) QTMRREG(IMXRT_TMR_CMPLD1_OFFSET, _chn) +#define QTMR_rCMPLD2(_chn) QTMRREG(IMXRT_TMR_CMPLD2_OFFSET, _chn) +#define QTMR_rCSCTRL(_chn) QTMRREG(IMXRT_TMR_CSCTRL_OFFSET, _chn) +#define QTMR_rFILT(_chn) QTMRREG(IMXRT_TMR_FILT_OFFSET, _chn) +#define QTMR_rDMA(_chn) QTMRREG(IMXRT_TMR_DMA_OFFSET, _chn) +#define QTMR_rENBL(_chn) QTMRREG(IMXRT_TMR_ENBL_OFFSET, _chn) + +#define FLEXPWM_TIMER 2 +#define FLEXPWM_SM 3 + +#define FREQ + +static void flexpwm_led_green(uint16_t cvalue) +{ + if (cvalue == 0) { + rMCTRL(FLEXPWM_TIMER) &= ~MCTRL_RUN(1 << FLEXPWM_SM); + px4_arch_configgpio(GPIO_nLED_GREEN); + + } else { + rMCTRL(FLEXPWM_TIMER) |= (1 << (FLEXPWM_SM + MCTRL_CLDOK_SHIFT)); + rVAL1(FLEXPWM_TIMER, FLEXPWM_SM) = (FLEXPWM_FREQ / LED_PWM_FREQ) - 1; + rVAL5(FLEXPWM_TIMER, FLEXPWM_SM) = (FLEXPWM_FREQ / LED_PWM_FREQ) - (cvalue * 3); + rMCTRL(FLEXPWM_TIMER) |= MCTRL_LDOK(1 << FLEXPWM_SM) | MCTRL_RUN(1 << FLEXPWM_SM); + px4_arch_configgpio(PWM_LED_GREEN); + } +} + +static void init_qtimer(unsigned channel) +{ + QTMR_rCNTR(channel) = 0; /* Reset counter */ + QTMR_rSCTRL(channel) = (TMR_SCTRL_OEN | TMR_SCTRL_FORCE); /* Enable output */ + QTMR_rCSCTRL(channel) = (TMR_CSCTRL_CL1_COMP1); + QTMR_rCOMP1(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCMPLD1(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCOMP2(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCMPLD2(channel) = 0x1; /* Store initial value to the duty-compare register */ + QTMR_rCTRL(channel) = (TMR_CTRL_PCS_DIV32 | TMR_CTRL_OUTMODE_TOG_ALT | TMR_CTRL_DIR | + TMR_CTRL_CM_MODE1); /* Run counter */ +} + +int +led_pwm_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel == 2) { + + if (cvalue == 0) { + px4_arch_configgpio(GPIO_nLED_RED); + + } else { + px4_arch_configgpio(PWM_LED_RED); + QTMR_rCMPLD1(0) = (uint16_t)cvalue * 256; + } + + } else if (channel == 1) { + flexpwm_led_green(cvalue); + + } else if (channel == 0) { + + if (cvalue == 0) { + px4_arch_configgpio(GPIO_nLED_BLUE); + + } else { + px4_arch_configgpio(PWM_LED_BLUE); + QTMR_rCMPLD1(1) = (uint16_t)cvalue * 256; + } + } + + return 0; +} + +int led_pwm_servo_init() +{ + /* PWM_LED_GREEN - FLEXPWM2_PWMB03 */ + imxrt_clockall_pwm2(); + + + /* PWM_LED_RED PWM_LED_BLUE - QTIMER4 */ + imxrt_clockall_timer4(); + + /* Clear all Faults */ + rFSTS0(FLEXPWM_TIMER) = FSTS_FFLAG_MASK; + rMCTRL(FLEXPWM_TIMER) |= (1 << (FLEXPWM_SM + MCTRL_CLDOK_SHIFT)); + + rCTRL2(FLEXPWM_TIMER, FLEXPWM_SM) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(FLEXPWM_TIMER, FLEXPWM_SM) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rVAL0(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rVAL2(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rVAL4(FLEXPWM_TIMER, FLEXPWM_SM) = 0; + rFFILT0(FLEXPWM_TIMER) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(FLEXPWM_TIMER, FLEXPWM_SM) = 0xf000; + rDISMAP1(FLEXPWM_TIMER, FLEXPWM_SM) = 0xf000; + + rOUTEN(FLEXPWM_TIMER) |= OUTEN_PWMB_EN(1 << FLEXPWM_SM); + + rDTSRCSEL(FLEXPWM_TIMER) = 0; + rMCTRL(FLEXPWM_TIMER) |= MCTRL_LDOK(1 << FLEXPWM_SM); + + /* QTMR */ + init_qtimer(0); + init_qtimer(1); + + /* Red - QTIMER4_TMR0 */ + imxrt_xbar_connect(IMXRT_XBARA1_OUT_IOMUX_XBAR_IO16_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER4_TMR0_OUT); + + /* Blue - QTIMER4_TMR1 */ + imxrt_xbar_connect(IMXRT_XBARA1_OUT_IOMUX_XBAR_IO17_SEL_OFFSET, IMXRT_XBARA1_IN_QTIMER4_TMR1_OUT); + + /* Set XBAR 16 and 17 as an output */ + putreg32((1 << 28) | (1 << 29), IMXRT_IOMUXC_GPR_GPR6); + + return OK; +} diff --git a/boards/nxp/tropic-community/src/usb.c b/boards/nxp/tropic-community/src/usb.c new file mode 100644 index 000000000000..99e885964307 --- /dev/null +++ b/boards/nxp/tropic-community/src/usb.c @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" +#include "imxrt_periphclks.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int imxrt_usb_initialize(void) +{ + imxrt_clockall_usboh3(); + + // We abuse VBUS2 to check if system power checks VBUS gets below 4.7V + putreg32(USB_ANALOG_USB_VBUS_DETECT_VBUSVALID_THRESH_4V7 | USB_ANALOG_USB_VBUS_DETECT_VBUSVALID_PWRUP_CMPS, + IMXRT_USB_ANALOG_USB2_VBUS_DETECT); + return 0; +} +/************************************************************************************ + * Name: imxrt_usbpullup + * + * Description: + * If USB is supported and the board supports a pullup via GPIO (for USB software + * connect and disconnect), then the board software must provide imxrt_usbpullup. + * See include/nuttx/usb/usbdev.h for additional description of this method. + * Alternatively, if no pull-up GPIO the following EXTERN can be redefined to be + * NULL. + * + ************************************************************************************/ + +__EXPORT +int imxrt_usbpullup(FAR struct usbdev_s *dev, bool enable) +{ + usbtrace(TRACE_DEVPULLUP, (uint16_t)enable); + + return OK; +} + +/************************************************************************************ + * Name: imxrt_usbsuspend + * + * Description: + * Board logic must provide the imxrt_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT +void imxrt_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} + +/************************************************************************************ + * Name: board_read_VBUS_state + * + * Description: + * All boards must provide a way to read the state of VBUS, this my be simple + * digital input on a GPIO. Or something more complicated like a Analong input + * or reading a bit from a USB controller register. + * + * Returns - 0 if connected. + * + ************************************************************************************/ + +int board_read_VBUS_state(void) +{ + + return (getreg32(IMXRT_USB_ANALOG_USB1_VBUS_DETECT_STAT) & USB_ANALOG_USB_VBUS_DETECT_STAT_VBUS_VALID) ? 0 : 1; +} diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index cd8385827e85..ba89894efee0 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -297,6 +297,29 @@ else() endif() +# create .hex if configured in NuttX +if (CONFIG_INTELHEX_BINARY) + + string(REPLACE ".elf" ".hex" hex_package ${PX4_BINARY_DIR}/${FW_NAME}) + + add_custom_command( + OUTPUT ${hex_package} + COMMAND + ${CMAKE_OBJCOPY} -O ihex + ${PX4_BINARY_DIR}/${PX4_CONFIG}.elf + ${hex_package} + DEPENDS + ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin + airframes_xml + parameters_xml + COMMENT "Creating ${hex_package}" + WORKING_DIRECTORY ${PX4_BINARY_DIR} + ) + + add_custom_target(px4_hex ALL DEPENDS ${hex_package}) + +endif() + # create .px4 with parameter and airframe metadata if (TARGET parameters_xml AND TARGET airframes_xml) @@ -383,7 +406,7 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/gdbinit.in ${PX4_BINARY_DIR}/.g if(NOT NUTTX_DIR MATCHES "external") if(CONFIG_ARCH_CHIP_MIMXRT1062DVL6A) set(DEBUG_DEVICE "MIMXRT1062XXX6A") - set(DEBUG_SVD_FILE "MIMXRT1052.svd") + set(DEBUG_SVD_FILE "MIMXRT1062.xml") elseif(CONFIG_ARCH_CHIP_MIMXRT1176DVMAA) set(DEBUG_DEVICE "MIMXRT1176DVMAA") set(DEBUG_SVD_FILE "MIMXRT1176_cm7.xml") diff --git a/platforms/nuttx/cmake/upload.cmake b/platforms/nuttx/cmake/upload.cmake index ce776b8a6247..9d3013fbd9ed 100644 --- a/platforms/nuttx/cmake/upload.cmake +++ b/platforms/nuttx/cmake/upload.cmake @@ -35,6 +35,7 @@ set(vendorstr_underscore) set(productstr_underscore) string(REPLACE " " "_" vendorstr_underscore ${CONFIG_CDCACM_VENDORSTR}) +string(REPLACE "," "_" vendorstr_underscore "${vendorstr_underscore}") string(REPLACE " " "_" productstr_underscore ${CONFIG_CDCACM_PRODUCTSTR}) set(serial_ports) diff --git a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c index cccfd41bea4c..9b3bab502227 100644 --- a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c @@ -95,7 +95,7 @@ struct boardinfo board_info = { .board_type = BOARD_TYPE, .board_rev = 0, .fw_size = 0, - .systick_mhz = 480, + .systick_mhz = STM32_CPUCLK_FREQUENCY / 1000000, }; static void board_init(void); diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt index cba22cb32025..deff20f14c9a 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/CMakeLists.txt @@ -35,7 +35,9 @@ px4_add_library(arch_board_reset board_reset.cpp ) +if (CONFIG_ARCH_FAMILY_IMXRT117x) target_link_libraries(arch_board_reset PRIVATE arch_board_romapi) +endif() # up_systemreset if (NOT DEFINED CONFIG_BUILD_FLAT) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp index 6f1e21700d0f..1d29a64dfbb4 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_reset/board_reset.cpp @@ -42,11 +42,12 @@ #include #include #include -#include - +#ifdef CONFIG_ARCH_FAMILY_IMXRT117x +#include #include #include +#endif #define BOOT_RTC_SIGNATURE 0xb007b007 #define PX4_IMXRT_RTC_REBOOT_REG 3 @@ -58,9 +59,13 @@ static int board_reset_enter_bootloader() { +#ifdef CONFIG_ARCH_FAMILY_IMXRT117x uint32_t regvalue = BOOT_RTC_SIGNATURE; modifyreg32(IMXRT_SNVS_LPCR, 0, SNVS_LPCR_GPR_Z_DIS); putreg32(regvalue, PX4_IMXRT_RTC_REBOOT_REG_ADDRESS); +#elif defined(BOARD_HAS_TEENSY_BOOTLOADER) + asm("BKPT #251"); /* Enter Teensy MKL02 bootloader */ +#endif return OK; } @@ -68,13 +73,18 @@ int board_reset(int status) { if (status == REBOOT_TO_BOOTLOADER) { board_reset_enter_bootloader(); + } + +#if defined(BOARD_HAS_ISP_BOOTLOADER) - } else if (status == REBOOT_TO_ISP) { + else if (status == REBOOT_TO_ISP) { uint32_t arg = 0xeb100000; ROM_API_Init(); ROM_RunBootloader(&arg); } +#endif + #if defined(BOARD_HAS_ON_RESET) board_on_reset(status); #endif diff --git a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp index 25c47ecffa1a..883d13908e92 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/led_pwm/led_pwm.cpp @@ -61,6 +61,7 @@ #include #include "hardware/imxrt_tmr.h" +#ifndef BOARD_HAS_CUSTOM_LED_PWM int led_pwm_servo_set(unsigned channel, uint8_t cvalue) { return 0; @@ -68,8 +69,8 @@ int led_pwm_servo_set(unsigned channel, uint8_t cvalue) int led_pwm_servo_init(void) { return 0; - } +#endif #if 0 && defined(BOARD_HAS_LED_PWM) || defined(BOARD_HAS_UI_LED_PWM) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp index 01389b120093..0c3a10d661dc 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp @@ -41,15 +41,155 @@ #include #include "chip.h" +#ifdef TONE_ALARM_FLEXPWM +#include "hardware/imxrt_flexpwm.h" +#else #include "hardware/imxrt_gpt.h" +#endif #include "imxrt_periphclks.h" +#define CAT4_(A, B, C, D) A##B##C##D +#define CAT4(A, B, C, D) CAT4_(A, B, C, D) + #define CAT3_(A, B, C) A##B##C #define CAT3(A, B, C) CAT3_(A, B, C) #define CAT2_(A, B) A##B #define CAT2(A, B) CAT2_(A, B) +#ifdef TONE_ALARM_FLEXPWM + + +/* +* Period of the free-running counter, in microseconds. +*/ +#define TONE_ALARM_COUNTER_PERIOD 4294967296 + +/* Tone Alarm configuration */ + +#define TONE_ALARM_TIMER_CLOCK BOARD_GPT_FREQUENCY /* The input clock frequency to the GPT block */ +#define TONE_ALARM_TIMER_BASE CAT3(IMXRT_FLEXPWM, TONE_ALARM_TIMER,_BASE) /* The Base address of the GPT */ +#define TONE_ALARM_TIMER_VECTOR CAT4(IMXRT_IRQ_FLEXPWM, TONE_ALARM_TIMER, _, TONE_ALARM_CHANNEL) /* The GPT Interrupt vector */ + +#if TONE_ALARM_TIMER == 1 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm1() /* The Clock Gating macro for this PWM */ +#elif TONE_ALARM_TIMER == 2 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm2() /* The Clock Gating macro for this PWM */ +#elif TONE_ALARM_TIMER == 3 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm3() /* The Clock Gating macro for this PWM */ +#elif TONE_ALARM_TIMER == 4 +# define TONE_ALARM_CLOCK_ALL() imxrt_clockall_pwm4() /* The Clock Gating macro for this PWM */ +#endif + +# define TONE_ALARM_TIMER_FREQ 1000000 + +#define SM_SPACING (IMXRT_FLEXPWM_SM1CNT_OFFSET-IMXRT_FLEXPWM_SM0CNT_OFFSET) + +/* Register accessors */ +#define _REG(_addr) (*(volatile uint16_t *)(_addr)) +#define _REG16(_base, _reg) (*(volatile uint16_t *)(_base + _reg)) +#define REG(_tmr, _sm, _reg) _REG16(TONE_ALARM_TIMER_BASE + ((_sm) * SM_SPACING), (_reg)) + + +#define rINIT(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0INIT_OFFSET) /* Initial Count Register */ +#define rCTRL(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL_OFFSET) /* Control Register */ +#define rCTRL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0CTRL2_OFFSET) /* Control 2 Register */ +#define rFSTS0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FSTS0_OFFSET) /* Fault Status Register */ +#define rVAL0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL0_OFFSET) /* Value Register 0 */ +#define rVAL1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL1_OFFSET) /* Value Register 1 */ +#define rVAL2(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL2_OFFSET) /* Value Register 2 */ +#define rVAL3(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL3_OFFSET) /* Value Register 3 */ +#define rVAL4(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL4_OFFSET) /* Value Register 4 */ +#define rVAL5(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0VAL5_OFFSET) /* Value Register 5 */ +#define rFFILT0(_tim) REG(_tim, 0, IMXRT_FLEXPWM_FFILT0_OFFSET) /* Fault Filter Register */ +#define rDISMAP0(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP0_OFFSET) /* Fault Disable Mapping Register 0 */ +#define rDISMAP1(_tim, _sm) REG(_tim, _sm,IMXRT_FLEXPWM_SM0DISMAP1_OFFSET) /* Fault Disable Mapping Register 1 */ +#define rOUTEN(_tim) REG(_tim, 0, IMXRT_FLEXPWM_OUTEN_OFFSET) /* Output Enable Register */ +#define rDTSRCSEL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_DTSRCSEL_OFFSET) /* PWM Source Select Register */ +#define rMCTRL(_tim) REG(_tim, 0, IMXRT_FLEXPWM_MCTRL_OFFSET) /* Master Control Register */ + +namespace ToneAlarmInterface +{ + +void init() +{ +#if defined(TONE_ALARM_TIMER) + /* configure the GPIO to the idle state */ + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); + + /* Enable the Module clock */ + + TONE_ALARM_CLOCK_ALL(); + + /* Clear all Faults */ + rFSTS0(TONE_ALARM_TIMER) = FSTS_FFLAG_MASK; + rMCTRL(TONE_ALARM_TIMER) |= (1 << (TONE_ALARM_CHANNEL + MCTRL_CLDOK_SHIFT)); + + rCTRL2(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rVAL0(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rVAL2(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rVAL4(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0; + rFFILT0(TONE_ALARM_TIMER) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0xf000; + rDISMAP1(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = 0xf000; + + rOUTEN(TONE_ALARM_TIMER) |= TONE_ALARM_FLEXPWM == PWMA_VAL ? OUTEN_PWMA_EN(1 << TONE_ALARM_CHANNEL) + : OUTEN_PWMB_EN(1 << TONE_ALARM_CHANNEL); + + rDTSRCSEL(TONE_ALARM_TIMER) = 0; + rMCTRL(TONE_ALARM_TIMER) |= MCTRL_LDOK(1 << TONE_ALARM_CHANNEL); +#endif /* TONE_ALARM_TIMER */ +} + +hrt_abstime start_note(unsigned frequency) +{ + hrt_abstime time_started = 0; +#if defined(TONE_ALARM_TIMER) + + rMCTRL(TONE_ALARM_TIMER) |= (1 << (TONE_ALARM_CHANNEL + MCTRL_CLDOK_SHIFT)); + + rVAL1(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = (TONE_ALARM_TIMER_FREQ / frequency) - 1; + + if (TONE_ALARM_FLEXPWM == PWMA_VAL) { + rVAL3(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = ((TONE_ALARM_TIMER_FREQ / frequency) / 2); + + } else { + rVAL5(TONE_ALARM_TIMER, TONE_ALARM_CHANNEL) = ((TONE_ALARM_TIMER_FREQ / frequency) / 2); + } + + rMCTRL(TONE_ALARM_TIMER) |= MCTRL_LDOK(1 << TONE_ALARM_CHANNEL) | MCTRL_RUN(1 << TONE_ALARM_CHANNEL); + + // configure the GPIO to enable timer output + irqstate_t flags = enter_critical_section(); + time_started = hrt_absolute_time(); + px4_arch_configgpio(GPIO_TONE_ALARM); + leave_critical_section(flags); +#endif /* TONE_ALARM_TIMER */ + + return time_started; +} + +void stop_note() +{ +#if defined(TONE_ALARM_TIMER) + /* stop the current note */ + + rMCTRL(TONE_ALARM_TIMER) &= ~MCTRL_RUN(1 << TONE_ALARM_CHANNEL); + + /* + * Make sure the GPIO is not driving the speaker. + */ + px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); +#endif /* TONE_ALARM_TIMER */ +} + +} /* namespace ToneAlarmInterface */ + +#else + /* Check that tone alarm and HRT timers are different */ #if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) # if TONE_ALARM_TIMER == HRT_TIMER @@ -134,8 +274,6 @@ #define CR_OM CAT3(GPT_CR_OM, TONE_ALARM_CHANNEL,_TOGGLE) /* Output Compare mode */ -#define CBRK_BUZZER_KEY 782097 - namespace ToneAlarmInterface { @@ -215,3 +353,5 @@ void stop_note() } } /* namespace ToneAlarmInterface */ + +#endif diff --git a/src/drivers/distance_sensor/cm8jl65/module.yaml b/src/drivers/distance_sensor/cm8jl65/module.yaml index 886dce7e3a78..0c9e70083693 100644 --- a/src/drivers/distance_sensor/cm8jl65/module.yaml +++ b/src/drivers/distance_sensor/cm8jl65/module.yaml @@ -18,7 +18,7 @@ parameters: values: 25: ROTATION_DOWNWARD_FACING 24: ROTATION_UPWARD_FACING - 12: ROTATION_BACKWARD_FACING + 4: ROTATION_BACKWARD_FACING 0: ROTATION_FORWARD_FACING 6: ROTATION_LEFT_FACING 2: ROTATION_RIGHT_FACING diff --git a/src/drivers/lights/rgbled_pwm/CMakeLists.txt b/src/drivers/lights/rgbled_pwm/CMakeLists.txt index 2001cdc4921d..6fc41d0301bd 100644 --- a/src/drivers/lights/rgbled_pwm/CMakeLists.txt +++ b/src/drivers/lights/rgbled_pwm/CMakeLists.txt @@ -41,3 +41,8 @@ px4_add_module( arch_io_pins arch_led_pwm ) + +target_link_libraries(drivers__rgbled_pwm + PRIVATE + drivers_board # Allows board to override PWM functions +) diff --git a/src/drivers/power_monitor/ina220/ina220.cpp b/src/drivers/power_monitor/ina220/ina220.cpp index 3667ee56d4b7..305d1a61a28f 100644 --- a/src/drivers/power_monitor/ina220/ina220.cpp +++ b/src/drivers/power_monitor/ina220/ina220.cpp @@ -56,7 +56,7 @@ INA220::INA220(const I2CSPIDriverConfig &config, int battery_index) : { float fvalue = MAX_CURRENT; _max_current = fvalue; - param_t ph = (_ch_type == PM_CH_TYPE_VBATT) ? param_find("INA220_CURRENT_BAT") : param_find("INA220_CURRENT_REG"); + param_t ph = (_ch_type == PM_CH_TYPE_VBATT) ? param_find("INA220_CUR_BAT") : param_find("INA220_CUR_REG"); if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) { _max_current = fvalue; diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index db88941a564b..7fa9e843f75c 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include using namespace time_literals; @@ -58,6 +59,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), + _sub_gnss_heading(node), _pub_moving_baseline_data(node), _pub_rtcm_stream(node), _channel_using_fix2(new bool[_max_channels]) @@ -100,6 +102,12 @@ UavcanGnssBridge::init() return res; } + res = _sub_gnss_heading.start(RelPosHeadingCbBinder(this, &UavcanGnssBridge::gnss_relative_sub_cb)); + + if (res < 0) { + PX4_WARN("GNSS relative sub failed %i", res); + return res; + } // UAVCAN_PUB_RTCM int32_t uavcan_pub_rtcm = 0; @@ -295,6 +303,7 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _rel_heading_valid = msg.reported_heading_acc_available; + _rel_heading = math::radians(msg.reported_heading_deg); + _rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg); +} template void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure &msg, uint8_t fix_type, @@ -463,9 +480,21 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.vdop = msg.pdop; } - report.heading = heading; - report.heading_offset = heading_offset; - report.heading_accuracy = heading_accuracy; + // Use heading from RelPosHeading message if available and we have RTK Fixed solution. + if (_rel_heading_valid && (fix_type == sensor_gps_s::FIX_TYPE_RTK_FIXED)) { + report.heading = _rel_heading; + report.heading_offset = NAN; + report.heading_accuracy = _rel_heading_accuracy; + + _rel_heading = NAN; + _rel_heading_accuracy = NAN; + _rel_heading_valid = false; + + } else { + report.heading = heading; + report.heading_offset = heading_offset; + report.heading_accuracy = heading_accuracy; + } report.noise_per_ms = noise_per_ms; report.jamming_indicator = jamming_indicator; diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index f2f28eb5bc86..f8205c3b38df 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -82,6 +83,7 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg); + void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure &msg); template void process_fixx(const uavcan::ReceivedDataStructure &msg, @@ -113,11 +115,16 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)> TimerCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > + RelPosHeadingCbBinder; + uavcan::INode &_node; uavcan::Subscriber _sub_auxiliary; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; + uavcan::Subscriber _sub_gnss_heading; uavcan::Publisher _pub_moving_baseline_data; uavcan::Publisher _pub_rtcm_stream; @@ -137,6 +144,10 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase bool _publish_rtcm_stream{false}; bool _publish_moving_baseline_data{false}; + float _rel_heading_accuracy{NAN}; + float _rel_heading{NAN}; + bool _rel_heading_valid{false}; + perf_counter_t _rtcm_stream_pub_perf{nullptr}; perf_counter_t _moving_baseline_data_pub_perf{nullptr}; }; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 918ab523c0d6..e6b520b60024 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -74,11 +74,6 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { - if (fabsf(vz) < _min_vz_for_valid_consistency) { - // We can only check consistency if there is vertical motion - return; - } - if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { _is_kinematically_consistent = false; @@ -86,7 +81,8 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) } } else { - if ((_test_ratio < 1.f) + if ((fabsf(vz) > _min_vz_for_valid_consistency) + && (_test_ratio < 1.f) && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us) ) { _is_kinematically_consistent = true; diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 7aad6620cee5..1278708d98aa 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -132,13 +132,11 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, float total_weight = 0.0f; // calculate weighting for each model assuming a normal distribution const float min_weight = 1e-5f; - uint8_t n_weight_clips = 0; for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { _model_weights(model_index) = gaussianDensity(model_index) * _model_weights(model_index); if (_model_weights(model_index) < min_weight) { - n_weight_clips++; _model_weights(model_index) = min_weight; } @@ -146,13 +144,7 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, } // normalise the weighting function - if (n_weight_clips < N_MODELS_EKFGSF) { - _model_weights /= total_weight; - - } else { - // all weights have collapsed due to excessive innovation variances so reset filters - reset(); - } + _model_weights /= total_weight; } // Calculate a composite yaw vector as a weighted average of the states for each model. @@ -332,11 +324,12 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co matrix::Matrix K; matrix::SquareMatrix P_new; + matrix::SquareMatrix S_inverse; sym::YawEstComputeMeasurementUpdate(_ekf_gsf[model_index].P, vel_obs_var, FLT_EPSILON, - &_ekf_gsf[model_index].S_inverse, + &S_inverse, &_ekf_gsf[model_index].S_det_inverse, &K, &P_new); @@ -355,21 +348,24 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index, const Vector2f &vel_NE, co _ekf_gsf[model_index].P(index, index) = fmaxf(_ekf_gsf[model_index].P(index, index), min_var); } - // test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] - const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); + // normalized innovation squared = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1] + _ekf_gsf[model_index].nis = _ekf_gsf[model_index].innov * (S_inverse * _ekf_gsf[model_index].innov); // Perform a chi-square innovation consistency test and calculate a compression scale factor // that limits the magnitude of innovations to 5-sigma - // If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma + // If the normalized innovation squared is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma // This protects from large measurement spikes - const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f; - - // Correct the state vector and capture the change in yaw angle - const float oldYaw = _ekf_gsf[model_index].X(2); + if (_ekf_gsf[model_index].nis > sq(5.f)) { + _ekf_gsf[model_index].innov *= sqrtf(sq(5.f) / _ekf_gsf[model_index].nis); + _ekf_gsf[model_index].nis = sq(5.f); + } - _ekf_gsf[model_index].X -= (K * _ekf_gsf[model_index].innov) * innov_comp_scale_factor; + // Correct the state vector + const Vector3f delta_state = -K * _ekf_gsf[model_index].innov; + const float yawDelta = delta_state(2); - const float yawDelta = _ekf_gsf[model_index].X(2) - oldYaw; + _ekf_gsf[model_index].X.xy() += delta_state.xy(); + _ekf_gsf[model_index].X(2) = wrap_pi(_ekf_gsf[model_index].X(2) + yawDelta); // apply the change in yaw angle to the AHRS // take advantage of sparseness in the yaw rotation matrix @@ -417,10 +413,7 @@ void EKFGSF_yaw::initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accura float EKFGSF_yaw::gaussianDensity(const uint8_t model_index) const { - // calculate transpose(innovation) * inv(S) * innovation - const float normDist = _ekf_gsf[model_index].innov.dot(_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov); - - return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * normDist); + return (1.f / (2.f * M_PI_F)) * sqrtf(_ekf_gsf[model_index].S_det_inverse) * expf(-0.5f * _ekf_gsf[model_index].nis); } bool EKFGSF_yaw::getLogData(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF], diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h index ec84ed06f925..8140477d9d65 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h @@ -121,7 +121,7 @@ class EKFGSF_yaw struct { matrix::Vector3f X{}; // Vel North (m/s), Vel East (m/s), yaw (rad)s matrix::SquareMatrix P{}; // covariance matrix - matrix::SquareMatrix S_inverse{}; // inverse of the innovation covariance matrix + float nis{}; // normalized innovation squared float S_det_inverse{}; // inverse of the innovation covariance matrix determinant matrix::Vector2f innov{}; // Velocity N,E innovation (m/s) } _ekf_gsf[N_MODELS_EKFGSF] {}; diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f83ba6e1e912..a40289f7b340 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2405,6 +2405,15 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) return; //TODO: change and set to NAN } + if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) { + if (!PX4_ISFINITE(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) { + // Apply offset + float yaw_offset = matrix::wrap_pi(math::radians(_param_ekf2_gps_yaw_off.get())); + vehicle_gps_position.heading_offset = yaw_offset; + vehicle_gps_position.heading = matrix::wrap_pi(vehicle_gps_position.heading - yaw_offset); + } + } + const float altitude_amsl = static_cast(vehicle_gps_position.altitude_msl_m); const float altitude_ellipsoid = static_cast(vehicle_gps_position.altitude_ellipsoid_m); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 61105a3c9b1c..0bec478e0d73 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -537,6 +537,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem // Used by EKF-GSF experimental yaw estimator (ParamExtFloat) _param_ekf2_gsf_tas_default, + (ParamFloat) _param_ekf2_gps_yaw_off, #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) diff --git a/src/modules/ekf2/params_gnss.yaml b/src/modules/ekf2/params_gnss.yaml index 55c530a125c9..957d2d70a7ce 100644 --- a/src/modules/ekf2/params_gnss.yaml +++ b/src/modules/ekf2/params_gnss.yaml @@ -46,6 +46,15 @@ parameters: min: 1.0 unit: SD decimal: 1 + EKF2_GPS_YAW_OFF: + description: + short: Heading/Yaw offset for dual antenna GPS + type: float + default: 0.0 + min: 0.0 + max: 360.0 + unit: deg + decimal: 1 EKF2_GPS_V_GATE: description: short: Gate size for GNSS velocity fusion diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 727aad54d4ee..3d7e873854d2 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -184,7 +184,6 @@ PARAM_DEFINE_INT32(SDLOG_UUID, 1); * * @value 0 Disabled * @value 2 XChaCha20 - * @value 3 AES * * @group SD Logging */ diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 058e9d95f1b3..f811eb82f014 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -399,17 +399,11 @@ MavlinkFTP::_workList(PayloadHeader *payload) payload->data[offset++] = kDirentSkip; *((char *)&payload->data[offset]) = '\0'; offset++; - payload->size = offset; - closedir(dp); + errorCode = kErrFailErrno; - return errorCode; - } - - // FIXME: does this ever happen? I would assume readdir always sets errno. - // no more entries? - if (payload->offset != 0 && offset == 0) { + } else if (offset == 0) { // User is requesting subsequent dir entries but there were none. This means the user asked - // to seek past EOF. + // to seek past EOF. This can happen with `payload->offset == 0` if the directory is empty. errorCode = kErrEOF; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b573e36fecae..7a1d3b3df734 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -566,10 +566,10 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility if (!target_ok) { - // Reject alien commands only if there is no forwarding or we've never seen target component before if (!_mavlink.get_forwarding_on() || !_mavlink.component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) { - acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED); + PX4_INFO("Ignore command %d from %d/%d to %d/%d", + cmd_mavlink.command, msg->sysid, msg->compid, cmd_mavlink.target_system, cmd_mavlink.target_component); } return; @@ -803,6 +803,16 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(msg, &ack); + // We should not clog the command_ack queue with acks that are not for us. + // Therefore, we drop them early and move on. + bool target_ok = evaluate_target_ok(0, ack.target_system, ack.target_component); + + if (!target_ok) { + PX4_DEBUG("Drop ack %d for %d from %d/%d to %d/%d\n", + ack.result, ack.command, msg->sysid, msg->compid, ack.target_system, ack.target_component); + return; + } + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink.get_channel()); vehicle_command_ack_s command_ack{}; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index e34f242d4ada..f27c112aac2c 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -585,6 +585,8 @@ void RTL::init_rtl_mission_type() _set_rtl_mission_type = RtlType::NONE; } + mission_s new_mission = _mission_sub.get(); + switch (new_rtl_mission_type) { case RtlType::RTL_DIRECT_MISSION_LAND: _rtl_mission_type_handle = new RtlDirectMissionLand(_navigator); @@ -593,13 +595,13 @@ void RTL::init_rtl_mission_type() break; case RtlType::RTL_MISSION_FAST: - _rtl_mission_type_handle = new RtlMissionFast(_navigator); + _rtl_mission_type_handle = new RtlMissionFast(_navigator, new_mission); _set_rtl_mission_type = RtlType::RTL_MISSION_FAST; _rtl_type = RtlType::RTL_MISSION_FAST; break; case RtlType::RTL_MISSION_FAST_REVERSE: - _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator); + _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator, new_mission); _set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; break; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index d3e07a36ddc3..ccfc91cfde65 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -46,10 +46,10 @@ static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5; -RtlMissionFast::RtlMissionFast(Navigator *navigator) : +RtlMissionFast::RtlMissionFast(Navigator *navigator, mission_s mission) : RtlBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE) { - + _mission = mission; } void RtlMissionFast::on_inactive() diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h index c782a471bd7a..d1231022b6f1 100644 --- a/src/modules/navigator/rtl_mission_fast.h +++ b/src/modules/navigator/rtl_mission_fast.h @@ -52,7 +52,7 @@ class Navigator; class RtlMissionFast : public RtlBase { public: - RtlMissionFast(Navigator *navigator); + RtlMissionFast(Navigator *navigator, mission_s mission); ~RtlMissionFast() = default; void on_activation() override; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index d78f6c7fa627..eb5f3d102246 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -46,10 +46,10 @@ static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5; -RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : +RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator, mission_s mission) : RtlBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE) { - + _mission = mission; } void RtlMissionFastReverse::on_inactive() diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index 168d20c2e341..9a30e6d70de5 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -52,7 +52,7 @@ class Navigator; class RtlMissionFastReverse : public RtlBase { public: - RtlMissionFastReverse(Navigator *navigator); + RtlMissionFastReverse(Navigator *navigator, mission_s mission); ~RtlMissionFastReverse() = default; void on_activation() override; diff --git a/src/modules/simulation/simulator_mavlink/CMakeLists.txt b/src/modules/simulation/simulator_mavlink/CMakeLists.txt index fc4fa94aa4b4..f5f39d7106b2 100644 --- a/src/modules/simulation/simulator_mavlink/CMakeLists.txt +++ b/src/modules/simulation/simulator_mavlink/CMakeLists.txt @@ -40,9 +40,7 @@ px4_add_module( -Wno-address-of-packed-member # TODO: fix in c_library_v2 INCLUDES ${CMAKE_BINARY_DIR}/mavlink - ${CMAKE_BINARY_DIR}/mavlink/development - ${CMAKE_BINARY_DIR}/mavlink/common - ${CMAKE_BINARY_DIR}/mavlink/standard + ${CMAKE_BINARY_DIR}/mavlink/${CONFIG_MAVLINK_DIALECT} SRCS SimulatorMavlink.cpp SimulatorMavlink.hpp diff --git a/src/systemcmds/param/param.cpp b/src/systemcmds/param/param.cpp index f1f355514c86..8194cfffa5ab 100644 --- a/src/systemcmds/param/param.cpp +++ b/src/systemcmds/param/param.cpp @@ -74,13 +74,6 @@ enum class COMPARE_ERROR_LEVEL { SILENT = 1, }; - -#ifdef __PX4_QURT -#define PARAM_PRINT PX4_INFO -#else -#define PARAM_PRINT PX4_INFO_RAW -#endif - static int do_save(const char *param_file_name); static int do_save_default(); static int do_load(const char *param_file_name); @@ -515,10 +508,10 @@ do_save_default() static int do_show(const char *search_string, bool only_changed) { - PARAM_PRINT("Symbols: x = used, + = saved, * = unsaved\n"); + PX4_INFO_RAW("Symbols: x = used, + = saved, * = unsaved\n"); // also show unused params if we show non-default values only param_foreach(do_show_print, (char *)search_string, only_changed, !only_changed); - PARAM_PRINT("\n %u/%u parameters used.\n", param_count_used(), param_count()); + PX4_INFO_RAW("\n %u/%u parameters used.\n", param_count_used(), param_count()); return 0; } @@ -530,7 +523,7 @@ do_show_for_airframe() int32_t sys_autostart = 0; param_get(param_find("SYS_AUTOSTART"), &sys_autostart); if (sys_autostart != 0) { - PARAM_PRINT("# Make sure to add all params from the current airframe (ID=%" PRId32 ") as well\n", sys_autostart); + PX4_INFO_RAW("# Make sure to add all params from the current airframe (ID=%" PRId32 ") as well\n", sys_autostart); } return 0; } @@ -538,9 +531,9 @@ do_show_for_airframe() static int do_show_all() { - PARAM_PRINT("Symbols: x = used, + = saved, * = unsaved\n"); + PX4_INFO_RAW("Symbols: x = used, + = saved, * = unsaved\n"); param_foreach(do_show_print, nullptr, false, false); - PARAM_PRINT("\n %u parameters total, %u used.\n", param_count(), param_count_used()); + PX4_INFO_RAW("\n %u parameters total, %u used.\n", param_count(), param_count_used()); return 0; } @@ -560,14 +553,14 @@ do_show_quiet(const char *param_name) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &ii)) { - PARAM_PRINT("%ld", (long)ii); + PX4_INFO_RAW("%ld\n", (long)ii); } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &ff)) { - PARAM_PRINT("%4.4f", (double)ff); + PX4_INFO_RAW("%4.4f\n", (double)ff); } break; @@ -589,7 +582,7 @@ do_find(const char *name) return 1; } - PARAM_PRINT("Found param %s at index %i\n", name, (int)ret); + PX4_INFO_RAW("Found param %s at index %i\n", name, (int)ret); return 0; } @@ -614,27 +607,27 @@ do_show_index(const char *index, bool used_index) return 1; } - PARAM_PRINT("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '), + PX4_INFO_RAW("index %d: %c %c %s [%d,%d] : ", i, (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param), param_get_used_index(param), param_get_index(param)); switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &ii)) { - PARAM_PRINT("%ld\n", (long)ii); + PX4_INFO_RAW("%ld\n", (long)ii); } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &ff)) { - PARAM_PRINT("%4.4f\n", (double)ff); + PX4_INFO_RAW("%4.4f\n", (double)ff); } break; default: - PARAM_PRINT("\n", 0 + param_type(param)); + PX4_INFO_RAW("\n", 0 + param_type(param)); } return 0; @@ -684,7 +677,7 @@ do_show_print(void *arg, param_t param) } } - PARAM_PRINT("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '), + PX4_INFO_RAW("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param), param_get_used_index(param), param_get_index(param)); @@ -695,7 +688,7 @@ do_show_print(void *arg, param_t param) switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { - PARAM_PRINT("%ld\n", (long)i); + PX4_INFO_RAW("%ld\n", (long)i); return; } @@ -703,18 +696,18 @@ do_show_print(void *arg, param_t param) case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { - PARAM_PRINT("%4.4f\n", (double)f); + PX4_INFO_RAW("%4.4f\n", (double)f); return; } break; default: - PARAM_PRINT("\n", 0 + param_type(param)); + PX4_INFO_RAW("\n", 0 + param_type(param)); return; } - PARAM_PRINT("\n", (unsigned long)param); + PX4_INFO_RAW("\n", (unsigned long)param); } static void @@ -739,12 +732,12 @@ do_show_print_for_airframe(void *arg, param_t param) int32_t i; float f; - PARAM_PRINT("param set-default %s ", p_name); + PX4_INFO_RAW("param set-default %s ", p_name); switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { - PARAM_PRINT("%ld\n", (long)i); + PX4_INFO_RAW("%ld\n", (long)i); return; } @@ -752,7 +745,7 @@ do_show_print_for_airframe(void *arg, param_t param) case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { - PARAM_PRINT("%4.4f\n", (double)f); + PX4_INFO_RAW("%4.4f\n", (double)f); return; } @@ -762,7 +755,7 @@ do_show_print_for_airframe(void *arg, param_t param) return; } - PARAM_PRINT("\n", (unsigned long)param); + PX4_INFO_RAW("\n", (unsigned long)param); } static int @@ -792,12 +785,12 @@ do_set(const char *name, const char *val, bool fail_on_not_found) int32_t newval = strtol(val, &end, 10); if (i != newval) { - PARAM_PRINT("%c %s: ", + PX4_INFO_RAW("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); - PARAM_PRINT("curr: %ld", (long)i); + PX4_INFO_RAW("curr: %ld", (long)i); param_set(param, &newval); - PARAM_PRINT(" -> new: %ld\n", (long)newval); + PX4_INFO_RAW(" -> new: %ld\n", (long)newval); } } @@ -814,12 +807,12 @@ do_set(const char *name, const char *val, bool fail_on_not_found) if (f != newval) { #pragma GCC diagnostic pop - PARAM_PRINT("%c %s: ", + PX4_INFO_RAW("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); - PARAM_PRINT("curr: %4.4f", (double)f); + PX4_INFO_RAW("curr: %4.4f", (double)f); param_set(param, &newval); - PARAM_PRINT(" -> new: %4.4f\n", (double)newval); + PX4_INFO_RAW(" -> new: %4.4f\n", (double)newval); } } diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index f3ba47f87176..730aa1d46052 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -96,7 +96,7 @@ void AutopilotTester::wait_until_ready() // Wait until we can arm CHECK(poll_condition_with_timeout( - [this]() { return _telemetry->health().is_armable; }, std::chrono::seconds(20))); + [this]() { return _telemetry->health().is_armable; }, std::chrono::seconds(45))); } void AutopilotTester::store_home()