diff --git a/.github/workflows/colcon.yml b/.github/workflows/colcon.yml index e35c782c0c87de..4d59e04062b0d3 100644 --- a/.github/workflows/colcon.yml +++ b/.github/workflows/colcon.yml @@ -156,7 +156,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 36a76dd1d6674d..1cf7932a1aa457 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -177,7 +177,7 @@ jobs: source ~/ccache.conf && ccache -s - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: D:/a/ardupilot/ardupilot/ccache key: ${{ steps.ccache_cache_timestamp.outputs.cache-key }}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/macos_build.yml b/.github/workflows/macos_build.yml index 0ad9e37f55bf73..bfa7010904366b 100644 --- a/.github/workflows/macos_build.yml +++ b/.github/workflows/macos_build.yml @@ -169,7 +169,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index ff865d9e110695..22c97610038df1 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -182,7 +182,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{ matrix.gcc }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index c6dba24a392fc1..150159b4c3ae8e 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -48,7 +48,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_dds.yml b/.github/workflows/test_dds.yml index 22fa60653ba5bb..9001de83a5ed34 100644 --- a/.github/workflows/test_dds.yml +++ b/.github/workflows/test_dds.yml @@ -161,7 +161,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.config }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml index 6e69b7362c8d44..2bec49b8bad9f0 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -181,7 +181,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{matrix.config}}-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_replay.yml b/.github/workflows/test_replay.yml index 99dfa7c97b6c08..7799d0317aec47 100644 --- a/.github/workflows/test_replay.yml +++ b/.github/workflows/test_replay.yml @@ -170,7 +170,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_scripts.yml b/.github/workflows/test_scripts.yml index 94559a422abb9d..923365dafcd97a 100644 --- a/.github/workflows/test_scripts.yml +++ b/.github/workflows/test_scripts.yml @@ -19,6 +19,7 @@ jobs: python-cleanliness, astyle-cleanliness, validate_board_list, + logger_metadata, ] steps: # git checkout the PR diff --git a/.github/workflows/test_sitl_blimp.yml b/.github/workflows/test_sitl_blimp.yml index 1d592fedb89c9a..7d9e55e51b2c85 100644 --- a/.github/workflows/test_sitl_blimp.yml +++ b/.github/workflows/test_sitl_blimp.yml @@ -182,7 +182,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_copter.yml b/.github/workflows/test_sitl_copter.yml index e5712fec1b2f63..07efd7f4d143cf 100644 --- a/.github/workflows/test_sitl_copter.yml +++ b/.github/workflows/test_sitl_copter.yml @@ -181,7 +181,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -302,7 +302,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-base-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 63d65bb5b568ca..de470a8e09ec2f 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -172,7 +172,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -222,7 +222,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_plane.yml b/.github/workflows/test_sitl_plane.yml index 34c112e59b3d3c..813560a94109bd 100644 --- a/.github/workflows/test_sitl_plane.yml +++ b/.github/workflows/test_sitl_plane.yml @@ -181,7 +181,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index d13185aca1c818..3c29c931be908f 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -180,7 +180,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_sub.yml b/.github/workflows/test_sitl_sub.yml index 958787a29b3618..0a8555c114c9c5 100644 --- a/.github/workflows/test_sitl_sub.yml +++ b/.github/workflows/test_sitl_sub.yml @@ -184,7 +184,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_sitl_tracker.yml b/.github/workflows/test_sitl_tracker.yml index 7708647b924e4b..518d648b7f8777 100644 --- a/.github/workflows/test_sitl_tracker.yml +++ b/.github/workflows/test_sitl_tracker.yml @@ -183,7 +183,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_size.yml b/.github/workflows/test_size.yml index c4be1bc0642f44..30e7f4bba2a4dc 100644 --- a/.github/workflows/test_size.yml +++ b/.github/workflows/test_size.yml @@ -97,7 +97,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/test_unit_tests.yml b/.github/workflows/test_unit_tests.yml index 3d94ad2db1f9e6..bc325afb7d3190 100644 --- a/.github/workflows/test_unit_tests.yml +++ b/.github/workflows/test_unit_tests.yml @@ -126,7 +126,7 @@ jobs: NOW=$(date -u +"%F-%T") echo "timestamp=${NOW}" >> $GITHUB_OUTPUT - name: ccache cache files - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: ~/.ccache key: ${{github.workflow}}-ccache-${{ matrix.toolchain }}-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/AntennaTracker/version.h b/AntennaTracker/version.h index eae1a8cad548fe..03c376adf1c849 100644 --- a/AntennaTracker/version.h +++ b/AntennaTracker/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "AntennaTracker V4.6.0-dev" +#define THISFIRMWARE "AntennaTracker V4.7.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 6 +#define FW_MINOR 7 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 47e45969020fc2..675b3b07eab030 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -688,12 +688,29 @@ void Copter::three_hz_loop() low_alt_avoidance(); } +// ap_value calculates a 32-bit bitmask representing various pieces of +// state about the Copter. It replaces a global variable which was +// used to track this state. +uint32_t Copter::ap_value() const +{ + uint32_t ret = 0; + + const bool *b = (const bool *)≈ + for (uint8_t i=0; i= 0) { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (velocity + 60.0f)); - } else { - lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (-velocity + 60.0f)); - } + lean_angle = -brake.gain * velocity * (1.0f + 500.0f / (fabsf(velocity) + 60.0f)); // do not let lean_angle be too far from brake_angle brake_angle = constrain_float(lean_angle, brake_angle - brake_rate, brake_angle + brake_rate); @@ -592,7 +588,7 @@ void ModePosHold::update_wind_comp_estimate() } // limit acceleration - const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * 981.0f; + const float accel_lim_cmss = tanf(radians(POSHOLD_WIND_COMP_LEAN_PCT_MAX * copter.aparm.angle_max * 0.01f)) * (GRAVITY_MSS*100); const float wind_comp_ef_len = wind_comp_ef.length(); if (!is_zero(accel_lim_cmss) && (wind_comp_ef_len > accel_lim_cmss)) { wind_comp_ef *= accel_lim_cmss / wind_comp_ef_len; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 51982efbed93db..7c5ffd3201388d 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -347,7 +347,6 @@ void Copter::update_auto_armed() */ bool Copter::should_log(uint32_t mask) { - ap.logging_started = logger.logging_started(); return logger.should_log(mask); } #endif diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 4d81279d0fdcba..a18f4742adda25 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduCopter V4.6.0-dev" +#define THISFIRMWARE "ArduCopter V4.7.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 6 +#define FW_MINOR 7 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 45a596bc6331a8..d2800c367631ff 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1372,43 +1372,27 @@ void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_mess mavlink_set_position_target_global_int_t pos_target; mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target); + + Location::AltFrame frame; + if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)pos_target.coordinate_frame, frame)) { + gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT"); + // Even though other parts of the command may be valid, reject the whole thing. + return; + } + // Unexpectedly, the mask is expecting "ones" for dimensions that should // be IGNORNED rather than INCLUDED. See mavlink documentation of the // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field. const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3) - bool msg_valid = true; AP_Mission::Mission_Command cmd = {0}; if (pos_target.type_mask & alt_mask) { - cmd.content.location.alt = pos_target.alt * 100; - cmd.content.location.relative_alt = false; - cmd.content.location.terrain_alt = false; - switch (pos_target.coordinate_frame) - { - case MAV_FRAME_GLOBAL: - case MAV_FRAME_GLOBAL_INT: - break; //default to MSL altitude - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT: - cmd.content.location.relative_alt = true; - break; - case MAV_FRAME_GLOBAL_TERRAIN_ALT: - case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT: - cmd.content.location.relative_alt = true; - cmd.content.location.terrain_alt = true; - break; - default: - gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT"); - msg_valid = false; - break; - } - - if (msg_valid) { - handle_change_alt_request(cmd); - } - } // end if alt_mask + const int32_t alt_cm = pos_target.alt * 100; + cmd.content.location.set_alt_cm(alt_cm, frame); + handle_change_alt_request(cmd); + } } MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet) diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b2939557adfdf5..a3da572611c77c 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -64,18 +64,6 @@ enum class RtlAutoland { }; -enum ChannelMixing { - MIXING_DISABLED = 0, - MIXING_UPUP = 1, - MIXING_UPDN = 2, - MIXING_DNUP = 3, - MIXING_DNDN = 4, - MIXING_UPUP_SWP = 5, - MIXING_UPDN_SWP = 6, - MIXING_DNUP_SWP = 7, - MIXING_DNDN_SWP = 8, -}; - // PID broadcast bitmask enum tuning_pid_bits { TUNING_BITS_ROLL = (1 << 0), diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 8fbfd59735cd24..72ac41a5557aad 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -43,12 +43,12 @@ bool ModeLoiterAltQLand::handle_guided_request(Location target_loc) // setup altitude #if AP_TERRAIN_AVAILABLE if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { - target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); + target_loc.set_alt_m(quadplane.qrtl_alt, Location::AltFrame::ABOVE_TERRAIN); } else { - target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); + target_loc.set_alt_m(quadplane.qrtl_alt, Location::AltFrame::ABOVE_HOME); } #else - target_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); + target_loc.set_alt_m(quadplane.qrtl_alt, Location::AltFrame::ABOVE_HOME); #endif plane.set_guided_WP(target_loc); diff --git a/ArduPlane/mode_qland.cpp b/ArduPlane/mode_qland.cpp index f3a08aabde7622..e59e3c9a50e954 100644 --- a/ArduPlane/mode_qland.cpp +++ b/ArduPlane/mode_qland.cpp @@ -9,7 +9,6 @@ bool ModeQLand::_enter() quadplane.throttle_wait = false; quadplane.setup_target_position(); poscontrol.set_state(QuadPlane::QPOS_LAND_DESCEND); - poscontrol.pilot_correction_done = false; quadplane.last_land_final_agl = plane.relative_ground_altitude(plane.g.rangefinder_landing); quadplane.landing_detect.lower_limit_start_ms = 0; quadplane.landing_detect.land_start_ms = 0; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index de820ee42091cb..87be3e11604369 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2905,7 +2905,8 @@ QuadPlane::ActiveFwdThr QuadPlane::get_vfwd_method(void) const /* map from pitch tilt to fwd throttle when enabled */ -void QuadPlane::assign_tilt_to_fwd_thr(void) { +void QuadPlane::assign_tilt_to_fwd_thr(void) +{ const auto fwd_thr_active = get_vfwd_method(); if (fwd_thr_active != ActiveFwdThr::NEW) { @@ -4201,7 +4202,7 @@ uint16_t QuadPlane::get_pilot_velocity_z_max_dn() const { if (is_zero(pilot_speed_z_max_dn)) { return abs(pilot_speed_z_max_up*100); - } + } return abs(pilot_speed_z_max_dn*100); } @@ -4499,8 +4500,9 @@ void SLT_Transition::set_last_fw_pitch() last_fw_nav_pitch_cd = plane.nav_pitch_cd; } -void SLT_Transition::force_transition_complete() { - transition_state = TRANSITION_DONE; +void SLT_Transition::force_transition_complete() +{ + transition_state = TRANSITION_DONE; in_forced_transition = false; transition_start_ms = 0; transition_low_airspeed_ms = 0; @@ -4594,6 +4596,11 @@ void QuadPlane::mode_enter(void) poscontrol.last_velocity_match_ms = 0; poscontrol.set_state(QuadPlane::QPOS_NONE); + // Clear any pilot corrections + poscontrol.pilot_correction_done = false; + poscontrol.pilot_correction_active = false; + poscontrol.target_vel_cms.zero(); + // clear guided takeoff wait on any mode change, but remember the // state for special behaviour guided_wait_takeoff_on_mode_enter = guided_wait_takeoff; diff --git a/ArduPlane/version.h b/ArduPlane/version.h index 2cc0a0941a1605..1994c5d7dba224 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduPlane V4.6.0-dev" +#define THISFIRMWARE "ArduPlane V4.7.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 6 +#define FW_MINOR 7 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/Blimp/version.h b/Blimp/version.h index f60d3c197858dd..b52a011aaa6c90 100644 --- a/Blimp/version.h +++ b/Blimp/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "Blimp V4.5.0-dev" +#define THISFIRMWARE "Blimp V4.7.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 7 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/Dockerfile b/Dockerfile index 22d8d67292ad25..dba07249d65e8c 100644 --- a/Dockerfile +++ b/Dockerfile @@ -20,6 +20,8 @@ RUN apt-get update && apt-get install --no-install-recommends -y \ lsb-release \ sudo \ tzdata \ + git \ + default-jre \ bash-completion COPY Tools/environment_install/install-prereqs-ubuntu.sh /ardupilot/Tools/environment_install/ @@ -39,9 +41,19 @@ RUN SKIP_AP_EXT_ENV=$SKIP_AP_EXT_ENV SKIP_AP_GRAPHIC_ENV=$SKIP_AP_GRAPHIC_ENV SK USER=${USER_NAME} \ Tools/environment_install/install-prereqs-ubuntu.sh -y +# Rectify git perms issue that seems to crop up only on OSX +RUN git config --global --add safe.directory $PWD + # Check that local/bin are in PATH for pip --user installed package RUN echo "if [ -d \"\$HOME/.local/bin\" ] ; then\nPATH=\"\$HOME/.local/bin:\$PATH\"\nfi" >> ~/.ardupilot_env +# Clone & install Micro-XRCE-DDS-Gen dependancy +RUN git clone --recurse-submodules https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git /home/${USER_NAME}/Micro-XRCE-DDS-Gen \ + && cd /home/${USER_NAME}/Micro-XRCE-DDS-Gen \ + && ./gradlew assemble \ + && export AP_ENV_LOC="/home/${USER_NAME}/.ardupilot_env" \ + && echo "export PATH=\$PATH:$PWD/scripts" >> $AP_ENV_LOC + # Create entrypoint as docker cannot do shell substitution correctly RUN export ARDUPILOT_ENTRYPOINT="/home/${USER_NAME}/ardupilot_entrypoint.sh" \ && echo "#!/bin/bash" > $ARDUPILOT_ENTRYPOINT \ diff --git a/Rover/version.h b/Rover/version.h index b0f269dc5c24e1..891bdcc935430c 100644 --- a/Rover/version.h +++ b/Rover/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduRover V4.6.0-dev" +#define THISFIRMWARE "ArduRover V4.7.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,7,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 6 +#define FW_MINOR 7 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 3a4dbddf6c46af..16eda3f59e8dc9 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -291,6 +291,7 @@ AP_HW_PhenixH7_Pro 1172 AP_HW_2RAWH743 1173 AP_HW_X-MAV-AP-H743V2 1174 AP_HW_BETAFPV_F4_2-3S_20A 1175 +AP_HW_MicoAir743AIOv1 1176 AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_FlywooH743Pro 1181 @@ -335,6 +336,9 @@ AP_HW_MFT-SEMA100 2000 AP_HW_SakuraRC-H743 2714 +# IDs 4000-4009 reserved for Karshak Drones +AP_HW_KRSHKF7_MINI 4000 + # IDs 4200-4220 reserved for HAKRC AP_HW_HAKRC_F405 4200 AP_HW_HAKRC_F405Wing 4201 @@ -357,6 +361,13 @@ AP_HW_UAV-DEV-UM982-G4 5224 AP_HW_UAV-DEV-M20D-G4 5225 AP_HW_UAV-DEV-Sensorboard-G4 5226 +# IDs 5240-5249 reserved for TM IT-Systemhaus +AP_HW_TM-SYS-BeastFC 5240 +AP_HW_TM-SYS-Sensornode 5241 +AP_HW_TM-SYS-OpenHDFPV 5242 +AP_HW_TM-SYS-VisualNAV 5243 +AP_HW_TM-SYS-Airspeed 5244 + # IDs 5250-5269 reserved for Team Black Sheep AP_HW_TBS_LUCID_H7 5250 AP_HW_TBS_LUCID_PRO 5251 @@ -401,7 +412,7 @@ AP_HW_DroneBuild_G2 5701 # IDs 7000-7099 reserved for CUAV AP_HW_CUAV-7-NANO 7000 -# IDs 7100-7109 reserved for VIEWPRO +# IDs 7100-7109 reserved for V-UAV AP_HW_VUAV-V7pro 7100 # please fill gaps in the above ranges rather than adding past ID #7109 diff --git a/Tools/AP_Bootloader/support.h b/Tools/AP_Bootloader/support.h index 15b19c0030c5f4..e96137bf8dfae5 100644 --- a/Tools/AP_Bootloader/support.h +++ b/Tools/AP_Bootloader/support.h @@ -1,5 +1,7 @@ #pragma once +#include + #define LED_ACTIVITY 1 #define LED_BOOTLOADER 2 diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 21826edd675fdb..3341a500568405 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -732,7 +732,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @User: Standard GSCALAR(imu_sample_rate, "INS_SAMPLE_RATE", 0), - // @Group: INS_ + // @Group: INS // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp GOBJECT(imu, "INS", AP_InertialSensor), #endif diff --git a/Tools/CodeStyle/astylerc b/Tools/CodeStyle/astylerc index 20bc80e4ec176b..75a467bc1dad1a 100644 --- a/Tools/CodeStyle/astylerc +++ b/Tools/CodeStyle/astylerc @@ -1,6 +1,6 @@ style=linux keep-one-line-statements -add-brackets +add-braces indent=spaces=4 indent-col1-comments min-conditional-indent=0 diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 36a352bfa72a8b..1c28dedc5fb2f5 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -620,13 +620,14 @@ def _select_programs_from_group(bld): else: groups = ['bin'] + possible_groups = list(_grouped_programs.keys()) + possible_groups.remove('bin') # Remove `bin` so as not to duplicate all items in bin if 'all' in groups: - groups = list(_grouped_programs.keys()) - groups.remove('bin') # Remove `bin` so as not to duplicate all items in bin + groups = possible_groups for group in groups: if group not in _grouped_programs: - bld.fatal('Group %s not found' % group) + bld.fatal(f'Group {group} not found, possible groups: {possible_groups}') target_names = _grouped_programs[group].keys() diff --git a/Tools/autotest/logger_metadata/enum_parse.py b/Tools/autotest/logger_metadata/enum_parse.py index 99227a211e9766..d9ab17f4b41afa 100755 --- a/Tools/autotest/logger_metadata/enum_parse.py +++ b/Tools/autotest/logger_metadata/enum_parse.py @@ -1,5 +1,9 @@ #!/usr/bin/env python +''' +AP_FLAKE8_CLEAN +''' + from __future__ import print_function import argparse @@ -10,6 +14,7 @@ topdir = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../../') topdir = os.path.realpath(topdir) + class EnumDocco(object): vehicle_map = { @@ -35,35 +40,35 @@ def match_enum_line(self, line): # attempts to extract name, value and comment from line. # Match: " FRED, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+)\s*,? *(?://[/>]* *(.*) *)?$", line) + m = re.match(r"\s*([A-Z0-9_a-z]+)\s*,? *(?://[/>]* *(.*) *)?$", line) if m is not None: return (m.group(1), None, m.group(2)) # Match: " FRED, /* optional comment */" - m = re.match("\s*([A-Z0-9_a-z]+)\s*,? *(?:/[*] *(.*) *[*]/ *)?$", line) + m = re.match(r"\s*([A-Z0-9_a-z]+)\s*,? *(?:/[*] *(.*) *[*]/ *)?$", line) if m is not None: return (m.group(1), None, m.group(2)) # Match: " FRED = 17, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+)\s*=\s*([-0-9]+)\s*,?(?:\s*//[/<]*\s*(.*) *)?$", + m = re.match(r"\s*([A-Z0-9_a-z]+)\s*=\s*([-0-9]+)\s*,?(?:\s*//[/<]*\s*(.*) *)?$", line) if m is not None: return (m.group(1), m.group(2), m.group(3)) # Match: " FRED = 17, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+) *= *([-0-9]+) *,?(?: */* *(.*) *)? *[*]/ *$", + m = re.match(r"\s*([A-Z0-9_a-z]+) *= *([-0-9]+) *,?(?: */* *(.*) *)? *[*]/ *$", line) if m is not None: return (m.group(1), m.group(2), m.group(3)) # Match: " FRED = 1U<<0, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+) *= *[(]?1U? *[<][<] *(\d+)(?:, *// *(.*) *)?", + m = re.match(r"\s*([A-Z0-9_a-z]+) *= *[(]?1U? *[<][<] *(\d+)(?:, *// *(.*) *)?", line) if m is not None: return (m.group(1), 1 << int(m.group(2)), m.group(3)) # Match: " FRED = 0xabc, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+) *= *(?:0[xX]([0-9A-Fa-f]+))(?:, *// *(.*) *)?", + m = re.match(r"\s*([A-Z0-9_a-z]+) *= *(?:0[xX]([0-9A-Fa-f]+))(?:, *// *(.*) *)?", line) if m is not None: return (m.group(1), int(m.group(2), 16), m.group(3)) @@ -71,19 +76,18 @@ def match_enum_line(self, line): '''start discarded matches - lines we understand but can't do anything with:''' # Match: " FRED = 17, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+) *= *(\w+) *,?(?: *// *(.*) *)?$", + m = re.match(r"\s*([A-Z0-9_a-z]+) *= *(\w+) *,?(?: *// *(.*) *)?$", line) if m is not None: return (None, None, None) # Match: " FRED = FOO(17), // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+) *= *(\w+) *\\( *(\w+) *\\) *,?(?: *// *(.*) *)?$", + m = re.match(r"\s*([A-Z0-9_a-z]+) *= *(\w+) *\\( *(\w+) *\\) *,?(?: *// *(.*) *)?$", line) if m is not None: return (None, None, None) - - # Match: " FRED = 1U<<0, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+) *= *[(]?3U? *[<][<] *(\d+)(?:, *// *(.*) *)?", + # Match: " FRED = 1U<<0, // optional comment" + m = re.match(r"\s*([A-Z0-9_a-z]+) *= *[(]?3U? *[<][<] *(\d+)(?:, *// *(.*) *)?", line) if m is not None: return (m.group(1), 1 << int(m.group(2)), m.group(3)) @@ -112,21 +116,21 @@ def debug(x): break line = line.rstrip() # print("state=%s line: %s" % (state, line)) - if re.match("\s*//.*", line): + if re.match(r"\s*//.*", line): continue if state == "outside": if re.match("class .*;", line) is not None: # forward-declaration of a class continue - m = re.match("class *([:\w]+)", line) + m = re.match(r"class *([:\w]+)", line) if m is not None: in_class = m.group(1) continue - m = re.match("namespace *(\w+)", line) + m = re.match(r"namespace *(\w+)", line) if m is not None: in_class = m.group(1) continue - m = re.match(".*enum\s*(class)? *([\w]+)\s*(?::.*_t)? *{(.*)};", line) + m = re.match(r".*enum\s*(class)? *([\w]+)\s*(?::.*_t)? *{(.*)};", line) if m is not None: # all one one line! Thanks! enum_name = m.group(2) @@ -142,7 +146,7 @@ def debug(x): enumerations.append(new_enumeration) continue - m = re.match(".*enum\s*(class)? *([\w]+)\s*(?::.*_t)? *{", line) + m = re.match(r".*enum\s*(class)? *([\w]+)\s*(?::.*_t)? *{", line) if m is not None: enum_name = m.group(2) debug("%s: %s" % (source_file, enum_name)) @@ -152,15 +156,15 @@ def debug(x): skip_enumeration = False continue if state == "inside": - if re.match("\s*$", line): + if re.match(r"\s*$", line): continue - if re.match("#if", line): + if re.match(r"#if", line): continue - if re.match("#endif", line): + if re.match(r"#endif", line): continue - if re.match("#else", line): + if re.match(r"#else", line): continue - if re.match(".*}\s*\w*(\s*=\s*[\w:]+)?;", line): + if re.match(r".*}\s*\w*(\s*=\s*[\w:]+)?;", line): # potential end of enumeration if not skip_enumeration: if enum_name is None: @@ -189,7 +193,7 @@ def debug(x): else: value = int(value) last_value = value -# print("entry=%s value=%s comment=%s" % (name, value, comment)) + # print("entry=%s value=%s comment=%s" % (name, value, comment)) entries.append(EnumDocco.EnumEntry(name, value, comment)) return enumerations @@ -219,6 +223,9 @@ def search_for_files(self, dirs_to_search): continue if filepath.endswith("libraries/AP_HAL/utility/getopt_cpp.h"): continue + # Failed to match ( IOEVENT_PWM = EVENT_MASK(1),) + if filepath.endswith("libraries/AP_IOMCU/iofirmware/iofirmware.cpp"): + continue self.files.append(filepath) if len(_next): self.search_for_files(_next) @@ -253,4 +260,3 @@ def run(self): sys.exit(1) s.run() -# print("Enumerations: %s" % s.enumerations) diff --git a/Tools/autotest/logger_metadata/parse.py b/Tools/autotest/logger_metadata/parse.py index 2f7517d2182f46..20bac7be3c0132 100755 --- a/Tools/autotest/logger_metadata/parse.py +++ b/Tools/autotest/logger_metadata/parse.py @@ -1,5 +1,9 @@ #!/usr/bin/env python +''' +AP_FLAKE8_CLEAN +''' + from __future__ import print_function import argparse @@ -33,7 +37,11 @@ # Regular expressions for finding message definitions in structure format re_start_messagedef = re.compile(r"^\s*{?\s*LOG_[A-Z0-9_]+_[MSGTA]+[A-Z0-9_]*\s*,") re_deffield = r'[\s\\]*"?([\w\-#?%]+)"?\s*' -re_full_messagedef = re.compile(r'\s*LOG_\w+\s*,\s*\w+\([^)]+\)[\s\\]*,' + f'{re_deffield},{re_deffield},' + r'[\s\\]*"?([\w,]+)"?[\s\\]*,' + f'{re_deffield},{re_deffield}' , re.MULTILINE) +re_full_messagedef = re.compile(r'\s*LOG_\w+\s*,\s*\w+\([^)]+\)[\s\\]*,' + + f'{re_deffield},{re_deffield},' + + r'[\s\\]*"?([\w,]+)"?[\s\\]*,' + + f'{re_deffield},{re_deffield}', + re.MULTILINE) re_fmt_define = re.compile(r'#define\s+(\w+_FMT)\s+"([\w\-#?%]+)"') re_units_define = re.compile(r'#define\s+(\w+_UNITS)\s+"([\w\-#?%]+)"') re_mults_define = re.compile(r'#define\s+(\w+_MULTS)\s+"([\w\-#?%]+)"') @@ -41,7 +49,9 @@ # Regular expressions for finding message definitions in Write calls re_start_writecall = re.compile(r"\s*[AP:]*logger[\(\)]*.Write[StreamingCrcl]*\(") re_writefield = r'\s*"([\w\-#?%,]+)"\s*' -re_full_writecall = re.compile(r'\s*[AP:]*logger[\(\)]*.Write[StreamingCrcl]*\(' + f'{re_writefield},{re_writefield},{re_writefield},({re_writefield},{re_writefield})?' , re.MULTILINE) +re_full_writecall = re.compile(r'\s*[AP:]*logger[\(\)]*.Write[StreamingCrcl]*\(' + + f'{re_writefield},{re_writefield},{re_writefield},({re_writefield},{re_writefield})?', + re.MULTILINE) # Regular expression for extracting unit and multipliers from structure re_units_mults_struct = re.compile(r"^\s*{\s*'([\w\-#?%!/])',"+r'\s*"?([\w\-#?%./]*)"?\s*}') @@ -64,6 +74,7 @@ 1e-9: "n" # nano- } + class LoggerDocco(object): vehicle_map = { @@ -93,7 +104,7 @@ class Docco(object): def __init__(self, name): self.name = name self.url = None - if isinstance(name,list): + if isinstance(name, list): self.description = [None] * len(name) else: self.description = None @@ -104,15 +115,15 @@ def __init__(self, name): def add_name(self, name): # If self.name/description aren't lists, convert them - if isinstance(self.name,str): + if isinstance(self.name, str): self.name = [self.name] self.description = [self.description] # Replace any existing empty descriptions with empty strings - for i in range(0,len(self.description)): + for i in range(0, len(self.description)): if self.description[i] is None: self.description[i] = "" # Extend the name and description lists - if isinstance(name,list): + if isinstance(name, list): self.name.extend(name) self.description.extend([None] * len(name)) else: @@ -120,8 +131,8 @@ def add_name(self, name): self.description.append(None) def set_description(self, desc): - if isinstance(self.description,list): - for i in range(0,len(self.description)): + if isinstance(self.description, list): + for i in range(0, len(self.description)): if self.description[i] is None: self.description[i] = desc else: @@ -147,7 +158,7 @@ def set_field_bits(self, field, bits): count = 0 entries = [] for bit in bits: - entries.append(EnumDocco.EnumEntry(bit, 1<throttle()); - const RC_Channel::AuxSwitchPos yaw = rc().get_channel_pos(AP::rcmap()->yaw()); - const RC_Channel::AuxSwitchPos roll = rc().get_channel_pos(AP::rcmap()->roll()); - const RC_Channel::AuxSwitchPos pitch = rc().get_channel_pos(AP::rcmap()->pitch()); + const RC_Channel::AuxSwitchPos throttle = rc().get_throttle_channel().get_aux_switch_pos(); + const RC_Channel::AuxSwitchPos yaw = rc().get_yaw_channel().get_aux_switch_pos(); + const RC_Channel::AuxSwitchPos roll = rc().get_roll_channel().get_aux_switch_pos(); + const RC_Channel::AuxSwitchPos pitch = rc().get_pitch_channel().get_aux_switch_pos(); Event result = Event::NONE; diff --git a/libraries/AP_Camera/AP_RunCam.h b/libraries/AP_Camera/AP_RunCam.h index 673dba0ff6e732..9eb62999236644 100644 --- a/libraries/AP_Camera/AP_RunCam.h +++ b/libraries/AP_Camera/AP_RunCam.h @@ -27,7 +27,6 @@ #include #include -#include #include #include diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index f61756d7d9ece8..14cb3b36a25996 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -35,6 +35,10 @@ class Location // set altitude void set_alt_cm(int32_t alt_cm, AltFrame frame); + // set_alt_m - set altitude in metres + void set_alt_m(float alt_m, AltFrame frame) { + set_alt_cm(alt_m*100, frame); + } // get altitude (in cm) in the desired frame // returns false on failure to get altitude in the desired frame which can only happen if the original frame or desired frame is: diff --git a/libraries/AP_Common/tests/test_location.cpp b/libraries/AP_Common/tests/test_location.cpp index c4a7cd90dcf9c1..f97bc769c678ea 100644 --- a/libraries/AP_Common/tests/test_location.cpp +++ b/libraries/AP_Common/tests/test_location.cpp @@ -280,6 +280,13 @@ TEST(Location, Tests) EXPECT_EQ(0, test_location4.loiter_xtrack); EXPECT_TRUE(test_location4.initialised()); + // test set_alt_m API: + Location loc = test_home; + loc.set_alt_m(1.71, Location::AltFrame::ABSOLUTE); + int32_t alt_in_cm_from_m; + EXPECT_TRUE(loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_in_cm_from_m)); + EXPECT_EQ(171, alt_in_cm_from_m); + // can't create a Location using a vector here as there's no origin for the vector to be relative to: // const Location test_location_empty{test_vect, Location::AltFrame::ABOVE_HOME}; // EXPECT_FALSE(test_location_empty.get_vector_from_origin_NEU(test_vec3)); diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index f8a6d78cde52fe..7738ac7f7447ed 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -99,6 +99,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("OFS", 1, Compass, _state._priv_instance[0].offset, 0), // @Param: DEC @@ -170,6 +171,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("MOT", 7, Compass, _state._priv_instance[0].motor_compensation, 0), #endif @@ -215,6 +217,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("OFS2", 10, Compass, _state._priv_instance[1].offset, 0), // @Param: MOT2_X @@ -242,6 +245,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("MOT2", 11, Compass, _state._priv_instance[1].motor_compensation, 0), #endif // COMPASS_MAX_INSTANCES @@ -272,6 +276,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss // @Increment: 1 // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("OFS3", 13, Compass, _state._priv_instance[2].offset, 0), // @Param: MOT3_X @@ -299,6 +304,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @Units: mGauss/A // @Increment: 1 // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("MOT3", 14, Compass, _state._priv_instance[2].motor_compensation, 0), #endif // COMPASS_MAX_INSTANCES @@ -390,6 +396,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @DisplayName: Compass soft-iron diagonal Z component // @Description: DIA_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("DIA", 24, Compass, _state._priv_instance[0].diagonals, 1.0), // @Param: ODI_X @@ -408,6 +415,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @DisplayName: Compass soft-iron off-diagonal Z component // @Description: ODI_Z in the compass soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("ODI", 25, Compass, _state._priv_instance[0].offdiagonals, 0), #if COMPASS_MAX_INSTANCES > 1 @@ -427,6 +435,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @DisplayName: Compass2 soft-iron diagonal Z component // @Description: DIA_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("DIA2", 26, Compass, _state._priv_instance[1].diagonals, 1.0), // @Param: ODI2_X @@ -445,6 +454,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @DisplayName: Compass2 soft-iron off-diagonal Z component // @Description: ODI_Z in the compass2 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("ODI2", 27, Compass, _state._priv_instance[1].offdiagonals, 0), #endif // COMPASS_MAX_INSTANCES @@ -465,6 +475,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @DisplayName: Compass3 soft-iron diagonal Z component // @Description: DIA_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("DIA3", 28, Compass, _state._priv_instance[2].diagonals, 1.0), // @Param: ODI3_X @@ -483,6 +494,7 @@ const AP_Param::GroupInfo Compass::var_info[] = { // @DisplayName: Compass3 soft-iron off-diagonal Z component // @Description: ODI_Z in the compass3 soft-iron calibration matrix: [[DIA_X, ODI_X, ODI_Y], [ODI_X, DIA_Y, ODI_Z], [ODI_Y, ODI_Z, DIA_Z]] // @User: Advanced + // @Calibration: 1 AP_GROUPINFO("ODI3", 29, Compass, _state._priv_instance[2].offdiagonals, 0), #endif // COMPASS_MAX_INSTANCES #endif // AP_COMPASS_DIAGONALS_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 36d8d1bf714f64..b7fcf460c37b22 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -348,9 +348,9 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_ if (battery.current_amps(current, instance)) { if (percentage == 100) { msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL - } else if (current < 0.0) { + } else if (is_negative(current)) { msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING - } else if (current > 0.0) { + } else if (is_positive(current)) { msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING } else { msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 49aace09526315..21243e5d08c40f 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -128,7 +128,7 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { // @Param: OPTION // @DisplayName: DroneCAN options // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS,6:UseHimarkServo,7:HobbyWingESC,8:EnableStats,9:EnableFlexDebug // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0), @@ -1503,6 +1503,62 @@ bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) { return true; } +#if AP_SCRIPTING_ENABLED +/* + handle FlexDebug message, holding a copy locally for a lua script to access + */ +void AP_DroneCAN::handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug &msg) +{ + if (!option_is_set(Options::ENABLE_FLEX_DEBUG)) { + return; + } + + // find an existing element in the list + const uint8_t source_node = transfer.source_node_id; + for (auto *p = flexDebug_list; p != nullptr; p = p->next) { + if (p->node_id == source_node && p->msg.id == msg.id) { + p->msg = msg; + p->timestamp_us = uint32_t(transfer.timestamp_usec); + return; + } + } + + // new message ID, add to the list. Note that this gets called + // only from one thread, so no lock needed + auto *p = NEW_NOTHROW FlexDebug; + if (p == nullptr) { + return; + } + p->node_id = source_node; + p->msg = msg; + p->timestamp_us = uint32_t(transfer.timestamp_usec); + p->next = flexDebug_list; + + // link into the list + flexDebug_list = p; +} + +/* + get the last FlexDebug message from a node + */ +bool AP_DroneCAN::get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t ×tamp_us, dronecan_protocol_FlexDebug &msg) const +{ + for (const auto *p = flexDebug_list; p != nullptr; p = p->next) { + if (p->node_id == node_id && p->msg.id == msg_id) { + if (timestamp_us == p->timestamp_us) { + // stale message + return false; + } + timestamp_us = p->timestamp_us; + msg = p->msg; + return true; + } + } + return false; +} + +#endif // AP_SCRIPTING_ENABLED + /* handle LogMessage debug */ diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 7264fad9472fcd..2d6e5ae2b7f78a 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -148,6 +148,7 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { USE_HIMARK_SERVO = (1U<<6), USE_HOBBYWING_ESC = (1U<<7), ENABLE_STATS = (1U<<8), + ENABLE_FLEX_DEBUG = (1U<<9), }; // check if a option is set @@ -176,6 +177,10 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { Canard::Publisher relay_hardpoint{canard_iface}; #endif +#if AP_SCRIPTING_ENABLED + bool get_FlexDebug(uint8_t node_id, uint16_t msg_id, uint32_t ×tamp_us, dronecan_protocol_FlexDebug &msg) const; +#endif + private: void loop(void); @@ -363,6 +368,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { Canard::Server node_info_server{canard_iface, node_info_req_cb}; uavcan_protocol_GetNodeInfoResponse node_info_rsp; +#if AP_SCRIPTING_ENABLED + Canard::ObjCallback FlexDebug_cb{this, &AP_DroneCAN::handle_FlexDebug}; + Canard::Subscriber FlexDebug_listener{FlexDebug_cb, _driver_index}; +#endif + #if AP_DRONECAN_HOBBYWING_ESC_SUPPORT /* Hobbywing ESC support. Note that we need additional meta-data as @@ -409,6 +419,16 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp); void handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp); void handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req); + +#if AP_SCRIPTING_ENABLED + void handle_FlexDebug(const CanardRxTransfer& transfer, const dronecan_protocol_FlexDebug& msg); + struct FlexDebug { + struct FlexDebug *next; + uint32_t timestamp_us; + uint8_t node_id; + dronecan_protocol_FlexDebug msg; + } *flexDebug_list; +#endif }; #endif // #if HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 8e6e49595e7484..2283933a552fa4 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -24,7 +24,7 @@ #include #include -#if defined(__APPLE__) +#if defined(__APPLE__) || defined(__OpenBSD__) #include #elif CONFIG_HAL_BOARD != HAL_BOARD_QURT #include diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index b4acb2fc0499a1..8792efd2488988 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -389,7 +389,7 @@ bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg) Location new_loc = _target_location; new_loc.lat = packet.lat; new_loc.lng = packet.lon; - new_loc.set_alt_cm(packet.alt*100, Location::AltFrame::ABSOLUTE); + new_loc.set_alt_m(packet.alt, Location::AltFrame::ABSOLUTE); // FOLLOW_TARGET is always AMSL, change the provided alt to // above home if we are configured for relative alt diff --git a/libraries/AP_HAL/utility/sparse-endian.h b/libraries/AP_HAL/utility/sparse-endian.h index f562e1166dca06..2162f034e418a3 100644 --- a/libraries/AP_HAL/utility/sparse-endian.h +++ b/libraries/AP_HAL/utility/sparse-endian.h @@ -57,6 +57,12 @@ typedef uint64_t __ap_bitwise be64_t; #undef be64toh #undef le64toh +#if !defined (__BYTE_ORDER) && defined (__OpenBSD__) +#define __BYTE_ORDER __BYTE_ORDER__ +#define __LITTLE_ENDIAN __ORDER_LITTLE_ENDIAN__ +#define __BIG_ENDIAN __ORDER_BIG_ENDIAN__ +#endif + #if __BYTE_ORDER == __LITTLE_ENDIAN #define bswap_16_on_le(x) __bswap_16(x) #define bswap_32_on_le(x) __bswap_32(x) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout1.png b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout1.png new file mode 100644 index 00000000000000..020bb3b6133bdb Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout1.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout2.png b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout2.png new file mode 100644 index 00000000000000..4181a731c50e37 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout2.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout3.png b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout3.png new file mode 100644 index 00000000000000..32090a0f8d997d Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout3.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout4.png b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout4.png new file mode 100644 index 00000000000000..5f9a880728e96d Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot-pinout4.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot.png b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot.png new file mode 100644 index 00000000000000..ef9a979ad866a9 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/MUPilot.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/README.md b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/README.md new file mode 100644 index 00000000000000..4706844c0884b3 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/README.md @@ -0,0 +1,291 @@ +# MUPilot Flight Controller + +The MUPilot flight controller is sold by [MUGIN UAV](http://https://www.muginuav.com/) + +![MUPilot Board](MUPilot.png "MUPilot") + +## Features + + - STM32F765 Microcontroller + - STM32F103 IOMCU + - Three IMUs: ICM20689, MPU6000 and BMI055 + - Internal vibration isolation for IMUs + - Two MS5611 SPI barometers + - IST8310 magnetometer + - MicroSD card slot + - 6 UARTs plus USB + - 14 PWM outputs with selectable 5V or 3.3V output voltage + - Four I2C and two CAN ports + - External Buzzer + - builtin RGB LED + - external safety Switch + - voltage monitoring for servo rail and Vcc + - two dedicated power input ports for external power bricks + +## Mechanical + + - 91mm x 46mm x 31mm + - 106g + +## Connectors + +![MUPilot Pinout1](MUPilot-pinout1.png "Pinout1") +**CAN1/2** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | VCC | +5V | +| 2 | CAN_H | +3.3V | +| 3 | CAN_L | +3.3V | +| 4 | GND | GND | + +**GPS&SAFETY** + +| Pin | Signal | Volt | +| :--: | :-----------: | :---: | +| 1 | VCC | +5V | +| 2 | UART_TX1 | +3.3V | +| 3 | UART_RX1 | +3.3V | +| 4 | I2C1_SCL | +3.3V | +| 5 | I2C1_SDA | +3.3V | +| 6 | SAFETY_SW | +3.3V | +| 7 | SAFETY_SW_LED | +3.3V | +| 8 | 3V3_OUT | +3.3V | +| 9 | BUZZER | +3.3V | +| 10 | GND | GND | + +**I2C1/2/3/4** + +| Pin | Signal | Volt | +| :--: | :------: | :---: | +| 1 | VCC | +5V | +| 2 | I2Cx_SCL | +3.3V | +| 3 | I2Cx _SDA| +3.3V | +| 4 | GND | GND | + +**TELEM1** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | UART_TX2| +3.3V | +| 3 | UART_RX2| +3.3V | +| 4 | CTS | +3.3V | +| 5 | RTS | +3.3V | +| 6 | GND | GND | + +**TELEM2** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | UART_TX6| +3.3V | +| 3 | UART_RX6| +3.3V | +| 4 | CTS | +3.3V | +| 5 | RTS | +3.3V | +| 6 | GND | GND | + +**UART4(GPS2)** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 |UART_TX3 | +3.3V | +| 3 |UART_RX3 | +3.3V | +| 4 |I2C2_SCL | +3.3V | +| 5 |I2C2_SDA | +3.3V | +| 6 | GND | GND | + +**DEBUG** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | VCC | +5V | +| 2 |UART_TX7| +3.3V | +| 3 |UART_RX7| +3.3V | +| 4 | SWDIO | +3.3V | +| 5 | SWCLK | +3.3V | +| 6 | GND | GND | + +![MUPilot Pinout2](MUPilot-pinout2.png "Pinout2") + +**POWER1** + +| Pin | Signal | Volt | +| :--: | :-------------: | :---: | +| 1 | VCC_IN | +5V | +| 2 | VCC_IN | +5V | +| 3 | CURRENT_ADC | +3.3V | +| 4 | VOLTAGE_ADC | +3.3V | +| 5 | GND | GND | +| 6 | GND | GND | + +**POWER2** + +| Pin | Signal | Volt | +| :--: | :----------------: | :---: | +| 1 | VCC_IN | +5V | +| 2 | VCC_IN | +5V | +| 3 |CURRENT_ADC/I2C1_SCL| +3.3V | +| 4 |VOLTAGE_ADC/I2C1_SDA| +3.3V | +| 5 | GND | GND | +| 6 | GND | GND | + +![MUPilot Pinout3](MUPilot-pinout3.png "Pinout3") + +**SBUS OUT** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | - | - | +| 2 |SBUS OUT| +3.3V | +| 3 | GND | GND | + +![MUPilot Pinout4](MUPilot-pinout4.png "Pinout4") + +**ADC** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | ADC_3V3 | +3.3V | +| 3 | ADC_6V6 | +6.6V | +| 4 | GND | GND | + +**SPI5** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | VCC | +5V | +| 2 | SCK | +3.3V | +| 3 | MISO | +3.3V | +| 5 | MOSI | +3.3V | +| 6 | CS1 | +3.3V | +| 7 | CS2 | +3.3V | +| 8 | GND | GND | + +**DSM/SBUS/RSSI** + +| Pin | Signal | Volt | +| :--: | :---------: | :---: | +| 1 | VCC | +5V | +| 2 | DSM/SBUS | +3.3V | +| 3 | RSSI | +3.3V | +| 4 | 3V3_OUT | +3.3V | +| 5 | GND | GND | + +## UART Mapping + + - SERIAL0 -> USB + - SERIAL1 -> UART2 (Telem1) + - SERIAL2 -> UART3 (Telem2) + - SERIAL3 -> UART1 (GPS) + - SERIAL4 -> UART4 (GPS2) + - SERIAL5 -> UART6 (spare) + - SERIAL6 -> UART7 (spare, debug) + - SERIAL7 -> USB2 (SLCAN) + +The Telem1 and Telem2 ports have RTS/CTS pins, the other UARTs do not +have RTS/CTS. + +The UART7 connector is labelled debug, but is available as a general +purpose UART with ArduPilot. + +## RC Input + +RC input is configured on the PPM pin, at one end of the servo rail, +marked RC in the above diagram. This pin supports all unidirectional RC protocols including PPM. The DSM/SBUS pin is also tied to the PPM pin.For CRSF/ELRS/etc. protocols +a full UART will need to be used with its SERIALx_PROTOCOL set to "23". + +## PWM Output + +The MUPilot supports up to 14 PWM outputs. First first 8 outputs (labelled +"M1 to M8") are controlled by a dedicated STM32F103 IO controller. These 8 +outputs support all PWM output formats, but not DShot. + +The remaining 6 outputs (labelled A1 to A6) are the "auxiliary" +outputs. These are directly attached to the STM32F765 and support all +PWM protocols as well as DShot. + +All 14 PWM outputs have GND on the top row, 5V on the middle row and +signal on the bottom row. + +The 8 main PWM outputs are in 3 groups: + + - PWM 1 and 2 in group1 + - PWM 3 and 4 in group2 + - PWM 5, 6, 7 and 8 in group3 + +The 6 auxiliary PWM outputs are in 2 groups: + + - PWM 1, 2, 3 and 4 in group1 + - PWM 5 and 6 in group2 + +Channels within the same group need to use the same output rate. If +any channel in a group uses DShot then all channels in the group need +to use DShot. + +The output levels of the auxiliary outputs can be selected by switch to be either 3.3V or 5V. The output level is 3.3V for the main outputs. + +## Battery Monitoring + +The board has two dedicated power monitor ports on 6 pin +connectors. The correct battery setting parameters are dependent on +the type of power brick which is connected. The first is analog only, the second may be either analog or I2C, depending on baseboard jumpers. +In order to enable monitoring, the BATT_MONITOR or BATT2_MONIOT parameter must be set. By default :ref:`BATT_MONITOR` is set to "4" for the included power module.. + +Default params for the first monitor are set and are: + +- BATT_VOLT_PIN = 2 +- BATT_CURR_PIN = 1 +- BATT_VOLT_MULT = 18.0 +- BATT_AMP_PERVLT = 24.0 (may need adjustment if supplied monitor is not used) + +## Compass + +The MUPilot has a builtin IST8310 compass. Due to potential +interference the board is usually used with an external I2C compass as +part of a GPS/Compass combination. + +## GPIOs + +The 6 auxiliary outputs can be used as GPIOs (relays, buttons, RPM etc). To +use them see https://ardupilot.org/rover/docs/common-gpios.html + +The numbering of the GPIOs for PIN variables in ArduPilot is: + + - MAIN1 101 + - MAIN2 102 + - MAIN3 103 + - MAIN4 104 + - MAIN5 105 + - MAIN6 106 + - MAIN7 107 + - MAIN8 108 + - AUX1 50 + - AUX2 51 + - AUX3 52 + - AUX4 53 + - AUX5 54 + - AUX6 55 + +## Analog inputs + +The MUPilot has 7 analog inputs + + - ADC Pin0 -> Battery Voltage + - ADC Pin1 -> Battery Current Sensor + - ADC Pin2 -> Battery Voltage 2 + - ADC Pin3 -> Battery Current Sensor 2 + - ADC Pin4 -> ADC port pin 2 + - ADC Pin14 -> ADC port pin 3 + - ADC Pin10 -> Board 5V Sense + - ADC Pin11 -> Board 3.3V Sense + - ADC Pin103 -> RSSI voltage monitoring + +## Loading Firmware + +The board comes pre-installed with an ArduPilot compatible bootloader, +allowing the loading of \*.apj firmware files with any ArduPilot +compatible ground station. + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/hwdef-bl.dat new file mode 100644 index 00000000000000..400e9a0929942e --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/hwdef-bl.dat @@ -0,0 +1,7 @@ +# hw definition file for processing by chibios_hwdef.py +# for MUPilot bootloader + +include ../fmuv5/hwdef-bl.dat + +undef APJ_BOARD_ID +APJ_BOARD_ID AP_HW_MUPilot diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/hwdef.dat new file mode 100644 index 00000000000000..bdcfd8454d54a6 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MUPilot/hwdef.dat @@ -0,0 +1,57 @@ +# hw definition file for processing by chibios_hwdef.py +# for MUPilot hardware. + +include ../fmuv5/hwdef.dat + +undef APJ_BOARD_ID +APJ_BOARD_ID AP_HW_MUPilot + +# extra LEDs, active low, used using the pixracer LED scheme +PH10 LED_R1 OUTPUT OPENDRAIN HIGH GPIO(0) +PH11 LED_G1 OUTPUT OPENDRAIN HIGH GPIO(1) +PH12 LED_B1 OUTPUT OPENDRAIN HIGH GPIO(2) + +undef AP_NOTIFY_GPIO_LED_RGB_RED_PIN +undef AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN + +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 0 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 1 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 2 + +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 + +define HAL_BATT_MONITOR_DEFAULT 4 + +#heaters +define HAL_HAVE_IMU_HEATER 1 +define HAL_IMU_TEMP_DEFAULT 45 +define HAL_IMUHEAT_P_DEFAULT 50 +define HAL_IMUHEAT_I_DEFAULT 0.07 + +undef PI6 +PI6 MS5611_BOARD_CS CS + +#SPI6 for extra BARO +PG13 SPI6_SCK SPI6 +PG12 SPI6_MISO SPI6 +PB5 SPI6_MOSI SPI6 + +SPIDEV ms5611_board SPI6 DEVID1 MS5611_BOARD_CS MODE3 20*MHZ 20*MHZ + +undef BARO +BARO MS56XX SPI:ms5611 +BARO MS56XX SPI:ms5611_board + + +undef IMU +undef PF11 + +PF11 ICM42688_CS CS SPEED_VERYLOW + +SPIDEV icm42688 SPI1 DEVID2 ICM42688_CS MODE3 2*MHZ 8*MHZ + +IMU Invensense SPI:icm20689 ROTATION_NONE +IMU Invensense SPI:icm20602 ROTATION_NONE +IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_270 +IMU BMI055 SPI:bmi055_a SPI:bmi055_g ROTATION_ROLL_180_YAW_90 +IMU BMI088 SPI:bmi055_a SPI:bmi055_g ROTATION_ROLL_180_YAW_90 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/README.md b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/README.md new file mode 100644 index 00000000000000..e6a704a67342cf --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/README.md @@ -0,0 +1,115 @@ +# SkySakura H743 Flight Controller + +The SkySakura H743 is a flight controller produced by [SkySakuraRC] + +## Features + + - MCU: STM32H743VIT6, 480MHz + - Gyro1: ICM42688 + - Gyro2: IIM42652 + - SD Card support + - BEC output: 5V 5A & 12V 5A (MAX 60W total) (switchable 12V) + - Barometer1: DPS310 + - Barometer2: ICP20100 + - Magnometer: IST8310 + - CAN bus support + - 7 UARTS: (USART1, USART2, USART3, UART4, USART6, UART7 with flow control, UART8) + - 2 I2C, I2C1 is used internally. + - 13 PWM outputs (12 motor outputs, 1 led) + - 4-12s wide voltage support + +## Pinout + +![SkySakura H743 Board](SkySakuraH743.png "SkySakura H743") + +## UART Mapping + +The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the +receive pin for UARTn. The Tn pin is the transmit pin for UARTn. +|Name|Pin|Function| +|:-|:-|:-| +|SERIAL0|COMPUTER|USB| +|SERIAL1|RX7/TX7|UART7 (MAVLink2, flow-control-capable)| +|SERIAL2|TX1/RX1|UART1 (MAVLink2, DMA-enabled)| +|SERIAL3|TX2/RX2|UART2 (USER)| +|SERIAL4|TX3/RX3|UART3 (GPS1, DMA-enabled)| +|SERIAL5|TX4/RX4|UART4 (RCIN, DMA-enabled)| +|SERIAL6|TX6/RX6|UART6 (DisplayPort, DMA-enabled)| +|SERIAL7|TX8/RX8|UART8 (ESC-Telemetry, RX8 on ESC connectors, TX8 can be used if protocol is change from ESC telem)| +|SERIAL8|COMPUTER|USB| + +## Safety Button + +SkySakura H743 supports safety button, with connection to sh1.0 6 pin connector, with buzzer and safety led on the same connector. +Safety button is defaulted to be disabled but can be enabled by setting the following parameter: + - BRD_SAFETY_DEFLT 1 + +## RC Input + +RC input is configured on UART4 with an sh1.0 connector. It supports all RC protocols except PPM. The SBUS pin on the HD VTX connector is tied directly the UART4 RX. If ELRS is used on UART4, then the SBUS lead from a DJI VTX must not be connected to the SBUS to prevent ELRS lock up on boot. + +## OSD Support + +SkySakura H743 supports HD OSD through UART6 by default. + +## VTX Power Control + +The 12VSW output voltage on the HD VTX connector is controlled by GPIO 85, via RELAY1 by default. Low activates the voltage. + +## PWM Output + +The SkySakura H743 has 13 PWM outputs. M1-M8 are linked to sh1.0 8 pin connectors. The first 8 outputs support bi-directional DShot and DShot. Output 9-13 only support non-DShot protocols and 13 is configured as NEOPIXEL LED by default. + +The PWM are in in two groups: + + - PWM 1-2 in group1 + - PWM 3-4 in group2 + - PWM 5-6 in group3 + - PWM 7-8 in group4 + - PWM 9-12 in group5 + - PWM 13 in group6 + +Channels within the same group need to use the same output rate. If +any channel in a group uses DShot then all channels in the group need +to use DShot. + +## Battery Monitoring + +The board has a builtin voltage sensor and external current monitor inputs. The voltage sensor can handle up to 12S LiPo batteries. The current sensor scale's default range is 120A, but will need to be adjusted according to which sensor is used. These inputs are present on the first ESC connector. +A second battery monitor can be also used. Its voltage sensor is capable of reading up to 6.6V maximum and is available on the A3 solder pad. Its current monitor input is on the A4 solder pad. + +The default battery setting parameters are: + + - BATT_MONITOR 4 + - BATT_VOLT_PIN 10 + - BATT_VOLT_MULT 34 + - BATT_CURR_PIN 11 + - set BATT2_MONITOR 4 + - BATT2_VOLT_PIN 12 + - BATT2_CURR_PIN 13 + - BATT2_VOLT_MULT 10 + - set BATT2_AMP_PERVLT to appropriate value for second current sensor + +## Compass + +The SkySakura H743 have a builtin IST8310 compass. Due to motor interference, users often disable this compass and use an external compass attached via the external SDA/SCL pins. + +## NeoPixel LED + +PWM13 provides external NeoPixel LED support. + + +## Firmware + +Firmware can bee found on the `firmware server < https://firmware.ardupilot.org`__ in the "SkySakuraH743" folders + + +## Loading Firmware + +Initial firmware load can be done with DFU by plugging in USB with the +bootloader button pressed. Then you should load the "with_bl.hex" +firmware, using your favourite DFU loading tool. + +Once the initial firmware is loaded you can update the firmware using +any ArduPilot ground station software. Updates should be done with the +\*.apj firmware files. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/SkySakuraH743-bottom.png b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/SkySakuraH743-bottom.png new file mode 100644 index 00000000000000..e516cbdf9dab8f Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/SkySakuraH743-bottom.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/SkySakuraH743_top.png b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/SkySakuraH743_top.png new file mode 100644 index 00000000000000..96bc4932d2f872 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/SkySakuraH743_top.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/Wiring diagram.pdf b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/Wiring diagram.pdf new file mode 100644 index 00000000000000..bd512382304666 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/Wiring diagram.pdf differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/defaults.parm new file mode 100644 index 00000000000000..7200b83a92dda8 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/defaults.parm @@ -0,0 +1,2 @@ +SERVO13_FUNCTION 120 +NTF_LED_TYPES 257 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/hwdef-bl.dat new file mode 100644 index 00000000000000..0c6bc0d525d7ed --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/hwdef-bl.dat @@ -0,0 +1,77 @@ +# hw definition file for processing by chibios_pins.py +# for SakuraH743 bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID 2714 + +# USB setup +USB_STRING_MANUFACTURER "SkySakura" + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +FLASH_SIZE_KB 2048 + +# bootloader starts at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +# the H743 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 128 + + +# order of UARTs (and USB). Allow bootloading on USB and telem1 +SERIAL_ORDER OTG1 UART7 + +# UART7 (telem1) +PE7 UART7_RX UART7 +PE8 UART7_TX UART7 +PE10 UART7_CTS UART7 +PE9 UART7_RTS UART7 + +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# make sure Vsw is on during bootloader +PE15 PINIO1 OUTPUT HIGH + +PB5 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 0 + +# Add CS pins to ensure they are high in bootloader +PA4 IMU1_CS CS +PE11 IMU2_CS CS +PC5 BARO2_CS CS +PB12 EXT_CS CS + + +# enable DFU by default +ENABLE_DFU_BOOT 1 + +# enable flashing from SD card: +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 +define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 1 + +# FATFS support: +define CH_CFG_USE_MEMCORE 1 +define CH_CFG_USE_HEAP 1 +define CH_CFG_USE_SEMAPHORES 0 +define CH_CFG_USE_MUTEXES 1 +define CH_CFG_USE_DYNAMIC 1 +define CH_CFG_USE_WAITEXIT 1 +define CH_CFG_USE_REGISTRY 1 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/hwdef.dat new file mode 100644 index 00000000000000..6d8b8e7cd4b0a8 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SkySakuraH743/hwdef.dat @@ -0,0 +1,229 @@ +# hw definition file for processing by chibios_pins.py +# for SakuraH743 + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID 2714 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +FLASH_SIZE_KB 2048 +env OPTIMIZE -Os + +# USB setup +USB_STRING_MANUFACTURER "SkySakura" + +#debug on USART6 -- Uncomment if debugging +#STDOUT_SERIAL SD6 +#STDOUT_BAUDRATE 115200 + + +# bootloader takes first sector +FLASH_RESERVE_START_KB 128 + + +# ChibiOS system timer +STM32_ST_USE_TIMER 12 +define CH_CFG_ST_RESOLUTION 16 + +# DRDY pins +PC4 DRDY_IIM42652_SPI1 INPUT +PB2 DRDY_ICM42688_SPI4 INPUT + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# This is the pin that senses USB being connected. It is an input pin +# setup as OPENDRAIN. +PD4 VBUS INPUT OPENDRAIN + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# allow to have have a dedicated safety switch pin +define HAL_HAVE_SAFETY_SWITCH 1 +PD10 LED_SAFETY OUTPUT LOW +PD11 SAFETY_IN INPUT PULLDOWN +#disable safety button by default +define BOARD_SAFETY_ENABLE_DEFAULT 0 + +# SPI1 for IMU1 (IIM42652) +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PA4 IMU1_CS CS + +# SPI2 - external (USER) +PB13 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 +PB12 EXT_CS CS +#PB12 FLASH_CS CS + +# SPI1 for BARO2 (DPS310) +PC5 BARO2_CS CS + +# SPI4 for IMU2 (ICM42688) +PE11 IMU2_CS CS +PE12 SPI4_SCK SPI4 +PE13 SPI4_MISO SPI4 +PE14 SPI4_MOSI SPI4 + +# two I2C bus +I2C_ORDER I2C1 I2C2 + +# I2C1 +PB6 I2C1_SCL I2C1 PULLUP +PB7 I2C1_SDA I2C1 PULLUP + +# I2C2 +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +# ADC +PC3 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC2 BATT_CURRENT_SENS ADC1 SCALE(1) +PC0 BATT2_VOLTAGE_SENS ADC2 SCALE(1) +PC1 BATT2_CURRENT_SENS ADC2 SCALE(1) + +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 13 +define HAL_BATT_CURR_PIN 12 +define HAL_BATT2_VOLT_PIN 10 +define HAL_BATT2_CURR_PIN 11 +define HAL_BATT_VOLT_SCALE 34.0 +define HAL_BATT_CURR_SCALE 40.0 +define HAL_BATT2_VOLT_SCALE 10 + +# LED setup +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 +PB5 LED_RED OUTPUT GPIO(83) +PB4 LED_GREEN OUTPUT GPIO(82) +PB3 LED_BLUE OUTPUT GPIO(81) + +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 83 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 82 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 81 + + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 UART7 USART1 USART2 USART3 UART4 USART6 UART8 OTG2 + +# USART1 (TELEM2) +PA10 USART1_RX USART1 +PA9 USART1_TX USART1 + +# USART2 (USER) +PD5 USART2_TX USART2 NODMA +PD6 USART2_RX USART2 NODMA +define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_None + +# USART3 (GPS1) +PD9 USART3_RX USART3 +PD8 USART3_TX USART3 + +# UART4 (RCIN) +PB9 UART4_TX UART4 +PB8 UART4_RX UART4 +define DEFAULT_SERIAL5_PROTOCOL SerialProtocol_RCIN + +# USART6 (DisplayPort) +PC7 USART6_RX USART6 +PC6 USART6_TX USART6 +define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_MSP_DisplayPort + +# UART7 (telem1) +PE7 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA +PE10 UART7_CTS UART7 +PE9 UART7_RTS UART7 + +# UART8 (ESC Telemetry) +PE0 UART8_RX UART8 NODMA +PE1 UART8_TX UART8 NODMA +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_ESCTelemetry + + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 +PD3 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(90) + +# Motors +PA2 TIM5_CH3 TIM5 PWM(1) GPIO(50) BIDIR +PA3 TIM5_CH4 TIM5 PWM(2) GPIO(51) +PB1 TIM3_CH4 TIM3 PWM(3) GPIO(52) BIDIR +PB0 TIM3_CH3 TIM3 PWM(4) GPIO(53) +PA1 TIM2_CH2 TIM2 PWM(5) GPIO(54) BIDIR +PA0 TIM2_CH1 TIM2 PWM(6) GPIO(55) +PE5 TIM15_CH1 TIM15 PWM(7) GPIO(56) BIDIR +PE6 TIM15_CH2 TIM15 PWM(8) GPIO(57) +PD12 TIM4_CH1 TIM4 PWM(9) GPIO(58) NODMA +PD13 TIM4_CH2 TIM4 PWM(10) GPIO(59) NODMA +PD14 TIM4_CH3 TIM4 PWM(11) GPIO(60) NODMA +PD15 TIM4_CH4 TIM4 PWM(12) GPIO(61) NODMA + +#LED +PA8 TIM1_CH1 TIM1 PWM(13) GPIO(62) # for WS2812 LED + +# Beeper +PD7 BUZZER OUTPUT GPIO(84) LOW +define HAL_BUZZER_PIN 84 + +# microSD support +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# GPIOs +PE15 VTX_PWR OUTPUT LOW GPIO(85) +define RELAY1_PIN_DEFAULT 85 + + +define HAL_STORAGE_SIZE 32768 + +# use last 2 pages for flash storage +# H743 has 16 pages of 128k each +STORAGE_FLASH_PAGE 14 + +# spi devices +SPIDEV icm42688 SPI4 DEVID1 IMU2_CS MODE3 2*MHZ 16*MHZ +SPIDEV iim42652 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 8*MHZ +SPIDEV dps310 SPI1 DEVID2 BARO2_CS MODE3 5*MHZ 5*MHZ + + +DMA_PRIORITY SPI1* SPI4* +DMA_NOSHARE SPI1* SPI4* TIM5* TIM3* TIM2* TIM15* + +# has an integreted mag, but still probe the i2c bus for all possible +# external compass types +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +COMPASS IST8310 I2C:0:0x0E false ROTATION_YAW_90 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 +define ALLOW_ARM_NO_COMPASS + +# two IMUs +# H743-V1, ICM42688, IIM42652 +IMU Invensensev3 SPI:icm42688 ROTATION_YAW_180 +IMU Invensensev3 SPI:iim42652 ROTATION_YAW_180 +define HAL_DEFAULT_INS_FAST_SAMPLE 1 + +# DPS310 integrated on SPI1 bus +BARO ICP201XX I2C:0:0x63 +BARO DPS310 SPI:dps310 + +define HAL_OS_FATFS_IO 1 +define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN +define HAL_FRAME_TYPE_DEFAULT 12 + +# enable DFU reboot for installing bootloader +# note that if firmware is build with --secure-bl then DFU is +# disabled +ENABLE_DFU_BOOT 1 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/Bottom.png b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/Bottom.png new file mode 100644 index 00000000000000..6113aa6bad9328 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/Bottom.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/README.md b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/README.md new file mode 100644 index 00000000000000..16b3a731c0f93f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/README.md @@ -0,0 +1,133 @@ +# TBS LUCID H7 Flight Controller + +The TBS LUCID H7 is a flight controller produced by [TBS](https://www.team-blacksheep.com/). + +## Features + + - MCU - STM32H743 32-bit processor running at 480 MHz + - IMU - Dual ICM42688 + - Barometer - DPS310 + - OSD - AT7456E + - microSD card slot + - 7x UARTs + - CAN support + - 13x PWM Outputs (12 Motor Output, 1 LED) + - Battery input voltage: 2S-6S + - BEC 3.3V 0.5A + - BEC 5V 3A + - BEC 9V 3A for video, gpio controlled, pinned out on HD VTX connector + - Selectable 5V or VBAT pad, for analog VTX, gpio controlled on/off + - Dual switchable camera inputs + +## Pinout + +![TBS LUCID H7 Board Top](Top.png "TBS LUCID H7 Top") +![TBS LUCID H7 Board Bottom](Bottom.png "TBS LUCID H7 Bottom") + +## UART Mapping + +The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the +receive pin for UARTn. The Tn pin is the transmit pin for UARTn. + + - SERIAL0 -> USB (MAVLink2) + - SERIAL1 -> UART1 (RX1 is SBUS in HD VTX connector) + - SERIAL2 -> UART2 (GPS, DMA-enabled) + - SERIAL3 -> UART3 (DisplayPort, DMA-enabled) + - SERIAL4 -> UART4 (MAVLink2, Telem1) + - SERIAL6 -> UART6 (RC Input, DMA-enabled) + - SERIAL7 -> UART7 (MAVLink2, Telem2, DMA and flow-control enabled) + - SERIAL8 -> UART8 (ESC Telemetry, RX8 on ESC connector for telem) + +## RC Input + +RC input is configured by default via the USART6 RX input. It supports all serial RC protocols except PPM. + +Note: If the receiver is FPort the receiver must be tied to the USART6 TX pin , RSSI_TYPE set to 3, +and SERIAL6_OPTIONS must be set to 7 (invert TX/RX, half duplex). For full duplex like CRSF/ELRS use both +RX6 and TX6 and set RSSI_TYPE also to 3. + +If SBUS is used on HD VTX connector (DJI TX), then SERIAL1_PROTOCOl should be set to "23" and SERIAL6_PROTOCOL changed to something else. + +## FrSky Telemetry + +FrSky Telemetry is supported using an unused UART, such as the T1 pin (UART1 transmit). +You need to set the following parameters to enable support for FrSky S.PORT: + + - SERIAL1_PROTOCOL 10 + - SERIAL1_OPTIONS 7 + +## OSD Support + +The TBS LUCID H7 supports OSD using OSD_TYPE 1 (MAX7456 driver) and simultaneously DisplayPort using TX3/RX3 on the HD VTX connector. + +## PWM Output + +The TBS LUCID H7 supports up to 13 PWM or DShot outputs. The pads for motor output +M1 to M4 are provided on both the motor connectors and on separate pads, plus +M9-13 on a separate pads for LED strip and other PWM outputs. + +The PWM is in 4 groups: + + - PWM 1-2 in group1 + - PWM 3-4 in group2 + - PWM 5-6 in group3 + - PWM 7-10 in group4 + - PWM 11-12 in group5 + - PWM 13 in group6 + +Channels within the same group need to use the same output rate. If +any channel in a group uses DShot then all channels in the group need +to use DShot. Channels 1-10 support bi-directional dshot. + +## Battery Monitoring + +The board has a built-in voltage sensor and external current sensor input. The current +sensor can read up to 130 Amps. The voltage sensor can handle up to 6S +LiPo batteries. + +The correct battery setting parameters are: + + - BATT_MONITOR 4 + - BATT_VOLT_PIN 10 + - BATT_CURR_PIN 11 + - BATT_VOLT_MULT 11.0 + - BATT_AMP_PERVLT 40 + +Pads for a second analog battery monitor are provided. To use: + +- Set BATT2_MONIOTOR 4 +- BATT2_VOLT_PIN 18 +- BATT2_CURR_PIN 7 +- BATT2_VOLT_MULT 11.0 +- BATT2_AMP_PERVLT as required + +## Analog RSSI and AIRSPEED inputs + +Analog RSSI uses RSSI_PIN 8 +Analog Airspeed sensor would use ARSPD_PIN 4 + +## Compass + +The TBS LUCID H7 does not have a builtin compass, but you can attach an external compass using I2C on the SDA and SCL pads. + +## VTX power control + +GPIO 81 controls the VSW pins which can be set to output either VBAT or 5V via a board jumper. Setting this GPIO high removes voltage supply to pins. RELAY2 is configured by default to control this GPIO and is low by default. + +GPIO 83 controls the VTX BEC output to pins marked "9V" and is included on the HD VTX connector. Setting this GPIO low removes voltage supply to this pin/pad. + +By default RELAY4 is configured to control this pin and sets the GPIO high. + +## Camera control + +GPIO 82 controls the camera output to the connectors marked "CAM1" and "CAM2". Setting this GPIO low switches the video output from CAM1 to CAM2. By default RELAY3 is configured to control this pin and sets the GPIO high. + +## Loading Firmware + +Initial firmware load can be done with DFU by plugging in USB with the +bootloader button pressed. Then you should load the "with_bl.hex" +firmware, using your favourite DFU loading tool. + +Once the initial firmware is loaded you can update the firmware using +any ArduPilot ground station software. Updates should be done with the +\*.apj firmware files. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/Top.png b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/Top.png new file mode 100644 index 00000000000000..f7d725fc5f6f53 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/Top.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/defaults.parm new file mode 100644 index 00000000000000..a54227e016329a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/defaults.parm @@ -0,0 +1,4 @@ +# setup for LEDs on chan13 +SERVO13_FUNCTION 120 +# Default VTX power to on +RELAY4_DEFAULT 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef-bl.dat new file mode 100644 index 00000000000000..6bb6be09ea0e7c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef-bl.dat @@ -0,0 +1,77 @@ +# hw definition file for processing by chibios_pins.py +# for TBS FCAPv1 H743 + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_TBS_LUCID_H7 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +FLASH_SIZE_KB 2048 + +# bootloader starts at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +# the H743 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 128 + +# order of UARTs (and USB). Allow bootloading on USB and telem1 +SERIAL_ORDER OTG1 UART7 + +# UART7 (telem1) +PE7 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA + +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +PE3 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 0 + +# Add CS pins to ensure they are high in bootloader +PC15 IMU1_CS CS +PB12 MAX7456_CS CS +PE11 IMU2_CS CS +PD4 EXT_CS1 CS +PE2 EXT_CS2 CS + +# Ensure GPIOs are on during bootloader +# Vsw +PD10 VSW OUTPUT LOW +# CamSw +PD11 CAMSW OUTPUT LOW +# 9V_EN +PC13 VPWR OUTPUT LOW + +# support flashing from SD card: +# enable DFU by default +ENABLE_DFU_BOOT 1 + +# FATFS support: +define CH_CFG_USE_MEMCORE 1 +define CH_CFG_USE_HEAP 1 +define CH_CFG_USE_SEMAPHORES 0 +define CH_CFG_USE_MUTEXES 1 +define CH_CFG_USE_DYNAMIC 1 +define CH_CFG_USE_WAITEXIT 1 +define CH_CFG_USE_REGISTRY 1 + +# microSD support +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +define FATFS_HAL_DEVICE SDCD1 +define HAL_OS_FATFS_IO 1 +define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef.dat new file mode 100644 index 00000000000000..614741e8a10db2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/TBS_LUCID_H7/hwdef.dat @@ -0,0 +1,219 @@ +# hw definition file for processing by chibios_pins.py +# for TBS FCAPv1 H743 + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_TBS_LUCID_H7 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +FLASH_SIZE_KB 2048 +env OPTIMIZE -Os +MCU_CLOCKRATE_MHZ 480 + +# bootloader takes first sector +FLASH_RESERVE_START_KB 128 + +# ChibiOS system timer +STM32_ST_USE_TIMER 12 +define CH_CFG_ST_RESOLUTION 16 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# SPI1 for IMU1 (ICM42688) +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PD7 SPI1_MOSI SPI1 +PC15 IMU1_CS CS + +# SPI2 for MAX7456 OSD +PB12 MAX7456_CS CS +PB13 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 + +# SPI3 - external +PB3 SPI3_SCK SPI3 +PB4 SPI3_MISO SPI3 +PB5 SPI3_MOSI SPI3 + +# external CS pins +PD4 EXT_CS1 CS +PE2 EXT_CS2 CS + +# SPI4 for IMU2 (ICM42688) +PE11 IMU2_CS CS +PE12 SPI4_SCK SPI4 +PE13 SPI4_MISO SPI4 +PE14 SPI4_MOSI SPI4 + +# two I2C bus +I2C_ORDER I2C2 I2C1 + +# I2C1 +PB6 I2C1_SCL I2C1 +PB7 I2C1_SDA I2C1 + +# I2C2 - Internal Baro +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +# ADC +PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC1 BATT_CURRENT_SENS ADC1 SCALE(1) +PA4 BATT2_VOLTAGE_SENS ADC1 SCALE(1) +PA7 BATT2_CURRENT_SENS ADC1 SCALE(1) + +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 10 +define HAL_BATT_CURR_PIN 11 +define HAL_BATT2_VOLT_PIN 18 +define HAL_BATT2_CURR_PIN 7 +define HAL_BATT_VOLT_SCALE 11.0 +define HAL_BATT_CURR_SCALE 40.0 +define HAL_BATT2_VOLT_SCALE 11.0 + +PC4 PRESSURE_SENS ADC1 SCALE(2) +define HAL_DEFAULT_AIRSPEED_PIN 4 + +PC5 RSSI_ADC ADC1 +define BOARD_RSSI_ANA_PIN 8 + +# LED +# green LED1 marked as B/E +# blue LED0 marked as ACT +define AP_NOTIFY_GPIO_LED_2_ENABLED 1 +PE3 LED0 OUTPUT LOW GPIO(90) # blue +PE4 LED1 OUTPUT LOW GPIO(91) # green +define HAL_GPIO_A_LED_PIN 91 +define HAL_GPIO_B_LED_PIN 90 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 EMPTY USART6 UART7 UART8 OTG2 + +# USART1 (DJI SBUS in connector) +PA10 USART1_RX USART1 NODMA +PA9 USART1_TX USART1 NODMA +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_None + +# USART2 (GPS1) +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 +define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_GPS + +# USART3 (DJI O3 in connector) +PD9 USART3_RX USART3 +PD8 USART3_TX USART3 +define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_MSP_DisplayPort + +# UART4 (Telem1) +PB9 UART4_TX UART4 NODMA +PB8 UART4_RX UART4 NODMA +define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_MAVLink2 + +# USART6 (RCIN) +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 +define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_RCIN + +# UART7 (Telem2) +PE7 UART7_RX UART7 +PE8 UART7_TX UART7 +PE10 UART7_CTS UART7 +PE9 UART7_RTS UART7 +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_MAVLink2 + +# UART8 (ESC Telemetry) +PE0 UART8_RX UART8 NODMA +PE1 UART8_TX UART8 NODMA +define DEFAULT_SERIAL8_PROTOCOL SerialProtocol_ESCTelemetry + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 +PD3 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) + +# Motors +PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50) BIDIR +PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) +PA0 TIM2_CH1 TIM2 PWM(3) GPIO(52) BIDIR +PA1 TIM2_CH2 TIM2 PWM(4) GPIO(53) +PA2 TIM5_CH3 TIM5 PWM(5) GPIO(54) BIDIR +PA3 TIM5_CH4 TIM5 PWM(6) GPIO(55) +PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56) BIDIR +PD13 TIM4_CH2 TIM4 PWM(8) GPIO(57) +PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58) BIDIR +PD15 TIM4_CH4 TIM4 PWM(10) GPIO(59) +# Disable DMA on PWM11-12 so that the LEDs get a channel +PE5 TIM15_CH1 TIM15 PWM(11) GPIO(60) NODMA +PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) NODMA +# Motors +PA8 TIM1_CH1 TIM1 PWM(13) GPIO(62) # for WS2812 LED + +# Beeper +PA15 BUZZER OUTPUT GPIO(32) LOW +define HAL_BUZZER_PIN 32 +define AP_NOTIFY_TONEALARM_ENABLED 1 + +# microSD support +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# GPIOs +# Vsw +PD10 VSW OUTPUT GPIO(81) LOW +# CamSw +PD11 CAMSW OUTPUT GPIO(82) LOW +# 9V_EN +PC13 VPWR OUTPUT GPIO(83) LOW + +define RELAY2_PIN_DEFAULT 81 +define RELAY3_PIN_DEFAULT 82 +define RELAY4_PIN_DEFAULT 83 + +DMA_PRIORITY SPI1* SPI4* +DMA_NOSHARE SPI1* SPI4* TIM3* TIM2* TIM5* TIM4* + +define HAL_STORAGE_SIZE 32768 + +# use last 2 pages for flash storage +# H743 has 16 pages of 128k each +STORAGE_FLASH_PAGE 14 + +# spi devices +SPIDEV imu1 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 16*MHZ # Clock is 100Mhz so highest clock <= 24Mhz is 100Mhz/8 +SPIDEV imu2 SPI4 DEVID1 IMU2_CS MODE3 2*MHZ 16*MHZ # Clock is 100Mhz so highest clock <= 24Mhz is 100Mhz/8 +SPIDEV osd SPI2 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ + +# no built-in compass, but probe the i2c bus for all possible +# external compass types +define ALLOW_ARM_NO_COMPASS +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_I2C_INTERNAL_MASK 2 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + +IMU Invensensev3 SPI:imu1 ROTATION_YAW_270 +IMU Invensensev3 SPI:imu2 ROTATION_YAW_180 +define HAL_DEFAULT_INS_FAST_SAMPLE 3 + +# DPS310 integrated on I2C2 bus +BARO DPS310 I2C:0:0x76 + +define HAL_OS_FATFS_IO 1 + +# setup for OSD +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 1 +ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VUAV-V7pro/README.md b/libraries/AP_HAL_ChibiOS/hwdef/VUAV-V7pro/README.md index 59063de3019466..4eb13fdb062110 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/VUAV-V7pro/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/VUAV-V7pro/README.md @@ -1,6 +1,6 @@ # VUAV-V7pro Flight Controller -The VUAV-V7pro flight controller is manufactured and sold by [VIEWPRO](http://www.viewprotech.com/). +The VUAV-V7pro flight controller is manufactured and sold by [V-UAV](http://www.v-uav.com/). ## Features diff --git a/libraries/AP_HAL_ChibiOS/hwdef/X-MAV-AP-H743v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/X-MAV-AP-H743v2/hwdef.dat index b5e36fcae241f3..231afa6a4fd55a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/X-MAV-AP-H743v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/X-MAV-AP-H743v2/hwdef.dat @@ -30,7 +30,7 @@ SERIAL_ORDER OTG1 USART2 UART4 USART1 USART6 UART8 USART3 UART5 UART7 # USB PA11 OTG_FS_DM OTG1 PA12 OTG_FS_DP OTG1 -PD0 VBUS INPUT OPENDRAIN +PE15 VBUS INPUT OPENDRAIN # Serial1 PD6 USART2_RX USART2 @@ -68,6 +68,10 @@ PE7 UART7_RX UART7 PA13 JTMS-SWDIO SWD PA14 JTCK-SWCLK SWD +# CAN +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + # two I2C bus I2C_ORDER I2C1 I2C4 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld index 8843b69dde6826..fe873acf1ca6cb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf.ld @@ -105,8 +105,8 @@ SECTIONS . = ALIGN(4); __ram3_init_text__ = LOADADDR(.fastramfunc); __ram3_init__ = .; - /* ChibiOS won't boot unless these are excluded */ - EXCLUDE_FILE (*vectors.o *crt0_v7m.o *crt1.o *board.o) + /* ChibiOS won't boot unless some of these are excluded */ + EXCLUDE_FILE (*vectors.o *crt0_v7m.o *crt1.o *board.o *chprintf.o) /* performance critical sections of ChibiOS */ *libch.a:ch*.*(.text*) *libch.a:nvic.*(.text*) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h730.ld b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h730.ld index d46fadcf7ca3a4..a0d7451d19c2aa 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h730.ld +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h730.ld @@ -106,8 +106,8 @@ SECTIONS __ram3_init_text__ = LOADADDR(.fastramfunc); __ram3_init__ = .; /* performance critical sections of ChibiOS */ - /* ChibiOS won't boot unless these are excluded */ - EXCLUDE_FILE (*vectors.o *crt0_v7m.o *crt1.o) *libch.a:ch*.*(.text*) + /* ChibiOS won't boot unless some of these are excluded */ + EXCLUDE_FILE (*vectors.o *crt0_v7m.o *crt1.o *chprintf.o) *libch.a:ch*.*(.text*) *libch.a:nvic.*(.text*) *libch.a:bouncebuffer.*(.text*) *libch.a:stm32_util.*(.text*) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h750.ld b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h750.ld index ab1f170f43ff60..ebe47db4a82114 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h750.ld +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/common_extf_h750.ld @@ -106,8 +106,8 @@ SECTIONS __ram3_init_text__ = LOADADDR(.fastramfunc); __ram3_init__ = .; /* performance critical sections of ChibiOS */ - /* ChibiOS won't boot unless these are excluded */ - EXCLUDE_FILE (*vectors.o *crt0_v7m.o *crt1.o) *libch.a:ch*.*(.text*) + /* ChibiOS won't boot unless some of these are excluded */ + EXCLUDE_FILE (*vectors.o *crt0_v7m.o *crt1.o *chprintf.o) *libch.a:ch*.*(.text*) *libch.a:nvic.*(.text*) *libch.a:bouncebuffer.*(.text*) *libch.a:stm32_util.*(.text*) diff --git a/libraries/AP_HAL_QURT/install.sh b/libraries/AP_HAL_QURT/install.sh new file mode 100755 index 00000000000000..b9bcc19bd0201e --- /dev/null +++ b/libraries/AP_HAL_QURT/install.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# script to install ArduPilot on a voxl2 board +# this assumes you have already installed the voxl-ardupilot.service file +# and /usr/bin/voxl-ardupilot script + +[ $# -eq 1 ] || { + echo "install.sh IPADDRESS" + exit 1 +} + +DEST="$1" + +set -e + +echo "Installing ArduPilot on $DEST" + +rsync -a build/QURT/bin/arducopter $DEST:/usr/lib/rfsa/adsp/ArduPilot.so +rsync -a build/QURT/ardupilot $DEST:/usr/bin/ + +echo "Restarting ArduPilot" +ssh $DEST systemctl restart voxl-ardupilot diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index d9a2a3ba3a84c2..ccc40042e8ea99 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -7,7 +7,7 @@ #include #include #include -#if defined (__clang__) || (defined (__APPLE__) && defined (__MACH__)) +#if defined (__clang__) || (defined (__APPLE__) && defined (__MACH__)) || defined (__OpenBSD__) #include #else #include @@ -385,7 +385,7 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_ goto failed; } -#if !defined(__APPLE__) +#if !defined(__APPLE__) && !defined(__OpenBSD__) pthread_setname_np(thread, name); #endif diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 39738637e02748..e809f4a43bb58e 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -714,6 +714,8 @@ bool UARTDriver::set_unbuffered_writes(bool on) { v &= ~O_NONBLOCK; #if defined(__APPLE__) && defined(__MACH__) fcntl(_fd, F_SETFL | F_NOCACHE, v | O_SYNC); +#elif defined(__OpenBSD__) + fcntl(_fd, F_SETFL, v | O_SYNC); #else fcntl(_fd, F_SETFL, v | O_DIRECT | O_SYNC); #endif diff --git a/libraries/AP_HAL_SITL/UART_utils.cpp b/libraries/AP_HAL_SITL/UART_utils.cpp index a3dfcccc286086..ca4628c57baf1e 100644 --- a/libraries/AP_HAL_SITL/UART_utils.cpp +++ b/libraries/AP_HAL_SITL/UART_utils.cpp @@ -19,7 +19,7 @@ #include "UARTDriver.h" -#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(__APPLE__) +#if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(__APPLE__) || defined(__OpenBSD__) #define USE_TERMIOS #endif diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index bda620b223f32b..60d4e4a1b2dd04 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -833,7 +833,7 @@ uint16_t AP_Logger::get_max_num_logs() { } /* we're started if any of the backends are started */ -bool AP_Logger::logging_started(void) { +bool AP_Logger::logging_started(void) const { for (uint8_t i=0; i< _next_backend; i++) { if (backends[i]->logging_started()) { return true; diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 3d25ca19d1085b..9fe2c07f26a14a 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -301,7 +301,7 @@ class AP_Logger // returns true if logging of a message should be attempted bool should_log(uint32_t mask) const; - bool logging_started(void); + bool logging_started(void) const; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX // currently only AP_Logger_File support this: diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 0dfd75032b9021..ccfcb7bc01d13d 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -227,14 +227,6 @@ bool Matrix3::invert() return success; } -template -void Matrix3::zero(void) -{ - a.x = a.y = a.z = 0; - b.x = b.y = b.z = 0; - c.x = c.y = c.z = 0; -} - // create rotation matrix for rotation about the vector v by angle theta // See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/ template diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 02e18699a53506..77ed088b8c9758 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -219,14 +219,14 @@ class Matrix3 { bool invert() WARN_IF_UNUSED; // zero the matrix - void zero(void); + void zero(void) { + memset((void*)this, 0, sizeof(*this)); + } // setup the identity matrix void identity(void) { + zero(); a.x = b.y = c.z = 1; - a.y = a.z = 0; - b.x = b.z = 0; - c.x = c.y = 0; } // check if any elements are NAN diff --git a/libraries/AP_NavEKF/AP_Nav_Common.h b/libraries/AP_NavEKF/AP_Nav_Common.h index 10037c4e1578db..a24f96e86e1431 100644 --- a/libraries/AP_NavEKF/AP_Nav_Common.h +++ b/libraries/AP_NavEKF/AP_Nav_Common.h @@ -19,6 +19,30 @@ #include #include +// enumeration corresponding to buts within nav_filter_status union. +// Only used for documentation purposes. +enum class NavFilterStatusBit { + ATTITUDE = 1, // attitude estimate valid + HORIZ_VEL = 2, // horizontal velocity estimate valid + VERT_VEL = 4, // vertical velocity estimate valid + HORIZ_POS_REL = 8, // relative horizontal position estimate valid + HORIZ_POS_ABS = 16, // absolute horizontal position estimate valid + VERT_POS = 32, // vertical position estimate valid + TERRAIN_ALT = 64, // terrain height estimate valid + CONST_POS_MODE = 128, // in constant position mode + PRED_HORIZ_POS_REL = 256, // expected good relative horizontal position estimate - used before takeoff + PRED_HORIZ_POS_ABS = 512, // expected good absolute horizontal position estimate - used before takeoff + TAKEOFF_DETECTED = 1024, // optical flow takeoff has been detected + TAKEOFF_EXPECTED = 2048, // compensating for baro errors during takeoff + TOUCHDOWN_EXPECTED = 4096, // compensating for baro errors during touchdown + USING_GPS = 8192, // using GPS position + GPS_GLITCHING = 16384, // GPS glitching is affecting navigation accuracy + GPS_QUALITY_GOOD = 32768, // can use GPS for navigation + INITALIZED = 65536, // has ever been healthy + REJECTING_AIRSPEED = 131072, // rejecting airspeed data + DEAD_RECKONING = 262144, // dead reckoning (e.g. no position or velocity source) +}; + union nav_filter_status { struct { bool attitude : 1; // 0 - true if attitude estimate is valid diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index 53276e13d3c535..246fc18915d17e 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -188,6 +188,7 @@ struct PACKED log_XKF3 { // @Field: FS: Filter fault status // @Field: TS: Filter timeout status bitmask (0:position measurement, 1:velocity measurement, 2:height measurement, 3:magnetometer measurement, 4:airspeed measurement, 5:drag measurement) // @Field: SS: Filter solution status +// @FieldBitmaskEnum: SS: NavFilterStatusBit // @Field: GPS: Filter GPS status // @Field: PI: Primary core index struct PACKED log_XKF4 { diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index fb7564e00085ee..77002b39cd610c 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -975,7 +975,7 @@ int8_t AP_CRSF_Telem::get_vertical_speed_packed() // prepare vario data void AP_CRSF_Telem::calc_baro_vario() { - _telem.bcast.baro_vario.altitude_packed = get_altitude_packed(); + _telem.bcast.baro_vario.altitude_packed = htobe16(get_altitude_packed()); _telem.bcast.baro_vario.vertical_speed_packed = get_vertical_speed_packed(); _telem_size = sizeof(BaroVarioFrame); @@ -987,7 +987,7 @@ void AP_CRSF_Telem::calc_baro_vario() // prepare vario data void AP_CRSF_Telem::calc_vario() { - _telem.bcast.vario.v_speed = int16_t(get_vspeed_ms() * 100.0f); + _telem.bcast.vario.v_speed = htobe16(int16_t(get_vspeed_ms() * 100.0f)); _telem_size = sizeof(VarioFrame); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VARIO; diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index 57f938a616b841..7085e05541d220 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -61,8 +61,9 @@ const AP_Param::GroupInfo AP_Scheduler::var_info[] = { // @Param: LOOP_RATE // @DisplayName: Scheduling main loop rate // @Description: This controls the rate of the main control loop in Hz. This should only be changed by developers. This only takes effect on restart. Values over 400 are considered highly experimental. - // @Values: 50:50Hz,100:100Hz,200:200Hz,250:250Hz,300:300Hz,400:400Hz + // @Range: 50 400 // @RebootRequired: True + // @Units: Hz // @User: Advanced AP_GROUPINFO("LOOP_RATE", 1, AP_Scheduler, _loop_rate_hz, SCHEDULER_DEFAULT_LOOP_RATE), diff --git a/libraries/AP_Scripting/applets/MissionSelector.lua b/libraries/AP_Scripting/applets/MissionSelector.lua index 53efa079ffff4e..b5a1c72807bd11 100644 --- a/libraries/AP_Scripting/applets/MissionSelector.lua +++ b/libraries/AP_Scripting/applets/MissionSelector.lua @@ -2,9 +2,6 @@ -- Must have Mission Reset switch assigned, it will function normally when armed or disarmed -- but also on the disarm to arm transition, it will load (if file exists) a file in the root named -- missionH.txt, missionM.txt, or missionH.txt corresponding to the the Mission Reset switch position of High/Mid/Low --- luacheck: only 0 ----@diagnostic disable: need-check-nil - local mission_loaded = false local rc_switch = rc:find_channel_for_option(24) --AUX FUNC sw for mission restart @@ -15,12 +12,12 @@ end local function read_mission(file_name) - -- Open file try and read header + -- Open file try and read header local file = io.open(file_name,"r") - local header = file:read('l') - if not header then + if not file then return update, 1000 --could not read, file probably does not exist end + local header = file:read('l') -- check header assert(string.find(header,'QGC WPL 110') == 1, file_name .. ': incorrect format') @@ -64,7 +61,6 @@ local function read_mission(file_name) end index = index + 1 end - file:close() end function update() @@ -88,4 +84,6 @@ function update() return update, 1000 end +gcs:send_text(5,"Loaded MissionSelector.lua") + return update, 5000 diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 059d6ce83a9e81..3a34534316de80 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -414,6 +414,16 @@ function CAN:get_device(buffer_len) end ---@return ScriptingCANBuffer_ud|nil function CAN:get_device2(buffer_len) end + +-- get latest FlexDebug message from a CAN node +---@param bus number -- CAN bus number, 0 for first bus, 1 for 2nd +---@param node number -- CAN node +---@param id number -- FlexDebug message ID +---@param last_us uint32_t_ud|integer|number -- timestamp of last received message, new message will be returned if timestamp is different +---@return uint32_t_ud|nil -- timestamp of message (first frame arrival time) +---@return string|nil -- up to 255 byte buffer +function DroneCAN_get_FlexDebug(bus,node,id,last_us) end + -- Auto generated binding -- desc diff --git a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua index 5032ac9b90a481..eaf28b7c6b95c6 100644 --- a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua @@ -1,5 +1,4 @@ -- mount-djirs2-driver.lua: DJIRS2 mount/gimbal driver --- luacheck: only 0 --[[ How to use @@ -12,6 +11,12 @@ Reboot the autopilot Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot set DJIR_DEBUG to 1 to display parsing and errors stats at 5sec. Set to 2 to display gimbal angles + + Advanced usage + The gimbal can be connected to an existing DroneCAN bus (on ArduPilot 4.6 or later) by setting CAN_Dx_PROTOCOL2=10 on that bus's driver + The gimbal can be used as the Nth mount (instead of the first) by setting MNTn_TYPE = 9 and modifying the script's user definitions + The gimbal can be used with the Scripting2 protocol by modifying the script's user definitions + Multiple gimbals can be used at once by duplicating the script and connecting them to different buses The following sources were used as a reference during the development of this script Constant Robotics DJIR SDK: https://github.com/ConstantRobotics/DJIR_SDK @@ -71,8 +76,9 @@ --]] ----@diagnostic disable: cast-local-type - +-- user definitions +local MOUNT_INSTANCE = 0 -- default to MNT1 +local CAN_INSTANCE = 0 -- default to first scripting CAN protocol -- global definitions local INIT_INTERVAL_MS = 3000 -- attempt to initialise the gimbal at this interval @@ -80,7 +86,6 @@ local UPDATE_INTERVAL_MS = 1 -- update interval in millis local REPLY_TIMEOUT_MS = 100 -- timeout waiting for reply after 0.1 sec local REQUEST_ATTITUDE_INTERVAL_MS = 100-- request attitude at 10hz local SET_ATTITUDE_INTERVAL_MS = 100 -- set attitude at 10hz -local MOUNT_INSTANCE = 0 -- always control the first mount/gimbal local SEND_FRAMEID = 0x223 -- send CAN messages with this frame id local RECEIVE_FRAMEID = 0x222 -- receive CAN messages with this frame id local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} @@ -109,15 +114,6 @@ local DJIR_DEBUG = Parameter("DJIR_DEBUG") -- debug level. 0:disabl --]] local DJIR_UPSIDEDOWN = Parameter("DJIR_UPSIDEDOWN") -- 0:rightsideup, 1:upsidedown --- bind parameters to variables -local CAN_P1_DRIVER = Parameter("CAN_P1_DRIVER") -- If using CAN1, should be 1:First driver -local CAN_P1_BITRATE = Parameter("CAN_P1_BITRATE") -- If using CAN1, should be 1000000 -local CAN_D1_PROTOCOL = Parameter("CAN_D1_PROTOCOL") -- If using CAN1, should be 10:Scripting -local CAN_P2_DRIVER = Parameter("CAN_P2_DRIVER") -- If using CAN2, should be 2:Second driver -local CAN_P2_BITRATE = Parameter("CAN_P2_BITRATE") -- If using CAN2, should be 1000000 -local CAN_D2_PROTOCOL = Parameter("CAN_D2_PROTOCOL") -- If using CAN2, should be 10:Scripting -local MNT1_TYPE = Parameter("MNT1_TYPE") -- should be 9:Scripting - -- message definitions local HEADER = 0xAA local RETURN_CODE = {SUCCESS=0x00, PARSE_ERROR=0x01, EXECUTION_FAILED=0x02, UNDEFINED=0xFF} @@ -143,18 +139,17 @@ local parse_length = 0 -- incoming message's packet length local parse_buff = {} -- message buffer holding roll, pitch and yaw angles from gimbal local parse_bytes_recv = 0 -- message buffer length. count of the number of bytes received in the message so far local last_send_seq = 0 -- last sequence number sent -local last_req_attitude_ms = 0 -- system time of last request for attitude -local last_set_attitude_ms = 0 -- system time of last set attitude call +local last_req_attitude_ms = uint32_t(0) -- system time of last request for attitude +local last_set_attitude_ms = uint32_t(0) -- system time of last set attitude call local REPLY_TYPE = {NONE=0, ATTITUDE=1, POSITION_CONTROL=2, SPEED_CONTROL=3} -- enum of expected reply types local expected_reply = REPLY_TYPE.NONE -- currently expected reply type -local expected_reply_ms = 0 -- system time that reply is first expected. used for timeouts +local expected_reply_ms = uint32_t(0) -- system time that reply is first expected. used for timeouts -- parsing status reporting variables -local last_print_ms = 0 -- system time that debug output was last printed +local last_print_ms = uint32_t(0) -- system time that debug output was last printed local bytes_read = 0 -- number of bytes read from gimbal local bytes_written = 0 -- number of bytes written to gimbal local bytes_error = 0 -- number of bytes read that could not be parsed -local msg_ignored = 0 -- number of ignored messages (because frame id does not match) local write_fails = 0 -- number of times write failed local execute_fails = 0 -- number of times that gimbal was unable to execute the command local reply_timeouts = 0 -- number of timeouts waiting for replies @@ -313,43 +308,21 @@ end -- perform any require initialisation function init() - -- check parameter settings - if CAN_D1_PROTOCOL:get() ~= 10 and CAN_D2_PROTOCOL:get() ~= 10 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_D1_PROTOCOL or CAN_D2_PROTOCOL=10") - do return end - end - if CAN_D1_PROTOCOL:get() == 10 and CAN_D2_PROTOCOL:get() == 10 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_D1_PROTOCOL or CAN_D2_PROTOCOL=0") - do return end - end - if CAN_D1_PROTOCOL:get() == 10 then - if CAN_P1_DRIVER:get() <= 0 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P1_DRIVER=1") - do return end - end - if CAN_P1_BITRATE:get() ~= 1000000 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P1_BITRATE=1000000") - do return end - end - end - if CAN_D2_PROTOCOL:get() == 10 then - if CAN_P2_DRIVER:get() <= 0 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P2_DRIVER=2") - do return end - end - if CAN_P2_BITRATE:get() ~= 1000000 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set CAN_P2_BITRATE=1000000") - do return end - end - end - if MNT1_TYPE:get() ~= 9 then - gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: set MNT1_TYPE=9") + local mnt_type_name = ("MNT%d_TYPE"):format(MOUNT_INSTANCE+1) + local mnt_type = param:get(mnt_type_name) + if mnt_type ~= 9 then + gcs:send_text(MAV_SEVERITY.CRITICAL, ("DJIR: set %s=9"):format(mnt_type_name)) do return end end - -- get CAN device - driver = CAN:get_device(25) - if driver then + -- get CAN device and filter to receive only replies from the gimbal + local buffer_size = 8 -- buffer up to two replies + if CAN_INSTANCE == 0 then + driver = CAN:get_device(buffer_size) + elseif CAN_INSTANCE == 1 then + driver = CAN:get_device2(buffer_size) + end + if driver and driver:add_filter(-1, RECEIVE_FRAMEID) then initialised = true gcs:send_text(MAV_SEVERITY.INFO, "DJIR: mount driver started") else @@ -519,7 +492,6 @@ function send_target_rates(roll_rate_degs, pitch_rate_degs, yaw_rate_degs) roll_rate_degs = roll_rate_degs or 0 pitch_rate_degs = pitch_rate_degs or 0 yaw_rate_degs = yaw_rate_degs or 0 - time_sec = time_sec or 2 -- ensure rates are integers. invert roll direction roll_rate_degs = -math.floor(roll_rate_degs + 0.5) @@ -564,18 +536,13 @@ end -- consume incoming CAN packets function read_incoming_packets() local canframe - repeat + while true do canframe = driver:read_frame() - if canframe then - if uint32_t(canframe:id()) == uint32_t(RECEIVE_FRAMEID) then - for i = 0, canframe:dlc()-1 do - parse_byte(canframe:data(i)) - end - else - msg_ignored = msg_ignored + 1 - end + if not canframe then return end + for i = 0, canframe:dlc()-1 do + parse_byte(canframe:data(i)) end - until not canframe + end end -- parse an byte from the gimbal @@ -746,12 +713,7 @@ function update() -- report parsing status if (DJIR_DEBUG:get() > 0) and ((now_ms - last_print_ms) > 5000) then last_print_ms = now_ms - gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: r:%u w:%u fail:%u,%u perr:%u to:%u ign:%u", bytes_read, bytes_written, write_fails, execute_fails, bytes_error, reply_timeouts, msg_ignored)) - end - - -- do not send any messages until CAN traffic has been seen - if msg_ignored == 0 and bytes_read == 0 then - return update, UPDATE_INTERVAL_MS + gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: r:%u w:%u fail:%u,%u perr:%u to:%u", bytes_read, bytes_written, write_fails, execute_fails, bytes_error, reply_timeouts)) end -- handle expected reply timeouts @@ -790,8 +752,8 @@ function update() return update, UPDATE_INTERVAL_MS end - -- send rate target - local roll_degs, pitch_degs, yaw_degs, yaw_is_ef = mount:get_rate_target(MOUNT_INSTANCE) + -- send rate target (ignoring earth-frame flag as the gimbal doesn't use it) + local roll_degs, pitch_degs, yaw_degs, _ = mount:get_rate_target(MOUNT_INSTANCE) if roll_degs and pitch_degs and yaw_degs then send_target_rates(roll_degs, pitch_degs, yaw_degs) return update, UPDATE_INTERVAL_MS diff --git a/libraries/AP_Scripting/drivers/mount-djirs2-driver.md b/libraries/AP_Scripting/drivers/mount-djirs2-driver.md index 7f4d6e058c36b9..fc45b3773bcf2d 100644 --- a/libraries/AP_Scripting/drivers/mount-djirs2-driver.md +++ b/libraries/AP_Scripting/drivers/mount-djirs2-driver.md @@ -13,6 +13,12 @@ DJI RS2 and RS3-Pro gimbal mount driver lua script - Reboot the autopilot - Copy the mount-djirs2-driver.lua script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot +## Advanced usage +- The gimbal can be connected to an existing DroneCAN bus (on ArduPilot 4.6 or later) by setting CAN_Dx_PROTOCOL2=10 on that bus's driver +- The gimbal can be used as the Nth mount (instead of the first) by setting MNTn_TYPE = 9 and modifying the script's user definitions +- The gimbal can be used with the Scripting2 protocol by modifying the script's user definitions +- Multiple gimbals can be used at once by duplicating the script and connecting them to different buses + ## Issues If the ground station reports "Pre-arm: Mount not healthy", update the diff --git a/libraries/AP_Scripting/examples/ESC_slew_rate.lua b/libraries/AP_Scripting/examples/ESC_slew_rate.lua index 5b1ba074de0d03..b85991a01721a5 100644 --- a/libraries/AP_Scripting/examples/ESC_slew_rate.lua +++ b/libraries/AP_Scripting/examples/ESC_slew_rate.lua @@ -44,7 +44,7 @@ function update() local pwm_mid = 0.5*(pwm_min+pwm_max) local pwm = math.floor(pwm_mid + (pwm_max-pwm_mid) * output) SRV_Channels:set_output_pwm_chan_timeout(chan-1, pwm, 100) - logger.write('ESLW', 'PWM,Freq', 'If', pwm, freq) + logger:write('ESLW', 'PWM,Freq', 'If', pwm, freq) gcs:send_named_float('PWN',pwm) gcs:send_named_float('FREQ',freq) end diff --git a/libraries/AP_Scripting/examples/FlexDebug.lua b/libraries/AP_Scripting/examples/FlexDebug.lua new file mode 100644 index 00000000000000..5b15906ae1d1d2 --- /dev/null +++ b/libraries/AP_Scripting/examples/FlexDebug.lua @@ -0,0 +1,50 @@ +--[[ + ArduPilot lua script to log debug messages from AM32 DroneCAN + ESCs on the flight controller + + To install set SCR_ENABLE=1 and put this script in APM/SCRIPTS/ on + the microSD of the flight controller then restart the flight + controller +--]] + +-- assume ESCs are nodes 30, 31, 32 and 33 +local ESC_BASE = 30 + +local AM32_DEBUG = 100 + +local last_tstamp = {} +local ts_zero = uint32_t(0) +local reported_version_error = false + +function log_AM32() + for i = 0, 3 do + local last_ts = last_tstamp[i] or ts_zero + tstamp_us, msg = DroneCAN_get_FlexDebug(0, ESC_BASE+i, AM32_DEBUG, last_ts) + if tstamp_us and msg then + version, commutation_interval, num_commands, num_input, rx_errors, rxframe_error, rx_ecode, auto_advance_level = string.unpack("get_FlexDebug(node_id, msg_id, tstamp_us, msg)) { + return 0; + } + + *new_uint32_t(L) = tstamp_us; + lua_pushlstring(L, (const char *)msg.u8.data, msg.u8.len); + + return 2; +} +#endif // HAL_ENABLE_DRONECAN_DRIVERS + #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index b0a7e2c36b77fd..d064df4552df60 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -34,3 +34,4 @@ int lua_mavlink_block_command(lua_State *L); int lua_print(lua_State *L); int lua_range_finder_handle_script_msg(lua_State *L); int lua_GCS_command_int(lua_State *L); +int lua_DroneCAN_get_FlexDebug(lua_State *L); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 743b8b7e61d300..9a1528b9dca4e7 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5058,7 +5058,7 @@ bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Locat out.lat = in.x; out.lng = in.y; - out.set_alt_cm(int32_t(in.z * 100), frame); + out.set_alt_m(in.z, frame); return true; } diff --git a/libraries/SITL/SIM_I2CDevice.cpp b/libraries/SITL/SIM_I2CDevice.cpp index e8d8a20aaeaa06..53bbcf317fcbef 100644 --- a/libraries/SITL/SIM_I2CDevice.cpp +++ b/libraries/SITL/SIM_I2CDevice.cpp @@ -242,7 +242,7 @@ void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, uint8_t va if (reg_data_len[reg] != 1) { AP_HAL::panic("Invalid set_register len"); } - reg_data[reg] = value; + reg_data[reg] = value << 24; } void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int8_t value) @@ -254,7 +254,7 @@ void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int8_t val if (reg_data_len[reg] != 1) { AP_HAL::panic("Invalid set_register len"); } - reg_data[reg] = value; + reg_data[reg] = value << 24; } int SITL::I2CRegisters_ConfigurableLength::rdwr(I2C::i2c_rdwr_ioctl_data *&data) diff --git a/libraries/SITL/SIM_Temperature_MCP9600.cpp b/libraries/SITL/SIM_Temperature_MCP9600.cpp index dd3b056a49b9e5..c05e7221504fa8 100644 --- a/libraries/SITL/SIM_Temperature_MCP9600.cpp +++ b/libraries/SITL/SIM_Temperature_MCP9600.cpp @@ -5,12 +5,19 @@ using namespace SITL; #include #include +enum class MCP9600StatusBits { + TH_UPDATE = (1 << 6) // data ready to read +}; + void MCP9600::init() { set_debug(true); - add_register("WHOAMI", MCP9600DevReg::WHOAMI, 2, I2CRegisters::RegMode::RDONLY); - set_register(MCP9600DevReg::WHOAMI, (uint16_t)0x40); + add_register("WHOAMI", MCP9600DevReg::WHOAMI, 1, I2CRegisters::RegMode::RDONLY); + set_register(MCP9600DevReg::WHOAMI, (uint8_t)0x40); + + add_register("SENSOR_STATUS", MCP9600DevReg::SENSOR_STATUS, 1, I2CRegisters::RegMode::RDWR); + set_register(MCP9600DevReg::SENSOR_STATUS, (uint8_t)0x00); add_register("SENSOR_CONFIG", MCP9600DevReg::SENSOR_CONFIG, 1, I2CRegisters::RegMode::RDWR); set_register(MCP9600DevReg::SENSOR_CONFIG, (uint8_t)0x00); @@ -32,14 +39,26 @@ void MCP9600::update(const class Aircraft &aircraft) // unconfigured; FIXME lack of fidelity return; } - if ((config & 0b111) != 1) { // FIXME: this is just the default config + if ((config & 0b111) != 0b10) { AP_HAL::panic("Unexpected filter configuration"); } if ((config >> 4) != 0) { // this is a K-type thermocouple, the default in the driver AP_HAL::panic("Unexpected thermocouple configuration"); } + + // dont update if we already have data ready to read (CHECKME: fidelity) + uint8_t status = 0; + get_reg_value(MCP9600DevReg::SENSOR_STATUS, config); + + if (status & (uint8_t)MCP9600StatusBits::TH_UPDATE) { + return; + } + static constexpr uint16_t factor = (1/0.0625); set_register(MCP9600DevReg::HOT_JUNC, uint16_t(htobe16(some_temperature + degrees(sinf(now_ms)) * factor))); + + // indicate we have data ready to read + set_register(MCP9600DevReg::SENSOR_STATUS, (uint8_t)MCP9600StatusBits::TH_UPDATE); } int MCP9600::rdwr(I2C::i2c_rdwr_ioctl_data *&data) diff --git a/libraries/SITL/SIM_Temperature_MCP9600.h b/libraries/SITL/SIM_Temperature_MCP9600.h index d8109cbaed558a..c0d2f3ae1ae21b 100644 --- a/libraries/SITL/SIM_Temperature_MCP9600.h +++ b/libraries/SITL/SIM_Temperature_MCP9600.h @@ -12,6 +12,7 @@ namespace SITL { class MCP9600DevReg : public I2CRegEnum { public: static constexpr uint8_t HOT_JUNC { 0x00 }; + static constexpr uint8_t SENSOR_STATUS { 0x04 }; static constexpr uint8_t SENSOR_CONFIG { 0x05 }; static constexpr uint8_t WHOAMI { 0x20 }; }; diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL index 993be80a62ec95..0856687a5acb42 160000 --- a/modules/DroneCAN/DSDL +++ b/modules/DroneCAN/DSDL @@ -1 +1 @@ -Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 +Subproject commit 0856687a5acb42b55d29bbad8f4fc9e19b4989fc