diff --git a/.github/problem-matchers/Lua.json b/.github/problem-matchers/Lua.json new file mode 100644 index 00000000000000..c62c44f18de5aa --- /dev/null +++ b/.github/problem-matchers/Lua.json @@ -0,0 +1,32 @@ +{ + "problemMatcher": [ + { + "owner": "Luacheck-problem-matcher", + "fileLocation": [ "relative", "${GITHUB_WORKSPACE}" ], + "pattern": [ + { + "regexp": "^( *)(.+.lua):(\\d+):(\\d+): (.*)$", + "file": 2, + "line": 3, + "column": 4, + "message": 5 + } + ] + }, + { + "owner": "Lua-language-server-problem-matcher", + "pattern": [ + { + "regexp": "^(.+.lua):(\\d+):(\\d+)", + "file": 1, + "line": 2, + "column": 3 + }, + { + "regexp": "^(.*)", + "message": 1 + } + ] + } + ] +} diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 1cf7932a1aa457..070817ef0702bf 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -187,7 +187,6 @@ jobs: PATH: /usr/bin:$(cygpath ${SYSTEMROOT})/system32 shell: C:\cygwin\bin\bash.exe -eo pipefail '{0}' run: >- - ln -sf /usr/bin/python3.7 /usr/bin/python && ln -sf /usr/bin/pip3.7 /usr/bin/pip && python3 -m pip install --progress-bar off empy==3.3.4 pexpect && python3 -m pip install --progress-bar off dronecan --upgrade && cp /usr/bin/ccache /usr/local/bin/ && diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index e8101d34b1a051..3697004b74f916 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -175,9 +175,7 @@ jobs: shell: bash run: | sudo apt-get update - sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-venv python3-setuptools python3-serial python3-gevent python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 - sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10 - update-alternatives --query python + sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-venv python3-setuptools python3-serial python3-gevent python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 cmake python3 --version pip3 install gevent @@ -186,24 +184,10 @@ jobs: sudo apt-get update sudo apt-get install python3.11 python3.11-venv python3.11-distutils -y sudo apt-get install python3 python3-pip python3-venv python3-setuptools python3-serial python3-cryptography python3-future python3-pyparsing python3-pyelftools - update-alternatives --query python pip3 install gevent python3 --version python3.11 --version which python3.11 - sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.11 11 - update-alternatives --query python - - rm -rf /usr/local/bin/cmake - sudo apt-get remove --purge --auto-remove cmake - sudo apt-get update && \ - sudo apt-get install -y software-properties-common lsb-release && \ - sudo apt-get clean all - wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null - sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main" - sudo apt-get update - sudo apt-get install cmake - git submodule update --init --recursive --depth=1 ./Tools/scripts/esp32_get_idf.sh diff --git a/.github/workflows/test_scripting.yml b/.github/workflows/test_scripting.yml index 53dbf016f5445b..9c425e59a2fb80 100644 --- a/.github/workflows/test_scripting.yml +++ b/.github/workflows/test_scripting.yml @@ -29,6 +29,9 @@ jobs: with: submodules: 'recursive' + - name: Register lua problem matcher + run: echo "::add-matcher::.github/problem-matchers/Lua.json" + - name: Lua Linter shell: bash run: | diff --git a/.gitignore b/.gitignore index b3d5003c68bde4..debfef06f055fc 100644 --- a/.gitignore +++ b/.gitignore @@ -171,7 +171,3 @@ ENV/ env.bak/ venv.bak/ autotest_result_*_junit.xml - -# Ignore ESP-IDF SDK defines -/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig -/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig diff --git a/AntennaTracker/ReleaseNotes.txt b/AntennaTracker/ReleaseNotes.txt index c03c34c3ab4d82..dd7edf0a299426 100644 --- a/AntennaTracker/ReleaseNotes.txt +++ b/AntennaTracker/ReleaseNotes.txt @@ -1,6 +1,53 @@ Antenna Tracker Release Notes: ------------------------------------------------------------------ -Release 4.6.0-beta 13 Nov 2024 +Release 4.6.0-beta2 11 Dec 2024 + +Changes from 4.6.0-beta1 + +1) Board specfic changes + +- FoxeerF405v2 supports BMP280 baro +- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro +- MUPilot support +- SkySakura H743 support +- TBS Lucid H7 support +- VUAV-V7pro README documentation fixed +- X-MAV AP-H743v2 CAN pin definition fixed + +2) Copter specific enhancements and bug fixes + +- AutoTune fix for calc of maximum angular acceleration +- Advanced Failsafe customer build server option + +3) Plane related enhancements and bug fixes + +- QuadPlane fix for QLand getting stuck in pilot repositioning +- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards) +- Takeoff direction fixed when no yaw source +- TECS correctly handles home altitude changes + +4) Bug Fixes and minor enhancements + +- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor +- CAN frame logging added to ease support +- CRSF reconnection after failsafe fixed +- EKF3 position and velocity resets default to user defined source +- Ethernet IP address default 192.168.144.x +- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed) +- Fence pre-arm check that vehicle is within polygon fence +- Fence handling of more than 256 items fixed +- FFT protection against divide-by-zero in Jain estimator +- Frsky telemetry apparent wind speed fixed +- Inertial sensors stop sensor converging if motors arm +- Inertial sensors check for changes to notch filters fixed +- Real Time Clock allowed to shift forward when disarmed +- ROS2/DDS get/set parameter service added +- Scripting gets memory handling improvements +- Scripting promote video-stream-information to applet +- Topotek gimbal driver uses GIA message to retrieve current angle +- Tramp VTX OSD power indicator fixed +------------------------------------------------------------------ +Release 4.6.0-beta1 13 Nov 2024 Changes from 4.5.7 diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a90c06b789acd9..44a14b195627f3 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -161,7 +161,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_GCS_ENABLE // @DisplayName: Ground Station Failsafe Enable // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. See FS_OPTIONS param for additional actions, or for cases allowing Mission continuation, when GCS failsafe is enabled. - // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START or RTL,7:Brake or Land + // @Values: 0:Disabled/NoAction,1:RTL,2:RTL or Continue with Mission in Auto Mode (Removed in 4.0+-see FS_OPTIONS),3:SmartRTL or RTL,4:SmartRTL or Land,5:Land,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL,7:Brake or Land // @User: Standard GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED), @@ -225,7 +225,7 @@ const AP_Param::Info Copter::var_info[] = { // @Param: FS_THR_ENABLE // @DisplayName: Throttle Failsafe Enable // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel - // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START or RTL,7:Enabled always Brake or Land + // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Removed in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land,6:Enabled Auto DO_LAND_START/DO_RETURN_PATH_START or RTL,7:Enabled always Brake or Land // @User: Standard GSCALAR(failsafe_throttle, "FS_THR_ENABLE", FS_THR_ENABLED_ALWAYS_RTL), @@ -1044,7 +1044,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FS_DR_ENABLE // @DisplayName: DeadReckon Failsafe Action // @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates - // @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START or RTL + // @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL // @User: Standard AP_GROUPINFO("FS_DR_ENABLE", 52, ParametersG2, failsafe_dr_enable, (uint8_t)Copter::FailsafeAction::RTL), diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 26bda5c232ac03..9a7ca0f1d49e5c 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,6 +1,53 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ -Release 4.6.0-beta 13 Nov 2024 +Release 4.6.0-beta2 11 Dec 2024 + +Changes from 4.6.0-beta1 + +1) Board specfic changes + +- FoxeerF405v2 supports BMP280 baro +- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro +- MUPilot support +- SkySakura H743 support +- TBS Lucid H7 support +- VUAV-V7pro README documentation fixed +- X-MAV AP-H743v2 CAN pin definition fixed + +2) Copter specific enhancements and bug fixes + +- AutoTune fix for calc of maximum angular acceleration +- Advanced Failsafe customer build server option + +3) Plane related enhancements and bug fixes + +- QuadPlane fix for QLand getting stuck in pilot repositioning +- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards) +- Takeoff direction fixed when no yaw source +- TECS correctly handles home altitude changes + +4) Bug Fixes and minor enhancements + +- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor +- CAN frame logging added to ease support +- CRSF reconnection after failsafe fixed +- EKF3 position and velocity resets default to user defined source +- Ethernet IP address default 192.168.144.x +- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed) +- Fence pre-arm check that vehicle is within polygon fence +- Fence handling of more than 256 items fixed +- FFT protection against divide-by-zero in Jain estimator +- Frsky telemetry apparent wind speed fixed +- Inertial sensors stop sensor converging if motors arm +- Inertial sensors check for changes to notch filters fixed +- Real Time Clock allowed to shift forward when disarmed +- ROS2/DDS get/set parameter service added +- Scripting gets memory handling improvements +- Scripting promote video-stream-information to applet +- Topotek gimbal driver uses GIA message to retrieve current angle +- Tramp VTX OSD power indicator fixed +------------------------------------------------------------------ +Release 4.6.0-beta1 13 Nov 2024 Changes from 4.5.7 diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 7d27791de5571d..1f6d0c61595027 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -789,7 +789,16 @@ float Plane::tecs_hgt_afe(void) coming. */ float hgt_afe; + if (flight_stage == AP_FixedWing::FlightStage::LAND) { + + #if AP_MAVLINK_MAV_CMD_SET_HAGL_ENABLED + // if external HAGL is active use that + if (get_external_HAGL(hgt_afe)) { + return hgt_afe; + } + #endif + hgt_afe = height_above_target(); #if AP_RANGEFINDER_ENABLED hgt_afe -= rangefinder_correction(); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 8dcd2352200863..78d30542f76afa 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -745,7 +745,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: RTL_AUTOLAND // @DisplayName: RTL auto land - // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. If this is set to 0 and there is a DO_LAND_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around without it changing RTL behaviour. For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation). + // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation).If this is set to 0 and there is a DO_LAND_START or DO_RETURN_PATH_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around (see wiki for aborting autolandings) without it changing RTL behaviour. // @Values: 0:Disable,1:Fly HOME then land via DO_LAND_START mission item, 2:Go directly to landing sequence via DO_LAND_START mission item, 3:OnlyForGoAround, 4:Go directly to landing sequence via DO_RETURN_PATH_START mission item // @User: Standard GSCALAR(rtl_autoland, "RTL_AUTOLAND", float(RtlAutoland::RTL_DISABLE)), diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index ed28f55e6d524a..b99666c5cf3035 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,6 +1,53 @@ ArduPilot Plane Release Notes: ------------------------------------------------------------------ -Release 4.6.0-beta 13 Nov 2024 +Release 4.6.0-beta2 11 Dec 2024 + +Changes from 4.6.0-beta1 + +1) Board specfic changes + +- FoxeerF405v2 supports BMP280 baro +- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro +- MUPilot support +- SkySakura H743 support +- TBS Lucid H7 support +- VUAV-V7pro README documentation fixed +- X-MAV AP-H743v2 CAN pin definition fixed + +2) Copter specific enhancements and bug fixes + +- AutoTune fix for calc of maximum angular acceleration +- Advanced Failsafe customer build server option + +3) Plane related enhancements and bug fixes + +- QuadPlane fix for QLand getting stuck in pilot repositioning +- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards) +- Takeoff direction fixed when no yaw source +- TECS correctly handles home altitude changes + +4) Bug Fixes and minor enhancements + +- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor +- CAN frame logging added to ease support +- CRSF reconnection after failsafe fixed +- EKF3 position and velocity resets default to user defined source +- Ethernet IP address default 192.168.144.x +- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed) +- Fence pre-arm check that vehicle is within polygon fence +- Fence handling of more than 256 items fixed +- FFT protection against divide-by-zero in Jain estimator +- Frsky telemetry apparent wind speed fixed +- Inertial sensors stop sensor converging if motors arm +- Inertial sensors check for changes to notch filters fixed +- Real Time Clock allowed to shift forward when disarmed +- ROS2/DDS get/set parameter service added +- Scripting gets memory handling improvements +- Scripting promote video-stream-information to applet +- Topotek gimbal driver uses GIA message to retrieve current angle +- Tramp VTX OSD power indicator fixed +------------------------------------------------------------------ +Release 4.6.0-beta1 13 Nov 2024 Changes from 4.5.7 diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index d8e3c11f7549a4..42cdcb3d456738 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -83,9 +83,12 @@ void ModeTakeoff::update() if (!takeoff_mode_setup) { plane.auto_state.takeoff_altitude_rel_cm = alt * 100; const uint16_t altitude = plane.relative_ground_altitude(false,true); - const float direction = degrees(ahrs.get_yaw()); + const Vector2f &groundspeed2d = ahrs.groundspeed_vector(); + const float direction = wrap_360(degrees(groundspeed2d.angle())); + const float groundspeed = groundspeed2d.length(); + // see if we will skip takeoff as already flying - if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { + if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && groundspeed > 3) { if (altitude >= alt) { gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering"); plane.next_WP_loc = plane.current_loc; @@ -115,7 +118,15 @@ void ModeTakeoff::update() plane.set_flight_stage(AP_FixedWing::FlightStage::TAKEOFF); - if (!plane.throttle_suppressed) { + /* + don't lock in the takeoff loiter location until we reach + a ground speed of AIRSPEED_MIN*0.3 to cope with aircraft + with no compass or poor compass. If flying in a very + strong headwind then the is_flying() check above will + eventually trigger + */ + if (!plane.throttle_suppressed && + groundspeed > plane.aparm.airspeed_min*0.3) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); plane.takeoff_state.start_time_ms = millis(); diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index a903e591c6e62c..48a28fa6b2d3e9 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -185,6 +185,73 @@ void GCS_MAVLINK_Rover::send_rangefinder() const distance, voltage); } + +void GCS_MAVLINK_Rover::send_water_depth() +{ + if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) { + return; + } + + // only send for boats: + if (!rover.is_boat()) { + return; + } + + RangeFinder *rangefinder = RangeFinder::get_singleton(); + + if (rangefinder == nullptr) { + return; + } + + // depth can only be measured by a downward-facing rangefinder: + if (!rangefinder->has_orientation(ROTATION_PITCH_270)) { + return; + } + + // get position + const AP_AHRS &ahrs = AP::ahrs(); + Location loc; + IGNORE_RETURN(ahrs.get_location(loc)); + + const auto num_sensors = rangefinder->num_sensors(); + for (uint8_t i=0; i= num_sensors) { + last_WATER_DEPTH_index = 0; + } + + const AP_RangeFinder_Backend *s = rangefinder->get_backend(last_WATER_DEPTH_index); + + if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) { + continue; + } + + // get temperature + float temp_C; + if (!s->get_temp(temp_C)) { + temp_C = 0.0f; + } + + const bool sensor_healthy = (s->status() == RangeFinder::Status::Good); + + mavlink_msg_water_depth_send( + chan, + AP_HAL::millis(), // time since system boot TODO: take time of measurement + last_WATER_DEPTH_index, // rangefinder instance + sensor_healthy, // sensor healthy + loc.lat, // latitude of vehicle + loc.lng, // longitude of vehicle + loc.alt * 0.01f, // altitude of vehicle (MSL) + ahrs.get_roll(), // roll in radians + ahrs.get_pitch(), // pitch in radians + ahrs.get_yaw(), // yaw in radians + s->distance(), // distance in meters + temp_C); // temperature in degC + + break; // only send one WATER_DEPTH message per loop + } + +} #endif // AP_RANGEFINDER_ENABLED /* @@ -400,6 +467,13 @@ bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id) } #endif +#if AP_RANGEFINDER_ENABLED + case MSG_WATER_DEPTH: + CHECK_PAYLOAD_SIZE(WATER_DEPTH); + send_water_depth(); + break; +#endif // AP_RANGEFINDER_ENABLED + default: return GCS_MAVLINK::try_send_message(id); } @@ -589,6 +663,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_WIND, #if AP_RANGEFINDER_ENABLED MSG_RANGEFINDER, + MSG_WATER_DEPTH, #endif MSG_DISTANCE_SENSOR, MSG_SYSTEM_TIME, diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index fd92cac3d5cfa3..10ca53be301375 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -29,8 +29,6 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK void send_position_target_global_int() override; - bool persist_streamrates() const override { return true; } - uint64_t capabilities() const override; void send_nav_controller_output() const override; @@ -68,6 +66,13 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK #if AP_RANGEFINDER_ENABLED void send_rangefinder() const override; + + // send WATER_DEPTH - metres and temperature + void send_water_depth(); + // state variable for the last rangefinder we sent a WATER_DEPTH + // message for. We cycle through the rangefinder backends to + // limit the amount of telemetry bandwidth we consume. + uint8_t last_WATER_DEPTH_index; #endif #if HAL_HIGH_LATENCY2_ENABLED diff --git a/Rover/Log.cpp b/Rover/Log.cpp index f13ad700add8d5..e474cda1250ed1 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -79,10 +79,6 @@ void Rover::Log_Write_Depth() (double)(s->distance()), temp_C); } -#if AP_RANGEFINDER_ENABLED - // send water depth and temp to ground station - gcs().send_message(MSG_WATER_DEPTH); -#endif } #endif diff --git a/Rover/ReleaseNotes.txt b/Rover/ReleaseNotes.txt index 9722121b97a890..def816e846fecf 100644 --- a/Rover/ReleaseNotes.txt +++ b/Rover/ReleaseNotes.txt @@ -1,6 +1,53 @@ Rover Release Notes: ------------------------------------------------------------------ -Release 4.6.0-beta 13 Nov 2024 +Release 4.6.0-beta2 11 Dec 2024 + +Changes from 4.6.0-beta1 + +1) Board specfic changes + +- FoxeerF405v2 supports BMP280 baro +- KakuteH7, H7-Mini, H7-Wing, F4 support SPA06 baro +- MUPilot support +- SkySakura H743 support +- TBS Lucid H7 support +- VUAV-V7pro README documentation fixed +- X-MAV AP-H743v2 CAN pin definition fixed + +2) Copter specific enhancements and bug fixes + +- AutoTune fix for calc of maximum angular acceleration +- Advanced Failsafe customer build server option + +3) Plane related enhancements and bug fixes + +- QuadPlane fix for QLand getting stuck in pilot repositioning +- QuikTune C++ conversion (allow running quiktun on F4 and f7 boards) +- Takeoff direction fixed when no yaw source +- TECS correctly handles home altitude changes + +4) Bug Fixes and minor enhancements + +- AIRSPEED_AUTOCAL mavlink message only sent when required and fixed for 2nd sensor +- CAN frame logging added to ease support +- CRSF reconnection after failsafe fixed +- EKF3 position and velocity resets default to user defined source +- Ethernet IP address default 192.168.144.x +- Fence autoenable fix when both RCn_OPTION=11/Fence and FENCE_AUTOENABLE = 3 (AutoEnableOnlyWhenArmed) +- Fence pre-arm check that vehicle is within polygon fence +- Fence handling of more than 256 items fixed +- FFT protection against divide-by-zero in Jain estimator +- Frsky telemetry apparent wind speed fixed +- Inertial sensors stop sensor converging if motors arm +- Inertial sensors check for changes to notch filters fixed +- Real Time Clock allowed to shift forward when disarmed +- ROS2/DDS get/set parameter service added +- Scripting gets memory handling improvements +- Scripting promote video-stream-information to applet +- Topotek gimbal driver uses GIA message to retrieve current angle +- Tramp VTX OSD power indicator fixed +------------------------------------------------------------------ +Release 4.6.0-beta1 13 Nov 2024 Changes from 4.5.7 diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 325dd047df8d45..a3a0665afdc313 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -296,6 +296,8 @@ AP_HW_CrazyF405 1177 AP_HW_FlywooF405HD_AIOv2 1180 AP_HW_FlywooH743Pro 1181 +AP_HW_JFB200 1200 + AP_HW_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 @@ -331,6 +333,7 @@ AP_HW_MountainEagleH743 1444 AP_HW_StellarF4 1500 AP_HW_GEPRCF745BTHD 1501 +AP_HW_GEPRC_TAKER_H743 1502 AP_HW_HGLRCF405V4 1524 @@ -430,7 +433,10 @@ AP_HW_CUAV-7-NANO 7000 # 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 +# IDs 7110-7119 reserved for AEROFOX +AP_HW_AEROFOX_H7 7110 + +# please fill gaps in the above ranges rather than adding past ID #7199 # OpenDroneID enabled boards. Use 10000 + the base board ID diff --git a/Tools/AP_Periph/imu.cpp b/Tools/AP_Periph/imu.cpp index c0c5aaf40128d7..06e961e44b1354 100644 --- a/Tools/AP_Periph/imu.cpp +++ b/Tools/AP_Periph/imu.cpp @@ -11,8 +11,15 @@ extern const AP_HAL::HAL &hal; void AP_Periph_FW::can_imu_update(void) { while (true) { - // sleep for a bit to avoid flooding the CPU - hal.scheduler->delay_microseconds(100); + // we need to delay by a ms value as hal->schedule->delay_microseconds_boost + // used in wait_for_sample() takes uint16_t + const uint32_t delay_ms = 1000U / g.imu_sample_rate; + hal.scheduler->delay(delay_ms); + + if (delay_ms == 0) { + // sleep for a bit to avoid flooding the CPU + hal.scheduler->delay_microseconds(100); + } imu.update(); @@ -38,6 +45,11 @@ void AP_Periph_FW::can_imu_update(void) pkt.accelerometer_latest[1] = tmp.y; pkt.accelerometer_latest[2] = tmp.z; + tmp = imu.get_gyro(); + pkt.rate_gyro_latest[0] = tmp.x; + pkt.rate_gyro_latest[1] = tmp.y; + pkt.rate_gyro_latest[2] = tmp.z; + uint8_t buffer[UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE]; uint16_t total_size = uavcan_equipment_ahrs_RawIMU_encode(&pkt, buffer, !canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE, diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index cf3e43b1b0210a..e598c9772444d4 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -212,13 +212,13 @@ static void test_div1000(void) for (uint32_t i=0; i<2000000; i++) { uint64_t v = 0; if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) { - AP_HAL::panic("ERROR: div1000 no random\n"); + AP_HAL::panic("ERROR: div1000 no random"); break; } uint64_t v1 = v / 1000ULL; uint64_t v2 = uint64_div1000(v); if (v1 != v2) { - AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n", + AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx", (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2); return; } @@ -228,7 +228,7 @@ static void test_div1000(void) for (uint32_t i=0; i<2000000; i++) { uint64_t v = 0; if (!hal.util->get_random_vals((uint8_t*)&v, sizeof(v))) { - AP_HAL::panic("ERROR: div1000 no random\n"); + AP_HAL::panic("ERROR: div1000 no random"); break; } chSysLock(); @@ -236,7 +236,7 @@ static void test_div1000(void) uint64_t v2 = uint64_div1000(v); chSysUnlock(); if (v1 != v2) { - AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n", + AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx", (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2); return; } diff --git a/Tools/Frame_params/ModalAI/SentinelM0065.parm b/Tools/Frame_params/ModalAI/SentinelM0065.parm new file mode 100644 index 00000000000000..264855d75bf58b --- /dev/null +++ b/Tools/Frame_params/ModalAI/SentinelM0065.parm @@ -0,0 +1,98 @@ +# parameters for the Sentinel M0065 PWM ESC drone + +# flight modes +FLTMODE1 0 +FLTMODE2 0 +FLTMODE3 0 +FLTMODE4 2 +FLTMODE5 0 +FLTMODE6 0 +FLTMODE_CH 6 + +# enable PID logging +LOG_BITMASK 65535 + +# mag field varies quite a lot between batteries +ARMING_MAGTHRESH 200 + +# IMU orientation +# 8 = ROLL 180 +AHRS_ORIENTATION 8 + +# Forced external compass +COMPASS_EXTERNAL 2 +# compass orientation +# ROTATION_ROLL_180_YAW_135 = 11 +COMPASS_ORIENT 11 + +# filtering +INS_GYRO_FILTER 40 +INS_HNTCH_ENABLE 1 +INS_HNTCH_ATT 40.0 +INS_HNTCH_BW 50.0 +INS_HNTCH_FM_RAT 1.0 +INS_HNTCH_FREQ 100.0 +INS_HNTCH_HMNCS 7 +INS_HNTCH_MODE 3 +INS_HNTCH_OPTS 2 +INS_HNTCH_REF 1.0 + +# run IMU at 2kHz +INS_GYRO_RATE 1 + +# a bit more agressive loiter +PILOT_SPEED_UP 500 +LOIT_BRK_ACCEL 500 +LOIT_BRK_JERK 1000 +LOIT_BRK_DELAY 0.200000 + +# Tune parameters +ATC_RAT_PIT_D 0.002 +ATC_RAT_PIT_I 0.08 +ATC_RAT_PIT_P 0.08 +ATC_RAT_RLL_D 0.002 +ATC_RAT_RLL_I 0.08 +ATC_RAT_RLL_P 0.08 + +# battery setup +BATT_LOW_VOLT 14.2 +BATT_OPTIONS 64 +BATT_VOLT_PIN 1 +BATT_CURR_PIN 2 +BATT_VOLT_MULT 1 +BATT_AMP_PERVLT 1 +BATT_AMP_OFFSET 0.0 + +# 4S battery range +MOT_BAT_VOLT_MAX 16.800000 +MOT_BAT_VOLT_MIN 13.200000 + +# Learned hover thrust +# MOT_THST_EXPO 0.550000011920928955 +# MOT_THST_HOVER 0.130549192428588867 + +# quad-X +FRAME_CLASS 1 + +# tweak R/C inputs +RC1_MIN 1000 +RC1_MAX 2000 +RC1_DZ 40 +RC2_MIN 1000 +RC2_MAX 2000 +RC2_REVERSED 1 +RC3_MIN 1000 +RC3_MAX 2000 +RC4_MIN 1000 +RC4_MAX 2000 +RC4_DZ 40 + +# add arming on right switch +RC7_OPTION 153 + +# Motor mappings +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 34 +SERVO3_FUNCTION 35 +SERVO4_FUNCTION 36 + diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 0e692be45a6fec..017c779b283e08 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -949,6 +949,7 @@ def configure_env(self, cfg, env): HAL_PERIPH_ENABLE_AIRSPEED = 1, HAL_PERIPH_ENABLE_MAG = 1, HAL_PERIPH_ENABLE_BARO = 1, + HAL_PERIPH_ENABLE_IMU = 1, HAL_PERIPH_ENABLE_RANGEFINDER = 1, HAL_PERIPH_ENABLE_BATTERY = 1, HAL_PERIPH_ENABLE_EFI = 1, @@ -963,6 +964,7 @@ def configure_env(self, cfg, env): HAL_WITH_ESC_TELEM = 1, AP_EXTENDED_ESC_TELEM_ENABLED = 1, AP_TERRAIN_AVAILABLE = 1, + HAL_GYROFFT_ENABLED = 0, ) class sitl_periph_gps(sitl_periph): diff --git a/Tools/ardupilotwaf/esp32.py b/Tools/ardupilotwaf/esp32.py index 5e30e39ce3a8ef..11df54d6ceae9a 100644 --- a/Tools/ardupilotwaf/esp32.py +++ b/Tools/ardupilotwaf/esp32.py @@ -57,6 +57,24 @@ def bldpath(path): #env.append_value('GIT_SUBMODULES', 'esp_idf') +# delete the output sdkconfig file when the input defaults changes. we take the +# stamp as the output so we can compute the path to the sdkconfig, yet it +# doesn't have to exist when we're done. +class clean_sdkconfig(Task.Task): + def keyword(self): + return "delete sdkconfig generated from" + + def run(self): + prefix = ".clean-stamp-" + for out in self.outputs: + if not out.name.startswith(prefix): + raise ValueError("not a stamp file: "+out) + dest = out.parent.abspath()+"/"+out.name[len(prefix):] + if os.path.exists(dest): + os.unlink(dest) + + # waf needs the output to exist after the task, so touch it + open(out.abspath(), "w").close() def pre_build(self): """Configure esp-idf as lib target""" @@ -74,8 +92,21 @@ def pre_build(self): ) esp_idf_showinc = esp_idf.build('showinc', target='esp-idf_build/includes.list') + + # task to delete the sdkconfig (thereby causing it to be regenerated) when + # the .defaults changes. it uses a stamp to find the sdkconfig. changing + # the sdkconfig WILL NOT cause it to be deleted as it's not an input. this + # is by design so the user can tweak it for testing purposes. + clean_sdkconfig_task = esp_idf_showinc.create_task("clean_sdkconfig", + src=self.srcnode.find_or_declare(self.env.AP_HAL_ESP32+"/sdkconfig.defaults"), + tgt=self.bldnode.find_or_declare("esp-idf_build/.clean-stamp-sdkconfig")) + esp_idf_showinc.post() + # ensure the sdkconfig will be deleted before the cmake configure occurs + # that regenerates it + esp_idf_showinc.cmake_config_task.set_run_after(clean_sdkconfig_task) + from waflib import Task class load_generated_includes(Task.Task): """After includes.list generated include it in env""" diff --git a/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic-quadplane.txt b/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic-quadplane.txt index d4ec44c2e00666..af7a9afbf39ed7 100644 --- a/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic-quadplane.txt +++ b/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic-quadplane.txt @@ -1,13 +1,14 @@ QGC WPL 110 0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 -1 0 3 84 0.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 -5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 -6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 -8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 -9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 -10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 -11 0 3 85 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 +1 0 0 223 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +2 0 3 84 0.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +7 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +8 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +11 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +12 0 3 85 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic.txt b/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic.txt index b6a88b10c2f84a..51f09350660893 100644 --- a/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic.txt +++ b/Tools/autotest/ArduPlane_Tests/FlyEachFrame/basic.txt @@ -1,13 +1,14 @@ QGC WPL 110 0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1 -1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 -3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 -5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 -6 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 -8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 -9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 -10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 -11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 +1 0 0 223 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +2 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 30.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1 +5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1 +6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1 +7 0 0 177 2.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +8 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1 +9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1 +10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1 +11 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1 +12 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1 diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 8b46a62473ec38..2f2a9ca811c55b 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -1374,7 +1374,10 @@ def StabilityPatch(self, # cut motor 1's to efficiency self.progress("Cutting motor 1 to 65% efficiency") - self.set_parameter("SIM_ENGINE_MUL", 0.65) + self.set_parameters({ + "SIM_ENGINE_MUL": 0.65, + "SIM_ENGINE_FAIL": 1 << 0, # motor 1 + }) while self.get_sim_time_cached() < tstart + holdtime: m = self.mav.recv_match(type='VFR_HUD', blocking=True) @@ -1719,8 +1722,6 @@ def MinAltFence(self): # Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance def MinAltFenceAvoid(self): '''Test Min Alt Fence Avoidance''' - self.takeoff(30, mode="LOITER") - """Hold loiter position.""" # enable fence, only min altitude # No action, rely on avoidance to prevent the breach @@ -1730,6 +1731,10 @@ def MinAltFenceAvoid(self): "FENCE_ALT_MIN": 20, "FENCE_ACTION": 0, }) + self.reboot_sitl() + + self.takeoff(30, mode="LOITER") + """Hold loiter position.""" # Try and fly past the fence self.set_rc(3, 1120) @@ -3283,8 +3288,8 @@ def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fa self.progress("Killing motor %u (%u%%)" % (fail_servo+1, fail_mul)) self.set_parameters({ - "SIM_ENGINE_FAIL": fail_servo, "SIM_ENGINE_MUL": fail_mul, + "SIM_ENGINE_FAIL": 1 << fail_servo, }) failed = True @@ -3343,10 +3348,7 @@ def MotorFail_test_frame(self, model, servo_count, frame_class, fail_servo=0, fa raise NotAchievedException("Vehicle is descending") self.progress("Fixing motors") - self.set_parameters({ - "SIM_ENGINE_FAIL": 0, - "SIM_ENGINE_MUL": 1.0, - }) + self.set_parameter("SIM_ENGINE_FAIL", 0) self.do_RTL() @@ -4168,8 +4170,7 @@ def _Parachute(self, command): self.takeoff(40) self.set_rc(9, 1500) self.set_parameters({ - "SIM_ENGINE_MUL": 0, - "SIM_ENGINE_FAIL": 1, + "SIM_ENGINE_FAIL": 1 << 1, # motor 2 }) self.wait_statustext('BANG! Parachute deployed', timeout=60) self.set_rc(9, 1000) @@ -4182,8 +4183,7 @@ def _Parachute(self, command): self.takeoff(loiter_alt, mode='LOITER') self.set_rc(9, 1100) self.set_parameters({ - "SIM_ENGINE_MUL": 0, - "SIM_ENGINE_FAIL": 1, + "SIM_ENGINE_FAIL": 1 << 1, # motor 2 }) tstart = self.get_sim_time() while self.get_sim_time_cached() < tstart + 5: @@ -8304,6 +8304,57 @@ def WatchAlts(self): self.do_RTL() + def TestTetherStuck(self): + """Test tethered vehicle stuck because of tether""" + # Enable tether simulation + self.set_parameters({ + "SIM_TETH_ENABLE": 1, + }) + self.delay_sim_time(2) + self.reboot_sitl() + + # Set tether line length + self.set_parameters({ + "SIM_TETH_LINELEN": 10, + }) + self.delay_sim_time(2) + + # Prepare and take off + self.wait_ready_to_arm() + self.arm_vehicle() + self.takeoff(10, mode='LOITER') + + # Simulate vehicle getting stuck by increasing RC throttle + self.set_rc(3, 1900) + self.delay_sim_time(5, reason='let tether get stuck') + + # Monitor behavior for 10 seconds + tstart = self.get_sim_time() + initial_alt = self.get_altitude() + stuck = True # Assume it's stuck unless proven otherwise + + while self.get_sim_time() - tstart < 10: + # Fetch current altitude + current_alt = self.get_altitude() + self.progress(f"current_alt={current_alt}") + + # Fetch and log battery status + battery_status = self.mav.recv_match(type='BATTERY_STATUS', blocking=True, timeout=1) + if battery_status: + self.progress(f"Battery: {battery_status}") + + # Check if the vehicle is stuck. + # We assume the vehicle is stuck if the current is high and the altitude is not changing + if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 2): + stuck = False # Vehicle moved or current is abnormal + break + + if not stuck: + raise NotAchievedException("Vehicle did not get stuck as expected") + + # Land and disarm the vehicle to ensure we can go down + self.land_and_disarm() + def fly_rangefinder_drivers_fly(self, rangefinders): '''ensure rangefinder gives height-above-ground''' self.change_mode('GUIDED') @@ -9411,9 +9462,6 @@ def FlyEachFrame(self): self.progress("Actually, no I'm not - it is an external simulation") continue model = frame_bits.get("model", frame) - # the model string for Callisto has crap in it.... we - # should really have another entry in the vehicleinfo data - # to carry the path to the JSON. defaults = self.model_defaults_filepath(frame) if not isinstance(defaults, list): defaults = [defaults] @@ -9928,6 +9976,15 @@ def SMART_RTL_EnterLeave(self): self.change_mode('ALT_HOLD') self.change_mode('SMART_RTL') + def SMART_RTL_Repeat(self): + '''Test whether Smart RTL catches the repeat''' + self.takeoff(alt_min=10, mode='GUIDED') + self.set_rc(3, 1500) + self.change_mode("CIRCLE") + self.delay_sim_time(1300) + self.change_mode("SMART_RTL") + self.wait_disarmed() + def GPSForYawCompassLearn(self): '''Moving baseline GPS yaw - with compass learning''' self.context_push() @@ -11971,8 +12028,8 @@ def GripperReleaseOnThrustLoss(self): self.context_push() self.context_collect('STATUSTEXT') self.set_parameters({ - "SIM_ENGINE_FAIL": 1, "SIM_ENGINE_MUL": 0.5, + "SIM_ENGINE_FAIL": 1 << 1, # motor 2 "FLIGHT_OPTIONS": 4, }) @@ -12251,6 +12308,7 @@ def tests2b(self): # this block currently around 9.5mins here self.AP_Avoidance, self.SMART_RTL, self.SMART_RTL_EnterLeave, + self.SMART_RTL_Repeat, self.RTL_TO_RALLY, self.FlyEachFrame, self.GPSBlending, @@ -12330,6 +12388,7 @@ def tests2b(self): # this block currently around 9.5mins here self.MAV_CMD_MISSION_START_p1_p2, self.ScriptingAHRSSource, self.CommonOrigin, + self.TestTetherStuck, ]) return ret @@ -12412,6 +12471,7 @@ def disabled_tests(self): "GPSForYawCompassLearn": "Vehicle currently crashed in spectacular fashion", "CompassMot": "Cuases an arithmetic exception in the EKF", "SMART_RTL_EnterLeave": "Causes a panic", + "SMART_RTL_Repeat": "Currently fails due to issue with loop detection", } diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 7fabd6cd078165..2aa1390402db4b 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -3661,7 +3661,7 @@ def FenceMinAltAutoEnableAbort(self): def FenceAutoEnableDisableSwitch(self): '''Tests autoenablement of regular fences and manual disablement''' self.set_parameters({ - "FENCE_TYPE": 11, # Set fence type to min alt + "FENCE_TYPE": 9, # Set fence type to min alt, max alt "FENCE_ACTION": 1, # Set action to RTL "FENCE_ALT_MIN": 50, "FENCE_ALT_MAX": 100, @@ -3672,44 +3672,88 @@ def FenceAutoEnableDisableSwitch(self): "FENCE_RET_ALT" : 0, "FENCE_RET_RALLY" : 0, "FENCE_TOTAL" : 0, + "RTL_ALTITUDE" : 75, "TKOFF_ALT" : 75, "RC7_OPTION" : 11, # AC_Fence uses Aux switch functionality }) + self.reboot_sitl() + self.context_collect("STATUSTEXT") + fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE # Grab Home Position self.mav.recv_match(type='HOME_POSITION', blocking=True) - self.set_rc_from_map({7: 1000}) # Turn fence off with aux function + self.set_rc(7, 1000) # Turn fence off with aux function, does not impact later auto-enable self.wait_ready_to_arm() + + self.progress("Check fence disabled at boot") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + if (m.onboard_control_sensors_enabled & fence_bit): + raise NotAchievedException("Fence is enabled at boot") + cruise_alt = 75 self.takeoff(cruise_alt, mode='TAKEOFF') - self.progress("Fly above ceiling and check there is no breach") + self.progress("Fly above ceiling and check there is a breach") + self.change_mode('FBWA') self.set_rc(3, 2000) - self.change_altitude(cruise_alt + 80, relative=True) + self.set_rc(2, 1000) + + self.wait_statustext("Max Alt fence breached", timeout=10, check_context=True) + self.wait_mode('RTL') + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) - if (not (m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Ceiling breached") + if (m.onboard_control_sensors_health & fence_bit): + raise NotAchievedException("Fence ceiling not breached") + + self.set_rc(3, 1500) + self.set_rc(2, 1500) + + self.progress("Wait for RTL alt reached") + self.wait_altitude(cruise_alt-5, cruise_alt+5, relative=True, timeout=30) self.progress("Return to cruise alt") self.set_rc(3, 1500) self.change_altitude(cruise_alt, relative=True) - self.progress("Fly below floor and check for no breach") - self.change_altitude(25, relative=True) + self.progress("Check fence breach cleared") m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) if (not (m.onboard_control_sensors_health & fence_bit)): - raise NotAchievedException("Fence Ceiling breached") + raise NotAchievedException("Fence breach not cleared") + + self.progress("Fly below floor and check for breach") + self.set_rc(2, 2000) + self.wait_statustext("Min Alt fence breached", timeout=10, check_context=True) + self.wait_mode("RTL") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + if (m.onboard_control_sensors_health & fence_bit): + raise NotAchievedException("Fence floor not breached") - self.progress("Fly above floor and check fence is not re-enabled") + self.change_mode("FBWA") + + self.progress("Fly above floor and check fence is enabled") self.set_rc(3, 2000) self.change_altitude(75, relative=True) m = self.mav.recv_match(type='SYS_STATUS', blocking=True) - self.progress("Got (%s)" % str(m)) + if (not (m.onboard_control_sensors_enabled & fence_bit)): + raise NotAchievedException("Fence Floor not enabled") + + self.progress("Toggle fence enable/disable") + self.set_rc(7, 2000) + self.delay_sim_time(2) + self.set_rc(7, 1000) + self.delay_sim_time(2) + + self.progress("Check fence is disabled") + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) if (m.onboard_control_sensors_enabled & fence_bit): - raise NotAchievedException("Fence Ceiling re-enabled") + raise NotAchievedException("Fence disable with switch failed") + + self.progress("Fly below floor and check for no breach") + self.change_altitude(40, relative=True) + m = self.mav.recv_match(type='SYS_STATUS', blocking=True) + if (not (m.onboard_control_sensors_health & fence_bit)): + raise NotAchievedException("Fence floor breached") self.progress("Return to cruise alt") self.set_rc(3, 1500) @@ -4156,9 +4200,7 @@ def FlyEachFrame(self): vinfo = vehicleinfo.VehicleInfo() vinfo_options = vinfo.options[self.vehicleinfo_key()] known_broken_frames = { - "plane-tailsitter": "does not take off; immediately emits 'AP: Transition VTOL done' while on ground", - "plane-ice" : "needs ICE control channel for ignition", - "quadplane-ice" : "needs ICE control channel for ignition", + "plane-tailsitter": "unstable in hover; unflyable in cruise", "quadplane-can" : "needs CAN periph", "stratoblimp" : "not expected to fly normally", "glider" : "needs balloon lift", @@ -4175,11 +4217,7 @@ def FlyEachFrame(self): self.progress("Actually, no I'm not - it is an external simulation") continue model = frame_bits.get("model", frame) - # the model string for Callisto has crap in it.... we - # should really have another entry in the vehicleinfo data - # to carry the path to the JSON. - actual_model = model.split(":")[0] - defaults = self.model_defaults_filepath(actual_model) + defaults = self.model_defaults_filepath(frame) if not isinstance(defaults, list): defaults = [defaults] self.customise_SITL_commandline( @@ -4805,6 +4843,41 @@ def TakeoffTakeoff4(self): self.fly_home_land_and_disarm() + def TakeoffTakeoff5(self): + '''Test the behaviour of a takeoff with no compass''' + self.set_parameters({ + "COMPASS_USE": 0, + "COMPASS_USE2": 0, + "COMPASS_USE3": 0, + }) + import copy + start_loc = copy.copy(SITL_START_LOCATION) + start_loc.heading = 175 + self.customise_SITL_commandline(["--home=%.9f,%.9f,%.2f,%.1f" % ( + start_loc.lat, start_loc.lng, start_loc.alt, start_loc.heading)]) + self.reboot_sitl() + self.change_mode("TAKEOFF") + + # waiting for the EKF to be happy won't work + self.delay_sim_time(20) + self.arm_vehicle() + + target_alt = self.get_parameter("TKOFF_ALT") + self.wait_altitude(target_alt-5, target_alt, relative=True) + + # Wait a bit for the Takeoff altitude to settle. + self.delay_sim_time(5) + + bearing_margin = 35 + loc = self.mav.location() + bearing_from_home = self.get_bearing(start_loc, loc) + if bearing_from_home < 0: + bearing_from_home += 360 + if abs(bearing_from_home - start_loc.heading) > bearing_margin: + raise NotAchievedException(f"Did not takeoff in the right direction {bearing_from_home}") + + self.fly_home_land_and_disarm() + def TakeoffGround(self): '''Test a rolling TAKEOFF.''' @@ -5326,6 +5399,40 @@ def AerobaticsScripting(self): else: raise NotAchievedException("Missing trick %s" % t) + def UniversalAutoLandScript(self): + '''Test UniversalAutoLandScript''' + applet_script = "UniversalAutoLand.lua" + self.customise_SITL_commandline(["--home", "-35.362938,149.165085,585,173"]) + + self.install_applet_script_context(applet_script) + self.context_collect('STATUSTEXT') + self.set_parameters({ + "SCR_ENABLE" : 1, + "SCR_VM_I_COUNT" : 1000000, + "RTL_AUTOLAND" : 2 + }) + self.reboot_sitl() + self.wait_text("Loaded UniversalAutoLand.lua", check_context=True) + self.set_parameters({ + "AUTOLAND_ENABLE" : 1, + "AUTOLAND_WP_ALT" : 55, + "AUTOLAND_WP_DIST" : 400 + }) + self.scripting_restart() + self.wait_text("Scripting: restarted", check_context=True) + + self.wait_ready_to_arm() + self.arm_vehicle() + self.change_mode("AUTO") + self.wait_text("Captured initial takeoff direction", check_context=True) + + self.wait_disarmed(120) + self.progress("Check the landed heading matches takeoff") + self.wait_heading(173, accuracy=5, timeout=1) + loc = mavutil.location(-35.362938, 149.165085, 585, 173) + if self.get_distance(loc, self.mav.location()) > 35: + raise NotAchievedException("Did not land close to home") + def SDCardWPTest(self): '''test BRD_SD_MISSION support''' spiral_script = "mission_spiral.lua" @@ -6225,14 +6332,18 @@ def GPSPreArms(self): def SetHomeAltChange(self): '''check modes retain altitude when home alt changed''' for mode in 'FBWB', 'CRUISE', 'LOITER': + self.set_rc(3, 1000) self.wait_ready_to_arm() home = self.home_position_as_mav_location() - self.takeoff(20) - higher_home = home + target_alt = 20 + self.takeoff(target_alt, mode="TAKEOFF") + self.delay_sim_time(20) # Give some time to altitude to stabilize. + self.set_rc(3, 1500) + self.change_mode(mode) + higher_home = copy.copy(home) higher_home.alt += 40 self.set_home(higher_home) - self.change_mode(mode) - self.wait_altitude(15, 25, relative=True, minimum_duration=10) + self.wait_altitude(home.alt+target_alt-5, home.alt+target_alt+5, relative=False, minimum_duration=10, timeout=11) self.disarm_vehicle(force=True) self.reboot_sitl() @@ -6268,6 +6379,23 @@ def SetHomeAltChange2(self): self.disarm_vehicle(force=True) self.reboot_sitl() + def SetHomeAltChange3(self): + '''same as SetHomeAltChange, but the home alt change occurs during TECS operation''' + self.wait_ready_to_arm() + home = self.home_position_as_mav_location() + target_alt = 20 + self.takeoff(target_alt, mode="TAKEOFF") + self.change_mode("LOITER") + self.delay_sim_time(20) # Let the plane settle. + + higher_home = copy.copy(home) + higher_home.alt += 40 + self.set_home(higher_home) + self.wait_altitude(home.alt+target_alt-5, home.alt+target_alt+5, relative=False, minimum_duration=10, timeout=10.1) + + self.disarm_vehicle(force=True) + self.reboot_sitl() + def ForceArm(self): '''check force-arming functionality''' self.set_parameter("SIM_GPS1_ENABLE", 0) @@ -6457,6 +6585,7 @@ def tests1a(self): self.Soaring, self.Terrain, self.TerrainMission, + self.UniversalAutoLandScript, ]) return ret @@ -6490,6 +6619,7 @@ def tests1b(self): self.TakeoffTakeoff2, self.TakeoffTakeoff3, self.TakeoffTakeoff4, + self.TakeoffTakeoff5, self.TakeoffGround, self.TakeoffIdleThrottle, self.TakeoffBadLevelOff, @@ -6538,6 +6668,7 @@ def tests1b(self): self.GPSPreArms, self.SetHomeAltChange, self.SetHomeAltChange2, + self.SetHomeAltChange3, self.ForceArm, self.MAV_CMD_EXTERNAL_WIND_ESTIMATE, self.GliderPullup, @@ -6549,7 +6680,6 @@ def disabled_tests(self): "LandingDrift": "Flapping test. See https://github.com/ArduPilot/ardupilot/issues/20054", "InteractTest": "requires user interaction", "ClimbThrottleSaturation": "requires https://github.com/ArduPilot/ardupilot/pull/27106 to pass", - "SetHomeAltChange": "https://github.com/ArduPilot/ardupilot/issues/5672", } diff --git a/Tools/autotest/default_params/plane-ice.parm b/Tools/autotest/default_params/plane-ice.parm index e6cb59cad0102c..48c2fb0b96b12f 100644 --- a/Tools/autotest/default_params/plane-ice.parm +++ b/Tools/autotest/default_params/plane-ice.parm @@ -1,5 +1,6 @@ ICE_ENABLE 1 ICE_RPM_CHAN 1 +ICE_RPM_THRESH 50 SERVO13_FUNCTION 67 SERVO14_FUNCTION 69 RC11_OPTION 179 diff --git a/Tools/autotest/default_params/quadplane.parm b/Tools/autotest/default_params/quadplane.parm index 9da3368889f2f4..2d60da1b0b8132 100644 --- a/Tools/autotest/default_params/quadplane.parm +++ b/Tools/autotest/default_params/quadplane.parm @@ -77,7 +77,7 @@ Q_A_RAT_PIT_P 0.103 Q_A_RAT_PIT_I 0.103 Q_A_RAT_PIT_D 0.001 Q_A_RAT_YAW_P 0.2 -Q_A_RAT_YAW_P 0.02 +Q_A_RAT_YAW_I 0.02 Q_A_ANG_RLL_P 6 Q_A_ANG_PIT_P 6 RTL_ALTITUDE 20.00 diff --git a/Tools/autotest/default_params/vee-gull_005.param b/Tools/autotest/default_params/vee-gull_005.param index ab340c7eaf020c..9415f055bdedc8 100644 --- a/Tools/autotest/default_params/vee-gull_005.param +++ b/Tools/autotest/default_params/vee-gull_005.param @@ -620,7 +620,7 @@ SIM_BATT_VOLTAGE,12.6 SIM_DRIFT_SPEED,0.05 SIM_DRIFT_TIME,5 SIM_ENGINE_FAIL,0 -SIM_ENGINE_MUL,1 +SIM_ENGINE_MUL,0 SIM_FLOAT_EXCEPT,1 SIM_FLOW_DELAY,0 SIM_FLOW_ENABLE,0 diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 721752a5c337d6..95e72e276f3e19 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -767,8 +767,7 @@ def FwdThrInVTOL(self): "Q_WVANE_ENABLE": 1, "Q_WVANE_GAIN": 1, "STICK_MIXING": 0, - "Q_FWD_THR_USE": 2, - "SIM_ENGINE_FAIL": 2}) # we want to fail the forward thrust motor only + "Q_FWD_THR_USE": 2}) self.takeoff(10, mode="QLOITER") self.set_rc(2, 1000) @@ -785,7 +784,7 @@ def FwdThrInVTOL(self): self.set_rc(2, 1500) self.delay_sim_time(5) loc1 = self.mav.location() - self.set_parameter("SIM_ENGINE_MUL", 0) # simulate a complete loss of forward motor thrust + self.set_parameter("SIM_ENGINE_FAIL", 1 << 2) # simulate a complete loss of forward motor thrust self.delay_sim_time(20) self.change_mode('QLAND') self.wait_disarmed(timeout=60) @@ -1475,7 +1474,10 @@ def VTOLQuicktune_CPP(self): (p, new_values[p], threshold)) self.progress("ensure we are not overtuned") - self.set_parameter('SIM_ENGINE_MUL', 0.9) + self.set_parameters({ + 'SIM_ENGINE_MUL': 0.9, + 'SIM_ENGINE_FAIL': 1 << 0, + }) self.delay_sim_time(5) diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index af876bdece7c80..ec6d8a5f4c7abe 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -6121,7 +6121,9 @@ def check_rangefinder(mav, m): if not self.current_onboard_log_contains_message("DPTH"): raise NotAchievedException("Expected DPTH log message") - # self.context_pop() + self.progress("Ensuring we get WATER_DEPTH at 12Hz with 2 backends") + self.set_message_rate_hz('WATER_DEPTH', 12) + self.assert_message_rate_hz('WATER_DEPTH', 12) def EStopAtBoot(self): '''Ensure EStop prevents arming when asserted at boot time''' diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 7cbc005c310aa6..4f09b396022f42 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -268,6 +268,8 @@ def define_is_whitelisted_for_feature_in_code(self, target, define): 'AP_OPTICALFLOW_ONBOARD_ENABLED', # only instantiated on Linux 'HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL', # entirely elided if no user 'AP_PLANE_BLACKBOX_LOGGING', # entirely elided if no user + 'AP_COMPASS_AK8963_ENABLED', # probed on a board-by-board basis, not on CubeOrange for example + 'AP_COMPASS_LSM303D_ENABLED', # probed on a board-by-board basis, not on CubeOrange for example ]) if target.lower() != "copter": feature_define_whitelist.add('MODE_ZIGZAG_ENABLED') diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index a5535d73da125c..6ff5fc7c342d5f 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -1715,8 +1715,8 @@ def update(self): if not self.connect(): self.progress("Failed to connect") return - self.check_poll() self.do_sport_read() + self.check_poll() def do_sport_read(self): self.buffer += self.do_read() @@ -13738,8 +13738,11 @@ def wait_rpm1(self, min_rpm=None, timeout=10): def FRSkySPort(self): '''Test FrSky SPort mode''' - self.set_parameter("SERIAL5_PROTOCOL", 4) # serial5 is FRSky sport - self.set_parameter("RPM1_TYPE", 10) # enable SITL RPM sensor + self.set_parameters({ + "SERIAL5_PROTOCOL": 4, # serial5 is FRSky sport + "RPM1_TYPE": 10, # enable SITL RPM sensor + "GPS1_TYPE": 100, # use simulated backend for consistent position + }) port = self.spare_network_port() self.customise_SITL_commandline([ "--serial5=tcp:%u" % port # serial5 spews to localhost port diff --git a/Tools/bootloaders/AEROFOX-H7_bl.bin b/Tools/bootloaders/AEROFOX-H7_bl.bin new file mode 100644 index 00000000000000..e35dcf11a726c6 Binary files /dev/null and b/Tools/bootloaders/AEROFOX-H7_bl.bin differ diff --git a/Tools/bootloaders/GEPRC_TAKER_H743_bl.bin b/Tools/bootloaders/GEPRC_TAKER_H743_bl.bin new file mode 100644 index 00000000000000..6a5152f03c9450 Binary files /dev/null and b/Tools/bootloaders/GEPRC_TAKER_H743_bl.bin differ diff --git a/Tools/bootloaders/GEPRC_TAKER_H743_bl.hex b/Tools/bootloaders/GEPRC_TAKER_H743_bl.hex new file mode 100644 index 00000000000000..2d751afcb6266f --- /dev/null +++ b/Tools/bootloaders/GEPRC_TAKER_H743_bl.hex @@ -0,0 +1,1168 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E30200083D2A00089A +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E30200089D410008C9410008CE +:10006000F5410008214200084D42000885100008B3 +:10007000AD100008D9100008051100083111000862 +:100080005911000885110008E3020008E302000886 +:10009000E3020008E3020008E302000879420008D6 +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E3020008E3020008E30200086C +:1000E000DD420008E3020008E3020008E302000822 +:1000F000E3020008E3020008E3020008B11100086F +:10010000E3020008E302000851430008E30200088C +:10011000E3020008E3020008E3020008E30200082B +:10012000DD11000805120008311200085D120008F8 +:1001300089120008E3020008E3020008E302000855 +:10014000E3020008E3020008E3020008E3020008FB +:10015000B1120008DD12000809130008E3020008CC +:10016000E3020008E3020008E3020008E3020008DB +:10017000E302000829360008E3020008E302000851 +:10018000E3020008E3020008E3020008E3020008BB +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E302000815360008E3020008E302000805 +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F02F0D8FB9B +:1003500003F0DEFC4FF055301F491B4A91423CBF71 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE702F0F0FB03F03CFD30 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E702F0D8BB000600200022002091 +:1003D0000000000808ED00E00000002000060020FA +:1003E00078480008002200205C22002060220020C3 +:1003F00084440020E0020008E0020008E002000857 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002001F0D2FDFEE701F028 +:100430005DFD00DFFEE7000038B500F02FFC00F0A6 +:10044000ABFD02F0BBFA054602F0EEFA0446C0B975 +:100450000E4B9D4217D001339D4241F2883512BFA9 +:10046000044600250124002002F0B2FA0CB100F08D +:1004700075F800F065FD284600F01EF900F06EF8F2 +:10048000F9E70025EFE70546EDE700BF010007B0FB +:1004900008B500F0D5FBA0F120035842584108BD33 +:1004A00007B541F21203022101A8ADF8043000F0B3 +:1004B000E5FB03B05DF804FB38B5302383F3118806 +:1004C000174803680BB101F04FFE0023154A4FF4A3 +:1004D0007A71134801F03EFE002383F31188124C19 +:1004E000236813B12368013B2360636813B1636819 +:1004F000013B63600D4D2B7833B963687BB90220F3 +:1005000000F088FC322363602B78032B07D16368EB +:100510002BB9022000F07EFC4FF47A73636038BD83 +:1005200060220020B90400088023002078220020E7 +:10053000084B187003280CD8DFE800F00805020803 +:10054000022000F05DBC022000F050BC024B0022F3 +:100550005A6070477822002080230020F8B5504B65 +:10056000504A1C461968013100F0998004339342C7 +:10057000F8D162684C4B9A4240F291804B4B9B6899 +:1005800003F1006303F5C0239A4280F088800020C5 +:1005900000F09EFB0220FFF7CBFF454B0021D3F874 +:1005A000E820C3F8E810D3F81021C3F81011D3F8ED +:1005B0001021D3F8EC20C3F8EC10D3F81421C3F8C1 +:1005C0001411D3F81421D3F8F020C3F8F010D3F8A5 +:1005D0001821C3F81811D3F81821D3F8802042F05D +:1005E0000062C3F88020D3F8802022F00062C3F8B4 +:1005F0008020D3F88020D3F8802042F00072C3F826 +:100600008020D3F8802022F00072C3F88020D3F835 +:10061000803072B64FF0E023C3F8084DD4E90004EF +:10062000BFF34F8FBFF36F8F224AC2F88410BFF31E +:100630004F8F536923F480335361BFF34F8FD2F848 +:10064000803043F6E076C3F3C905C3F34E335B0154 +:1006500003EA060C29464CEA81770139C2F8747224 +:10066000F9D2203B13F1200FF2D1BFF34F8FBFF32C +:100670006F8FBFF34F8FBFF36F8F536923F4003336 +:1006800053610023C2F85032BFF34F8FBFF36F8F17 +:10069000302383F31188854680F308882047F8BD0E +:1006A0000000060820000608FFFF050800220020C1 +:1006B0000044025800ED00E02DE9F04F93B0B44B38 +:1006C0002022FF2100900AA89D6800F0E3FBB14AB8 +:1006D0001378A3B90121B04811700360302383F36C +:1006E000118803680BB101F03FFD0023AB4A4FF4C2 +:1006F0007A71A94801F02EFD002383F31188009B35 +:1007000013B1A74B009A1A60A64A1378032B03D0A3 +:1007100000231370A24A53604FF0000A009CD34696 +:100720005646D146012000F06BFB24B19C4B1B6860 +:10073000002B00F02682002000F082FA0390039B39 +:10074000002BF2DB012000F051FB039B213B1F2B10 +:10075000E8D801A252F823F0D907000801080008E0 +:100760009508000825070008250700082507000848 +:1007700027090008F70A0008110A0008730A000890 +:100780009B0A0008C10A000825070008D30A0008D0 +:1007900025070008450B0008790800082507000810 +:1007A000890B0008E50700087908000825070008FC +:1007B000730A000825070008250700082507000818 +:1007C0002507000825070008250700082507000859 +:1007D00025070008950800080220FFF759FE0028A9 +:1007E00040F0F981009B022105A8BAF1000F08BF73 +:1007F0001C4641F21233ADF8143000F03FFA91E795 +:100800004FF47A7000F01CFA071EEBDB0220FFF7B2 +:100810003FFE0028E6D0013F052F00F2DE81DFE831 +:1008200007F0030A0D1013360523042105A80593CC +:1008300000F024FA17E004215548F9E704215A484A +:10084000F6E704215948F3E74FF01C08404608F149 +:10085000040800F03FFA0421059005A800F00EFA04 +:10086000B8F12C0FF2D101204FF0000900FA07F780 +:1008700047EA0B0B5FFA8BFB00F058FB26B10BF03D +:100880000B030B2B08BF0024FFF70AFE4AE70421E5 +:100890004748CDE7002EA5D00BF00B030B2BA1D1C1 +:1008A0000220FFF7F5FD074600289BD00120002617 +:1008B00000F00EFA0220FFF73BFE1FFA86F84046D2 +:1008C00000F016FA0446B0B1039940460136A1F192 +:1008D00040025142514100F01BFA0028EDD1BA46C6 +:1008E000044641F21213022105A83E46ADF8143029 +:1008F00000F0C4F916E725460120FFF719FE244B46 +:100900009B68AB4207D9284600F0E4F9013040F07B +:1009100067810435F3E70025224BBA463E461D7039 +:100920001F4B5D60A8E7002E3FF45CAF0BF00B039C +:100930000B2B7FF457AF0220FFF7FAFD322000F0B7 +:100940007FF9B0F10008FFF64DAF18F003077FF410 +:1009500049AF0F4A08EB0503926893423FF642AF56 +:10096000B8F5807F3FF73EAF124BB845019323DDCA +:100970004FF47A7000F064F90390039A002AFFF6AE +:1009800031AF039A0137019B03F8012BEDE700BF5C +:10099000002200207C23002060220020B9040008EF +:1009A000802300207822002004220020082200203A +:1009B0000C2200207C220020C820FFF769FD07469A +:1009C00000283FF40FAF1F2D11D8C5F120020AAB4C +:1009D00025F0030084494245184428BF424601924D +:1009E00000F032FA019AFF217F4800F053FA4FEAF3 +:1009F000A803C8F387027C492846019300F052FA05 +:100A0000064600283FF46DAF019B05EB830533E7F5 +:100A10000220FFF73DFD00283FF4E4AE00F096F918 +:100A200000283FF4DFAE0027B846704B9B68BB42FE +:100A300018D91F2F11D80A9B01330ED027F00303BA +:100A400012AA134453F8203C05934046042205A9FA +:100A5000043700F001FB8046E7E7384600F03AF93A +:100A60000590F2E7CDF81480042105A800F006F9FE +:100A700002E70023642104A8049300F0F5F800289D +:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA +:100A9000049800F051F90590E6E70023642104A8CA +:100AA000049300F0E1F800287FF49CAE0220FFF7E9 +:100AB000EFFC00283FF496AE049800F03FF9EAE717 +:100AC0000220FFF7E5FC00283FF48CAE00F04EF961 +:100AD000E1E70220FFF7DCFC00283FF483AE05A924 +:100AE000142000F049F907460421049004A800F0FE +:100AF000C5F83946B9E7322000F0A2F8071EFFF624 +:100B000071AEBB077FF46EAE384A07EB09039268FB +:100B100093423FF667AE0220FFF7BAFC00283FF48D +:100B200061AE27F003074F44B9453FF4A5AE4846F0 +:100B300009F1040900F0CEF80421059005A800F0A1 +:100B40009DF8F1E74FF47A70FFF7A2FC00283FF41C +:100B500049AE00F0FBF8002844D00A9B01330BD0CB +:100B600008220AA9002000F09DF900283AD020228E +:100B7000FF210AA800F08EF9FFF792FC1C4801F053 +:100B80002DFA13B0BDE8F08F002E3FF42BAE0BF022 +:100B90000B030B2B7FF426AE0023642105A80593DD +:100BA00000F062F8074600287FF41CAE0220FFF731 +:100BB0006FFC804600283FF415AEFFF771FC41F250 +:100BC000883001F00BFA059800F0E4F946463C46FF +:100BD00000F0ACF9A6E506464EE64FF0000901E646 +:100BE000BA467EE637467CE67C22002000220020C2 +:100BF000A08601000F4B70B51B780C460133DBB2A9 +:100C0000012B11D80C4D4FF47A732968A2FB0332E3 +:100C100022460E6A01462846B047844204D1074B5B +:100C2000002201201A7070BD4FF4FA7001F0D6F95D +:100C30000020F8E710220020B0260020B423002076 +:100C4000002307B5024601210DF107008DF807309A +:100C5000FFF7D0FF20B19DF8070003B05DF804FB5B +:100C60004FF0FF30F9E700000A46042108B5FFF70E +:100C7000C1FF80F00100C0B2404208BD30B4054C55 +:100C80000A46014623682046DD69034BAC4630BC6A +:100C9000604700BFB0260020A086010070B50A4E54 +:100CA00000240A4D01F0B4FC308028683388834268 +:100CB00008D901F0A9FC2B6804440133B4F5C02F16 +:100CC0002B60F2D370BD00BFB62300208823002024 +:100CD00001F06CBD00F1006000F5C02000687047B5 +:100CE00000F10060920000F5C02001F0EDBC0000B2 +:100CF000054B1A68054B1B889B1A834202D9104486 +:100D000001F082BC0020704788230020B623002019 +:100D100038B50446074D29B128682044BDE838405D +:100D200001F08ABC2868204401F074FC0028F3D04C +:100D300038BD00BF882300200020704700F1FF501D +:100D400000F58F10D0F8000870470000064991F8B0 +:100D5000243033B100230822086A81F82430FFF7D9 +:100D6000BFBF0120704700BF8C230020014B1868D3 +:100D7000704700BF0010005C194B01380322084483 +:100D800070B51D68174BC5F30B042D0C1E88A642C9 +:100D90000BD15C680A46013C824213460FD214F91B +:100DA000016F4EB102F8016BF6E7013A03F1080357 +:100DB000ECD181420B4602D22C2203F8012B0424F1 +:100DC000094A1688AE4204D1984284BF967803F847 +:100DD000016B013C02F10402F3D1581A70BD00BF4F +:100DE0000010005C1422002048440008022803D1AF +:100DF000024B4FF000529A61704700BF0008025842 +:100E0000022803D1024B4FF400529A61704700BF91 +:100E100000080258022804D1024A536983F400539F +:100E2000536170470008025870B504464FF47A7653 +:100E30004CB1412C254628BF412506FB05F0641B1B +:100E400001F0CCF8F4E770BD002310B5934203D055 +:100E5000CC5CC4540133F9E710BD0000013810B573 +:100E600010F9013F3BB191F900409C4203D11AB106 +:100E70000131013AF4E71AB191F90020981A10BD36 +:100E80001046FCE703460246D01A12F9011B00295E +:100E9000FAD1704702440346934202D003F8011B83 +:100EA000FAE770472DE9F8431F4D14460746884678 +:100EB00095F8242052BBDFF870909CB395F824304D +:100EC0002BB92022FF2148462F62FFF7E3FF95F858 +:100ED00024004146C0F1080205EB8000A24228BF71 +:100EE0002246D6B29200FFF7AFFF95F82430A41B3C +:100EF00017441E449044E4B2F6B2082E85F82460EC +:100F0000DBD1FFF723FF0028D7D108E02B6A03EBE2 +:100F100082038342CFD0FFF719FF0028CBD10020F6 +:100F2000BDE8F8830120FBE78C230020024B1A78F0 +:100F3000024B1A70704700BFB4230020102200201B +:100F400010B5114C114800F079FB21460F4800F014 +:100F5000A1FB24684FF47A70D4F89020D2F80438BA +:100F600043F00203C2F80438FFF75EFF0849204649 +:100F700000F09EFCD4F89020D2F8043823F002034D +:100F8000C2F8043810BD00BF04460008B026002097 +:100F90000C46000870470000F8B571B60023012028 +:100FA0001A46194600F058FA0446802001F09CFCCD +:100FB000002849D00025254A80274FF4D06C3D26D3 +:100FC000136913F0C06F26D1D2F8103113F0C06F3F +:100FD00021D1236805F1006199602368D8602368F6 +:100FE0005F602368C3F800C021680B6843F0010309 +:100FF0000B6021680B6823F01E030B6021680B68EF +:10100000DB07FCD4237B8035616806FA03F3B5F572 +:10101000001F0B60D4D1204600F05EFAB5F5001F2A +:1010200011D000240A4E0B4D012001F0BFFB338884 +:10103000A34205D928682044013401F0FDFAF6E7FF +:10104000002001F0B3FB61B6F8BD00BF00200052E4 +:10105000B62300208823002030B50A44084D914271 +:101060000DD011F8013B5840082340F30004013B28 +:101070002C4013F0FF0384EA5000F6D1EFE730BDB7 +:101080002083B8ED08B5074B074A196801F03D0108 +:10109000996053680BB190689847BDE8084001F02B +:1010A00005BD00BF00000240B823002008B5084B72 +:1010B0001968890901F03D018A019A60054AD368DF +:1010C0000BB110699847BDE8084001F0EFBC00BFC4 +:1010D00000000240B823002008B5084B1968090C2D +:1010E00001F03D010A049A60054A53690BB1906909 +:1010F0009847BDE8084001F0D9BC00BF000002409D +:10110000B823002008B5084B1968890D01F03D018E +:101110008A059A60054AD3690BB1106A9847BDE801 +:10112000084001F0C3BC00BF00000240B82300200B +:1011300008B5074B074A596801F03D01D960536A69 +:101140000BB1906A9847BDE8084001F0AFBC00BF02 +:1011500000000240B823002008B5084B59688909EF +:1011600001F03D018A01DA60054AD36A0BB1106BC8 +:101170009847BDE8084001F099BC00BF000002405C +:10118000B823002008B5084B5968090C01F03D014F +:101190000A04DA60054A536B0BB1906B9847BDE8BF +:1011A000084001F083BC00BF00000240B8230020CB +:1011B00008B5084B5968890D01F03D018A05DA60D0 +:1011C000054AD36B0BB1106C9847BDE8084001F09D +:1011D0006DBC00BF00000240B823002008B5074BDB +:1011E000074A196801F03D019960536C0BB1906C8E +:1011F0009847BDE8084001F059BC00BF0004024018 +:10120000B823002008B5084B1968890901F03D0191 +:101210008A019A60054AD36C0BB1106D9847BDE8FE +:10122000084001F043BC00BF00040240B823002086 +:1012300008B5084B1968090C01F03D010A049A60D1 +:10124000054A536D0BB1906D9847BDE8084001F019 +:101250002DBC00BF00040240B823002008B5084B95 +:101260001968890D01F03D018A059A60054AD36D20 +:101270000BB1106E9847BDE8084001F017BC00BFE5 +:1012800000040240B823002008B5074B074A5968FC +:1012900001F03D01D960536E0BB1906E9847BDE8E7 +:1012A000084001F003BC00BF00040240B823002046 +:1012B00008B5084B5968890901F03D018A01DA60D7 +:1012C000054AD36E0BB1106F9847BDE8084001F096 +:1012D000EDBB00BF00040240B823002008B5084B56 +:1012E0005968090C01F03D010A04DA60054A536FA0 +:1012F0000BB1906F9847BDE8084001F0D7BB00BF25 +:1013000000040240B823002008B5084B5968890D35 +:1013100001F03D018A05DA60054AD36F13B1D2F8B6 +:1013200080009847BDE8084001F0C0BB00040240BF +:10133000B823002000230C4910B51A460B4C0B6053 +:1013400054F82300026001EB430004334260402B59 +:10135000F6D1074A4FF0FF339360D360C2F80834E8 +:10136000C2F80C3410BD00BFB82300205844000858 +:10137000000002400F28F8B510D9102810D011280D +:1013800011D0122808D10F240720DFF8C8E0012669 +:10139000DEF80050A04208D9002653E00446F4E7E6 +:1013A0000F240020F1E70724FBE706FA00F73D428F +:1013B0004AD1264C4FEA001C3D4304EB00160EEBCD +:1013C000C000CEF80050C0E90123FBB273B1204841 +:1013D000D0F8D83043F00103C0F8D830D0F800314D +:1013E00043F00103C0F80031D0F8003117F47F4F0B +:1013F0000ED01748D0F8D83043F00203C0F8D830E8 +:10140000D0F8003143F00203C0F80031D0F80031C9 +:1014100054F80C00036823F01F030360056815F0FF +:101420000105FBD104EB0C033D2493F80CC05F686D +:1014300004FA0CF43C6021240560446112B1987BED +:1014400000F054F83046F8BD0130A3E758440008D6 +:1014500000440258B823002010B5302484F31188CA +:10146000FFF788FF002383F3118810BD10B50446F1 +:10147000807B00F051F801231549627B03FA02F2E8 +:101480000B6823EA0203DAB20B6072B9114AD2F890 +:10149000D81021F00101C2F8D810D2F8001121F0C3 +:1014A0000101C2F80011D2F8002113F47F4F0ED1D0 +:1014B000084BD3F8D82022F00202C3F8D820D3F882 +:1014C000002122F00202C3F80021D3F8003110BD40 +:1014D000B82300200044025808B5302383F3118854 +:1014E000FFF7C4FF002383F3118808BD090100F151 +:1014F0006043012203F56143C9B283F8001300F091 +:101500001F039A4043099B0003F1604303F56143C5 +:10151000C3F880211A60704700F01F0301229A402F +:10152000430900F160409B0000F5614003F1604316 +:1015300003F56143C3F88020C3F88021002380F8BD +:1015400000337047026843681143016003B11847D4 +:101550007047000013B5406B00F58054D4F8A438F0 +:101560001A681178042914D1017C022911D1197942 +:10157000012312898B4013420BD101A94C3002F098 +:1015800085F9D4F8A4480246019B2179206800F02F +:10159000DFF902B010BD0000143002F007B90000FE +:1015A0004FF0FF33143002F001B900004C3002F06C +:1015B000D9B900004FF0FF334C3002F0D3B900002E +:1015C000143002F0D5B800004FF0FF31143002F0B3 +:1015D000CFB800004C3002F0A5B900004FF0FF3248 +:1015E0004C3002F09FB900000020704710B500F5A4 +:1015F0008054D4F8A4381A681178042917D1017CD2 +:10160000022914D15979012352898B4013420ED1FA +:10161000143002F067F8024648B1D4F8A4484FF4F9 +:10162000407361792068BDE8104000F07FB910BDBB +:10163000406BFFF7DBBF0000704700007FB5124B27 +:1016400001250426044603600023057400F184028A +:1016500043602946C0E902330C4B029014300193D9 +:101660004FF44073009602F019F8094B04F69442C7 +:10167000294604F14C000294CDE900634FF4407315 +:1016800002F0E0F804B070BD5845000831160008BB +:10169000551500080A68302383F311880B790B3342 +:1016A00042F823004B79133342F823008B7913B1AE +:1016B0000B3342F8230000F58053C3F8A41802232B +:1016C0000374002080F311887047000038B5037F51 +:1016D000044613B190F85430ABB90125201D022106 +:1016E000FFF730FF04F114006FF00101257700F0DF +:1016F0009DFC04F14C0084F854506FF00101BDE8EA +:10170000384000F093BC38BD10B5012104460430C8 +:10171000FFF718FF0023237784F8543010BD000032 +:1017200038B504460025143001F0D0FF04F14C0018 +:10173000257702F09FF8201D84F854500121FFF70F +:1017400001FF2046BDE83840FFF750BF90F88030D9 +:1017500003F06003202B06D190F881200023212A7A +:1017600003D81F2A06D800207047222AFBD1C0E9DF +:101770001D3303E0034A426707228267C3670120E3 +:10178000704700BF2C22002037B500F58055D5F8F2 +:10179000A4381A68117804291AD1017C022917D1BA +:1017A0001979012312898B40134211D100F14C04A5 +:1017B000204602F01FF958B101A9204602F066F850 +:1017C000D5F8A4480246019B2179206800F0C0F8B2 +:1017D00003B030BD01F10B03F0B550F8236085B0C4 +:1017E00004460D46FEB1302383F3118804EB8507D0 +:1017F000301D0821FFF7A6FEFB6806F14C005B696F +:101800001B681BB1019002F04FF8019803A902F088 +:101810003DF8024648B1039B2946204600F098F85F +:10182000002383F3118805B0F0BDFB685A69126884 +:10183000002AF5D01B8A013B1340F1D104F180024C +:10184000EAE70000133138B550F82140ECB13023FD +:1018500083F3118804F58053D3F8A42813685279D0 +:1018600003EB8203DB689B695D6845B10421601866 +:10187000FFF768FE294604F1140001F03DFF204601 +:10188000FFF7B4FE002383F3118838BD70470000D2 +:1018900001F00ABA01234022002110B5044600F8E5 +:1018A000303BFFF7F7FA0023C4E9013310BD000015 +:1018B00010B53023044683F31188242241600021AF +:1018C0000C30FFF7E7FA204601F010FA022300205F +:1018D000237080F3118810BD70B500EB81030546BD +:1018E00050690E461446DA6018B110220021FFF745 +:1018F000D1FAA06918B110220021FFF7CBFA3146C6 +:101900002846BDE8704001F0F7BA00008368202245 +:10191000002103F0011310B5044683601030FFF777 +:10192000B9FA2046BDE8104001F072BBF0B40125C1 +:1019300000EB810447898D40E4683D43A4694581FB +:1019400023600023A2606360F0BC01F08FBB000045 +:10195000F0B4012500EB810407898D40E4683D4324 +:101960006469058123600023A2606360F0BC01F01C +:1019700005BC000070B50223002504462422037034 +:101980002946C0F888500C3040F8045CFFF782FA12 +:10199000204684F8705001F043FA63681B6823B155 +:1019A00029462046BDE87040184770BD0378052BD6 +:1019B00010B504460AD080F88C30052303704368C4 +:1019C0001B680BB1042198470023A36010BD0000E1 +:1019D0000178052906D190F88C20436802701B68B5 +:1019E00003B118477047000070B590F87030044696 +:1019F00013B1002380F8703004F18002204601F01A +:101A00002BFB63689B68B3B994F8803013F06005D2 +:101A100035D00021204601F01DFE0021204601F0B6 +:101A20000DFE63681B6813B1062120469847062304 +:101A300084F8703070BD204698470028E4D0B4F890 +:101A40008630A26F9A4288BFA36794F98030A56F51 +:101A5000002B4FF0300380F20381002D00F0F28064 +:101A6000092284F8702083F3118800212046D4E9EC +:101A70001D23FFF76DFF002383F31188DAE794F845 +:101A8000812003F07F0343EA022340F202329342B3 +:101A900000F0C58021D8B3F5807F48D00DD8012B48 +:101AA0003FD0022B00F09380002BB2D104F18802CA +:101AB00062670222A267E367C1E7B3F5817F00F0A6 +:101AC0009B80B3F5407FA4D194F88230012BA0D144 +:101AD000B4F8883043F0020332E0B3F5006F4DD024 +:101AE00017D8B3F5A06F31D0A3F5C063012B90D800 +:101AF0006368204694F882205E6894F88310B4F8F6 +:101B00008430B047002884D0436863670368A367C4 +:101B10001AE0B3F5106F36D040F6024293427FF4DC +:101B200078AF5C4B63670223A3670023C3E794F895 +:101B30008230012B7FF46DAFB4F8883023F00203BC +:101B4000A4F88830C4E91D55E56778E7B4F880301B +:101B5000B3F5A06F0ED194F88230204684F88A3015 +:101B600001F0BCF963681B6813B101212046984756 +:101B7000032323700023C4E91D339CE704F18B0386 +:101B800063670123C3E72378042B10D1302383F349 +:101B900011882046FFF7BAFE85F311880321636898 +:101BA00084F88B5021701B680BB12046984794F83D +:101BB0008230002BDED084F88B30042323706368DE +:101BC0001B68002BD6D0022120469847D2E794F814 +:101BD000843020461D0603F00F010AD501F02EFACD +:101BE000012804D002287FF414AF2B4B9AE72B4B2B +:101BF00098E701F015FAF3E794F88230002B7FF4B0 +:101C000008AF94F8843013F00F01B3D01A062046C1 +:101C100002D501F037FDADE701F028FDAAE794F801 +:101C20008230002B7FF4F5AE94F8843013F00F016E +:101C3000A0D01B06204602D501F00CFD9AE701F06A +:101C4000FDFC97E7142284F8702083F311882B465B +:101C50002A4629462046FFF769FE85F31188E9E602 +:101C60005DB1152284F8702083F31188002120468D +:101C7000D4E91D23FFF75AFEFDE60B2284F87020FD +:101C800083F311882B462A4629462046FFF760FE3B +:101C9000E3E700BF88450008804500088445000848 +:101CA00038B590F870300446002B3ED0063BDAB2CF +:101CB0000F2A34D80F2B32D8DFE803F03731310840 +:101CC000223231313131313131313737856FB0F82E +:101CD00086309D4214D2C3681B8AB5FBF3F203FB26 +:101CE00012556DB9302383F311882B462A462946B5 +:101CF000FFF72EFE85F311880A2384F870300EE07A +:101D0000142384F87030302383F311880023204695 +:101D10001A461946FFF70AFE002383F3118838BDDF +:101D2000C36F03B198470023E7E70021204601F085 +:101D300091FC0021204601F081FC63681B6813B10F +:101D40000621204698470623D7E7000010B590F8F3 +:101D500070300446142B29D017D8062B05D001D893 +:101D60001BB110BD093B022BFBD80021204601F01E +:101D700071FC0021204601F061FC63681B6813B10F +:101D8000062120469847062319E0152BE9D10B239D +:101D900080F87030302383F3118800231A461946E7 +:101DA000FFF7D6FD002383F31188DAE7C3689B6948 +:101DB0005B68002BD5D1C36F03B19847002384F82B +:101DC0007030CEE700238268037503691B68996849 +:101DD0009142FBD25A6803604260106058607047BD +:101DE00000238268037503691B6899689142FBD8D8 +:101DF0005A680360426010605860704708B5084632 +:101E0000302383F311880A7D0023052A06D8DFE8F2 +:101E100002F00B050503120E826913604FF0FF33C9 +:101E20008361FFF7CFFF002383F3118808BD826928 +:101E3000936801339360D0E9003213605A60EDE794 +:101E4000FFF7C0BF054BD968087518680268536072 +:101E50001A600122D8600275FEF7D4BA402400202F +:101E60000C4B30B5DD684B1C87B004460FD02B46B9 +:101E7000094A684600F06EF92046FFF7E3FF009B31 +:101E800013B1684600F070F9A86907B030BDFFF7DC +:101E9000D9FFF9E740240020FD1D000838B50C4D9E +:101EA00004468161EB6881689A68914203D8BDE875 +:101EB0003840FFF787BF1846FFF792FF012301461E +:101EC000EC6020462375BDE83840FEF79BBA00BFA2 +:101ED00040240020044B1A68DB6890689B68984295 +:101EE00094BF00200120704740240020084B10B50B +:101EF0001C68D868226853601A600122DC60227571 +:101F0000FFF76EFF01462046BDE81040FEF77ABAA3 +:101F10004024002038B50123084C0025237065605B +:101F200001F0A4FD01F0CAFD0549064801F05CFE80 +:101F30000223237085F3118838BD00BFA826002036 +:101F4000904500084024002000F044B9034A51683D +:101F500053685B1A9842FBD8704700BF001000E03E +:101F60008B600223086108468B8270478368A3F167 +:101F7000840243F8142C026943F8442C426943F864 +:101F8000402C094A43F8242CC268A3F1200043F8EE +:101F9000182C022203F80C2C002203F80B2C034A05 +:101FA00043F8102C704700BF1D0400084024002097 +:101FB00008B5FFF7DBFFBDE80840FFF741BF0000B1 +:101FC000024BDB6898610F20FFF73CBF40240020E4 +:101FD000302383F31188FFF7F3BF000008B50146F3 +:101FE000302383F311880820FFF73AFF002383F39F +:101FF000118808BD064BDB6839B1426818605A6029 +:10200000136043600420FFF72BBF4FF0FF30704791 +:10201000402400200368984206D01A68026050608D +:1020200018469961FFF70CBF7047000038B50446A9 +:102030000D462068844200D138BD036823605C608F +:102040008561FFF7FDFEF4E7036810B59C68A242C6 +:102050000CD85C688A600B604C602160596099689C +:102060008A1A9A604FF0FF33836010BD121B1B6801 +:10207000ECE700000A2938BF0A2170B504460D4676 +:102080000A26601901F0ECFC01F0D4FC041BA54207 +:1020900003D8751C04462E46F3E70A2E04D9012006 +:1020A000BDE8704001F0E0BD70BD0000F8B5144B14 +:1020B0000D460A2A4FF00A07D96103F11001826028 +:1020C00038BF0A22416019691446016048601861EE +:1020D000A81801F0B5FC01F0ADFC431B0646A34275 +:1020E00006D37C1C28192746354601F0B9FCF2E7D7 +:1020F0000A2F04D90120BDE8F84001F0B5BDF8BDB4 +:1021000040240020F8B506460D4601F093FC0F4A26 +:10211000134653F8107F9F4206D12A4601463046A7 +:10212000BDE8F840FFF7C2BFD169BB68441A2C195B +:1021300028BF2C46A34202D92946FFF79BFF22461F +:1021400031460348BDE8F840FFF77EBF4024002039 +:1021500050240020C0E90323002310B45DF8044B91 +:102160004361FFF7CFBF000010B5194C23699842B7 +:102170000DD08168D0E9003213605A609A680A4431 +:102180009A60002303604FF0FF33A36110BD026823 +:10219000234643F8102F53600022026022699A42BE +:1021A00003D1BDE8104001F055BC936881680B4431 +:1021B000936001F03FFC2269E1699268441AA242EF +:1021C000E4D91144BDE81040091AFFF753BF00BF1E +:1021D000402400202DE9F047DFF8BC8008F110070B +:1021E0002C4ED8F8105001F025FCD8F81C40AA68F5 +:1021F000031B9A423ED814444FF00009D5E900323F +:10220000C8F81C4013605A60C5F80090D8F8103028 +:10221000B34201D101F01EFC89F31188D5E90331E5 +:1022200028469847302383F311886B69002BD8D058 +:1022300001F000FC6A69A0EB040982464A450DD210 +:10224000022001F011FD0022D8F81030B34208D16D +:1022500051462846BDE8F047FFF728BF121A22442E +:10226000F2E712EB09092946384638BF4A46FFF71C +:10227000EBFEB5E7D8F81030B34208D01444C8F8E4 +:102280001C00211AA960BDE8F047FFF7F3BEBDE8C6 +:10229000F08700BF50240020402400200020704719 +:1022A000FEE70000704700004FF0FF30704700006D +:1022B00002290CD0032904D00129074818BF0020A7 +:1022C0007047032A05D8054800EBC2007047044850 +:1022D00070470020704700BF684600083C2200207D +:1022E0001C46000870B59AB005460846144601A978 +:1022F00000F0C2F801A8FEF7C5FD431C0022C6B2DB +:102300005B001046C5E9003423700323023404F84F +:10231000013C01ABD1B202348E4201D81AB070BD7B +:1023200013F8011B013204F8010C04F8021CF1E758 +:1023300008B5302383F311880348FFF725FA0023FB +:1023400083F3118808BD00BFB026002090F88030CC +:1023500003F01F02012A07D190F881200B2A03D134 +:102360000023C0E91D3315E003F06003202B08D1E2 +:10237000B0F884302BB990F88120212A03D81F2A85 +:1023800004D8FFF7E3B9222AEBD0FAE7034A426701 +:1023900007228267C3670120704700BF33220020F5 +:1023A00007B5052917D8DFE801F019160319192018 +:1023B000302383F31188104A01210190FFF78CFA32 +:1023C000019802210D4AFFF787FA0D48FFF7A8F997 +:1023D000002383F3118803B05DF804FB302383F3FB +:1023E00011880748FFF772F9F2E7302383F3118869 +:1023F0000348FFF789F9EBE7BC450008E045000812 +:10240000B026002038B50C4D0C4C2A460C4904F17E +:102410000800FFF767FF05F1CA0204F1100009493F +:10242000FFF760FF05F5CA7204F118000649BDE820 +:102430003840FFF757BF00BF883F00203C220020F4 +:102440009C450008A6450008B145000870B5044643 +:1024500008460D46FEF716FDC6B220460134037845 +:102460000BB9184670BD32462946FEF7F7FC002826 +:10247000F3D10120F6E700002DE9F04705460C46B0 +:10248000FEF700FD2A49C6B22846FFF7DFFF08B174 +:102490000936F6B227492846FFF7D8FF08B11036AB +:1024A000F6B2632E0BD8DFF88880DFF88890224FD1 +:1024B000DFF890A02E7846B92670BDE8F08729464F +:1024C0002046BDE8F04701F0A1BF252E2CD1072200 +:1024D00041462846FEF7C2FC60B9184B224603F17C +:1024E000100153F8040B8B4242F8040BF9D1073565 +:1024F0001034DFE7082249462846FEF7AFFC98B9BA +:10250000A21C0F4B197802320909C95D02F8041C9C +:1025100013F8011B01F00F015345C95D02F8031CBC +:10252000F0D118340835C5E7013504F8016BC1E76F +:1025300088460008B1450008A146000890460008FA +:1025400000E8F11F0CE8F11FBFF34F8F044B1A692D +:102550005107FCD1D3F810215207F8D1704700BFC2 +:102560000020005208B50D4B1B78ABB9FFF7ECFF0C +:102570000B4BDA68D10704D50A4A5A6002F1883257 +:102580005A60D3F80C21D20706D5064AC3F80421B5 +:1025900002F18832C3F8042108BD00BFE6410020E3 +:1025A000002000522301674508B5114B1B78F3B991 +:1025B000104B1A69510703D5DA6842F04002DA601D +:1025C000D3F81021520705D5D3F80C2142F0400270 +:1025D000C3F80C21FFF7B8FF064BDA6842F001029E +:1025E000DA60D3F80C2142F00102C3F80C2108BDD7 +:1025F000E6410020002000520F289ABF00F58060BD +:1026000040040020704700004FF400307047000085 +:10261000102070470F2808B50BD8FFF7EDFF00F525 +:1026200000330268013204D104308342F9D1012021 +:1026300008BD0020FCE700000F2838B505463FD84C +:10264000FFF782FF1F4CFFF78DFF4FF0FF33072886 +:102650006361C4F814311DD82361FFF775FF0302CD +:1026600043F02403E360E36843F08003E3602369FD +:102670005A07FCD42846FFF767FFFFF7BDFF4FF46A +:10268000003100F0B3F92846FFF78EFFBDE838406F +:10269000FFF7C0BFC4F81031FFF756FFA0F10803E1 +:1026A0001B0243F02403C4F80C31D4F80C3143F07E +:1026B0008003C4F80C31D4F810315B07FBD4D9E7A0 +:1026C000002038BD002000522DE9F84F05460C4689 +:1026D000104645EA0203DE0602D00020BDE8F88F6E +:1026E00020F01F00DFF8BCB0DFF8BCA0FFF73AFF16 +:1026F00004EB0008444503D10120FFF755FFEDE747 +:1027000020222946204601F071FE10B920352034E0 +:10271000F0E72B4605F120021F68791CDDD1043358 +:102720009A42F9D105F178431B481C4EB3F5801F3E +:102730001B4B38BF184603F1F80332BFD946D146C8 +:102740001E46FFF701FF0760A5EB040C336804F198 +:102750001C0143F002033360231FD9F8007017F007 +:102760000507FAD153F8042F8B424CF80320F4D11B +:10277000BFF34F8FFFF7E8FE4FF0FF3320222146D3 +:1027800003602846336823F00203336001F02EFE15 +:102790000028BBD03846B0E7142100520C2000526C +:1027A00014200052102000521021005210B5084C85 +:1027B000237828B11BB9FFF7D5FE0123237010BD84 +:1027C000002BFCD02070BDE81040FFF7EDBE00BF2D +:1027D000E64100202DE9F04F0D4685B0814658B105 +:1027E00011F00D0614BF2022082211F008030193F6 +:1027F00004D0431E03426AD0002435E0002E37D0B7 +:1028000009F11F0121F01F094FF00108324F05F0B7 +:102810000403DFF8D0A005EA080BBBF1000F32D0AB +:102820007869C0072FD408F101080C37B8F1060FFA +:10283000F3D19EB9294D4946A819019201F060F9DA +:10284000044600283AD11836019A782EF3D1494629 +:1028500001F056F90446002830D1019A4946204833 +:1028600001F04EF9044668BB204605B0BDE8F08F84 +:102870000029C9D101462846029201F041F90446D7 +:10288000E0B9029AC0E713B178694107CBD5AC072C +:1028900002D578698007C6D5019911B17869010719 +:1028A000C1D51820494600FB08A0CDE9022301F05C +:1028B00027F90446DDE902230028B4D04A46002166 +:1028C000204601E04A460021FEF7E4FACCE7024642 +:1028D000002E95D198E700BFB446000818420020AA +:1028E000E8410020004200200121FFF773BF0000F3 +:1028F000F8B5144D01241827134E40F2FF32002181 +:102900000120FEF7C7FA07FB046001342A6955F875 +:102910000C1F01F0E1F8062CF5D137254FF4C05417 +:102920002046FFF7E1FF014628B122460748BDE8EF +:10293000F84001F0D1B8C4EBC404013D4FEAD4041F +:10294000EED1F8BDB446000800420020E841002066 +:102950000244074BD2B210B5904200D110BD441CC6 +:1029600000B253F8200041F8040BE0B2F4E700BFD6 +:10297000504000580E4B30B51C6F240405D41C6F1A +:102980001C671C6F44F400441C670A4C0244236813 +:10299000D2B243F480732360074B904200D130BD24 +:1029A000441C51F8045B00B243F82050E0B2F4E755 +:1029B00000440258004802585040005807B5012210 +:1029C00001A90020FFF7C4FF019803B05DF804FBE4 +:1029D00013B50446FFF7F2FFA04205D0012201A97A +:1029E00000200194FFF7C6FF02B010BD0144BFF301 +:1029F0004F8F064B884204D3BFF34F8FBFF36F8FC7 +:102A00007047C3F85C022030F4E700BF00ED00E03F +:102A1000034B1A681AB9034AD2F8D0241A607047D7 +:102A2000904200200040025808B5FFF7F1FF024B2A +:102A30001868C0F3806008BD90420020EFF309835E +:102A4000054968334A6B22F001024A6383F309881F +:102A5000002383F31188704700EF00E0302080F3FB +:102A6000118862B60D4B0E4AD96821F4E061090461 +:102A7000090C0A430B49DA60D3F8FC2042F080725B +:102A8000C3F8FC20084AC2F8B01F116841F00101E8 +:102A900011602022DA7783F82200704700ED00E011 +:102AA0000003FA0555CEACC5001000E0302310B588 +:102AB00083F311880E4B5B6813F4006314D0F1EEBE +:102AC000103AEFF309844FF08073683CE361094BDF +:102AD000DB6B236684F30988FFF7FCF910B1064B22 +:102AE000A36110BD054BFBE783F31188F9E700BF35 +:102AF00000ED00E000EF00E02F04000832040008C1 +:102B000070B5BFF34F8FBFF36F8F1A4A0021C2F821 +:102B10005012BFF34F8FBFF36F8F536943F40033ED +:102B20005361BFF34F8FBFF36F8FC2F88410BFF3B1 +:102B30004F8FD2F8803043F6E074C3F3C900C3F37B +:102B40004E335B0103EA0406014646EA817501390A +:102B5000C2F86052F9D2203B13F1200FF2D1BFF33B +:102B60004F8F536943F480335361BFF34F8FBFF3EB +:102B70006F8F70BD00ED00E0FEE70000214B2248A2 +:102B8000224A70B5904237D3214BC11EDA1C121A6B +:102B900022F003028B4238BF00220021FEF77AF9AF +:102BA0001C4A0023C2F88430BFF34F8FD2F8803024 +:102BB00043F6E074C3F3C900C3F34E335B0103EA89 +:102BC0000406014646EA81750139C2F86C52F9D211 +:102BD000203B13F1200FF2D1BFF34F8FBFF36F8F64 +:102BE000BFF34F8FBFF36F8F0023C2F85032BFF394 +:102BF0004F8FBFF36F8F70BD53F8041B40F8041B59 +:102C0000C0E700BFD448000884440020844400206A +:102C10008444002000ED00E0074BD3F8D81021EAEF +:102C20000001C3F8D810D3F8002122EA0002C3F84B +:102C30000021D3F8003170470044025870B5D0E944 +:102C4000244300224FF0FF359E6804EB42135101EC +:102C5000D3F80009002805DAD3F8000940F08040D5 +:102C6000C3F80009D3F8000B002805DAD3F8000BED +:102C700040F08040C3F8000B013263189642C3F85D +:102C80000859C3F8085BE0D24FF00113C4F81C38B0 +:102C900070BD0000890141F02001016103699B06BC +:102CA000FCD41220FFF752B910B50A4C2046FEF7AB +:102CB000F1FD094BC4F89030084BC4F89430084C2F +:102CC0002046FEF7E7FD074BC4F89030064BC4F8EA +:102CD000943010BD944200200000084020470008B6 +:102CE00030430020000004402C47000870B50378F2 +:102CF0000546012B5CD1434BD0F89040984258D107 +:102D0000414B0E216520D3F8D82042F00062C3F871 +:102D1000D820D3F8002142F00062C3F80021D3F894 +:102D20000021D3F8802042F00062C3F88020D3F85D +:102D3000802022F00062C3F88020D3F88030FEF7B4 +:102D4000D5FB324BE360324BC4F800380023D5F892 +:102D50009060C4F8003EC02323604FF40413A363C3 +:102D60003369002BFCDA01230C203361FFF7EEF806 +:102D70003369DB07FCD41220FFF7E8F83369002B36 +:102D8000FCDA00262846A660FFF758FF6B68C4F8F7 +:102D90001068DB68C4F81468C4F81C6883BB1D4B5A +:102DA000A3614FF0FF336361A36843F00103A360A5 +:102DB00070BD194B9842C9D1134B4FF08060D3F8C6 +:102DC000D82042F00072C3F8D820D3F8002142F096 +:102DD0000072C3F80021D3F80021D3F8802042F01C +:102DE0000072C3F88020D3F8802022F00072C3F86C +:102DF0008020D3F88030FFF70FFF0E214D209EE793 +:102E0000064BCDE794420020004402584014004095 +:102E100003002002003C30C030430020083C30C09A +:102E2000F8B5D0F89040054600214FF000662046E6 +:102E3000FFF730FFD5F8941000234FF001128F6890 +:102E40004FF0FF30C4F83438C4F81C2804EB4312A8 +:102E500001339F42C2F80069C2F8006BC2F808094A +:102E6000C2F8080BF2D20B68D5F89020C5F898305C +:102E7000636210231361166916F01006FBD112204D +:102E8000FFF764F8D4F8003823F4FE63C4F8003880 +:102E9000A36943F4402343F01003A3610923C4F85A +:102EA0001038C4F814380B4BEB604FF0C043C4F833 +:102EB000103B094BC4F8003BC4F81069C4F8003952 +:102EC000D5F8983003F1100243F48013C5F8982028 +:102ED000A362F8BDFC46000840800010D0F89020A6 +:102EE00090F88A10D2F8003823F4FE6343EA011305 +:102EF000C2F80038704700002DE9F84300EB810369 +:102F0000D0F890500C468046DA680FFA81F94801F3 +:102F1000166806F00306731E022B05EB41134FF0F3 +:102F2000000194BFB604384EC3F8101B4FF00101E6 +:102F300004F1100398BF06F1805601FA03F391697A +:102F400098BF06F5004600293AD0578A04F1580187 +:102F5000374349016F50D5F81C180B430021C5F8C1 +:102F60001C382B180127C3F81019A7405369611E9C +:102F70009BB3138A928B9B08012A88BF5343D8F8CE +:102F80009820981842EA034301F140022146C8F80C +:102F90009800284605EB82025360FFF77BFE08EBA2 +:102FA0008900C3681B8A43EA845348341E43640182 +:102FB0002E51D5F81C381F43C5F81C78BDE8F8839E +:102FC00005EB4917D7F8001B21F40041C7F8001B97 +:102FD000D5F81C1821EA0303C0E704F13F030B4AAC +:102FE0002846214605EB83035A60FFF753FE05EBA5 +:102FF0004910D0F8003923F40043C0F80039D5F85F +:103000001C3823EA0707D7E70080001000040002FD +:10301000D0F894201268C0F89820FFF70FBE000087 +:103020005831D0F8903049015B5813F4004004D077 +:1030300013F4001F0CBF0220012070474831D0F864 +:10304000903049015B5813F4004004D013F4001F82 +:103050000CBF02200120704700EB8101CB68196A88 +:103060000B6813604B6853607047000000EB8103EE +:1030700030B5DD68AA691368D36019B9402B84BFE5 +:10308000402313606B8A1468D0F890201C4402EB34 +:103090004110013C09B2B4FBF3F46343033323F062 +:1030A000030343EAC44343F0C043C0F8103B2B681A +:1030B00003F00303012B0ED1D2F8083802EB4110C4 +:1030C00013F4807FD0F8003B14BF43F0805343F0EB +:1030D0000053C0F8003B02EB4112D2F8003B43F032 +:1030E0000443C2F8003B30BD2DE9F041D0F89060B8 +:1030F00005460C4606EB4113D3F8087B3A07C3F8A4 +:10310000087B08D5D6F814381B0704D500EB8103DB +:10311000DB685B689847FA071FD5D6F81438DB07D9 +:103120001BD505EB8403D968CCB98B69488A5A68EA +:10313000B2FBF0F600FB16228AB91868DA689042F2 +:103140000DD2121AC3E90024302383F311882146DB +:103150002846FFF78BFF84F31188BDE8F081012337 +:1031600003FA04F26B8923EA02036B81CB68002B1C +:10317000F3D021462846BDE8F041184700EB810313 +:103180004A0170B5DD68D0F890306C692668E66059 +:1031900056BB1A444FF40020C2F810092A6802F006 +:1031A0000302012A0AB20ED1D3F8080803EB421435 +:1031B00010F4807FD4F8000914BF40F0805040F034 +:1031C0000050C4F8000903EB4212D2F8000940F0A5 +:1031D0000440C2F800090122D3F8340802FA01F1D0 +:1031E0000143C3F8341870BD19B9402E84BF402084 +:1031F000206020681A442E8A8419013CB4FBF6F43E +:1032000040EAC44040F00050C6E700002DE9F84312 +:10321000D0F8906005460C464F0106EB4113D3F8F9 +:10322000088918F0010FC3F808891CD0D6F81038A7 +:10323000DB0718D500EB8103D3F80CC0DCF81430A1 +:10324000D3F800E0DA68964530D2A2EB0E024FF0D8 +:1032500000091A60C3F80490302383F31188FFF744 +:103260008DFF89F3118818F0800F1DD0D6F83438FF +:103270000126A640334217D005EB84030134D5F86C +:103280009050D3F80CC0E4B22F44DCF8142005EBC6 +:103290000434D2F800E05168714514D3D5F83438BD +:1032A00023EA0606C5F83468BDE8F883012303FA6B +:1032B00001F2038923EA02030381DCF80830002BC2 +:1032C000D1D09847CFE7AEEB0103BCF810008342A2 +:1032D00028BF0346D7F8180980B2B3EB800FE3D8B4 +:1032E0009068A0F1040959F8048FC4F80080A0EB9D +:1032F00009089844B8F1040FF5D818440B449060BD +:103300005360C8E72DE9F84FD0F8905004466E6935 +:10331000AB691E4016F480586E6103D0BDE8F84FCB +:10332000FEF728BB002E12DAD5F8003E9B0705D029 +:10333000D5F8003E23F00303C5F8003ED5F8043865 +:10334000204623F00103C5F80438FEF741FB37059A +:1033500005D52046FFF772FC2046FEF727FBB00498 +:103360000CD5D5F8083813F0060FEB6823F470532A +:103370000CBF43F4105343F4A053EB6031071BD54B +:103380006368DB681BB9AB6923F00803AB61237882 +:10339000052B0CD1D5F8003E9A0705D0D5F8003E94 +:1033A00023F00303C5F8003E2046FEF711FB6368D7 +:1033B000DB680BB120469847F30200F1BA80B702F0 +:1033C00026D5D4F8909000274FF0010A09EB471258 +:1033D000D2F8003B03F44023B3F5802F11D1D2F88B +:1033E000003B002B0DDA62890AFA07F322EA030395 +:1033F000638104EB8703DB68DB6813B13946204641 +:1034000098470137D4F89430FFB29B689F42DDD9CA +:10341000F00619D5D4F89000026AC2F30A1702F038 +:103420000F0302F4F012B2F5802F00F0CA80B2F55B +:10343000402F09D104EB8303002200F58050DB68A4 +:103440001B6A974240F0B0803003D5F8185835D544 +:10345000E90303D500212046FFF746FEAA0303D562 +:1034600001212046FFF740FE6B0303D502212046D1 +:10347000FFF73AFE2F0303D503212046FFF734FE62 +:10348000E80203D504212046FFF72EFEA90203D54A +:1034900005212046FFF728FE6A0203D506212046B3 +:1034A000FFF722FE2B0203D507212046FFF71CFE63 +:1034B000EF0103D508212046FFF716FE700340F107 +:1034C000A780E90703D500212046FFF79FFEAA0742 +:1034D00003D501212046FFF799FE6B0703D5022192 +:1034E0002046FFF793FE2F0703D503212046FFF761 +:1034F0008DFEEE0603D504212046FFF787FEA806C1 +:1035000003D505212046FFF781FE690603D5062174 +:103510002046FFF77BFE2A0603D507212046FFF74A +:1035200075FEEB0574D520460821BDE8F84FFFF77E +:103530006DBED4F890904FF0000B4FF0010AD4F814 +:1035400094305FFA8BF79B689F423FF638AF09EBE8 +:103550004713D3F8002902F44022B2F5802F20D17E +:10356000D3F80029002A1CDAD3F8002942F090424F +:10357000C3F80029D3F80029002AFBDB3946D4F828 +:103580009000FFF787FB22890AFA07F322EA030378 +:10359000238104EB8703DB689B6813B1394620461F +:1035A00098470BF1010BCAE7910701D1D0F80080D1 +:1035B000072A02F101029CBF03F8018B4FEA182889 +:1035C0003FE704EB830300F58050DA68D2F818C0B7 +:1035D000DCF80820DCE9001CA1EB0C0C00218F4278 +:1035E00008D1DB689B699A683A449A605A683A4401 +:1035F0005A6029E711F0030F01D1D0F800808C4503 +:1036000001F1010184BF02F8018B4FEA1828E6E7B7 +:10361000BDE8F88F08B50348FFF774FEBDE8084021 +:10362000FFF744BA9442002008B50348FFF76AFE4A +:10363000BDE80840FFF73ABA30430020D0F8903098 +:1036400003EB4111D1F8003B43F40013C1F8003BF8 +:1036500070470000D0F8903003EB4111D1F80039E9 +:1036600043F40013C1F8003970470000D0F89030DF +:1036700003EB4111D1F8003B23F40013C1F8003BE8 +:1036800070470000D0F8903003EB4111D1F80039B9 +:1036900023F40013C1F800397047000030B504333B +:1036A000039C0172002104FB0325C160C0E906539D +:1036B000049B0363059BC0E90000C0E90422C0E944 +:1036C0000842C0E90A11436330BD00000022416A8C +:1036D000C260C0E90411C0E90A226FF00101FEF7DF +:1036E000A5BC0000D0E90432934201D1C2680AB9F6 +:1036F000181D704700207047036919600021C268D7 +:103700000132C260C269134482699342036124BFDB +:10371000436A0361FEF77EBC38B504460D46E36894 +:103720003BB162690020131D1268A3621344E36277 +:1037300007E0237A33B929462046FEF75BFC0028D0 +:10374000EDDA38BD6FF00100FBE70000C368C26925 +:10375000013BC3604369134482699342436124BFC0 +:10376000436A436100238362036B03B118477047C8 +:1037700070B53023044683F31188866A3EB9FFF79B +:10378000CBFF054618B186F31188284670BDA36AA1 +:10379000E26A13F8015B9342A36202D32046FFF76B +:1037A000D5FF002383F31188EFE700002DE9F84FE0 +:1037B00004460E46174698464FF0300989F31188A3 +:1037C0000025AA46D4F828B0BBF1000F09D1414624 +:1037D0002046FFF7A1FF20B18BF311882846BDE8F2 +:1037E000F88FD4E90A12A7EB050B521A934528BFAC +:1037F0009346BBF1400F1BD9334601F1400251F80B +:10380000040B914243F8040BF9D1A36A40364035CA +:103810004033A362D4E90A239A4202D32046FFF739 +:1038200095FF8AF31188BD42D8D289F31188C9E780 +:1038300030465A46FDF708FBA36A5E445D445B448C +:10384000A362E7E710B5029C0433017203FB042175 +:10385000C460C0E906130023C0E90A33039B036375 +:10386000049BC0E90000C0E90422C0E908424363A8 +:1038700010BD0000026A6FF00101C260426AC0E937 +:1038800004220022C0E90A22FEF7D0BBD0E90423BB +:103890009A4201D1C26822B9184650F8043B0B6025 +:1038A000704700231846FAE7C3680021C269013354 +:1038B000C3604369134482699342436124BF436AEE +:1038C0004361FEF7A7BB000038B504460D46E36828 +:1038D0003BB1236900201A1DA262E2691344E3622E +:1038E00007E0237A33B929462046FEF783FB0028F8 +:1038F000EDDA38BD6FF00100FBE7000003691960E5 +:10390000C268013AC260C26913448269934203618A +:1039100024BF436A036100238362036B03B118472A +:103920007047000070B530230D460446114683F3FE +:103930001188866A2EB9FFF7C7FF10B186F3118888 +:1039400070BDA36A1D70A36AE26A01339342A36249 +:1039500004D3E16920460439FFF7D0FF002080F34B +:103960001188EDE72DE9F84F04460D46904699463B +:103970004FF0300A8AF311880026B346A76A4FB980 +:1039800049462046FFF7A0FF20B187F31188304653 +:10399000BDE8F88FD4E90A073A1AA8EB0607974260 +:1039A00028BF1746402F1BD905F1400355F8042BBB +:1039B0009D4240F8042BF9D1A36A40364033A362FC +:1039C000D4E90A239A4204D3E16920460439FFF777 +:1039D00095FF8BF311884645D9D28AF31188CDE73C +:1039E00029463A46FDF730FAA36A3D443E443B443B +:1039F000A362E5E7D0E904239A4217D1C3689BB1DB +:103A0000836A8BB1043B9B1A0ED01360C368013BE1 +:103A1000C360C3691A4483699A42026124BF436A3E +:103A20000361002383620123184670470023FBE7EC +:103A300000F01EBA014B586A704700BF000C0040EE +:103A4000034B002258631A610222DA60704700BFFC +:103A5000000C0040014B0022DA607047000C00406F +:103A6000014B5863704700BF000C0040024B034AF3 +:103A70001A60034A5A607047E443002088440020DB +:103A800000000220074B494210B55C68201A08402C +:103A90001968821A8A4203D3A24201D85A6010BD23 +:103AA0000020FCE7E443002008B5302383F31188AD +:103AB000FFF7E8FF002383F3118808BD04480121C4 +:103AC000044B03600023C0E901330C3000F0D4B88C +:103AD000EC430020A93A0008CB1D083A23F0070365 +:103AE000591A521A012110B4D2080024C0E9004327 +:103AF00084600C301C605A605DF8044B00F0BCB868 +:103B00002DE9F84F364ECD1D0F46002818BF06464A +:103B1000082A4FEAD50538BF082206F10C08341DE3 +:103B20009146404600F0C4F809F10701C9F1000EC2 +:103B3000224624686CB9404600F0C4F83368CBB321 +:103B400008224946E8009847044698B340E90267CE +:103B500030E004EB010CD4F804A00CEA0E0C0AF1DE +:103B60000100ACF1080304EBC0009842E0D9A0EBDF +:103B70000C0CB5EBEC0F4FEAEC0BD9D89C421CD2E5 +:103B800004F10802AB45A3EB02024FEAE2026260D5 +:103B900009D9691CED43206803EBC1025D445560FF +:103BA00043F8310022601C465F60404644F8086BD1 +:103BB00000F088F82046BDE8F88FAA45216802D1B8 +:103BC00011602346EFE7013504EBC50344F83510D7 +:103BD00003F10801401AC01058601360F1E700BFFC +:103BE000EC430020FEE7000070B51B4B00250446A7 +:103BF00086B058600E4685620163FEF72FFF04F120 +:103C00001003A560E562C4E904334FF0FF33C4E953 +:103C10000044C4E90635FFF70DFF2B46024604F1C8 +:103C200034012046C4E9082380230C4A2565FEF7A9 +:103C300097F901230A4AE0600092037568467268AA +:103C40000192B268CDE90223064BCDE90435FEF7B7 +:103C5000AFF906B070BD00BFA826002038470008A5 +:103C60003D470008E53B0008024AD36A1843D0628A +:103C7000704700BF40240020C0E900008160704709 +:103C80008368013B002B10B583600CDA074BDC68BE +:103C90004368A061206063601C6044600520FEF7FB +:103CA000D1F8A06910BD0020FCE700BF402400202F +:103CB00008B5302383F31188FFF7E2FF002383F375 +:103CC000118808BD08B5302383F311888368013358 +:103CD000002B836007DC036800211A6802605060D3 +:103CE0001846FEF7DBF8002383F3118808BD0000B7 +:103CF0004B6843608B688360CB68C3600B6943612A +:103D00004B6903628B6943620B6803607047000074 +:103D100008B53C4B40F2FF713B48D3F888200A437A +:103D2000C3F88820D3F8882022F4FF6222F007022B +:103D3000C3F88820D3F88820D3F8E0200A43C3F8DA +:103D4000E020D3F808210A43C3F808212F4AD3F80A +:103D500008311146FFF7CCFF00F5806002F11C012D +:103D6000FFF7C6FF00F5806002F13801FFF7C0FFE2 +:103D700000F5806002F15401FFF7BAFF00F58060A2 +:103D800002F17001FFF7B4FF00F5806002F18C01D1 +:103D9000FFF7AEFF00F5806002F1A801FFF7A8FF72 +:103DA00000F5806002F1C401FFF7A2FF00F580601A +:103DB00002F1E001FFF79CFF00F5806002F1FC01D9 +:103DC000FFF796FF02F58C7100F58060FFF790FF1A +:103DD00000F01AF90E4BD3F8902242F00102C3F81A +:103DE0009022D3F8942242F00102C3F894220522D3 +:103DF000C3F898204FF06052C3F89C20054AC3F8DE +:103E0000A02008BD004402580000025844470008A2 +:103E100000ED00E01F00080308B500F0C9FAFEF746 +:103E200079F8104BD3F8DC2042F04002C3F8DC20D4 +:103E3000D3F8042122F04002C3F80421D3F804315E +:103E4000094B1A6842F008021A601A6842F004022C +:103E50001A60FEF7DDFDFEF74BFDBDE80840FEF7FA +:103E6000D1BA00BF00440258001802487047000051 +:103E7000114BD3F8E82042F00802C3F8E820D3F849 +:103E8000102142F00802C3F810210C4AD3F8103177 +:103E9000D36B43F00803D363C722094B9A624FF0F8 +:103EA000FF32DA6200229A615A63DA605A600122B4 +:103EB0005A611A60704700BF004402580010005C4D +:103EC000000C0040094A08B51169D3680B40D9B20B +:103ED0009B076FEA0101116107D5302383F3118835 +:103EE000FEF732F8002383F3118808BD000C004070 +:103EF000064BD3F8DC200243C3F8DC20D3F80421BE +:103F00001043C3F80401D3F8043170470044025849 +:103F100008B53C4B4FF0FF31D3F8802062F00042EF +:103F2000C3F88020D3F8802002F00042C3F880203C +:103F3000D3F88020D3F88420C3F88410D3F88420E9 +:103F40000022C3F88420D3F88400D86F40F0FF40EB +:103F500040F4FF0040F4DF4040F07F00D867D86FA6 +:103F600020F0FF4020F4FF0020F4DF4020F07F002D +:103F7000D867D86FD3F888006FEA40506FEA505086 +:103F8000C3F88800D3F88800C0F30A00C3F888009B +:103F9000D3F88800D3F89000C3F89010D3F89000BD +:103FA000C3F89020D3F89000D3F89400C3F894108D +:103FB000D3F89400C3F89420D3F89400D3F8980071 +:103FC000C3F89810D3F89800C3F89820D3F8980055 +:103FD000D3F88C00C3F88C10D3F88C00C3F88C2075 +:103FE000D3F88C00D3F89C00C3F89C10D3F89C1035 +:103FF000C3F89C20D3F89C30FDF79CF9BDE808403D +:1040000000F0AEB90044025808B50122534BC3F882 +:104010000821534BD3F8F42042F00202C3F8F420F5 +:10402000D3F81C2142F00202C3F81C210222D3F86B +:104030001C314C4BDA605A689104FCD54A4A1A602C +:1040400001229A60494ADA6000221A614FF4404224 +:104050009A61444B9A699204FCD51A6842F48072C2 +:104060001A603F4B1A6F12F4407F04D04FF4803235 +:104070001A6700221A671A6842F001021A60384B68 +:104080001A685007FCD500221A611A6912F038022A +:10409000FBD1012119604FF0804159605A67344AC1 +:1040A000DA62344A1A611A6842F480321A602C4B80 +:1040B0001A689103FCD51A6842F480521A601A6893 +:1040C0009204FCD52C4A2D499A6200225A63196346 +:1040D00001F57C01DA6301F2E71199635A64284A19 +:1040E0001A64284ADA621A6842F0A8521A601C4B15 +:1040F0001A6802F02852B2F1285FF9D148229A6179 +:104100004FF48862DA6140221A621F4ADA641F4A59 +:104110001A651F4A5A651F4A9A6532231E4A136060 +:10412000136803F00F03022BFAD10D4A136943F011 +:1041300003031361136903F03803182BFAD14FF00E +:104140000050FFF7D5FE4FF08040FFF7D1FE4FF053 +:104150000040BDE80840FFF7CBBE00BF0080005123 +:10416000004402580048025800C000F0020000015C +:104170000000FF010088900812102000630209016E +:10418000470E0508DD0BBF012000002000000110D4 +:104190000910E00000010110002000524FF0B04271 +:1041A00008B5D2F8883003F00103C2F8883023B193 +:1041B000044A13680BB150689847BDE80840FEF701 +:1041C00075BC00BF044400204FF0B04208B5D2F8DF +:1041D000883003F00203C2F8883023B1044A9368A0 +:1041E0000BB1D0689847BDE80840FEF75FBC00BF40 +:1041F000044400204FF0B04208B5D2F8883003F0F4 +:104200000403C2F8883023B1044A13690BB1506922 +:104210009847BDE80840FEF749BC00BF04440020B1 +:104220004FF0B04208B5D2F8883003F00803C2F866 +:10423000883023B1044A93690BB1D0699847BDE82F +:104240000840FEF733BC00BF044400204FF0B042EA +:1042500008B5D2F8883003F01003C2F8883023B1D3 +:10426000044A136A0BB1506A9847BDE80840FEF74C +:104270001DBC00BF044400204FF0B04310B5D3F87C +:10428000884004F47872C3F88820A30604D5124A43 +:10429000936A0BB1D06A9847600604D50E4A136B37 +:1042A0000BB1506B9847210604D50B4A936B0BB1A9 +:1042B000D06B9847E20504D5074A136C0BB1506CDC +:1042C0009847A30504D5044A936C0BB1D06C98476A +:1042D000BDE81040FEF7EABB044400204FF0B043B5 +:1042E00010B5D3F8884004F47C42C3F888206205F6 +:1042F00004D5164A136D0BB1506D9847230504D5AC +:10430000124A936D0BB1D06D9847E00404D50F4A63 +:10431000136E0BB1506E9847A10404D50B4A936EEF +:104320000BB1D06E9847620404D5084A136F0BB1E5 +:10433000506F9847230404D5044A936F0BB1D06F94 +:104340009847BDE81040FEF7B1BB00BF0444002011 +:1043500008B5FFF7B7FDBDE80840FEF7A7BB0000B2 +:10436000062108B50846FDF7C1F806210720FDF72C +:10437000BDF806210820FDF7B9F806210920FDF750 +:10438000B5F806210A20FDF7B1F806211720FDF740 +:10439000ADF806212820FDF7A9F809217A20FDF7BC +:1043A000A5F807213220BDE80840FDF79FB80000BE +:1043B00008B5FFF7ADFD00F00BF8FDF769FAFDF762 +:1043C0003BF9FFF753FDBDE80840FFF731BB0000A4 +:1043D0000023054A19460133102BC2E9001102F1EE +:1043E0000802F8D1704700BF0444002010B501391D +:1043F0000244904201D1002005E0037811F8014FFA +:10440000A34201D0181B10BD0130F2E7034611F89A +:10441000012B03F8012B002AF9D1704753544D3377 +:104420003248373F3F3F0053544D3332483733789B +:104430002F3732780053544D3332483734332F37C7 +:1044400035332F373530000001105A000310590062 +:1044500001205800032056001000024008000240CE +:104460000008024000000B00280002400800024043 +:104470000408024006010C0040000240080002400F +:104480000808024010020D005800024008000240D7 +:104490000C08024016030E00700002400C0002409F +:1044A0001008024000040F00880002400C00024087 +:1044B0001408024006051000A00002400C00024053 +:1044C0001808024010061100B80002400C0002401B +:1044D0001C08024016072F00100402400804024086 +:1044E0002008024000083800280402400804024066 +:1044F0002408024006093900400402400804024032 +:1045000028080240100A3A005804024008040240F9 +:104510002C080240160B3B00700402400C040240C1 +:1045200030080240000C3C00880402400C040240A9 +:1045300034080240060D4400A00402400C0402406E +:1045400038080240100E4500B80402400C04024036 +:104550003C080240160F460000000000B515000898 +:10456000A1150008DD150008C9150008D5150008BB +:10457000C1150008AD15000899150008E9150008D7 +:104580000000000001000000000000006330000097 +:104590008C45000898240020A826002041726475EC +:1045A00050696C6F740025424F415244252D424C96 +:1045B000002553455249414C2500000002000000EF +:1045C00000000000D5170008451800084000400012 +:1045D000583F0020683F002002000000000000005B +:1045E00003000000000000008D180008000000001B +:1045F00010000000783F00200000000001000000D3 +:10460000000000009442002001010200A1230008E4 +:10461000B12200084D2300083123000843000000A8 +:104620002446000809024300020100C032090400C8 +:10463000000102020100052400100105240100010F +:10464000042402020524060001070582030800FF76 +:1046500009040100020A00000007050102400000F1 +:1046600007058102400000001200000070460008AB +:10467000120110010200004009124157000201021C +:10468000030100000403090425424F415244250060 +:1046900047455052435F54414B45525F483734338E +:1046A00000303132333435363738394142434445AE +:1046B0004600000000000020000002000200000090 +:1046C000000000300000040008000000000000248A +:1046D00000000800040000000004000000FC0000CE +:1046E000020000000000043000800000080000000C +:1046F0000000003800000100010000000000000080 +:10470000E9190008A11C00084D1D000840004000E8 +:10471000CC430020CC43002001000000DC430020FB +:1047200080000000400100000800000000010000BF +:1047300000100000080000006D61696E0069646C83 +:10474000650000000001806A00000000AAAAAAAA71 +:1047500000010064FFFF00000000000000A00A004C +:104760000505000100000000AAAAAAAA0000000195 +:10477000CCFF000000000000000000000000050465 +:1047800000000000AAAAAAAA00000000FFDC0000A6 +:104790000000000000000000100000050000000004 +:1047A000AAAAAAAA20000000FBCF00000000000077 +:1047B000000000000001000000000000AAAAAAAA50 +:1047C00000010000FFFF00000000000000000000EA +:1047D0000000000000000000AAAAAAAA0000000031 +:1047E000FFFF0000000000000000000000000000CB +:1047F00000000000AAAAAAAA00000000FFFF000013 +:1048000000000000000000000000000000000000A8 +:10481000AAAAAAAA00000000FFFF000000000000F2 +:10482000000000000000000000000000AAAAAAAAE0 +:1048300000000000FFFF000000000000000000007A +:104840000000000000000000AAAAAAAA00000000C0 +:10485000FFFF00000000000000000000000000005A +:1048600000000000AAAAAAAA00000000FFFF0000A2 +:104870000000000000000000DE0500000000000055 +:1048800000001A0000000000FF000000000000000F +:104890001C44000883040000274400085004000062 +:1048A0003544000800960000000008009600000053 +:1048B000000800000400000084460008000000001A +:1048C00000000000000000000000000000000000E8 +:0448D00000000000E4 +:00000001FF diff --git a/Tools/environment_install/install-prereqs-alpine.sh b/Tools/environment_install/install-prereqs-alpine.sh index c01da408db3ef6..5f1dd9af6e6022 100755 --- a/Tools/environment_install/install-prereqs-alpine.sh +++ b/Tools/environment_install/install-prereqs-alpine.sh @@ -17,7 +17,6 @@ apk update && apk add --no-cache \ libxml2-dev \ libxslt-dev \ git \ - && ln -sf python3 /usr/bin/python \ && rm -rf /var/cache/apk/* python3 -m pip install --user --no-deps --no-cache-dir empy==3.3.4 pexpect ptyprocess --break-system-packages diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 5c205577ba73a6..943af058f7296f 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -120,6 +120,11 @@ elif [ ${RELEASE_CODENAME} == 'noble' ]; then SITLCFML_VERSION="2.6" PYTHON_V="python3" PIP=pip3 +elif [ ${RELEASE_CODENAME} == 'oracular' ]; then + SITLFML_VERSION="2.6" + SITLCFML_VERSION="2.6" + PYTHON_V="python3" + PIP=pip3 elif [ ${RELEASE_CODENAME} == 'groovy' ] || [ ${RELEASE_CODENAME} == 'bullseye' ]; then SITLFML_VERSION="2.5" @@ -176,7 +181,8 @@ ARM_LINUX_PKGS="g++-arm-linux-gnueabihf $INSTALL_PKG_CONFIG" if [ ${RELEASE_CODENAME} == 'bookworm' ] || [ ${RELEASE_CODENAME} == 'lunar' ] || [ ${RELEASE_CODENAME} == 'mantic' ] || - [ ${RELEASE_CODENAME} == 'noble' ]; then + [ ${RELEASE_CODENAME} == 'noble' ] || + [ ${RELEASE_CODENAME} == 'oracular' ]; then # on Lunar (and presumably later releases), we install in venv, below PYTHON_PKGS+=" numpy pyparsing psutil" SITL_PKGS="python3-dev" @@ -189,7 +195,8 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then if [ ${RELEASE_CODENAME} == 'bookworm' ] || [ ${RELEASE_CODENAME} == 'lunar' ] || [ ${RELEASE_CODENAME} == 'mantic' ] || - [ ${RELEASE_CODENAME} == 'noble' ]; then + [ ${RELEASE_CODENAME} == 'noble' ] || + [ ${RELEASE_CODENAME} == 'oracular' ]; then PYTHON_PKGS+=" matplotlib scipy opencv-python pyyaml" SITL_PKGS+=" xterm libcsfml-dev libcsfml-audio${SITLCFML_VERSION} libcsfml-dev libcsfml-graphics${SITLCFML_VERSION} libcsfml-network${SITLCFML_VERSION} libcsfml-system${SITLCFML_VERSION} libcsfml-window${SITLCFML_VERSION} libsfml-audio${SITLFML_VERSION} libsfml-dev libsfml-graphics${SITLFML_VERSION} libsfml-network${SITLFML_VERSION} libsfml-system${SITLFML_VERSION} libsfml-window${SITLFML_VERSION}" else @@ -283,8 +290,9 @@ elif [ ${RELEASE_CODENAME} == 'lunar' ]; then elif [ ${RELEASE_CODENAME} == 'buster' ]; then SITL_PKGS+=" libpython3-stdlib" # for argparse elif [ ${RELEASE_CODENAME} != 'mantic' ] && - [ ${RELEASE_CODENAME} != 'noble' ]; then - SITL_PKGS+=" python-argparse" + [ ${RELEASE_CODENAME} != 'noble' ] && + [ ${RELEASE_CODENAME} != 'oracular' ]; then + SITL_PKGS+=" python-argparse" fi # Check for graphical package for MAVProxy @@ -305,6 +313,9 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then elif [ ${RELEASE_CODENAME} == 'noble' ]; then SITL_PKGS+=" libgtk-3-dev libwxgtk3.2-dev " # see below + elif [ ${RELEASE_CODENAME} == 'oracular' ]; then + SITL_PKGS+=" libgtk-3-dev libwxgtk3.2-dev " + # see below elif apt-cache search python-wxgtk3.0 | grep wx; then SITL_PKGS+=" python-wxgtk3.0" elif apt-cache search python3-wxgtk4.0 | grep wx; then @@ -325,7 +336,8 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then SITL_PKGS+=" python3-wxgtk4.0" SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev" # for pygame elif [ ${RELEASE_CODENAME} == 'mantic' ] || - [ ${RELEASE_CODENAME} == 'noble' ]; then + [ ${RELEASE_CODENAME} == 'noble' ] || + [ ${RELEASE_CODENAME} == 'oracular' ]; then PYTHON_PKGS+=" wxpython opencv-python" SITL_PKGS+=" python3-wxgtk4.0" SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev" # for pygame @@ -380,6 +392,8 @@ if [ ${RELEASE_CODENAME} == 'bookworm' ] || PYTHON_VENV_PACKAGE=python3.11-venv elif [ ${RELEASE_CODENAME} == 'noble' ]; then PYTHON_VENV_PACKAGE=python3.12-venv +elif [ ${RELEASE_CODENAME} == 'oracular' ]; then + PYTHON_VENV_PACKAGE=python3.12-venv fi if [ -n "$PYTHON_VENV_PACKAGE" ]; then @@ -416,7 +430,8 @@ fi if [ ${RELEASE_CODENAME} == 'bookworm' ] || [ ${RELEASE_CODENAME} == 'lunar' ] || [ ${RELEASE_CODENAME} == 'mantic' ] || - [ ${RELEASE_CODENAME} == 'noble' ]; then + [ ${RELEASE_CODENAME} == 'noble' ] || + [ ${RELEASE_CODENAME} == 'oracular' ]; then # must do this ahead of wxPython pip3 run :-/ $PIP install $PIP_USER_ARGUMENT -U attrdict3 fi diff --git a/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 b/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 index 5cc7735a5b0f24..18d57af4748801 100644 --- a/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 +++ b/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 @@ -15,8 +15,6 @@ Start-BitsTransfer -Source "https://firmware.ardupilot.org/Tools/STM32-tools/gcc Write-Output "Installing Cygwin x64 (4/8)" Start-Process -wait -FilePath $PSScriptRoot\setup-x86_64.exe -ArgumentList "--root=C:\cygwin64 --no-startmenu --local-package-dir=$env:USERPROFILE\Downloads --site=http://cygwin.mirror.constant.com --packages autoconf,automake,ccache,cygwin32-gcc-g++,gcc-g++=7.4.0-1,libgcc1=7.4.0.1,gcc-core=7.4.0-1,git,libtool,make,gawk,libexpat-devel,libxml2-devel,python37,python37-future,python37-lxml,python37-pip,libxslt-devel,python37-devel,procps-ng,zip,gdb,ddd --quiet-mode" -Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/python3.7 /usr/bin/python'" -Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/pip3.7 /usr/bin/pip'" Write-Output "Downloading extra Python packages (5/8)" Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'python3.7 -m pip install empy==3.3.4 pyserial pymavlink intelhex dronecan pexpect'" diff --git a/Tools/environment_install/install-prereqs-windows.ps1 b/Tools/environment_install/install-prereqs-windows.ps1 index 543f1370c9d0be..fef9fa1845ded6 100644 --- a/Tools/environment_install/install-prereqs-windows.ps1 +++ b/Tools/environment_install/install-prereqs-windows.ps1 @@ -15,8 +15,6 @@ Start-BitsTransfer -Source "https://firmware.ardupilot.org/Tools/STM32-tools/gcc Write-Output "Installing Cygwin x64 (4/7)" Start-Process -wait -FilePath $PSScriptRoot\setup-x86_64.exe -ArgumentList "--root=C:\cygwin64 --no-startmenu --local-package-dir=$env:USERPROFILE\Downloads --site=http://cygwin.mirror.constant.com --packages autoconf,automake,ccache,cygwin32-gcc-g++,gcc-g++=7.4.0-1,libgcc1=7.4.0.1,gcc-core=7.4.0-1,git,libtool,make,gawk,libexpat-devel,libxml2-devel,python37,python37-future,python37-lxml,python37-pip,libxslt-devel,python37-devel,procps-ng,zip,gdb,ddd,xterm --quiet-mode" -Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/python3.7 /usr/bin/python'" -Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/pip3.7 /usr/bin/pip'" Write-Output "Downloading extra Python packages (5/7)" Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'python3.7 -m pip install empy==3.3.4 pyserial pymavlink intelhex dronecan pexpect'" diff --git a/Tools/scripts/CAN/CAN_playback.py b/Tools/scripts/CAN/CAN_playback.py index bdd4825f4fa221..b3b8cc0ecc66f8 100755 --- a/Tools/scripts/CAN/CAN_playback.py +++ b/Tools/scripts/CAN/CAN_playback.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 ''' -playback a set of CAN frames from libraries/AP_Scripting/examples/CAN_logger.lua onto a CAN bus + playback a set of CAN frames + capture frames either using libraries/AP_Scripting/examples/CAN_logger.lua + or the CAN_Pn_OPTIONS bit to enable CAN logging ''' import dronecan @@ -9,6 +11,7 @@ import threading from pymavlink import mavutil from dronecan.driver.common import CANFrame +import struct # get command line arguments @@ -16,6 +19,7 @@ parser = ArgumentParser(description='CAN playback') parser.add_argument("logfile", default=None, type=str, help="logfile") parser.add_argument("canport", default=None, type=str, help="CAN port") +parser.add_argument("--bus", default=0, type=int, help="CAN bus") args = parser.parse_args() @@ -28,8 +32,27 @@ tstart = time.time() first_tstamp = None +def dlc_to_datalength(dlc): + # Data Length Code 9 10 11 12 13 14 15 + # Number of data bytes 12 16 20 24 32 48 64 + if (dlc <= 8): + return dlc + elif (dlc == 9): + return 12 + elif (dlc == 10): + return 16 + elif (dlc == 11): + return 20 + elif (dlc == 12): + return 24 + elif (dlc == 13): + return 32 + elif (dlc == 14): + return 48 + return 64 + while True: - m = mlog.recv_match(type='CANF') + m = mlog.recv_match(type=['CANF','CAFD']) if m is None: print("Rewinding") @@ -38,15 +61,25 @@ first_tstamp = None continue + if getattr(m,'bus',0) != args.bus: + continue + if first_tstamp is None: first_tstamp = m.TimeUS dt = time.time() - tstart dt2 = (m.TimeUS - first_tstamp)*1.0e-6 if dt2 > dt: time.sleep(dt2 - dt) - data = [m.B0, m.B1, m.B2, m.B3, m.B4, m.B5, m.B6, m.B7] - data = data[:m.DLC] + + canfd = m.get_type() == 'CAFD' + if canfd: + data = struct.pack(".*)::update',), @@ -284,6 +284,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED', r'GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands'), ('AP_SERIALMANAGER_REGISTER_ENABLED', r'AP_SerialManager::register_port'), ('AP_QUICKTUNE_ENABLED', r'AP_Quicktune::update'), + ('AP_FILTER_ENABLED', r'AP_Filters::update'), + ('AP_CAN_LOGGING_ENABLED', r'AP_CANManager::can_logging_callback'), ] def progress(self, msg): diff --git a/Tools/scripts/generate_manifest.py b/Tools/scripts/generate_manifest.py index 722c24b551ee60..86c76ee400da42 100755 --- a/Tools/scripts/generate_manifest.py +++ b/Tools/scripts/generate_manifest.py @@ -107,6 +107,7 @@ "MicoAir405v2" : ("MicoAir F405 v2.1", "MicoAir"), "MicoAir405Mini" : ("MicoAir F405 Mini", "MicoAir"), "GEPRCF745BTHD": ("TAKER F745 BT","GEPRC"), + "GEPRC_TAKER_H743": ("TAKER H743 BT","GEPRC"), } class Firmware(): diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index c50c7581e866df..5b768c99f9d931 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1208,6 +1208,28 @@ bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &v accel_offset_NED.z = -_accel_offset_target.z * 0.01; return true; } + +// get target velocity in m/s in NED frame +bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED) +{ + if (!is_active_xy() || !is_active_z()) { + return false; + } + vel_target_NED.xy() = _vel_target.xy() * 0.01; + vel_target_NED.z = -_vel_target.z * 0.01; + return true; +} + +// get target acceleration in m/s/s in NED frame +bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED) +{ + if (!is_active_xy() || !is_active_z()) { + return false; + } + accel_target_NED.xy() = _accel_target.xy() * 0.01; + accel_target_NED.z = -_accel_target.z * 0.01; + return true; +} #endif /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index f52c7a9c5c9ea7..ff5de984985978 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -339,6 +339,12 @@ class AC_PosControl // units are m, m/s and m/s/s in NED frame bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED); bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED); + + // get target velocity in m/s in NED frame + bool get_vel_target(Vector3f &vel_target_NED); + + // get target acceleration in m/s/s in NED frame + bool get_accel_target(Vector3f &accel_target_NED); #endif /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 5e7aa6c5d98367..2f86b6d8291e45 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -644,7 +644,7 @@ void AC_AutoTune_Multi::twitching_test_angle(float angle, float rate, float angl } // twitching_measure_acceleration - measure rate of change of measurement -void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const +void AC_AutoTune_Multi::twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const { if (rate_max < rate) { rate_max = rate; diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index e89dd4c5893c5c..6987c1c252c1d8 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -156,7 +156,7 @@ class AC_AutoTune_Multi : public AC_AutoTune void twitching_test_angle(float angle, float rate, float angle_target, float &meas_angle_min, float &meas_angle_max, float &meas_rate_min, float &meas_rate_max); // measure acceleration during twitch test - void twitching_measure_acceleration(float &accel_average, float rate, float rate_max) const; + void twitching_measure_acceleration(float &accel_average, float rate, float &rate_max) const; // updating_rate_d_up - increase D and adjust P to optimize the D term for a little bounce back // optimize D term while keeping the maximum just below the target by adjusting P diff --git a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp index 54859110fb833a..b8b45a8e6f5a9e 100644 --- a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp +++ b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp @@ -6,6 +6,7 @@ #include "AP_OADijkstra.h" #include "AP_OABendyRuler.h" #include +#include #if AP_OAPATHPLANNER_BENDYRULER_ENABLED void AP_OABendyRuler::Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index db214b2e494e17..d6255bd44912b2 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -219,7 +219,6 @@ void AC_Fence::update() // if someone changes the parameter we want to enable or disable everything if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) { // reset the auto mask since we just reconfigured all of fencing - _auto_enable_mask = AC_FENCE_ALL_FENCES; _last_enabled = _enabled; _last_auto_enabled = _auto_enabled; if (_enabled) { @@ -238,9 +237,9 @@ void AC_Fence::update() } // enable or disable configured fences present in fence_types -// also updates the bitmask of auto enabled fences if update_auto_mask is true +// also updates the _min_alt_state enum if update_auto_enable is true // returns a bitmask of fences that were changed -uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) +uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable) { uint8_t fences = _configured_fences.get() & fence_types; uint8_t enabled_fences = _enabled_fences; @@ -250,9 +249,9 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) enabled_fences &= ~fences; } - // fences that were manually changed are no longer eligible for auto-enablement or disablement - if (update_auto_mask) { - _auto_enable_mask &= ~fences; + if (update_auto_enable && (fences & AC_FENCE_TYPE_ALT_MIN) != 0) { + // remember that min-alt fence was manually enabled/disabled + _min_alt_state = value ? MinAltState::MANUALLY_ENABLED : MinAltState::MANUALLY_DISABLED; } uint8_t fences_to_change = _enabled_fences ^ enabled_fences; @@ -260,6 +259,7 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) if (!fences_to_change) { return 0; } + #if HAL_LOGGING_ENABLED AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE); if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) { @@ -305,7 +305,11 @@ void AC_Fence::auto_enable_fence_on_arming(void) return; } - const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false); + // reset min alt state, after an auto-enable the min alt fence can be auto-enabled on + // reaching altitude + _min_alt_state = MinAltState::DEFAULT; + + const uint8_t fences = enable(true, AC_FENCE_ARMING_FENCES, false); print_fence_message("auto-enabled", fences); } @@ -318,7 +322,7 @@ void AC_Fence::auto_disable_fence_on_disarming(void) return; } - const uint8_t fences = enable(false, _auto_enable_mask, false); + const uint8_t fences = enable(false, AC_FENCE_ALL_FENCES, false); print_fence_message("auto-disabled", fences); } @@ -332,7 +336,10 @@ void AC_Fence::auto_enable_fence_after_takeoff(void) return; } - const uint8_t fences = enable(true, _auto_enable_mask, false); + // reset min-alt state + _min_alt_state = MinAltState::DEFAULT; + + const uint8_t fences = enable(true, AC_FENCE_ALL_FENCES, false); print_fence_message("auto-enabled", fences); } @@ -342,14 +349,18 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const uint8_t auto_disable = 0; switch (auto_enabled()) { case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: - auto_disable = _auto_enable_mask; + auto_disable = AC_FENCE_ALL_FENCES; break; case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: default: // when auto disable is not set we still need to disable the altmin fence on landing - auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN; + auto_disable = AC_FENCE_TYPE_ALT_MIN; break; } + if (_min_alt_state == MinAltState::MANUALLY_ENABLED) { + // don't auto-disable min alt fence if manually enabled + auto_disable &= ~AC_FENCE_TYPE_ALT_MIN; + } return auto_disable; } @@ -469,8 +480,20 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c return false; } + auto breached_fences = _breached_fences; + if (auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { + Location loc; + if (!AP::ahrs().get_location(loc)) { + hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position"); + return false; + } + if (_poly_loader.breached(loc)) { + breached_fences |= AC_FENCE_TYPE_POLYGON; + } + } + // check no limits are currently breached - if (_breached_fences) { + if (breached_fences) { char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1); AC_Fence::get_fence_names(_breached_fences, e); @@ -511,7 +534,7 @@ bool AC_Fence::check_fence_alt_max() float alt; AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up + const float _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence if (_curr_alt >= _alt_max) { @@ -560,7 +583,7 @@ bool AC_Fence::check_fence_alt_min() float alt; AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up + const float _curr_alt = -alt; // translate Down to Up // check if we are under the altitude fence if (_curr_alt <= _alt_min) { @@ -603,7 +626,7 @@ bool AC_Fence::auto_enable_fence_floor() // altitude fence check if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled - || !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) // has been manually disabled + || _min_alt_state == MinAltState::MANUALLY_DISABLED // user has manually disabled the fence || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) { // not enabled @@ -612,7 +635,7 @@ bool AC_Fence::auto_enable_fence_floor() float alt; AP::ahrs().get_relative_position_D_home(alt); - _curr_alt = -alt; // translate Down to Up + const float _curr_alt = -alt; // translate Down to Up // check if we are over the altitude fence if (!floor_enabled() && _curr_alt >= _alt_min + _margin) { @@ -709,11 +732,36 @@ uint8_t AC_Fence::check(bool disable_auto_fences) // clear any breach from disabled fences clear_breach(fences_to_disable); + if (_min_alt_state == MinAltState::MANUALLY_ENABLED) { + // if user has manually enabled the min-alt fence then don't auto-disable + fences_to_disable &= ~AC_FENCE_TYPE_ALT_MIN; + } + // report on any fences that were auto-disabled if (fences_to_disable) { print_fence_message("auto-disabled", fences_to_disable); } +#if 0 + /* + this debug log message is very useful both when developing tests + and doing manual SITL fence testing + */ + { + float alt; + AP::ahrs().get_relative_position_D_home(alt); + + AP::logger().WriteStreaming("FENC", "TimeUS,EN,AE,CF,EF,DF,Alt", "QIIIIIf", + AP_HAL::micros64(), + enabled(), + _auto_enabled, + _configured_fences, + get_enabled_fences(), + disabled_fences, + alt*-1); + } +#endif + // return immediately if disabled if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { return 0; diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 3165d7b73d4d33..c4a821f6e2aa40 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/libraries/AC_Fence/AC_Fence.h @@ -249,10 +249,7 @@ class AC_Fence float _circle_breach_distance; // distance beyond the circular fence // other internal variables - uint8_t _auto_enable_mask = AC_FENCE_ALL_FENCES; // fences that can be auto-enabled or auto-disabled float _home_distance; // distance from home in meters (provided by main code) - float _curr_alt; - // breach information uint8_t _breached_fences; // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE) @@ -262,6 +259,13 @@ class AC_Fence uint32_t _manual_recovery_start_ms; // system time in milliseconds that pilot re-took manual control + enum class MinAltState + { + DEFAULT = 0, + MANUALLY_ENABLED, + MANUALLY_DISABLED + } _min_alt_state; + AC_PolyFence_loader _poly_loader{_total, _options}; // polygon fence }; diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index dc0cb16b2ec8dc..4d8974c9246c49 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -172,7 +172,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { // @Param: OPTIONS // @DisplayName: ADS-B Options // @Description: Options for emergency failsafe codes and device capabilities - // @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe,3:Sagetech MXS use External Config + // @Bitmask: 0:Ping200X Send GPS,1:Squawk 7400 on RC failsafe,2:Squawk 7400 on GCS failsafe,3:Sagetech MXS use External Config,4:Transmit in traditional Mode 3A/C only and inhibit Mode-S and ES (ADSB) transmissions // @User: Advanced AP_GROUPINFO("OPTIONS", 15, AP_ADSB, _options, 0), @@ -697,7 +697,6 @@ void AP_ADSB::handle_out_control(const mavlink_uavionix_adsb_out_control_t &pack { out_state.ctrl.baroCrossChecked = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_EXTERNAL_BARO_CROSSCHECKED; out_state.ctrl.airGroundState = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_ON_GROUND; - out_state.ctrl.identActive = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_IDENT_BUTTON_ACTIVE; out_state.ctrl.modeAEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_A_ENABLED; out_state.ctrl.modeCEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_C_ENABLED; out_state.ctrl.modeSEnabled = packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_MODE_S_ENABLED; @@ -707,6 +706,10 @@ void AP_ADSB::handle_out_control(const mavlink_uavionix_adsb_out_control_t &pack out_state.ctrl.emergencyState = packet.emergencyStatus; memcpy(out_state.ctrl.callsign, packet.flight_id, sizeof(out_state.ctrl.callsign)); out_state.ctrl.x_bit = packet.x_bit; + + if (packet.state & UAVIONIX_ADSB_OUT_CONTROL_STATE::UAVIONIX_ADSB_OUT_CONTROL_STATE_IDENT_BUTTON_ACTIVE) { + IGNORE_RETURN(ident_start()); + } } /* @@ -930,6 +933,18 @@ uint32_t AP_ADSB::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNu return outputNumber; } +// Trigger a Mode 3/A transponder IDENT. This should only be done when requested to do so by an Air Traffic Controller. +// See wikipedia for IDENT explanation https://en.wikipedia.org/wiki/Transponder_(aeronautics) +bool AP_ADSB::ident_start() +{ + if (!healthy()) { + return false; + } + out_state.ctrl.identActive = true; + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"ADSB: IDENT!"); + return true; +} + // methods for embedded class Location bool AP_ADSB::Loc::speed_accuracy(float &sacc) const { @@ -958,6 +973,26 @@ bool AP_ADSB::Loc::vertical_accuracy(float &vacc) const return true; } +uint8_t AP_ADSB::convert_maxknots_to_enum(const float maxAircraftSpeed_knots) +{ + if (maxAircraftSpeed_knots <= 0) { + // not set or unknown. no bits set + return 0; + } else if (maxAircraftSpeed_knots <= 75) { + return 1; + } else if (maxAircraftSpeed_knots <= 150) { + return 2; + } else if (maxAircraftSpeed_knots <= 300) { + return 3; + } else if (maxAircraftSpeed_knots <= 600) { + return 4; + } else if (maxAircraftSpeed_knots <= 1200) { + return 5; + } else { + return 6; + } +} + AP_ADSB *AP::ADSB() { return AP_ADSB::get_singleton(); diff --git a/libraries/AP_ADSB/AP_ADSB.h b/libraries/AP_ADSB/AP_ADSB.h index 0ebf1b1ce413cc..a66b57202c1968 100644 --- a/libraries/AP_ADSB/AP_ADSB.h +++ b/libraries/AP_ADSB/AP_ADSB.h @@ -78,6 +78,7 @@ class AP_ADSB { Squawk_7400_FS_RC = (1<<1), Squawk_7400_FS_GCS = (1<<2), SagteTech_MXS_External_Config = (1<<3), + Mode3_Only = (1<<4), }; // for holding parameters @@ -207,6 +208,8 @@ class AP_ADSB { // confirm a value is a valid callsign static bool is_valid_callsign(uint16_t octal) WARN_IF_UNUSED; + static uint8_t convert_maxknots_to_enum(const float maxAircraftSpeed_knots); + // Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value // stored on a GCS as a string field in different format, but then transmitted // over mavlink as a float which is always a decimal. @@ -214,13 +217,7 @@ class AP_ADSB { // Trigger a Mode 3/A transponder IDENT. This should only be done when requested to do so by an Air Traffic Controller. // See wikipedia for IDENT explanation https://en.wikipedia.org/wiki/Transponder_(aeronautics) - bool ident_start() { - if (!healthy() || ((out_state.cfg.rfSelect & UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED) == 0)) { - return false; - } - out_state.ctrl.identActive = true; - return true; - } + bool ident_start(); AP_ADSB::Type get_type(uint8_t instance) const; diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp index 67d3e73e25d8ce..ec4e49b7d4c7b7 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp @@ -120,7 +120,7 @@ void AP_ADSB_Sagetech_MXS::update() (mxs_state.inst.icao != (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get() || mxs_state.inst.emitter != convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get()) || mxs_state.inst.size != _frontend.out_state.cfg.lengthWidth.get() || - mxs_state.inst.maxSpeed != convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots) + mxs_state.inst.maxSpeed != (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots) )) { last.packet_initialize_ms = now_ms; send_install_msg(); @@ -150,7 +150,7 @@ void AP_ADSB_Sagetech_MXS::update() last.operating_rf_select = _frontend.out_state.cfg.rfSelect; last.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled; last.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled; - last.modeSEnabled = _frontend.out_state.ctrl.modeSEnabled; + last.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled; last.operating_alt = _frontend._my_loc.alt; last.packet_Operating_ms = now_ms; @@ -358,7 +358,7 @@ void AP_ADSB_Sagetech_MXS::auto_config_installation() mxs_state.inst.sda = sg_sda_t::sdaUnknown; mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get()); mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get(); - mxs_state.inst.maxSpeed = convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots); + mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots); mxs_state.inst.altOffset = 0; // Alt encoder offset is legacy field that should always be 0. mxs_state.inst.antenna = sg_antenna_t::antBottom; @@ -511,7 +511,7 @@ void AP_ADSB_Sagetech_MXS::send_install_msg() mxs_state.inst.icao = (uint32_t)_frontend.out_state.cfg.ICAO_id_param.get(); mxs_state.inst.emitter = convert_emitter_type_to_sg(_frontend.out_state.cfg.emitterType.get()); mxs_state.inst.size = (sg_size_t)_frontend.out_state.cfg.lengthWidth.get(); - mxs_state.inst.maxSpeed = convert_airspeed_knots_to_sg(_frontend.out_state.cfg.maxAircraftSpeed_knots); + mxs_state.inst.maxSpeed = (sg_airspeed_t)AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots); mxs_state.inst.antenna = sg_antenna_t::antBottom; last.msg.type = SG_MSG_TYPE_HOST_INSTALL; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp index bb510a738aa1d2..1a574ed23b2ebb 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_MAVLink.cpp @@ -177,22 +177,7 @@ uint8_t AP_ADSB_uAvionix_MAVLink::get_encoded_callsign_null_char() uint8_t encoded_null = 0; - if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 0) { - // not set or unknown. no bits set - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 75) { - encoded_null |= 0x01; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 150) { - encoded_null |= 0x02; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 300) { - encoded_null |= 0x03; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 600) { - encoded_null |= 0x04; - } else if (_frontend.out_state.cfg.maxAircraftSpeed_knots <= 1200) { - encoded_null |= 0x05; - } else { - encoded_null |= 0x06; - } - + encoded_null = AP_ADSB::convert_maxknots_to_enum(_frontend.out_state.cfg.maxAircraftSpeed_knots); if (_frontend.out_state.cfg.rf_capable & ADSB_BITBASK_RF_CAPABILITIES_1090ES_IN) { encoded_null |= 0x10; diff --git a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp index 0f92293c519f24..cb9f6cfc45aa7e 100644 --- a/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp +++ b/libraries/AP_ADSB/AP_ADSB_uAvionix_UCP.cpp @@ -411,8 +411,8 @@ void AP_ADSB_uAvionix_UCP::send_Transponder_Control() _frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request msg.modeAEnabled = _frontend.out_state.ctrl.modeAEnabled; msg.modeCEnabled = _frontend.out_state.ctrl.modeCEnabled; - msg.modeSEnabled = _frontend.out_state.ctrl.modeSEnabled; - msg.es1090TxEnabled = _frontend.out_state.ctrl.es1090TxEnabled; + msg.modeSEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.modeSEnabled; + msg.es1090TxEnabled = (_frontend._options & uint32_t(AP_ADSB::AdsbOption::Mode3_Only)) ? 0 : _frontend.out_state.ctrl.es1090TxEnabled; // if enabled via param ADSB_OPTIONS, use squawk 7400 while in any Loss-Comms related failsafe // https://www.faa.gov/documentLibrary/media/Notice/N_JO_7110.724_5-2-9_UAS_Lost_Link_2.pdf diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 945ad67d414b6e..e8fa5cd623a61c 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -346,9 +346,9 @@ void AP_AHRS::update_state(void) state.primary_core = _get_primary_core_index(); state.wind_estimate_ok = _wind_estimate(state.wind_estimate); state.EAS2TAS = AP_AHRS_Backend::get_EAS2TAS(); - state.airspeed_ok = _airspeed_estimate(state.airspeed, state.airspeed_estimate_type); - state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true); - state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec); + state.airspeed_ok = _airspeed_EAS(state.airspeed, state.airspeed_estimate_type); + state.airspeed_true_ok = _airspeed_TAS(state.airspeed_true); + state.airspeed_vec_ok = _airspeed_TAS(state.airspeed_vec); state.quat_ok = _get_quaternion(state.quat); state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude); state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat); @@ -931,7 +931,7 @@ bool AP_AHRS::_should_use_airspeed_sensor(uint8_t airspeed_index) const // return an airspeed estimate if available. return true // if we have an estimate -bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const +bool AP_AHRS::_airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &airspeed_estimate_type) const { #if AP_AHRS_DCM_ENABLED || (AP_AIRSPEED_ENABLED && AP_GPS_ENABLED) const uint8_t idx = get_active_airspeed_index(); @@ -970,20 +970,20 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED case EKFType::DCM: airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #endif #if AP_AHRS_SIM_ENABLED case EKFType::SIM: airspeed_estimate_type = AirspeedEstimateType::SIM; - return sim.airspeed_estimate(airspeed_ret); + return sim.airspeed_EAS(airspeed_ret); #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #else return false; #endif @@ -999,7 +999,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs case EKFType::EXTERNAL: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #else return false; #endif @@ -1027,18 +1027,18 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs #if AP_AHRS_DCM_ENABLED // fallback to DCM airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; - return dcm.airspeed_estimate(idx, airspeed_ret); + return dcm.airspeed_EAS(idx, airspeed_ret); #endif return false; } -bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const +bool AP_AHRS::_airspeed_TAS(float &airspeed_ret) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED case EKFType::DCM: - return dcm.airspeed_estimate_true(airspeed_ret); + return dcm.airspeed_TAS(airspeed_ret); #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: @@ -1064,7 +1064,7 @@ bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable -bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const +bool AP_AHRS::_airspeed_TAS(Vector3f &vec) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1126,14 +1126,14 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance, return false; } -// return a synthetic airspeed estimate (one derived from sensors +// return a synthetic EAS estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. bool AP_AHRS::synthetic_airspeed(float &ret) const { #if AP_AHRS_DCM_ENABLED - return dcm.synthetic_airspeed(ret); + return dcm.synthetic_airspeed_EAS(ret); #endif return false; } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 10f9deed01df92..04ce489dad7e39 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -152,7 +152,7 @@ class AP_AHRS { // get air density / sea level density - decreases as altitude climbs float get_air_density_ratio(void) const; - // return an airspeed estimate if available. return true + // return an (equivalent) airspeed estimate if available. return true // if we have an estimate bool airspeed_estimate(float &airspeed_ret) const; @@ -195,7 +195,7 @@ class AP_AHRS { return AP_AHRS_Backend::airspeed_sensor_enabled(airspeed_index); } - // return a synthetic airspeed estimate (one derived from sensors + // return a synthetic (equivalent) airspeed estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. @@ -873,7 +873,7 @@ class AP_AHRS { // return an airspeed estimate if available. return true // if we have an estimate - bool _airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &status) const; + bool _airspeed_EAS(float &airspeed_ret, AirspeedEstimateType &status) const; // return secondary attitude solution if available, as eulers in radians bool _get_secondary_attitude(Vector3f &eulers) const; @@ -892,11 +892,11 @@ class AP_AHRS { // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate - bool _airspeed_estimate_true(float &airspeed_ret) const; + bool _airspeed_TAS(float &airspeed_ret) const; // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable - bool _airspeed_vector_true(Vector3f &vec) const; + bool _airspeed_TAS(Vector3f &vec) const; // return the quaternion defining the rotation from NED to XYZ (body) axes bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 0e70ce96620ab3..99ad8f2dc82c96 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -135,13 +135,13 @@ class AP_AHRS_Backend // return an airspeed estimate if available. return true // if we have an estimate - virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; } - virtual bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const { return false; } + virtual bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED { return false; } + virtual bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { return false; } // return a true airspeed estimate (navigation airspeed) if // available. return true if we have an estimate - bool airspeed_estimate_true(float &airspeed_ret) const WARN_IF_UNUSED { - if (!airspeed_estimate(airspeed_ret)) { + bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED { + if (!airspeed_EAS(airspeed_ret)) { return false; } airspeed_ret *= get_EAS2TAS(); @@ -156,6 +156,7 @@ class AP_AHRS_Backend // get apparent to true airspeed ratio static float get_EAS2TAS(void); + static float get_TAS2EAS(void) { return 1.0/get_EAS2TAS(); } // return true if airspeed comes from an airspeed sensor, as // opposed to an IMU estimate diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index d7aa5c7cd3da50..a9179502d33248 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -724,16 +724,16 @@ AP_AHRS_DCM::drift_correction(float deltat) return; } - float airspeed = _last_airspeed; + float airspeed_TAS = _last_airspeed_TAS; #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled()) { - airspeed = AP::airspeed()->get_airspeed(); + airspeed_TAS = AP::airspeed()->get_airspeed() * get_EAS2TAS(); } #endif // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind - velocity = _dcm_matrix.colx() * airspeed; + velocity = _dcm_matrix.colx() * airspeed_TAS; // add in wind estimate velocity += _wind; @@ -762,7 +762,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // take positive component in X direction. This mimics a pitot // tube - _last_airspeed = MAX(airspeed.x, 0); + _last_airspeed_TAS = MAX(airspeed.x, 0); } if (have_gps()) { @@ -1084,31 +1084,31 @@ bool AP_AHRS_DCM::get_location(Location &loc) const return _have_position; } -bool AP_AHRS_DCM::airspeed_estimate(float &airspeed_ret) const +bool AP_AHRS_DCM::airspeed_EAS(float &airspeed_ret) const { #if AP_AIRSPEED_ENABLED const auto *airspeed = AP::airspeed(); if (airspeed != nullptr) { - return airspeed_estimate(airspeed->get_primary(), airspeed_ret); + return airspeed_EAS(airspeed->get_primary(), airspeed_ret); } #endif // airspeed_estimate will also make this nullptr check and act // appropriately when we call it with a dummy sensor ID. - return airspeed_estimate(0, airspeed_ret); + return airspeed_EAS(0, airspeed_ret); } -// return an airspeed estimate: +// return an (equivalent) airspeed estimate: // - from a real sensor if available // - otherwise from a GPS-derived wind-triangle estimate (if GPS available) // - otherwise from a cached wind-triangle estimate value (but returning false) -bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const +bool AP_AHRS_DCM::airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { - // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: + // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate - if (!get_unconstrained_airspeed_estimate(airspeed_index, airspeed_ret)) { + if (!get_unconstrained_airspeed_EAS(airspeed_index, airspeed_ret)) { return false; } @@ -1127,12 +1127,12 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) return true; } -// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: +// airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate -bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const +bool AP_AHRS_DCM::get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { #if AP_AIRSPEED_ENABLED if (airspeed_sensor_enabled(airspeed_index)) { @@ -1143,13 +1143,13 @@ bool AP_AHRS_DCM::get_unconstrained_airspeed_estimate(uint8_t airspeed_index, fl if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) { // estimated via GPS speed and wind - airspeed_ret = _last_airspeed; + airspeed_ret = _last_airspeed_TAS * get_TAS2EAS(); return true; } // Else give the last estimate, but return false. // This is used by the dead-reckoning code - airspeed_ret = _last_airspeed; + airspeed_ret = _last_airspeed_TAS * get_TAS2EAS(); return false; } @@ -1182,7 +1182,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void) Vector2f gndVelADS; Vector2f gndVelGPS; float airspeed = 0; - const bool gotAirspeed = airspeed_estimate_true(airspeed); + const bool gotAirspeed = airspeed_TAS(airspeed); const bool gotGPS = (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D); if (gotAirspeed) { const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed}; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index e4e13ded40fa40..8f9288d014ba87 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -82,18 +82,18 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { // return an airspeed estimate if available. return true // if we have an estimate - bool airspeed_estimate(float &airspeed_ret) const override; + bool airspeed_EAS(float &airspeed_ret) const override; // return an airspeed estimate if available. return true // if we have an estimate from a specific sensor index - bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override; + bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override; - // return a synthetic airspeed estimate (one derived from sensors + // return a synthetic EAS estimate (one derived from sensors // other than an actual airspeed sensor), if available. return // true if we have a synthetic airspeed. ret will not be modified // on failure. - bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED { - ret = _last_airspeed; + bool synthetic_airspeed_EAS(float &ret) const WARN_IF_UNUSED { + ret = _last_airspeed_TAS; return true; } @@ -173,12 +173,12 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { // DCM matrix from the eulers. Called internally we may. void reset(bool recover_eulers); - // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_estimate which fills in airspeed_ret in this order: + // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order: // airspeed as filled-in by an enabled airspeed sensor // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation // Or if none of the above, fills-in using the previous airspeed estimate // Return false: if we are using the previous airspeed estimate - bool get_unconstrained_airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const; + bool get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const; // primary representation of attitude of board used for all inertial calculations Matrix3f _dcm_matrix; @@ -262,7 +262,7 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { Vector3f _last_fuse; Vector3f _last_vel; uint32_t _last_wind_time; - float _last_airspeed; + float _last_airspeed_TAS; uint32_t _last_consistent_heading; // estimated wind in m/s diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index c1e26f18909b1e..4f9b223cdbd521 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -41,7 +41,7 @@ bool AP_AHRS_SIM::wind_estimate(Vector3f &wind) const return true; } -bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const +bool AP_AHRS_SIM::airspeed_EAS(float &airspeed_ret) const { if (_sitl == nullptr) { return false; @@ -52,9 +52,9 @@ bool AP_AHRS_SIM::airspeed_estimate(float &airspeed_ret) const return true; } -bool AP_AHRS_SIM::airspeed_estimate(uint8_t index, float &airspeed_ret) const +bool AP_AHRS_SIM::airspeed_EAS(uint8_t index, float &airspeed_ret) const { - return airspeed_estimate(airspeed_ret); + return airspeed_EAS(airspeed_ret); } bool AP_AHRS_SIM::get_quaternion(Quaternion &quat) const diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 037b64c388c805..924a0d6beaf579 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -68,11 +68,11 @@ class AP_AHRS_SIM : public AP_AHRS_Backend { // return an airspeed estimate if available. return true // if we have an estimate - bool airspeed_estimate(float &airspeed_ret) const override; + bool airspeed_EAS(float &airspeed_ret) const override; // return an airspeed estimate if available. return true // if we have an estimate from a specific sensor index - bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override; + bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override; // return a ground vector estimate in meters/second, in North/East order Vector2f groundspeed_vector() override; diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 450567ce2c6238..a2bfa34d1db37c 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -36,7 +36,7 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_ y_angle = 270; break; default: - AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation); + AP_HAL::panic("Unsupported AHRS view %u", (unsigned)rotation); } _pitch_trim_deg = pitch_trim_deg; diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h index a16375362bed72..9bc05d7aa22cf6 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h @@ -58,7 +58,7 @@ class AP_AdvancedFailsafe { AP_Param::setup_object_defaults(this, var_info); if (_singleton != nullptr) { - AP_HAL::panic("AP_Logger must be singleton"); + AP_HAL::panic("AP_AdvancedFailsafe must be singleton"); } _singleton = this; diff --git a/libraries/AP_Airspeed/Airspeed_Calibration.cpp b/libraries/AP_Airspeed/Airspeed_Calibration.cpp index 0a416d2150e1ff..16170f9c81ea1b 100644 --- a/libraries/AP_Airspeed/Airspeed_Calibration.cpp +++ b/libraries/AP_Airspeed/Airspeed_Calibration.cpp @@ -164,32 +164,43 @@ void AP_Airspeed::update_calibration(const Vector3f &vground, int16_t max_airspe for (uint8_t i=0; i 1 @@ -92,6 +98,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 2_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 2_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 2_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 2_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[1], "2_", 42, AP_BattMonitor, backend_var_info[1]), #endif @@ -116,6 +128,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 3_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 3_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 3_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 3_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[2], "3_", 43, AP_BattMonitor, backend_var_info[2]), #endif @@ -140,6 +158,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 4_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 4_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 4_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 4_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[3], "4_", 44, AP_BattMonitor, backend_var_info[3]), #endif @@ -164,6 +188,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 5_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 5_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 5_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 5_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[4], "5_", 45, AP_BattMonitor, backend_var_info[4]), #endif @@ -188,6 +218,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 6_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 6_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 6_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 6_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[5], "6_", 46, AP_BattMonitor, backend_var_info[5]), #endif @@ -212,6 +248,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 7_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 7_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 7_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 7_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[6], "7_", 47, AP_BattMonitor, backend_var_info[6]), #endif @@ -236,6 +278,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 8_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 8_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 8_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 8_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[7], "8_", 48, AP_BattMonitor, backend_var_info[7]), #endif @@ -260,6 +308,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: 9_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: 9_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: 9_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: 9_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[8], "9_", 49, AP_BattMonitor, backend_var_info[8]), #endif @@ -284,6 +338,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: A_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: A_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: A_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: A_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[9], "A_", 50, AP_BattMonitor, backend_var_info[9]), #endif @@ -308,6 +368,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: B_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: B_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: B_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: B_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[10], "B_", 51, AP_BattMonitor, backend_var_info[10]), #endif @@ -332,6 +398,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: C_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: C_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: C_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: C_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[11], "C_", 52, AP_BattMonitor, backend_var_info[11]), #endif @@ -356,6 +428,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: D_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: D_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: D_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: D_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[12], "D_", 53, AP_BattMonitor, backend_var_info[12]), #endif @@ -380,6 +458,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: E_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: E_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: E_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: E_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[13], "E_", 54, AP_BattMonitor, backend_var_info[13]), #endif @@ -404,6 +488,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: F_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: F_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: F_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: F_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[14], "F_", 55, AP_BattMonitor, backend_var_info[14]), #endif @@ -428,6 +518,12 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_INA2xx.cpp // @Group: G_ // @Path: AP_BattMonitor_ESC.cpp + // @Group: G_ + // @Path: AP_BattMonitor_INA239.cpp + // @Group: G_ + // @Path: AP_BattMonitor_INA3221.cpp + // @Group: G_ + // @Path: AP_BattMonitor_AD7091R5.cpp AP_SUBGROUPVARPTR(drivers[15], "G_", 56, AP_BattMonitor, backend_var_info[15]), #endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp index 70e37eab43c420..955541a3a2c70a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_AD7091R5.cpp @@ -84,7 +84,7 @@ const AP_Param::GroupInfo AP_BattMonitor_AD7091R5::var_info[] = { // @User: Advanced AP_GROUPINFO("VLT_OFFSET", 61, AP_BattMonitor_AD7091R5, _volt_offset, 0), - // Param indexes must be 56 to 61 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp index 7c65c84fa672c3..6e341dd0356922 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp @@ -55,7 +55,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Analog::var_info[] = { // @User: Advanced AP_GROUPINFO("VLT_OFFSET", 6, AP_BattMonitor_Analog, _volt_offset, 0), - // Param indexes must be less than 10 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 728e3b918adca3..51ea04184c4fc2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -26,6 +26,27 @@ #include "AP_ESC_Telem/AP_ESC_Telem.h" #endif +/* + All backends use the same parameter table and set of indices. Therefore, two + backends must not use the same index. The list of used indices and + corresponding backends is below. + + 1-6: AP_BattMonitor_Analog.cpp + 10-11: AP_BattMonitor_SMBus.cpp + 20: AP_BattMonitor_Sum.cpp + 22-24: AP_BattMonitor_INA3221.cpp + 25-26: AP_BattMonitor_INA2xx.cpp + 27-28: AP_BattMonitor_INA2xx.cpp, AP_BattMonitor_INA239.cpp (legacy duplication) + 30: AP_BattMonitor_DroneCAN.cpp + 36: AP_BattMonitor_ESC.cpp + 40-43: AP_BattMonitor_FuelLevel_Analog.cpp + 45-48: AP_BattMonitor_FuelLevel_Analog.cpp + 50-51: AP_BattMonitor_Synthetic_Current.cpp + 56-61: AP_BattMonitor_AD7091R5.cpp + + Usage does not need to be contiguous. The maximum possible index is 63. +*/ + /* base class constructor. This incorporates initialisation as well. diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index d505dcc20d93ac..9c4c5ccbe38030 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -26,7 +26,7 @@ const AP_Param::GroupInfo AP_BattMonitor_DroneCAN::var_info[] = { // @User: Advanced AP_GROUPINFO("CURR_MULT", 30, AP_BattMonitor_DroneCAN, _curr_mult, 1.0), - // Param indexes must be between 30 and 35 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp index 7b76295f3b5a2a..065dc52fbe192a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp @@ -22,8 +22,6 @@ const AP_Param::GroupInfo AP_BattMonitor_ESC::var_info[] = { - // Param indexes must be between 36 and 39 to avoid conflict with other battery monitor param tables loaded by pointer - // @Param: ESC_MASK // @DisplayName: ESC mask // @Description: If 0 all connected ESCs will be used. If non-zero, only those selected in will be used. @@ -31,7 +29,7 @@ const AP_Param::GroupInfo AP_BattMonitor_ESC::var_info[] = { // @User: Standard AP_GROUPINFO("ESC_MASK", 36, AP_BattMonitor_ESC, _mask, 0), - // Param indexes must be between 36 and 39 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp index d5d6c324bf7e82..2c8f9933295b3d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_FuelLevel_Analog.cpp @@ -58,8 +58,6 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = { // @Values: -1:Disabled, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 5:Navigator, 13:Pixhawk2_PM2/CubeOrange_PM2, 14:CubeOrange, 16:Durandal, 100:PX4-v1 AP_GROUPINFO("FL_PIN", 43, AP_BattMonitor_FuelLevel_Analog, _pin, -1), - // index 44 unused and available - // @Param: FL_FF // @DisplayName: First order term // @Description: First order polynomial fit term @@ -88,7 +86,7 @@ const AP_Param::GroupInfo AP_BattMonitor_FuelLevel_Analog::var_info[] = { // @User: Advanced AP_GROUPINFO("FL_OFF", 48, AP_BattMonitor_FuelLevel_Analog, _fuel_fit_offset, 0), - // Param indexes must be between 40 and 49 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp index d59a5f2dbba4a8..d4f0a343f96d1c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA239.cpp @@ -47,7 +47,9 @@ const AP_Param::GroupInfo AP_BattMonitor_INA239::var_info[] = { // @Units: Ohm // @User: Advanced AP_GROUPINFO("SHUNT", 28, AP_BattMonitor_INA239, rShunt, HAL_BATTMON_INA239_SHUNT_RESISTANCE), - + + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS + AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp index cf25c819740bec..3994452dc57a60 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp @@ -108,7 +108,9 @@ const AP_Param::GroupInfo AP_BattMonitor_INA2XX::var_info[] = { // @Units: Ohm // @User: Advanced AP_GROUPINFO("SHUNT", 28, AP_BattMonitor_INA2XX, rShunt, DEFAULT_BATTMON_INA2XX_SHUNT), - + + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS + AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp index cad3a04acce100..b0804de7802751 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp @@ -67,15 +67,13 @@ uint8_t AP_BattMonitor_INA3221::address_driver_count; const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = { - // Param indexes must be between 56 and 61 to avoid conflict with other battery monitor param tables loaded by pointer - // @Param: I2C_BUS // @DisplayName: Battery monitor I2C bus number // @Description: Battery monitor I2C bus number // @Range: 0 3 // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("I2C_BUS", 56, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS), + AP_GROUPINFO("I2C_BUS", 22, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS), // @Param: I2C_ADDR // @DisplayName: Battery monitor I2C address @@ -83,7 +81,7 @@ const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = { // @Range: 0 127 // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("I2C_ADDR", 57, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR), + AP_GROUPINFO("I2C_ADDR", 23, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR), // @Param: CHANNEL // @DisplayName: INA3221 channel @@ -91,7 +89,9 @@ const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = { // @Range: 1 3 // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("CHANNEL", 58, AP_BattMonitor_INA3221, channel, 1), + AP_GROUPINFO("CHANNEL", 24, AP_BattMonitor_INA3221, channel, 1), + + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index eac0d2b02b5f51..8865a42d37296c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -113,7 +113,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @DisplayName: Low battery failsafe action // @Description: What action the vehicle should perform if it hits a low battery failsafe // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,6:Loiter to QLand - // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START or RTL + // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate // @Values{Tracker}: 0:None @@ -125,7 +125,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @DisplayName: Critical battery failsafe action // @Description: What action the vehicle should perform if it hits a critical battery failsafe // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,5:Parachute,6:Loiter to QLand - // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START or RTL + // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:Auto DO_LAND_START/DO_RETURN_PATH_START or RTL // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate // @Values{Tracker}: 0:None diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp index 0d778bed742adc..5a45fd1d340f22 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp @@ -10,8 +10,6 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_BattMonitor_SMBus::var_info[] = { - // Param indexes must be between 10 and 19 to avoid conflict with other battery monitor param tables loaded by pointer - // @Param: I2C_BUS // @DisplayName: Battery monitor I2C bus number // @Description: Battery monitor I2C bus number @@ -28,7 +26,7 @@ const AP_Param::GroupInfo AP_BattMonitor_SMBus::var_info[] = { // @RebootRequired: True AP_GROUPINFO("I2C_ADDR", 11, AP_BattMonitor_SMBus, _address, AP_BATTMONITOR_SMBUS_I2C_ADDR), - // Param indexes must be between 10 and 19 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp index dcc37cc27838b6..8405020c37e85c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp @@ -19,8 +19,6 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_BattMonitor_Sum::var_info[] = { - // Param indexes must be between 20 and 29 to avoid conflict with other battery monitor param tables loaded by pointer - // @Param: SUM_MASK // @DisplayName: Battery Sum mask // @Description: 0: sum of remaining battery monitors, If none 0 sum of specified monitors. Current will be summed and voltages averaged. @@ -28,7 +26,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Sum::var_info[] = { // @User: Standard AP_GROUPINFO("SUM_MASK", 20, AP_BattMonitor_Sum, _sum_mask, 0), - // Param indexes must be between 20 and 29 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp index acf807ff4c6545..43d2566e4396e5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Synthetic_Current.cpp @@ -27,7 +27,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Synthetic_Current::var_info[] = { // also inherit analog backend parameters AP_SUBGROUPEXTENSION("", 51, AP_BattMonitor_Synthetic_Current, AP_BattMonitor_Analog::var_info), - // Param indexes must be between 50 and 55 to avoid conflict with other battery monitor param tables loaded by pointer + // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS AP_GROUPEND }; diff --git a/libraries/AP_CANManager/AP_CANIfaceParams.cpp b/libraries/AP_CANManager/AP_CANIfaceParams.cpp index b088f392c7478d..4a917553880da7 100644 --- a/libraries/AP_CANManager/AP_CANIfaceParams.cpp +++ b/libraries/AP_CANManager/AP_CANIfaceParams.cpp @@ -43,6 +43,14 @@ const AP_Param::GroupInfo AP_CANManager::CANIface_Params::var_info[] = { // @User: Advanced AP_GROUPINFO("FDBITRATE", 3, AP_CANManager::CANIface_Params, _fdbitrate, HAL_CANFD_SUPPORTED), #endif + + // @Param: OPTIONS + // @DisplayName: CAN per-interface options + // @Description: CAN per-interface options + // @Bitmask: 0:LogAllFrames + // @User: Advanced + AP_GROUPINFO("OPTIONS", 4, AP_CANManager::CANIface_Params, _options, 0), + // Index 3 occupied by Param: DEBUG AP_GROUPEND }; diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index ebf574df99236b..a39d4d1f6b6e11 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -41,6 +41,15 @@ #include #include +#include + +/* + avoid a recursion issue with config defines + */ +#if AP_CAN_LOGGING_ENABLED && !HAL_LOGGING_ENABLED +#undef AP_CAN_LOGGING_ENABLED +#define AP_CAN_LOGGING_ENABLED 0 +#endif #define LOG_TAG "CANMGR" #define LOG_BUFFER_SIZE 1024 @@ -260,6 +269,10 @@ void AP_CANManager::init() _drivers[drv_num]->init(drv_num, enable_filter); } + +#if AP_CAN_LOGGING_ENABLED + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_CANManager::check_logging_enable, void)); +#endif } #else void AP_CANManager::init() @@ -372,6 +385,7 @@ void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag, if (loglevel > _loglevel) { return; } + WITH_SEMAPHORE(_sem); if ((LOG_BUFFER_SIZE - _log_pos) < (10 + strlen(tag) + strlen(fmt))) { // reset log pos @@ -509,6 +523,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) frame_buffer->push(frame); break; } +#if HAL_CANFD_SUPPORTED case MAVLINK_MSG_ID_CANFD_FRAME: { mavlink_canfd_frame_t p; mavlink_msg_canfd_frame_decode(&msg, &p); @@ -523,6 +538,7 @@ void AP_CANManager::handle_can_frame(const mavlink_message_t &msg) frame_buffer->push(frame); break; } +#endif } process_frame_buffer(); } @@ -684,12 +700,15 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram } } const uint8_t data_len = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); +#if HAL_CANFD_SUPPORTED if (frame.isCanFDFrame()) { if (HAVE_PAYLOAD_SPACE(can_forward.chan, CANFD_FRAME)) { mavlink_msg_canfd_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id, bus, data_len, frame.id, const_cast(frame.data)); } - } else { + } else +#endif + { if (HAVE_PAYLOAD_SPACE(can_forward.chan, CAN_FRAME)) { mavlink_msg_can_frame_send(can_forward.chan, can_forward.system_id, can_forward.component_id, bus, data_len, frame.id, const_cast(frame.data)); @@ -698,6 +717,61 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram } #endif // HAL_GCS_ENABLED +#if AP_CAN_LOGGING_ENABLED +/* + handler for CAN frames for frame logging + */ +void AP_CANManager::can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame) +{ +#if HAL_CANFD_SUPPORTED + if (frame.canfd) { + struct log_CAFD pkt { + LOG_PACKET_HEADER_INIT(LOG_CAFD_MSG), + time_us : AP_HAL::micros64(), + bus : bus, + id : frame.id, + dlc : frame.dlc + }; + memcpy(pkt.data, frame.data, frame.dlcToDataLength(frame.dlc)); + AP::logger().WriteBlock(&pkt, sizeof(pkt)); + return; + } +#endif + struct log_CANF pkt { + LOG_PACKET_HEADER_INIT(LOG_CANF_MSG), + time_us : AP_HAL::micros64(), + bus : bus, + id : frame.id, + dlc : frame.dlc + }; + memcpy(pkt.data, frame.data, frame.dlc); + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + +/* + see if we need to enable/disable the CAN logging callback + */ +void AP_CANManager::check_logging_enable(void) +{ + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + const bool enabled = _interfaces[i].option_is_set(CANIface_Params::Options::LOG_ALL_FRAMES); + uint8_t &logging_id = _interfaces[i].logging_id; + auto *can = hal.can[i]; + if (can == nullptr) { + continue; + } + if (enabled && logging_id == 0) { + can->register_frame_callback( + FUNCTOR_BIND_MEMBER(&AP_CANManager::can_logging_callback, void, uint8_t, const AP_HAL::CANFrame &), + logging_id); + } else if (!enabled && logging_id != 0) { + can->unregister_frame_callback(logging_id); + } + } +} + +#endif // AP_CAN_LOGGING_ENABLED + AP_CANManager& AP::can() { return *AP_CANManager::get_singleton(); diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index f820d8317482de..561c9c468054a7 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -126,10 +126,23 @@ class AP_CANManager static const struct AP_Param::GroupInfo var_info[]; + enum class Options : uint32_t { + LOG_ALL_FRAMES = (1U<<0), + }; + + bool option_is_set(Options option) const { + return (_options & uint32_t(option)) != 0; + } + private: AP_Int8 _driver_number; AP_Int32 _bitrate; AP_Int32 _fdbitrate; + AP_Int32 _options; + +#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED + uint8_t logging_id; +#endif }; //Parameter Interface for CANDrivers @@ -198,6 +211,14 @@ class AP_CANManager void process_frame_buffer(void); #endif // HAL_GCS_ENABLED + +#if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED + /* + handler for CAN frames for logging + */ + void can_logging_callback(uint8_t bus, const AP_HAL::CANFrame &frame); + void check_logging_enable(void); +#endif }; namespace AP diff --git a/libraries/AP_CANManager/AP_CANManager_config.h b/libraries/AP_CANManager/AP_CANManager_config.h index c8b9f8af91cef7..383abe0041dc9f 100644 --- a/libraries/AP_CANManager/AP_CANManager_config.h +++ b/libraries/AP_CANManager/AP_CANManager_config.h @@ -5,3 +5,8 @@ #ifndef AP_CAN_SLCAN_ENABLED #define AP_CAN_SLCAN_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS #endif + +#ifndef AP_CAN_LOGGING_ENABLED +#define AP_CAN_LOGGING_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif + diff --git a/libraries/AP_CANManager/LogStructure.h b/libraries/AP_CANManager/LogStructure.h new file mode 100644 index 00000000000000..3f2e2c33ccbeff --- /dev/null +++ b/libraries/AP_CANManager/LogStructure.h @@ -0,0 +1,79 @@ +#pragma once +/* + log structures for AP_CANManager + */ + +#include +#include "AP_CANManager_config.h" + +#define LOG_IDS_FROM_CANMANAGER \ + LOG_CANF_MSG, \ + LOG_CAFD_MSG + +// @LoggerMessage: CANF +// @Description: CAN Frame +// @Field: TimeUS: Time since system startup +// @Field: Bus: bus number +// @Field: Id: frame identifier +// @Field: DLC: data length code +// @Field: B0: byte 0 +// @Field: B1: byte 1 +// @Field: B2: byte 2 +// @Field: B3: byte 3 +// @Field: B4: byte 4 +// @Field: B5: byte 5 +// @Field: B6: byte 6 +// @Field: B7: byte 7 +struct PACKED log_CANF { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t bus; + uint32_t id; + uint8_t dlc; + uint8_t data[8]; +}; + +// @LoggerMessage: CAFD +// @Description: CANFD Frame +// @Field: TimeUS: Time since system startup +// @Field: Bus: bus number +// @Field: Id: frame identifier +// @Field: DLC: data length code +// @Field: D0: data 0 +// @Field: D1: data 1 +// @Field: D2: data 2 +// @Field: D3: data 3 +// @Field: D4: data 4 +// @Field: D5: data 5 +// @Field: D6: data 6 +// @Field: D7: data 7 +struct PACKED log_CAFD { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t bus; + uint32_t id; + uint8_t dlc; + uint64_t data[8]; +}; + +#if !AP_CAN_LOGGING_ENABLED +#define LOG_STRUCTURE_FROM_CANMANAGER +#else +#define LOG_STRUCTURE_FROM_CANMANAGER \ + { LOG_CANF_MSG, sizeof(log_CANF), \ + "CANF", \ + "Q" "B" "I" "B" "B" "B" "B" "B" "B" "B" "B" "B", \ + "TimeUS," "Bus," "Id," "DLC," "B0," "B1," "B2," "B3," "B4," "B5," "B6," "B7", \ + "s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + false \ + }, \ + { LOG_CAFD_MSG, sizeof(log_CAFD), \ + "CAFD", \ + "Q" "B" "I" "B" "Q" "Q" "Q" "Q" "Q" "Q" "Q" "Q", \ + "TimeUS," "Bus," "Id," "DLC," "D0," "D1," "D2," "D3," "D4," "D5," "D6," "D7", \ + "s" "#" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + "F" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-" "-", \ + false \ + }, +#endif // AP_CAN_LOGGING_ENABLED diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 288b0506bb54ee..aee97439dca3a3 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "AP_Camera_Backend.h" #include "AP_Camera_Servo.h" #include "AP_Camera_Relay.h" @@ -14,6 +15,7 @@ #include "AP_Camera_MAVLink.h" #include "AP_Camera_MAVLinkCamV2.h" #include "AP_Camera_Scripting.h" +#include "AP_RunCam.h" const AP_Param::GroupInfo AP_Camera::var_info[] = { @@ -41,10 +43,24 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = { // @Path: AP_Camera_Params.cpp AP_SUBGROUPINFO(_params[1], "2", 13, AP_Camera, AP_Camera_Params), #endif +#if AP_CAMERA_RUNCAM_ENABLED + // @Group: 1_RC_ + // @Path: AP_RunCam.cpp + AP_SUBGROUPVARPTR(_backends[0], "1_RC_", 14, AP_Camera, _backend_var_info[0]), +#if AP_CAMERA_MAX_INSTANCES > 1 + // @Group: 2_RC_ + // @Path: AP_RunCam.cpp + AP_SUBGROUPVARPTR(_backends[1], "2_RC_", 15, AP_Camera, _backend_var_info[1]), +#endif +#endif AP_GROUPEND }; +#if AP_CAMERA_RUNCAM_ENABLED +const AP_Param::GroupInfo *AP_Camera::_backend_var_info[AP_CAMERA_MAX_INSTANCES]; +#endif + extern const AP_HAL::HAL& hal; AP_Camera::AP_Camera(uint32_t _log_camera_bit) : @@ -238,6 +254,17 @@ void AP_Camera::init() case CameraType::SCRIPTING: _backends[instance] = NEW_NOTHROW AP_Camera_Scripting(*this, _params[instance], instance); break; +#endif +#if AP_CAMERA_RUNCAM_ENABLED + // check for RunCam driver + case CameraType::RUNCAM: + if (_backends[instance] == nullptr) { // may have already been created by the conversion code + _backends[instance] = NEW_NOTHROW AP_RunCam(*this, _params[instance], instance, _runcam_instances); + _backend_var_info[instance] = AP_RunCam::var_info; + AP_Param::load_object_from_eeprom(_backends[instance], _backend_var_info[instance]); + _runcam_instances++; + } + break; #endif case CameraType::NONE: break; @@ -899,7 +926,11 @@ AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const void AP_Camera::convert_params() { // exit immediately if CAM1_TYPE has already been configured - if (_params[0].type.configured()) { + if (_params[0].type.configured() +#if AP_CAMERA_RUNCAM_ENABLED + && _params[1].type.configured() +#endif + ) { return; } @@ -919,6 +950,42 @@ void AP_Camera::convert_params() } _params[0].type.set_and_save(cam1_type); +#if AP_CAMERA_RUNCAM_ENABLED + // RunCam PARAMETER_CONVERSION - Added: Nov-2024 ahead of 4.7 release + + // Since slot 1 is essentially used by the trigger type, we will use slot 2 for runcam + int8_t rc_type = 0; + // find vehicle's top level key + uint16_t k_param_vehicle_key; + if (!AP_Param::find_top_level_key_by_pointer(AP::vehicle(), k_param_vehicle_key)) { + return; + } + + // RunCam protocol configured so set cam type to RunCam + bool rc_protocol_configured = false; + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + if (serial_manager && serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0)) { + rc_protocol_configured = true; + } + + const AP_Param::ConversionInfo rc_type_info = { + k_param_vehicle_key, AP_GROUP_ELEM_IDX(1, 1), AP_PARAM_INT8, "CAM_RC_TYPE" + }; + AP_Int8 rc_type_old; + const bool found_rc_type = AP_Param::find_old_parameter(&rc_type_info, &rc_type_old); + + if (rc_protocol_configured || (found_rc_type && rc_type_old.get() > 0)) { + rc_type = int8_t(CameraType::RUNCAM); + _backends[1] = NEW_NOTHROW AP_RunCam(*this, _params[1], 1, _runcam_instances); + _backend_var_info[1] = AP_RunCam::var_info; + AP_Param::convert_class(k_param_vehicle_key, &_backends[1], _backend_var_info[1], 1, false); + AP_Param::invalidate_count(); + _runcam_instances++; + } + + _params[1].type.set_and_save(rc_type); +#endif // AP_CAMERA_RUNCAM_ENABLED + // convert CAM_DURATION (in deci-seconds) to CAM1_DURATION (in seconds) int8_t cam_duration = 0; if (AP_Param::get_param_by_index(this, 1, AP_PARAM_INT8, &cam_duration) && (cam_duration > 0)) { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index a7b7f602ac43e2..e1f0a07433d62d 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -23,6 +23,7 @@ class AP_Camera_Mount; class AP_Camera_MAVLink; class AP_Camera_MAVLinkCamV2; class AP_Camera_Scripting; +class AP_RunCam; /// @class Camera /// @brief Object managing a Photo or video camera @@ -37,6 +38,7 @@ class AP_Camera { friend class AP_Camera_MAVLink; friend class AP_Camera_MAVLinkCamV2; friend class AP_Camera_Scripting; + friend class AP_RunCam; public: @@ -72,6 +74,9 @@ class AP_Camera { #endif #if AP_CAMERA_SCRIPTING_ENABLED SCRIPTING = 7, // Scripting backend +#endif +#if AP_CAMERA_RUNCAM_ENABLED + RUNCAM = 8, // RunCam backend #endif }; @@ -216,6 +221,11 @@ class AP_Camera { // parameters for backends AP_Camera_Params _params[AP_CAMERA_MAX_INSTANCES]; +#if AP_CAMERA_RUNCAM_ENABLED + // var info pointer for RunCam + static const struct AP_Param::GroupInfo *_backend_var_info[AP_CAMERA_MAX_INSTANCES]; + uint8_t _runcam_instances; +#endif private: diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index c2d5868c10acac..6954c8b9e2cf20 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -274,7 +274,6 @@ void AP_Camera_Backend::send_video_stream_information(mavlink_channel_t chan) co #if AP_CAMERA_INFO_FROM_SCRIPT_ENABLED void AP_Camera_Backend::set_stream_information(mavlink_video_stream_information_t stream_info) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Camera %u VIDEO_STREAM_INFORMATION (%s) set from script", _instance, stream_info.name); _stream_info = stream_info; }; #endif // AP_CAMERA_INFO_FROM_SCRIPT_ENABLED @@ -296,32 +295,46 @@ void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const { // getting corresponding mount instance for camera - const AP_Mount* mount = AP::mount(); + AP_Mount* mount = AP::mount(); if (mount == nullptr) { return; } + + // get latest POI from mount Quaternion quat; - Location loc; + Location camera_loc; Location poi_loc; - if (!mount->get_poi(get_mount_instance(), quat, loc, poi_loc)) { - return; + const bool have_poi_loc = mount->get_poi(get_mount_instance(), quat, camera_loc, poi_loc); + + // if failed to get POI, get camera location directly from AHRS + // and attitude directly from mount + bool have_camera_loc = have_poi_loc; + if (!have_camera_loc) { + have_camera_loc = AP::ahrs().get_location(camera_loc); + mount->get_attitude_quaternion(get_mount_instance(), quat); } + + // calculate attitude quaternion in earth frame using AHRS yaw + Quaternion quat_ef; + quat_ef.from_euler(0, 0, AP::ahrs().get_yaw()); + quat_ef *= quat; + // send camera fov status message only if the last calculated values aren't stale const float quat_array[4] = { - quat.q1, - quat.q2, - quat.q3, - quat.q4 + quat_ef.q1, + quat_ef.q2, + quat_ef.q3, + quat_ef.q4 }; mavlink_msg_camera_fov_status_send( chan, AP_HAL::millis(), - loc.lat, - loc.lng, - loc.alt * 10, - poi_loc.lat, - poi_loc.lng, - poi_loc.alt * 10, + have_camera_loc ? camera_loc.lat : INT32_MAX, + have_camera_loc ? camera_loc.lng : INT32_MAX, + have_camera_loc ? camera_loc.alt * 10 : INT32_MAX, + have_poi_loc ? poi_loc.lat : INT32_MAX, + have_poi_loc ? poi_loc.lng : INT32_MAX, + have_poi_loc ? poi_loc.alt * 10 : INT32_MAX, quat_array, horizontal_fov() > 0 ? horizontal_fov() : NaNf, vertical_fov() > 0 ? vertical_fov() : NaNf diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 749d2985f0ab40..7c4b092996d2ad 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting + // @Values: 0:None, 1:Servo, 2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi/Topotek/Viewpro/Xacti), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting, 8:RunCam // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 90f69ea7da44c8..fa1ac3c4877eac 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -62,3 +62,7 @@ #ifndef AP_CAMERA_INFO_FROM_SCRIPT_ENABLED #define AP_CAMERA_INFO_FROM_SCRIPT_ENABLED AP_CAMERA_SCRIPTING_ENABLED #endif + +#ifndef AP_CAMERA_RUNCAM_ENABLED +#define AP_CAMERA_RUNCAM_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && HAL_RUNCAM_ENABLED +#endif diff --git a/libraries/AP_Camera/AP_RunCam.cpp b/libraries/AP_Camera/AP_RunCam.cpp index fadecee6d03098..ce089e694bcd91 100644 --- a/libraries/AP_Camera/AP_RunCam.cpp +++ b/libraries/AP_Camera/AP_RunCam.cpp @@ -23,7 +23,7 @@ */ #include "AP_RunCam.h" -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED #include #include @@ -36,7 +36,7 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = { // @DisplayName: RunCam device type // @Description: RunCam device type used to determine OSD menu structure and shutter options. // @Values: 0:Disabled, 1:RunCam Split Micro/RunCam with UART, 2:RunCam Split, 3:RunCam Split4 4k, 4:RunCam Hybrid/RunCam Thumb Pro, 5:Runcam 2 4k - AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceType::Disabled), AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO_FLAGS("TYPE", 1, AP_RunCam, _cam_type, int(DeviceModel::SplitMicro), AP_PARAM_FLAG_ENABLE), // @Param: FEATURES // @DisplayName: RunCam features available @@ -55,13 +55,13 @@ const AP_Param::GroupInfo AP_RunCam::var_info[] = { // @DisplayName: RunCam button delay before allowing further button presses // @Description: Time it takes for the a RunCam button press to be actived in ms. If this is too short then commands can get out of sync. // @User: Advanced - AP_GROUPINFO("BTN_DELAY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY), + AP_GROUPINFO("BTN_DELY", 4, AP_RunCam, _button_delay_ms, RUNCAM_DEFAULT_BUTTON_PRESS_DELAY), // @Param: MDE_DELAY // @DisplayName: RunCam mode delay before allowing further button presses // @Description: Time it takes for the a RunCam mode button press to be actived in ms. If a mode change first requires a video recording change then double this value is used. If this is too short then commands can get out of sync. // @User: Advanced - AP_GROUPINFO("MDE_DELAY", 5, AP_RunCam, _mode_delay_ms, 800), + AP_GROUPINFO("MDE_DELY", 5, AP_RunCam, _mode_delay_ms, 800), // @Param: CONTROL // @DisplayName: RunCam control option @@ -118,13 +118,24 @@ AP_RunCam::Menu AP_RunCam::_menus[RUNCAM_MAX_DEVICE_TYPES] = { { 6, { 3, 10, 2, 2, 8 }}, // Runcam 2 4K }; -AP_RunCam::AP_RunCam() +const char* AP_RunCam::_models[RUNCAM_MAX_DEVICE_TYPES] = { + "SplitMicro", + "Split", + "Split4k", + "Hybrid", + "Run24k" +}; + +AP_RunCam::AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance, uint8_t runcam_instance) + : AP_Camera_Backend(frontend, params, instance), _runcam_instance(runcam_instance) { AP_Param::setup_object_defaults(this, var_info); - if (_singleton != nullptr) { - AP_HAL::panic("AP_RunCam must be singleton"); + if (_singleton != nullptr && _singleton->_instance == instance) { + AP_HAL::panic("AP_RunCam instance must be a singleton %u", instance); + } + if (_singleton == nullptr) { + _singleton = this; } - _singleton = this; _cam_type.set(constrain_int16(_cam_type, 0, RUNCAM_MAX_DEVICE_TYPES)); _video_recording = VideoOption(_cam_control_option & uint8_t(ControlOption::VIDEO_RECORDING_AT_BOOT)); } @@ -134,19 +145,19 @@ void AP_RunCam::init() { AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); if (serial_manager) { - uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, 0); + uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_RunCam, _runcam_instance); } if (uart != nullptr) { /* if the user has setup a serial port as a runcam then default type to the split micro (Andy's development platform!). This makes setup a bit easier for most users while still enabling parameters to be hidden for users - without a runcam + without a RunCam */ - _cam_type.set_default(int8_t(DeviceType::SplitMicro)); + _cam_type.set_default(int8_t(DeviceModel::SplitMicro)); AP_Param::invalidate_count(); } - if (_cam_type.get() == int8_t(DeviceType::Disabled)) { + if (_cam_type.get() == int8_t(DeviceModel::Disabled)) { uart = nullptr; return; } @@ -156,7 +167,7 @@ void AP_RunCam::init() } // Split and Runcam 2 4k requires two mode presses to get into the menu - if (_cam_type.get() == int8_t(DeviceType::Split) || _cam_type.get() == int8_t(DeviceType::Run24k)) { + if (_cam_type.get() == int8_t(DeviceModel::Split) || _cam_type.get() == int8_t(DeviceModel::Run24k)) { _menu_enter_level = -1; _in_menu = -1; } @@ -221,7 +232,7 @@ void AP_RunCam::osd_option() { // input update loop void AP_RunCam::update() { - if (uart == nullptr || _cam_type.get() == int8_t(DeviceType::Disabled)) { + if (uart == nullptr || _cam_type.get() == int8_t(DeviceModel::Disabled)) { return; } @@ -551,12 +562,12 @@ void AP_RunCam::handle_2_key_simulation_process(Event ev) case Event::IN_MENU_ENTER: // in a sub-menu and save-and-exit was selected - if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) { + if (_in_menu > 1 && get_top_menu_length() > 0 && _sub_menu_pos == (get_sub_menu_length(_top_menu_pos) - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) { simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _button_delay_ms); _sub_menu_pos = 0; _in_menu--; // in the top-menu and save-and-exit was selected - } else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceType(_cam_type.get()) != DeviceType::Run24k) { + } else if (_in_menu == 1 && get_top_menu_length() > 0 && _top_menu_pos == (get_top_menu_length() - 1) && DeviceModel(_cam_type.get()) != DeviceModel::Run24k) { simulate_camera_button(ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_WIFI_BTN, _mode_delay_ms); _in_menu--; _state = State::EXITING_MENU; @@ -712,7 +723,7 @@ void AP_RunCam::handle_5_key_simulation_response(const Request& request) // command to start recording AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const { - if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) { + if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) { return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN; } else { return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_START_RECORDING; @@ -721,7 +732,7 @@ AP_RunCam::ControlOperation AP_RunCam::start_recording_command() const { // command to stop recording AP_RunCam::ControlOperation AP_RunCam::stop_recording_command() const { - if (DeviceType(_cam_type.get()) == DeviceType::Split4k || DeviceType(_cam_type.get()) == DeviceType::Hybrid || DeviceType(_cam_type.get()) == DeviceType::Run24k) { + if (DeviceModel(_cam_type.get()) == DeviceModel::Split4k || DeviceModel(_cam_type.get()) == DeviceModel::Hybrid || DeviceModel(_cam_type.get()) == DeviceModel::Run24k) { return ControlOperation::RCDEVICE_PROTOCOL_SIMULATE_POWER_BTN; } else { return ControlOperation::RCDEVICE_PROTOCOL_CHANGE_STOP_RECORDING; @@ -1058,8 +1069,88 @@ uint8_t AP_RunCam::Request::get_expected_response_length(const Command command) return 0; } +// AP_Camera API + +// return true if healthy +bool AP_RunCam::healthy() const +{ + return camera_ready(); +} + +// momentary switch to change camera between picture and video modes +void AP_RunCam::cam_mode_toggle() +{ + +} + +// entry point to actually take a picture. returns true on success +bool AP_RunCam::trigger_pic() +{ + return false; +} + +// send camera information message to GCS +void AP_RunCam::send_camera_information(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!camera_ready() || _cam_type.get() <= 0 || _cam_type.get() > int8_t(ARRAY_SIZE(_models))) { + return; + } + + static const uint8_t vendor_name[32] = "RunCam"; + uint8_t model_name[32] {}; + strncpy((char *)model_name, _models[_cam_type.get()-1], MIN(sizeof(model_name), sizeof(_models[_cam_type.get()-1]))); + const char cam_definition_uri[140] {}; + + // capability flags + uint32_t flags = 0; + + if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_START_RECORDING)) { + flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO; + } + + if (has_feature(Feature::RCDEVICE_PROTOCOL_FEATURE_CHANGE_MODE)) { + flags |= CAMERA_CAP_FLAGS_CAPTURE_IMAGE; + } + + // send CAMERA_INFORMATION message + mavlink_msg_camera_information_send( + chan, + AP_HAL::millis(), // time_boot_ms + vendor_name, // vendor_name uint8_t[32] + model_name, // model_name uint8_t[32] + 0, // firmware version uint32_t + NaNf, // focal_length float (mm) + NaNf, // sensor_size_h float (mm) + NaNf, // sensor_size_v float (mm) + 0, // resolution_h uint16_t (pix) + 0, // resolution_v uint16_t (pix) + 0, // lens_id uint8_t + flags, // flags uint32_t (CAMERA_CAP_FLAGS) + 0, // cam_definition_version uint16_t + cam_definition_uri, // cam_definition_uri char[140] + _instance + 1); // gimbal_device_id uint8_t +} + +// send camera settings message to GCS +void AP_RunCam::send_camera_settings(mavlink_channel_t chan) const +{ + // exit immediately if not initialised + if (!camera_ready()) { + return; + } + + // send CAMERA_SETTINGS message + mavlink_msg_camera_settings_send( + chan, + AP_HAL::millis(), // time_boot_ms + _video_recording == VideoOption::RECORDING ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey) + NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown + NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown +} + AP_RunCam *AP::runcam() { return AP_RunCam::get_singleton(); } -#endif // HAL_RUNCAM_ENABLED +#endif // AP_CAMERA_RUNCAM_ENABLED diff --git a/libraries/AP_Camera/AP_RunCam.h b/libraries/AP_Camera/AP_RunCam.h index 9eb62999236644..27b329fb68e147 100644 --- a/libraries/AP_Camera/AP_RunCam.h +++ b/libraries/AP_Camera/AP_RunCam.h @@ -23,7 +23,9 @@ #include "AP_Camera_config.h" -#if HAL_RUNCAM_ENABLED +#include "AP_Camera_Backend.h" + +#if AP_CAMERA_RUNCAM_ENABLED #include #include @@ -38,10 +40,10 @@ /// @class AP_RunCam /// @brief Object managing a RunCam device -class AP_RunCam +class AP_RunCam : public AP_Camera_Backend { public: - AP_RunCam(); + AP_RunCam(AP_Camera &frontend, AP_Camera_Params ¶ms, uint8_t instance, uint8_t runcam_instance); // do not allow copies CLASS_NO_COPY(AP_RunCam); @@ -51,7 +53,7 @@ class AP_RunCam return _singleton; } - enum class DeviceType { + enum class DeviceModel { Disabled = 0, SplitMicro = 1, // video support only Split = 2, // camera and video support @@ -79,22 +81,49 @@ class AP_RunCam VIDEO_RECORDING_AT_BOOT = (1 << 4) }; + + // return true if healthy + bool healthy() const override; + + // momentary switch to change camera between picture and video modes + void cam_mode_toggle() override; + + // entry point to actually take a picture. returns true on success + bool trigger_pic() override; + + // send camera information message to GCS + void send_camera_information(mavlink_channel_t chan) const override; + + // send camera settings message to GCS + void send_camera_settings(mavlink_channel_t chan) const override; + // initialize the RunCam driver - void init(); + void init() override; // camera button simulation bool simulate_camera_button(const ControlOperation operation, const uint32_t transition_timeout = RUNCAM_DEFAULT_BUTTON_PRESS_DELAY); // start the video void start_recording(); // stop the video void stop_recording(); + // start or stop video recording. returns true on success + // set start_recording = true to start record, false to stop recording + bool record_video(bool _start_recording) override { + if (_start_recording) { + start_recording(); + } else { + stop_recording(); + } + return true; + } + // enter the OSD menu void enter_osd(); // exit the OSD menu void exit_osd(); // OSD control determined by camera options void osd_option(); - // update loop - void update(); + // update - should be called at 50hz + void update() override; // Check whether arming is allowed bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; @@ -265,6 +294,10 @@ class AP_RunCam static uint8_t _sub_menu_lengths[RUNCAM_NUM_SUB_MENUS]; // shared inbound scratch space uint8_t _recv_buf[RUNCAM_MAX_PACKET_SIZE]; // all the response contexts use same recv buffer + // the runcam instance + uint8_t _runcam_instance; + + static const char* _models[RUNCAM_MAX_DEVICE_TYPES]; class Request; @@ -435,4 +468,4 @@ namespace AP AP_RunCam *runcam(); }; -#endif // HAL_RUNCAM_ENABLED +#endif // AP_CAMERA_RUNCAM_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 0a7a81689b802a..e5404898d9bb90 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -1452,7 +1452,7 @@ void AP_DDS_Client::write_time_topic() const bool success = builtin_interfaces_msg_Time_serialize_topic(&ub, &time_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: XRCE_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: XRCE_Client failed to serialize"); } } } @@ -1468,7 +1468,7 @@ void AP_DDS_Client::write_nav_sat_fix_topic() const bool success = sensor_msgs_msg_NavSatFix_serialize_topic(&ub, &nav_sat_fix_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1485,7 +1485,7 @@ void AP_DDS_Client::write_static_transforms() const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &tx_static_transforms_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1502,7 +1502,7 @@ void AP_DDS_Client::write_battery_state_topic() const bool success = sensor_msgs_msg_BatteryState_serialize_topic(&ub, &battery_state_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1519,7 +1519,7 @@ void AP_DDS_Client::write_local_pose_topic() const bool success = geometry_msgs_msg_PoseStamped_serialize_topic(&ub, &local_pose_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1536,7 +1536,7 @@ void AP_DDS_Client::write_tx_local_velocity_topic() const bool success = geometry_msgs_msg_TwistStamped_serialize_topic(&ub, &tx_local_velocity_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1552,7 +1552,7 @@ void AP_DDS_Client::write_tx_local_airspeed_topic() const bool success = geometry_msgs_msg_Vector3Stamped_serialize_topic(&ub, &tx_local_airspeed_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1568,7 +1568,7 @@ void AP_DDS_Client::write_imu_topic() const bool success = sensor_msgs_msg_Imu_serialize_topic(&ub, &imu_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1585,7 +1585,7 @@ void AP_DDS_Client::write_geo_pose_topic() const bool success = geographic_msgs_msg_GeoPoseStamped_serialize_topic(&ub, &geo_pose_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1602,7 +1602,7 @@ void AP_DDS_Client::write_clock_topic() const bool success = rosgraph_msgs_msg_Clock_serialize_topic(&ub, &clock_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1618,7 +1618,7 @@ void AP_DDS_Client::write_gps_global_origin_topic() uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GPS_GLOBAL_ORIGIN_PUB)].dw_id, &ub, topic_size); const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &gps_global_origin_topic); if (!success) { - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1634,7 +1634,7 @@ void AP_DDS_Client::write_goal_topic() uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size); const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic); if (!success) { - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } @@ -1651,7 +1651,7 @@ void AP_DDS_Client::write_status_topic() const bool success = ardupilot_msgs_msg_Status_serialize_topic(&ub, &status_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: DDS_Client failed to serialize"); } } } diff --git a/libraries/AP_DDS/AP_DDS_config.h b/libraries/AP_DDS/AP_DDS_config.h index 022bce3d39dc69..3a68a349d5a3c8 100644 --- a/libraries/AP_DDS/AP_DDS_config.h +++ b/libraries/AP_DDS/AP_DDS_config.h @@ -161,7 +161,7 @@ #ifndef AP_DDS_DEFAULT_UDP_IP_ADDR #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.13.2" +#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.144.2" #else #define AP_DDS_DEFAULT_UDP_IP_ADDR "127.0.0.1" #endif diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 58cdc7e1b6a0c9..c0707ebe79d8c5 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -1953,6 +1953,24 @@ void AP_DroneCAN::logging(void) return; } const auto &s = *stats; + +// @LoggerMessage: CANS +// @Description: CAN Bus Statistics +// @Field: TimeUS: Time since system startup +// @Field: I: driver index +// @Field: T: transmit success count +// @Field: Trq: transmit request count +// @Field: Trej: transmit reject count +// @Field: Tov: transmit overflow count +// @Field: Tto: transmit timeout count +// @Field: Tab: transmit abort count +// @Field: R: receive count +// @Field: Rov: receive overflow count +// @Field: Rer: receive error count +// @Field: Bo: bus offset error count +// @Field: Etx: ESC successful send count +// @Field: Stx: Servo successful send count +// @Field: Ftx: ESC/Servo failed-to-send count AP::logger().WriteStreaming("CANS", "TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx", "s#-------------", diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 17f62791526b91..f4ff9dea05a224 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -758,52 +758,52 @@ void AP_ESC_Telem::update() telemdata.power_percentage); } #endif //AP_EXTENDED_ESC_TELEM_ENABLED - } #if AP_EXTENDED_DSHOT_TELEM_V2_ENABLED - // Write an EDTv2 message, if there is any update - uint16_t edt2_status = telemdata.edt2_status; - uint16_t edt2_stress = telemdata.edt2_stress; - if (EDT2_HAS_NEW_DATA(edt2_status | edt2_stress)) { - // Could probably be faster/smaller with bitmasking, but not sure - uint8_t status = 0; - if (EDT2_HAS_NEW_DATA(edt2_stress)) { - status |= uint8_t(log_Edt2_Status::HAS_STRESS_DATA); - } - if (EDT2_HAS_NEW_DATA(edt2_status)) { - status |= uint8_t(log_Edt2_Status::HAS_STATUS_DATA); - } - if (EDT2_ALERT_BIT_FROM_STATUS(edt2_status)) { - status |= uint8_t(log_Edt2_Status::ALERT_BIT); - } - if (EDT2_WARNING_BIT_FROM_STATUS(edt2_status)) { - status |= uint8_t(log_Edt2_Status::WARNING_BIT); - } - if (EDT2_ERROR_BIT_FROM_STATUS(edt2_status)) { - status |= uint8_t(log_Edt2_Status::ERROR_BIT); - } - // An EDT2 status message is: - // id: starts from 0 - // stress: the current stress which comes from edt2_stress - // max_stress: the maximum stress which comes from edt2_status - // status: the status bits which come from both - const struct log_Edt2 pkt_edt2{ - LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)), - time_us : now_us64, - instance : i, - stress : EDT2_STRESS_FROM_STRESS(edt2_stress), - max_stress : EDT2_STRESS_FROM_STATUS(edt2_status), - status : status, - }; - if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) { - // Only clean the telem_updated bits if the write succeeded. - // This is important because, if rate limiting is enabled, - // the log-on-change behavior may lose a lot of entries - telemdata.edt2_status &= ~EDT2_TELEM_UPDATED; - telemdata.edt2_stress &= ~EDT2_TELEM_UPDATED; + // Write an EDTv2 message, if there is any update + uint16_t edt2_status = telemdata.edt2_status; + uint16_t edt2_stress = telemdata.edt2_stress; + if (EDT2_HAS_NEW_DATA(edt2_status | edt2_stress)) { + // Could probably be faster/smaller with bitmasking, but not sure + uint8_t status = 0; + if (EDT2_HAS_NEW_DATA(edt2_stress)) { + status |= uint8_t(log_Edt2_Status::HAS_STRESS_DATA); + } + if (EDT2_HAS_NEW_DATA(edt2_status)) { + status |= uint8_t(log_Edt2_Status::HAS_STATUS_DATA); + } + if (EDT2_ALERT_BIT_FROM_STATUS(edt2_status)) { + status |= uint8_t(log_Edt2_Status::ALERT_BIT); + } + if (EDT2_WARNING_BIT_FROM_STATUS(edt2_status)) { + status |= uint8_t(log_Edt2_Status::WARNING_BIT); + } + if (EDT2_ERROR_BIT_FROM_STATUS(edt2_status)) { + status |= uint8_t(log_Edt2_Status::ERROR_BIT); + } + // An EDT2 status message is: + // id: starts from 0 + // stress: the current stress which comes from edt2_stress + // max_stress: the maximum stress which comes from edt2_status + // status: the status bits which come from both + const struct log_Edt2 pkt_edt2{ + LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)), + time_us : now_us64, + instance : i, + stress : EDT2_STRESS_FROM_STRESS(edt2_stress), + max_stress : EDT2_STRESS_FROM_STATUS(edt2_status), + status : status, + }; + if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) { + // Only clean the telem_updated bits if the write succeeded. + // This is important because, if rate limiting is enabled, + // the log-on-change behavior may lose a lot of entries + telemdata.edt2_status &= ~EDT2_TELEM_UPDATED; + telemdata.edt2_stress &= ~EDT2_TELEM_UPDATED; + } } - } #endif // AP_EXTENDED_DSHOT_TELEM_V2_ENABLED + } } } #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_Filesystem/README.md b/libraries/AP_Filesystem/README.md index cad4d76fc6637c..22d9c2654bb828 100644 --- a/libraries/AP_Filesystem/README.md +++ b/libraries/AP_Filesystem/README.md @@ -70,11 +70,12 @@ parameter. The format is: ``` uint8_t type:4; // AP_Param type NONE=0, INT8=1, INT16=2, INT32=3, FLOAT=4 - uint8_t flags:4; // for future use + uint8_t flags:4; // bit 0: default value included, bits 1-3: for future use uint8_t common_len:4; // number of name bytes in common with previous entry, 0..15 uint8_t name_len:4; // non-common length of param name -1 (0..15) uint8_t name[name_len]; // name uint8_t data[]; // value, length given by variable type + uint8_t default[]; // optional default value, included if flags bit 0 is set ``` There may be any number of leading zero pad bytes before the start of @@ -91,6 +92,10 @@ file. The name_len field is the number of non-common characters, minus one. +The default value is included only if requested by the withdefaults +query string and if different from the set value. Otherwise, flags +bit 0 will not be set, and default will be of zero length. + ### Query Strings The file name @PARAM/param.pck can optionally be extended with query @@ -101,6 +106,11 @@ string elements to change the result. For example: that means to download 10 parameters starting with parameter number 50. + - @PARAM/param.pck?withdefaults=1 + +that means to include the default values in the returned data, where +it is different from the parameter's set value. + ### Parameter Client Examples The script Tools/scripts/param_unpack.py can be used to unpack a diff --git a/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp b/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp index f617b4850a92af..fe76f284297e28 100644 --- a/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp +++ b/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp @@ -46,17 +46,17 @@ class FlashTest : public AP_HAL::HAL::Callbacks { bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) { if (sector > 1) { - AP_HAL::panic("FATAL: write to sector %u\n", (unsigned)sector); + AP_HAL::panic("FATAL: write to sector %u", (unsigned)sector); } if (offset + length > flash_sector_size) { - AP_HAL::panic("FATAL: write to sector %u at offset %u length %u\n", + AP_HAL::panic("FATAL: write to sector %u at offset %u length %u", (unsigned)sector, (unsigned)offset, (unsigned)length); } uint8_t *b = &flash[sector][offset]; if ((offset & 1) || (length & 1)) { - AP_HAL::panic("FATAL: invalid write at %u:%u len=%u\n", + AP_HAL::panic("FATAL: invalid write at %u:%u len=%u", (unsigned)sector, (unsigned)offset, (unsigned)length); @@ -66,7 +66,7 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data const uint16_t v = le16toh_ptr(&data[i*2]); uint16_t v2 = le16toh_ptr(&b[i*2]); if (v & !v2) { - AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n", + AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x", (unsigned)sector, unsigned(offset+i), b[i], @@ -74,7 +74,7 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data } #ifndef AP_FLASHSTORAGE_MULTI_WRITE if (v != v2 && v != 0xFFFF && v2 != 0xFFFF) { - AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n", + AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x", (unsigned)sector, unsigned(offset+i), b[i], @@ -90,10 +90,10 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data bool FlashTest::flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length) { if (sector > 1) { - AP_HAL::panic("FATAL: read from sector %u\n", (unsigned)sector); + AP_HAL::panic("FATAL: read from sector %u", (unsigned)sector); } if (offset + length > flash_sector_size) { - AP_HAL::panic("FATAL: read from sector %u at offset %u length %u\n", + AP_HAL::panic("FATAL: read from sector %u at offset %u length %u", (unsigned)sector, (unsigned)offset, (unsigned)length); @@ -105,7 +105,7 @@ bool FlashTest::flash_read(uint8_t sector, uint32_t offset, uint8_t *data, uint1 bool FlashTest::flash_erase(uint8_t sector) { if (sector > 1) { - AP_HAL::panic("FATAL: erase sector %u\n", (unsigned)sector); + AP_HAL::panic("FATAL: erase sector %u", (unsigned)sector); } memset(&flash[sector][0], 0xFF, flash_sector_size); return true; diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 79a69b88b29621..4d6f1e164ccc96 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1619,7 +1619,7 @@ void AP_GPS::handle_gps_rtcm_data(mavlink_channel_t chan, const mavlink_message_ */ bool AP_GPS::parse_rtcm_injection(mavlink_channel_t chan, const mavlink_gps_rtcm_data_t &pkt) { - if (chan >= MAVLINK_COMM_NUM_BUFFERS) { + if (static_cast(chan) >= MAVLINK_COMM_NUM_BUFFERS) { return false; } if (rtcm.parsers[chan] == nullptr) { diff --git a/libraries/AP_Generator/AP_Generator.cpp b/libraries/AP_Generator/AP_Generator.cpp index f1906bdc8fb503..7cac48cf786f0b 100644 --- a/libraries/AP_Generator/AP_Generator.cpp +++ b/libraries/AP_Generator/AP_Generator.cpp @@ -13,6 +13,8 @@ along with this program. If not, see . */ +#pragma GCC optimize("Os") + #include "AP_Generator.h" #if HAL_GENERATOR_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_Backend.cpp b/libraries/AP_Generator/AP_Generator_Backend.cpp index 6a74ee53e2c245..c3405ceec03342 100644 --- a/libraries/AP_Generator/AP_Generator_Backend.cpp +++ b/libraries/AP_Generator/AP_Generator_Backend.cpp @@ -12,6 +12,8 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ +#pragma GCC optimize("Os") + #include "AP_Generator_Backend.h" #if HAL_GENERATOR_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.cpp b/libraries/AP_Generator/AP_Generator_IE_2400.cpp index 26f2bf35b660a4..7c0a5f05e45c00 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_2400.cpp @@ -13,6 +13,8 @@ along with this program. If not, see . */ +#pragma GCC optimize("Os") + #include "AP_Generator_IE_2400.h" #if AP_GENERATOR_IE_2400_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp index c0e9aae3a2a95b..c1a78e8a6f5935 100644 --- a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp @@ -13,6 +13,8 @@ along with this program. If not, see . */ +#pragma GCC optimize("Os") + #include "AP_Generator_IE_650_800.h" #if AP_GENERATOR_IE_650_800_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp index 7a13bed22e5742..4af0f8cbd84a1f 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp @@ -13,6 +13,8 @@ along with this program. If not, see . */ +#pragma GCC optimize("Os") + #include "AP_Generator_IE_FuelCell.h" #if AP_GENERATOR_IE_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.cpp b/libraries/AP_Generator/AP_Generator_RichenPower.cpp index 21750784e51a63..d5e0337c5f0f2a 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.cpp +++ b/libraries/AP_Generator/AP_Generator_RichenPower.cpp @@ -13,6 +13,8 @@ along with this program. If not, see . */ +#pragma GCC optimize("Os") + #include "AP_Generator_config.h" #if AP_GENERATOR_RICHENPOWER_ENABLED diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index b92274a6a5d516..629432bd1fac11 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -273,8 +273,8 @@ class AP_HAL::CANIface #ifndef HAL_BOOTLOADER_BUILD HAL_Semaphore sem; #endif - // allow up to 2 callbacks per interface - FrameCb cb[2]; + // allow up to 3 callbacks per interface + FrameCb cb[3]; } callbacks; uint32_t bitrate_; diff --git a/libraries/AP_HAL/DSP.cpp b/libraries/AP_HAL/DSP.cpp index 10b33e38d07ef5..932149d9726c7c 100644 --- a/libraries/AP_HAL/DSP.cpp +++ b/libraries/AP_HAL/DSP.cpp @@ -286,6 +286,11 @@ float DSP::calculate_jains_estimator(const FFTWindowState* fft, const float* rea float y1 = real_fft[k_max-1]; float y2 = real_fft[k_max]; float y3 = real_fft[k_max+1]; + + if (is_zero(y2) || is_zero(y1)) { + return 0.0f; + } + float d = 0.0f; if (y1 > y3) { diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 5d4274ac706a4e..96fb4e1b8870f2 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -1329,9 +1329,6 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) */ void RCOutput::cork(void) { - if (corked) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } corked = true; #if HAL_WITH_IO_MCU if (iomcu_enabled) { diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index b8c995a4b2b9c9..4bc97e1414b443 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -191,7 +191,7 @@ void UARTDriver::thread_rx_init(void) uart_rx_thread, nullptr); if (uart_rx_thread_ctx == nullptr) { - AP_HAL::panic("Could not create UART RX thread\n"); + AP_HAL::panic("Could not create UART RX thread"); } } } @@ -209,7 +209,7 @@ void UARTDriver::thread_init(void) uart_thread_trampoline, this); if (uart_thread_ctx == nullptr) { - AP_HAL::panic("Could not create UART TX thread\n"); + AP_HAL::panic("Could not create UART TX thread"); } } } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/AEROFOX-H7_IMG.png b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/AEROFOX-H7_IMG.png new file mode 100644 index 00000000000000..221e5db5988ad4 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/AEROFOX-H7_IMG.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/AEROFOX-H7_pinout.png b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/AEROFOX-H7_pinout.png new file mode 100644 index 00000000000000..7aa7f573a4da70 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/AEROFOX-H7_pinout.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/README.md b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/README.md new file mode 100644 index 00000000000000..5f5b4dbc24b255 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/README.md @@ -0,0 +1,101 @@ +# AEROFOX-H7 Flight Controller + +The AEROFOX-H7 is a flight controller produced by AEROFOX(http://aerofox.cn) + + + +## Features + Processor + STM32H743 + + Sensors + ADIS16470 (appears in the advanced version) + ICM45686 (appears in the advanced version) + ICM42688 + QMC5883L + SPL06-001 + + Power + 2S-12S (MAX60V) Lipo input voltage + 5V BEC for system power supply( 5V peripheral current limit 1.2A) + 5V/12V BEC for VTX( Current limit 2.5A, need strong heat dissipation) + Dual power automatic switching and condition monitoring + + Interfaces + 16x PWM output + 7x UARTs for RC, TELEM, GPS and other peripherals + 2x I2C ports for external compass, airspeed, baro + 2x CAN port + 4x Relay output + 4x ADC input + + FPC connector + The connector includes an SPI, an I2C, an PWM IMU heating control pin. + +## Pinout + + +## UART Mapping + +All UARTs, except UART1, are DMA enabled. UART corresponding to each SERIAL port, and its default protocol, are shown below: +- SERIAL0 -> USB (MAVLink2) +- SERIAL1 -> UART7 (ESC Telemetry) +- SERIAL2 -> UART4 (User configured) +- SERIAL3 -> UART5 (User configured) +- SERIAL4 -> USART2 (User configured) +- SERIAL5 -> USART1 (GPS) +- SERIAL6 -> UART8 (RCIN) +- SERIAL7 -> USART3 (MAVLink2) + +## RC Input + +SERIAL 6 is configured for RC input by default and is compatible with all ArduPilot supported protocols except PPM. For protocols requiring half-duplex serial to transmit telemetry (such as FPort) you should set SERAIL6_OPTIONS = 4 (Half-Duplex) + +## PWM Output + +The AEROFOXH7 support up to 16PWM outputs. All pins support DShot. Outputs 1-8 support bi-directional DShot. + +The 16 PWM outputs are in 9 groups: + +- PWM 1,2 in group1 +- PWM 3,4 in group2 +- PWM 5,6 in group3 +- PWM 7,8 in group4 +- PWM 9,10 in group5 +- PWM 11 in group6 +- PWM 12 in group7 +- PWM 13,14 in group8 +- PWM 15,16 in group9 + +Channels within the same group need to use the same output rate. If any channel in a group uses DShot, then all channels in that group need to use DShot. + +## Battery Monitoring + +The board has a builting voltage and current sensor. The voltage sensor can handle up +to 12S LiPo batteries. + +### The power A is onboard voltage sensor +It is enabled by default and has the following parameters set by default:s +- BATT_MONITOR 4 +- BATT_VOLT_PIN 19 +- BATT_CURR_PIN 9 +- BATT_VOLT_MULT 21 +- BATT_AMP_PERVL 40 + +### The power B is external PMU input +An additional power monitor input is provided and can be enabled by setting: +- BATT_MONITOR 4, then reboot and set the following: +- BATT_VOLT_PIN 10 +- BATT_CURR_PIN 11 +- BATT_VOLT_MULT 34 +- BATT_AMP_PERVLT should be set as required by the specific monitor used + +## Compass + +A 5883L compass is installed inside the H7 flight control. When high current devices such as ESC and BEC are installed under the flight control board, the on-board compass is usually disabled and an external compass used mounted to minimize motor current effects. + +## 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. The +hardware also supports the PX4 Betaflight INAV firmware, which needs to be changed with STlink. + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/hwdef-bl.dat new file mode 100644 index 00000000000000..fca224f35da435 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/hwdef-bl.dat @@ -0,0 +1,53 @@ +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 24000000 + +# board ID for firmware load +APJ_BOARD_ID AP_HW_AEROFOX_H7 + +FLASH_SIZE_KB 2048 + +# bootloader is installed 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 + +#define LED pins +PE14 LED_BOOTLOADER OUTPUT OPENDRAIN HIGH # red +PE12 LED_ACTIVITY OUTPUT OPENDRAIN HIGH # green +PE13 LED_RED OUTPUT OPENDRAIN HIGH # blue + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART1 + +# UART1 is debug +PA9 USART1_TX USART1 NODMA +PA10 USART1_RX USART1 NODMA + +PC13 VDD_3V3_SENSORS_EN OUTPUT LOW OPENDRAIN +PC4 nVDD_5V_PERIPH_EN OUTPUT HIGH OPENDRAIN +PE8 nVDD_12V_PERIPH_EN OUTPUT LOW OPENDRAIN + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define HAL_USE_EMPTY_STORAGE 1 +define HAL_STORAGE_SIZE 32768 +define HAL_LED_ON 1 + +# define BOOTLOADER_DEBUG SD7 + +# Add CS pins to ensure they are high in bootloader +PD7 16470_CS CS +PD4 42688_1_CS CS +PC15 42688_2_CS CS + +PD10 FRAM_CS CS SPEED_VERYLOW +PC14 FLASH_CS CS \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/hwdef.dat new file mode 100644 index 00000000000000..fde69f988ab9d4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AEROFOX-H7/hwdef.dat @@ -0,0 +1,238 @@ +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 24000000 + +# board ID for firmware load +APJ_BOARD_ID AP_HW_AEROFOX_H7 + +FLASH_SIZE_KB 2048 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 128 + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# supports upto 8MBits/s +CANFD_SUPPORTED 8 + +# with 2M flash we can afford to optimize for speed +env OPTIMIZE -O2 + +# system timer +STM32_ST_USE_TIMER 5 + +# UART +SERIAL_ORDER OTG1 UART7 UART4 UART5 USART2 USART1 UART8 USART3 OTG2 + +#serial1 +PA15 UART7_TX UART7 +PA8 UART7_RX UART7 +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_ESCTelemetry + +# serial2 +PC10 UART4_TX UART4 +PC11 UART4_RX UART4 +define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_None + +# serial3 +PC12 UART5_TX UART5 +PD2 UART5_RX UART5 +define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_None + +# serial4 +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 +define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_None + +# serial5 +PA9 USART1_TX USART1 NODMA +PA10 USART1_RX USART1 NODMA +define DEFAULT_SERIAL5_PROTOCOL SerialProtocol_GPS + +# serial6 +PE1 UART8_TX UART8 +PE0 UART8_RX UART8 +define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_RCIN + +# serial7 +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_MAVLink2 + +# default the 2nd interface to MAVLink2 +define HAL_OTG2_PROTOCOL SerialProtocol_MAVLink2 + +# CAN +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 + +# I2C +I2C_ORDER I2C1 I2C2 + +PB6 I2C1_SCL I2C1 +PB7 I2C1_SDA I2C1 + +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +# SPI +PB3 SPI1_SCK SPI1 +PB4 SPI1_MISO SPI1 +PB5 SPI1_MOSI SPI1 + +PD3 SPI2_SCK SPI2 +PC2 SPI2_MISO SPI2 +PC3 SPI2_MOSI SPI2 + +PE2 SPI4_SCK SPI4 +PE5 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 + +# IMU +IMU ADIS1647x SPI:16470 ROTATION_PITCH_180 ADIS_DRDY_PIN +IMU Invensensev3 SPI:42688_1 ROTATION_NONE +IMU Invensensev3 SPI:42688_2 ROTATION_PITCH_180_YAW_270 + +SPIDEV 16470 SPI1 DEVID1 16470_CS MODE3 1*MHZ 2*MHZ +SPIDEV 42688_1 SPI1 DEVID2 42688_1_CS MODE3 2*MHZ 16*MHZ +SPIDEV 42688_2 SPI4 DEVID3 42688_2_CS MODE3 2*MHZ 16*MHZ + +SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ +SPIDEV sdcard SPI2 DEVID2 FLASH_CS MODE0 1*MHZ 8*MHZ + +PD7 16470_CS CS +PD4 42688_1_CS CS +PC15 42688_2_CS CS + +PD10 FRAM_CS CS SPEED_VERYLOW +PC14 FLASH_CS CS + +PE7 DRDY1_ADIS16470 INPUT GPIO(90) +define ADIS_DRDY_PIN 90 + +# ADC +PA5 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PB0 BATT_CURRENT_SENS ADC1 SCALE(1) + +PC0 BATT2_VOLTAGE_SENS ADC1 SCALE(1) +PC1 BATT2_CURRENT_SENS ADC1 SCALE(1) + +PB1 FMU_SERVORAIL_VCC_SENS ADC1 SCALE(12) +PA4 VDD_5V_SENS ADC1 SCALE(2) + +# GPIO +PA0 TIM2_CH1 TIM2 PWM(1) GPIO(50) BIDIR +PA1 TIM2_CH2 TIM2 PWM(2) GPIO(51) +PE9 TIM1_CH1 TIM1 PWM(3) GPIO(52) BIDIR +PE11 TIM1_CH2 TIM1 PWM(4) GPIO(53) +PD12 TIM4_CH1 TIM4 PWM(5) GPIO(54) BIDIR +PD13 TIM4_CH2 TIM4 PWM(6) GPIO(55) +PC6 TIM3_CH1 TIM3 PWM(7) GPIO(56) BIDIR +PC7 TIM3_CH2 TIM3 PWM(8) GPIO(57) + +PA2 TIM15_CH1 TIM15 PWM(9) GPIO(58) +PA3 TIM15_CH2 TIM15 PWM(10) GPIO(59) +PA7 TIM14_CH1 TIM14 PWM(11) GPIO(60) +PA6 TIM13_CH1 TIM13 PWM(12) GPIO(61) +PC9 TIM8_CH4 TIM8 PWM(13) GPIO(62) +PC8 TIM8_CH3 TIM8 PWM(14) GPIO(63) +PB15 TIM12_CH2 TIM12 PWM(15) GPIO(64) +PB14 TIM12_CH1 TIM12 PWM(16) GPIO(65) + +PB9 TIM17_CH1 TIM17 ALARM + +PE3 EXTERN_GPIO1 OUTPUT GPIO(1) +PE4 EXTERN_GPIO2 OUTPUT GPIO(2) +PD11 EXTERN_GPIO3 OUTPUT GPIO(3) +#PD14 EXTERN_GPIO4 OUTPUT GPIO(4) + +PD14 LED_SAFETY OUTPUT +PD15 SAFETY_IN INPUT PULLDOWN + +PC13 VDD_3V3_SENSORS_EN OUTPUT LOW OPENDRAIN +PC4 nVDD_5V_PERIPH_EN OUTPUT LOW OPENDRAIN +PE8 nVDD_12V_PERIPH_EN OUTPUT HIGH OPENDRAIN + +PE10 VDD_5V_HIPOWER_oOC INPUT PULLUP +PC5 VDD_BRICK_nVALID INPUT PULLUP + +# I2C BMP280 or SPL06 +BARO SPL06 I2C:0:0x76 +BARO BMP280 I2C:0:0x76 + +# COMPASS +COMPASS QMC5883L I2C:0:0x0D false ROTATION_NONE + +# LED setup is similar to PixRacer + +define AP_NOTIFY_GPIO_LED_RGB_ENABLED 1 +PE14 LED_RED OUTPUT GPIO(10) +PE12 LED_GREEN OUTPUT GPIO(11) +PE13 LED_BLUE OUTPUT GPIO(12) + +define AP_NOTIFY_GPIO_LED_RGB_RED_PIN 10 +define AP_NOTIFY_GPIO_LED_RGB_GREEN_PIN 11 +define AP_NOTIFY_GPIO_LED_RGB_BLUE_PIN 12 + + +# IMU heater +PB8 HEATER_EN OUTPUT LOW GPIO(80) +define HAL_HEATER_GPIO_PIN 80 +define HAL_HAVE_IMU_HEATER 1 +define HAL_IMU_TEMP_DEFAULT 40 +define HAL_IMUHEAT_P_DEFAULT 50 +define HAL_IMUHEAT_I_DEFAULT 0.07 +define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 40 + +# compass +define HAL_PROBE_EXTERNAL_I2C_COMPASSES + +define HAL_SKIP_AUTO_INTERNAL_I2C_PROBE + +define HAL_I2C_INTERNAL_MASK 1 + +# SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define HAL_CHIBIOS_ARCH_FMUV3 1 + +define BOARD_TYPE_DEFAULT 3 + +# safety switch +define HAL_HAVE_SAFETY_SWITCH 1 + +# FRAM +define HAL_WITH_RAMTRON 1 +define HAL_STORAGE_SIZE 32768 + +# open all IMUs +define HAL_EKF_IMU_MASK_DEFAULT 7 + +# IMU fast sample +define HAL_DEFAULT_INS_FAST_SAMPLE 7 + +# Enable FAT +define HAL_OS_FATFS_IO 1 + +# DMA +DMA_NOSHARE SPI2* UART2* UART4* UART5* + +ENABLE_DFU_BOOT 1 + +define HAL_BATT_MONITOR 4 +define HAL_BATT_VOLT_PIN 19 +define HAL_BATT_CURR_PIN 9 +define HAL_BATT_VOLT_SCALE 21 +define HAL_BATT_CURR_SCALE 40 + +define HAL_BATT2_VOLT_PIN 10 +define HAL_BATT2_CURR_PIN 11 +define HAL_BATT2_VOLT_SCALE 34 +define HAL_BATT2_CURR_SCALE 40 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/README.md b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/README.md new file mode 100644 index 00000000000000..171554ad32dd23 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/README.md @@ -0,0 +1,99 @@ +# GEPRC TAKER H743 BT Flight Controller + +The TAKER H743 BT is a flight controller produced by [GEPRC](https://geprc.com/). + +## Features + + - STM32H743 microcontroller + - MPU6000+ICM42688 dual IMU + - SPL06-001 barometer + - microSD based 512MB flash logging + - AT7456E OSD + - 7 UARTs + - 8 PWM outputs + +## Pinout + +![TAKER H743 BT Board](TAKER_H743_BT_Board_Top.jpg "GEPRC_TAKER_H743") +![TAKER H743 BT Board](TAKER_H743_BT_Board_Bottom.jpg "GEPRC_TAKER_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. + + - SERIAL0 -> USB + - SERIAL1 -> UART1 (DisplayPort, DMA-enabled) + - SERIAL2 -> UART2 (RCIN, DMA-enabled) + - SERIAL3 -> UART3 (connected to internal BT module, not currently usable by ArduPilot) + - SERIAL4 -> UART4 (GPS) + - SERIAL6 -> UART6 (User) + - SERIAL7 -> UART7 (User) + - SERIAL8 -> UART8 (ESC Telemetry) + +## RC Input + +RC input is configured by default via the USAR2 RX input. It supports all unidirectional RC protocols except PPM. FPort and full duplex protocols, like CRSF/ELRS, will need to use TX2 also. + +Note: +If the receiver is FPort or a full duplex protocol, then the receiver must be tied to the USART2 TX pin and [SERIAL2_OPTIONS](https://ardupilot.org/copter/docs/parameters.html#serial2-options) = 7 (invert TX/RX, half duplex), and [RSSI_TYPE](https://ardupilot.org/copter/docs/parameters.html#rssi-type) =3. + +## FrSky Telemetry + +FrSky Telemetry is supported using the Tx pin of any UART excluding SERIAL3UART3. To enable support for FrSky S.PORT (the example shows SERIAL6), you need to set the following parameters. + + - SERIAL6_PROTOCOL 10 + - SERIAL6_OPTIONS 7 + +## OSD Support + +The TAKER H743 BT supports analog OSD using its internal OSD chip and simultaneously HD goggle DisplayPort OSDs via the HD VTX connector. + +## VTX Support + +The SH1.0-6P connector supports a standard DJI HD VTX connection. Pin 1 of the connector is 12v (or VBAT by solder pad selection) so be careful not to connect to devices expecting 5v. + +## PWM Output + +The TAKER H743 BT supports up to 9 PWM/DShot outputs. The pads for motor output +M1 to M4 are on the esc connector, M5-M8 are solder pads, plus M9 is defaulted for serial LED strip or can be used as another PWM output. + +The PWM is in 4 groups: + + - PWM 1-4 in group1 + - PWM 5-6 in group2 + - PWM 7-8 in group3 + - PWM 9 in group4 + +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-8 support bi-directional DShot. + +## Battery Monitoring + +The board has a internal voltage sensor and connections on the ESC connector for an external current sensor input. +The voltage sensor can handle up to 6S. +LiPo batteries. + +The default battery parameters are: + + - BATT_MONITOR 4 + - BATT_VOLT_PIN 13 + - BATT_VOLT_SCALE 11.1 + - BATT_CURR_PIN 12 + - BATT_CURR_SCALE 28.5 + +## Compass + +The TAKER H743 BT does not have a builtin compass, but you can attach an external compass using I2C on the SDA and SCL pads. + +## 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/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Bottom.jpg b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Bottom.jpg new file mode 100644 index 00000000000000..e25a71a2bd56c0 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Bottom.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Top.jpg b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Top.jpg new file mode 100644 index 00000000000000..6c37cf13a78da3 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/TAKER_H743_BT_Board_Top.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/defaults.parm new file mode 100644 index 00000000000000..6455bd215b277c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/defaults.parm @@ -0,0 +1,4 @@ +# setup for LEDs on chan9 +SERVO9_FUNCTION 120 + +OSD_TYPE2 5 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef-bl.dat new file mode 100644 index 00000000000000..8987e979a8cf5f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef-bl.dat @@ -0,0 +1,54 @@ +# hw definition file for processing by chibios_pins.py +# for GEPRFH743-BT-HD bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_GEPRC_TAKER_H743 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +define STM32_LSECLK 32768U +define STM32_LSEDRV (3U << 3U) + +FLASH_SIZE_KB 2048 + +# bootloader starts at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +FLASH_BOOTLOADER_LOAD_KB 384 + + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 + +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +PD2 BUZZER OUTPUT LOW PULLDOWN + +PC13 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 0 + +# Motors for esc init +PB0 PWMOUT1 OUTPUT LOW +PB1 PWMOUT2 OUTPUT LOW +PB5 PWMOUT3 OUTPUT LOW +PB4 PWMOUT4 OUTPUT LOW +PD12 PWMOUT5 OUTPUT LOW +PD13 PWMOUT6 OUTPUT LOW +PC8 PWMOUT7 OUTPUT LOW +PC9 PWMOUT8 OUTPUT LOW + +# Add CS pins to ensure they are high in bootloader +PA15 SDCARD_CS CS +PE4 MAX7456_CS CS +PA4 MPU6000_CS CS +PB12 ICM42605_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef.dat new file mode 100644 index 00000000000000..2ddc1245395eb7 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/GEPRC_TAKER_H743/hwdef.dat @@ -0,0 +1,182 @@ +# hw definition file for processing by chibios_pins.py +# for TAKER H743 BT hardware. +# thanks to betaflight for pin information + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_GEPRC_TAKER_H743 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 +MCU_CLOCKRATE_MHZ 480 + +FLASH_SIZE_KB 2048 + +# leave 2 sectors free +FLASH_RESERVE_START_KB 384 + + +# only one I2C bus +I2C_ORDER I2C1 + +# I2C1 for baro +PB8 I2C1_SCL I2C1 +PB9 I2C1_SDA I2C1 + +# order of UARTs (and USB), +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 EMPTY USART6 UART7 UART8 + +# buzzer +#define HAL_BUZZER_PIN 80 + +# PA10 IO-debug-console +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# SPI1 for MPU6000 +PA4 MPU6000_CS CS +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +# SPI2 for ICM42688 +PB12 ICM42605_CS CS +PB13 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 + +# SPI3 for SDCard +PA15 SDCARD_CS CS +PC10 SPI3_SCK SPI3 +PC11 SPI3_MISO SPI3 +PC12 SPI3_MOSI SPI3 + +# SPI4 for MAX7456 OSD +PE4 MAX7456_CS CS +PE2 SPI4_SCK SPI4 +PE5 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 + +PC3 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC2 BATT_CURRENT_SENS ADC1 SCALE(1) + +# define default battery setup +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 13 +define HAL_BATT_CURR_PIN 12 +define HAL_BATT_VOLT_SCALE 11.13 +define HAL_BATT_CURR_SCALE 28.5 + +PC13 LED0 OUTPUT LOW GPIO(90) # LED +define AP_NOTIFY_GPIO_LED_1_ENABLED 1 +define AP_NOTIFY_GPIO_LED_1_PIN 90 + +# In order to accommodate bi-directional dshot certain devices cannot be DMA enabled +# NODMA indicates these devices, if you remove it they will still not be resolved for DMA + +# USART1 +PA10 USART1_RX USART1 +PA9 USART1_TX USART1 +define DEFAULT_SERIAL1_PROTOCOL SerialProtocol_MSP_DisplayPort + +# USART2 +# RC input defaults to UART to allow for bi-dir dshot +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# USART3 (BT) +PB11 USART3_RX USART3 NODMA +PB10 USART3_TX USART3 NODMA +define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_None + +# UART4 (GPS) +PA0 UART4_TX UART4 +PA1 UART4_RX UART4 + +# UART6 +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 +define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_None + +# UART7 +PE7 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA +define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_None + +# UART8 +PE0 UART8_RX UART8 NODMA +PE1 UART8_TX UART8 NODMA +define DEFAULT_SERIAL8_PROTOCOL SerialProtocol_ESCTelemetry + +# Motors, bi-directional dshot capable +PB0 TIM3_CH3 TIM3 PWM(1) GPIO(50) BIDIR # M1 +PB1 TIM3_CH4 TIM3 PWM(2) GPIO(51) # M2 +PB5 TIM3_CH2 TIM3 PWM(3) GPIO(52) # M3 +PB4 TIM3_CH1 TIM3 PWM(4) GPIO(53) BIDIR # M4 +PD12 TIM4_CH1 TIM4 PWM(5) GPIO(54) BIDIR # M5 +PD13 TIM4_CH2 TIM4 PWM(6) GPIO(55) # M6 +PC8 TIM8_CH3 TIM8 PWM(7) GPIO(56) BIDIR # M7 +PC9 TIM8_CH4 TIM8 PWM(8) GPIO(57) # M8 + +# extra PWM outs +PA8 TIM1_CH1 TIM1 PWM(9) GPIO(58) # led pin +PD2 BUZZER OUTPUT GPIO(80) LOW +define HAL_BUZZER_PIN 80 +define HAL_BUZZER_ON 1 +define HAL_BUZZER_OFF 0 + +DMA_PRIORITY TIM3* TIM4* USART2* +DMA_NOSHARE SPI1* SPI2* + +define HAL_STORAGE_SIZE 16384 +STORAGE_FLASH_PAGE 1 + +# spi devices +SPIDEV mpu6000 SPI1 DEVID1 MPU6000_CS MODE3 1*MHZ 8*MHZ +SPIDEV icm42688 SPI2 DEVID1 ICM42605_CS MODE3 2*MHZ 16*MHZ +SPIDEV sdcard SPI3 DEVID1 SDCARD_CS MODE0 400*KHZ 25*MHZ +SPIDEV osd SPI4 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ + +# IMU setup +IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180 +IMU Invensense SPI:mpu6000 ROTATION_YAW_90 +define HAL_DEFAULT_INS_FAST_SAMPLE 3 + +# 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 0 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + +# one BARO +#BARO BMP280 I2C:0:0x76 +#BARO DPS310 I2C:0:0x76 +BARO SPL06 I2C:0:0x76 + +define HAL_OS_FATFS_IO 1 +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + +# setup for OSD +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 1 +ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin + +#only enable BMP280 Baro +#define AP_BARO_BACKEND_DEFAULT_ENABLED 0 +#undef define AP_BARO_BMP280_ENABLED +#define AP_BARO_BMP280_ENABLED 1 +define AP_BARO_SPL06_ENABLED 1 +define AP_BARO_MS56XX_ENABLED 1 + + + +# need to probe external baros even 'though we're minimised to allow custom build options: +undef AP_BARO_PROBE_EXTERNAL_I2C_BUSES +define AP_BARO_PROBE_EXTERNAL_I2C_BUSES 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat index 895e3703bc86b1..9dd7533f6c2a1a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HolybroG4_GPS/hwdef.dat @@ -74,9 +74,9 @@ define HAL_I2C_INTERNAL_MASK 3 I2C_ORDER I2C1 I2C2 # one SPI bus (for IMU, unused) -#PA5 SPI1_SCK SPI1 -#PA6 SPI1_MISO SPI1 -#PA7 SPI1_MOSI SPI1 +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 # SPI CS PC4 GYR_CS CS @@ -95,6 +95,10 @@ COMPASS IST8310 I2C:0:0x0E false ROTATION_NONE # compass RM3100 on can-F9P-v2 COMPASS RM3100 I2C:0:0x20 false ROTATION_NONE +# baro, disabled by default +BARO ICP201XX I2C:0:0x64 +define AP_PERIPH_BARO_ENABLE_DEFAULT 0 + define HAL_USE_ADC FALSE define STM32_ADC_USE_ADC1 FALSE define HAL_DISABLE_ADC_DRIVER TRUE @@ -130,9 +134,17 @@ DMA_NOSHARE USART3* # add support for moving baseline yaw define GPS_MOVING_BASELINE 1 +SPIDEV icm42688 SPI1 DEVID1 ICM_CS MODE0 24*MHZ 24*MHZ + +IMU Invensensev3 SPI:icm42688 ROTATION_YAW_180 + +define HAL_PERIPH_ENABLE_IMU + # GPS+MAG+LEDs define HAL_PERIPH_ENABLE_GPS define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO +define HAL_PERIPH_ENABLE_IMU define HAL_PERIPH_ENABLE_NOTIFY define HAL_PERIPH_ENABLE_RC_OUT diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat index 5ce9b33c622d03..adddcffdf65057 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteH7/hwdef.dat @@ -130,6 +130,7 @@ DMA_NOSHARE SPI1* # spi devices SPIDEV mpu6000 SPI4 DEVID1 mpu6000_CS MODE3 1*MHZ 4*MHZ SPIDEV icm42688 SPI4 DEVID1 mpu6000_CS MODE3 1*MHZ 16*MHZ +SPIDEV bmi270 SPI4 DEVID1 mpu6000_CS MODE3 1*MHZ 10*MHZ # Clock is 100Mhz so highest clock <= 10Mhz is 100Mhz/16 SPIDEV sdcard SPI1 DEVID1 SDCARD_CS MODE0 400*KHZ 25*MHZ SPIDEV osd SPI2 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ @@ -147,6 +148,7 @@ define HAL_COMPASS_AUTO_ROT_DEFAULT 2 # one IMU IMU Invensense SPI:mpu6000 ROTATION_YAW_180 IMU Invensensev3 SPI:icm42688 ROTATION_ROLL_180_YAW_270 +IMU BMI270 SPI:bmi270 ROTATION_ROLL_180_YAW_270 # one BARO BARO BMP280 I2C:0:0x76 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/OrqaF405Pro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/OrqaF405Pro/hwdef.dat index 0d5a0f9d3f911f..9841d4f9d5bea9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/OrqaF405Pro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/OrqaF405Pro/hwdef.dat @@ -138,6 +138,7 @@ SPIDEV imu1 SPI1 DEVID1 GYRO1_CS MODE3 1*MHZ 8*MHZ SPIDEV imu2 SPI1 DEVID1 GYRO1_CS MODE3 1*MHZ 8*MHZ IMU Invensense SPI:imu1 ROTATION_YAW_90 +IMU Invensensev3 SPI:imu2 ROTATION_PITCH_180 DMA_NOSHARE TIM2_UP TIM3_UP SPI1* DMA_PRIORITY TIM2_UP TIM3_UP SPI1* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat index 46b4ac476a7ef1..48921930f79149 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F405/hwdef.dat @@ -19,9 +19,6 @@ env AP_PERIPH 1 define STM32_ST_USE_TIMER 5 define CH_CFG_ST_RESOLUTION 32 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 16000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat index 840e642e23e056..ec933521d9ff9b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F412/hwdef.dat @@ -20,9 +20,6 @@ env AP_PERIPH 1 STM32_ST_USE_TIMER 5 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 16000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat index 726e7869457d20..c70e2a742581b3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-F9P/hwdef.dat @@ -20,9 +20,6 @@ env AP_PERIPH 1 STM32_ST_USE_TIMER 5 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 16000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h index 384e48c331291f..83b9f8999a138b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32f47_mcuconf.h @@ -158,7 +158,11 @@ #define STM32_PLLI2SQ_VALUE 4 #define STM32_PLLI2SR_VALUE 2 #define STM32_PLLI2SSRC STM32_PLLI2SSRC_PLLSRC +#if defined(STM32F413_MCUCONF) +#define STM32_CK48MSEL STM32_CK48MSEL_PLLI2S +#else #define STM32_CK48MSEL STM32_CK48MSEL_PLLSAI +#endif #elif HAL_EXPECTED_SYSCLOCK == 168000000 // medium frequency variants of F4, such as F405, F427 @@ -539,6 +543,8 @@ #define STM32_IRQ_UART6_PRIORITY 12 #define STM32_IRQ_UART7_PRIORITY 12 #define STM32_IRQ_UART8_PRIORITY 12 +#define STM32_IRQ_UART9_PRIORITY 12 +#define STM32_IRQ_UART10_PRIORITY 12 #define STM32_IRQ_USART1_PRIORITY 12 #define STM32_IRQ_USART2_PRIORITY 12 #define STM32_IRQ_USART3_PRIORITY 12 @@ -547,6 +553,8 @@ #define STM32_IRQ_USART6_PRIORITY 12 #define STM32_IRQ_USART7_PRIORITY 12 #define STM32_IRQ_USART8_PRIORITY 12 +#define STM32_IRQ_USART9_PRIORITY 12 +#define STM32_IRQ_USART10_PRIORITY 12 /* * USB driver system settings. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index 3c6e149ee06ec9..bd666a4957daab 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc @@ -48,6 +48,7 @@ define AP_WINCH_ENABLED 0 define AP_CAMERA_BACKEND_DEFAULT_ENABLED 0 define AP_CAMERA_RELAY_ENABLED AP_CAMERA_ENABLED define AP_CAMERA_SERVO_ENABLED AP_CAMERA_ENABLED +define AP_CAMERA_RUNCAM_ENABLED AP_CAMERA_ENABLED # no SLCAN on these boards (use can-over-mavlink if required) define AP_CAN_SLCAN_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat index 5345cd66178be8..82c206bfa60676 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/mRo-M10095/hwdef.dat @@ -18,9 +18,6 @@ define HAL_STORAGE_SIZE 800 # setup build for a peripheral firmware env AP_PERIPH 1 -# enable watchdog -define HAL_WATCHDOG_ENABLED_DEFAULT true - # crystal frequency OSCILLATOR_HZ 8000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F413xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F413xx.py new file mode 100644 index 00000000000000..3202cf2bb0bab9 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32F413xx.py @@ -0,0 +1,857 @@ +#!/usr/bin/env python +''' +these tables are generated from the STM32 datasheet RM0430 in en.DM00305666.pdf for the +STM32F413 +''' + +# additional build information for ChibiOS +build = { + "CHIBIOS_STARTUP_MK" : "os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32f4xx.mk", + "CHIBIOS_PLATFORM_MK" : "os/hal/ports/STM32/STM32F4xx/platform.mk" +} + +# MCU parameters +mcu = { + # location of MCU serial number + 'UDID_START' : 0x1FFF7A10, + + # ram map, as list of (address, size-kb, flags) + # flags of 1 means DMA-capable + # flags of 2 means faster memory for CPU intensive work + 'RAM_MAP' : [ + (0x20000000, 320, 1), # main memory, SRAM1/SRAM2 DMA safe + + ], + + 'EXPECTED_CLOCK' : 100000000, + + 'DEFINES' : { + 'STM32F4' : '1', + } +} + +DMA_Map = { + # format is (DMA_TABLE, StreamNum, Channel) + # extracted from tabula-rm0430-stm32f413423-advanced-armbased-32bit-mcus-stmicroelectronics-dma.csv + "ADC1" : [(2,0,0),(2,4,0)], + "AES_IN" : [(2,6,2)], + "AES_OUT" : [(2,5,2)], + "DAC1" : [(1,5,7)], + "DAC2" : [(1,6,7)], + "DFSDM1_FLT0" : [(2,6,3),(2,0,7)], + "DFSDM1_FLT1" : [(2,1,3),(2,4,3)], + "DFSDM2_FLT0" : [(2,0,8),(2,4,8)], + "DFSDM2_FLT1" : [(2,1,8),(2,5,8)], + "DFSDM2_FLT2" : [(2,2,8),(2,6,8)], + "DFSDM2_FLT3" : [(2,3,8),(2,7,8)], + "I2C1_RX" : [(1,0,1),(1,5,1)], + "I2C1_TX" : [(1,1,0),(1,6,1),(1,7,1)], + "I2C2_RX" : [(1,2,7),(1,3,7)], + "I2C2_TX" : [(1,7,7)], + "I2C3_RX" : [(1,1,1),(1,2,3)], + "I2C3_TX" : [(1,4,3),(1,5,6)], + "I2CFMP1_RX" : [(1,3,1),(1,0,7)], + "I2CFMP1_TX" : [(1,1,2),(1,7,4)], + "I2S2_EXT_RX" : [(1,3,3)], + "I2S2_EXT_TX" : [(1,4,2)], + "I2S3_EXT_RX" : [(1,2,2),(1,0,3)], + "I2S3_EXT_TX" : [(1,5,2)], + "QUADSPI" : [(2,7,3)], + "SAI1_A" : [(2,1,0),(2,3,0)], + "SAI1_B" : [(2,5,0),(2,4,1)], + "SDIO" : [(2,3,4),(2,6,4)], + "SPI1_RX" : [(2,0,3),(2,2,3)], + "SPI1_TX" : [(2,2,2),(2,3,3),(2,5,3)], + "SPI2_RX" : [(1,3,0)], + "SPI2_TX" : [(1,4,0)], + "SPI3_RX" : [(1,0,0),(1,2,0)], + "SPI3_TX" : [(1,5,0),(1,7,0)], + "SPI4_RX" : [(2,0,4),(2,4,4),(2,3,5)], + "SPI4_TX" : [(2,1,4),(2,4,5)], + "SPI5_RX" : [(2,3,2),(2,5,7)], + "SPI5_TX" : [(2,4,2),(2,5,5),(2,6,7)], + "TIM1_CH1" : [(2,6,0),(2,1,6),(2,3,6)], + "TIM1_CH2" : [(2,6,0),(2,2,6)], + "TIM1_CH3" : [(2,6,0),(2,6,6)], + "TIM1_CH4" : [(2,4,6)], + "TIM1_COM" : [(2,4,6)], + "TIM1_TRIG" : [(2,0,6),(2,4,6)], + "TIM1_UP" : [(2,5,6)], + "TIM2_CH1" : [(1,5,3)], + "TIM2_CH2" : [(1,6,3)], + "TIM2_CH3" : [(1,1,3)], + "TIM2_CH4" : [(1,6,3),(1,7,3)], + "TIM2_UP" : [(1,1,3),(1,7,3)], + "TIM3_CH1" : [(1,4,5)], + "TIM3_CH2" : [(1,5,5)], + "TIM3_CH3" : [(1,7,5)], + "TIM3_CH4" : [(1,2,5)], + "TIM3_TRIG" : [(1,4,5)], + "TIM3_UP" : [(1,2,5)], + "TIM4_CH1" : [(1,0,2)], + "TIM4_CH2" : [(1,3,2)], + "TIM4_CH3" : [(1,7,2)], + "TIM4_UP" : [(1,6,2)], + "TIM5_CH1" : [(1,2,6)], + "TIM5_CH2" : [(1,4,6)], + "TIM5_CH3" : [(1,0,6)], + "TIM5_CH4" : [(1,1,6),(1,3,6)], + "TIM5_TRIG" : [(1,1,6),(1,3,6)], + "TIM5_UP" : [(1,0,6),(1,6,6)], + "TIM6_UP" : [(1,1,7)], + "TIM7_UP" : [(1,2,1),(1,4,1)], + "TIM8_CH1" : [(2,2,0),(2,2,7)], + "TIM8_CH2" : [(2,2,0),(2,3,7)], + "TIM8_CH3" : [(2,2,0),(2,4,7)], + "TIM8_CH4" : [(2,7,7)], + "TIM8_COM" : [(2,7,7)], + "TIM8_TRIG" : [(2,7,7)], + "TIM8_UP" : [(2,1,7)], + "UART10_RX" : [(2,0,5),(2,3,9)], + "UART10_TX" : [(2,7,6),(2,5,9)], + "UART4_RX" : [(1,2,4)], + "UART4_TX" : [(1,4,4)], + "UART5_RX" : [(1,0,4)], + "UART5_TX" : [(1,7,8)], + "UART7_RX" : [(1,3,5)], + "UART7_TX" : [(1,1,5)], + "UART8_RX" : [(1,6,5)], + "UART8_TX" : [(1,0,5)], + "UART9_RX" : [(2,7,0)], + "UART9_TX" : [(2,0,1)], + "USART1_RX" : [(2,2,4),(2,5,4)], + "USART1_TX" : [(2,7,4)], + "USART2_RX" : [(1,5,4),(1,7,6)], + "USART2_TX" : [(1,6,4)], + "USART3_RX" : [(1,1,4)], + "USART3_TX" : [(1,3,4),(1,4,7)], + "USART6_RX" : [(2,1,5),(2,2,5)], + "USART6_TX" : [(2,6,5),(2,7,5)], +} + +AltFunction_map = { + # format is PIN:FUNCTION : AFNUM + # extracted from tabula-stm32f413cg-altfunc.csv + "PA0:EVENTOUT" : 15, + "PA0:TIM2_CH1" : 1, + "PA0:TIM2_ETR" : 1, + "PA0:TIM5_CH1" : 2, + "PA0:TIM8_ETR" : 3, + "PA0:UART4_TX" : 8, + "PA0:USART2_CTS" : 7, + "PA1:EVENTOUT" : 15, + "PA1:I2S4_SD" : 5, + "PA1:QUADSPI_BK1_IO3" : 9, + "PA1:SPI4_MOSI" : 5, + "PA1:TIM2_CH2" : 1, + "PA1:TIM5_CH2" : 2, + "PA1:UART4_RX" : 8, + "PA1:USART2_RTS" : 7, + "PA2:EVENTOUT" : 15, + "PA2:FSMC_D4" : 12, + "PA2:FSMC_DA4" : 12, + "PA2:I2S2_CKIN" : 5, + "PA2:TIM2_CH3" : 1, + "PA2:TIM5_CH3" : 2, + "PA2:TIM9_CH1" : 3, + "PA2:USART2_TX" : 7, + "PA3:EVENTOUT" : 15, + "PA3:FSMC_D5" : 12, + "PA3:FSMC_DA5" : 12, + "PA3:I2S2_MCK" : 5, + "PA3:SAI1_SD_B" : 10, + "PA3:TIM2_CH4" : 1, + "PA3:TIM5_CH4" : 2, + "PA3:TIM9_CH2" : 3, + "PA3:USART2_RX" : 7, + "PA4:DFSDM1_DATIN1" : 8, + "PA4:EVENTOUT" : 15, + "PA4:FSMC_D6" : 12, + "PA4:FSMC_DA6" : 12, + "PA4:I2S1_WS" : 5, + "PA4:I2S3_WS" : 6, + "PA4:SPI1_NSS" : 5, + "PA4:SPI3_NSS" : 6, + "PA4:USART2_CK" : 7, + "PA5:DFSDM1_CKIN1" : 8, + "PA5:EVENTOUT" : 15, + "PA5:FSMC_D7" : 12, + "PA5:FSMC_DA7" : 12, + "PA5:I2S1_CK" : 5, + "PA5:SPI1_SCK" : 5, + "PA5:TIM2_CH1" : 1, + "PA5:TIM2_ETR" : 1, + "PA5:TIM8_CH1N" : 3, + "PA6:DFSDM2_CKIN1" : 7, + "PA6:EVENTOUT" : 15, + "PA6:I2S2_MCK" : 6, + "PA6:QUADSPI_BK2_IO0" : 10, + "PA6:SDIO_CMD" : 12, + "PA6:SPI1_MISO" : 5, + "PA6:TIM13_CH1" : 9, + "PA6:TIM1_BKIN" : 1, + "PA6:TIM3_CH1" : 2, + "PA6:TIM8_BKIN" : 3, + "PA7:DFSDM2_DATIN1" : 7, + "PA7:EVENTOUT" : 15, + "PA7:I2S1_SD" : 5, + "PA7:QUADSPI_BK2_IO1" : 10, + "PA7:SPI1_MOSI" : 5, + "PA7:TIM14_CH1" : 9, + "PA7:TIM1_CH1N" : 1, + "PA7:TIM3_CH2" : 2, + "PA7:TIM8_CH1N" : 3, + "PA8:CAN3_RX" : 11, + "PA8:DFSDM1_CKOUT" : 6, + "PA8:EVENTOUT" : 15, + "PA8:I2C3_SCL" : 4, + "PA8:MCO_1" : 0, + "PA8:SDIO_D1" : 12, + "PA8:TIM1_CH1" : 1, + "PA8:UART7_RX" : 8, + "PA8:USART1_CK" : 7, + "PA8:USB_FS_SOF" : 10, + "PA9:DFSDM2_CKIN3" : 3, + "PA9:EVENTOUT" : 15, + "PA9:I2C3_SMBA" : 4, + "PA9:I2S2_CK" : 5, + "PA9:SDIO_D2" : 12, + "PA9:SPI2_SCK" : 5, + "PA9:TIM1_CH2" : 1, + "PA9:USART1_TX" : 7, + "PA9:USB_FS_VBUS" : 10, + "PA10:DFSDM2_DATIN3" : 3, + "PA10:EVENTOUT" : 15, + "PA10:I2S2_SD" : 5, + "PA10:I2S5_SD" : 6, + "PA10:SPI2_MOSI" : 5, + "PA10:SPI5_MOSI" : 6, + "PA10:TIM1_CH3" : 1, + "PA10:USART1_RX" : 7, + "PA10:USB_FS_ID" : 10, + "PA11:CAN1_RX" : 9, + "PA11:DFSDM2_CKIN5" : 3, + "PA11:EVENTOUT" : 15, + "PA11:I2S2_WS" : 5, + "PA11:SPI2_NSS" : 5, + "PA11:SPI4_MISO" : 6, + "PA11:TIM1_CH4" : 1, + "PA11:UART4_RX" : 11, + "PA11:USART1_CTS" : 7, + "PA11:USART6_TX" : 8, + "PA11:USB_FS_DM" : 10, + "PA12:CAN1_TX" : 9, + "PA12:DFSDM2_DATIN5" : 3, + "PA12:EVENTOUT" : 15, + "PA12:SPI2_MISO" : 5, + "PA12:SPI5_MISO" : 6, + "PA12:TIM1_ETR" : 1, + "PA12:UART4_TX" : 11, + "PA12:USART1_RTS" : 7, + "PA12:USART6_RX" : 8, + "PA12:USB_FS_DP" : 10, + "PA13:EVENTOUT" : 15, + "PA13:JTMS-SWDIO" : 0, + "PA14:EVENTOUT" : 15, + "PA14:JTCK-SWCLK" : 0, + "PA15:CAN3_TX" : 11, + "PA15:EVENTOUT" : 15, + "PA15:I2S1_WS" : 5, + "PA15:I2S3_WS" : 6, + "PA15:JTDI" : 0, + "PA15:SAI1_MCLK_A" : 10, + "PA15:SPI1_NSS" : 5, + "PA15:SPI3_NSS" : 6, + "PA15:TIM2_CH1" : 1, + "PA15:TIM2_ETR" : 1, + "PA15:UART7_TX" : 8, + "PA15:USART1_TX" : 7, + "PB0:EVENTOUT" : 15, + "PB0:I2S5_CK" : 6, + "PB0:SPI5_SCK" : 6, + "PB0:TIM1_CH2N" : 1, + "PB0:TIM3_CH3" : 2, + "PB0:TIM8_CH2N" : 3, + "PB1:DFSDM1_DATIN0" : 8, + "PB1:EVENTOUT" : 15, + "PB1:I2S5_WS" : 6, + "PB1:QUADSPI_CLK" : 9, + "PB1:SPI5_NSS" : 6, + "PB1:TIM1_CH3N" : 1, + "PB1:TIM3_CH4" : 2, + "PB1:TIM8_CH3N" : 3, + "PB2:DFSDM1_CKIN0" : 6, + "PB2:EVENTOUT" : 15, + "PB2:LPTIM1_OUT" : 1, + "PB2:QUADSPI_CLK" : 9, + "PB3:CAN3_RX" : 11, + "PB3:EVENTOUT" : 15, + "PB3:I2C2_SDA" : 9, + "PB3:I2CFMP1_SDA" : 4, + "PB3:I2S1_CK" : 5, + "PB3:I2S3_CK" : 6, + "PB3:JTDO-SWO" : 0, + "PB3:SAI1_SD_A" : 10, + "PB3:SPI1_SCK" : 5, + "PB3:SPI3_SCK" : 6, + "PB3:TIM2_CH2" : 1, + "PB3:UART7_RX" : 8, + "PB3:USART1_RX" : 7, + "PB4:CAN3_TX" : 11, + "PB4:EVENTOUT" : 15, + "PB4:I2C3_SDA" : 9, + "PB4:I2S3EXT_SD" : 7, + "PB4:JTRST" : 0, + "PB4:SAI1_SCK_A" : 10, + "PB4:SDIO_D0" : 12, + "PB4:SPI1_MISO" : 5, + "PB4:SPI3_MISO" : 6, + "PB4:TIM3_CH1" : 2, + "PB4:UART7_TX" : 8, + "PB5:CAN2_RX" : 9, + "PB5:EVENTOUT" : 15, + "PB5:I2C1_SMBA" : 4, + "PB5:I2S1_SD" : 5, + "PB5:I2S3_SD" : 6, + "PB5:LPTIM1_IN1" : 1, + "PB5:SAI1_FS_A" : 10, + "PB5:SDIO_D3" : 12, + "PB5:SPI1_MOSI" : 5, + "PB5:SPI3_MOSI" : 6, + "PB5:TIM3_CH2" : 2, + "PB5:UART5_RX" : 11, + "PB6:CAN2_TX" : 9, + "PB6:DFSDM2_CKIN7" : 6, + "PB6:EVENTOUT" : 15, + "PB6:I2C1_SCL" : 4, + "PB6:LPTIM1_ETR" : 1, + "PB6:QUADSPI_BK1_NCS" : 10, + "PB6:SDIO_D0" : 12, + "PB6:TIM4_CH1" : 2, + "PB6:UART5_TX" : 11, + "PB6:USART1_TX" : 7, + "PB7:DFSDM2_DATIN7" : 6, + "PB7:EVENTOUT" : 15, + "PB7:FSMC_NL" : 12, + "PB7:I2C1_SDA" : 4, + "PB7:LPTIM1_IN2" : 1, + "PB7:TIM4_CH2" : 2, + "PB7:USART1_RX" : 7, + "PB8:CAN1_RX" : 8, + "PB8:DFSDM2_CKIN1" : 7, + "PB8:EVENTOUT" : 15, + "PB8:I2C1_SCL" : 4, + "PB8:I2C3_SDA" : 9, + "PB8:I2S5_SD" : 6, + "PB8:LPTIM1_OUT" : 1, + "PB8:SDIO_D4" : 12, + "PB8:SPI5_MOSI" : 6, + "PB8:TIM10_CH1" : 3, + "PB8:TIM4_CH3" : 2, + "PB8:UART5_RX" : 11, + "PB9:CAN1_TX" : 8, + "PB9:DFSDM2_DATIN1" : 6, + "PB9:EVENTOUT" : 15, + "PB9:I2C1_SDA" : 4, + "PB9:I2C2_SDA" : 9, + "PB9:I2S2_WS" : 5, + "PB9:SDIO_D5" : 12, + "PB9:SPI2_NSS" : 5, + "PB9:TIM11_CH1" : 3, + "PB9:TIM4_CH4" : 2, + "PB9:UART5_TX" : 11, + "PB10:DFSDM2_CKOUT" : 10, + "PB10:EVENTOUT" : 15, + "PB10:I2C2_SCL" : 4, + "PB10:I2CFMP1_SCL" : 9, + "PB10:I2S2_CK" : 5, + "PB10:I2S3_MCK" : 6, + "PB10:SDIO_D7" : 12, + "PB10:SPI2_SCK" : 5, + "PB10:TIM2_CH3" : 1, + "PB10:USART3_TX" : 7, + "PB11:EVENTOUT" : 15, + "PB11:I2C2_SDA" : 4, + "PB11:I2S2_CKIN" : 5, + "PB11:TIM2_CH4" : 1, + "PB11:USART3_RX" : 7, + "PB12:CAN2_RX" : 9, + "PB12:DFSDM1_DATIN1" : 10, + "PB12:EVENTOUT" : 15, + "PB12:FSMC_D13" : 12, + "PB12:FSMC_DA13" : 12, + "PB12:I2C2_SMBA" : 4, + "PB12:I2S2_WS" : 5, + "PB12:I2S3_CK" : 7, + "PB12:I2S4_WS" : 6, + "PB12:SPI2_NSS" : 5, + "PB12:SPI3_SCK" : 7, + "PB12:SPI4_NSS" : 6, + "PB12:TIM1_BKIN" : 1, + "PB12:UART5_RX" : 11, + "PB12:USART3_CK" : 8, + "PB13:CAN2_TX" : 9, + "PB13:DFSDM1_CKIN1" : 10, + "PB13:EVENTOUT" : 15, + "PB13:I2CFMP1_SMBA" : 4, + "PB13:I2S2_CK" : 5, + "PB13:I2S4_CK" : 6, + "PB13:SPI2_SCK" : 5, + "PB13:SPI4_SCK" : 6, + "PB13:TIM1_CH1N" : 1, + "PB13:UART5_TX" : 11, + "PB13:USART3_CTS" : 8, + "PB14:DFSDM1_DATIN2" : 8, + "PB14:EVENTOUT" : 15, + "PB14:FSMC_D0" : 10, + "PB14:FSMC_DA0" : 10, + "PB14:I2CFMP1_SDA" : 4, + "PB14:I2S2EXT_SD" : 6, + "PB14:SDIO_D6" : 12, + "PB14:SPI2_MISO" : 5, + "PB14:TIM12_CH1" : 9, + "PB14:TIM1_CH2N" : 1, + "PB14:TIM8_CH2N" : 3, + "PB14:USART3_RTS" : 7, + "PB15:DFSDM1_CKIN2" : 8, + "PB15:EVENTOUT" : 15, + "PB15:I2CFMP1_SCL" : 4, + "PB15:I2S2_SD" : 5, + "PB15:RTC_REFIN" : 0, + "PB15:SDIO_CK" : 12, + "PB15:SPI2_MOSI" : 5, + "PB15:TIM12_CH2" : 9, + "PB15:TIM1_CH3N" : 1, + "PB15:TIM8_CH3N" : 3, + "PC0:DFSDM2_CKIN4" : 3, + "PC0:EVENTOUT" : 15, + "PC0:LPTIM1_IN1" : 1, + "PC0:SAI1_MCLK_B" : 7, + "PC1:DFSDM2_DATIN4" : 3, + "PC1:EVENTOUT" : 15, + "PC1:LPTIM1_OUT" : 1, + "PC1:SAI1_SD_B" : 7, + "PC2:DFSDM1_CKOUT" : 8, + "PC2:DFSDM2_DATIN7" : 3, + "PC2:EVENTOUT" : 15, + "PC2:FSMC_NWE" : 12, + "PC2:I2S2EXT_SD" : 6, + "PC2:LPTIM1_IN2" : 1, + "PC2:SAI1_SCK_B" : 7, + "PC2:SPI2_MISO" : 5, + "PC3:DFSDM2_CKIN7" : 3, + "PC3:EVENTOUT" : 15, + "PC3:FSMC_A0" : 12, + "PC3:I2S2_SD" : 5, + "PC3:LPTIM1_ETR" : 1, + "PC3:SAI1_FS_B" : 7, + "PC3:SPI2_MOSI" : 5, + "PC4:DFSDM2_CKIN2" : 3, + "PC4:EVENTOUT" : 15, + "PC4:FSMC_NE4" : 12, + "PC4:I2S1_MCK" : 5, + "PC4:QUADSPI_BK2_IO2" : 10, + "PC5:DFSDM2_DATIN2" : 3, + "PC5:EVENTOUT" : 15, + "PC5:FSMC_NOE" : 12, + "PC5:I2CFMP1_SMBA" : 4, + "PC5:QUADSPI_BK2_IO3" : 10, + "PC5:USART3_RX" : 7, + "PC6:DFSDM1_CKIN3" : 6, + "PC6:DFSDM2_DATIN6" : 7, + "PC6:EVENTOUT" : 15, + "PC6:FSMC_D1" : 10, + "PC6:FSMC_DA1" : 10, + "PC6:I2CFMP1_SCL" : 4, + "PC6:I2S2_MCK" : 5, + "PC6:SDIO_D6" : 12, + "PC6:TIM3_CH1" : 2, + "PC6:TIM8_CH1" : 3, + "PC6:USART6_TX" : 8, + "PC7:DFSDM1_DATIN3" : 10, + "PC7:DFSDM2_CKIN6" : 7, + "PC7:EVENTOUT" : 15, + "PC7:I2CFMP1_SDA" : 4, + "PC7:I2S2_CK" : 5, + "PC7:I2S3_MCK" : 6, + "PC7:SDIO_D7" : 12, + "PC7:SPI2_SCK" : 5, + "PC7:TIM3_CH2" : 2, + "PC7:TIM8_CH2" : 3, + "PC7:USART6_RX" : 8, + "PC8:DFSDM2_CKIN3" : 7, + "PC8:EVENTOUT" : 15, + "PC8:QUADSPI_BK1_IO2" : 9, + "PC8:SDIO_D0" : 12, + "PC8:TIM3_CH3" : 2, + "PC8:TIM8_CH3" : 3, + "PC8:USART6_CK" : 8, + "PC9:DFSDM2_DATIN3" : 7, + "PC9:EVENTOUT" : 15, + "PC9:I2C3_SDA" : 4, + "PC9:I2S2_CKIN" : 5, + "PC9:MCO_2" : 0, + "PC9:QUADSPI_BK1_IO0" : 9, + "PC9:SDIO_D1" : 12, + "PC9:TIM3_CH4" : 2, + "PC9:TIM8_CH4" : 3, + "PC10:DFSDM2_CKIN5" : 3, + "PC10:EVENTOUT" : 15, + "PC10:I2S3_CK" : 6, + "PC10:QUADSPI_BK1_IO1" : 9, + "PC10:SDIO_D2" : 12, + "PC10:SPI3_SCK" : 6, + "PC10:USART3_TX" : 7, + "PC11:DFSDM2_DATIN5" : 3, + "PC11:EVENTOUT" : 15, + "PC11:FSMC_D2" : 10, + "PC11:FSMC_DA2" : 10, + "PC11:I2S3EXT_SD" : 5, + "PC11:QUADSPI_BK2_NCS" : 9, + "PC11:SDIO_D3" : 12, + "PC11:SPI3_MISO" : 6, + "PC11:UART4_RX" : 8, + "PC11:USART3_RX" : 7, + "PC12:EVENTOUT" : 15, + "PC12:FSMC_D3" : 10, + "PC12:FSMC_DA3" : 10, + "PC12:I2S3_SD" : 6, + "PC12:SDIO_CK" : 12, + "PC12:SPI3_MOSI" : 6, + "PC12:UART5_TX" : 8, + "PC12:USART3_CK" : 7, + "PC13:EVENTOUT" : 15, + "PC14:EVENTOUT" : 15, + "PC15:EVENTOUT" : 15, + "PD0:CAN1_RX" : 9, + "PD0:DFSDM2_CKIN6" : 3, + "PD0:EVENTOUT" : 15, + "PD0:FSMC_D2" : 12, + "PD0:FSMC_DA2" : 12, + "PD0:UART4_RX" : 11, + "PD1:CAN1_TX" : 9, + "PD1:DFSDM2_DATIN6" : 3, + "PD1:EVENTOUT" : 15, + "PD1:FSMC_D3" : 12, + "PD1:FSMC_DA3" : 12, + "PD1:UART4_TX" : 11, + "PD2:DFSDM2_CKOUT" : 3, + "PD2:EVENTOUT" : 15, + "PD2:FSMC_NWE" : 10, + "PD2:SDIO_CMD" : 12, + "PD2:TIM3_ETR" : 2, + "PD2:UART5_RX" : 8, + "PD3:DFSDM1_DATIN0" : 6, + "PD3:EVENTOUT" : 15, + "PD3:FSMC_CLK" : 12, + "PD3:I2S2_CK" : 5, + "PD3:QUADSPI_CLK" : 9, + "PD3:SPI2_SCK" : 5, + "PD3:TRACED1" : 0, + "PD3:USART2_CTS" : 7, + "PD4:DFSDM1_CKIN0" : 6, + "PD4:EVENTOUT" : 15, + "PD4:FSMC_NOE" : 12, + "PD4:USART2_RTS" : 7, + "PD5:DFSDM2_CKOUT" : 3, + "PD5:EVENTOUT" : 15, + "PD5:FSMC_NWE" : 12, + "PD5:USART2_TX" : 7, + "PD6:DFSDM1_DATIN1" : 6, + "PD6:EVENTOUT" : 15, + "PD6:FSMC_NWAIT" : 12, + "PD6:I2S3_SD" : 5, + "PD6:SPI3_MOSI" : 5, + "PD6:USART2_RX" : 7, + "PD7:DFSDM1_CKIN1" : 6, + "PD7:EVENTOUT" : 15, + "PD7:FSMC_NE1" : 12, + "PD7:USART2_CK" : 7, + "PD8:EVENTOUT" : 15, + "PD8:FSMC_D13" : 12, + "PD8:FSMC_DA13" : 12, + "PD8:USART3_TX" : 7, + "PD9:EVENTOUT" : 15, + "PD9:FSMC_D14" : 12, + "PD9:FSMC_DA14" : 12, + "PD9:USART3_RX" : 7, + "PD10:EVENTOUT" : 15, + "PD10:FSMC_D15" : 12, + "PD10:FSMC_DA15" : 12, + "PD10:UART4_TX" : 8, + "PD10:USART3_CK" : 7, + "PD11:DFSDM2_DATIN2" : 3, + "PD11:EVENTOUT" : 15, + "PD11:FSMC_A16" : 12, + "PD11:I2CFMP1_SMBA" : 4, + "PD11:QUADSPI_BK1_IO0" : 9, + "PD11:USART3_CTS" : 7, + "PD12:DFSDM2_CKIN2" : 3, + "PD12:EVENTOUT" : 15, + "PD12:FSMC_A17" : 12, + "PD12:I2CFMP1_SCL" : 4, + "PD12:QUADSPI_BK1_IO1" : 9, + "PD12:TIM4_CH1" : 2, + "PD12:USART3_RTS" : 7, + "PD13:EVENTOUT" : 15, + "PD13:FSMC_A18" : 12, + "PD13:I2CFMP1_SDA" : 4, + "PD13:QUADSPI_BK1_IO3" : 9, + "PD13:TIM4_CH2" : 2, + "PD14:DFSDM2_CKIN0" : 10, + "PD14:EVENTOUT" : 15, + "PD14:FSMC_D0" : 12, + "PD14:FSMC_DA0" : 12, + "PD14:I2CFMP1_SCL" : 4, + "PD14:TIM4_CH3" : 2, + "PD14:UART9_RX" : 11, + "PD15:DFSDM2_DATIN0" : 10, + "PD15:EVENTOUT" : 15, + "PD15:FSMC_D1" : 12, + "PD15:FSMC_DA1" : 12, + "PD15:I2CFMP1_SDA" : 4, + "PD15:TIM4_CH4" : 2, + "PD15:UART9_TX" : 11, + "PE0:DFSDM2_CKIN4" : 3, + "PE0:EVENTOUT" : 15, + "PE0:FSMC_NBL0" : 12, + "PE0:TIM4_ETR" : 2, + "PE0:UART8_RX" : 8, + "PE1:DFSDM2_DATIN4" : 3, + "PE1:EVENTOUT" : 15, + "PE1:FSMC_NBL1" : 12, + "PE1:UART8_TX" : 8, + "PE2:EVENTOUT" : 15, + "PE2:FSMC_A23" : 12, + "PE2:I2S4_CK" : 5, + "PE2:I2S5_CK" : 6, + "PE2:QUADSPI_BK1_IO2" : 9, + "PE2:SAI1_MCLK_A" : 7, + "PE2:SPI4_SCK" : 5, + "PE2:SPI5_SCK" : 6, + "PE2:TRACECLK" : 0, + "PE2:UART10_RX" : 11, + "PE3:EVENTOUT" : 15, + "PE3:FSMC_A19" : 12, + "PE3:SAI1_SD_B" : 7, + "PE3:TRACED0" : 0, + "PE3:UART10_TX" : 11, + "PE4:DFSDM1_DATIN3" : 8, + "PE4:EVENTOUT" : 15, + "PE4:FSMC_A20" : 12, + "PE4:I2S4_WS" : 5, + "PE4:I2S5_WS" : 6, + "PE4:SAI1_SD_A" : 7, + "PE4:SPI4_NSS" : 5, + "PE4:SPI5_NSS" : 6, + "PE4:TRACED1" : 0, + "PE5:DFSDM1_CKIN3" : 8, + "PE5:EVENTOUT" : 15, + "PE5:FSMC_A21" : 12, + "PE5:SAI1_SCK_A" : 7, + "PE5:SPI4_MISO" : 5, + "PE5:SPI5_MISO" : 6, + "PE5:TIM9_CH1" : 3, + "PE5:TRACED2" : 0, + "PE6:EVENTOUT" : 15, + "PE6:FSMC_A22" : 12, + "PE6:I2S4_SD" : 5, + "PE6:I2S5_SD" : 6, + "PE6:SAI1_FS_A" : 7, + "PE6:SPI4_MOSI" : 5, + "PE6:SPI5_MOSI" : 6, + "PE6:TIM9_CH2" : 3, + "PE6:TRACED3" : 0, + "PE7:DFSDM1_DATIN2" : 6, + "PE7:EVENTOUT" : 15, + "PE7:FSMC_D4" : 12, + "PE7:FSMC_DA4" : 12, + "PE7:QUADSPI_BK2_IO0" : 10, + "PE7:TIM1_ETR" : 1, + "PE7:UART7_RX" : 8, + "PE8:DFSDM1_CKIN2" : 6, + "PE8:EVENTOUT" : 15, + "PE8:FSMC_D5" : 12, + "PE8:FSMC_DA5" : 12, + "PE8:QUADSPI_BK2_IO1" : 10, + "PE8:TIM1_CH1N" : 1, + "PE8:UART7_TX" : 8, + "PE9:DFSDM1_CKOUT" : 6, + "PE9:EVENTOUT" : 15, + "PE9:FSMC_D6" : 12, + "PE9:FSMC_DA6" : 12, + "PE9:QUADSPI_BK2_IO2" : 10, + "PE9:TIM1_CH1" : 1, + "PE10:DFSDM2_DATIN0" : 3, + "PE10:EVENTOUT" : 15, + "PE10:FSMC_D7" : 12, + "PE10:FSMC_DA7" : 12, + "PE10:QUADSPI_BK2_IO3" : 10, + "PE10:TIM1_CH2N" : 1, + "PE11:DFSDM2_CKIN0" : 3, + "PE11:EVENTOUT" : 15, + "PE11:FSMC_D8" : 12, + "PE11:FSMC_DA8" : 12, + "PE11:I2S4_WS" : 5, + "PE11:I2S5_WS" : 6, + "PE11:SPI4_NSS" : 5, + "PE11:SPI5_NSS" : 6, + "PE11:TIM1_CH2" : 1, + "PE12:DFSDM2_DATIN7" : 3, + "PE12:EVENTOUT" : 15, + "PE12:FSMC_D9" : 12, + "PE12:FSMC_DA9" : 12, + "PE12:I2S4_CK" : 5, + "PE12:I2S5_CK" : 6, + "PE12:SPI4_SCK" : 5, + "PE12:SPI5_SCK" : 6, + "PE12:TIM1_CH3N" : 1, + "PE13:DFSDM2_CKIN7" : 3, + "PE13:EVENTOUT" : 15, + "PE13:FSMC_D10" : 12, + "PE13:FSMC_DA10" : 12, + "PE13:SPI4_MISO" : 5, + "PE13:SPI5_MISO" : 6, + "PE13:TIM1_CH3" : 1, + "PE14:DFSDM2_DATIN1" : 10, + "PE14:EVENTOUT" : 15, + "PE14:FSMC_D11" : 12, + "PE14:FSMC_DA11" : 12, + "PE14:I2S4_SD" : 5, + "PE14:I2S5_SD" : 6, + "PE14:SPI4_MOSI" : 5, + "PE14:SPI5_MOSI" : 6, + "PE14:TIM1_CH4" : 1, + "PE15:DFSDM2_CKIN1" : 10, + "PE15:EVENTOUT" : 15, + "PE15:FSMC_D12" : 12, + "PE15:FSMC_DA12" : 12, + "PE15:TIM1_BKIN" : 1, + "PF0:EVENTOUT" : 15, + "PF0:FSMC_A0" : 12, + "PF0:I2C2_SDA" : 4, + "PF1:EVENTOUT" : 15, + "PF1:FSMC_A1" : 12, + "PF1:I2C2_SCL" : 4, + "PF2:EVENTOUT" : 15, + "PF2:FSMC_A2" : 12, + "PF2:I2C2_SMBA" : 4, + "PF3:EVENTOUT" : 15, + "PF3:FSMC_A3" : 12, + "PF3:TIM5_CH1" : 2, + "PF4:EVENTOUT" : 15, + "PF4:FSMC_A4" : 12, + "PF4:TIM5_CH2" : 2, + "PF5:EVENTOUT" : 15, + "PF5:FSMC_A5" : 12, + "PF5:TIM5_CH3" : 2, + "PF6:EVENTOUT" : 15, + "PF6:QUADSI_BK1_IO3" : 9, + "PF6:SAI1_SD_B" : 7, + "PF6:TI10_CH1" : 3, + "PF6:TRACED0" : 0, + "PF6:UART7_RX" : 8, + "PF7:EVENTOUT" : 15, + "PF7:QUADSPI_BK1_IO2" : 9, + "PF7:SAI1_MCLK_B" : 7, + "PF7:TIM11_CH1" : 3, + "PF7:TRACED1" : 0, + "PF7:UART7_TX" : 8, + "PF8:EVENTOUT" : 15, + "PF8:QUADSPI_BK1_IO0" : 10, + "PF8:SAI1_SCK_B" : 7, + "PF8:TIM13_CH1" : 9, + "PF8:UART8_RX" : 8, + "PF9:EVENTOUT" : 15, + "PF9:QUADSPI_BK1_IO1" : 10, + "PF9:SAI1_FS_B" : 7, + "PF9:TIM14_CH1" : 9, + "PF9:UART8_TX" : 8, + "PF10:EVENTOUT" : 15, + "PF10:TIM1_ETR" : 1, + "PF10:TIM5_CH4" : 2, + "PF11:EVENTOUT" : 15, + "PF11:TIM8_ETR" : 3, + "PF12:EVENTOUT" : 15, + "PF12:FSMC_A6" : 12, + "PF12:TIM8_BKIN" : 3, + "PF13:EVENTOUT" : 15, + "PF13:FSMC_A7" : 12, + "PF13:I2CFMP1_SMBA" : 4, + "PF14:EVENTOUT" : 15, + "PF14:FSMC_A8" : 12, + "PF14:I2CFMP1_SCL" : 4, + "PF15:EVENTOUT" : 15, + "PF15:FSMC_A9" : 12, + "PF15:I2CFMP1_SDA" : 4, + "PG0:CAN1_RX" : 9, + "PG0:EVENTOUT" : 15, + "PG0:FSMC_A10" : 12, + "PG0:UART9_RX" : 11, + "PG1:CAN1_TX" : 9, + "PG1:EVENTOUT" : 15, + "PG1:FSMC_A11" : 12, + "PG1:UART9_TX" : 11, + "PG2:EVENTOUT" : 15, + "PG2:FSMC_A12" : 12, + "PG3:EVENTOUT" : 15, + "PG3:FSMC_A13" : 12, + "PG4:EVENTOUT" : 15, + "PG4:FSMC_A14" : 12, + "PG5:EVENTOUT" : 15, + "PG5:FSMC_A15" : 12, + "PG6:EVENTOUT" : 15, + "PG6:QUADSPI_BK1_NCS" : 10, + "PG7:EVENTOUT" : 15, + "PG7:USART6_CK" : 8, + "PG8:EVENTOUT" : 15, + "PG8:USART6_RTS" : 8, + "PG9:EVENTOUT" : 15, + "PG9:FSMC_NE2" : 12, + "PG9:QUADSPI_BK2_IO2" : 9, + "PG9:USART6_RX" : 8, + "PG10:EVENTOUT" : 15, + "PG10:FSMC_NE3" : 12, + "PG11:CAN2_RX" : 9, + "PG11:EVENTOUT" : 15, + "PG11:UART10_RX" : 11, + "PG12:CAN2_TX" : 9, + "PG12:EVENTOUT" : 15, + "PG12:FSMC_NE4" : 12, + "PG12:UART10_TX" : 11, + "PG12:USART6_RTS" : 8, + "PG13:EVENTOUT" : 15, + "PG13:FSMC_A24" : 12, + "PG13:TRACED2" : 0, + "PG13:USART6_CTS" : 8, + "PG14:EVENTOUT" : 15, + "PG14:FSMC_A25" : 12, + "PG14:QUADSPI_BK2_IO3" : 9, + "PG14:TRACED3" : 0, + "PG14:USART6_TX" : 8, + "PG15:EVENTOUT" : 15, + "PG15:USART6_CTS" : 8, + "PH0:EVENTOUT" : 15, + "PH1:EVENTOUT" : 15, +} + +ADC1_map = { + # format is PIN : ADC1_CHAN + # extracted from tabula-stm32f413cg-adc.csv + "PA0" : 0, + "PA1" : 1, + "PA2" : 2, + "PA3" : 3, + "PA4" : 4, + "PA5" : 5, + "PA6" : 6, + "PA7" : 7, + "PB0" : 8, + "PB1" : 9, + "PC0" : 10, + "PC1" : 11, + "PC2" : 12, + "PC3" : 13, + "PC4" : 14, + "PC5" : 15, +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/addfunc_parse.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/addfunc_parse.py index 4f5ebc5a46a813..609814fe0e085e 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/addfunc_parse.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/addfunc_parse.py @@ -28,7 +28,7 @@ def is_pin(str): return False def parse_adc_table(fname, table): - csvt = csv.reader(open(fname,'rb')) + csvt = csv.reader(open(fname,'r')) i = 0 for row in csvt: for col in row: diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py index b5940b9861e9ef..dd3b1774eab0a6 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/af_parse.py @@ -86,10 +86,10 @@ def parse_af_table(fname, table): parse_af_table(sys.argv[1], table) -sys.stdout.write("AltFunction_map = {\n"); +sys.stdout.write("AltFunction_map = {\n") sys.stdout.write('\t# format is PIN:FUNCTION : AFNUM\n') sys.stdout.write('\t# extracted from %s\n' % os.path.basename(sys.argv[1])) for k in sorted(table.keys(), key=cmp_to_key(pin_compare)): s = '"' + k + '"' sys.stdout.write('\t%-20s\t:\t%s,\n' % (s, table[k])) -sys.stdout.write("}\n"); +sys.stdout.write("}\n") diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index 4954c9be656eb5..a262f9fa13bb3b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -468,3 +468,7 @@ #ifndef AP_CUSTOMROTATIONS_ENABLED #define AP_CUSTOMROTATIONS_ENABLED 0 #endif + +#ifndef AP_QUICKTUNE_ENABLED +#define AP_QUICKTUNE_ENABLED 0 +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py index d0edd0f57f8fb7..f4dbab8fd5069c 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_parse.py @@ -11,7 +11,7 @@ def parse_dma_table(fname, table): dma_num = 1 - csvt = csv.reader(open(fname,'rb')) + csvt = csv.reader(open(fname,'r')) i = 0 last_channel = -1 for row in csvt: @@ -78,7 +78,7 @@ def check_full_table(table): sys.stdout.write('\t# format is (DMA_TABLE, StreamNum, Channel)\n') sys.stdout.write('\t# extracted from %s\n' % os.path.basename(sys.argv[1])) -for k in sorted(table.iterkeys()): +for k in sorted(table.keys()): s = '"%s"' % k sys.stdout.write('\t%-10s\t:\t[' % s) for i in range(len(table[k])): diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp index 09cb6668215000..6c3c9a686802a6 100644 --- a/libraries/AP_HAL_ChibiOS/system.cpp +++ b/libraries/AP_HAL_ChibiOS/system.cpp @@ -330,16 +330,14 @@ void panic(const char *errormsg, ...) INTERNAL_ERROR(AP_InternalError::error_t::panic); va_list ap; - va_start(ap, errormsg); - vprintf(errormsg, ap); - va_end(ap); - - hal.scheduler->delay_microseconds(10000); + uint16_t delay_ms = 10000; while (1) { va_start(ap, errormsg); vprintf(errormsg, ap); va_end(ap); - hal.scheduler->delay(500); + printf("\n"); + hal.scheduler->delay(delay_ms); + delay_ms = 500; } #else // we don't support variable args in bootlaoder diff --git a/libraries/AP_HAL_ESP32/README.md b/libraries/AP_HAL_ESP32/README.md index cfd829fcde7669..71a680ac80457f 100644 --- a/libraries/AP_HAL_ESP32/README.md +++ b/libraries/AP_HAL_ESP32/README.md @@ -20,7 +20,6 @@ Tools/environment_install/install-prereqs-arch.sh # from: https://docs.espressif.com/projects/esp-idf/en/latest/esp32/get-started/linux-setup.html sudo apt-get install git wget flex bison gperf cmake ninja-build ccache libffi-dev libssl-dev dfu-util sudo apt-get install python3 python3-pip python3-setuptools -sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10 #or sudo pacman -S --needed gcc git make flex bison gperf python-pip cmake ninja ccache dfu-util libusb @@ -68,11 +67,11 @@ https://docs.espressif.com/projects/esp-idf/en/latest/get-started/ - in expansion of macro 'configSUPPORT_STATIC_ALLOCATION' warning: "CONFIG_SUPPORT_STATIC_ALLOCATION" is not defined -this means your 'sdkconfig' file that the IDF relies on is perhaps a bit out of date or out of sync with your IDF. +this means your 'sdkconfig' file that the IDF relies on is perhaps a bit out of date or out of sync with your IDF. This should not happen, please file a bug if it does! -You can simply remove sdkconfig file and idf build system will recreate it using sdkconfig.defaults, which should fix the problem. +Changing the sdkconfig.defaults file will cause the sdkconfig to be deleted and regenerated. The sdkconfig will also be regenerated if it is manually deleted. -If you need to change sdkconfig, you can edit sdkconfig manually or to use ninja menuconfig: +If you need to change sdkconfig (which will not cause it to be deleted), you can edit sdkconfig manually or to use ninja menuconfig: So double check you are using the correct IDF version: ``` @@ -86,7 +85,7 @@ press [tab] then enter on the [exit] box to exit the app done. the 'sdkconfig' file in this folder should have been updated cd ../../../.. -If you want to make changes to sdkconfig (sdkconfig is in git ignore list) permanent and to commit them back in git, you should edit sdkconfig.defaults manually or to use ninja save-defconfig tool after menuconfig. +If you want to make changes to sdkconfig (sdkconfig is in the build dir) permanent and to commit them back in git, you should edit sdkconfig.defaults manually or use ninja save-defconfig tool after menuconfig and replace sdkconfig.defaults with defconfig. 5. Recommanded way to flash the firmware : ``` diff --git a/libraries/AP_HAL_ESP32/SdCard.cpp b/libraries/AP_HAL_ESP32/SdCard.cpp index 4644797cf32e3a..2ad6f3a596cc71 100644 --- a/libraries/AP_HAL_ESP32/SdCard.cpp +++ b/libraries/AP_HAL_ESP32/SdCard.cpp @@ -207,7 +207,7 @@ void mount_sdcard_spi() ESP_LOGI(TAG, "Initializing SD card as SDSPI"); esp_vfs_fat_sdmmc_mount_config_t mount_config = { .format_if_mount_failed = false, - .max_files = 10, + .max_files = 5, .allocation_unit_size = 16 * 1024 }; diff --git a/libraries/AP_HAL_ESP32/targets/esp32/.gitignore b/libraries/AP_HAL_ESP32/targets/esp32/.gitignore deleted file mode 100644 index ec58be23462c00..00000000000000 --- a/libraries/AP_HAL_ESP32/targets/esp32/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/sdkconfig.old -/board.txt diff --git a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt index 10cca76197529b..9931d646592c27 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt +++ b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.16) set(CMAKE_TOOLCHAIN_FILE $ENV{IDF_PATH}/tools/cmake/toolchain-esp32.cmake CACHE STRING "") @@ -28,8 +28,9 @@ idf_build_process(esp32 esp_system esp_rom esp_timer - - SDKCONFIG ${CMAKE_CURRENT_LIST_DIR}/sdkconfig + + # treat sdkconfig as build product generated by the defaults + SDKCONFIG ${CMAKE_BINARY_DIR}/sdkconfig SDKCONFIG_DEFAULTS ${CMAKE_CURRENT_LIST_DIR}/sdkconfig.defaults BUILD_DIR ${CMAKE_BINARY_DIR}) @@ -122,3 +123,76 @@ idf_build_executable(${elf_file}) set(CMAKE_EXPORT_COMPILE_COMMANDS 1) +# Additional targets for measuring RAM use: size, size-components, size-files +# - Adapted from ${IDF_PATH}/tools/cmake/project.cmake +# +# Reference: +# - https://docs.espressif.com/projects/esp-idf/en/v5.0/esp32s3/api-guides/performance/size.html#minimizing-binary-size +# +# Usage: +# cd ./build/esp32s3xxx/esp-idf_build +# ninja -v -v size +# +set(mapfile "${CMAKE_BINARY_DIR}/${CMAKE_PROJECT_NAME}.map") +# Add cross-reference table to the map file +target_link_options(${elf_file} PRIVATE "-Wl,--cref") +# Add this symbol as a hint for esp_idf_size to guess the target name +target_link_options(${elf_file} PRIVATE "-Wl,--defsym=IDF_TARGET_${idf_target}=0") +# Enable map file output +target_link_options(${elf_file} PRIVATE "-Wl,--Map=${mapfile}") +# Check if linker supports --no-warn-rwx-segments +execute_process(COMMAND ${CMAKE_LINKER} "--no-warn-rwx-segments" "--version" + RESULT_VARIABLE result + OUTPUT_QUIET + ERROR_QUIET) +if(${result} EQUAL 0) + # Do not print RWX segment warnings + target_link_options(${elf_file} PRIVATE "-Wl,--no-warn-rwx-segments") +endif() +if(CONFIG_ESP_ORPHAN_SECTION_WARNING) + # Print warnings if orphan sections are found + target_link_options(${elf_file} PRIVATE "-Wl,--orphan-handling=warn") +endif() + +idf_build_get_property(idf_path IDF_PATH) +idf_build_get_property(python PYTHON) + +set(idf_size ${python} -m esp_idf_size) + +# Add size targets, depend on map file, run esp_idf_size +# OUTPUT_JSON is passed for compatibility reasons, SIZE_OUTPUT_FORMAT +# environment variable is recommended and has higher priority +add_custom_target(size + COMMAND ${CMAKE_COMMAND} + -D "IDF_SIZE_TOOL=${idf_size}" + -D "MAP_FILE=${mapfile}" + -D "OUTPUT_JSON=${OUTPUT_JSON}" + -P "${idf_path}/tools/cmake/run_size_tool.cmake" + DEPENDS ${mapfile} + USES_TERMINAL + VERBATIM +) + +add_custom_target(size-files + COMMAND ${CMAKE_COMMAND} + -D "IDF_SIZE_TOOL=${idf_size}" + -D "IDF_SIZE_MODE=--files" + -D "MAP_FILE=${mapfile}" + -D "OUTPUT_JSON=${OUTPUT_JSON}" + -P "${idf_path}/tools/cmake/run_size_tool.cmake" + DEPENDS ${mapfile} + USES_TERMINAL + VERBATIM +) + +add_custom_target(size-components + COMMAND ${CMAKE_COMMAND} + -D "IDF_SIZE_TOOL=${idf_size}" + -D "IDF_SIZE_MODE=--archives" + -D "MAP_FILE=${mapfile}" + -D "OUTPUT_JSON=${OUTPUT_JSON}" + -P "${idf_path}/tools/cmake/run_size_tool.cmake" + DEPENDS ${mapfile} + USES_TERMINAL + VERBATIM +) diff --git a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults index b94fe2d026695e..9027170b1c7e3d 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults +++ b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig.defaults @@ -10,6 +10,7 @@ CONFIG_APP_RETRIEVE_LEN_ELF_SHA=16 CONFIG_ESPTOOLPY_FLASHMODE_QIO=y CONFIG_ESPTOOLPY_FLASHFREQ_80M=y CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y CONFIG_PARTITION_TABLE_CUSTOM=y CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="../partitions.csv" CONFIG_COMPILER_OPTIMIZATION_PERF=y diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/.gitignore b/libraries/AP_HAL_ESP32/targets/esp32s3/.gitignore deleted file mode 100644 index ec58be23462c00..00000000000000 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/sdkconfig.old -/board.txt diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt index c00f753174fea0..8627deba644654 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.16) set(CMAKE_TOOLCHAIN_FILE $ENV{IDF_PATH}/tools/cmake/toolchain-esp32s3.cmake CACHE STRING "") @@ -29,7 +29,8 @@ idf_build_process(esp32s3 esp_rom esp_timer - SDKCONFIG ${CMAKE_CURRENT_LIST_DIR}/sdkconfig + # treat sdkconfig as build product generated by the defaults + SDKCONFIG ${CMAKE_BINARY_DIR}/sdkconfig SDKCONFIG_DEFAULTS ${CMAKE_CURRENT_LIST_DIR}/sdkconfig.defaults BUILD_DIR ${CMAKE_BINARY_DIR} ) @@ -121,3 +122,77 @@ target_link_libraries(${elf_file} idf_build_executable(${elf_file}) set(CMAKE_EXPORT_COMPILE_COMMANDS 1) + +# Additional targets for measuring RAM use: size, size-components, size-files +# - Adapted from ${IDF_PATH}/tools/cmake/project.cmake +# +# Reference: +# - https://docs.espressif.com/projects/esp-idf/en/v5.0/esp32s3/api-guides/performance/size.html#minimizing-binary-size +# +# Usage: +# cd ./build/esp32s3xxx/esp-idf_build +# ninja -v -v size +# +set(mapfile "${CMAKE_BINARY_DIR}/${CMAKE_PROJECT_NAME}.map") +# Add cross-reference table to the map file +target_link_options(${elf_file} PRIVATE "-Wl,--cref") +# Add this symbol as a hint for esp_idf_size to guess the target name +target_link_options(${elf_file} PRIVATE "-Wl,--defsym=IDF_TARGET_${idf_target}=0") +# Enable map file output +target_link_options(${elf_file} PRIVATE "-Wl,--Map=${mapfile}") +# Check if linker supports --no-warn-rwx-segments +execute_process(COMMAND ${CMAKE_LINKER} "--no-warn-rwx-segments" "--version" + RESULT_VARIABLE result + OUTPUT_QUIET + ERROR_QUIET) +if(${result} EQUAL 0) + # Do not print RWX segment warnings + target_link_options(${elf_file} PRIVATE "-Wl,--no-warn-rwx-segments") +endif() +if(CONFIG_ESP_ORPHAN_SECTION_WARNING) + # Print warnings if orphan sections are found + target_link_options(${elf_file} PRIVATE "-Wl,--orphan-handling=warn") +endif() + +idf_build_get_property(idf_path IDF_PATH) +idf_build_get_property(python PYTHON) + +set(idf_size ${python} -m esp_idf_size) + +# Add size targets, depend on map file, run esp_idf_size +# OUTPUT_JSON is passed for compatibility reasons, SIZE_OUTPUT_FORMAT +# environment variable is recommended and has higher priority +add_custom_target(size + COMMAND ${CMAKE_COMMAND} + -D "IDF_SIZE_TOOL=${idf_size}" + -D "MAP_FILE=${mapfile}" + -D "OUTPUT_JSON=${OUTPUT_JSON}" + -P "${idf_path}/tools/cmake/run_size_tool.cmake" + DEPENDS ${mapfile} + USES_TERMINAL + VERBATIM +) + +add_custom_target(size-files + COMMAND ${CMAKE_COMMAND} + -D "IDF_SIZE_TOOL=${idf_size}" + -D "IDF_SIZE_MODE=--files" + -D "MAP_FILE=${mapfile}" + -D "OUTPUT_JSON=${OUTPUT_JSON}" + -P "${idf_path}/tools/cmake/run_size_tool.cmake" + DEPENDS ${mapfile} + USES_TERMINAL + VERBATIM +) + +add_custom_target(size-components + COMMAND ${CMAKE_COMMAND} + -D "IDF_SIZE_TOOL=${idf_size}" + -D "IDF_SIZE_MODE=--archives" + -D "MAP_FILE=${mapfile}" + -D "OUTPUT_JSON=${OUTPUT_JSON}" + -P "${idf_path}/tools/cmake/run_size_tool.cmake" + DEPENDS ${mapfile} + USES_TERMINAL + VERBATIM +) diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults index 22da63b62b92a7..c346336842fc5a 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig.defaults @@ -20,6 +20,7 @@ CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES_TWO=y CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND=n CONFIG_ESP_SLEEP_GPIO_RESET_WORKAROUND=n CONFIG_ESP32S3_INSTRUCTION_CACHE_32KB=y +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y CONFIG_ESP_INT_WDT=n CONFIG_ESP_TASK_WDT_INIT=n CONFIG_ESP_IPC_TASK_STACK_SIZE=1536 diff --git a/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp b/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp index faf450ee98701c..866c23a990c232 100644 --- a/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp +++ b/libraries/AP_HAL_Linux/AnalogIn_Navio2.cpp @@ -20,7 +20,7 @@ void AnalogSource_Navio2::set_channel(uint8_t pin) } if (asprintf(&channel_path, "%s/ch%d", ADC_BASE_PATH, pin) == -1) { - AP_HAL::panic("asprintf failed\n"); + AP_HAL::panic("asprintf failed"); } if (_fd >= 0) { diff --git a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp index ba53789c8a0bce..cc512f53e5ace1 100644 --- a/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp +++ b/libraries/AP_HAL_Linux/CameraSensor_Mt9v117.cpp @@ -126,7 +126,7 @@ CameraSensor_Mt9v117::CameraSensor_Mt9v117(const char *device_path, _configure_sensor_qvga(); break; default: - AP_HAL::panic("mt9v117: unsupported resolution\n"); + AP_HAL::panic("mt9v117: unsupported resolution"); break; } @@ -409,7 +409,7 @@ void CameraSensor_Mt9v117::_init_sensor() id = _read_reg16(CHIP_ID); if (id != MT9V117_CHIP_ID) { - AP_HAL::panic("Mt9v117: bad chip id 0x%04x\n", id); + AP_HAL::panic("Mt9v117: bad chip id 0x%04x", id); } _soft_reset(); _apply_patch(); diff --git a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp index 2429449950bd42..a7817371eb2d91 100644 --- a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp +++ b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp @@ -88,7 +88,7 @@ void OpticalFlow_Onboard::init() if (!_camerasensor->set_format(HAL_OPTFLOW_ONBOARD_SENSOR_WIDTH, HAL_OPTFLOW_ONBOARD_SENSOR_HEIGHT, V4L2_MBUS_FMT_UYVY8_2X8)) { - AP_HAL::panic("OpticalFlow_Onboard: couldn't set subdev fmt\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't set subdev fmt"); } _format = V4L2_PIX_FMT_NV12; #endif @@ -100,7 +100,7 @@ void OpticalFlow_Onboard::init() if (_format != V4L2_PIX_FMT_NV12 && _format != V4L2_PIX_FMT_GREY && _format != V4L2_PIX_FMT_YUYV) { - AP_HAL::panic("OpticalFlow_Onboard: format not supported\n"); + AP_HAL::panic("OpticalFlow_Onboard: format not supported"); } if (_width == HAL_OPTFLOW_ONBOARD_OUTPUT_WIDTH && @@ -272,7 +272,7 @@ void OpticalFlow_Onboard::_run_optflow() convert_buffer = (uint8_t *)calloc(1, convert_buffer_size); if (!convert_buffer) { - AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate conversion buffer\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate conversion buffer"); } } @@ -286,7 +286,7 @@ void OpticalFlow_Onboard::_run_optflow() free(convert_buffer); } - AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate crop buffer\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't allocate crop buffer"); } } @@ -322,7 +322,7 @@ void OpticalFlow_Onboard::_run_optflow() free(output_buffer); } - AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame\n"); + AP_HAL::panic("OpticalFlow_Onboard: couldn't get frame"); } if (_format == V4L2_PIX_FMT_YUYV) { diff --git a/libraries/AP_HAL_Linux/RCInput_Navio2.cpp b/libraries/AP_HAL_Linux/RCInput_Navio2.cpp index 225976ea0b6d0d..4fb9e232705d7c 100644 --- a/libraries/AP_HAL_Linux/RCInput_Navio2.cpp +++ b/libraries/AP_HAL_Linux/RCInput_Navio2.cpp @@ -23,7 +23,7 @@ void RCInput_Navio2::init() for (size_t i = 0; i < ARRAY_SIZE(channels); i++) { channels[i] = open_channel(i); if (channels[i] < 0) { - AP_HAL::panic("[RCInput_Navio2]: failed to open channels\n"); + AP_HAL::panic("[RCInput_Navio2]: failed to open channels"); } } } @@ -64,7 +64,7 @@ int RCInput_Navio2::open_channel(int channel) { char *channel_path; if (asprintf(&channel_path, "%s/ch%d", RCIN_SYSFS_PATH, channel) == -1) { - AP_HAL::panic("[RCInput_Navio2]: not enough memory\n"); + AP_HAL::panic("[RCInput_Navio2]: not enough memory"); } int fd = ::open(channel_path, O_RDONLY|O_CLOEXEC); diff --git a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp index c141f40d62ed3c..999f25328a6443 100644 --- a/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp @@ -32,7 +32,7 @@ using namespace Linux; static void catch_sigbus(int sig) { - AP_HAL::panic("RCOutputAioPRU.cpp:SIGBUS error generated\n"); + AP_HAL::panic("RCOutputAioPRU.cpp:SIGBUS error generated"); } void RCOutput_AioPRU::init() { diff --git a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp index b20ae3071b1257..a36b9fac4ac7e6 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PRU.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_PRU.cpp @@ -23,7 +23,7 @@ static const uint8_t chan_pru_map[]= {10,8,11,9,7,6,5,4,3,2,1,0}; static void catch_sigbus(int sig) { - AP_HAL::panic("RCOutput.cpp:SIGBUS error generated\n"); + AP_HAL::panic("RCOutput.cpp:SIGBUS error generated"); } void RCOutput_PRU::init() { diff --git a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp index f64273e96492d8..b2e8d7b483258b 100644 --- a/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp @@ -31,7 +31,7 @@ using namespace Linux; static void catch_sigbus(int sig) { - AP_HAL::panic("RCOutput.cpp:SIGBUS error generated\n"); + AP_HAL::panic("RCOutput.cpp:SIGBUS error generated"); } void RCOutput_ZYNQ::init() { diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp index e88b388b342b2f..b2f705e4b4426a 100644 --- a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp @@ -57,7 +57,7 @@ void SPIUARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) _buffer = NEW_NOTHROW uint8_t[rxS]; if (_buffer == nullptr) { hal.console->printf("Not enough memory\n"); - AP_HAL::panic("Not enough memory\n"); + AP_HAL::panic("Not enough memory"); } } diff --git a/libraries/AP_HAL_Linux/VideoIn.cpp b/libraries/AP_HAL_Linux/VideoIn.cpp index 1e251d7d4b6152..d685181d6f126e 100644 --- a/libraries/AP_HAL_Linux/VideoIn.cpp +++ b/libraries/AP_HAL_Linux/VideoIn.cpp @@ -45,7 +45,7 @@ bool VideoIn::get_frame(Frame &frame) { if (!_streaming) { if (!_set_streaming(true)) { - AP_HAL::panic("couldn't start streaming\n"); + AP_HAL::panic("couldn't start streaming"); return false; } _streaming = true; diff --git a/libraries/AP_HAL_QURT/RCOutput.cpp b/libraries/AP_HAL_QURT/RCOutput.cpp index eb509d070954ae..70b9ceee8188ef 100644 --- a/libraries/AP_HAL_QURT/RCOutput.cpp +++ b/libraries/AP_HAL_QURT/RCOutput.cpp @@ -8,19 +8,27 @@ extern const AP_HAL::HAL& hal; using namespace QURT; +// ESC specific definitions #define ESC_PACKET_TYPE_PWM_CMD 1 #define ESC_PACKET_TYPE_FB_RESPONSE 128 #define ESC_PACKET_TYPE_FB_POWER_STATUS 132 -#define ESC_PKT_HEADER 0xAF +// IO board specific definitions +#define IO_PACKET_TYPE_PWM_HIRES_CMD 6 + +// Generic definitions +#define PKT_HEADER 0xAF +#define PACKET_TYPE_VERSION_EXT_REQUEST 24 +#define PACKET_TYPE_VERSION_EXT_RESPONSE 131 void RCOutput::init() { - fd = sl_client_config_uart(QURT_UART_ESC, baudrate); + baudrate = ESC_BAUDRATE; + fd = sl_client_config_uart(QURT_UART_ESC_IO, baudrate); if (fd == -1) { - HAP_PRINTF("Failed to open ESC UART"); + HAP_PRINTF("Failed to open ESC / IO UART"); } - HAP_PRINTF("ESC UART: %d", fd); + HAP_PRINTF("ESC / IO UART: %d", fd); } void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) @@ -77,14 +85,14 @@ void RCOutput::read(uint16_t *period_us, uint8_t len) } /* - send a packet with CRC to the ESC + send a packet with CRC to the ESC or IO board */ -void RCOutput::send_esc_packet(uint8_t type, uint8_t *data, uint16_t size) +void RCOutput::send_packet(uint8_t type, uint8_t *data, uint16_t size) { uint16_t packet_size = size + 5; uint8_t out[packet_size]; - out[0] = ESC_PKT_HEADER; + out[0] = PKT_HEADER; out[1] = packet_size; out[2] = type; @@ -106,15 +114,91 @@ static int16_t pwm_to_esc(uint16_t pwm) return int16_t(800*p); } -/* - send current commands to ESCs - */ -void RCOutput::send_receive(void) +void RCOutput::scan_for_hardware(void) { - if (fd == -1) { - return; + // Alternate between sending a version request and looking for the + // version response + static bool request_version = true; + if (request_version) { + HAP_PRINTF("RCOUTPUT requesting version"); + + uint8_t data = 0; + send_packet(PACKET_TYPE_VERSION_EXT_REQUEST, &data, 1); + request_version = false; + } else { + HAP_PRINTF("RCOUTPUT checking response"); + + check_response(); + + // If we still haven't discovered what HW is out there then + // try a different baudrate + if (hw_type == HWType::UNKNOWN) { + if (baudrate == ESC_BAUDRATE) { + baudrate = IO_BAUDRATE; + } else { + baudrate = ESC_BAUDRATE; + } + + sl_client_config_uart(QURT_UART_ESC_IO, baudrate); + + request_version = true; + } + } +} + +void RCOutput::send_esc_command(void) +{ + int16_t data[5] {}; + + // We don't set any LEDs + data[4] = 0; + + for (uint8_t i=0; i 5) { + last_fb_req_ms = now_ms; + // request feedback from one ESC + last_fb_idx = (last_fb_idx+1) % 4; + data[last_fb_idx] |= 1; + } + + send_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *)data, sizeof(data)); + + check_response(); +} + +void RCOutput::send_io_command(void) +{ + struct PACKED { + uint8_t command_type; + uint16_t vals[8]; + } hires_pwm_cmd; + + hires_pwm_cmd.command_type = 0; + + // Resolution of commands in the packet is 0.05us = 50ns + // Convert from standard 1us resolution to IO command resolution + for (uint32_t idx=0; idxget_safety_mask(); } - - int16_t data[5] {}; - for (uint8_t i=0; i<4; i++) { + for (uint8_t i=0; i 5) { - last_fb_req_ms = now_ms; - // request feedback from one ESC - last_fb_idx = (last_fb_idx+1) % 4; - data[last_fb_idx] |= 1; +/* + send current commands to ESC or IO board + */ +void RCOutput::send_receive(void) +{ + // No point proceeding if we were not able to open the UART + if (fd == -1) { + return; } - send_esc_packet(ESC_PACKET_TYPE_PWM_CMD, (uint8_t *)data, sizeof(data)); + switch (hw_type) { + case HWType::UNKNOWN: + scan_for_hardware(); + break; + case HWType::ESC: + case HWType::IO: + send_pwm_output(); + break; + default: + return; + } +} - check_response(); +std::string RCOutput::board_id_to_name(uint16_t board_id) +{ + switch (board_id) { + case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)"); + case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)"); + case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)"); + case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)"); + case 35: return std::string("ModalAi I/O Expander (M0065)"); + case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)"); + case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)"); + case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)"); + case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)"); + case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)"); + case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)"); + case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)"); + default: return std::string("Unknown Board"); + } +} + +void RCOutput::handle_version_feedback(const struct extended_version_info &pkt) +{ + uint16_t hw_ver = pkt.hw_version; + + // Check to see if we have a recognized HW id + if (hw_ver == 35) { + // We found an IO board + hw_type = HWType::IO; + } else { + // Just assume an ESC and don't try to compare against a list of + // known ESCs since that list will likely expand over time. + // If the hardware responded with a valid version packet at the ESC + // baud rate then it is very likely an ESC. + hw_type = HWType::ESC; + } + + // Dump all the version information + HAP_PRINTF("RCOUTPUT: Board ID: %i", pkt.id); + HAP_PRINTF("RCOUTPUT: Board Type : %i: %s", hw_ver, board_id_to_name(hw_ver).c_str()); + + HAP_PRINTF("RCOUTPUT: Unique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + pkt.unique_id[11], pkt.unique_id[10], pkt.unique_id[9], pkt.unique_id[8], + pkt.unique_id[7], pkt.unique_id[6], pkt.unique_id[5], pkt.unique_id[4], + pkt.unique_id[3], pkt.unique_id[2], pkt.unique_id[1], pkt.unique_id[0]); + + HAP_PRINTF("RCOUTPUT: Firmware : version %4d, hash %.12s", pkt.sw_version, pkt.firmware_git_version); + HAP_PRINTF("RCOUTPUT: Bootloader : version %4d, hash %.12s", pkt.bootloader_version, pkt.bootloader_git_version); } /* @@ -185,27 +336,35 @@ void RCOutput::handle_power_status(const struct esc_power_status &pkt) void RCOutput::check_response(void) { uint8_t buf[256]; - struct PACKED esc_packet { + struct PACKED data_packet { uint8_t header; uint8_t length; uint8_t type; union { + struct extended_version_info ver_info; struct esc_response_v2 resp_v2; struct esc_power_status power_status; } u; }; auto n = sl_client_uart_read(fd, (char *)buf, sizeof(buf)); + // TODO: Maintain a count of total received bytes over time + // HAP_PRINTF("RCOUTPUT response bytes: %d", n); while (n >= 3) { - const auto *pkt = (struct esc_packet *)buf; - if (pkt->header != ESC_PKT_HEADER || pkt->length > n) { + const auto *pkt = (struct data_packet *)buf; + if (pkt->header != PKT_HEADER || pkt->length > n) { return; } const uint16_t crc = calc_crc_modbus(&pkt->length, pkt->length-3); const uint16_t crc2 = buf[pkt->length-2] | buf[pkt->length-1]<<8; if (crc != crc2) { + // TODO: Maintain a count of failed CRCs over time + // HAP_PRINTF("RCOUTPUT CRC fail on input"); return; } switch (pkt->type) { + case PACKET_TYPE_VERSION_EXT_RESPONSE: + handle_version_feedback(pkt->u.ver_info); + break; case ESC_PACKET_TYPE_FB_RESPONSE: handle_esc_feedback(pkt->u.resp_v2); break; diff --git a/libraries/AP_HAL_QURT/RCOutput.h b/libraries/AP_HAL_QURT/RCOutput.h index ee3f95aac2f27e..c2be76ccb7fd6b 100644 --- a/libraries/AP_HAL_QURT/RCOutput.h +++ b/libraries/AP_HAL_QURT/RCOutput.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include "AP_HAL_QURT.h" #include @@ -32,19 +33,51 @@ class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend /* force the safety switch on, disabling output from the ESCs/servos */ - bool force_safety_on(void) override { safety_on = true; return true; } + bool force_safety_on(void) override + { + safety_on = true; + return true; + } /* force the safety switch off, enabling output from the ESCs/servos */ - void force_safety_off(void) override { safety_on = false; } + void force_safety_off(void) override + { + safety_on = false; + } private: - const uint32_t baudrate = 2000000; + enum { + IO_BAUDRATE = 921600, + ESC_BAUDRATE = 2000000 + } baudrate; + + enum class HWType { + UNKNOWN = 0, // Unknown board type + ESC = 1, // ESC + IO = 2, // IO board + }; + HWType hw_type = HWType::UNKNOWN; + void scan_for_hardware(void); void send_receive(void); void check_response(void); - void send_esc_packet(uint8_t type, uint8_t *data, uint16_t size); + void send_pwm_output(void); + void send_io_command(void); + void send_esc_command(void); + void send_packet(uint8_t type, uint8_t *data, uint16_t size); + + struct PACKED extended_version_info { + uint8_t id; + uint16_t sw_version; + uint16_t hw_version; + uint8_t unique_id[12]; + char firmware_git_version[12]; + char bootloader_git_version[12]; + uint16_t bootloader_version; + uint16_t crc; + }; struct PACKED esc_response_v2 { uint8_t id_state; // bits 0:3 = state, bits 4:7 = ID @@ -64,13 +97,17 @@ class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend int16_t current; //Total Current (8mA resolution) }; + void handle_version_feedback(const struct extended_version_info &pkt); void handle_esc_feedback(const struct esc_response_v2 &pkt); void handle_power_status(const struct esc_power_status &pkt); + std::string board_id_to_name(uint16_t board_id); + int fd = -1; uint16_t enable_mask; static const uint8_t channel_count = 4; uint16_t period[channel_count]; + uint16_t pwm_output[channel_count]; volatile bool need_write; bool corked; HAL_Semaphore mutex; diff --git a/libraries/AP_HAL_QURT/interface.h b/libraries/AP_HAL_QURT/interface.h index fb3019f127a826..1c919da1c255ed 100644 --- a/libraries/AP_HAL_QURT/interface.h +++ b/libraries/AP_HAL_QURT/interface.h @@ -46,4 +46,4 @@ extern "C" { // IDs for serial ports #define QURT_UART_GPS 6 #define QURT_UART_RCIN 7 -#define QURT_UART_ESC 2 +#define QURT_UART_ESC_IO 2 // UART for the ESC or IO board that bridges to ESC diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 4850e40b027a40..06fe08506f710c 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -400,17 +400,18 @@ void SITL_State::_simulator_servos(struct sitl_input &input) } float engine_mul = _sitl?_sitl->engine_mul.get():1; - uint8_t engine_fail = _sitl?_sitl->engine_fail.get():0; + uint32_t engine_fail = _sitl?_sitl->engine_fail.get():0; float throttle = 0.0f; - if (engine_fail >= ARRAY_SIZE(input.servos)) { - engine_fail = 0; - } // apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter - if (_vehicle != Rover) { - input.servos[engine_fail] = ((input.servos[engine_fail]-1000) * engine_mul) + 1000; - } else { - input.servos[engine_fail] = static_cast(((input.servos[engine_fail] - 1500) * engine_mul) + 1500); + for (uint8_t i=0; i(((input.servos[i] - 1500) * engine_mul) + 1500); + } + } } if (_vehicle == ArduPlane) { diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index ccc40042e8ea99..329617d877b2c4 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -414,7 +414,7 @@ void Scheduler::check_thread_stacks(void) const uint8_t ncheck = 8; for (uint8_t i=0; istack_min[i] != stackfill) { - AP_HAL::panic("stack overflow in thread %s\n", p->name); + AP_HAL::panic("stack overflow in thread %s", p->name); } } } diff --git a/libraries/AP_HAL_SITL/Storage.cpp b/libraries/AP_HAL_SITL/Storage.cpp index 98e28e5f9989ba..c82f8351f2fb8e 100644 --- a/libraries/AP_HAL_SITL/Storage.cpp +++ b/libraries/AP_HAL_SITL/Storage.cpp @@ -279,7 +279,7 @@ static bool sitl_flash_write(uint32_t addr, const uint8_t *data, uint32_t length } #if defined(HAL_FLASH_MIN_WRITE_SIZE) && HAL_FLASH_MIN_WRITE_SIZE == 32 if ((length % HAL_FLASH_MIN_WRITE_SIZE) != 0 || (addr % HAL_FLASH_MIN_WRITE_SIZE) != 0) { - AP_HAL::panic("Attempt to write flash at %u length %u\n", addr, length); + AP_HAL::panic("Attempt to write flash at %u length %u", addr, length); } // emulate H7 requirement that writes be to untouched bytes for (uint32_t i=0; iread_registers(BMI160_REG_ERR_REG, &v, 1); if (!r) { - AP_HAL::panic("BMI160: couldn't read ERR_REG\n"); + AP_HAL::panic("BMI160: couldn't read ERR_REG"); } if (v) { - AP_HAL::panic("BMI160: error detected on ERR_REG\n"); + AP_HAL::panic("BMI160: error detected on ERR_REG"); } #endif } @@ -514,4 +514,4 @@ bool AP_InertialSensor_BMI160::_init() } return ret; -} \ No newline at end of file +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 30d06b02b16c17..300125068d1699 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -55,6 +55,15 @@ void AP_InertialSensor_Backend::_set_gyro_oversampling(uint8_t instance, uint8_t _imu._gyro_over_sampling[instance] = n; } +/* + while sensors are converging to get the true sample rate we re-init the notch filters. + stop doing this if the user arms + */ +bool AP_InertialSensor_Backend::sensors_converging() const +{ + return AP_HAL::millis64() < HAL_INS_CONVERGANCE_MS && !hal.util->get_soft_armed(); +} + /* update the sensor rate for FIFO sensors diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 7ab5b420d817ec..38e0a66b8b0e0d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -233,7 +233,7 @@ class AP_InertialSensor_Backend void _update_sensor_rate(uint16_t &count, uint32_t &start_us, float &rate_hz) const __RAMFUNC__; // return true if the sensors are still converging and sampling rates could change significantly - bool sensors_converging() const { return AP_HAL::millis() < HAL_INS_CONVERGANCE_MS; } + bool sensors_converging() const; // set accelerometer max absolute offset for calibration void _set_accel_max_abs_offset(uint8_t instance, float offset); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index d22a7a224f30b1..36d69db3fbde71 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -343,15 +343,11 @@ void AP_InertialSensor_Invensensev3::start() // pre-calculate backend period backend_period_us = 1000000UL / backend_rate_hz; - if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) || - !_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) { + if (!_imu.register_gyro(gyro_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype)) || + !_imu.register_accel(accel_instance, sampling_rate_hz, dev->get_bus_id_devtype(devtype))) { return; } - // update backend sample rate - _set_accel_raw_sample_rate(accel_instance, backend_rate_hz); - _set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz); - // indicate what multiplier is appropriate for the sensors' // readings to fit them into an int16_t: _set_raw_sample_accel_multiplier(accel_instance, multiplier_accel); @@ -381,16 +377,15 @@ void AP_InertialSensor_Invensensev3::start() // get a startup banner to output to the GCS bool AP_InertialSensor_Invensensev3::get_output_banner(char* banner, uint8_t banner_len) { - if (fast_sampling) { - snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz", - gyro_instance, + snprintf(banner, banner_len, "IMU%u:%s%s sampling %.1fkHz/%.1fkHz", + gyro_instance, + fast_sampling ? " fast" : " normal", #if HAL_INS_HIGHRES_SAMPLE - highres_sampling ? ", high-resolution" : + highres_sampling ? " hi-res" : #endif - "" , backend_rate_hz * 0.001); - return true; - } - return false; + "", backend_rate_hz * 0.001, + sampling_rate_hz * 0.001); + return true; } /* @@ -669,21 +664,21 @@ void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t r } // calculate the fast sampling backend rate -uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const +uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const { // constrain the gyro rate to be at least the loop rate - uint8_t loop_limit = 1; - if (get_loop_rate_hz() > base_odr) { - loop_limit = 2; + uint8_t min_base_rate_multiplier = 1; + if (get_loop_rate_hz() > base_backend_rate) { + min_base_rate_multiplier = 2; } - if (get_loop_rate_hz() > base_odr * 2) { - loop_limit = 4; + if (get_loop_rate_hz() > base_backend_rate * 2) { + min_base_rate_multiplier = 4; } // constrain the gyro rate to be a 2^N multiple - uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8); + uint8_t fast_sampling_rate_multiplier = constrain_int16(get_fast_sampling_rate(), min_base_rate_multiplier, 8); // calculate rate we will be giving samples to the backend - return constrain_int16(base_odr * fast_sampling_rate, base_odr, max_odr); + return constrain_int16(base_backend_rate * fast_sampling_rate_multiplier, base_backend_rate, max_backend_rate); } /* @@ -806,6 +801,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) } } } + sampling_rate_hz = backend_rate_hz; // sampling rate and backend rate are the same // disable gyro and accel as per 12.9 in the ICM-42688 docs register_write(INV3REG_PWR_MGMT0, 0x00); @@ -862,6 +858,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void) { backend_rate_hz = 1600; + sampling_rate_hz = 1600; // use low-noise mode register_write(INV3REG_70_PWR_MGMT0, 0x0f); hal.scheduler->delay_microseconds(300); @@ -886,27 +883,24 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void) */ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void) { - uint8_t odr_config = 4; - backend_rate_hz = 1600; - // always fast sampling - fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; + uint8_t odr_config; + backend_rate_hz = 800; - if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) { - backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4); + fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; + if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() && fast_sampling) { + // For ICM45686 we only set 3200 or 6400Hz as sampling rates + // we don't need to read FIFO faster than requested rate + backend_rate_hz = calculate_fast_sampling_backend_rate(800, 6400); + } else { + fast_sampling = false; } - // this sensor actually only supports 2 speeds - backend_rate_hz = constrain_int16(backend_rate_hz, 3200, 6400); - - switch (backend_rate_hz) { - case 6400: // 6.4Khz - odr_config = 3; - break; - case 3200: // 3.2Khz - odr_config = 4; - break; - default: - break; + if (backend_rate_hz <= 3200) { + odr_config = 0x04; // 3200Hz + sampling_rate_hz = 3200; + } else { + odr_config = 0x03; // 6400Hz + sampling_rate_hz = 6400; } // Disable FIFO first diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index 6b4fc49a8c5c49..889fda4f76c86c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -58,7 +58,7 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend void set_filter_and_scaling_icm42670(void); void set_filter_and_scaling_icm456xy(void); void fifo_reset(); - uint16_t calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const; + uint16_t calculate_fast_sampling_backend_rate(uint16_t base_backend_rate, uint16_t max_backend_rate) const; /* Read samples from FIFO */ void read_fifo(); @@ -141,4 +141,5 @@ class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend float temp_filtered; LowPassFilter2pFloat temp_filter; + uint32_t sampling_rate_hz; }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index a62fd1eb2fc1a6..08c6d41792d0c5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -424,7 +424,7 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor() if (_drdy_pin_num_a >= 0) { _drdy_pin_a = hal.gpio->channel(_drdy_pin_num_a); if (_drdy_pin_a == nullptr) { - AP_HAL::panic("LSM9DS0: null accel data-ready GPIO channel\n"); + AP_HAL::panic("LSM9DS0: null accel data-ready GPIO channel"); } _drdy_pin_a->mode(HAL_GPIO_INPUT); @@ -433,7 +433,7 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor() if (_drdy_pin_num_g >= 0) { _drdy_pin_g = hal.gpio->channel(_drdy_pin_num_g); if (_drdy_pin_g == nullptr) { - AP_HAL::panic("LSM9DS0: null gyro data-ready GPIO channel\n"); + AP_HAL::panic("LSM9DS0: null gyro data-ready GPIO channel"); } _drdy_pin_g->mode(HAL_GPIO_INPUT); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp index d95cfce0f0fcce..beef216fc265f2 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS1.cpp @@ -231,7 +231,7 @@ bool AP_InertialSensor_LSM9DS1::_init_sensor() if (_drdy_pin_num_xg >= 0) { _drdy_pin_xg = hal.gpio->channel(_drdy_pin_num_xg); if (_drdy_pin_xg == nullptr) { - AP_HAL::panic("LSM9DS1: null accel data-ready GPIO channel\n"); + AP_HAL::panic("LSM9DS1: null accel data-ready GPIO channel"); } _drdy_pin_xg->mode(HAL_GPIO_INPUT); } diff --git a/libraries/AP_InternalError/AP_InternalError.cpp b/libraries/AP_InternalError/AP_InternalError.cpp index 0f23b2d1d42771..c1319b112663bd 100644 --- a/libraries/AP_InternalError/AP_InternalError.cpp +++ b/libraries/AP_InternalError/AP_InternalError.cpp @@ -127,7 +127,7 @@ void AP_stack_overflow(const char *thread_name) } hal.util->persistent_data.fault_type = 42; // magic value if (!hal.util->get_soft_armed()) { - AP_HAL::panic("stack overflow %s\n", thread_name); + AP_HAL::panic("stack overflow %s", thread_name); } } @@ -137,7 +137,7 @@ void AP_memory_guard_error(uint32_t size) INTERNAL_ERROR(AP_InternalError::error_t::mem_guard); if (!hal.util->get_soft_armed()) { ::printf("memory guard error size=%u\n", unsigned(size)); - AP_HAL::panic("memory guard size=%u\n", unsigned(size)); + AP_HAL::panic("memory guard size=%u", unsigned(size)); } } diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 6d9db1a43b9a77..e2f18ef6fe100c 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -403,7 +403,7 @@ void AP_Logger_Backend::validate_WritePrioritisedBlock(const void *pBuffer, } else { strncpy(name, "?NM?", ARRAY_SIZE(name)); } - AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)\n", + AP_HAL::panic("Size mismatch for %u (%s) (expected=%u got=%u)", type, name, type_len, size); } } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 418b8de1c84920..3d388f9795f50f 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -135,6 +135,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include #include #include #include @@ -1159,6 +1160,7 @@ LOG_STRUCTURE_FROM_GPS \ { LOG_RSSI_MSG, sizeof(log_RSSI), \ "RSSI", "Qff", "TimeUS,RXRSSI,RXLQ", "s-%", "F--", true }, \ LOG_STRUCTURE_FROM_BARO \ +LOG_STRUCTURE_FROM_CANMANAGER \ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_POWR_MSG, sizeof(log_POWR), \ "POWR","QffHHB","TimeUS,Vcc,VServo,Flags,AccFlags,Safety", "svv---", "F00---", true }, \ @@ -1266,6 +1268,7 @@ enum LogMessages : uint8_t { LOG_RCOUT_MSG, LOG_RSSI_MSG, LOG_IDS_FROM_BARO, + LOG_IDS_FROM_CANMANAGER, LOG_POWR_MSG, LOG_MCU_MSG, LOG_IDS_FROM_AHRS, diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 8dc31bbfa33de3..0b64b1063bb0e2 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -655,18 +655,25 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati } #endif -// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame -// returns true on success -bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +// get attitude as a quaternion. returns true on success. +// att_quat will be an earth-frame quaternion rotated such that +// yaw is in body-frame. +bool AP_Mount::get_attitude_quaternion(uint8_t instance, Quaternion& att_quat) { auto *backend = get_instance(instance); if (backend == nullptr) { return false; } + return backend->get_attitude_quaternion(att_quat); +} +// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame +// returns true on success +bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg) +{ // re-use get_attitude_quaternion and convert to Euler angles Quaternion att_quat; - if (!backend->get_attitude_quaternion(att_quat)) { + if (!get_attitude_quaternion(instance, att_quat)) { return false; } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 54762e915307fc..d34f5816368938 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -206,6 +206,11 @@ class AP_Mount bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const; #endif + // get attitude as a quaternion. returns true on success. + // att_quat will be an earth-frame quaternion rotated such that + // yaw is in body-frame. + bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat); + // get mount's current attitude in euler angles in degrees. yaw angle is in body-frame // returns true on success bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 67b673d656ebeb..62b46e0c4e275b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -271,26 +271,10 @@ class AP_Mount_Backend // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal) virtual bool suppress_heartbeat() const { return false; } -#if AP_MOUNT_POI_TO_LATLONALT_ENABLED - // calculate the Location that the gimbal is pointing at - void calculate_poi(); -#endif - // change to RC_TARGETTING mode if rc inputs have changed by more than the dead zone // should be called on every update void set_rctargeting_on_rcinput_change(); - // get pilot input (in the range -1 to +1) received through RC - void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const; - - // get angle or rate targets from pilot RC - // target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s - void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const; - - // get angle targets (in radians) to a Location - // returns true on success, false on failure - bool get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const WARN_IF_UNUSED; - // get angle targets (in radians) to ROI location // returns true on success, false on failure bool get_angle_target_to_roi(MountTarget& angle_rad) const WARN_IF_UNUSED; @@ -319,7 +303,6 @@ class AP_Mount_Backend uint8_t _instance; // this instance's number MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) - bool _yaw_lock; // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame // structure for MAVLink Targeting angle and rate targets struct { @@ -328,6 +311,26 @@ class AP_Mount_Backend MountTarget rate_rads; // rate target in rad/s } mnt_target; +private: + + // get pilot input (in the range -1 to +1) received through RC + void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const; + + // get angle or rate targets from pilot RC + // target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s + void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const; + + // get angle targets (in radians) to a Location + // returns true on success, false on failure + bool get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const WARN_IF_UNUSED; + +#if AP_MOUNT_POI_TO_LATLONALT_ENABLED + // calculate the Location that the gimbal is pointing at + void calculate_poi(); +#endif + + bool _yaw_lock; // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame + #if AP_MOUNT_POI_TO_LATLONALT_ENABLED struct { HAL_Semaphore sem; // semaphore protecting this structure diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 98c8e58a6ca758..b58f2254dbbbff 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1,7 +1,7 @@ -#include "AP_NavEKF2.h" - #include "AP_NavEKF2_core.h" +#include "AP_NavEKF2.h" + #include #include #include diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 7e8dfadb0e5c5c..57b837d9e47d09 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1,8 +1,9 @@ +#include "AP_NavEKF3_core.h" + #include "AP_NavEKF3.h" #include -#include "AP_NavEKF3_core.h" #include #include #include diff --git a/libraries/AP_Networking/AP_Networking_CAN.cpp b/libraries/AP_Networking/AP_Networking_CAN.cpp index 875fece574b334..250e78b0228cb6 100644 --- a/libraries/AP_Networking/AP_Networking_CAN.cpp +++ b/libraries/AP_Networking/AP_Networking_CAN.cpp @@ -182,7 +182,11 @@ void AP_Networking_CAN::mcast_server(void) const uint16_t timeout_us = 2000; while (frame_buffers[bus]->peek(frame)) { - auto retcode = get_caniface(bus)->send(frame, + auto *cbus = get_caniface(bus); + if (cbus == nullptr) { + break; + } + auto retcode = cbus->send(frame, AP_HAL::micros64() + timeout_us, AP_HAL::CANIface::IsMAVCAN); if (retcode == 0) { diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index a29aed082c40aa..342257972f8d7e 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -75,9 +75,9 @@ #define AP_NETWORKING_DEFAULT_DHCP_ENABLE AP_NETWORKING_DHCP_AVAILABLE #endif -// Default Static IP Address: 192.168.13.14 +// Default Static IP Address: 192.168.144.14 #ifndef AP_NETWORKING_DEFAULT_STATIC_IP_ADDR -#define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR "192.168.13.14" +#define AP_NETWORKING_DEFAULT_STATIC_IP_ADDR "192.168.144.14" #endif // Default Netmask: 24 @@ -88,9 +88,9 @@ #endif -// Default Static IP Address: 192.168.13.1 +// Default Static IP Address: 192.168.144.1 #ifndef AP_NETWORKING_DEFAULT_STATIC_GW_ADDR -#define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR "192.168.13.1" +#define AP_NETWORKING_DEFAULT_STATIC_GW_ADDR "192.168.144.1" #endif // Default MAC Address: C2:AF:51:03:CF:46 @@ -109,7 +109,7 @@ #if AP_NETWORKING_TESTS_ENABLED #ifndef AP_NETWORKING_TEST_IP -#define AP_NETWORKING_TEST_IP "192.168.13.2" +#define AP_NETWORKING_TEST_IP "192.168.144.2" #endif #endif diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 9962780398a661..d23a10e085ae40 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -1616,7 +1616,7 @@ void AP_Param::load_defaults_file_from_filesystem(const char *default_file, bool #endif } else { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - AP_HAL::panic("Failed to load defaults from %s\n", default_file); + AP_HAL::panic("Failed to load defaults from %s", default_file); #else printf("Failed to load defaults from %s\n", default_file); #endif diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index 2b2a4094ff5528..79b0aca7f2433a 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -173,7 +173,7 @@ #define GOBJECTN(v, pname, name, class) { name, (const void *)&AP_PARAM_VEHICLE_NAME.v, {group_info : class::var_info}, 0, Parameters::k_param_ ## pname, AP_PARAM_GROUP } #define PARAM_VEHICLE_INFO { "", (const void *)&AP_PARAM_VEHICLE_NAME, {group_info : AP_Vehicle::var_info}, 0, Parameters::k_param_vehicle, AP_PARAM_GROUP } #define AP_VAREND { "", nullptr, {group_info : nullptr }, 0, 0, AP_PARAM_NONE } - +#define AP_GROUP_ELEM_IDX(subgrp_idx, grp_idx) (grp_idx << 6 | subgrp_idx) enum ap_var_type { AP_PARAM_NONE = 0, diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index 409a3352e3a2c2..88d5f5b4f3e44f 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -92,21 +92,33 @@ void AP_Proximity_LD06::get_readings() // Loops through all bytes that were received while (nbytes-- > 0) { - // Gets and logs the current byte being read - const uint8_t c = _uart->read(); + uint8_t c; + if (!_uart->read(c)) { + break; + } // Stores the byte in an array if the byte is a start byte or we have already read a start byte - if (c == LD_START_CHAR || _response_data) { - - // Sets to true if a start byte has been read, default false otherwise - _response_data = true; - + if (c == LD_START_CHAR || _byte_count) { // Stores the next byte in an array _response[_byte_count] = c; + if (_byte_count < START_DATA_LENGTH) { + _byte_count++; + continue; + } + + const uint32_t total_packet_length = _response[START_DATA_LENGTH] + 3; + if (total_packet_length > ARRAY_SIZE(_response)) { + // invalid packet received; throw away all data and + // start again. + _byte_count = 0; + _uart->discard_input(); + break; + } + _byte_count++; - if (_byte_count == _response[START_DATA_LENGTH] + 3) { - + if (_byte_count == total_packet_length) { + const uint32_t current_ms = AP_HAL::millis(); // Stores the last distance taken, used to reduce number of readings taken @@ -121,7 +133,6 @@ void AP_Proximity_LD06::get_readings() // Resets the bytes read and whether or not we are reading data to accept a new payload _byte_count = 0; - _response_data = false; } } } diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.h b/libraries/AP_Proximity/AP_Proximity_LD06.h index c51adbb102e821..af8bd37edd7df6 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.h +++ b/libraries/AP_Proximity/AP_Proximity_LD06.h @@ -57,7 +57,6 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial // Store and keep track of the bytes being read from the sensor uint8_t _response[MESSAGE_LENGTH_LD06]; - bool _response_data; uint16_t _byte_count; // Store for error-tracking purposes diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp index f0f7cafdea6af9..2ceb6c0ed6aa96 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp @@ -45,7 +45,7 @@ AP_RCProtocol_SRXL2::AP_RCProtocol_SRXL2(AP_RCProtocol &_frontend) : AP_RCProtoc { #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) if (_singleton != nullptr) { - AP_HAL::panic("Duplicate SRXL2 handler\n"); + AP_HAL::panic("Duplicate SRXL2 handler"); } _singleton = this; @@ -64,12 +64,12 @@ void AP_RCProtocol_SRXL2::_bootstrap(uint8_t device_id) // Init the local SRXL device if (!srxlInitDevice(device_id, SRXL_DEVICE_PRIORITY, SRXL_DEVICE_INFO, device_id)) { - AP_HAL::panic("Failed to initialize SRXL2 device\n"); + AP_HAL::panic("Failed to initialize SRXL2 device"); } // Init the SRXL bus: The bus index must always be < SRXL_NUM_OF_BUSES -- in this case, it can only be 0 if (!srxlInitBus(0, 0, SRXL_SUPPORTED_BAUD_RATES)) { - AP_HAL::panic("Failed to initialize SRXL2 bus\n"); + AP_HAL::panic("Failed to initialize SRXL2 bus"); } _device_id = device_id; diff --git a/libraries/AP_RCProtocol/spm_srxl.cpp b/libraries/AP_RCProtocol/spm_srxl.cpp index 4c8653a3f2e931..de5a923ec86359 100644 --- a/libraries/AP_RCProtocol/spm_srxl.cpp +++ b/libraries/AP_RCProtocol/spm_srxl.cpp @@ -74,27 +74,27 @@ const uint16_t srxlCRCTable[] = /// PUBLIC VARIABLES /// -SrxlChannelData srxlChData = {0, 0, 0, {0}}; -SrxlTelemetryData srxlTelemData = {0}; +SrxlChannelData srxlChData; +SrxlTelemetryData srxlTelemData; SrxlVtxData srxlVtxData = {0, 0, 1, 0, 0, 1}; /// LOCAL VARIABLES /// -static SrxlDevice srxlThisDev = {0}; +static SrxlDevice srxlThisDev; static SrxlBus srxlBus[SRXL_NUM_OF_BUSES]; static bool srxlChDataIsFailsafe = false; static bool srxlTelemetryPhase = false; #ifdef SRXL_INCLUDE_MASTER_CODE -static uint32_t srxlFailsafeChMask = 0; // Tracks all active channels for use during failsafe transmission +static uint32_t srxlFailsafeChMask; // Tracks all active channels for use during failsafe transmission #endif -static SrxlBindData srxlBindInfo = {0, 0, 0, 0}; -static SrxlReceiverStats srxlRx = {0}; -static uint16_t srxlTelemSuppressCount = 0; +static SrxlBindData srxlBindInfo; +static SrxlReceiverStats srxlRx; +static uint16_t srxlTelemSuppressCount; #ifdef SRXL_INCLUDE_FWD_PGM_CODE -static SrxlFullID srxlFwdPgmDevice = {0, 0}; // Device that should accept Forward Programming connection by default -static uint8_t srxlFwdPgmBuffer[FWD_PGM_MAX_DATA_SIZE] = {0}; -static uint8_t srxlFwdPgmBufferLength = 0; +static SrxlFullID srxlFwdPgmDevice; // Device that should accept Forward Programming connection by default +static uint8_t srxlFwdPgmBuffer[FWD_PGM_MAX_DATA_SIZE]; +static uint8_t srxlFwdPgmBufferLength; #endif // Include additional header and externs if using STM32 hardware acceleration diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index 77002b39cd610c..7f53cb27d56a67 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -93,6 +93,7 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void) add_scheduler_entry(50, 100); // heartbeat 10Hz add_scheduler_entry(5, 20); // parameters 50Hz (generally not active unless requested by the TX) add_scheduler_entry(50, 200); // baro_vario 5Hz + add_scheduler_entry(50, 200); // vario 5Hz add_scheduler_entry(50, 120); // Attitude and compass 8Hz add_scheduler_entry(200, 1000); // VTX parameters 1Hz add_scheduler_entry(1300, 500); // battery 2Hz diff --git a/libraries/AP_Radio/AP_Radio_bk2425.cpp b/libraries/AP_Radio/AP_Radio_bk2425.cpp index 773205cf8823e2..d9dd185d6a8977 100644 --- a/libraries/AP_Radio/AP_Radio_bk2425.cpp +++ b/libraries/AP_Radio/AP_Radio_bk2425.cpp @@ -140,7 +140,7 @@ AP_Radio_beken::AP_Radio_beken(AP_Radio &_radio) : bool AP_Radio_beken::init(void) { if (_irq_handler_ctx != nullptr) { - AP_HAL::panic("AP_Radio_beken: double instantiation of irq_handler\n"); + AP_HAL::panic("AP_Radio_beken: double instantiation of irq_handler"); } chVTObjectInit(&timeout_vt); _irq_handler_ctx = chThdCreateFromHeap(NULL, diff --git a/libraries/AP_Radio/AP_Radio_cc2500.cpp b/libraries/AP_Radio/AP_Radio_cc2500.cpp index cce9b4ffcf0f90..83ee84f9cec74e 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.cpp +++ b/libraries/AP_Radio/AP_Radio_cc2500.cpp @@ -85,7 +85,7 @@ bool AP_Radio_cc2500::init(void) { #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS if (_irq_handler_ctx != nullptr) { - AP_HAL::panic("AP_Radio_cc2500: double instantiation of irq_handler\n"); + AP_HAL::panic("AP_Radio_cc2500: double instantiation of irq_handler"); } chVTObjectInit(&timeout_vt); _irq_handler_ctx = chThdCreateFromHeap(NULL, diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp index fc27088469221c..43bebe982c6a10 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.cpp +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -264,7 +264,7 @@ bool AP_Radio_cypress::init(void) dev = hal.spi->get_device(CYRF_SPI_DEVICE); #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS if (_irq_handler_ctx != nullptr) { - AP_HAL::panic("AP_Radio_cypress: double instantiation of irq_handler\n"); + AP_HAL::panic("AP_Radio_cypress: double instantiation of irq_handler"); } chVTObjectInit(&timeout_vt); _irq_handler_ctx = chThdCreateFromHeap(NULL, diff --git a/libraries/AP_Scripting/applets/UniversalAutoLand.lua b/libraries/AP_Scripting/applets/UniversalAutoLand.lua new file mode 100644 index 00000000000000..282a644e5df051 --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.lua @@ -0,0 +1,178 @@ +--[[ Upon Arming , creates a four item mission consisting of: NAV_TAKEOFF, DO_LAND_START,Final Approach WP opposite bearing from HOME of heading used during takeoff, at TKOFF_ALT or SCR_USER3 above home, SCR_USER2 or 2X TKOFF_DIST, and a LAND waypoint at HOME and stops until next disarm/boot. SCR_USER1 is used to enable or disable it. +--]] +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local PARAM_TABLE_KEY = 80 +local PARAM_TABLE_PREFIX = "AUTOLAND_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add param table') + +-- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +--[[ + // @Param: AUTOLAND_ENABLE + // @DisplayName: AUTOLAND ENABLE + // @Description: enable AUTOLAND script action + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local AULND_ENABLE = bind_add_param('ENABLE', 1, 1) +local enable = AULND_ENABLE:get() + +--[[ + // @Param: AUTOLAND_WP_ALT + // @DisplayName: Final approach waypoint alt + // @Description: Altitude of final approach waypoint created by script + // @Range: 1 100 + // @Units: m + // @User: Standard +--]] +local AULND_ALT = bind_add_param('WP_ALT', 2, 0) +local final_wp_alt = AULND_ALT:get() +--[[ + // @Param: AUTOLAND_WP_DIST + // @DisplayName: Final approach waypoint distance + // @Description: Distance from landng point (HOME) to final approach waypoint created by script in the opposite direction of initial takeoff + // @Range: 0 1000 + // @Units: m + // @User: Standard +--]] +local AULND_DIST = bind_add_param('WP_DIST', 3, 0) +local final_wp_dist = AULND_DIST:get() + +FRAME_GLOBAL = 3 +NAV_WAYPOINT = 16 +NAV_TAKEOFF = 22 +NAV_LAND = 21 +DO_LAND_START = 189 + +TAKEOFF_PITCH = 15 + +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +local function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +function create_final_approach_WP(i,bearing,dist,alt) --index,degs,m,m + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + loc:offset_bearing(bearing,dist) ---degs and meters + + item:seq(i) + item:frame(FRAME_GLOBAL) + item:command(NAV_WAYPOINT) + item:param1(0) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(alt) + return item +end + +function create_takeoff_WP(alt) + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + + item:seq(1) + item:frame(FRAME_GLOBAL) + item:command(NAV_TAKEOFF) + item:param1(TAKEOFF_PITCH) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(alt) + return item +end + +function create_land_WP() + local item = mavlink_mission_item_int_t() + local loc = ahrs:get_home() + + item:seq(4) + item:frame(FRAME_GLOBAL) + item:command(NAV_LAND) + item:param1(15) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(loc:lat()) + item:y(loc:lng()) + item:z(0) + return item +end + +function create_do_land_start_WP() + local item = mavlink_mission_item_int_t() + + item:seq(2) + item:frame(FRAME_GLOBAL) + item:command(DO_LAND_START) + item:param1(0) + item:param2(0) + item:param3(0) + item:param4(0) + item:x(0) + item:y(0) + item:z(0) + return item +end + +function update() + if not arming:is_armed() then --if disarmed, wait until armed + mission_loaded = false + return update,1000 + end + + if not mission_loaded then --if first time after arm and enabled is valid then create mission + local home = ahrs:get_home() + local location = ahrs:get_location() + local speed = gps:ground_speed(0) + local alt = baro:get_altitude() + if location and home and speed > (0.5 * param:get("AIRSPEED_MIN")) then + local yaw = gps:ground_course(0) + mission:set_item(3,create_final_approach_WP(3,wrap_180(yaw+180),final_wp_dist,final_wp_alt)) + mission:set_item(4,create_land_WP()) + mission_loaded = true + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("Captured initial takeoff direction = %.1f at %.1f m and %.1f m/s",yaw, alt, speed)) + end + end + return update, 200 +end + +gcs:send_text(MAV_SEVERITY.INFO,"Loaded UniversalAutoLand.lua") +if enable == 1 then + if final_wp_dist == 0 or final_wp_alt ==0 then + gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Must set Final Waypoint alt and dist values!")) + return + end + mission:clear() + local item = mavlink_mission_item_int_t() + item:command(NAV_WAYPOINT) + mission:set_item(0,item) + mission:set_item(1,create_takeoff_WP(param:get("TKOFF_ALT"))) + mission:set_item(2,create_do_land_start_WP()) + gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Set Final Waypoint alt and dist values!")) + return update, 1000 +else + gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("Script disabled by AUTOLAND_ENABLE")) +return +end + diff --git a/libraries/AP_Scripting/applets/UniversalAutoLand.md b/libraries/AP_Scripting/applets/UniversalAutoLand.md new file mode 100644 index 00000000000000..b2b2b568905d95 --- /dev/null +++ b/libraries/AP_Scripting/applets/UniversalAutoLand.md @@ -0,0 +1,9 @@ +This script is intended to allow easy, unpre-planned operation at any location with the protection of a do-land-start autoland sequence for failsafes that accounts for takeoff direction (ie wind direction). Final approach objects must be considered before you launch. + +If enabled by AUTOLAND_ENABLE =1, setups up an autotakeoff waypoint as first waypoint and upon Arming , adds mission items consisting of: DO_LAND_START,Final Approach WP opposite bearing from HOME of heading used during takeoff, to AUTOLAND_WP_ALT above home, and at AUTOLAND_WP_DIST distancee, and a LAND waypoint at HOME and stops until next disarm/boot. Warnings are given if the AUTOLAND parameters are not set. + +In use you just arm and switch to AUTO, and then switch to other flight modes after takeoff is completed to fly around.....relatively assured that a failsafe (assuming defaults for Failsafe) will result in an autolanding in the correct direction. You can also just switch back to AUTO or RTL to do an autoland. + +Caveats: be sure you have tested and setup autoland and AUTOLAND parameters. Setting AUTOLAND_WP_ALT and _WP_DIST for a good glide path on a final approach is required (be aware of possible obstructions when using). Values of 400 meters distance and 55 meters altitude work well for typcial 1m wingspan/1 kg foam planes. RTL_AUTOLAND must be set and a value = 2 is recommended also. + +Be aware of obstacles that might come into play on a final approach. Be aware that occasionally final approach waypoints may not be exactly in line with the takeoff if the GPS signal quality is compromised during takeoff. diff --git a/libraries/AP_Scripting/applets/VTOL-quicktune.lua b/libraries/AP_Scripting/applets/VTOL-quicktune.lua index 29c5c3ae2646e7..b36e79bcc6d8ea 100644 --- a/libraries/AP_Scripting/applets/VTOL-quicktune.lua +++ b/libraries/AP_Scripting/applets/VTOL-quicktune.lua @@ -19,6 +19,7 @@ local MAV_SEVERITY_INFO = 6 local MAV_SEVERITY_NOTICE = 5 +local MAV_SEVERITY_CRITICAL = 2 local MAV_SEVERITY_EMERGENCY = 0 local PARAM_TABLE_KEY = 8 @@ -40,7 +41,7 @@ function bind_add_param(name, idx, default_value) end -- setup quicktune specific parameters -assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 14), 'could not add param table') +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 15), 'could not add param table') --[[ // @Param: QUIK_ENABLE @@ -170,6 +171,15 @@ local QUIK_MAX_REDUCE = bind_add_param('MAX_REDUCE', 13, 20) --]] local QUIK_OPTIONS = bind_add_param('OPTIONS', 14, 0) +--[[ + // @Param: QUIK_ANGLE_MAX + // @DisplayName: maximum angle error for tune abort + // @Description: If while tuning the angle error goes over this limit then the tune will aborts to prevent a bad oscillation in the case of the tuning algorithm failing. If you get an error "Tuning: attitude error ABORTING" and you think it is a false positive then you can either raise this parameter or you can try increasing the QUIK_DOUBLE_TIME to do the tune more slowly. A value of zero disables this check. + // @Units: deg + // @User: Standard +--]] +local QUIK_ANGLE_MAX = bind_add_param('ANGLE_MAX', 15, 10) + local OPTIONS_TWO_POSITION = (1<<0) local INS_GYRO_FILTER = bind_param("INS_GYRO_FILTER") @@ -222,6 +232,7 @@ local tune_done_time = nil local slew_parm = nil local slew_target = 0 local slew_delta = 0 +local aborted = false local axes_done = {} local filters_done = {} @@ -511,6 +522,14 @@ function update() sw_pos_tune = 2 sw_pos_save = -1 end + if aborted then + if sw_pos == 0 then + aborted = false + else + -- after an abort require low switch position to reset + return + end + end if sw_pos == sw_pos_tune and (not arming:is_armed() or not vehicle:get_likely_flying()) and get_time() > last_warning + 5 then gcs:send_text(MAV_SEVERITY_EMERGENCY, string.format("Tuning: Must be flying to tune")) last_warning = get_time() @@ -523,6 +542,7 @@ function update() restore_all_params() gcs:send_text(MAV_SEVERITY_EMERGENCY, string.format("Tuning: reverted")) tune_done_time = nil + aborted = true end reset_axes_done() return @@ -539,6 +559,19 @@ function update() return end + if QUIK_ANGLE_MAX:get() > 0 and need_restore then + local att_error = AC_AttitudeControl:get_att_error_angle_deg() + if att_error > QUIK_ANGLE_MAX:get() then + need_restore = false + restore_all_params() + gcs:send_text(MAV_SEVERITY_CRITICAL, string.format("Tuning: attitude error %.1fdeg - ABORTING", att_error)) + tune_done_time = nil + aborted = true + reset_axes_done() + return + end + end + if get_time() - last_stage_change < STAGE_DELAY then update_slew_gain() return diff --git a/libraries/AP_Scripting/applets/follow-target-send.lua b/libraries/AP_Scripting/applets/follow-target-send.lua new file mode 100644 index 00000000000000..461979aa1ae7ab --- /dev/null +++ b/libraries/AP_Scripting/applets/follow-target-send.lua @@ -0,0 +1,121 @@ +-- Send the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one +-- +-- How To Use +-- 1. copy this script to the autopilot's "scripts" directory +-- 2. within the "scripts" directory create a "modules" directory +-- 3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory +-- 4. the FOLLOW_TARGET message will be published at 10hz + +-- load mavlink message definitions from modules/MAVLink directory +local mavlink_msgs = require("MAVLink/mavlink_msgs") + +-- global definitions +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local UPDATE_INTERVAL_MS = 100 -- update at about 10hz +local FOLLOW_TARGET_CAPABILITIES = {POS=2^0, VEL=2^1, ACCEL=2^2, ATT_RATES=2^3} + + -- setup script specific parameters +local PARAM_TABLE_KEY = 88 +local PARAM_TABLE_PREFIX = "FOLT_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table') + +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', PARAM_TABLE_PREFIX .. name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +--[[ + // @Param: FOLT_ENABLE + // @DisplayName: Follow Target Send Enable + // @Description: Follow Target Send Enable + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local FOLT_ENABLE = bind_add_param("ENABLE", 1, 1) + +--[[ + // @Param: FOLT_MAV_CHAN + // @DisplayName: Follow Target Send MAVLink Channel + // @Description: MAVLink channel to which FOLLOW_TARGET should be sent + // @Range: 0 10 + // @User: Standard +--]] +local FOLT_MAV_CHAN = bind_add_param("MAV_CHAN", 2, 0) + +-- initialize MAVLink rx with number of messages, and buffer depth +mavlink:init(2, 5) + +-- send FOLLOW_TARGET message +local function send_follow_target_msg() + + -- get vehicle location + local curr_loc = ahrs:get_location() + if curr_loc == nil then + do return end + end + local capabilities = FOLLOW_TARGET_CAPABILITIES.POS + + -- get vehicle target velocity in m/s in NED frame + local vel_target_NED = poscontrol:get_vel_target() + if vel_target_NED ~= nil then + capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.VEL + else + vel_target_NED = Vector3f() + end + + -- get vehicle target acceleration in m/s/s in NED frame + local accel_target_NED = poscontrol:get_accel_target() + if accel_target_NED ~= nil then + capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.ACCEL + else + accel_target_NED = Vector3f() + end + + -- get vehicle current attitude as quaternion and rates + local attitude_quat = ahrs:get_quaternion() + local curr_rot_rate = ahrs:get_gyro() + if attitude_quat ~= nil and curr_rot_rate ~= nil then + capabilities = capabilities + FOLLOW_TARGET_CAPABILITIES.ATT_RATES + else + attitude_quat = Quaternion() + curr_rot_rate = Vector3f() + end + local curr_rot_rate_NED = ahrs:body_to_earth(curr_rot_rate) + + -- prepare FOLLOW_TARGET message + local follow_target_msg = {} + follow_target_msg.timestamp = millis():toint() + follow_target_msg.est_capabilities = capabilities + follow_target_msg.lat = curr_loc:lat() + follow_target_msg.lon = curr_loc:lng() + follow_target_msg.alt = curr_loc:alt() * 0.01 + follow_target_msg.vel = {vel_target_NED:x(), vel_target_NED:y(), vel_target_NED:z()} + follow_target_msg.acc = {accel_target_NED:x(), accel_target_NED:y(), accel_target_NED:z()} + follow_target_msg.attitude_q = {attitude_quat:q1(), attitude_quat:q2(), attitude_quat:q3(), attitude_quat:q4()} + follow_target_msg.rates = {curr_rot_rate_NED:x(), curr_rot_rate_NED:y(), curr_rot_rate_NED:z()} + follow_target_msg.position_cov = {0, 0, 0} + follow_target_msg.custom_state = 0 + + -- send FOLLOW_TARGET message + mavlink:send_chan(FOLT_MAV_CHAN:get(), mavlink_msgs.encode("FOLLOW_TARGET", follow_target_msg)) +end + +-- display welcome message +gcs:send_text(MAV_SEVERITY.INFO, "follow-target-send script loaded") + +-- update function to receive location from payload and move vehicle to reduce payload's oscillation +local function update() + + -- exit immediately if not enabled + if (FOLT_ENABLE:get() <= 0) then + return update, 1000 + end + + -- send FOLLOW_TARGET message + send_follow_target_msg() + + return update, UPDATE_INTERVAL_MS +end + +return update() diff --git a/libraries/AP_Scripting/applets/follow-target-send.md b/libraries/AP_Scripting/applets/follow-target-send.md new file mode 100644 index 00000000000000..a0b203d989ba42 --- /dev/null +++ b/libraries/AP_Scripting/applets/follow-target-send.md @@ -0,0 +1,15 @@ +# Follow Target Send + +Sends the FOLLOW_TARGET mavlink message to allow other vehicles to follow this one + +# Parameters + +- FOLT_ENABLE : Set to 1 to enable this script +- FOLT_MAV_CHAN : MAVLink channel to which FOLLOW_TARGET should be sent + +# How To Use + +1. copy this script to the autopilot's "scripts" directory +2. within the "scripts" directory create a "modules" directory +3. copy the MAVLink/mavlink_msgs_xxx files to the "scripts" directory +4. the FOLLOW_TARGET message will be published at 10hz diff --git a/libraries/AP_Scripting/applets/video-stream-information.lua b/libraries/AP_Scripting/applets/video-stream-information.lua new file mode 100644 index 00000000000000..503149a272590a --- /dev/null +++ b/libraries/AP_Scripting/applets/video-stream-information.lua @@ -0,0 +1,302 @@ + --[[ + Populate the VIDEO_STREAM_INFORMATION message based on user parameters + --]] + +local PARAM_TABLE_KEY = 87 +local PARAM_TABLE_PREFIX = "VID1_" + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local VID_TYPE_ENUM = {RTSP=0, RTPUDP=1, TCP_MPEG=2, MPEG_TS=3} +local CAMMODEL_ENUM = {UNKNOWN=0, SIYI_A8=1, SIYI_ZR10=2, SIYI_ZR30=3, SIYI_ZT30_ZOOM=4, SIYI_ZT30_WIDE=5, + SIYI_ZT30_IR=6, SIYI_ZT6_RGB=7, SIYI_ZT6_IR=8, HERELINK_WIFIAP=9, HERELINK_USB_TETHERING=10, + TOPOTEK_1080P=11, TOPOTEK_480P=12, VIEWPRO=13} +local TEXT_PREFIX_STR = "video-stream-information:" +local SIYI_IP_DEFAULT = '192.168.144.25:8554' +local TOPOTEK_IP_DEFAULT = '192.168.144.108:554' +local VIEWPRO_IP_DEFAULT = '192.168.2.119:554' + + -- add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + + -- setup script specific parameters + assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 15), 'could not add param table') + + --[[ + // @Param: VID1_CAMMODEL + // @DisplayName: Camera1 Video Stream Camera Model + // @Description: Video stream camera model + // @Values: 0:Unknown, 1:Siyi A8, 2:Siyi ZR10, 3:Siyi ZR30, 4:Siyi ZT30 Zoom, 5:Siyi ZT30 Wide, 6:Siyi ZT30 IR, 7:Siyi ZT6 RGB, 8:Siyi ZT6 IR, 9:Herelink WifiAP, 10:Herelink USB-tethering, 11:Topotek 1080p, 12:Topotek 480p, 13:Viewpro + // @User: Standard +--]] +-- values should match CAMMODEL_ENUM +local VID1_CAMMODEL = bind_add_param('CAMMODEL', 1, 0) + +--[[ + // @Param: VID1_ID + // @DisplayName: Camera1 Video Stream Id + // @Description: Video stream id + // @Range: 0 50 + // @User: Standard +--]] +local VID1_ID = bind_add_param('ID', 2, 1) + +--[[ + // @Param: VID1_TYPE + // @DisplayName: Camera1 Video Stream Type + // @Description: Video stream type + // @Values: 0:RTSP, 1:RTPUDP, 2:TCP_MPEG, 3:MPEG_TS + // @User: Standard +--]] +-- values should match VID_TYPE_ENUM +local VID1_TYPE = bind_add_param('TYPE', 3, 0) + +--[[ + // @Param: VID1_FLAG + // @DisplayName: Camera1 Video Stream Flags + // @Description: Video stream flags + // @Bitmask: 0:Running,1:Thermal,2:Thermal Range Enabled + // @User: Standard +--]] +local VID1_FLAG = bind_add_param('FLAG', 4, 1) + +--[[ + // @Param: VID1_FRAME_RATE + // @DisplayName: Camera1 Video Stream Frame Rate + // @Description: Video stream frame rate + // @Range: 0 50 + // @User: Standard +--]] +local VID1_FR = bind_add_param('FRAME_RATE', 5, 30) + +--[[ + // @Param: VID1_HRES + // @DisplayName: Camera1 Video Stream Horizontal Resolution + // @Description: Video stream horizontal resolution + // @Range: 0 4096 + // @User: Standard +--]] +local VID1_HRES = bind_add_param('HRES', 6, 1920) + +--[[ + // @Param: VID1_VRES + // @DisplayName: Camera1 Video Stream Vertical Resolution + // @Description: Video stream vertical resolution + // @Range: 0 4096 + // @User: Standard +--]] +local VID1_VRES = bind_add_param('VRES', 7, 1080) + + --[[ + // @Param: VID1_BITRATE + // @DisplayName: Camera1 Video Stream Bitrate + // @Description: Video stream bitrate + // @Range: 0 10000 + // @User: Standard +--]] +local VID1_BITR = bind_add_param('BITRATE', 8, 1500) + +--[[ + // @Param: VID1_HFOV + // @DisplayName: Camera1 Video Stream Horizontal FOV + // @Description: Video stream horizontal FOV + // @Range: 0 360 + // @User: Standard +--]] +local VID1_HFOV = bind_add_param('HFOV', 9, 0) + +--[[ + // @Param: VID1_ENCODING + // @DisplayName: Camera1 Video Stream Encoding + // @Description: Video stream encoding + // @Values: 0:Unknown, 1:H264, 2:H265 + // @User: Standard +--]] +local VID1_ENCODING = bind_add_param('ENCODING', 10, 1) + +--[[ + // @Param: VID1_IPADDR0 + // @DisplayName: Camera1 Video Stream IP Address 0 + // @Description: Video stream IP Address first octet + // @Range: 0 255 + // @User: Standard +--]] +local VID1_IPADDR0 = bind_add_param('IPADDR0', 11, -1) + +--[[ + // @Param: VID1_IPADDR1 + // @DisplayName: Camera1 Video Stream IP Address 1 + // @Description: Video stream IP Address second octet + // @Range: 0 255 + // @User: Standard +--]] +local VID1_IPADDR1 = bind_add_param('IPADDR1', 12, -1) + +--[[ + // @Param: VID1_IPADDR2 + // @DisplayName: Camera1 Video Stream IP Address 2 + // @Description: Video stream IP Address third octet + // @Range: 0 255 + // @User: Standard +--]] +local VID1_IPADDR2 = bind_add_param('IPADDR2', 13, -1) + +--[[ + // @Param: VID1_IPADDR3 + // @DisplayName: Camera1 Video Stream IP Address 3 + // @Description: Video stream IP Address fourth octet + // @Range: 0 255 + // @User: Standard +--]] +local VID1_IPADDR3 = bind_add_param('IPADDR3', 14, -1) + +--[[ + // @Param: VID1_IPPORT + // @DisplayName: Camera1 Video Stream IP Address Port + // @Description: Video stream IP Address Port + // @Range: 0 65535 + // @User: Standard +--]] +local VID1_IPPORT = bind_add_param('IPPORT', 15, -1) + +function set_video_stream_information() + local INSTANCE = 0 + local name = 'Video' + + -- set defaults by camera model + local uri_ip = '' + local uri_suffix = '' + local hfov = 50 + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_A8 then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/main.264' + hfov = 81 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_ZR10 then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/main.264' + hfov = 62 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_ZR30 then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/main.264' + hfov = 58 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_ZT30_ZOOM then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/video2' + hfov = 58 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_ZT30_WIDE then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/video2' + hfov = 85 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_ZT30_IR then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/video1' + hfov = 24 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_ZT6_RGB then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/video2' + hfov = 85 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.SIYI_ZT6_IR then + uri_ip = SIYI_IP_DEFAULT + uri_suffix = '/video1' + hfov = 32 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.HERELINK_WIFIAP then + uri_ip = '192.168.42.1:8554' + uri_suffix = '/fpv_stream' + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.HERELINK_USB_TETHERING then + uri_ip = '192.168.42.129:8554' + uri_suffix = '/fpv_stream' + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.TOPOTEK_1080P then + uri_ip = TOPOTEK_IP_DEFAULT + uri_suffix = '/stream=0' + hfov = 50 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.TOPOTEK_480P then + uri_ip = TOPOTEK_IP_DEFAULT + uri_suffix = '/stream=1' + hfov = 50 + end + if VID1_CAMMODEL:get() == CAMMODEL_ENUM.VIEWPRO then + uri_ip = VIEWPRO_IP_DEFAULT + end + + -- calculate hfov + if VID1_HFOV:get() ~= 0 then + hfov = VID1_HFOV:get() + end + + -- construct uri + if VID1_IPADDR0:get() > 0 or VID1_IPADDR1:get() > 0 or + VID1_IPADDR2:get() > 0 or VID1_IPADDR3:get() > 0 or + VID1_IPPORT:get() > 0 then + uri_ip = math.floor(VID1_IPADDR0:get()) .. '.' .. + math.floor(VID1_IPADDR1:get()) .. '.' .. + math.floor(VID1_IPADDR2:get()) .. '.' .. + math.floor(VID1_IPADDR3:get()) .. ':' .. + math.floor(VID1_IPPORT:get()) + end + local uri + if VID1_TYPE:get() == VID_TYPE_ENUM.RTPUDP or VID1_TYPE:get() == VID_TYPE_ENUM.MPEG_TS then + -- sanity check port number + if VID1_IPPORT:get() < 0 then + gcs:send_text(MAV_SEVERITY.ERROR, TEXT_PREFIX_STR .. "check VID1_IPPORT") + do return end + end + uri = tostring(math.floor(VID1_IPPORT:get())) + elseif VID1_TYPE:get() == VID_TYPE_ENUM.RTSP then + -- sanity check IP address + if uri_ip == '' then + gcs:send_text(MAV_SEVERITY.ERROR, TEXT_PREFIX_STR .. "check VID1_IPADDR params") + do return end + end + uri = 'rtsp://' .. uri_ip .. uri_suffix + else + uri = uri_ip .. uri_suffix + end + + -- create Video Stream Information message + local stream_info = mavlink_video_stream_information_t() + stream_info:stream_id(VID1_ID:get()) + stream_info:count(1) -- hard coded to just a single stream + stream_info:type(VID1_TYPE:get()) + stream_info:flags(VID1_FLAG:get()) + stream_info:framerate(VID1_FR:get()) + stream_info:resolution_h(VID1_HRES:get()) + stream_info:resolution_v(VID1_VRES:get()) + stream_info:bitrate(VID1_BITR:get()) + stream_info:rotation(0) -- video image rotation clockwise, hardcoded to zero + stream_info:hfov(hfov) + stream_info:encoding(VID1_ENCODING:get()) + + for i = 0, #name do + stream_info:name(i, name:byte(i+1)) + end + for i = 0, #uri do + stream_info:uri(i, uri:byte(i+1)) + end + + -- update camera library with the latest stream information + camera:set_stream_information(INSTANCE, stream_info) +end + +-- print welcome message +gcs:send_text(MAV_SEVERITY.INFO, "video-stream-information script loaded") + +-- update function runs every 5 secs +function update() + set_video_stream_information() + return update, 5000 +end + +return update() diff --git a/libraries/AP_Scripting/applets/video-stream-information.md b/libraries/AP_Scripting/applets/video-stream-information.md new file mode 100644 index 00000000000000..ebd1c998d13a5f --- /dev/null +++ b/libraries/AP_Scripting/applets/video-stream-information.md @@ -0,0 +1,32 @@ +# Video Stream Information + +This script updates the ArduPilot Camera library with the values required to populate the VIDEO_STREAM_INFORMATION +mavlink message allow the GCS to find and display live video to the user + +# Parameters + +- VID1_CAMMODEL: Video stream camera model (1:Siyi A8, 2:Siyi ZR10, 3:Siyi ZR30, 4:Siyi ZT30 Zoom, 5:Siyi ZT30 Wide, 6:Siyi ZT30 IR, 7:Siyi ZT6 RGB, 8:Siyi ZT6 IR, 9:Herelink WifiAP, 10:Herelink USB-tethering, 11:Topotek 1080p, 12:Topotek 480p, 13:Viewpro) +- VID1_ID: Video stream id +- VID1_TYPE: Video stream type (0:RTSP, 1:RTPUDP, 2:TCP_MPEG, 3:MPEG_TS) +- VID1_FLAG: Video stream flags (Bitmask: 0:Running,1:Thermal,2:Thermal Range Enabled) +- VID1_FRAME_RATE: Video stream frame rate +- VID1_HRES: Video stream horizontal resolution +- VID1_VRES: Video stream vertical resolution +- VID1_BITR: Video stream bitrate +- VID1_HFOV: Video stream horizontal FOV in degrees +- VID1_ENCODING: Video stream encoding (0:Unknown, 1:H264, 2:H265) +- VID1_IPADDR0: Video stream IP Address first octet +- VID1_IPADDR1: Video stream IP Address second octet +- VID1_IPADDR2: Video stream IP Address third octet +- VID1_IPADDR3: Video stream IP Address fourth octet +- VID1_IPPORT: Video Stream IP Address Port + +# How To Use + +1. Setup the camera gimbal as described on the ArduPilot wiki including ethernet setup +2. Check the IP address of the camera gimbal +3. Setup scripting per https://ardupilot.org/plane/docs/common-lua-scripts.html and reboot the autopilot +4. Copy this script to the vehicle autopilot's "scripts" directory +5. Set the VID1_CAMMODEL parameter to the camera gimbal model and adjust the new VID1 params as required +6. If necessary using the GCS's mavlink inspector to confirm the VIDEO_STREAM_INFORMATION URI field is correct +7. Confirm the ground station can display the live video diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 677d0baadfab1c..4e15c6d33d0330 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1686,6 +1686,14 @@ function mavlink_video_stream_information_t_ud:uri(index) end ---@param value integer function mavlink_video_stream_information_t_ud:uri(index, value) end +-- get field +---@return integer +function mavlink_video_stream_information_t_ud:encoding() end + +-- set field +---@param value integer +function mavlink_video_stream_information_t_ud:encoding(value) end + -- Populate the fields of the VIDEO_STREAM_INFORMATION message ---@param instance integer ---@param stream_info mavlink_video_stream_information_t_ud @@ -3753,6 +3761,10 @@ AC_AttitudeControl = {} ---@return number -- yaw slew rate function AC_AttitudeControl:get_rpy_srate() end +-- Return the angle between the target thrust vector and the current thrust vector in degrees. +---@return number -- attitude error +function AC_AttitudeControl:get_att_error_angle_deg() end + -- desc AR_AttitudeControl = {} @@ -3777,6 +3789,14 @@ function poscontrol:set_posvelaccel_offset(pos_offset_NED, vel_offset_NED, accel ---@return Vector3f_ud|nil function poscontrol:get_posvelaccel_offset() end +-- get position controller's target velocity in m/s in NED frame +---@return Vector3f_ud|nil +function poscontrol:get_vel_target() end + +-- get position controller's target acceleration in m/s/s in NED frame +---@return Vector3f_ud|nil +function poscontrol:get_accel_target() end + -- desc AR_PosControl = {} diff --git a/libraries/AP_Scripting/examples/glide_into_wind.lua b/libraries/AP_Scripting/examples/glide_into_wind.lua new file mode 100644 index 00000000000000..389ac155eb8d63 --- /dev/null +++ b/libraries/AP_Scripting/examples/glide_into_wind.lua @@ -0,0 +1,399 @@ +-- Glide into wind, LUA script for glide into wind functionality + +-- Background +-- When flying a fixed-wing drone on ad-hoc BVLOS missions, it might not be +-- suitable for the drone to return to home if the C2 link is lost, since that +-- might mean flying without control for an extended time and distance. One +-- option in ArduPlane is to set FS_Long to Glide, which makes the drone glide +-- and land in the direction it happened to have when the command was invoked, +-- without regard to the wind. This script offers a way to decrease the kinetic +-- energy in this blind landing by means of steering the drone towards the wind +-- as GLIDE is initiated, hence lowering the ground speed. The intention is to +-- minimize impact energy at landing - foremost for any third party, but also to +-- minimize damage to the drone. + +-- Functionality and setup +-- 1. Set SCR_ENABLE = 1 +-- 2. Put script in scripts folder, boot twice +-- 3. A new parameter has appeared: +-- - GLIDE_WIND_ENABL (0=disable, 1=enable) +-- 4. Set GLIDE_WIND_ENABL = 1 +-- 5. Read the docs on FS: +-- https://ardupilot.org/plane/docs/apms-failsafe-function.html#failsafe-parameters-and-their-meanings +-- 6. Set FS_LONG_ACTN = 2 +-- 7. Set FS_LONG_TIMEOUT as appropriate +-- 8. Set FS_GCS_ENABL = 1 +-- 9. If in simulation, set SIM_WIND_SPD = 4 to get a reliable wind direction. +-- 10. Test in simulation: Fly a mission, disable heartbeats by typing 'set +-- heartbeat 0' into mavproxy/SITL, monitor what happens in the console. If +-- QGC or similar GCS is used, make sure it does not send heartbeats. +-- 11. Test in flight: Fly a mission, monitor estimated wind direction from GCS, +-- then fail GCS link and see what happens. +-- 12. Once heading is into wind script will stop steering and not steer again +-- until state machine is reset and failsafe is triggered again. Steering in low +-- airspeeds (thr=0) increases risks of stall and it is preferable touch +-- ground in level roll attitude. If the script parameter hdg_ok_lim is set +-- to tight or the wind estimate is not stable, the script will anyhow stop +-- steering after override_time_lim and enter FBWA - otherwise the script +-- would hinder the GLIDE fail safe. +-- 13. Script will stop interfering as soon as a new goto-point is received or +-- the flight mode is changed by the operator or the remote pilot. + +-- During the fail safe maneuver a warning tune is played. + +-- State machine +-- CAN_TRIGGER +-- - Do: Nothing +-- - Change state: If the failsafe GLIDE is triggered: if FS_GCS_ENABL is set +-- and FS_LONG_ACTN is 2, change to TRIGGERED else change to CANCELED +-- +-- TRIGGERED +-- - Do: First use GUIDED mode to steer into wind, then switch to FBWA to +-- Glide into wind. Play warning tune. +-- - Change state: If flight mode is changed by operator/remote pilot or +-- operator/remote pilot sends a new goto point, change state to CANCELED +-- +-- CANCELED +-- - Do: Nothing +-- - Change state: When new heart beat arrive, change state to CAN_TRIGGER + +-- Credits +-- This script is developed by agising at UASolutions, commissioned by, and in +-- cooperation with Remote.aero, with funding from Swedish AeroEDIH, in response +-- to a need from the Swedish Sea Rescue Society (Sjöräddningssällskapet, SSRS). + +-- Disable diagnostics related to reading parameters to pass linter +---@diagnostic disable: need-check-nil +---@diagnostic disable: param-type-mismatch + +-- Tuning parameters +local looptime = 250 -- Short looptime +local long_looptime = 2000 -- Long looptime, GLIDE_WIND is not enabled +local tune_repeat_t = 1000 -- How often to play tune in glide into wind, [ms] +local hdg_ok_lim = 15 -- Acceptable heading error in deg (when to stop steering) +local hdg_ok_t_lim = 5000 -- Stop steering towards wind after hdg_ok_t_lim ms with error less than hdg_ok_lim +local override_time_lim = 15000 -- Max time in GUIDED during GLIDE, after limit set FBWA independent of hdg + +-- GCS text levels +local _INFO = 6 +local _WARNING = 4 + +-- Plane flight modes mapping +local mode_FBWA = 5 +local mode_GUIDED = 15 + +-- Tunes +local _tune_glide_warn = "MFT240 L16 cdefgfgfgfg" -- The warning tone played during GLIDE_WIND + +--State variable +local fs_state = nil + +-- Flags +local override_enable = false -- Flag to allow RC channel loverride + +-- Variables +local wind_dir_rad = nil -- param for wind dir in rad +local wind_dir_180 = nil -- param for wind dir in deg +local hdg_error = nil -- Heading error, hdg vs wind_dir_180 +local gw_enable = nil -- glide into wind enable flag +local hdg = nil -- vehicle heading +local wind = Vector3f() -- wind 3Dvector +local link_lost_for = nil -- link loss time counter +local last_seen = nil -- timestamp last received heartbeat +local tune_time_since = 0 -- Timer for last played tune +local hdg_ok_t = 0 -- Timer +local expected_flight_mode = nil -- Flight mode set by this script +local location_here = nil -- Current location +local location_upwind = nil -- Location to hold the target location +local user_notified = false -- Flag to keep track user being notified or not +local failed_location_counter = 0 -- Counter for failed location requests, possible GPS denied +local upwind_distance = 500 -- Distance to the upwind location, minimum 4x turn radius +local override_time = 0 -- Time since override started in ms + +-- Add param table +local PARAM_TABLE_KEY = 74 +local PARAM_TABLE_PREFIX = "GLIDE_WIND_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table') + +------- +-- Init +------- + +function _init() + -- Add and init paramters + GLIDE_WIND_ENABL = bind_add_param('ENABL', 1, 0) + + -- Init parameters + FS_GCS_ENABL = bind_param('FS_GCS_ENABL') -- Is set to 1 if GCS lol should trigger FS after FS_LONG_TIMEOUT + FS_LONG_TIMEOUT = bind_param('FS_LONG_TIMEOUT') -- FS long timeout in seconds + FS_LONG_ACTN = bind_param('FS_LONG_ACTN') -- Is set to 2 for Glide + + send_to_gcs(_INFO, 'LUA: FS_LONG_TIMEOUT timeout: ' .. FS_LONG_TIMEOUT:get() .. 's') + + -- Test paramter + if GLIDE_WIND_ENABL:get() == nil then + send_to_gcs(_INFO, 'LUA: Something went wrong, GLIDE_WIND_ENABL not created') + return _init(), looptime + else + gw_enable = GLIDE_WIND_ENABL:get() + send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable) + end + + -- Init last_seen. + last_seen = gcs:last_seen() + + -- Init link_lost_for [ms] to FS_LONG_TIMEOUT [s] to prevent link to recover without + -- new heartbeat. This is to properly init the state machine. + link_lost_for = FS_LONG_TIMEOUT:get() * 1000 + + -- Warn if GLIDE_WIND_ENABL is set and FS_LONG_ACTN is not GLIDE + if gw_enable == 1 and FS_LONG_ACTN:get() ~= 2 then + send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.') + end + + -- Init fs_state machine to CANCELED. A heartbeat is required to set the state + -- to CAN_TRIGGER from where Glide into wind can be triggered. + fs_state = 'CANCELED' + + -- All set, go to update + return update(), long_looptime +end + + +------------ +-- Main loop +------------ + +function update() + -- Check if state of GLIDE_WIND_ENABL parameter changed, print every change + if gw_enable ~= GLIDE_WIND_ENABL:get() then + gw_enable = GLIDE_WIND_ENABL:get() + send_to_gcs(_INFO, 'LUA: GLIDE_WIND_ENABL: ' .. gw_enable) + -- If GLIDE_WIND_ENABL was enabled, warn if not FS_LONG_ACTN is set accordingly + if gw_enable == 1 then + if FS_LONG_ACTN:get() ~=2 then + send_to_gcs(_WARNING, 'GLIDE_WIND_ENABL is set, but FS_LONG_ACTN is not GLIDE.') + end + end + end + + -- -- If feature is not enabled, loop slowly + if gw_enable == 0 then + return update, long_looptime + end + + -- GLIDE_WIND_ENABL is enabled, look for triggers + -- Monitor time since last gcs heartbeat + if last_seen == gcs:last_seen() then + link_lost_for = link_lost_for + looptime + else + -- There has been a new heartbeat, update last_seen and reset link_lost_for + last_seen = gcs:last_seen() + link_lost_for = 0 + end + + -- Run the state machine + -- State CAN_TRIGGER + if fs_state == 'CAN_TRIGGER' then + if link_lost_for > FS_LONG_TIMEOUT:get() * 1000 then + -- Double check that FS_GCS_ENABL is set + if FS_GCS_ENABL:get() == 1 and FS_LONG_ACTN:get() == 2 then + fs_state = "TRIGGERED" + -- Reset some variables + hdg_ok_t = 0 + user_notified = false + override_enable = true + override_time = 0 + failed_location_counter = 0 + -- Set mode to GUIDED before entering TRIGGERED state + set_flight_mode(mode_GUIDED, 'LUA: Glide into wind state TRIGGERED') + else + -- Do not trigger glide into wind, require new heart beats to get here again + fs_state = "CANCELED" + end + end + -- State TRIGGERED + elseif fs_state == "TRIGGERED" then + -- Check for flight mode changes from outside script + if vehicle:get_mode() ~= expected_flight_mode then + fs_state = "CANCELED" + send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: flight mode change') + end + + -- In GUIDED, check for target location changes from outside script (operator) + if vehicle:get_mode() == mode_GUIDED then + if not locations_are_equal(vehicle:get_target_location(), location_upwind) then + fs_state = "CANCELED" + send_to_gcs(_INFO, 'LUA: Glide into wind state CANCELED: new goto-point') + end + end + + -- State CANCELED + elseif fs_state == "CANCELED" then + -- Await link is not lost + if link_lost_for < FS_LONG_TIMEOUT:get() * 1000 then + fs_state = "CAN_TRIGGER" + send_to_gcs(_INFO, 'LUA: Glide into wind state CAN_TRIGGER') + end + end + + -- State TRIGGERED actions + if fs_state == "TRIGGERED" then + -- Get the heading angle + hdg = math.floor(math.deg(ahrs:get_yaw())) + + -- Get wind direction. Function wind_estimate returns x and y for direction wind blows in, add pi to get true wind dir + wind = ahrs:wind_estimate() + wind_dir_rad = math.atan(wind:y(), wind:x())+math.pi + wind_dir_180 = math.floor(wrap_180(math.deg(wind_dir_rad))) + hdg_error = wrap_180(wind_dir_180 - hdg) + + -- Check if we are close to target heading + if math.abs(hdg_error) < hdg_ok_lim then + -- If we have been close to target heading for hdg_ok_t_lim, switch back to FBWA + if hdg_ok_t > hdg_ok_t_lim then + if override_enable then + set_flight_mode(mode_FBWA,'LUA: Glide into wind steering complete, GLIDE in FBWA') + end + -- Do not override again until state machine has triggered again + override_enable = false + else + hdg_ok_t = hdg_ok_t + looptime + end + -- Heading error is big, reset timer hdg_ok_t + else + hdg_ok_t = 0 + end + + -- Play tune every tune_repeat_t [ms] + if tune_time_since > tune_repeat_t then + -- Play tune and reset timer + send_to_gcs(_INFO, 'LUA: Play warning tune') + play_tune(_tune_glide_warn) + tune_time_since = 0 + else + tune_time_since = tune_time_since + looptime + end + + -- If not steered into wind yet, update goto point into wind + if override_enable then + -- Check override time, if above limit, switch back to FBWA + override_time = override_time + looptime + if override_time > override_time_lim then + set_flight_mode(mode_FBWA, "LUA: Glide into wind override time out, GLIDE in current heading") + override_enable = false + end + -- Get current position and handle if not valid + location_here = ahrs:get_location() + if location_here == nil then + -- In case we cannot get location for some time we must give up and continue with GLIDE + failed_location_counter = failed_location_counter + 1 + if failed_location_counter > 5 then + set_flight_mode(mode_FBWA, "LUA: Glide failed to get location, GLIDE in current heading") + override_enable = false + return update, looptime + end + gcs:send_text(_WARNING, "LUA: Glide failed to get location") + return update, looptime + end + -- Calc upwind position, copy and modify location_here + location_upwind = location_here:copy() + location_upwind:offset_bearing(wind_dir_180, upwind_distance) + + -- Set location_upwind as GUIDED target + if vehicle:set_target_location(location_upwind) then + if not user_notified then + send_to_gcs(_INFO, "LUA: Guided target set " .. upwind_distance .. "m away at bearing " .. wind_dir_180) + -- Just notify once + user_notified = true + end + else + -- Most likely we are not in GUIDED anymore (operator changed mode), state machine will handle this in next loop. + gcs:send_text(_WARNING, "LUA: Glide failed to set upwind target") + end + end + end + return update, looptime +end + + +------------------- +-- Helper functions +------------------- + +-- Set mode and wait for mode change +function set_flight_mode(mode, message) + expected_flight_mode = mode + vehicle:set_mode(expected_flight_mode) + return wait_for_mode_change(mode, message, 0) +end + +-- Wait for mode change +function wait_for_mode_change(mode, message, attempt) + -- If mode change does not go through after 10 attempts, give up + if attempt > 10 then + send_to_gcs(_WARNING, 'LUA: Glide into wind mode change failed.') + return update, looptime + -- If mode change does not go through, wait and try again + elseif vehicle:get_mode() ~= mode then + return wait_for_mode_change(mode, message, attempt + 1), 5 + -- Mode change has gone through + else + send_to_gcs(_INFO, message) + return update, looptime + end +end + +-- Function to compare two Location objects +function locations_are_equal(loc1, loc2) + -- If either location is nil, they are not equal + if not loc1 or not loc2 then + return false + end + -- Compare latitude and longitude, return bool + return loc1:lat() == loc2:lat() and loc1:lng() == loc2:lng() +end + +-- bind a parameter to a variable +function bind_param(name) + local p = Parameter() + assert(p:init(name), string.format('could not find %s parameter', name)) + return p +end + +-- Add a parameter and bind it to a variable +function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name)) + return bind_param(PARAM_TABLE_PREFIX .. name) +end + +-- Print to GCS +function send_to_gcs(level, mess) + gcs:send_text(level, mess) +end + +-- Play tune +function play_tune(tune) + notify:play_tune(tune) +end + +-- Returns the angle in range 0-360 +function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +-- Returns the angle in range -180-180 +function wrap_180(angle) + local res = wrap_360(angle) + if res > 180 then + res = res - 360 + end + return res +end + +-- Start up the script +return _init, 2000 diff --git a/libraries/AP_Scripting/examples/message_interval.lua b/libraries/AP_Scripting/examples/message_interval.lua index 48f844a987973c..bda75436b87cf7 100644 --- a/libraries/AP_Scripting/examples/message_interval.lua +++ b/libraries/AP_Scripting/examples/message_interval.lua @@ -44,6 +44,12 @@ gcs:send_text(MAV_SEVERITY.INFO, "Loaded message_interval.lua") function update() -- this is the loop which periodically runs for i = 1, #intervals do -- we want to iterate over every specified interval local channel, message, interval_hz = table.unpack(intervals[i]) -- this extracts the channel, MAVLink ID, and interval + + -- Lua checks get the unpacked types wrong, these are the correct types + ---@cast channel integer + ---@cast message uint32_t_ud + ---@cast interval_hz number + local interval_us = -1 if interval_hz > 0 then interval_us = math.floor(1000000 / interval_hz) -- convert the interval to microseconds diff --git a/libraries/AP_Scripting/examples/set_VIDEO_STREAM_INFORMATION.lua b/libraries/AP_Scripting/examples/set_VIDEO_STREAM_INFORMATION.lua deleted file mode 100644 index 7f8a20844e8ba4..00000000000000 --- a/libraries/AP_Scripting/examples/set_VIDEO_STREAM_INFORMATION.lua +++ /dev/null @@ -1,33 +0,0 @@ - --[[ - Populate the fields of the VIDEO_STREAM_INFORMATION message sent by the selected camera instance. - --]] - function set_video_stream_information() - -- set the Video Stream Information data - local stream_info = mavlink_video_stream_information_t() - - local INSTANCE = 0 - local name = 'Video' - local uri = '127.0.0.1:5600' - - stream_info:framerate(30) - stream_info:bitrate(1500) - stream_info:flags(1) -- VIDEO_STREAM_STATUS_FLAGS_RUNNING - stream_info:resolution_h(1920) - stream_info:resolution_v(1080) - stream_info:rotation(0) - stream_info:hfov(50) - stream_info:stream_id(1) - stream_info:count(1) - stream_info:type(1) -- VIDEO_STREAM_TYPE_RTPUDP - for i = 0, #name do - stream_info:name(i, name:byte(i+1)) - end - for i = 0, #uri do - stream_info:uri(i, uri:byte(i+1)) - end - - camera:set_stream_information(INSTANCE, stream_info) - -end - -return set_video_stream_information() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c8c72e337748a5..a9decc2c92c7e4 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -768,12 +768,15 @@ singleton AC_PrecLand method get_target_location boolean Location'Null include AC_AttitudeControl/AC_AttitudeControl.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AC_AttitudeControl depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AC_AttitudeControl method get_rpy_srate void float'Ref float'Ref float'Ref +singleton AC_AttitudeControl method get_att_error_angle_deg float include AC_AttitudeControl/AC_PosControl.h depends APM_BUILD_COPTER_OR_HELI singleton AC_PosControl depends APM_BUILD_COPTER_OR_HELI singleton AC_PosControl rename poscontrol singleton AC_PosControl method set_posvelaccel_offset boolean Vector3f Vector3f Vector3f singleton AC_PosControl method get_posvelaccel_offset boolean Vector3f'Null Vector3f'Null Vector3f'Null +singleton AC_PosControl method get_vel_target boolean Vector3f'Null +singleton AC_PosControl method get_accel_target boolean Vector3f'Null include APM_Control/AR_AttitudeControl.h depends APM_BUILD_TYPE(APM_BUILD_Rover) singleton AR_AttitudeControl depends APM_BUILD_TYPE(APM_BUILD_Rover) @@ -826,6 +829,7 @@ userdata mavlink_video_stream_information_t field count uint8_t'skip_check read userdata mavlink_video_stream_information_t field type uint8_t'skip_check read write userdata mavlink_video_stream_information_t field name'array 32 uint8_t'skip_check read write userdata mavlink_video_stream_information_t field uri'array 160 uint8_t'skip_check read write +userdata mavlink_video_stream_information_t field encoding uint8_t'skip_check read write include AP_Camera/AP_Camera.h singleton AP_Camera depends AP_CAMERA_ENABLED && (AP_CAMERA_SCRIPTING_ENABLED == 1) diff --git a/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua new file mode 100644 index 00000000000000..e5b853917c0f83 --- /dev/null +++ b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_FOLLOW_TARGET.lua @@ -0,0 +1,16 @@ +local FOLLOW_TARGET = {} +FOLLOW_TARGET.id = 144 +FOLLOW_TARGET.fields = { + { "timestamp", "begin(state[i].baudrate(), @@ -676,6 +672,15 @@ uint32_t AP_SerialManager::find_baudrate(enum SerialProtocol protocol, uint8_t i return _state->baudrate(); } +void AP_SerialManager::set_and_default_baud(enum SerialProtocol protocol, uint8_t instance, uint32_t _baud) +{ + const struct UARTState *_state = find_protocol_instance(protocol, instance); + if (_state == nullptr) { + return; + } + state->baud.set_and_default(_baud); +} + // find_portnum - find port number (SERIALn index) for a protocol and instance, -1 for not found int8_t AP_SerialManager::find_portnum(enum SerialProtocol protocol, uint8_t instance) const { diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index be4a2be0c3ef45..e093e75825e2d5 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -130,6 +130,8 @@ class AP_SerialManager { // accessors for AP_Periph to set baudrate and type void set_protocol_and_baud(uint8_t sernum, enum SerialProtocol protocol, uint32_t baudrate); + void set_and_default_baud(enum SerialProtocol protocol, uint8_t instance, uint32_t _baud); + static uint32_t map_baudrate(int32_t rate); // parameter var table diff --git a/libraries/AP_SerialManager/AP_SerialManager_config.h b/libraries/AP_SerialManager/AP_SerialManager_config.h index 22c2f871b3c1e9..4e8669c69e144b 100644 --- a/libraries/AP_SerialManager/AP_SerialManager_config.h +++ b/libraries/AP_SerialManager/AP_SerialManager_config.h @@ -113,8 +113,6 @@ #define AP_SERIALMANAGER_GIMBAL_BUFSIZE_RX 128 #define AP_SERIALMANAGER_GIMBAL_BUFSIZE_TX 128 -#define AP_SERIALMANAGER_VOLZ_BAUD 115 - #define AP_SERIALMANAGER_ROBOTIS_BUFSIZE_RX 128 #define AP_SERIALMANAGER_ROBOTIS_BUFSIZE_TX 128 diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 69575aab7a8205..b0cbe34087e29f 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -751,10 +751,10 @@ void AP_TECS::_update_throttle_with_airspeed(void) const float nomThr = aparm.throttle_cruise * 0.01f; const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add - // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced // drag increase during turns. - const float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); - STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); + const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); + STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f); const float ff_throttle = nomThr + STEdot_dem / K_thr2STE; // Calculate PD + FF throttle @@ -905,10 +905,10 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge, float pi // Calculate additional throttle for turn drag compensation including throttle nudging const Matrix3f &rotMat = _ahrs.get_rotation_body_to_ned(); // Use the demanded rate of change of total energy as the feed-forward demand, but add - // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced + // additional component which scales with (1/(cos(bank angle)**2) - 1) to compensate for induced // drag increase during turns. - float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); - float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi * cosPhi, 0.1f, 1.0f) - 1.0f); + const float cosPhi_squared = (rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y); + float STEdot_dem = _rollComp * (1.0f/constrain_float(cosPhi_squared, 0.1f, 1.0f) - 1.0f); _throttle_dem = _throttle_dem + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); constrain_throttle(); diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 4c4343b5ee950c..484177ed920d9c 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -219,9 +219,6 @@ class AP_TECS { AP_Float _pitch_ff_k; AP_Float _accel_gf; - // temporary _pitch_max_limit. Cleared on each loop. Clear when >= 90 - int8_t _pitch_max_limit = 90; - // current height estimate (above field elevation) float _height; diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 9f7d70bb24b0b4..4b6a95807fb72d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -35,11 +35,8 @@ extern AP_IOMCU iomcu; 2nd group of parameters */ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { -#if HAL_RUNCAM_ENABLED - // @Group: CAM_RC_ - // @Path: ../AP_Camera/AP_RunCam.cpp - AP_SUBGROUPINFO(runcam, "CAM_RC_", 1, AP_Vehicle, AP_RunCam), -#endif + + // 1: RunCam #if HAL_GYROFFT_ENABLED // @Group: FFT_ @@ -450,9 +447,6 @@ void AP_Vehicle::setup() gyro_fft.init(1000); #endif #endif -#if HAL_RUNCAM_ENABLED - runcam.init(); -#endif #if HAL_HOTT_TELEM_ENABLED hott_telem.init(); #endif @@ -615,9 +609,6 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #if HAL_NMEA_OUTPUT_ENABLED SCHED_TASK_CLASS(AP_NMEA_Output, &vehicle.nmea, update, 50, 50, 180), #endif -#if HAL_RUNCAM_ENABLED - SCHED_TASK_CLASS(AP_RunCam, &vehicle.runcam, update, 50, 50, 200), -#endif #if HAL_GYROFFT_ENABLED SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210), diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index f34be5accbfc90..f68350c8ca127f 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -47,7 +47,6 @@ #include #include // Serial manager library #include -#include #include #include #include @@ -373,9 +372,6 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_RSSI rssi; #endif -#if HAL_RUNCAM_ENABLED - AP_RunCam runcam; -#endif #if HAL_GYROFFT_ENABLED AP_GyroFFT gyro_fft; #endif diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp index c457627a4176bb..ff318b36fd624e 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp @@ -122,7 +122,7 @@ AP_VisualOdom::AP_VisualOdom() AP_Param::setup_object_defaults(this, var_info); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (_singleton != nullptr) { - AP_HAL::panic("must be singleton"); + AP_HAL::panic("AP_VisualOdom must be singleton"); } #endif _singleton = this; diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp index c5318ef3ce5d91..d306a718b67db1 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp @@ -62,13 +62,16 @@ void AP_Volz_Protocol::init(void) return; } - const AP_SerialManager &serial_manager = AP::serialmanager(); + AP_SerialManager &serial_manager = AP::serialmanager(); port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Volz,0); if (port == nullptr) { // No port configured return; } + // update baud param in case user looks at it + serial_manager.set_and_default_baud(AP_SerialManager::SerialProtocol_Volz, 0, 115200); + // Create thread to handle output if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Volz_Protocol::loop, void), "Volz", diff --git a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h index d17fa76570dd8e..c6c9bc338f264b 100644 --- a/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h +++ b/libraries/AP_Volz_Protocol/AP_Volz_Protocol.h @@ -88,7 +88,6 @@ class AP_Volz_Protocol { // Loop in thread to output to uart void loop(); - uint8_t sent_count; void init(void); @@ -120,6 +119,8 @@ class AP_Volz_Protocol { void read_telem(); void process_response(const CMD &cmd); + uint8_t sent_count; + struct { CMD_ID types[3] { CMD_ID::READ_CURRENT, diff --git a/libraries/Filter/AP_Filter_config.h b/libraries/Filter/AP_Filter_config.h index d94dd9df1b720a..d89e7a0a620e50 100644 --- a/libraries/Filter/AP_Filter_config.h +++ b/libraries/Filter/AP_Filter_config.h @@ -2,14 +2,16 @@ #include -#ifndef AP_FILTER_NUM_FILTERS -#if BOARD_FLASH_SIZE > 1024 -#define AP_FILTER_NUM_FILTERS 8 -#else -#define AP_FILTER_NUM_FILTERS 0 -#endif -#endif - #ifndef AP_FILTER_ENABLED -#define AP_FILTER_ENABLED AP_FILTER_NUM_FILTERS > 0 +#define AP_FILTER_ENABLED BOARD_FLASH_SIZE > 1024 #endif + +#if AP_FILTER_ENABLED + #ifndef AP_FILTER_NUM_FILTERS + #if BOARD_FLASH_SIZE > 1024 + #define AP_FILTER_NUM_FILTERS 8 + #else + #define AP_FILTER_NUM_FILTERS 4 + #endif // BOARD_FLASH_SIZE + #endif // AP_FILTER_NUM_FILTERS +#endif // AP_FILTER_ENABLED diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 649a12a6ad6337..aa67f61e8ceddc 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -403,7 +403,6 @@ class GCS_MAVLINK #if AP_WINCH_ENABLED virtual void send_winch_status() const {}; #endif - void send_water_depth() const; int8_t battery_remaining_pct(const uint8_t instance) const; #if HAL_HIGH_LATENCY2_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e670339e7b756b..91dfd299c3142d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -5966,57 +5966,6 @@ void GCS_MAVLINK::send_generator_status() const } #endif -#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) -void GCS_MAVLINK::send_water_depth() const -{ - if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) { - return; - } - - RangeFinder *rangefinder = RangeFinder::get_singleton(); - - if (rangefinder == nullptr || !rangefinder->has_orientation(ROTATION_PITCH_270)){ - return; - } - - // get position - const AP_AHRS &ahrs = AP::ahrs(); - Location loc; - IGNORE_RETURN(ahrs.get_location(loc)); - - for (uint8_t i=0; inum_sensors(); i++) { - const AP_RangeFinder_Backend *s = rangefinder->get_backend(i); - - if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) { - continue; - } - - // get temperature - float temp_C; - if (!s->get_temp(temp_C)) { - temp_C = 0.0f; - } - - const bool sensor_healthy = (s->status() == RangeFinder::Status::Good); - - mavlink_msg_water_depth_send( - chan, - AP_HAL::millis(), // time since system boot TODO: take time of measurement - i, // rangefinder instance - sensor_healthy, // sensor healthy - loc.lat, // latitude of vehicle - loc.lng, // longitude of vehicle - loc.alt * 0.01f, // altitude of vehicle (MSL) - ahrs.get_roll(), // roll in radians - ahrs.get_pitch(), // pitch in radians - ahrs.get_yaw(), // yaw in radians - s->distance(), // distance in meters - temp_C); // temperature in degC - } - -} -#endif // AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) - #if HAL_ADSB_ENABLED void GCS_MAVLINK::send_uavionix_adsb_out_status() const { @@ -6547,13 +6496,6 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) break; #endif -#if AP_RANGEFINDER_ENABLED && APM_BUILD_TYPE(APM_BUILD_Rover) - case MSG_WATER_DEPTH: - CHECK_PAYLOAD_SIZE(WATER_DEPTH); - send_water_depth(); - break; -#endif - #if HAL_HIGH_LATENCY2_ENABLED case MSG_HIGH_LATENCY2: CHECK_PAYLOAD_SIZE(HIGH_LATENCY2); diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index b59ba9d93c3119..7a7db6a68366a1 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -55,10 +55,7 @@ extern mavlink_system_t mavlink_system; /// @param chan Channel to send to static inline bool valid_channel(mavlink_channel_t chan) { -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wtautological-constant-out-of-range-compare" - return chan < MAVLINK_COMM_NUM_BUFFERS; -#pragma clang diagnostic pop + return static_cast(chan) < MAVLINK_COMM_NUM_BUFFERS; } mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 1d4c7cc77fd0ca..bae3499173beca 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -242,6 +242,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Copter}: 178:FlightMode Pause/Resume // @Values{Plane}: 179:ICEngine start / stop // @Values{Copter, Plane}: 180:Test autotuned gains after tune is complete + // @Values{Plane}: 181: QuickTune // @Values{Rover}: 201:Roll // @Values{Rover}: 202:Pitch // @Values{Rover}: 207:MainSail @@ -251,7 +252,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Rover}: 211:Walking Height // @Values{Copter, Rover, Plane}: 212:Mount1 Roll, 213:Mount1 Pitch, 214:Mount1 Yaw, 215:Mount2 Roll, 216:Mount2 Pitch, 217:Mount2 Yaw // @Values{Copter}: 219:Transmitter Tuning - // @Values{Copter, Rover, Plane}: 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8 + // @Values{Copter, Rover, Plane}: 300:Scripting1, 301:Scripting2, 302:Scripting3, 303:Scripting4, 304:Scripting5, 305:Scripting6, 306:Scripting7, 307:Scripting8, 308:Scripting9, 309:Scripting10, 310:Scripting11, 311:Scripting12, 312:Scripting13, 313:Scripting14, 314:Scripting15, 315:Scripting16 // @User: Standard AP_GROUPINFO_FRAME("OPTION", 6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER|AP_PARAM_FRAME_PLANE|AP_PARAM_FRAME_BLIMP), @@ -684,6 +685,14 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos case AUX_FUNC::SCRIPTING_6: case AUX_FUNC::SCRIPTING_7: case AUX_FUNC::SCRIPTING_8: + case AUX_FUNC::SCRIPTING_9: + case AUX_FUNC::SCRIPTING_10: + case AUX_FUNC::SCRIPTING_11: + case AUX_FUNC::SCRIPTING_12: + case AUX_FUNC::SCRIPTING_13: + case AUX_FUNC::SCRIPTING_14: + case AUX_FUNC::SCRIPTING_15: + case AUX_FUNC::SCRIPTING_16: #endif #if AP_VIDEOTX_ENABLED case AUX_FUNC::VTX_POWER: @@ -741,7 +750,7 @@ void RC_Channel::init_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos #endif case AUX_FUNC::MOTOR_ESTOP: case AUX_FUNC::RC_OVERRIDE_ENABLE: -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED case AUX_FUNC::RUNCAM_CONTROL: case AUX_FUNC::RUNCAM_OSD_CONTROL: #endif @@ -848,7 +857,7 @@ const RC_Channel::LookupTable RC_Channel::lookuptable[] = { #endif { AUX_FUNC::SAILBOAT_MOTOR_3POS,"SailboatMotor"}, { AUX_FUNC::SURFACE_TRACKING,"SurfaceTracking"}, -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED { AUX_FUNC::RUNCAM_CONTROL,"RunCamControl"}, { AUX_FUNC::RUNCAM_OSD_CONTROL,"RunCamOSDControl"}, #endif @@ -1168,7 +1177,7 @@ bool RC_Channel::do_aux_function_camera_lens(const AuxSwitchPos ch_flag) } #endif // AP_CAMERA_ENABLED -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED void RC_Channel::do_aux_function_runcam_control(const AuxSwitchPos ch_flag) { AP_RunCam *runcam = AP::runcam(); @@ -1473,7 +1482,7 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch break; #endif // AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED -#if HAL_RUNCAM_ENABLED +#if AP_CAMERA_RUNCAM_ENABLED case AUX_FUNC::RUNCAM_CONTROL: do_aux_function_runcam_control(ch_flag); break; @@ -1863,6 +1872,14 @@ bool RC_Channel::do_aux_function(const AUX_FUNC ch_option, const AuxSwitchPos ch case AUX_FUNC::SCRIPTING_6: case AUX_FUNC::SCRIPTING_7: case AUX_FUNC::SCRIPTING_8: + case AUX_FUNC::SCRIPTING_9: + case AUX_FUNC::SCRIPTING_10: + case AUX_FUNC::SCRIPTING_11: + case AUX_FUNC::SCRIPTING_12: + case AUX_FUNC::SCRIPTING_13: + case AUX_FUNC::SCRIPTING_14: + case AUX_FUNC::SCRIPTING_15: + case AUX_FUNC::SCRIPTING_16: #endif break; diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 7194fa092b80e3..378ca95e13f9f2 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -294,9 +294,17 @@ class RC_Channel { SCRIPTING_6 = 305, SCRIPTING_7 = 306, SCRIPTING_8 = 307, + SCRIPTING_9 = 308, + SCRIPTING_10 = 309, + SCRIPTING_11 = 310, + SCRIPTING_12 = 311, + SCRIPTING_13 = 312, + SCRIPTING_14 = 313, + SCRIPTING_15 = 314, + SCRIPTING_16 = 315, // this must be higher than any aux function above - AUX_FUNCTION_MAX = 308, + AUX_FUNCTION_MAX = 316, }; // auxiliary switch handling (n.b.: we store this as 2-bits!): diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index c0469abc7cf20a..138e5ae8755343 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -839,6 +839,11 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel) sitl->models.slung_payload_sim.update(get_position_relhome(), velocity_ef, accel_earth, wind_ef); #endif + // update tether +#if AP_SIM_TETHER_ENABLED + sitl->models.tether_sim.update(location); +#endif + // allow for changes in physics step adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1)); } @@ -1273,18 +1278,26 @@ void Aircraft::add_twist_forces(Vector3f &rot_accel) } } -#if AP_SIM_SLUNGPAYLOAD_ENABLED -// add body-frame force due to slung payload -void Aircraft::add_slungpayload_forces(Vector3f &body_accel) +// add body-frame force due to slung payload and tether +void Aircraft::add_external_forces(Vector3f &body_accel) { - Vector3f forces_ef; - sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef); + Vector3f total_force; +#if AP_SIM_SLUNGPAYLOAD_ENABLED + Vector3f forces_ef_slung; + sitl->models.slung_payload_sim.get_forces_on_vehicle(forces_ef_slung); + total_force += forces_ef_slung; +#endif + +#if AP_SIM_TETHER_ENABLED + Vector3f forces_ef_tether; + sitl->models.tether_sim.get_forces_on_vehicle(forces_ef_tether); + total_force += forces_ef_tether; +#endif // convert ef forces to body-frame accelerations (acceleration = force / mass) - const Vector3f accel_bf = dcm.transposed() * forces_ef / mass; - body_accel += accel_bf; + const Vector3f accel_bf_tether = dcm.transposed() * total_force / mass; + body_accel += accel_bf_tether; } -#endif /* get position relative to home diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index e84a078ac8fafc..a6d7b273c09b6b 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -322,10 +322,8 @@ class Aircraft { void add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel); void add_twist_forces(Vector3f &rot_accel); -#if AP_SIM_SLUNGPAYLOAD_ENABLED - // add body-frame force due to slung payload - void add_slungpayload_forces(Vector3f &body_accel); -#endif + // add body-frame force due to payload + void add_external_forces(Vector3f &body_accel); // get local thermal updraft float get_local_updraft(const Vector3d ¤tPos); diff --git a/libraries/SITL/SIM_Airspeed_DLVR.cpp b/libraries/SITL/SIM_Airspeed_DLVR.cpp index 265185fae44562..46fc1c531800ef 100644 --- a/libraries/SITL/SIM_Airspeed_DLVR.cpp +++ b/libraries/SITL/SIM_Airspeed_DLVR.cpp @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_AIRSPEED_DLVR_ENABLED + #include "SIM_Airspeed_DLVR.h" #include "SITL.h" @@ -68,3 +72,5 @@ void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft) // To Do: Add a sensor board temperature offset parameter temperature = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt); } + +#endif // AP_SIM_AIRSPEED_DLVR_ENABLED diff --git a/libraries/SITL/SIM_Airspeed_DLVR.h b/libraries/SITL/SIM_Airspeed_DLVR.h index cb21efdb514e4f..6bb95bc2eba71a 100644 --- a/libraries/SITL/SIM_Airspeed_DLVR.h +++ b/libraries/SITL/SIM_Airspeed_DLVR.h @@ -1,5 +1,9 @@ #pragma once +#include "SIM_config.h" + +#if AP_SIM_AIRSPEED_DLVR_ENABLED + #include "SIM_I2CDevice.h" namespace SITL { @@ -26,3 +30,5 @@ class Airspeed_DLVR : public I2CDevice }; } // namespace SITL + +#endif // AP_SIM_AIRSPEED_DLVR_ENABLED diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp index eaf1c1b19fb63b..f2982335c5b429 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.cpp @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED + #include "SIM_BattMonitor_SMBus_Maxell.h" SITL::Maxell::Maxell() : @@ -15,3 +19,5 @@ SITL::Maxell::Maxell() : set_register(SMBusBattGenericDevReg::SERIAL, (uint16_t)37); } + +#endif // AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h index 5652b48cbdd872..e620f0dd2e7dd3 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Maxell.h @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED + #include "SIM_BattMonitor_SMBus_Generic.h" #include @@ -25,3 +29,5 @@ class Maxell : public SIM_BattMonitor_SMBus_Generic }; } // namespace SITL + +#endif // AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp b/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp index adadfda70ea245..ea1d1dddbfbd4b 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.cpp @@ -1,4 +1,9 @@ +#include "SIM_config.h" + +#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED + #include "SIM_BattMonitor_SMBus_Rotoye.h" + #include SITL::Rotoye::Rotoye() : @@ -28,3 +33,5 @@ void SITL::Rotoye::update(const class Aircraft &aircraft) set_register(SMBusBattRotoyeDevReg::TEMP_EXT, int16_t(outside_temp + 100)); // it's a little warmer inside.... (10 degrees here) } } + +#endif // AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED diff --git a/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.h b/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.h index a7617249b9db24..9c33e9f6ee5d96 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.h +++ b/libraries/SITL/SIM_BattMonitor_SMBus_Rotoye.h @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED + #include "SIM_BattMonitor_SMBus_Generic.h" #include @@ -33,3 +37,5 @@ class Rotoye : public SIM_BattMonitor_SMBus_Generic }; } // namespace SITL + +#endif // AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index d5435047f95fb9..c3f2fba1012955 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -338,15 +338,15 @@ void Frame::load_frame_params(const char *model_json) } else { IGNORE_RETURN(asprintf(&fname, "@ROMFS/models/%s", model_json)); if (AP::FS().stat(model_json, &st) != 0) { - AP_HAL::panic("%s failed to load\n", model_json); + AP_HAL::panic("%s failed to load", model_json); } } if (fname == nullptr) { - AP_HAL::panic("%s failed to load\n", model_json); + AP_HAL::panic("%s failed to load", model_json); } AP_JSON::value *obj = AP_JSON::load_json(model_json); if (obj == nullptr) { - AP_HAL::panic("%s failed to load\n", model_json); + AP_HAL::panic("%s failed to load", model_json); } enum class VarType { diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 8d1b1daf0aaa91..2b5c6af0570a4d 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -24,6 +24,7 @@ #include "SIM_I2C.h" #include "SIM_ToshibaLED.h" #include "SIM_MaxSonarI2CXL.h" +#include "SIM_BattMonitor_SMBus_Generic.h" #include "SIM_BattMonitor_SMBus_Maxell.h" #include "SIM_BattMonitor_SMBus_Rotoye.h" #include "SIM_Airspeed_DLVR.h" @@ -59,20 +60,38 @@ static IgnoredI2CDevice ignored; #if AP_SIM_TOSHIBALED_ENABLED static ToshibaLED toshibaled; #endif +#if AP_SIM_MAXSONAR_I2C_XL_ENABLED static MaxSonarI2CXL maxsonari2cxl; static MaxSonarI2CXL maxsonari2cxl_2; +#endif +#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED static Maxell maxell; +#endif +#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED static Rotoye rotoye; +#endif static SIM_BattMonitor_SMBus_Generic smbus_generic; +#if AP_SIM_AIRSPEED_DLVR_ENABLED static Airspeed_DLVR airspeed_dlvr; +#endif +#if AP_SIM_TEMPERATURE_TSYS01_ENABLED static TSYS01 tsys01; +#endif #if AP_SIM_TSYS03_ENABLED static TSYS03 tsys03; #endif +#if AP_SIM_TEMPERATURE_MCP9600_ENABLED static MCP9600 mcp9600; +#endif +#if AP_SIM_ICM40609_ENABLED static ICM40609 icm40609; +#endif +#if AP_SIM_MS5525_ENABLED static MS5525 ms5525; +#endif +#if AP_SIM_MS5611_ENABLED static MS5611 ms5611; +#endif #if AP_SIM_LP5562_ENABLED static LP5562 lp5562; #endif @@ -95,25 +114,43 @@ struct i2c_device_at_address { uint8_t addr; I2CDevice &device; } i2c_devices[] { +#if AP_SIM_MAXSONAR_I2C_XL_ENABLED { 0, 0x70, maxsonari2cxl }, // RNGFNDx_TYPE = 2, RNGFNDx_ADDR = 112 +#endif +#if AP_SIM_TEMPERATURE_MCP9600_ENABLED { 0, 0x60, mcp9600 }, // 0x60 is low address +#endif +#if AP_SIM_MAXSONAR_I2C_XL_ENABLED { 0, 0x71, maxsonari2cxl_2 }, // RNGFNDx_TYPE = 2, RNGFNDx_ADDR = 113 +#endif +#if AP_SIM_ICM40609_ENABLED { 1, 0x01, icm40609 }, +#endif #if AP_SIM_TOSHIBALED_ENABLED { 1, 0x55, toshibaled }, #endif { 1, 0x38, ignored }, // NCP5623 { 1, 0x39, ignored }, // NCP5623C { 1, 0x40, ignored }, // KellerLD +#if AP_SIM_MS5525_ENABLED { 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4 +#endif #if AP_SIM_INA3221_ENABLED { 1, 0x42, ina3221 }, #endif +#if AP_SIM_TEMPERATURE_TSYS01_ENABLED { 1, 0x77, tsys01 }, +#endif +#if AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED { 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13 +#endif +#if AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED { 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13 +#endif { 3, 0x0B, smbus_generic}, // BATTx_MONITOR 7, BATTx_I2C_ADDR 13 +#if AP_SIM_AIRSPEED_DLVR_ENABLED { 2, 0x28, airspeed_dlvr }, // ARSPD_TYPE = 7 5inch H2O sensor +#endif #if AP_SIM_LP5562_ENABLED { 2, 0x30, lp5562 }, // LP5562 RGB LED driver #endif @@ -126,7 +163,9 @@ struct i2c_device_at_address { #if AP_SIM_TSYS03_ENABLED { 2, 0x40, tsys03 }, #endif +#if AP_SIM_MS5611_ENABLED { 2, 0x77, ms5611 }, // MS5611: BARO_PROBE_EXT = 2 +#endif #if AP_SIM_COMPASS_QMC5883L_ENABLED { 2, 0x0D, qmc5883l }, #endif diff --git a/libraries/SITL/SIM_ICM40609.h b/libraries/SITL/SIM_ICM40609.h index 121b82b957f6f7..300634edddf568 100644 --- a/libraries/SITL/SIM_ICM40609.h +++ b/libraries/SITL/SIM_ICM40609.h @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_ICM40609_ENABLED + #include "SIM_Invensense_v3.h" namespace SITL { @@ -21,3 +25,5 @@ class ICM40609 : public InvensenseV3 }; } // namespace SITL + +#endif // AP_SIM_ICM40609_ENABLED diff --git a/libraries/SITL/SIM_JSON_Master.cpp b/libraries/SITL/SIM_JSON_Master.cpp index 9b8c3b59060364..a6b92591d404fb 100644 --- a/libraries/SITL/SIM_JSON_Master.cpp +++ b/libraries/SITL/SIM_JSON_Master.cpp @@ -37,15 +37,15 @@ void JSON_Master::init(const int32_t num_slaves) uint16_t port = 9002 + 10 * i; if (!list->sock_in.reuseaddress()) { - AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s\n", port, strerror(errno)); + AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s", port, strerror(errno)); } if (!list->sock_in.bind("127.0.0.1", port)) { - AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s\n", port, strerror(errno)); + AP_HAL::panic("JSON master: socket reuseaddress failed on port: %d - %s", port, strerror(errno)); } if (!list->sock_in.set_blocking(false)) { - AP_HAL::panic( "JSON master: socket set_blocking(false) failed on port: %d - %s\n", port, strerror(errno)); + AP_HAL::panic( "JSON master: socket set_blocking(false) failed on port: %d - %s", port, strerror(errno)); } printf("Slave %u: listening on %u\n", list->instance, port); diff --git a/libraries/SITL/SIM_MS5525.cpp b/libraries/SITL/SIM_MS5525.cpp index 760c4202f92a90..53bd9f887f58c9 100644 --- a/libraries/SITL/SIM_MS5525.cpp +++ b/libraries/SITL/SIM_MS5525.cpp @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_MS5525_ENABLED + #include "SIM_MS5525.h" #include @@ -72,3 +76,5 @@ void MS5525::get_pressure_temperature_readings(float &P_Pa, float &Temp_C) const uint8_t instance = 0; // TODO: work out which sensor this is P_Pa = AP::sitl()->state.airspeed_raw_pressure[instance] + AP::sitl()->airspeed[instance].offset; } + +#endif // AP_SIM_MS5525_ENABLED diff --git a/libraries/SITL/SIM_MS5525.h b/libraries/SITL/SIM_MS5525.h index f64b82ff0b383a..bbc11f49678387 100644 --- a/libraries/SITL/SIM_MS5525.h +++ b/libraries/SITL/SIM_MS5525.h @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_MS5525_ENABLED + #include "SIM_MS5XXX.h" #include @@ -43,3 +47,5 @@ class MS5525 : public MS5XXX }; } // namespace SITL + +#endif // AP_SIM_MS5525_ENABLED diff --git a/libraries/SITL/SIM_MS5611.cpp b/libraries/SITL/SIM_MS5611.cpp index d205665d5ef888..7d20614b8158b5 100644 --- a/libraries/SITL/SIM_MS5611.cpp +++ b/libraries/SITL/SIM_MS5611.cpp @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_MS5611_ENABLED + #include "SIM_MS5611.h" #include @@ -121,3 +125,5 @@ void MS5611::get_pressure_temperature_readings(float &P_Pa, float &Temp_C) // TO DO add in wind correction by inheritting from AP_Baro_SITL_Generic? // P_Pa += AP_Baro_SITL::wind_pressure_correction(instance); } + +#endif // AP_SIM_MS5611_ENABLED diff --git a/libraries/SITL/SIM_MS5611.h b/libraries/SITL/SIM_MS5611.h index 7822a470d460a4..34e8c30f36f8f8 100644 --- a/libraries/SITL/SIM_MS5611.h +++ b/libraries/SITL/SIM_MS5611.h @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_MS5611_ENABLED + #include "SIM_MS5XXX.h" #include @@ -43,3 +47,5 @@ class MS5611 : public MS5XXX }; } // namespace SITL + +#endif // AP_SIM_MS5611_ENABLED diff --git a/libraries/SITL/SIM_MaxSonarI2CXL.h b/libraries/SITL/SIM_MaxSonarI2CXL.h index a18d981745fd9d..df30aa883e24af 100644 --- a/libraries/SITL/SIM_MaxSonarI2CXL.h +++ b/libraries/SITL/SIM_MaxSonarI2CXL.h @@ -1,5 +1,9 @@ #pragma once +#include "SIM_config.h" + +#if AP_SIM_MAXSONAR_I2C_XL_ENABLED + #include "SIM_I2CDevice.h" namespace SITL { @@ -29,3 +33,5 @@ class MaxSonarI2CXL : public I2CDevice, public I2CCommandResponseDevice }; } // namespace SITL + +#endif // AP_SIM_MAXSONAR_I2C_XL_ENABLED diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 4e019dde9b4de4..273c2fc06dad7d 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -49,12 +49,10 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot add_shove_forces(rot_accel, body_accel); add_twist_forces(rot_accel); -#if AP_SIM_SLUNGPAYLOAD_ENABLED - // add forces from slung payload - add_slungpayload_forces(body_accel); -#endif + // add forces from slung payload or tether payload + add_external_forces(body_accel); } - + /* update the multicopter simulation by one time step */ diff --git a/libraries/SITL/SIM_Temperature_MCP9600.cpp b/libraries/SITL/SIM_Temperature_MCP9600.cpp index c05e7221504fa8..7718fc181aa4a7 100644 --- a/libraries/SITL/SIM_Temperature_MCP9600.cpp +++ b/libraries/SITL/SIM_Temperature_MCP9600.cpp @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_TEMPERATURE_MCP9600_ENABLED + #include "SIM_Temperature_MCP9600.h" using namespace SITL; @@ -65,3 +69,5 @@ int MCP9600::rdwr(I2C::i2c_rdwr_ioctl_data *&data) { return I2CRegisters_ConfigurableLength::rdwr(data); } + +#endif // AP_SIM_TEMPERATURE_MCP9600_ENABLED diff --git a/libraries/SITL/SIM_Temperature_MCP9600.h b/libraries/SITL/SIM_Temperature_MCP9600.h index c0d2f3ae1ae21b..79d5b40e5e3de3 100644 --- a/libraries/SITL/SIM_Temperature_MCP9600.h +++ b/libraries/SITL/SIM_Temperature_MCP9600.h @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_TEMPERATURE_MCP9600_ENABLED + #include "SIM_I2CDevice.h" /* @@ -36,3 +40,5 @@ class MCP9600 : public I2CDevice, private I2CRegisters_ConfigurableLength }; } // namespace SITL + +#endif // AP_SIM_TEMPERATURE_MCP9600_ENABLED diff --git a/libraries/SITL/SIM_Temperature_TSYS01.cpp b/libraries/SITL/SIM_Temperature_TSYS01.cpp index 5785a731527aeb..68f5f55e332605 100644 --- a/libraries/SITL/SIM_Temperature_TSYS01.cpp +++ b/libraries/SITL/SIM_Temperature_TSYS01.cpp @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_TEMPERATURE_TSYS01_ENABLED + #include "SIM_Temperature_TSYS01.h" #include @@ -196,3 +200,5 @@ float SITL::TSYS01::get_sim_temperature() const // To Do: Add a sensor board temperature offset parameter return AP_Baro::get_temperatureC_for_alt_amsl(sim_alt) + 25; } + +#endif // AP_SIM_TEMPERATURE_TSYS01_ENABLED diff --git a/libraries/SITL/SIM_Temperature_TSYS01.h b/libraries/SITL/SIM_Temperature_TSYS01.h index 5ad9bff8321194..8b7127d0a17db2 100644 --- a/libraries/SITL/SIM_Temperature_TSYS01.h +++ b/libraries/SITL/SIM_Temperature_TSYS01.h @@ -1,3 +1,7 @@ +#include "SIM_config.h" + +#if AP_SIM_TEMPERATURE_TSYS01_ENABLED + #include "SIM_I2CDevice.h" /* @@ -63,3 +67,5 @@ class TSYS01 : public I2CDevice }; } // namespace SITL + +#endif // AP_SIM_TEMPERATURE_TSYS01_ENABLED diff --git a/libraries/SITL/SIM_Tether.cpp b/libraries/SITL/SIM_Tether.cpp new file mode 100644 index 00000000000000..005347b4c25ce0 --- /dev/null +++ b/libraries/SITL/SIM_Tether.cpp @@ -0,0 +1,309 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simulate a static tether attached to the vehicle and ground +*/ + +#include "SIM_config.h" + +#if AP_SIM_TETHER_ENABLED + +#include "SIM_Tether.h" +#include "SITL.h" +#include +#include "SIM_Aircraft.h" +#include +#include +#include + +using namespace SITL; + +// TetherSim parameters +const AP_Param::GroupInfo TetherSim::var_info[] = { + // @Param: ENABLE + // @DisplayName: Tether Simulation Enable/Disable + // @Description: Enable or disable the tether simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, TetherSim, enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: DENSITY + // @DisplayName: Tether Wire Density + // @Description: Linear mass density of the tether wire + // @Range: 0 1 + // @User: Advanced + AP_GROUPINFO("DENSITY", 2, TetherSim, tether_linear_density, 0.0167), + + // @Param: LINELEN + // @DisplayName: Tether Maximum Line Length + // @Description: Maximum length of the tether line in meters + // @Units: m + // @Range: 0 100 + // @User: Advanced + AP_GROUPINFO("LINELEN", 3, TetherSim, max_line_length, 100.0), + + // @Param: SYSID + // @DisplayName: Tether Simulation MAVLink System ID + // @Description: MAVLink system ID for the tether simulation, used to distinguish it from other systems on the network + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("SYSID", 4, TetherSim, sys_id, 2), + + // @Param: STUCK + // @DisplayName: Tether Stuck Enable/Disable + // @Description: Enable or disable a stuck tether simulation + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("STUCK", 5, TetherSim, tether_stuck, 0), + + // @Param: SPGCNST + // @DisplayName: Tether Spring Constant + // @Description: Spring constant for the tether to simulate elastic forces when stretched beyond its maximum length + // @Range: 0 255 + // @User: Advanced + AP_GROUPINFO("SPGCNST", 6, TetherSim, tether_spring_constant, 100), + + AP_GROUPEND +}; + +// TetherSim handles interaction with main vehicle +TetherSim::TetherSim() +{ + AP_Param::setup_object_defaults(this, var_info); +} + + +void TetherSim::update(const Location& veh_pos) +{ + if (!enable) { + return; + } + + if (veh_pos.is_zero()) { + return; + } + + // initialise fixed tether ground location + const uint32_t now_us = AP_HAL::micros(); + if (!initialised) { + // capture EKF origin + auto *sitl = AP::sitl(); + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return; + } + + // more initialisation + last_update_us = now_us; + initialised = true; + } + + // calculate dt and update tether forces + const float dt = (now_us - last_update_us)*1.0e-6; + last_update_us = now_us; + + update_tether_force(veh_pos, dt); + + // send tether location to GCS at 5hz + // TO-Do: add provision to make the tether movable + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_report_ms >= reporting_period_ms) { + last_report_ms = now_ms; + send_report(); + write_log(); + } +} + +// get earth-frame forces on the vehicle from the tether +// returns true on success and fills in forces_ef argument, false on failure +bool TetherSim::get_forces_on_vehicle(Vector3f& forces_ef) const +{ + if (!enable) { + return false; + } + + forces_ef = veh_forces_ef; + return true; +} + +// send a report to the vehicle control code over MAVLink +void TetherSim::send_report(void) +{ + if (!mavlink_connected && mav_socket.connect(target_address, target_port)) { + ::printf("Tether System connected to %s:%u\n", target_address, (unsigned)target_port); + mavlink_connected = true; + } + if (!mavlink_connected) { + return; + } + + // get current time + uint32_t now_ms = AP_HAL::millis(); + + // send heartbeat at 1hz + const uint8_t component_id = MAV_COMP_ID_USER11; + if (now_ms - last_heartbeat_ms >= 1000) { + last_heartbeat_ms = now_ms; + + const mavlink_heartbeat_t heartbeat{ + custom_mode: 0, + type : MAV_TYPE_GROUND_ROVER, + autopilot : MAV_AUTOPILOT_INVALID, + base_mode: 0, + system_status: 0, + mavlink_version: 0, + }; + + mavlink_message_t msg; + mavlink_msg_heartbeat_encode_status( + sys_id.get(), + component_id, + &mav_status, + &msg, + &heartbeat); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + mav_socket.send(buf, len); + } + + // send a GLOBAL_POSITION_INT messages + { + Location tether_anchor_loc; + int32_t alt_amsl_cm, alt_rel_cm; + if (!get_tether_ground_location(tether_anchor_loc) || + !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_amsl_cm) || + !tether_anchor_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rel_cm)) { + return; + } + const mavlink_global_position_int_t global_position_int{ + time_boot_ms: now_ms, + lat: tether_anchor_loc.lat, + lon: tether_anchor_loc.lng, + alt: alt_amsl_cm * 10, // amsl alt in mm + relative_alt: alt_rel_cm * 10, // relative alt in mm + vx: 0, // velocity in cm/s + vy: 0, // velocity in cm/s + vz: 0, // velocity in cm/s + hdg: 0 // heading in centi-degrees + }; + mavlink_message_t msg; + mavlink_msg_global_position_int_encode_status(sys_id, component_id, &mav_status, &msg, &global_position_int); + uint8_t buf[300]; + const uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); + if (len > 0) { + mav_socket.send(buf, len); + } + } +} + +void TetherSim::write_log() +{ +#if HAL_LOGGING_ENABLED + // write log of tether state + // @LoggerMessage: TETH + // @Description: Tether state + // @Field: TimeUS: Time since system startup + // @Field: Len: Tether length + // @Field: VFN: Force on vehicle in North direction + // @Field: VFE: Force on vehicle in East direction + // @Field: VFD: Force on vehicle in Down direction + AP::logger().WriteStreaming("TETH", + "TimeUS,Len,VFN,VFE,VFD", // labels + "s----", // units + "F----", // multipliers + "Qffff", // format + AP_HAL::micros64(), + (float)tether_length, + (double)veh_forces_ef.x, + (double)veh_forces_ef.y, + (double)veh_forces_ef.z); +#endif +} +// returns true on success and fills in tether_ground_loc argument, false on failure +bool TetherSim::get_tether_ground_location(Location& tether_ground_loc) const +{ + // get EKF origin + auto *sitl = AP::sitl(); + if (sitl == nullptr) { + return false; + } + const Location ekf_origin = sitl->state.home; + if (ekf_origin.lat == 0 && ekf_origin.lng == 0) { + return false; + } + + tether_ground_loc = ekf_origin; + return true; +} + +void TetherSim::update_tether_force(const Location& veh_pos, float dt) +{ + + Location tether_anchor_loc; + if (!get_tether_ground_location(tether_anchor_loc)) { + return; + } + + // Step 1: Calculate the vector from the tether anchor to the vehicle + Vector3f tether_vector = veh_pos.get_distance_NED(tether_anchor_loc); + tether_length = tether_vector.length(); + + // Step 2: Check if tether is taut (length exceeds maximum allowed length) or stuck + if (tether_length > max_line_length) { + + // Calculate the stretch beyond the maximum length + float stretch = MAX(tether_length - max_line_length, 0.0f); + + // Apply a spring-like penalty force proportional to the stretch + float penalty_force_magnitude = tether_spring_constant * stretch; + + // Direction of force is along the tether, pulling toward the anchor + veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Exceeded maximum length."); + + return; + } + + if (tether_stuck) { + + // Calculate the stretch beyond the maximum length + float stretch = MAX(tether_length - tether_not_stuck_length, 0.0f); + + // Apply a spring-like penalty force proportional to the stretch + float penalty_force_magnitude = tether_spring_constant * stretch; + + // Direction of force is directly along the tether, towards the tether anchor point + veh_forces_ef = tether_vector.normalized() * penalty_force_magnitude; + + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Tether: Stuck."); + + return; + } else { + tether_not_stuck_length = tether_length; + } + + // Step 3: Calculate the weight of the tether being lifted + // The weight is proportional to the current tether length + const float tether_weight_force = tether_linear_density * tether_length * GRAVITY_MSS; + + // Step 4: Calculate the tension force + Vector3f tension_force_NED = tether_vector.normalized() * tether_weight_force; + + // Step 5: Apply the force to the vehicle + veh_forces_ef = tension_force_NED; +} + +#endif diff --git a/libraries/SITL/SIM_Tether.h b/libraries/SITL/SIM_Tether.h new file mode 100644 index 00000000000000..4a7366e1089cd5 --- /dev/null +++ b/libraries/SITL/SIM_Tether.h @@ -0,0 +1,95 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + Simulate a tethered vehicle. + Models the forces exerted by the tether and reports them to the vehicle simulation. The dynamics are not accurate but provide a very rough approximation intended to test a "stuck tether". + */ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_TETHER_ENABLED + +#include +#include +#include +#include + +namespace SITL { + +// TetherSim simulates a tethered to a vehicle +class TetherSim { +public: + // constructor + TetherSim(); + + // Update the tether simulation state using the vehicle's earth-frame position + void update(const Location& veh_pos); + + // Retrieve earth-frame forces on the vehicle due to the tether + // Returns true on success and fills in the forces_ef argument; false on failure + bool get_forces_on_vehicle(Vector3f& forces_ef) const; + + // Parameter table for configuration + static const struct AP_Param::GroupInfo var_info[]; + +private: + // Parameters + AP_Int8 enable; // Enable or disable the tether simulation + AP_Float tether_linear_density; // Linear mass density of the tether in kg/m + AP_Float max_line_length; // Maximum allowed tether length in meters + AP_Int8 sys_id; // MAVLink system ID for GCS reporting + AP_Int8 tether_stuck; // Set to 1 to simulate a stuck tether + AP_Float tether_spring_constant; // Spring constant for modeling tether stretch + + // Send MAVLink messages to the GCS + void send_report(); + + // Write tether simulation state to onboard log + void write_log(); + + // Retrieve the location of the tether anchor point on the ground + // Returns true on success and fills in the tether_anchor_loc argument; false on failure + bool get_tether_ground_location(Location& tether_anchor_loc) const; + + // Update forces exerted by the tether based on the vehicle's position + // Takes the vehicle's position and the simulation time step (dt) as inputs + void update_tether_force(const Location& veh_pos, float dt); + + // Socket connection variables for MAVLink communication + const char *target_address = "127.0.0.1"; // Address for MAVLink socket communication + const uint16_t target_port = 5763; // Port for MAVLink socket communication + SocketAPM_native mav_socket { false }; // Socket for MAVLink communication + bool initialised; // True if the simulation class is initialized + uint32_t last_update_us; // Timestamp of the last update in microseconds + + // MAVLink reporting variables + const float reporting_period_ms = 100; // Reporting period in milliseconds + uint32_t last_report_ms; // Timestamp of the last MAVLink report sent to GCS + uint32_t last_heartbeat_ms; // Timestamp of the last MAVLink heartbeat sent to GCS + bool mavlink_connected; // True if MAVLink connection is established + mavlink_status_t mav_status; // Status of MAVLink communication + + // Tether simulation variables + Vector3f veh_forces_ef; // Earth-frame forces on the vehicle due to the tether + float tether_length = 0.0f; // Current tether length in meters + float tether_not_stuck_length = 0.0f; // Tether length when the tether is not stuck +}; + +} // namespace SITL + +#endif // AP_SIM_TETHER_ENABLED diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index 3633bb4b1a7394..e9c147c6330438 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -116,7 +116,7 @@ XPlane::XPlane(const char *frame_str) : #endif if (!load_dref_map(XPLANE_JSON)) { - AP_HAL::panic("%s failed to load\n", XPLANE_JSON); + AP_HAL::panic("%s failed to load", XPLANE_JSON); } } diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index d4175772492781..5d6850bd39c01e 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -57,6 +57,10 @@ #define AP_SIM_SLUNGPAYLOAD_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_TETHER_ENABLED +#define AP_SIM_TETHER_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_FLIGHTAXIS_ENABLED #define AP_SIM_FLIGHTAXIS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif @@ -144,6 +148,42 @@ #define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED) #endif +#ifndef AP_SIM_AIRSPEED_DLVR_ENABLED +#define AP_SIM_AIRSPEED_DLVR_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED +#define AP_SIM_BATT_MONITOR_SMBUS_MAXELL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED +#define AP_SIM_BATT_MONITOR_SMBUS_ROTOYE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_SIM_INA3221_ENABLED #define AP_SIM_INA3221_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif + +#ifndef AP_SIM_ICM40609_ENABLED +#define AP_SIM_ICM40609_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_MS5525_ENABLED +#define AP_SIM_MS5525_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_MS5611_ENABLED +#define AP_SIM_MS5611_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_MAXSONAR_I2C_XL_ENABLED +#define AP_SIM_MAXSONAR_I2C_XL_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_TEMPERATURE_MCP9600_ENABLED +#define AP_SIM_TEMPERATURE_MCP9600_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_TEMPERATURE_TSYS01_ENABLED +#define AP_SIM_TEMPERATURE_TSYS01_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index e329ed232a9609..3adb9cc673ffd6 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -78,8 +78,8 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Param: ENGINE_MUL // @DisplayName: Engine failure thrust scaler // @Description: Thrust from Motors in SIM_ENGINE_FAIL will be multiplied by this factor - // @Units: ms - AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 1), + // @Range: 0 1 + AP_GROUPINFO("ENGINE_MUL", 8, SIM, engine_mul, 0), // @Param: WIND_SPD // @DisplayName: Simulated Wind speed // @Description: Allows you to emulate wind in sim @@ -246,7 +246,7 @@ const AP_Param::GroupInfo SIM::var_info[] = { // @Param: ENGINE_FAIL // @DisplayName: Engine Fail Mask // @Description: mask of motors which SIM_ENGINE_MUL will be applied to - // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8 + // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32 AP_GROUPINFO("ENGINE_FAIL", 58, SIM, engine_fail, 0), AP_SUBGROUPINFO(models, "", 59, SIM, SIM::ModelParm), AP_SUBGROUPEXTENSION("", 60, SIM, var_mag), @@ -1367,6 +1367,12 @@ const AP_Param::GroupInfo SIM::ModelParm::var_info[] = { AP_SUBGROUPPTR(flightaxis_ptr, "RFL_", 5, SIM::ModelParm, FlightAxis), #endif +#if AP_SIM_TETHER_ENABLED + // @Group: TETH_ + // @Path: ./SIM_Tether.cpp + AP_SUBGROUPINFO(tether_sim, "TETH_", 6, SIM::ModelParm, TetherSim), +#endif + AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 4edda5efd21e48..d80f641c5f0e7d 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -28,6 +28,7 @@ #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" #include "SIM_SlungPayload.h" +#include "SIM_Tether.h" #include "SIM_GPS.h" #include "SIM_DroneCANDevice.h" #include "SIM_ADSB_Sagetech_MXS.h" @@ -196,7 +197,7 @@ class SIM { AP_Float drift_speed; // degrees/second/minute AP_Float drift_time; // period in minutes AP_Float engine_mul; // engine multiplier - AP_Int8 engine_fail; // engine servo to fail (0-7) + AP_Int32 engine_fail; // mask of engine/motor servo outputs to fail // initial offset on GPS lat/lon, used to shift origin AP_Float gps_init_lat_ofs; @@ -338,6 +339,9 @@ class SIM { #if AP_SIM_SLUNGPAYLOAD_ENABLED SlungPayloadSim slung_payload_sim; #endif +#if AP_SIM_TETHER_ENABLED + TetherSim tether_sim; +#endif #if AP_SIM_FLIGHTAXIS_ENABLED FlightAxis *flightaxis_ptr; #endif diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index 98d5e0b7a06e42..ccba94be53b180 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -612,7 +612,7 @@ SIM_BATT_VOLTAGE,12.6 SIM_DRIFT_SPEED,0.05 SIM_DRIFT_TIME,5 SIM_ENGINE_FAIL,0 -SIM_ENGINE_MUL,1 +SIM_ENGINE_MUL,0 SIM_FLOAT_EXCEPT,1 SIM_FLOW_DELAY,0 SIM_FLOW_ENABLE,0 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 960c2c713c2c90..353379fa5a10dc 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -632,7 +632,7 @@ SIM_BATT_VOLTAGE,12.6 SIM_DRIFT_SPEED,0.05 SIM_DRIFT_TIME,5 SIM_ENGINE_FAIL,0 -SIM_ENGINE_MUL,1 +SIM_ENGINE_MUL,0 SIM_FLOAT_EXCEPT,1 SIM_FLOW_DELAY,0 SIM_FLOW_ENABLE,0