diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index bb2d3bc2f6787..7532fa6108496 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -13,62 +13,108 @@ static void failsafe_check_static() copter.failsafe_check(); } +#include +// #define GOT_HERE() do { DEV_PRINTF("got to %s:%d\n", __FILE__, __LINE__); qurt_timer_sleep(5000); } while(false); +#define GOT_HERE() do {} while(0) + + void Copter::init_ardupilot() { + GOT_HERE(); + // init winch #if AP_WINCH_ENABLED g2.winch.init(); #endif + GOT_HERE(); + // initialise notify system notify.init(); + + GOT_HERE(); + notify_flight_mode(); + GOT_HERE(); + // initialise battery monitor battery.init(); + GOT_HERE(); + // Init RSSI rssi.init(); - + + GOT_HERE(); + + // Any use of barometer crashes the DSP barometer.init(); + GOT_HERE(); + // setup telem slots with serial ports gcs().setup_uarts(); +GOT_HERE(); + #if OSD_ENABLED == ENABLED osd.init(); #endif +GOT_HERE(); + // update motor interlock state update_using_interlock(); +GOT_HERE(); + #if FRAME_CONFIG == HELI_FRAME // trad heli specific initialisation heli_init(); #endif + +GOT_HERE(); + #if FRAME_CONFIG == HELI_FRAME input_manager.set_loop_rate(scheduler.get_loop_rate_hz()); #endif +GOT_HERE(); + init_rc_in(); // sets up rc channels from radio +GOT_HERE(); + // initialise surface to be tracked in SurfaceTracking // must be before rc init to not override initial switch position surface_tracking.init((SurfaceTracking::Surface)copter.g2.surftrak_mode.get()); +GOT_HERE(); + // allocate the motors class allocate_motors(); +GOT_HERE(); + // initialise rc channels including setting mode rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM_AIRMODE); +GOT_HERE(); + rc().init(); +GOT_HERE(); + // sets up motors and output to escs init_rc_out(); +GOT_HERE(); + // check if we should enter esc calibration mode esc_calibration_startup_check(); +GOT_HERE(); + // motors initialised so parameters can be sent ap.initialised_params = true; @@ -76,51 +122,77 @@ void Copter::init_ardupilot() relay.init(); #endif +GOT_HERE(); + /* * setup the 'main loop is dead' check. Note that this relies on * the RC library being initialised. */ hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000); +GOT_HERE(); + // Do GPS init gps.set_log_gps_bit(MASK_LOG_GPS); +GOT_HERE(); + gps.init(); +GOT_HERE(); + AP::compass().set_log_bit(MASK_LOG_COMPASS); +GOT_HERE(); + AP::compass().init(); #if AP_AIRSPEED_ENABLED +GOT_HERE(); + airspeed.set_log_bit(MASK_LOG_IMU); #endif #if AP_OAPATHPLANNER_ENABLED +GOT_HERE(); + g2.oa.init(); #endif +GOT_HERE(); + attitude_control->parameter_sanity_check(); #if AP_OPTICALFLOW_ENABLED // initialise optical flow sensor +GOT_HERE(); + optflow.init(MASK_LOG_OPTFLOW); #endif // AP_OPTICALFLOW_ENABLED #if HAL_MOUNT_ENABLED // initialise camera mount +GOT_HERE(); + camera_mount.init(); #endif #if AP_CAMERA_ENABLED // initialise camera +GOT_HERE(); + camera.init(); #endif #if AC_PRECLAND_ENABLED // initialise precision landing +GOT_HERE(); + init_precland(); #endif #if AP_LANDINGGEAR_ENABLED // initialise landing gear position +GOT_HERE(); + landinggear.init(); #endif @@ -130,67 +202,95 @@ void Copter::init_ardupilot() // read Baro pressure at ground //----------------------------- +GOT_HERE(); + + // Any use of barometer crashes the DSP barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); #if RANGEFINDER_ENABLED == ENABLED // initialise rangefinder +GOT_HERE(); + init_rangefinder(); #endif #if HAL_PROXIMITY_ENABLED // init proximity sensor +GOT_HERE(); + g2.proximity.init(); #endif #if AP_BEACON_ENABLED // init beacons used for non-gps position estimation +GOT_HERE(); + g2.beacon.init(); #endif #if AP_RPM_ENABLED +GOT_HERE(); + // initialise AP_RPM library rpm_sensor.init(); #endif #if MODE_AUTO_ENABLED == ENABLED +GOT_HERE(); + // initialise mission library mode_auto.mission.init(); #endif #if MODE_SMARTRTL_ENABLED == ENABLED +GOT_HERE(); + // initialize SmartRTL g2.smart_rtl.init(); #endif #if HAL_LOGGING_ENABLED +GOT_HERE(); + // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); #endif + GOT_HERE(); + startup_INS_ground(); #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED + GOT_HERE(); + custom_control.init(); #endif // set landed flags set_land_complete(true); +GOT_HERE(); + set_land_complete_maybe(true); +GOT_HERE(); // enable CPU failsafe failsafe_enable(); +GOT_HERE(); ins.set_log_raw_bit(MASK_LOG_IMU_RAW); +GOT_HERE(); motors->output_min(); // output lowest possible value to motors // attempt to set the initial_mode, else set to STABILIZE +GOT_HERE(); if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { // set mode to STABILIZE will trigger mode change notification to pilot set_mode(Mode::Number::STABILIZE, ModeReason::UNAVAILABLE); } +GOT_HERE(); pos_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); vel_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); hgt_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); @@ -209,11 +309,16 @@ void Copter::startup_INS_ground() ahrs.init(); ahrs.set_vehicle_class(AP_AHRS::VehicleClass::COPTER); + GOT_HERE(); + // Warm up and calibrate gyro offsets ins.init(scheduler.get_loop_rate_hz()); + GOT_HERE(); + // reset ahrs including gyro bias ahrs.reset(); + GOT_HERE(); } // position_ok - returns true if the horizontal absolute position is ok and home position is set diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index d20eef7e50c60..cc81bc33a14c3 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -16,6 +16,13 @@ '*.cpp', ] +COMMON_VEHICLE_DEPENDENT_CAN_LIBRARIES = [ + 'AP_CANManager', + 'AP_KDECAN', + 'AP_PiccoloCAN', + 'AP_PiccoloCAN/piccolo_protocol', +] + COMMON_VEHICLE_DEPENDENT_LIBRARIES = [ 'AP_Airspeed', 'AP_AccelCal', @@ -26,7 +33,6 @@ 'AP_BattMonitor', 'AP_BoardConfig', 'AP_Camera', - 'AP_CANManager', 'AP_Common', 'AP_Compass', 'AP_Declination', @@ -34,7 +40,6 @@ 'AP_HAL', 'AP_HAL_Empty', 'AP_InertialSensor', - 'AP_KDECAN', 'AP_Math', 'AP_Mission', 'AP_DAL', @@ -73,8 +78,6 @@ 'AP_SBusOut', 'AP_IOMCU', 'AP_Parachute', - 'AP_PiccoloCAN', - 'AP_PiccoloCAN/piccolo_protocol', 'AP_RAMTRON', 'AP_RCProtocol', 'AP_Radio', @@ -248,11 +251,8 @@ def ap_get_all_libraries(bld): def ap_common_vehicle_libraries(bld): libraries = COMMON_VEHICLE_DEPENDENT_LIBRARIES - if bld.env.DEST_BINFMT == 'pe': - libraries += [ - 'AC_Fence', - 'AC_AttitudeControl', - ] + if bld.env.with_can: + libraries.extend(COMMON_VEHICLE_DEPENDENT_CAN_LIBRARIES) return libraries diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index b25591523afaf..32b3818f124d5 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -38,7 +38,10 @@ def __init__(self): def configure(self, cfg): cfg.env.TOOLCHAIN = cfg.options.toolchain or self.toolchain cfg.env.ROMFS_FILES = [] - cfg.load('toolchain') + if hasattr(self,'configure_toolchain'): + self.configure_toolchain(cfg) + else: + cfg.load('toolchain') cfg.load('cxx_checks') env = waflib.ConfigSet.ConfigSet() @@ -190,6 +193,8 @@ def srcpath(path): cfg.msg("Configured VSCode Intellisense:", 'no', color='YELLOW') def cc_version_gte(self, cfg, want_major, want_minor): + if cfg.env.TOOLCHAIN == "custom": + return True (major, minor, patchlevel) = cfg.env.CC_VERSION return (int(major) > want_major or (int(major) == want_major and int(minor) >= want_minor)) @@ -199,6 +204,8 @@ def configure_env(self, cfg, env): # make easy to override them. Convert back to list before consumption. env.DEFINES = {} + env.with_can = self.with_can + # potentially set extra defines from an environment variable: if cfg.options.define is not None: for (n, v) in [d.split("=") for d in cfg.options.define]: @@ -1618,3 +1625,96 @@ class SITL_x86_64_linux_gnu(SITL_static): class SITL_arm_linux_gnueabihf(SITL_static): toolchain = 'arm-linux-gnueabihf' + +class QURT(Board): + '''support for QURT based boards''' + toolchain = 'custom' + + def __init__(self): + self.with_can = False + + def configure_toolchain(self, cfg): + cfg.env.CXX_NAME = 'gcc' + cfg.env.HEXAGON_SDK_DIR = "/opt/hexagon-sdk/4.1.0.4-lite" + cfg.env.CC_VERSION = ('4','1','0') + cfg.env.TOOLCHAIN_DIR = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools" + cfg.env.COMPILER_CC = cfg.env.TOOLCHAIN_DIR + "/bin/hexagon-clang" + cfg.env.COMPILER_CXX = cfg.env.TOOLCHAIN_DIR + "/bin/hexagon-clang++" + cfg.env.LINK_CXX = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools/bin/hexagon-link" + cfg.env.CXX = ["ccache", cfg.env.COMPILER_CXX] + cfg.env.CC = ["ccache", cfg.env.COMPILER_CC] + cfg.env.CXX_TGT_F = ['-c', '-o'] + cfg.env.CC_TGT_F = ['-c', '-o'] + cfg.env.CCLNK_SRC_F = [] + cfg.env.CXXLNK_SRC_F = [] + cfg.env.CXXLNK_TGT_F = ['-o'] + cfg.env.CCLNK_TGT_F = ['-o'] + cfg.env.CPPPATH_ST = '-I%s' + cfg.env.DEFINES_ST = '-D%s' + cfg.env.AR = cfg.env.HEXAGON_SDK_DIR + "/tools/HEXAGON_Tools/8.4.05/Tools/bin/hexagon-ar" + cfg.env.ARFLAGS = 'rcs' + cfg.env.cxxstlib_PATTERN = 'lib%s.a' + cfg.env.cstlib_PATTERN = 'lib%s.a' + cfg.env.LIBPATH_ST = '-L%s' + cfg.env.LIB_ST = '-l%s' + cfg.env.SHLIB_MARKER = '' + cfg.env.STLIBPATH_ST = '-L%s' + cfg.env.STLIB_MARKER = '' + cfg.env.STLIB_ST = '-l%s' + cfg.env.LDFLAGS = [ + '-lgcc', + cfg.env.TOOLCHAIN_DIR + '/target/hexagon/lib/v66/G0/pic/finiS.o' + ] + + def configure_env(self, cfg, env): + super(QURT, self).configure_env(cfg, env) + + env.BOARD_CLASS = "QURT" + env.HEXAGON_APP = "libardupilot.so" + env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/qurt"] + env.INCLUDES += [cfg.env.HEXAGON_SDK_DIR + "/rtos/qurt/computev66/include/posix"] + env.CXXFLAGS += ["-fPIC", "-MD"] + env.CFLAGS += ["-fPIC", "-MD"] + + env.DEFINES.update( + CONFIG_HAL_BOARD = 'HAL_BOARD_QURT', + CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_NONE', + AP_SIM_ENABLED = 0, + ) + + env.LINKFLAGS = [ + "-march=hexagon", + "-mcpu=hexagonv66", + "-shared", + "-call_shared", + "-G0", + cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/initS.o", + f"-L{cfg.env.TOOLCHAIN_DIR}/target/hexagon/lib/v66/G0/pic", + "-Bsymbolic", + cfg.env.TOOLCHAIN_DIR + "/target/hexagon/lib/v66/G0/pic/libgcc.a", + "--wrap=malloc", + "--wrap=calloc", + "--wrap=free", + "--wrap=realloc", + "--wrap=memalign", + "--wrap=printf", + "--wrap=__stack_chk_fail", + "-lc" + ] + + if not cfg.env.DEBUG: + env.CXXFLAGS += [ + '-O2', + ] + + env.AP_LIBRARIES += [ + 'AP_HAL_QURT', + ] + + def build(self, bld): + super(QURT, self).build(bld) + + def get_name(self): + # get name of class + return self.__class__.__name__ + diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 26b8c0e313fd4..4f1e7a8626d0c 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -261,8 +261,13 @@ def sign_firmware(image, private_keyfile): try: import monocypher except ImportError: - Logs.error("Please install monocypher with: python3 -m pip install pymonocypher") + Logs.error("Please install monocypher with: python3 -m pip install pymonocypher==3.1.3.2") return None + + if monocypher.__version__ != "3.1.3.2": + Logs.error("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") + return None + try: key = open(private_keyfile, 'r').read() except Exception as ex: diff --git a/Tools/ardupilotwaf/toolchain.py b/Tools/ardupilotwaf/toolchain.py index eb86084307cb2..5fec76f829552 100644 --- a/Tools/ardupilotwaf/toolchain.py +++ b/Tools/ardupilotwaf/toolchain.py @@ -133,6 +133,8 @@ def configure(cfg): _filter_supported_cxx_compilers('g++', 'clang++') cfg.msg('Using toolchain', cfg.env.TOOLCHAIN) + if cfg.env.TOOLCHAIN == "custom": + return if cfg.env.TOOLCHAIN == 'native': cfg.load('compiler_cxx compiler_c') diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index 07a0e13b43654..ebc16cf3b2f53 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -465,6 +465,47 @@ def progress_text(self, current_value): return (f"Want=({self.target.lat},{self.target.lng}) distance={self.horizontal_error(current_value)}") +class WaitAndMaintainEKFFlags(WaitAndMaintain): + '''Waits for EKF status flags to include required_flags and have + error_bits *not* set.''' + def __init__(self, test_suite, required_flags, error_bits, **kwargs): + super(WaitAndMaintainEKFFlags, self).__init__(test_suite, **kwargs) + self.required_flags = required_flags + self.error_bits = error_bits + self.last_EKF_STATUS_REPORT = None + + def announce_start_text(self): + return f"Waiting for EKF value {self.required_flags}" + + def get_current_value(self): + self.last_EKF_STATUS_REPORT = self.test_suite.assert_receive_message('EKF_STATUS_REPORT', timeout=10) + return self.last_EKF_STATUS_REPORT.flags + + def validate_value(self, value): + if value & self.error_bits: + return False + + if (value & self.required_flags) != self.required_flags: + return False + + return True + + def success_text(self): + return "EKF Flags OK" + + def timeoutexception(self): + self.progress("Last EKF status report:") + self.progress(self.test_suite.dump_message_verbose(self.last_EKF_STATUS_REPORT)) + + return AutoTestTimeoutException(f"Failed to get EKF.flags={self.required_flags}") + + def progress_text(self, current_value): + error_bits_str = "" + if current_value & self.error_bits: + error_bits_str = " (error bits present)" + return (f"Want=({self.required_flags}) got={current_value}{error_bits_str}") + + class WaitAndMaintainArmed(WaitAndMaintain): def get_current_value(self): return self.test_suite.armed() @@ -7994,8 +8035,10 @@ def wait_heartbeat(self, drain_mav=True, quiet=False, *args, **x): if m.get_srcSystem() == self.sysid_thismav(): return m - def wait_ekf_happy(self, timeout=45, require_absolute=True): + def wait_ekf_happy(self, require_absolute=True, **kwargs): """Wait for EKF to be happy""" + if "timeout" not in kwargs: + kwargs["timeout"] = 45 """ if using SITL estimates directly """ if (int(self.get_parameter('AHRS_EKF_TYPE')) == 10): @@ -8015,35 +8058,10 @@ def wait_ekf_happy(self, timeout=45, require_absolute=True): mavutil.mavlink.ESTIMATOR_POS_VERT_ABS | mavutil.mavlink.ESTIMATOR_PRED_POS_HORIZ_ABS) error_bits |= mavutil.mavlink.ESTIMATOR_GPS_GLITCH - self.wait_ekf_flags(required_value, error_bits, timeout=timeout) + WaitAndMaintainEKFFlags(self, required_value, error_bits, **kwargs).run() - def wait_ekf_flags(self, required_value, error_bits, timeout=30): - self.progress("Waiting for EKF value %u" % required_value) - last_print_time = 0 - tstart = self.get_sim_time() - m = None - while timeout is None or self.get_sim_time_cached() < tstart + timeout: - m = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=timeout) - if m is None: - continue - current = m.flags - errors = current & error_bits - everything_ok = (errors == 0 and - current & required_value == required_value) - if everything_ok or self.get_sim_time_cached() - last_print_time > 1: - self.progress("Wait EKF.flags: required:%u current:%u errors=%u" % - (required_value, current, errors)) - last_print_time = self.get_sim_time_cached() - if everything_ok: - self.progress("EKF Flags OK") - return True - m_str = str(m) - if m is not None: - m_str = self.dump_message_verbose(m) - self.progress("Last EKF_STATUS_REPORT message:") - self.progress(m_str) - raise AutoTestTimeoutException("Failed to get EKF.flags=%u" % - required_value) + def wait_ekf_flags(self, required_value, error_bits, **kwargs): + WaitAndMaintainEKFFlags(self, required_value, error_bits, **kwargs).run() def wait_gps_disable(self, position_horizontal=True, position_vertical=False, timeout=30): """Disable GPS and wait for EKF to report the end of assistance from GPS.""" diff --git a/Tools/scripts/signing/README.md b/Tools/scripts/signing/README.md index dc4032c00987f..06fe1d6527b29 100644 --- a/Tools/scripts/signing/README.md +++ b/Tools/scripts/signing/README.md @@ -12,7 +12,7 @@ firmware doesn't match any of the public keys in the bootloader. To generate a public/private key pair, run the following command: ``` - python3 -m pip install pymonocypher + python3 -m pip install pymonocypher==3.1.3.2 Tools/scripts/signing/generate_keys.py NAME ``` @@ -131,7 +131,13 @@ This opens a secure command session using your private_key.dat file to allow the securecommand getpublickeys will return the number of public keys...you will need this next securecommand removepublickeys 0 X where X is the number of public keys...this removes them ``` -Re-run the 'getpublickeys' command again to verify that all keys have been removed. + +For example, if you have a standard firmware with the 3 ArduPilot +public keys and one of your own public keys then X will be 4 in the +above command. + +Re-run the 'getpublickeys' command again to verify that all keys have +been removed. Now exit MAVProxy and build a firmware using the normal bootloader but still using the --signed-fw option: diff --git a/Tools/scripts/signing/generate_keys.py b/Tools/scripts/signing/generate_keys.py index 5da4388c308f9..38b7d38650452 100755 --- a/Tools/scripts/signing/generate_keys.py +++ b/Tools/scripts/signing/generate_keys.py @@ -9,9 +9,12 @@ try: import monocypher except ImportError: - print("Please install monocypher with: python3 -m pip install pymonocypher") + print("Please install monocypher with: python3 -m pip install pymonocypher==3.1.3.2") sys.exit(1) +if monocypher.__version__ != "3.1.3.2": + Logs.error("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") + sys.exit(1) if len(sys.argv) != 2: print("Usage: generate_keys.py BASENAME") diff --git a/Tools/scripts/signing/make_secure_bl.py b/Tools/scripts/signing/make_secure_bl.py index bd60bee3c7af0..5187b0f15d3b1 100755 --- a/Tools/scripts/signing/make_secure_bl.py +++ b/Tools/scripts/signing/make_secure_bl.py @@ -7,12 +7,6 @@ import os import base64 -try: - import monocypher -except ImportError: - print("Please install monocypher with: python3 -m pip install pymonocypher") - sys.exit(1) - # get command line arguments from argparse import ArgumentParser parser = ArgumentParser(description='make_secure_bl') diff --git a/Tools/scripts/signing/make_secure_fw.py b/Tools/scripts/signing/make_secure_fw.py index 4e916090fc227..8c8862cb62f8c 100755 --- a/Tools/scripts/signing/make_secure_fw.py +++ b/Tools/scripts/signing/make_secure_fw.py @@ -10,9 +10,13 @@ try: import monocypher except ImportError: - print("Please install monocypher with: python3 -m pip install pymonocypher") + print("Please install monocypher with: python3 -m pip install pymonocypher==3.1.3.2") sys.exit(1) +if monocypher.__version__ != "3.1.3.2": + Logs.error("must use monocypher 3.1.3.2, please run: python3 -m pip install pymonocypher==3.1.3.2") + return None + key_len = 32 sig_len = 64 sig_version = 30437 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index b26bdeb472714..74e3543297cbd 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -34,6 +34,9 @@ #include #include +#include +#define GOT_HERE() do { DEV_PRINTF("got to %s:%d\n", __FILE__, __LINE__); qurt_timer_sleep(5000); } while(false); + extern const AP_HAL::HAL& hal; // this is the speed in m/s above which we first get a yaw lock with @@ -177,12 +180,15 @@ void AP_AHRS_DCM::matrix_update(void) void AP_AHRS_DCM::reset(bool recover_eulers) { + GOT_HERE(); + // reset the integration terms _omega_I.zero(); _omega_P.zero(); _omega_yaw_P.zero(); _omega.zero(); + GOT_HERE(); // if the caller wants us to try to recover to the current // attitude then calculate the dcm matrix from the current // roll/pitch/yaw values @@ -205,14 +211,6 @@ AP_AHRS_DCM::reset(bool recover_eulers) // Get body frame accel vector Vector3f initAccVec = _ins.get_accel(); - uint8_t counter = 0; - - // the first vector may be invalid as the filter starts up - while ((initAccVec.length() < 9.0f || initAccVec.length() > 11) && counter++ < 20) { - _ins.wait_for_sample(); - _ins.update(); - initAccVec = _ins.get_accel(); - } // normalise the acceleration vector if (initAccVec.length() > 5.0f) { @@ -228,12 +226,14 @@ AP_AHRS_DCM::reset(bool recover_eulers) _dcm_matrix.from_euler(roll, pitch, 0); } + GOT_HERE(); // pre-calculate some trig for CPU purposes: _cos_yaw = cosf(yaw); _sin_yaw = sinf(yaw); _last_startup_ms = AP_HAL::millis(); + GOT_HERE(); } /* diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index baca15dee2d20..591426b8a3f58 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -17,6 +17,7 @@ #if AP_BARO_SPL06_ENABLED #include +#include #include extern const AP_HAL::HAL &hal; diff --git a/libraries/AP_Baro/AP_Baro_atmosphere.cpp b/libraries/AP_Baro/AP_Baro_atmosphere.cpp index 63667432e8f45..da81e1033d280 100644 --- a/libraries/AP_Baro/AP_Baro_atmosphere.cpp +++ b/libraries/AP_Baro/AP_Baro_atmosphere.cpp @@ -314,6 +314,7 @@ float AP_Baro::_get_EAS2TAS(void) const #endif } +#if AP_SIM_ENABLED // lookup expected temperature in degrees C for a given altitude. Used for SITL backend float AP_Baro::get_temperatureC_for_alt_amsl(const float alt_amsl) { @@ -329,6 +330,7 @@ float AP_Baro::get_pressure_for_alt_amsl(const float alt_amsl) get_pressure_temperature_for_alt_amsl(alt_amsl, pressure, temp_K); return pressure; } +#endif // AP_SIM_ENABLED /* return sea level pressure given a current altitude and pressure reading diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp index e60b8e04b1479..a7632e2a0ab27 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp +++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp @@ -439,6 +439,23 @@ bool AP_BoardConfig::_in_error_loop; void AP_BoardConfig::throw_error(const char *err_type, const char *fmt, va_list arg) { + HAP_PRINTF("throw_error: %s", err_type); + qurt_timer_sleep(5000); + uint32_t last_ms = AP_HAL::millis(); + uint32_t last_us = AP_HAL::micros(); + while (true) { + qurt_timer_sleep(1000000); + uint32_t now_ms = AP_HAL::millis(); + uint32_t now_us = AP_HAL::micros(); + uint32_t dt_ms = now_ms - last_ms; + uint32_t dt_us = now_us - last_us; + HAP_PRINTF("tick! %u %u dt_ms=%u dt_us=%u\n", + unsigned(now_ms), unsigned(now_us), + unsigned(dt_ms), unsigned(dt_us)); + last_ms = now_ms; + last_us = now_us; + } + _in_error_loop = true; /* to give the user the opportunity to connect to USB we keep diff --git a/libraries/AP_Common/AP_Common.cpp b/libraries/AP_Common/AP_Common.cpp index 19f2002ac4121..ad1910b72e9d8 100644 --- a/libraries/AP_Common/AP_Common.cpp +++ b/libraries/AP_Common/AP_Common.cpp @@ -19,6 +19,7 @@ #include #include "AP_Common.h" +#include extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp index b9804818e85cb..9e282504992ee 100644 --- a/libraries/AP_Common/c++.cpp +++ b/libraries/AP_Common/c++.cpp @@ -7,21 +7,103 @@ #include #include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include + +/* + These are the heap access function exported by the SLPI DSP image for Qurt OS. +*/ +extern "C" { + void *fc_heap_alloc(size_t size); + void fc_heap_free(void* ptr); + size_t fc_heap_size(void); + size_t fc_heap_usage(void); +} + +void *operator new (size_t size, std::nothrow_t const ¬hrow) noexcept +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +void *operator new[](size_t size, std::nothrow_t const ¬hrow) noexcept +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +/* + These variants are for new without std::nothrow. We don't want to ever + use these from ardupilot code + */ +void *operator new (size_t size) +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + + +void *operator new[](size_t size) +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +/* + Override delete to free up memory to correct heap +*/ + +// extern const AP_HAL::HAL& hal; + +void operator delete (void *p) noexcept +{ + if (p) { + fc_heap_free(p); + } + + // DEV_PRINTF("Heap size: %u, heap usage: %u", fc_heap_size(), fc_heap_usage()); +} + +void operator delete[](void *ptr) noexcept +{ + if (ptr) { + fc_heap_free(ptr); + } +} + +#else + /* globally override new and delete to ensure that we always start with zero memory. This ensures consistent behaviour. */ -void * operator new(size_t size) +void * operator new (size_t size) { if (size < 1) { size = 1; } - return(calloc(size, 1)); + return (calloc(size, 1)); } -void operator delete(void *p) +void operator delete (void *p) { - if (p) free(p); + if (p) { + free(p); + } } void * operator new[](size_t size) @@ -29,10 +111,14 @@ void * operator new[](size_t size) if (size < 1) { size = 1; } - return(calloc(size, 1)); + return (calloc(size, 1)); } void operator delete[](void * ptr) { - if (ptr) free(ptr); + if (ptr) { + free(ptr); + } } + +#endif diff --git a/libraries/AP_Compass/AP_Compass_IST8308.cpp b/libraries/AP_Compass/AP_Compass_IST8308.cpp index c4c3ea499feec..256a106a80877 100644 --- a/libraries/AP_Compass/AP_Compass_IST8308.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8308.cpp @@ -168,7 +168,7 @@ bool AP_Compass_IST8308::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); diff --git a/libraries/AP_Compass/AP_Compass_IST8310.cpp b/libraries/AP_Compass/AP_Compass_IST8310.cpp index c2f368ce58a1e..3fb9baf2e7bbe 100644 --- a/libraries/AP_Compass/AP_Compass_IST8310.cpp +++ b/libraries/AP_Compass/AP_Compass_IST8310.cpp @@ -178,7 +178,7 @@ bool AP_Compass_IST8310::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); diff --git a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp index 713ff00befe2f..828c9cf632ec1 100644 --- a/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp +++ b/libraries/AP_Compass/AP_Compass_LIS3MDL.cpp @@ -113,7 +113,7 @@ bool AP_Compass_LIS3MDL::init() } set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a LIS3MDL on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); + printf("Found a LIS3MDL on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); set_rotation(compass_instance, rotation); diff --git a/libraries/AP_Compass/AP_Compass_MMC3416.cpp b/libraries/AP_Compass/AP_Compass_MMC3416.cpp index b8003869c599d..f069a822f5ea1 100644 --- a/libraries/AP_Compass/AP_Compass_MMC3416.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC3416.cpp @@ -99,7 +99,7 @@ bool AP_Compass_MMC3416::init() set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a MMC3416 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); + printf("Found a MMC3416 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); set_rotation(compass_instance, rotation); diff --git a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp index d671f3deb5b79..79db90cf8faae 100644 --- a/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp +++ b/libraries/AP_Compass/AP_Compass_MMC5xx3.cpp @@ -115,7 +115,7 @@ bool AP_Compass_MMC5XX3::init() set_dev_id(compass_instance, dev->get_bus_id()); - printf("Found a MMC5983 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance); + printf("Found a MMC5983 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), compass_instance); set_rotation(compass_instance, rotation); diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp index ad1bea49892a3..d5b81ec4a4640 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp @@ -123,7 +123,7 @@ bool AP_Compass_QMC5883L::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); diff --git a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp index 80d6e46f1c699..7bc8293820a5f 100644 --- a/libraries/AP_Compass/AP_Compass_QMC5883P.cpp +++ b/libraries/AP_Compass/AP_Compass_QMC5883P.cpp @@ -134,7 +134,7 @@ bool AP_Compass_QMC5883P::init() set_dev_id(_instance, _dev->get_bus_id()); printf("%s found on bus %u id %u address 0x%02x\n", name, - _dev->bus_num(), _dev->get_bus_id(), _dev->get_bus_address()); + _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address()); set_rotation(_instance, _rotation); diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 63f575bb5be2d..2fabb7b3a6cb3 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -16,6 +16,9 @@ #include "AP_Filesystem.h" #include "AP_Filesystem_config.h" + +#if AP_FILESYSTEM_FILE_READING_ENABLED + #include #include #include @@ -202,7 +205,9 @@ AP_Filesystem::DirHandle *AP_Filesystem::opendir(const char *pathname) (strlen(pathname) == 1 && pathname[0] == '/')) { virtual_dirent.backend_ofs = 0; virtual_dirent.d_off = 0; +#if AP_FILESYSTEM_HAVE_DIRENT_DTYPE virtual_dirent.de.d_type = DT_DIR; +#endif } else { virtual_dirent.backend_ofs = 255; } @@ -418,3 +423,4 @@ AP_Filesystem &FS() } } +#endif // AP_FILESYSTEM_FILE_READING_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index c2bd20e765563..cc8619ca52152 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -54,7 +54,7 @@ struct dirent { #define AP_FILESYSTEM_FORMAT_ENABLED 1 #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_QURT #include "AP_Filesystem_posix.h" #endif diff --git a/libraries/AP_Filesystem/AP_Filesystem_QURT.h b/libraries/AP_Filesystem/AP_Filesystem_QURT.h new file mode 100644 index 0000000000000..b1e618e41d96a --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_QURT.h @@ -0,0 +1,12 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "AP_Filesystem_backend.h" diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp index 9eadd7f607b40..d673ee71d0f01 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp @@ -244,7 +244,9 @@ struct dirent *AP_Filesystem_Sys::readdir(void *dirp) // we have reached end of list return nullptr; } +#if AP_FILESYSTEM_HAVE_DIRENT_DTYPE dtracker->curr_file.d_type = DT_REG; +#endif size_t max_length = ARRAY_SIZE(dtracker->curr_file.d_name); strncpy_noterm(dtracker->curr_file.d_name, sysfs_file_list[dtracker->file_offset].name, max_length); dtracker->file_offset++; diff --git a/libraries/AP_Filesystem/AP_Filesystem_config.h b/libraries/AP_Filesystem/AP_Filesystem_config.h index a184051610cc4..f5a97fb4fad56 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_config.h +++ b/libraries/AP_Filesystem/AP_Filesystem_config.h @@ -23,7 +23,7 @@ #endif #ifndef AP_FILESYSTEM_POSIX_ENABLED -#define AP_FILESYSTEM_POSIX_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) +#define AP_FILESYSTEM_POSIX_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_QURT) #endif #ifndef AP_FILESYSTEM_ROMFS_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 5a6169053472d..2e0a9ed1b501c 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -26,10 +26,13 @@ #if defined(__APPLE__) #include -#else +#elif CONFIG_HAL_BOARD != HAL_BOARD_QURT #include #endif + +#if AP_FILESYSTEM_POSIX_HAVE_UTIME #include +#endif extern const AP_HAL::HAL& hal; @@ -88,8 +91,12 @@ int32_t AP_Filesystem_Posix::write(int fd, const void *buf, uint32_t count) int AP_Filesystem_Posix::fsync(int fd) { +#if AP_FILESYSTEM_POSIX_HAVE_FSYNC FS_CHECK_ALLOWED(-1); return ::fsync(fd); +#else + return 0; +#endif } int32_t AP_Filesystem_Posix::lseek(int fd, int32_t offset, int seek_from) @@ -111,7 +118,7 @@ int AP_Filesystem_Posix::unlink(const char *pathname) pathname = map_filename(pathname); // we match the FATFS interface and use unlink // for both files and directories - int ret = ::rmdir(pathname); + int ret = ::rmdir(const_cast(pathname)); if (ret == -1) { ret = ::unlink(pathname); } @@ -122,7 +129,7 @@ int AP_Filesystem_Posix::mkdir(const char *pathname) { FS_CHECK_ALLOWED(-1); pathname = map_filename(pathname); - return ::mkdir(pathname, 0775); + return ::mkdir(const_cast(pathname), 0775); } void *AP_Filesystem_Posix::opendir(const char *pathname) @@ -155,6 +162,7 @@ int AP_Filesystem_Posix::rename(const char *oldpath, const char *newpath) // return free disk space in bytes int64_t AP_Filesystem_Posix::disk_free(const char *path) { +#if AP_FILESYSTEM_POSIX_HAVE_STATFS FS_CHECK_ALLOWED(-1); path = map_filename(path); struct statfs stats; @@ -162,11 +170,15 @@ int64_t AP_Filesystem_Posix::disk_free(const char *path) return -1; } return (((int64_t)stats.f_bavail) * stats.f_bsize); +#else + return -1; +#endif } // return total disk space in bytes int64_t AP_Filesystem_Posix::disk_space(const char *path) { +#if AP_FILESYSTEM_POSIX_HAVE_STATFS FS_CHECK_ALLOWED(-1); path = map_filename(path); struct statfs stats; @@ -174,6 +186,9 @@ int64_t AP_Filesystem_Posix::disk_space(const char *path) return -1; } return (((int64_t)stats.f_blocks) * stats.f_bsize); +#else + return -1; +#endif } @@ -182,6 +197,7 @@ int64_t AP_Filesystem_Posix::disk_space(const char *path) */ bool AP_Filesystem_Posix::set_mtime(const char *filename, const uint32_t mtime_sec) { +#if AP_FILESYSTEM_POSIX_HAVE_UTIME FS_CHECK_ALLOWED(false); filename = map_filename(filename); struct utimbuf times {}; @@ -189,6 +205,9 @@ bool AP_Filesystem_Posix::set_mtime(const char *filename, const uint32_t mtime_s times.modtime = mtime_sec; return utime(filename, ×) == 0; +#else + return false; +#endif } #endif // AP_FILESYSTEM_POSIX_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.h b/libraries/AP_Filesystem/AP_Filesystem_posix.h index e44764213aee4..2e291db70f77d 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.h +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.h @@ -29,6 +29,22 @@ #include #include "AP_Filesystem_backend.h" +#ifndef AP_FILESYSTEM_POSIX_HAVE_UTIME +#define AP_FILESYSTEM_POSIX_HAVE_UTIME 1 +#endif + +#ifndef AP_FILESYSTEM_POSIX_HAVE_FSYNC +#define AP_FILESYSTEM_POSIX_HAVE_FSYNC 1 +#endif + +#ifndef AP_FILESYSTEM_POSIX_HAVE_STATFS +#define AP_FILESYSTEM_POSIX_HAVE_STATFS 1 +#endif + +#ifndef O_CLOEXEC +#define O_CLOEXEC 0 +#endif + class AP_Filesystem_Posix : public AP_Filesystem_Backend { public: diff --git a/libraries/AP_Filesystem/posix_compat.h b/libraries/AP_Filesystem/posix_compat.h index 4ccf56b5c5f43..46b81223c1c42 100644 --- a/libraries/AP_Filesystem/posix_compat.h +++ b/libraries/AP_Filesystem/posix_compat.h @@ -77,6 +77,11 @@ char *tmpnam(char s[L_tmpnam]); #define SEEK_END 2 #endif +#ifndef __cplusplus +/* + only redefine posix functions for C code (eg. lua). + for C++ use the AP_Filsystem APIs +*/ #define FILE APFS_FILE #define fopen(p,m) apfs_fopen(p,m) #define fprintf(stream, format, args...) apfs_fprintf(stream, format, ##args) @@ -101,6 +106,7 @@ char *tmpnam(char s[L_tmpnam]); #define remove(pathname) apfs_remove(pathname) int sprintf(char *str, const char *format, ...); #endif +#endif #ifdef __cplusplus } diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 24b6a66d3b5c6..7a49997c7317d 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -14,6 +14,7 @@ #define HAL_BOARD_CHIBIOS 10 #define HAL_BOARD_F4LIGHT 11 // reserved #define HAL_BOARD_ESP32 12 +#define HAL_BOARD_QURT 13 #define HAL_BOARD_EMPTY 99 /* Default board subtype is -1 */ @@ -139,6 +140,8 @@ #include #elif CONFIG_HAL_BOARD == HAL_BOARD_ESP32 #include +#elif CONFIG_HAL_BOARD == HAL_BOARD_QURT + #include #else #error "Unknown CONFIG_HAL_BOARD type" #endif @@ -382,3 +385,7 @@ #endif #define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON) + +#ifndef AP_FILESYSTEM_HAVE_DIRENT_DTYPE +#define AP_FILESYSTEM_HAVE_DIRENT_DTYPE 1 +#endif diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h index da22f1a8be1d9..9d447e25e980d 100644 --- a/libraries/AP_HAL/Util.h +++ b/libraries/AP_HAL/Util.h @@ -44,11 +44,6 @@ class AP_HAL::Util { // set command line parameters to the eeprom on start virtual void set_cmdline_parameters() {}; - // run a debug shall on the given stream if possible. This is used - // to support dropping into a debug shell to run firmware upgrade - // commands - virtual bool run_debug_shell(AP_HAL::BetterStream *stream) = 0; - enum safety_state : uint8_t { SAFETY_NONE, SAFETY_DISARMED, @@ -154,7 +149,7 @@ class AP_HAL::Util { virtual void *malloc_type(size_t size, Memory_Type mem_type) { return calloc(1, size); } virtual void free_type(void *ptr, size_t size, Memory_Type mem_type) { return free(ptr); } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) = 0; virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) = 0; diff --git a/libraries/AP_HAL/board/qurt.h b/libraries/AP_HAL/board/qurt.h new file mode 100644 index 0000000000000..a2adebee0c18b --- /dev/null +++ b/libraries/AP_HAL/board/qurt.h @@ -0,0 +1,130 @@ +#pragma once + +#include + +#define HAL_BOARD_NAME "QURT" +#define HAL_CPU_CLASS HAL_CPU_CLASS_1000 +#define HAL_MEM_CLASS HAL_MEM_CLASS_1000 +#define HAL_STORAGE_SIZE 32768 +#define HAL_STORAGE_SIZE_AVAILABLE HAL_STORAGE_SIZE + +// only include if compiling C++ code +#ifdef __cplusplus +#include +#define HAL_Semaphore QURT::Semaphore +#define HAL_BinarySemaphore QURT::BinarySemaphore +#endif + +#ifndef HAL_HAVE_HARDWARE_DOUBLE +#define HAL_HAVE_HARDWARE_DOUBLE 0 +#endif + +#ifndef HAL_WITH_EKF_DOUBLE +#define HAL_WITH_EKF_DOUBLE HAL_HAVE_HARDWARE_DOUBLE +#endif + +#ifndef HAL_GYROFFT_ENABLED +#define HAL_GYROFFT_ENABLED 0 +#endif + +/* + disable features for initial port + */ +#define AP_SCRIPTING_ENABLED 0 +#define HAL_HAVE_BOARD_VOLTAGE 0 +#define HAL_HAVE_SERVO_VOLTAGE 0 +#define HAL_HAVE_SAFETY_SWITCH 0 +#define HAL_WITH_MCU_MONITORING 0 +#define HAL_USE_QUADSPI 0 +#define HAL_WITH_DSP 0 +#define HAL_CANFD_SUPPORTED 0 +#define HAL_NUM_CAN_IFACES 0 +#define AP_CRASHDUMP_ENABLED 0 +#define HAL_ENABLE_DFU_BOOT 0 +#define AP_FILESYSTEM_HAVE_DIRENT_DTYPE 0 +#define HAL_PICCOLO_CAN_ENABLE 0 +#define LUA_USE_LONGJMP 1 +#define AP_MAVLINK_FTP_ENABLED 0 +#define HAL_LOGGING_MAVLINK_ENABLED 0 +#define HAL_LOGGING_FILESYSTEM_ENABLED 0 +#define AP_FILESYSTEM_POSIX_HAVE_UTIME 0 +#define AP_FILESYSTEM_POSIX_HAVE_FSYNC 0 +#define AP_FILESYSTEM_POSIX_HAVE_STATFS 0 +// Get undefined symbols if this isn't enabled: +// _ZN7NavEKF39Log_WriteEv +// _ZNK7AP_AHRS11Write_AHRS2Ev +// _ZNK7AP_AHRS9Write_POSEv +// #define HAL_LOGGING_ENABLED 0 +// #define AP_GRIPPER_ENABLED 0 +// #define AP_AIRSPEED_ENABLED 0 +// #define AP_SRV_CHANNELS_ENABLED 0 +// #define HAL_RUNCAM_ENABLED 0 +// #define HAL_HOTT_TELEM_ENABLED 0 +// #define HAL_VISUALODOM_ENABLED 0 +// #define AP_VIDEOTX_ENABLED 0 +// #define AP_SMARTAUDIO_ENABLED 0 +// #define AP_TRAMP_ENABLED 0 +// #define AP_OPENDRONEID_ENABLED 0 +// #define HAL_EFI_ENABLED 0 +// #define AP_TEMPERATURE_SENSOR_ENABLED 0 +// #define AP_KDECAN_ENABLED 0 +// #define AP_AIS_ENABLED 0 +// #define HAL_NMEA_OUTPUT_ENABLED 0 +// #define AP_FENCE_ENABLED 0 +// #define AP_CUSTOMROTATIONS_ENABLED 0 +// #define AP_DDS_ENABLED 0 +// #define AP_CHECK_FIRMWARE_ENABLED 0 +// #define HAL_MSP_ENABLED 0 +// #define HAL_EXTERNAL_AHRS_ENABLED 0 +// #define HAL_GENERATOR_ENABLED 0 +// #define HAL_CANMANAGER_ENABLED 0 + +// SITL on Hardware definitions +// env SIM_ENABLED 1 +// +// #define INS_MAX_INSTANCES 2 +// #define HAL_COMPASS_MAX_SENSORS 2 +// +// #define AP_GPS_BACKEND_DEFAULT_ENABLED 0 +// #define AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED 0 +// +// #define HAL_NAVEKF2_AVAILABLE 0 +// #define EK3_FEATURE_BODY_ODOM 0 +// #define EK3_FEATURE_EXTERNAL_NAV 0 +// #define EK3_FEATURE_DRAG_FUSION 0 +// #define HAL_ADSB_ENABLED 0 +// #define HAL_PROXIMITY_ENABLED 0 +// #define HAL_VISUALODOM_ENABLED 0 +// #define HAL_GENERATOR_ENABLED 0 +// +// #define HAL_MSP_OPTICALFLOW_ENABLED 0 +// #define HAL_SUPPORT_RCOUT_SERIAL 0 +// #define HAL_HOTT_TELEM_ENABLED 0 +// #define HAL_HIGH_LATENCY2 0 +// +// #define AP_SIM_INS_FILE_ENABLED 0 +// End of SITL on Hardware definitions + +#define HAL_PARAM_DEFAULTS_PATH nullptr +#define HAL_BOARD_STORAGE_DIRECTORY "APM" +#define HAL_BOARD_LOG_DIRECTORY "APM/logs" +#define HAL_BOARD_TERRAIN_DIRECTORY "APM/terrain" + +#define USE_LIBC_REALLOC 1 + +/* + barometer list + */ +#define PROBE_BARO_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(GET_I2C_DEVICE(bus, addr)),##args)) +#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(ICP101XX, 3, 0x63) + +/* + IMU list + */ +#define PROBE_IMU_SPI(driver, devname, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname),##args)) +#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensensev3, "INV3", ROTATION_NONE) + +/* + bring in missing standard library functions + */ +#include diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 03a8b3e745027..29d5e31d05a57 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -96,7 +96,7 @@ void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *Util::allocate_heap_memory(size_t size) { diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 8e431484a096d..f11dbf7239aad 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -37,7 +37,6 @@ class ChibiOS::Util : public AP_HAL::Util { return static_cast(util); } - bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; } uint32_t available_memory() override; // get path to custom defaults file for AP_Param @@ -49,7 +48,7 @@ class ChibiOS::Util : public AP_HAL::Util { void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) override; virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override; @@ -143,7 +142,7 @@ class ChibiOS::Util : public AP_HAL::Util { FlashBootloader flash_bootloader() override; #endif -#ifdef ENABLE_HEAP +#if ENABLE_HEAP static memory_heap_t scripting_heap; #endif // ENABLE_HEAP diff --git a/libraries/AP_HAL_ESP32/Util.cpp b/libraries/AP_HAL_ESP32/Util.cpp index d232ff890765e..2eab971532339 100644 --- a/libraries/AP_HAL_ESP32/Util.cpp +++ b/libraries/AP_HAL_ESP32/Util.cpp @@ -90,7 +90,7 @@ void Util::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *Util::allocate_heap_memory(size_t size) { diff --git a/libraries/AP_HAL_ESP32/Util.h b/libraries/AP_HAL_ESP32/Util.h index 3360e1ad5a197..8e9ba9dc94221 100644 --- a/libraries/AP_HAL_ESP32/Util.h +++ b/libraries/AP_HAL_ESP32/Util.h @@ -28,17 +28,13 @@ class ESP32::Util : public AP_HAL::Util return static_cast(util); } - bool run_debug_shell(AP_HAL::BetterStream *stream) override - { - return false; - } uint32_t available_memory() override; // Special Allocation Routines void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) override; virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override; @@ -89,7 +85,7 @@ class ESP32::Util : public AP_HAL::Util FlashBootloader flash_bootloader() override; #endif -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // static memory_heap_t scripting_heap; #endif // ENABLE_HEAP diff --git a/libraries/AP_HAL_Empty/Util.h b/libraries/AP_HAL_Empty/Util.h index 3dae90c2f82e5..55e590a20e37b 100644 --- a/libraries/AP_HAL_Empty/Util.h +++ b/libraries/AP_HAL_Empty/Util.h @@ -4,6 +4,4 @@ #include "AP_HAL_Empty_Namespace.h" class Empty::Util : public AP_HAL::Util { -public: - bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; } }; diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp index dc9d8083a17b1..1638476e090cc 100644 --- a/libraries/AP_HAL_Linux/Util.cpp +++ b/libraries/AP_HAL_Linux/Util.cpp @@ -242,7 +242,7 @@ int Util::get_hw_arm32() return -ENOENT; } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *Util::allocate_heap_memory(size_t size) { struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap)); diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h index 9aa0a82314678..11a29dacb31c5 100644 --- a/libraries/AP_HAL_Linux/Util.h +++ b/libraries/AP_HAL_Linux/Util.h @@ -29,7 +29,6 @@ class Util : public AP_HAL::Util { } void init(int argc, char *const *argv); - bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; } /** return commandline arguments, if available @@ -72,7 +71,7 @@ class Util : public AP_HAL::Util { bool get_system_id(char buf[50]) override; bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd virtual void *allocate_heap_memory(size_t size) override; virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override; @@ -117,7 +116,7 @@ class Util : public AP_HAL::Util { const char *custom_defaults = HAL_PARAM_DEFAULTS_PATH; static const char *_hw_names[UTIL_NUM_HARDWARES]; -#ifdef ENABLE_HEAP +#if ENABLE_HEAP struct heap_allocation_header { size_t allocation_size; // size of allocated block, not including this header }; diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT.h b/libraries/AP_HAL_QURT/AP_HAL_QURT.h new file mode 100644 index 0000000000000..2c753c203d973 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT.h @@ -0,0 +1,25 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +/* Your layer exports should depend on AP_HAL.h ONLY. */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include "HAL_QURT_Class.h" +#include "AP_HAL_QURT_Main.h" + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h new file mode 100644 index 0000000000000..e0b57992c2513 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Main.h @@ -0,0 +1,18 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#define AP_MAIN qurt_arducopter_main + diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h new file mode 100644 index 0000000000000..279c588e041fb --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Namespace.h @@ -0,0 +1,29 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +namespace QURT { + class UARTDriver; + class UDPDriver; + class Util; + class Scheduler; + class Storage; + class Util; + class Semaphore; + class RCInput; + class RCOutput; +} + diff --git a/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h new file mode 100644 index 0000000000000..95a6b970037a2 --- /dev/null +++ b/libraries/AP_HAL_QURT/AP_HAL_QURT_Private.h @@ -0,0 +1,22 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +/* Umbrella header for all private headers of the AP_HAL_QURT module. + * Only import this header from inside AP_HAL_QURT + */ + +#include "UARTDriver.h" +#include "Util.h" diff --git a/libraries/AP_HAL_QURT/DeviceBus.cpp b/libraries/AP_HAL_QURT/DeviceBus.cpp new file mode 100644 index 0000000000000..e53c2380ebff0 --- /dev/null +++ b/libraries/AP_HAL_QURT/DeviceBus.cpp @@ -0,0 +1,135 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "DeviceBus.h" + +#include +#include +#include + +#include "Scheduler.h" +#include "Semaphores.h" + +using namespace QURT; + +extern const AP_HAL::HAL& hal; + +DeviceBus::DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority) : + thread_priority(_thread_priority), semaphore() +{ +} + +/* + per-bus callback thread +*/ +void DeviceBus::bus_thread(void) +{ + while (true) { + uint64_t now = AP_HAL::micros64(); + DeviceBus::callback_info *callback; + + // find a callback to run + for (callback = callbacks; callback; callback = callback->next) { + if (now >= callback->next_usec) { + while (now >= callback->next_usec) { + callback->next_usec += callback->period_usec; + } + // call it with semaphore held + if (semaphore.take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + callback->cb(); + semaphore.give(); + } + } + } + + // work out when next loop is needed + uint64_t next_needed = 0; + now = AP_HAL::micros64(); + + for (callback = callbacks; callback; callback = callback->next) { + if (next_needed == 0 || + callback->next_usec < next_needed) { + next_needed = callback->next_usec; + if (next_needed < now) { + next_needed = now; + } + } + } + + // delay for at most 50ms, to handle newly added callbacks + uint32_t delay = 50000; + if (next_needed >= now && next_needed - now < delay) { + delay = next_needed - now; + } + // don't delay for less than 100usec, so one thread doesn't + // completely dominate the CPU + if (delay < 100) { + delay = 100; + } + hal.scheduler->delay_microseconds(delay); + } + return; +} + +AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb, AP_HAL::Device *_hal_device) +{ + if (!thread_started) { + thread_started = true; + hal_device = _hal_device; + // setup a name for the thread + char name[30]; + switch (hal_device->bus_type()) { + case AP_HAL::Device::BUS_TYPE_I2C: + snprintf(name, sizeof(name), "APM_I2C:%u", + hal_device->bus_num()); + break; + + case AP_HAL::Device::BUS_TYPE_SPI: + snprintf(name, sizeof(name), "APM_SPI:%u", + hal_device->bus_num()); + break; + default: + break; + } +#ifdef BUSDEBUG + printf("%s:%d Thread Start\n", __PRETTY_FUNCTION__, __LINE__); +#endif + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&DeviceBus::bus_thread, void), + name, HAL_QURT_DEVICE_STACK_SIZE, thread_priority, 0); + } + DeviceBus::callback_info *callback = new DeviceBus::callback_info; + if (callback == nullptr) { + return nullptr; + } + callback->cb = cb; + callback->period_usec = period_usec; + callback->next_usec = AP_HAL::micros64() + period_usec; + + // add to linked list of callbacks on thread + callback->next = callbacks; + callbacks = callback; + + return callback; +} + +/* + * Adjust the timer for the next call: it needs to be called from the bus + * thread, otherwise it will race with it + */ +bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + // TODO: add timer adjustment + return true; +} diff --git a/libraries/AP_HAL_QURT/DeviceBus.h b/libraries/AP_HAL_QURT/DeviceBus.h new file mode 100644 index 0000000000000..d62db9ccf2ded --- /dev/null +++ b/libraries/AP_HAL_QURT/DeviceBus.h @@ -0,0 +1,55 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include "Semaphores.h" +#include "AP_HAL_QURT.h" +#include "Scheduler.h" + +#define HAL_QURT_DEVICE_STACK_SIZE 8192 + +namespace QURT +{ + +class DeviceBus +{ +public: + DeviceBus(AP_HAL::Scheduler::priority_base _thread_priority); + + struct DeviceBus *next; + HAL_Semaphore semaphore; + + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb, AP_HAL::Device *hal_device); + bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec); + void bus_thread(void); + +private: + struct callback_info { + struct callback_info *next; + AP_HAL::Device::PeriodicCb cb; + uint32_t period_usec; + uint64_t next_usec; + } *callbacks; + AP_HAL::Scheduler::priority_base thread_priority; + bool thread_started; + AP_HAL::Device *hal_device; +}; + +} + + diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp new file mode 100644 index 0000000000000..d01e194fcb47f --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.cpp @@ -0,0 +1,134 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include "HAL_QURT_Class.h" +#include "AP_HAL_QURT_Private.h" +#include "Scheduler.h" +#include "Storage.h" +#include "Semaphores.h" +#include "RCInput.h" +#include "RCOutput.h" +#include "I2CDevice.h" +#include "SPIDevice.h" +#include +#include +#include +#include + +using namespace QURT; + +static UARTDriver serial0Driver("/dev/console"); +static UARTDriver serial1Driver("/dev/tty-4"); +static UARTDriver serial2Driver("/dev/tty-2"); + +static SPIDeviceManager spiDeviceManager; +static Empty::AnalogIn analogIn; +static Empty::Storage storageDriver; +static Empty::GPIO gpioDriver; +static Empty::RCInput rcinDriver; +static RCOutput rcoutDriver; +static Util utilInstance; +static Scheduler schedulerInstance; +static I2CDeviceManager i2c_mgr_instance; + +bool qurt_ran_overtime; + +HAL_QURT::HAL_QURT() : + AP_HAL::HAL( + &serial0Driver, + &serial1Driver, + &serial2Driver, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + nullptr, + &i2c_mgr_instance, + &spiDeviceManager, + nullptr, + &analogIn, + &storageDriver, + &serial0Driver, + &gpioDriver, + &rcinDriver, + &rcoutDriver, + &schedulerInstance, + &utilInstance, + nullptr, + nullptr, + nullptr) +{ +} + + +// QURT interface pointers +qurt_func_ptrs_t qurt_func_ptrs {}; + +static HAL_QURT::Callbacks *_callbacks; + +void HAL_QURT::main_thread(void) +{ + HAP_PRINTF("In main_thread!"); + rcinDriver.init(); + _callbacks->setup(); + scheduler->set_system_initialized(); + + HAP_PRINTF("starting loop"); + + for (;;) { + _callbacks->loop(); + } +} + +void HAL_QURT::start_main_thread(Callbacks* callbacks) +{ + _callbacks = callbacks; + scheduler->thread_create(FUNCTOR_BIND_MEMBER(&HAL_QURT::main_thread, void), "main_thread", + 500000, + AP_HAL::Scheduler::PRIORITY_MAIN, + 0); +} + +void HAL_QURT::run(int argc, char* const argv[], Callbacks* callbacks) const +{ + assert(callbacks); + + /* initialize all drivers and private members here. + * up to the programmer to do this in the correct order. + * Scheduler should likely come first. */ + scheduler->init(); + schedulerInstance.hal_initialized(); + serial0Driver.begin(115200); + + HAP_PRINTF("Creating thread!"); + + const_cast(this)->start_main_thread(callbacks); +} + +const AP_HAL::HAL& AP_HAL::get_HAL() { + static const HAL_QURT *hal; + if (hal == nullptr) { + hal = new HAL_QURT; + HAP_PRINTF("allocated HAL_QURT of size %u", sizeof(*hal)); + } + return *hal; +} + +#endif diff --git a/libraries/AP_HAL_QURT/HAL_QURT_Class.h b/libraries/AP_HAL_QURT/HAL_QURT_Class.h new file mode 100644 index 0000000000000..28c8868d5c297 --- /dev/null +++ b/libraries/AP_HAL_QURT/HAL_QURT_Class.h @@ -0,0 +1,28 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#pragma once + +#include + +#include "AP_HAL_QURT_Namespace.h" +#include "interface.h" + +class HAL_QURT : public AP_HAL::HAL { +public: + HAL_QURT(); + void run(int argc, char* const* argv, Callbacks* callbacks) const override; + void start_main_thread(Callbacks* callbacks); + void main_thread(void); +}; diff --git a/libraries/AP_HAL_QURT/I2CDevice.cpp b/libraries/AP_HAL_QURT/I2CDevice.cpp new file mode 100644 index 0000000000000..2e9cdc1410ba2 --- /dev/null +++ b/libraries/AP_HAL_QURT/I2CDevice.cpp @@ -0,0 +1,143 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "I2CDevice.h" + +#include +#include +#include +#include "Scheduler.h" + +extern qurt_func_ptrs_t qurt_func_ptrs; + +using namespace QURT; + +/* + 4 I2C buses + + bus1: mag + bus2: power manager + bus4: external spare bus + bus5: barometer (internal) +*/ +static uint8_t i2c_bus_ids[] = { 1, 2, 4, 5 }; + +static uint32_t i2c_internal_mask = (1U<<3); + +I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(i2c_bus_ids)]; + +I2CDeviceManager::I2CDeviceManager(void) +{ + /* + open all the buses with a default speed + */ + HAP_PRINTF("In I2CDeviceManager constructor"); +} + +I2CDevice::I2CDevice(uint8_t busnum, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms) : + _retries(10), + _address(address), + bus(I2CDeviceManager::businfo[busnum]) +{ + HAP_PRINTF("Constructing I2CDevice %u 0x%02x %u", unsigned(busnum), unsigned(address), unsigned(bus_clock)); + qurt_timer_sleep(5000); + + if (bus.fd == -2) { + HAP_PRINTF("Calling _config_i2c_bus_func_t %u", + unsigned(i2c_bus_ids[busnum])); qurt_timer_sleep(5000); + bus.fd = qurt_func_ptrs._config_i2c_bus_func_t(i2c_bus_ids[busnum], address, bus_clock/1000); + HAP_PRINTF("Opened I2C bus %u -> %d", unsigned(busnum), bus.fd); + } + set_device_bus(busnum); + set_device_address(address); + asprintf(&pname, "I2C:%u:%02x", + (unsigned)busnum, (unsigned)address); +} + +I2CDevice::~I2CDevice() +{ + free(pname); +} + +bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + if (bus.fd <= 0) { + return false; + } + if (!bus.semaphore.check_owner()) { + HAP_PRINTF("I2C: not owner of 0x%x", (unsigned)get_bus_id()); + return false; + } + if (bus.last_address != _address) { + HAP_PRINTF("I2C: changing address for fd %d to 0x%02x", bus.fd, _address); + qurt_func_ptrs._set_i2c_address_func_t(bus.fd, _address); + bus.last_address = _address; + } + return qurt_func_ptrs._i2c_transfer_func_t(bus.fd, send, send_len, recv, recv_len) == 0; +} + +/* + register a periodic callback +*/ +AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + return bus.register_periodic_callback(period_usec, cb, this); +} + + +/* + adjust a periodic callback +*/ +bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + return bus.adjust_timer(h, period_usec); +} + +AP_HAL::OwnPtr +I2CDeviceManager::get_device(uint8_t bus, uint8_t address, + uint32_t bus_clock, + bool use_smbus, + uint32_t timeout_ms) +{ + if (bus >= ARRAY_SIZE(i2c_bus_ids)) { + return AP_HAL::OwnPtr(nullptr); + } + auto dev = AP_HAL::OwnPtr(new I2CDevice(bus, address, bus_clock, use_smbus, timeout_ms)); + return dev; +} + +/* + get mask of bus numbers for all configured I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask(void) const +{ + return ((1U << ARRAY_SIZE(i2c_bus_ids)) - 1); +} + +/* + get mask of bus numbers for all configured internal I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask_internal(void) const +{ + return i2c_internal_mask; +} + +/* + get mask of bus numbers for all configured external I2C buses +*/ +uint32_t I2CDeviceManager::get_bus_mask_external(void) const +{ + return get_bus_mask() & ~get_bus_mask_internal(); +} diff --git a/libraries/AP_HAL_QURT/I2CDevice.h b/libraries/AP_HAL_QURT/I2CDevice.h new file mode 100644 index 0000000000000..b72200926304c --- /dev/null +++ b/libraries/AP_HAL_QURT/I2CDevice.h @@ -0,0 +1,134 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include + +#include +#include +#include + +#include "Semaphores.h" +#include "Scheduler.h" +#include "DeviceBus.h" + +namespace QURT +{ + +class I2CBus : public DeviceBus +{ +public: + I2CBus():DeviceBus(AP_HAL::Scheduler::PRIORITY_I2C) {}; + uint32_t bus_clock; + int fd = -2; + uint8_t last_address; +}; + +class I2CDevice : public AP_HAL::I2CDevice +{ +public: + static I2CDevice *from(AP_HAL::I2CDevice *dev) + { + return static_cast(dev); + } + + I2CDevice(uint8_t bus, uint8_t address, uint32_t bus_clock, bool use_smbus, uint32_t timeout_ms); + ~I2CDevice(); + + /* See AP_HAL::I2CDevice::set_address() */ + void set_address(uint8_t address) override + { + _address = address; + } + + /* See AP_HAL::I2CDevice::set_retries() */ + void set_retries(uint8_t retries) override + { + _retries = retries; + } + + /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */ + bool set_speed(enum Device::Speed speed) override + { + return true; + } + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + bool read_registers_multiple(uint8_t first_reg, uint8_t *recv, + uint32_t recv_len, uint8_t times) override + { + return false; + }; + + /* See AP_HAL::Device::register_periodic_callback() */ + AP_HAL::Device::PeriodicHandle register_periodic_callback( + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + + /* See AP_HAL::Device::adjust_periodic_callback() */ + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + + AP_HAL::Semaphore* get_semaphore() override //TODO check all + { + // if asking for invalid bus number use bus 0 semaphore + return &bus.semaphore; + } + +protected: + I2CBus &bus; + uint8_t _retries; + uint8_t _address; + char *pname; + +}; + +class I2CDeviceManager : public AP_HAL::I2CDeviceManager +{ +public: + friend class I2CDevice; + + static I2CBus businfo[]; + + // constructor + I2CDeviceManager(); + + static I2CDeviceManager *from(AP_HAL::I2CDeviceManager *i2c_mgr) + { + return static_cast(i2c_mgr); + } + + AP_HAL::OwnPtr get_device(uint8_t bus, uint8_t address, + uint32_t bus_clock=100000, + bool use_smbus = false, + uint32_t timeout_ms=4) override; + + /* + get mask of bus numbers for all configured I2C buses + */ + uint32_t get_bus_mask(void) const override; + + /* + get mask of bus numbers for all configured external I2C buses + */ + uint32_t get_bus_mask_external(void) const override; + + /* + get mask of bus numbers for all configured internal I2C buses + */ + uint32_t get_bus_mask_internal(void) const override; +}; +} diff --git a/libraries/AP_HAL_QURT/RCInput.cpp b/libraries/AP_HAL_QURT/RCInput.cpp new file mode 100644 index 0000000000000..90fb90f1c0078 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.cpp @@ -0,0 +1,31 @@ + +#include "RCInput.h" + +using namespace QURT; +RCInput::RCInput() +{} + +void RCInput::init() +{} + +bool RCInput::new_input() { + return false; +} + +uint8_t RCInput::num_channels() { + return 0; +} + +uint16_t RCInput::read(uint8_t chan) { + if (chan == 2) return 900; /* throttle should be low, for safety */ + else return 1500; +} + +uint8_t RCInput::read(uint16_t* periods, uint8_t len) { + for (uint8_t i = 0; i < len; i++){ + if (i == 2) periods[i] = 900; + else periods[i] = 1500; + } + return len; +} + diff --git a/libraries/AP_HAL_QURT/RCInput.h b/libraries/AP_HAL_QURT/RCInput.h new file mode 100644 index 0000000000000..d9918ad2c736d --- /dev/null +++ b/libraries/AP_HAL_QURT/RCInput.h @@ -0,0 +1,14 @@ +#pragma once + +#include "AP_HAL_QURT.h" + +class QURT::RCInput : public AP_HAL::RCInput { +public: + RCInput(); + void init() override; + bool new_input() override; + uint8_t num_channels() override; + uint16_t read(uint8_t ch) override; + uint8_t read(uint16_t* periods, uint8_t len) override; + virtual const char *protocol() const override { return "Empty"; } +}; diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp new file mode 100644 index 0000000000000..07fb94aa8477c --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -0,0 +1,41 @@ + +#include "RCOutput.h" +#include + +using namespace QURT; + +void RCOutput::init() {} + +void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {} + +uint16_t RCOutput::get_freq(uint8_t chan) { + return 50; +} + +void RCOutput::enable_ch(uint8_t chan) +{} + +void RCOutput::disable_ch(uint8_t chan) +{} + +void RCOutput::write(uint8_t chan, uint16_t period_us) +{ + if (chan < ARRAY_SIZE(value)) { + value[chan] = period_us; + } +} + +uint16_t RCOutput::read(uint8_t chan) +{ + if (chan < ARRAY_SIZE(value)) { + return value[chan]; + } + return 900; +} + +void RCOutput::read(uint16_t* period_us, uint8_t len) +{ + len = MIN(len, ARRAY_SIZE(value)); + memcpy(period_us, value, len*sizeof(value[0])); +} + diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h new file mode 100644 index 0000000000000..1c78839b50311 --- /dev/null +++ b/libraries/AP_HAL_QURT/RCOutput.h @@ -0,0 +1,18 @@ +#pragma once + +#include "AP_HAL_QURT.h" + +class QURT::RCOutput : public AP_HAL::RCOutput { + void init() override; + void set_freq(uint32_t chmask, uint16_t freq_hz) override; + uint16_t get_freq(uint8_t ch) override; + void enable_ch(uint8_t ch) override; + void disable_ch(uint8_t ch) override; + void write(uint8_t ch, uint16_t period_us) override; + uint16_t read(uint8_t ch) override; + void read(uint16_t* period_us, uint8_t len) override; + void cork(void) override {} + void push(void) override {} +private: + uint16_t value[16]; +}; diff --git a/libraries/AP_HAL_QURT/SPIDevice.cpp b/libraries/AP_HAL_QURT/SPIDevice.cpp new file mode 100644 index 0000000000000..2ba6ebaee025e --- /dev/null +++ b/libraries/AP_HAL_QURT/SPIDevice.cpp @@ -0,0 +1,139 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "SPIDevice.h" + +#include +#include +#include +#include "Scheduler.h" +#include "Semaphores.h" +#include + +using namespace QURT; + +#define MHZ (1000U*1000U) +#define KHZ (1000U) + +const char *device_names[] = {"INV1", "INV2", "INV3"}; + +extern qurt_func_ptrs_t qurt_func_ptrs; + +static SPIBus *spi_bus; + +SPIBus::SPIBus(void): + DeviceBus(Scheduler::PRIORITY_SPI) +{ + fd = qurt_func_ptrs._config_spi_bus_func_t(); + HAP_PRINTF("Created SPI bus -> %d", fd); +} + +SPIDevice::SPIDevice(const char *name, SPIBus &_bus) + : bus(_bus) +{ + set_device_bus(0); + set_device_address(1); + set_speed(AP_HAL::Device::SPEED_LOW); + + pname = name; +} + +SPIDevice::~SPIDevice() +{ +} + +bool SPIDevice::set_speed(AP_HAL::Device::Speed _speed) +{ + return true; +} + +bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + //HAP_PRINTF("SPI transfer %p %u %p %u [%02x %02x]", send, send_len, recv, recv_len, send?send[0]:1, recv?recv[0]:1); + if (!send || !recv) { + // simplest cases + return transfer_fullduplex(send, recv, recv_len?recv_len:send_len); + } + uint8_t buf[send_len+recv_len]; + if (send_len > 0) { + memcpy(buf, send, send_len); + } + if (recv_len > 0) { + memset(&buf[send_len], 0, recv_len); + } + transfer_fullduplex(buf, buf, send_len+recv_len); + if (recv_len > 0) { + memcpy(recv, &buf[send_len], recv_len); + } + return true; +} + +bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) +{ + uint8_t dummy[len]; + memset(dummy, 0xFF, sizeof(dummy)); + if (send == nullptr) { + send = dummy; + } + if (recv == nullptr) { + recv = dummy; + } + int ret = qurt_func_ptrs._spi_transfer_func_t(bus.fd, send, recv, len); + return ret == 0; +} + +void SPIDevice::acquire_bus(bool accuire) +{ +} + +AP_HAL::Semaphore *SPIDevice::get_semaphore() +{ + return &bus.semaphore; +} + +AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + return bus.register_periodic_callback(period_usec, cb, this); +} + +bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + return bus.adjust_timer(h, period_usec); +} + +AP_HAL::OwnPtr +SPIDeviceManager::get_device(const char *name) +{ + uint8_t i; + for (i = 0; i(nullptr); + } + + if (spi_bus == nullptr) { + spi_bus = new SPIBus(); + } + + HAP_PRINTF("*&*&*&*&* got SPI name %s", name); + + return AP_HAL::OwnPtr(new SPIDevice(name, *spi_bus)); +} + diff --git a/libraries/AP_HAL_QURT/SPIDevice.h b/libraries/AP_HAL_QURT/SPIDevice.h new file mode 100644 index 0000000000000..65a55858204e0 --- /dev/null +++ b/libraries/AP_HAL_QURT/SPIDevice.h @@ -0,0 +1,65 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#pragma once + +#include +#include +#include +#include "AP_HAL_QURT.h" + +#include "Semaphores.h" +#include "Scheduler.h" +#include "DeviceBus.h" + +namespace QURT +{ + +class SPIBus : public DeviceBus +{ +public: + SPIBus(); + int fd = -1; +}; + +class SPIDevice : public AP_HAL::SPIDevice +{ +public: + SPIDevice(const char *name, SPIBus &bus); + virtual ~SPIDevice(); + + bool set_speed(AP_HAL::Device::Speed speed) override; + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) override; + AP_HAL::Semaphore *get_semaphore() override; + AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + +private: + SPIBus &bus; + const char *pname; + void acquire_bus(bool accuire); +}; + +class SPIDeviceManager : public AP_HAL::SPIDeviceManager +{ +public: + friend class SPIDevice; + + AP_HAL::OwnPtr get_device(const char *name) override; +}; +} + diff --git a/libraries/AP_HAL_QURT/Scheduler.cpp b/libraries/AP_HAL_QURT/Scheduler.cpp new file mode 100644 index 0000000000000..8a5ef4ec1334c --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.cpp @@ -0,0 +1,333 @@ +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include "AP_HAL_QURT.h" +#include "Scheduler.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "UARTDriver.h" +#include "Storage.h" +#include "RCOutput.h" +#include +#include "Thread.h" + +using namespace QURT; + +extern const AP_HAL::HAL& hal; + +Scheduler::Scheduler() +{ +} + +void Scheduler::init() +{ + _main_task_pid = getpid(); + + // setup the timer thread - this will call tasks at 1kHz + pthread_attr_t thread_attr; + struct sched_param param; + + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 16000); + + param.sched_priority = APM_TIMER_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + + pthread_create(&_timer_thread_ctx, &thread_attr, &Scheduler::_timer_thread, this); + + // the UART thread runs at a medium priority + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 16000); + + param.sched_priority = APM_UART_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + + pthread_create(&_uart_thread_ctx, &thread_attr, &Scheduler::_uart_thread, this); + + // the IO thread runs at lower priority + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 16000); + + param.sched_priority = APM_IO_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + + pthread_create(&_io_thread_ctx, &thread_attr, &Scheduler::_io_thread, this); +} + +#define APM_QURT_MAX_PRIORITY 20 +#define APM_QURT_TIMER_PRIORITY 15 +#define APM_QURT_UART_PRIORITY 14 +#define APM_QURT_NET_PRIORITY 14 +#define APM_QURT_RCIN_PRIORITY 13 +#define APM_QURT_MAIN_PRIORITY 12 +#define APM_QURT_IO_PRIORITY 10 +#define APM_QURT_SCRIPTING_PRIORITY 1 +#define AP_QURT_SENSORS_SCHED_PRIO 12 + +uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority) const +{ + uint8_t thread_priority = APM_QURT_IO_PRIORITY; + static const struct { + priority_base base; + uint8_t p; + } priority_map[] = { + { PRIORITY_BOOST, APM_QURT_MAIN_PRIORITY}, + { PRIORITY_MAIN, APM_QURT_MAIN_PRIORITY}, + { PRIORITY_SPI, AP_QURT_SENSORS_SCHED_PRIO+1}, + { PRIORITY_I2C, AP_QURT_SENSORS_SCHED_PRIO}, + { PRIORITY_CAN, APM_QURT_TIMER_PRIORITY}, + { PRIORITY_TIMER, APM_QURT_TIMER_PRIORITY}, + { PRIORITY_RCIN, APM_QURT_RCIN_PRIORITY}, + { PRIORITY_IO, APM_QURT_IO_PRIORITY}, + { PRIORITY_UART, APM_QURT_UART_PRIORITY}, + { PRIORITY_STORAGE, APM_QURT_IO_PRIORITY}, + { PRIORITY_SCRIPTING, APM_QURT_SCRIPTING_PRIORITY}, + { PRIORITY_NET, APM_QURT_NET_PRIORITY}, + }; + for (uint8_t i=0; iset_stack_size(10 * 1024 + stack_size); + + /* + * We should probably store the thread handlers and join() when exiting, + * but let's the thread manage itself for now. + */ + thread->set_auto_free(true); + + if (!thread->start(name, SCHED_FIFO, thread_priority)) { + delete thread; + return false; + } + + return true; +} + +void Scheduler::delay_microseconds(uint16_t usec) +{ + qurt_timer_sleep(usec); +} + +void Scheduler::delay(uint16_t ms) +{ + if (!in_main_thread()) { + ::printf("ERROR: delay() from timer process\n"); + return; + } + uint64_t start = AP_HAL::micros64(); + uint64_t now; + + while (((now=AP_HAL::micros64()) - start)/1000 < ms) { + delay_microseconds(1000); + if (_min_delay_cb_ms <= ms) { + call_delay_cb(); + } + } +} + +void Scheduler::register_timer_process(AP_HAL::MemberProc proc) +{ + for (uint8_t i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i] == proc) { + return; + } + } + + if (_num_timer_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) { + _timer_proc[_num_timer_procs] = proc; + _num_timer_procs++; + } else { + hal.console->printf("Out of timer processes\n"); + } +} + +void Scheduler::register_io_process(AP_HAL::MemberProc proc) +{ + for (uint8_t i = 0; i < _num_io_procs; i++) { + if (_io_proc[i] == proc) { + return; + } + } + + if (_num_io_procs < QURT_SCHEDULER_MAX_TIMER_PROCS) { + _io_proc[_num_io_procs] = proc; + _num_io_procs++; + } else { + hal.console->printf("Out of IO processes\n"); + } +} + +void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) +{ + _failsafe = failsafe; +} + +void Scheduler::suspend_timer_procs() +{ + _timer_suspended = true; +} + +void Scheduler::resume_timer_procs() +{ + _timer_suspended = false; + if (_timer_event_missed == true) { + _run_timers(false); + _timer_event_missed = false; + } +} + +void Scheduler::reboot(bool hold_in_bootloader) +{ + HAP_PRINTF("**** REBOOT REQUESTED ****"); + qurt_timer_sleep(2000000); + exit(1); +} + +void Scheduler::_run_timers(bool called_from_timer_thread) +{ + if (_in_timer_proc) { + return; + } + _in_timer_proc = true; + + if (!_timer_suspended) { + // now call the timer based drivers + for (int i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i]) { + _timer_proc[i](); + } + } + } else if (called_from_timer_thread) { + _timer_event_missed = true; + } + + // and the failsafe, if one is setup + if (_failsafe != nullptr) { + _failsafe(); + } + + _in_timer_proc = false; +} + +extern bool qurt_ran_overtime; + +void *Scheduler::_timer_thread(void *arg) +{ + Scheduler *sched = (Scheduler *)arg; + + while (!sched->_hal_initialized) { + sched->delay_microseconds(1000); + } + while (true) { + sched->delay_microseconds(1000); + + // run registered timers + sched->_run_timers(true); + } + return nullptr; +} + +void Scheduler::_run_io(void) +{ + if (_in_io_proc) { + return; + } + _in_io_proc = true; + + if (!_timer_suspended) { + // now call the IO based drivers + for (int i = 0; i < _num_io_procs; i++) { + if (_io_proc[i]) { + _io_proc[i](); + } + } + } + + _in_io_proc = false; +} + +void *Scheduler::_uart_thread(void *arg) +{ + Scheduler *sched = (Scheduler *)arg; + + while (!sched->_hal_initialized) { + sched->delay_microseconds(1000); + } + while (true) { + sched->delay_microseconds(1000); + + // process any pending serial bytes + //((UARTDriver *)hal.uartA)->timer_tick(); + for (uint8_t i = 0; i < hal.num_serial; i++) { + auto *p = hal.serial(i); + if (p != nullptr) { + p->_timer_tick(); + } + } + } + return nullptr; +} + +void *Scheduler::_io_thread(void *arg) +{ + Scheduler *sched = (Scheduler *)arg; + + while (!sched->_hal_initialized) { + sched->delay_microseconds(1000); + } + while (true) { + sched->delay_microseconds(1000); + + // run registered IO processes + sched->_run_io(); + } + return nullptr; +} + +bool Scheduler::in_main_thread() const +{ + return getpid() == _main_task_pid; +} + +void Scheduler::set_system_initialized() { + if (_initialized) { + AP_HAL::panic("PANIC: scheduler::system_initialized called" + "more than once"); + } + _initialized = true; +} + +void Scheduler::hal_initialized(void) +{ + HAP_PRINTF("HAL is initialised"); + _hal_initialized = true; +} +#endif diff --git a/libraries/AP_HAL_QURT/Scheduler.h b/libraries/AP_HAL_QURT/Scheduler.h new file mode 100644 index 0000000000000..553597ddd57f4 --- /dev/null +++ b/libraries/AP_HAL_QURT/Scheduler.h @@ -0,0 +1,80 @@ +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "AP_HAL_QURT_Namespace.h" +#include +#include +#include + +#define QURT_SCHEDULER_MAX_TIMER_PROCS 8 + +#define APM_MAIN_PRIORITY 180 +#define APM_TIMER_PRIORITY 181 +#define APM_UART_PRIORITY 60 +#define APM_IO_PRIORITY 58 + +/* Scheduler implementation: */ +class QURT::Scheduler : public AP_HAL::Scheduler { +public: + Scheduler(); + /* AP_HAL::Scheduler methods */ + + void init() override; + void delay(uint16_t ms) override; + void delay_microseconds(uint16_t us) override; + void register_timer_process(AP_HAL::MemberProc) override; + void register_io_process(AP_HAL::MemberProc) override; + void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us) override; + void suspend_timer_procs(); + void resume_timer_procs(); + void reboot(bool hold_in_bootloader) override; + + bool in_main_thread() const override; + void hal_initialized(); + + void set_system_initialized() override; + bool is_system_initialized() override { return _initialized; } + + bool thread_create(AP_HAL::MemberProc proc, const char *name, + uint32_t stack_size, priority_base base, int8_t priority) override; + +private: + bool _initialized; + volatile bool _hal_initialized; + AP_HAL::Proc _delay_cb; + uint16_t _min_delay_cb_ms; + AP_HAL::Proc _failsafe; + + volatile bool _timer_suspended; + + AP_HAL::MemberProc _timer_proc[QURT_SCHEDULER_MAX_TIMER_PROCS]; + uint8_t _num_timer_procs; + volatile bool _in_timer_proc; + + AP_HAL::MemberProc _io_proc[QURT_SCHEDULER_MAX_TIMER_PROCS]; + uint8_t _num_io_procs; + volatile bool _in_io_proc; + + volatile bool _timer_event_missed; + + pid_t _main_task_pid; + pthread_t _timer_thread_ctx; + pthread_t _io_thread_ctx; + pthread_t _storage_thread_ctx; + pthread_t _uart_thread_ctx; + + uint8_t calculate_thread_priority(priority_base base, int8_t priority) const; + + static void *_timer_thread(void *arg); + static void *_io_thread(void *arg); + static void *_storage_thread(void *arg); + static void *_uart_thread(void *arg); + + void _run_timers(bool called_from_timer_thread); + void _run_io(void); +}; +#endif + + + diff --git a/libraries/AP_HAL_QURT/Semaphores.cpp b/libraries/AP_HAL_QURT/Semaphores.cpp new file mode 100644 index 0000000000000..1c7c280e0a0bb --- /dev/null +++ b/libraries/AP_HAL_QURT/Semaphores.cpp @@ -0,0 +1,111 @@ +#include + +#include "Semaphores.h" + +extern const AP_HAL::HAL& hal; + +using namespace QURT; + +// construct a semaphore +Semaphore::Semaphore() +{ + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + pthread_mutex_init(&_lock, &attr); +} + +bool Semaphore::give() +{ + return pthread_mutex_unlock(&_lock) == 0; +} + +bool Semaphore::check_owner() +{ + return owner == pthread_self(); +} + +bool Semaphore::take(uint32_t timeout_ms) +{ + if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) { + auto ok = pthread_mutex_lock(&_lock) == 0; + if (ok) { + owner = pthread_self(); + } + return ok; + } + if (take_nonblocking()) { + return true; + } + uint64_t start = AP_HAL::micros64(); + do { + hal.scheduler->delay_microseconds(200); + if (take_nonblocking()) { + return true; + } + } while ((AP_HAL::micros64() - start) < timeout_ms*1000); + return false; +} + +bool Semaphore::take_nonblocking() +{ + const auto ok = pthread_mutex_trylock(&_lock) == 0; + if (ok) { + owner = pthread_self(); + } + return ok; +} + +/* + binary semaphore using condition variables + */ + +BinarySemaphore::BinarySemaphore(bool initial_state) : + AP_HAL::BinarySemaphore(initial_state) +{ + pthread_cond_init(&cond, NULL); + pending = initial_state; +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + struct timespec ts; + if (clock_gettime(CLOCK_REALTIME, &ts) != 0) { + return false; + } + ts.tv_sec += timeout_us/1000000UL; + ts.tv_nsec += (timeout_us % 1000000U) * 1000UL; + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) { + return false; + } + } + pending = false; + return true; +} + +bool BinarySemaphore::wait_blocking(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + if (pthread_cond_wait(&cond, &mtx._lock) != 0) { + return false; + } + } + pending = false; + return true; +} + +void BinarySemaphore::signal(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + pending = true; + pthread_cond_signal(&cond); + } +} diff --git a/libraries/AP_HAL_QURT/Semaphores.h b/libraries/AP_HAL_QURT/Semaphores.h new file mode 100644 index 0000000000000..e017641d89672 --- /dev/null +++ b/libraries/AP_HAL_QURT/Semaphores.h @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include +#include + +namespace QURT { + +class Semaphore : public AP_HAL::Semaphore { +public: + friend class BinarySemaphore; + Semaphore(); + bool give() override; + bool take(uint32_t timeout_ms) override; + bool take_nonblocking() override; + bool check_owner(void); +protected: + // qurt_mutex_t _lock; + pthread_mutex_t _lock; + pthread_t owner; +}; + + +class BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + +protected: + Semaphore mtx; + pthread_cond_t cond; + bool pending; +}; + +} diff --git a/libraries/AP_HAL_QURT/Storage.cpp b/libraries/AP_HAL_QURT/Storage.cpp new file mode 100644 index 0000000000000..d9d1f74fd8353 --- /dev/null +++ b/libraries/AP_HAL_QURT/Storage.cpp @@ -0,0 +1,49 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +#include +#include +#include +#include +#include +#include +#include +#include "Storage.h" + +using namespace QURT; + +/* + This stores 'eeprom' data on the filesystem, with a 16k size + + Data is written on the ARM frontend via a RPC call + */ + +extern const AP_HAL::HAL& hal; + +volatile bool Storage::dirty; +uint8_t Storage::buffer[QURT_STORAGE_SIZE]; +Semaphore Storage::lock; + +void Storage::read_block(void *dst, uint16_t loc, size_t n) +{ + if (loc >= sizeof(buffer)-(n-1)) { + return; + } + memcpy(dst, &buffer[loc], n); +} + +void Storage::write_block(uint16_t loc, const void *src, size_t n) +{ + if (loc >= sizeof(buffer)-(n-1)) { + return; + } + if (memcmp(src, &buffer[loc], n) != 0) { + lock.take(HAL_SEMAPHORE_BLOCK_FOREVER); + memcpy(&buffer[loc], src, n); + dirty = true; + lock.give(); + } +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_QURT/Storage.h b/libraries/AP_HAL_QURT/Storage.h new file mode 100644 index 0000000000000..6f5771150b00c --- /dev/null +++ b/libraries/AP_HAL_QURT/Storage.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include "AP_HAL_QURT_Namespace.h" +#include "Semaphores.h" +#include + +#define QURT_STORAGE_SIZE HAL_STORAGE_SIZE + +class QURT::Storage : public AP_HAL::Storage +{ +public: + Storage() {} + + static Storage *from(AP_HAL::Storage *storage) { + return static_cast(storage); + } + + void init() {} + void read_block(void *dst, uint16_t src, size_t n); + void write_block(uint16_t dst, const void* src, size_t n); + + static volatile bool dirty; + static uint8_t buffer[QURT_STORAGE_SIZE]; + static Semaphore lock; +}; + diff --git a/libraries/AP_HAL_QURT/Thread.cpp b/libraries/AP_HAL_QURT/Thread.cpp new file mode 100644 index 0000000000000..9123903841121 --- /dev/null +++ b/libraries/AP_HAL_QURT/Thread.cpp @@ -0,0 +1,186 @@ +/* + * Copyright (C) 2016 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "Thread.h" + +#include +#include +#include +#include +#include + +#include +#include +#include "Scheduler.h" + +extern const AP_HAL::HAL &hal; + +namespace QURT { + + +void *Thread::_run_trampoline(void *arg) +{ + Thread *thread = static_cast(arg); + thread->_run(); + + if (thread->_auto_free) { + delete thread; + } + + return nullptr; +} + +bool Thread::_run() +{ + if (!_task) { + return false; + } + + _task(); + + return true; +} + +size_t Thread::get_stack_usage() +{ + return 0; +} + +bool Thread::start(const char *name, int policy, int prio) +{ + if (_started) { + return false; + } + + struct sched_param param = { .sched_priority = prio }; + pthread_attr_t attr; + int r; + + pthread_attr_init(&attr); + + /* + we need to run as root to get realtime scheduling. Allow it to + run as non-root for debugging purposes, plus to allow the Replay + tool to run + */ + if ((r = pthread_attr_setschedparam(&attr, ¶m)) != 0) { + AP_HAL::panic("Failed to set attributes for thread '%s': %s", + name, strerror(r)); + } + + _stack_size = (_stack_size+1023) & ~1023; + HAP_PRINTF("Using stack_size=%u", unsigned(_stack_size)); + if (_stack_size) { + if (pthread_attr_setstacksize(&attr, _stack_size) != 0) { + return false; + } + } + + r = pthread_create(&_ctx, &attr, &Thread::_run_trampoline, this); + if (r != 0) { + AP_HAL::panic("Failed to create thread '%s': %s", + name, strerror(r)); + } + pthread_attr_destroy(&attr); + + _started = true; + + return true; +} + +bool Thread::is_current_thread() +{ + return pthread_equal(pthread_self(), _ctx); +} + +bool Thread::join() +{ + void *ret; + + if (_ctx == 0) { + return false; + } + + if (pthread_join(_ctx, &ret) != 0 || + (intptr_t)ret != 0) { + return false; + } + + return true; +} + + +bool PeriodicThread::set_rate(uint32_t rate_hz) +{ + if (_started || rate_hz == 0) { + return false; + } + + _period_usec = hz_to_usec(rate_hz); + + return true; +} + +bool Thread::set_stack_size(size_t stack_size) +{ + if (_started) { + return false; + } + + _stack_size = MAX(stack_size, (size_t) PTHREAD_STACK_MIN); + + return true; +} + +bool PeriodicThread::_run() +{ + if (_period_usec == 0) { + return false; + } + + uint64_t next_run_usec = AP_HAL::micros64() + _period_usec; + + while (!_should_exit) { + uint64_t dt = next_run_usec - AP_HAL::micros64(); + if (dt > _period_usec) { + // we've lost sync - restart + next_run_usec = AP_HAL::micros64(); + } else { + qurt_timer_sleep(dt); + } + next_run_usec += _period_usec; + + _task(); + } + + _started = false; + _should_exit = false; + + return true; +} + +bool PeriodicThread::stop() +{ + if (!is_started()) { + return false; + } + + _should_exit = true; + + return true; +} + +} diff --git a/libraries/AP_HAL_QURT/Thread.h b/libraries/AP_HAL_QURT/Thread.h new file mode 100644 index 0000000000000..9d351bdab3539 --- /dev/null +++ b/libraries/AP_HAL_QURT/Thread.h @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2016 Intel Corporation. All rights reserved. + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include +#include +#include + +#include + +namespace QURT { + +/* + * Interface abstracting threads + */ +class Thread { +public: + FUNCTOR_TYPEDEF(task_t, void); + + Thread(task_t t) : _task(t) { } + + virtual ~Thread() { } + + bool start(const char *name, int policy, int prio); + + bool is_current_thread(); + + bool is_started() const { return _started; } + + size_t get_stack_usage(); + + bool set_stack_size(size_t stack_size); + + void set_auto_free(bool auto_free) { _auto_free = auto_free; } + + virtual bool stop() { return false; } + + bool join(); + +protected: + static void *_run_trampoline(void *arg); + + /* + * Run the task assigned in the constructor. May be overriden in case it's + * preferred to use Thread as an interface or when user wants to aggregate + * some initialization or teardown for the thread. + */ + virtual bool _run(); + + void _poison_stack(); + + task_t _task; + bool _started = false; + bool _should_exit = false; + bool _auto_free = false; + pthread_t _ctx = 0; + + struct stack_debug { + uint32_t *start; + uint32_t *end; + } _stack_debug; + + size_t _stack_size = 0; +}; + +class PeriodicThread : public Thread { +public: + PeriodicThread(Thread::task_t t) + : Thread(t) + { } + + bool set_rate(uint32_t rate_hz); + + bool stop() override; + +protected: + bool _run() override; + + uint64_t _period_usec = 0; +}; + +} diff --git a/libraries/AP_HAL_QURT/UARTDriver.cpp b/libraries/AP_HAL_QURT/UARTDriver.cpp new file mode 100644 index 0000000000000..d3845ccc1fcbb --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.cpp @@ -0,0 +1,46 @@ + +#include "UARTDriver.h" +#include + +extern const AP_HAL::HAL& hal; + +QURT::UARTDriver::UARTDriver(const char *name) +{ + if (strcmp(name, "/dev/console") == 0) { + _is_console = true; + HAP_PRINTF("UART console created"); + } +} + +void QURT::UARTDriver::printf(const char *fmt, ...) +{ + if (_is_console) { + va_list ap; + char buf[300]; + va_start(ap, fmt); + vsnprintf(buf, sizeof(buf), fmt, ap); + va_end(ap); + HAP_PRINTF(buf); + qurt_timer_sleep(5000); + } +} + +/* QURT implementations of virtual methods */ +void QURT::UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) {} +void QURT::UARTDriver::_end() {} +void QURT::UARTDriver::_flush() {} +bool QURT::UARTDriver::is_initialized() { return false; } +bool QURT::UARTDriver::tx_pending() { return false; } + +uint32_t QURT::UARTDriver::_available() { return 0; } +uint32_t QURT::UARTDriver::txspace() { return 1; } +bool QURT::UARTDriver::_discard_input() { return false; } +size_t QURT::UARTDriver::_write(const uint8_t *buffer, size_t size) +{ + return size; +} +ssize_t QURT::UARTDriver::_read(uint8_t *buffer, uint16_t size) +{ + return 0; +} + diff --git a/libraries/AP_HAL_QURT/UARTDriver.h b/libraries/AP_HAL_QURT/UARTDriver.h new file mode 100644 index 0000000000000..438540f67518f --- /dev/null +++ b/libraries/AP_HAL_QURT/UARTDriver.h @@ -0,0 +1,46 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "AP_HAL_QURT.h" +#include "Semaphores.h" +#include + +#define CONSOLE_BUFFER_SIZE 64 + +class QURT::UARTDriver : public AP_HAL::UARTDriver { +public: + UARTDriver(const char *name); + + bool is_initialized() override; + bool tx_pending() override; + + /* Empty implementations of Stream virtual methods */ + uint32_t txspace() override; + + void printf(const char *fmt, ...) override; + +protected: + void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; + size_t _write(const uint8_t *buffer, size_t size) override; + ssize_t _read(uint8_t *buffer, uint16_t size) override WARN_IF_UNUSED; + void _end() override; + void _flush() override; + uint32_t _available() override; + bool _discard_input() override; + bool _is_console{ false }; + char _console_buffer[CONSOLE_BUFFER_SIZE]; +}; diff --git a/libraries/AP_HAL_QURT/Util.cpp b/libraries/AP_HAL_QURT/Util.cpp new file mode 100644 index 0000000000000..391a8cb3cf1db --- /dev/null +++ b/libraries/AP_HAL_QURT/Util.cpp @@ -0,0 +1,71 @@ +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "Semaphores.h" +#include + +extern const AP_HAL::HAL& hal; + +#include "Util.h" + +using namespace QURT; + +#if ENABLE_HEAP +void *Util::allocate_heap_memory(size_t size) +{ + struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap)); + if (new_heap != nullptr) { + new_heap->max_heap_size = size; + new_heap->current_heap_usage = 0; + } + return (void *)new_heap; +} + +void *Util::heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) +{ + if (h == nullptr) { + return nullptr; + } + + struct heap *heapp = (struct heap*)h; + + // extract appropriate headers. We use the old_size from the + // header not from the caller. We use SITL to catch cases they + // don't match (which would be a lua bug) + old_size = 0; + heap_allocation_header *old_header = nullptr; + if (ptr != nullptr) { + old_header = ((heap_allocation_header *)ptr) - 1; + old_size = old_header->allocation_size; + } + + if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) { + // fail the allocation as we don't have the memory. Note that we don't simulate fragmentation + return nullptr; + } + + heapp->current_heap_usage -= old_size; + if (new_size == 0) { + free(old_header); + return nullptr; + } + + heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header)); + if (new_header == nullptr) { + // total failure to allocate, this is very surprising in SITL + return nullptr; + } + heapp->current_heap_usage += new_size; + new_header->allocation_size = new_size; + void *new_mem = new_header + 1; + + if (ptr == nullptr) { + return new_mem; + } + memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size); + free(old_header); + return new_mem; +} + +#endif // ENABLE_HEAP + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_QURT diff --git a/libraries/AP_HAL_QURT/Util.h b/libraries/AP_HAL_QURT/Util.h new file mode 100644 index 0000000000000..9f42cf7bf7e3d --- /dev/null +++ b/libraries/AP_HAL_QURT/Util.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include "AP_HAL_QURT_Namespace.h" + +class QURT::Util : public AP_HAL::Util { +public: + /* + set HW RTC in UTC microseconds + */ + void set_hw_rtc(uint64_t time_utc_usec) override {} + + /* + get system clock in UTC microseconds + */ + uint64_t get_hw_rtc() const override { return 0; } + +#if ENABLE_HEAP + // heap functions, note that a heap once alloc'd cannot be dealloc'd + virtual void *allocate_heap_memory(size_t size) override; + virtual void *heap_realloc(void *h, void *ptr, size_t old_size, size_t new_size) override; +#endif // ENABLE_HEAP + +#if ENABLE_HEAP + struct heap_allocation_header { + size_t allocation_size; // size of allocated block, not including this header + }; + + struct heap { + size_t max_heap_size; + size_t current_heap_usage; + }; +#endif // ENABLE_HEAP +}; diff --git a/libraries/AP_HAL_QURT/interface.h b/libraries/AP_HAL_QURT/interface.h new file mode 100644 index 0000000000000..c70055883831f --- /dev/null +++ b/libraries/AP_HAL_QURT/interface.h @@ -0,0 +1,62 @@ +#define __EXPORT __attribute__ ((visibility ("default"))) + +// Should be in termios.h +typedef unsigned int speed_t; + +// TODO: This has to be defined in the slpi_proc build and in the PX4 build. +// Make it accessible from one file to both builds. +typedef struct { + int (*advertise_func_ptr)(const char *topic_name); + int (*subscribe_func_ptr)(const char *topic_name); + int (*unsubscribe_func_ptr)(const char *topic_name); + int (*topic_data_func_ptr)(const char *name, const uint8_t *data, int data_len_in_bytes); + + /* + get a fd for the SPI bus + */ + int (*_config_spi_bus_func_t)(); + + /* + perform a SPI bus transfer + */ + int (*_spi_transfer_func_t)(int fd, const uint8_t *send, uint8_t *recv, const unsigned length); + + /* + configure an I2C device, returns a file descriptor + */ + int (*_config_i2c_bus_func_t)(uint8_t busnum, uint8_t i2c_address, uint32_t frequency); + + /* + change I2C address + */ + int (*_set_i2c_address_func_t)(int fd, uint8_t i2c_address); + + /* + I2C transfer, returns 0 on success + */ + int (*_i2c_transfer_func_t)(int fd, const uint8_t *send_data, const unsigned send_len, uint8_t *recv_data, const unsigned recv_len); + + int (*open_uart_func_t)(uint8_t, speed_t); + int (*write_uart_func_t)(int, const void *, size_t); + int (*read_uart_func_t)(int, void *, size_t); + int (*register_interrupt_callback)(int (*)(int, void *, void *), void *arg); +} qurt_func_ptrs_t; + +#ifndef __cplusplus +#error "C++ should be defined!!!" +#endif + +extern "C" { + int px4muorb_orb_initialize(qurt_func_ptrs_t *func_ptrs, int32_t clock_offset_us) __EXPORT; + + int px4muorb_topic_advertised(const char *name) __EXPORT; + + int px4muorb_add_subscriber(const char *name) __EXPORT; + + int px4muorb_remove_subscriber(const char *name) __EXPORT; + + int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + + float px4muorb_get_cpu_load(void) __EXPORT; + +} diff --git a/libraries/AP_HAL_QURT/replace.cpp b/libraries/AP_HAL_QURT/replace.cpp new file mode 100644 index 0000000000000..343a8ae9388cf --- /dev/null +++ b/libraries/AP_HAL_QURT/replace.cpp @@ -0,0 +1,191 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "replace.h" +#include "interface.h" + +extern qurt_func_ptrs_t qurt_func_ptrs; + +extern "C" { + +// this is not declared in qurt headers +void HAP_debug(const char *msg, int level, const char *filename, int line); + +void HAP_printf(const char *file, int line, const char *format, ...) +{ + va_list ap; + char buf[300]; + + va_start(ap, format); + vsnprintf(buf, sizeof(buf), format, ap); + va_end(ap); + HAP_debug(buf, 0, file, line); + //usleep(20000); +} + +/** + QURT doesn't have strnlen() +**/ +size_t strnlen(const char *s, size_t max) +{ + size_t len; + + for (len = 0; len < max; len++) { + if (s[len] == '\0') { + break; + } + } + return len; +} + +int vasprintf(char **ptr, const char *format, va_list ap) +{ + int ret; + va_list ap2; + + va_copy(ap2, ap); + ret = vsnprintf(nullptr, 0, format, ap2); + va_end(ap2); + if (ret < 0) return ret; + + (*ptr) = (char *)malloc(ret+1); + if (!*ptr) return -1; + + va_copy(ap2, ap); + ret = vsnprintf(*ptr, ret+1, format, ap2); + va_end(ap2); + + return ret; +} + +int asprintf(char **ptr, const char *format, ...) +{ + va_list ap; + int ret; + + *ptr = nullptr; + va_start(ap, format); + ret = vasprintf(ptr, format, ap); + va_end(ap); + + return ret; +} + +void *memmem(const void *haystack, size_t haystacklen, + const void *needle, size_t needlelen) +{ + if (needlelen == 0) { + return const_cast(haystack); + } + while (haystacklen >= needlelen) { + char *p = (char *)memchr(haystack, *(const char *)needle, + haystacklen-(needlelen-1)); + if (!p) return NULL; + if (memcmp(p, needle, needlelen) == 0) { + return p; + } + haystack = p+1; + haystacklen -= (p - (const char *)haystack) + 1; + } + return NULL; +} + +char *strndup(const char *s, size_t n) +{ + char *ret; + + n = strnlen(s, n); + ret = (char*)malloc(n+1); + if (!ret) { + return NULL; + } + memcpy(ret, s, n); + ret[n] = 0; + + return ret; +} + +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) +{ + return 0; +} + +// INVESTIGATE: What is this needed on QURT? +int apfs_rename(const char *oldpath, const char *newpath) +{ + return 0; +} + +// INVESTIGATE: How to enable +void lua_abort() {} +const char* lua_get_modules_path() {return NULL;} +int lua_get_current_ref() {return 0;} + +// INVESTIGATE: Seems important :-) +int ArduPilot_main(int argc, const char *argv[]) +{ + return 0; +} + +} + +extern "C" int qurt_arducopter_main(int argc, char* const argv[]); + +int px4muorb_orb_initialize(qurt_func_ptrs_t *func_ptrs, int32_t clock_offset_us) +{ + HAP_PRINTF("About to call qurt_arducopter_main %p", &qurt_arducopter_main); + + qurt_func_ptrs = *func_ptrs; + HAP_PRINTF("qurt_func_ptrs->_config_i2c_bus_func_t=%p", qurt_func_ptrs._config_i2c_bus_func_t); + + qurt_arducopter_main(0, NULL); + + HAP_PRINTF("qurt_arducopter_main RETURNED"); + + return 0; +} + +int px4muorb_topic_advertised(const char *name) +{ + return 0; +} + +int px4muorb_add_subscriber(const char *name) +{ + return 0; +} + +int px4muorb_remove_subscriber(const char *name) +{ + return 0; +} + +int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) +{ + return 0; +} + +float px4muorb_get_cpu_load(void) +{ + return 0.0f; +} + +int __wrap_printf(const char *fmt, ...) +{ + va_list ap; + + char buf[300]; + va_start(ap, fmt); + vsnprintf(buf, sizeof(buf), fmt, ap); + va_end(ap); + HAP_PRINTF(buf); + qurt_timer_sleep(5000); + + return 0; +} diff --git a/libraries/AP_HAL_QURT/replace.h b/libraries/AP_HAL_QURT/replace.h new file mode 100644 index 0000000000000..33d6c83b4b5b3 --- /dev/null +++ b/libraries/AP_HAL_QURT/replace.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + /* + work around broken headers + */ + size_t strnlen(const char *s, size_t maxlen); + char *strndup(const char *s, size_t n); + int asprintf(char **, const char *, ...); + off_t lseek(int, off_t, int); + DIR *opendir (const char *); + int unlink(const char *pathname); + void *memmem(const void *haystack, size_t haystacklen, + const void *needle, size_t needlelen); + + //typedef int32_t pid_t; + pid_t getpid (void); + + void HAP_printf(const char *file, int line, const char *fmt, ...); + + int __wrap_printf(const char *fmt, ...); + +#ifdef __cplusplus +} +#endif + +#define HAP_PRINTF(...) HAP_printf(__FILE__, __LINE__, __VA_ARGS__) + +extern volatile int _last_dsp_line; +extern volatile const char *_last_dsp_file; +extern volatile uint32_t _last_counter; + +#define HAP_LINE() do { _last_dsp_line = __LINE__; _last_dsp_file = __FILE__; _last_counter++; } while (0) + +// missing defines from math.h +#define M_SQRT1_2 0.70710678118654752440 diff --git a/libraries/AP_HAL_QURT/system.cpp b/libraries/AP_HAL_QURT/system.cpp new file mode 100644 index 0000000000000..bff78c8c0b756 --- /dev/null +++ b/libraries/AP_HAL_QURT/system.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +#include + +#include +#include + +#include "replace.h" +#include + +extern const AP_HAL::HAL& hal; + +namespace AP_HAL { + +static struct { + uint64_t start_time; +} state; + +void init() +{ + state.start_time = micros64(); + // we don't want exceptions in flight code. That is the job of SITL + feclearexcept(FE_OVERFLOW | FE_DIVBYZERO | FE_INVALID); +} + +void panic(const char *errormsg, ...) +{ + char buf[200]; + va_list ap; + va_start(ap, errormsg); + vsnprintf(buf, sizeof(buf), errormsg, ap); + va_end(ap); + HAP_PRINTF(buf); + qurt_timer_sleep(100000); + exit(1); +} + +uint32_t micros() +{ + return micros64() & 0xFFFFFFFF; +} + +uint32_t millis() +{ + return millis64() & 0xFFFFFFFF; +} + +uint64_t micros64() +{ + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + uint64_t ret = ts.tv_sec*1000*1000ULL + ts.tv_nsec/1000U; + ret -= state.start_time; + return ret; +} + +uint64_t millis64() +{ + return micros64() / 1000; +} + +} // namespace AP_HAL diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp index f5c6bbeeaaef9..727456e5157a0 100644 --- a/libraries/AP_HAL_SITL/Util.cpp +++ b/libraries/AP_HAL_SITL/Util.cpp @@ -85,7 +85,7 @@ bool HALSITL::Util::get_system_id(char buf[50]) return get_system_id_unformatted((uint8_t *)buf, len); } -#ifdef ENABLE_HEAP +#if ENABLE_HEAP void *HALSITL::Util::allocate_heap_memory(size_t size) { struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap)); diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h index 588fc2a25a311..ac73c6a365a23 100644 --- a/libraries/AP_HAL_SITL/Util.h +++ b/libraries/AP_HAL_SITL/Util.h @@ -17,10 +17,6 @@ class HALSITL::Util : public AP_HAL::Util { Util(SITL_State *_sitlState) : sitlState(_sitlState) {} - bool run_debug_shell(AP_HAL::BetterStream *stream) override { - return false; - } - /** how much free memory do we have in bytes. */ @@ -47,7 +43,7 @@ class HALSITL::Util : public AP_HAL::Util { bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) override; void dump_stack_trace(); -#ifdef ENABLE_HEAP +#if ENABLE_HEAP // heap functions, note that a heap once alloc'd cannot be dealloc'd void *allocate_heap_memory(size_t size) override; void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) override; @@ -94,7 +90,7 @@ class HALSITL::Util : public AP_HAL::Util { static ToneAlarm_SF _toneAlarm; #endif -#ifdef ENABLE_HEAP +#if ENABLE_HEAP struct heap_allocation_header { size_t allocation_size; // size of allocated block, not including this header }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index bf62d4cdfced3..816fbeff5164d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -392,7 +392,7 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = { // @Description: Conrols when automatic gyro calibration is performed // @Values: 0:Never, 1:Start-up only // @User: Advanced - AP_GROUPINFO("_GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 1), + AP_GROUPINFO("_GYR_CAL", 24, AP_InertialSensor, _gyro_cal_timing, 0), // @Param: _TRIM_OPTION // @DisplayName: Accel cal trim option diff --git a/libraries/AP_JSON/AP_JSON.cpp b/libraries/AP_JSON/AP_JSON.cpp index 32ee85fe2beef..5d2e4c3dae0db 100644 --- a/libraries/AP_JSON/AP_JSON.cpp +++ b/libraries/AP_JSON/AP_JSON.cpp @@ -41,6 +41,9 @@ #include #include +// only needed for simulation environments for now +#if AP_SIM_ENABLED + /* load JSON file, returning a value object or nullptr on failure */ @@ -716,3 +719,5 @@ std::string AP_JSON::parse(value &out, const std::string &s) ::parse(out, s.begin(), s.end(), &err); return err; } + +#endif // AP_SIM_ENABLED diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp index 0f72760d02c30..6c2130615302c 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_PX4Flow.cpp @@ -78,7 +78,7 @@ bool AP_OpticalFlow_PX4Flow::scan_buses(void) struct i2c_integral_frame frame; success = tdev->read_registers(REG_INTEGRAL_FRAME, (uint8_t *)&frame, sizeof(frame)); if (success) { - printf("Found PX4Flow on bus %u\n", bus); + printf("Found PX4Flow on bus %u\n", unsigned(bus)); dev = std::move(tdev); break; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp index 608f2492efa3b..37cf958647ccb 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp @@ -207,7 +207,7 @@ bool AP_RangeFinder_PulsedLightLRF::init(void) } } - printf("Found LidarLite device=0x%x v2=%d v3hp=%d\n", _dev->get_bus_id(), (int)v2_hardware, (int)v3hp_hardware); + printf("Found LidarLite device=0x%x v2=%d v3hp=%d\n", unsigned(_dev->get_bus_id()), (int)v2_hardware, (int)v3hp_hardware); _dev->get_semaphore()->give(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp index 51c10dec8cd00..e83a0994437ed 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L0X.cpp @@ -259,7 +259,7 @@ bool AP_RangeFinder_VL53L0X::check_id(void) v2 != 0xAA) { return false; } - printf("Detected VL53L0X on bus 0x%x\n", dev->get_bus_id()); + printf("Detected VL53L0X on bus 0x%x\n", unsigned(dev->get_bus_id())); return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp index e0eba359eee8f..ffced316d2276 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_VL53L1X.cpp @@ -81,7 +81,7 @@ bool AP_RangeFinder_VL53L1X::check_id(void) (v2 != 0xCC)) { return false; } - printf("Detected VL53L1X on bus 0x%x\n", dev->get_bus_id()); + printf("Detected VL53L1X on bus 0x%x\n", unsigned(dev->get_bus_id())); return true; } diff --git a/libraries/AP_Scripting/lua/src/luac.c b/libraries/AP_Scripting/lua/src/luac.c index 549ad3950047d..03b101d40fa02 100644 --- a/libraries/AP_Scripting/lua/src/luac.c +++ b/libraries/AP_Scripting/lua/src/luac.c @@ -1,3 +1,4 @@ +#if 0 /* ** $Id: luac.c,v 1.76 2018/06/19 01:32:02 lhf Exp $ ** Lua compiler (saves bytecodes to files; also lists bytecodes) @@ -448,3 +449,5 @@ static void PrintFunction(const Proto* f, int full) if (full) PrintDebug(f); for (i=0; ip[i],full); } + +#endif diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b05851bdfe7f8..959c607adae13 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -294,6 +294,10 @@ AP_Vehicle& vehicle = *AP_Vehicle::get_singleton(); extern AP_Vehicle& vehicle; #endif +#include +// #define GOT_HERE() do { DEV_PRINTF("got to %s:%d\n", __FILE__, __LINE__); qurt_timer_sleep(5000); } while(false); +#define GOT_HERE() + /* setup is called when the sketch starts */ @@ -410,27 +414,36 @@ void AP_Vehicle::setup() // init_ardupilot is where the vehicle does most of its initialisation. init_ardupilot(); +GOT_HERE(); + #if AP_SCRIPTING_ENABLED scripting.init(); #endif // AP_SCRIPTING_ENABLED +GOT_HERE(); + #if AP_AIRSPEED_ENABLED airspeed.init(); +GOT_HERE(); if (airspeed.enabled()) { airspeed.calibrate(true); } +GOT_HERE(); #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) else { +GOT_HERE(); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "No airspeed sensor"); } #endif #endif // AP_AIRSPEED_ENABLED +GOT_HERE(); #if AP_SRV_CHANNELS_ENABLED SRV_Channels::init(); #endif +GOT_HERE(); // gyro FFT needs to be initialized really late #if HAL_GYROFFT_ENABLED #if AP_SCHEDULER_ENABLED @@ -439,84 +452,105 @@ void AP_Vehicle::setup() gyro_fft.init(1000); #endif #endif +GOT_HERE(); #if HAL_RUNCAM_ENABLED runcam.init(); #endif +GOT_HERE(); #if HAL_HOTT_TELEM_ENABLED hott_telem.init(); #endif +GOT_HERE(); #if HAL_VISUALODOM_ENABLED // init library used for visual position estimation visual_odom.init(); #endif +GOT_HERE(); #if AP_VIDEOTX_ENABLED vtx.init(); #endif +GOT_HERE(); #if AP_SMARTAUDIO_ENABLED smartaudio.init(); #endif +GOT_HERE(); #if AP_TRAMP_ENABLED tramp.init(); #endif +GOT_HERE(); #if AP_PARAM_KEY_DUMP AP_Param::show_all(hal.console, true); #endif +GOT_HERE(); send_watchdog_reset_statustext(); +GOT_HERE(); #if AP_OPENDRONEID_ENABLED opendroneid.init(); #endif // init EFI monitoring +GOT_HERE(); #if HAL_EFI_ENABLED efi.init(); #endif +GOT_HERE(); #if AP_TEMPERATURE_SENSOR_ENABLED temperature_sensor.init(); #endif +GOT_HERE(); #if AP_KDECAN_ENABLED kdecan.init(); #endif +GOT_HERE(); #if AP_AIS_ENABLED ais.init(); #endif +GOT_HERE(); #if HAL_NMEA_OUTPUT_ENABLED nmea.init(); #endif +GOT_HERE(); #if AP_FENCE_ENABLED fence.init(); #endif +GOT_HERE(); #if AP_CUSTOMROTATIONS_ENABLED custom_rotations.init(); #endif +GOT_HERE(); #if AP_FILTER_ENABLED filters.init(); #endif +GOT_HERE(); #if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED for (uint8_t i = 0; i +#include + +#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) #define FORCE_VERSION_H_INCLUDE #include @@ -12,6 +15,7 @@ #undef FORCE_VERSION_H_INCLUDE const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] {}; +#endif /* send_text implementation for dummy GCS diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 720142a839ef5..920bd6a406991 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -588,6 +588,7 @@ void GCS_MAVLINK::ftp_worker(void) { // calculates how much string length is needed to fit this in a list response int GCS_MAVLINK::gen_dir_entry(char *dest, size_t space, const char *path, const struct dirent * entry) { +#if AP_FILESYSTEM_FILE_READING_ENABLED const bool is_file = entry->d_type == DT_REG || entry->d_type == DT_LNK; if (space < 3) { @@ -616,6 +617,9 @@ int GCS_MAVLINK::gen_dir_entry(char *dest, size_t space, const char *path, const } else { return hal.util->snprintf(dest, space, "D%s%c", entry->d_name, (char)0); } +#else + return -1; +#endif // AP_FILESYSTEM_FILE_READING_ENABLED } // list the contents of a directory, skip the offset number of entries before providing data