diff --git a/.github/problem-matchers/autotestfail.json b/.github/problem-matchers/autotestfail.json new file mode 100644 index 00000000000000..ccda5c1a940f7e --- /dev/null +++ b/.github/problem-matchers/autotestfail.json @@ -0,0 +1,17 @@ +{ + "__comment": "by buzz try to match common autotest warnings and errors that arent caught by gcc.json or python.json", + "problemMatcher": [ + { + "owner": "autotest-fail-matcher", + "severity": "error", + "pattern": [ + { + "regexp": "^(.*)(TIMEOUT|Build failed|FAILED STEP):(.*)$", + "column": 1, + "code": 2, + "message": 3 + } + ] + } + ] +} \ No newline at end of file diff --git a/.github/problem-matchers/autotestwarn.json b/.github/problem-matchers/autotestwarn.json new file mode 100644 index 00000000000000..6aa77a7ece6baa --- /dev/null +++ b/.github/problem-matchers/autotestwarn.json @@ -0,0 +1,17 @@ +{ + "__comment": "by buzz try to match common autotest warnings and errors that arent caught by gcc.json or python.json", + "problemMatcher": [ + { + "owner": "autotest-warn-matcher", + "severity": "warning", + "pattern": [ + { + "regexp": "^(.*)(WARN|WARNING):(.*)$", + "column": 1, + "code": 2, + "message": 3 + } + ] + } + ] +} \ No newline at end of file diff --git a/.github/problem-matchers/gcc.json b/.github/problem-matchers/gcc.json new file mode 100644 index 00000000000000..bd5ab6c00a7608 --- /dev/null +++ b/.github/problem-matchers/gcc.json @@ -0,0 +1,18 @@ +{ + "__comment": "Taken from vscode-cpptools's Extension/package.json gcc rule", + "problemMatcher": [ + { + "owner": "gcc-problem-matcher", + "pattern": [ + { + "regexp": "^(.*):(\\d+):(\\d+):\\s+(?:fatal\\s+)?(warning|error):\\s+(.*)$", + "file": 1, + "line": 2, + "column": 3, + "severity": 4, + "message": 5 + } + ] + } + ] +} \ No newline at end of file diff --git a/.github/problem-matchers/python.json b/.github/problem-matchers/python.json new file mode 100644 index 00000000000000..a14ab2dd7610b0 --- /dev/null +++ b/.github/problem-matchers/python.json @@ -0,0 +1,22 @@ +{ + "__comment": "inspired by https://github.com/microsoft/vscode-python/issues/3828#issuecomment-575439587", + "problemMatcher": [ + { + "owner": "python-problem-matcher", + "pattern": [ + { + "regexp": "^.*File \\\"([^\\\"]|.*)\\\", line (\\d+).*", + "file": 1, + "line": 2 + }, + { + "regexp": "^.*raise.*$" + }, + { + "regexp": "^\\s*(.*)\\s*$", + "message": 1 + } + ] + } + ] +} \ No newline at end of file diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index 47ada972560a46..26185663fe7ac5 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -151,6 +151,7 @@ jobs: matrix: config: [ esp32buzz, + esp32s3empty, ] gcc: [10] @@ -158,22 +159,50 @@ jobs: - uses: actions/checkout@v4 with: submodules: 'recursive' + + - name: Register gcc problem matcher + run: echo "::add-matcher::.github/problem-matchers/gcc.json" + + - name: Register python problem matcher + run: echo "::add-matcher::.github/problem-matchers/python.json" + + - name: Register autotest warn matcher + run: echo "::add-matcher::.github/problem-matchers/autotestwarn.json" + + - name: Register autotest fail matcher + run: echo "::add-matcher::.github/problem-matchers/autotestfail.json" + - name: Install Prerequisites shell: bash run: | - sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-setuptools python3-serial python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 + 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 + python --version + pip3 install gevent - rm -rf /usr/local/bin/cmake + # we actualy want 3.11 .. but the above only gave us 3.10, not ok with esp32 builds. + sudo add-apt-repository ppa:deadsnakes/ppa + 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 + python --version + python3.11 --version + which python3.11 + sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.11 11 + update-alternatives --query python - sudo apt remove --purge --auto-remove cmake - sudo apt update && \ - sudo apt install -y software-properties-common lsb-release && \ - sudo apt clean all + 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 update - sudo apt install cmake + sudo apt-get update + sudo apt-get install cmake git submodule update --init --recursive --depth=1 @@ -183,7 +212,7 @@ jobs: cd modules/esp_idf echo "Installing ESP-IDF tools...." - ./install.sh 2>&1 > /dev/null + ./install.sh esp32,esp32s3 cd ../.. @@ -195,32 +224,48 @@ jobs: source ~/.bash_profile PATH="/github/home/.local/bin:$PATH" echo $PATH - + echo "### Configure ${{matrix.config}} :rocket:" >> $GITHUB_STEP_SUMMARY cd modules/esp_idf ./install.sh source ./export.sh cd ../.. python -m pip install --progress-bar off future lxml pymavlink MAVProxy pexpect flake8 geocoder empy==3.3.4 dronecan which cmake - ./waf configure --board ${{matrix.config}} - ./waf plane - cp build/esp32buzz/esp-idf_build/ardupilot.bin ./ArduPlane.bin - cp build/esp32buzz/esp-idf_build/ardupilot.elf ./ArduPlane.elf - ./waf copter - cp build/esp32buzz/esp-idf_build/ardupilot.bin ./ArduCopter.bin - cp build/esp32buzz/esp-idf_build/ardupilot.elf ./ArduCopter.elf + ./waf configure --board ${{matrix.config}} + echo './waf configure --board ${{matrix.config}}' >> $GITHUB_STEP_SUMMARY + echo "### Build Plane ${{matrix.config}} :rocket:" >> $GITHUB_STEP_SUMMARY + echo './waf plane' >> $GITHUB_STEP_SUMMARY + ./waf plane | tee >( grep -vE 'Compiling|Generat|includes.list|Parsing|Validation|Building|Linking|Detecting|linker|\*\*\*\*\*\*\*|Partition|SubType|Checking|esp-idf' --color=never --line-buffered > summary.log) + cat summary.log >> $GITHUB_STEP_SUMMARY + + cp build/${{matrix.config}}/esp-idf_build/ardupilot.bin ./ArduPlane.${{matrix.config}}.bin + cp build/${{matrix.config}}/esp-idf_build/ardupilot.elf ./ArduPlane.${{matrix.config}}.elf + echo "### Build Copter ${{matrix.config}} :rocket:" >> $GITHUB_STEP_SUMMARY + echo './waf copter' >> $GITHUB_STEP_SUMMARY + ./waf copter | tee >( grep -vE 'Compiling|Generat|includes.list|Parsing|Validation|Building|Linking|Detecting|linker|\*\*\*\*\*\*\*|Partition|SubType|Checking|esp-idf' --color=never --line-buffered > summary2.log) + + cat summary2.log >> $GITHUB_STEP_SUMMARY + + cp build/${{matrix.config}}/esp-idf_build/ardupilot.bin ./ArduCopter.${{matrix.config}}.bin + cp build/${{matrix.config}}/esp-idf_build/ardupilot.elf ./ArduCopter.${{matrix.config}}.elf + + cp build/${{matrix.config}}/esp-idf_build/bootloader/bootloader.bin ./bootloader.${{matrix.config}}.bin + cp build/${{matrix.config}}/esp-idf_build/partition_table/partition-table.bin ./partition-table.${{matrix.config}}.bin + + echo "### Assets avail in artifact:" >> $GITHUB_STEP_SUMMARY + ls bootloader* partition* Ardu*.elf Ardu*.bin >> $GITHUB_STEP_SUMMARY - name: Archive artifacts uses: actions/upload-artifact@v3 with: name: esp32-binaries -${{matrix.config}} path: | - /home/runner/work/ardupilot/ardupilot/ArduPlane.bin - /home/runner/work/ardupilot/ardupilot/ArduPlane.elf - /home/runner/work/ardupilot/ardupilot/ArduCopter.bin - /home/runner/work/ardupilot/ardupilot/ArduCopter.elf - /home/runner/work/ardupilot/ardupilot/build/esp32buzz/esp-idf_build/bootloader/bootloader.bin - /home/runner/work/ardupilot/ardupilot/build/esp32buzz/esp-idf_build/partition_table/partition-table.bin + /home/runner/work/ardupilot/ardupilot/ArduPlane.${{matrix.config}}.bin + /home/runner/work/ardupilot/ardupilot/ArduPlane.${{matrix.config}}.elf + /home/runner/work/ardupilot/ardupilot/ArduCopter.${{matrix.config}}.bin + /home/runner/work/ardupilot/ardupilot/ArduCopter.${{matrix.config}}.elf + /home/runner/work/ardupilot/ardupilot/bootloader.${{matrix.config}}.bin + /home/runner/work/ardupilot/ardupilot/partition-table.${{matrix.config}}.bin retention-days: 14 diff --git a/.github/workflows/test_chibios.yml b/.github/workflows/test_chibios.yml index 3fe7c730eac021..6fe345defd02f7 100644 --- a/.github/workflows/test_chibios.yml +++ b/.github/workflows/test_chibios.yml @@ -155,7 +155,10 @@ jobs: CubeRedPrimary-bootloader, configure-all, build-options-defaults-test, - signing + signing, + CubeOrange-PPP, + CubeOrange-SOHW, + Pixhawk6X-PPPGW, ] toolchain: [ chibios, diff --git a/.github/workflows/test_coverage.yml b/.github/workflows/test_coverage.yml index dda488770f7762..c6dba24a392fc1 100644 --- a/.github/workflows/test_coverage.yml +++ b/.github/workflows/test_coverage.yml @@ -72,7 +72,7 @@ jobs: Tools/scripts/run_coverage.py -f else Tools/scripts/run_coverage.py -i - ./waf configure --board sitl_periph_gps --debug --coverage + ./waf configure --board sitl_periph_universal --debug --coverage ./waf build --target bin/AP_Periph Tools/scripts/run_coverage.py -i Tools/autotest/autotest.py test.CAN --debug --coverage diff --git a/.github/workflows/test_sitl_periph.yml b/.github/workflows/test_sitl_periph.yml index 0f4b5453fbef42..df6588792cfe7c 100644 --- a/.github/workflows/test_sitl_periph.yml +++ b/.github/workflows/test_sitl_periph.yml @@ -187,12 +187,12 @@ jobs: PATH="/github/home/.local/bin:$PATH" python modules/DroneCAN/dronecan_dsdlc/dronecan_dsdlc.py -O dsdlc_generated modules/DroneCAN/DSDL/uavcan modules/DroneCAN/DSDL/dronecan modules/DroneCAN/DSDL/com --run-test - - name: build sitl_periph_gps + - name: build sitl_periph_universal shell: bash run: | git config --global --add safe.directory ${GITHUB_WORKSPACE} PATH="/github/home/.local/bin:$PATH" - ./waf configure --board sitl_periph_gps + ./waf configure --board sitl_periph_universal ./waf build --target bin/AP_Periph ccache -s ccache -z diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index d17c0d154e16e2..444ad7f2118c64 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -192,6 +192,8 @@ jobs: - name: build rover ${{ matrix.toolchain }} shell: bash run: | + sudo apt-get update || true + sudo apt-get -y install ppp || true git config --global --add safe.directory ${GITHUB_WORKSPACE} if [[ ${{ matrix.toolchain }} == "clang" ]]; then export CC=clang diff --git a/.gitmodules b/.gitmodules index fbfe76753a51d6..6a900f5fbaad67 100644 --- a/.gitmodules +++ b/.gitmodules @@ -39,3 +39,6 @@ path = modules/Micro-CDR url = https://github.com/ardupilot/Micro-CDR.git branch = master +[submodule "modules/lwip"] + path = modules/lwip + url = https://github.com/ArduPilot/lwip.git diff --git a/.vscode/settings.default.json b/.vscode/settings.default.json index d6469cb6b6bae8..1a5ba955dd21ab 100644 --- a/.vscode/settings.default.json +++ b/.vscode/settings.default.json @@ -1,4 +1,5 @@ { "cSpell.language": "en-GB", - "astyle.astylerc": "${workspaceFolder}/Tools/CodeStyle/astylerc" + "astyle.astylerc": "${workspaceFolder}/Tools/CodeStyle/astylerc", + "Lua.runtime.version": "Lua 5.3" } diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 0fbe19edaa8ca8..270d3a74df838f 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -161,12 +161,6 @@ void GCS_MAVLINK_Tracker::send_pid_tuning() } } -bool GCS_MAVLINK_Tracker::handle_guided_request(AP_Mission::Mission_Command&) -{ - // do nothing - return false; -} - /* default stream rates to 1Hz */ @@ -313,6 +307,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_MAG_CAL_PROGRESS, #endif MSG_EKF_STATUS_REPORT, + MSG_BATTERY_STATUS, }; static const ap_message STREAM_PARAMS_msgs[] = { MSG_NEXT_PARAM @@ -463,7 +458,7 @@ bool GCS_MAVLINK_Tracker::set_home(const Location& loc, bool _lock) { return tracker.set_home(loc); } -void GCS_MAVLINK_Tracker::handleMessage(const mavlink_message_t &msg) +void GCS_MAVLINK_Tracker::handle_message(const mavlink_message_t &msg) { switch (msg.msgid) { @@ -471,9 +466,38 @@ void GCS_MAVLINK_Tracker::handleMessage(const mavlink_message_t &msg) handle_set_attitude_target(msg); break; +#if AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED // When mavproxy 'wp sethome' case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: - { + handle_message_mission_write_partial_list(msg); + break; + + // XXX receive a WP from GCS and store in EEPROM if it is HOME + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_message_mission_item(msg); + break; +#endif + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + handle_message_manual_control(msg); + break; + + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + handle_message_global_position_int(msg); + break; + + case MAVLINK_MSG_ID_SCALED_PRESSURE: + handle_message_scaled_pressure(msg); + break; + } + + GCS_MAVLINK::handle_message(msg); +} + + +#if AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED +void GCS_MAVLINK_Tracker::handle_message_mission_write_partial_list(const mavlink_message_t &msg) +{ // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(&msg, &packet); @@ -483,13 +507,10 @@ void GCS_MAVLINK_Tracker::handleMessage(const mavlink_message_t &msg) waypoint_receiving = true; send_message(MSG_NEXT_MISSION_REQUEST_WAYPOINTS); } - break; - } +} - // XXX receive a WP from GCS and store in EEPROM if it is HOME - case MAVLINK_MSG_ID_MISSION_ITEM: - { - // decode +void GCS_MAVLINK_Tracker::handle_message_mission_item(const mavlink_message_t &msg) +{ mavlink_mission_item_t packet; MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED; @@ -572,48 +593,37 @@ void GCS_MAVLINK_Tracker::handleMessage(const mavlink_message_t &msg) } mission_failed: - // we are rejecting the mission/waypoint + // send ACK (including in success case) mavlink_msg_mission_ack_send( chan, msg.sysid, msg.compid, result, MAV_MISSION_TYPE_MISSION); - break; - } +} +#endif - case MAVLINK_MSG_ID_MANUAL_CONTROL: - { +void GCS_MAVLINK_Tracker::handle_message_manual_control(const mavlink_message_t &msg) +{ mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(&msg, &packet); tracker.tracking_manual_control(packet); - break; - } +} - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: - { +void GCS_MAVLINK_Tracker::handle_message_global_position_int(const mavlink_message_t &msg) +{ // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(&msg, &packet); tracker.tracking_update_position(packet); - break; - } +} - case MAVLINK_MSG_ID_SCALED_PRESSURE: - { - // decode +void GCS_MAVLINK_Tracker::handle_message_scaled_pressure(const mavlink_message_t &msg) +{ mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(&msg, &packet); tracker.tracking_update_pressure(packet); - break; - } - - default: - handle_common_message(msg); - break; - } // end switch -} // end handle mavlink - +} // send position tracker is using void GCS_MAVLINK_Tracker::send_global_position_int() diff --git a/AntennaTracker/GCS_Mavlink.h b/AntennaTracker/GCS_Mavlink.h index 38f7cdb483e712..79610f00311ec0 100644 --- a/AntennaTracker/GCS_Mavlink.h +++ b/AntennaTracker/GCS_Mavlink.h @@ -37,8 +37,12 @@ class GCS_MAVLINK_Tracker : public GCS_MAVLINK void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override; void mavlink_check_target(const mavlink_message_t &msg); - void handleMessage(const mavlink_message_t &msg) override; - bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; + void handle_message(const mavlink_message_t &msg) override; + void handle_message_mission_write_partial_list(const mavlink_message_t &msg); + void handle_message_mission_item(const mavlink_message_t &msg); + void handle_message_manual_control(const mavlink_message_t &msg); + void handle_message_global_position_int(const mavlink_message_t &msg); + void handle_message_scaled_pressure(const mavlink_message_t &msg); void handle_set_attitude_target(const mavlink_message_t &msg); void send_global_position_int() override; diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp index 742d0247189d56..5a70a09eb44305 100644 --- a/AntennaTracker/Log.cpp +++ b/AntennaTracker/Log.cpp @@ -1,6 +1,6 @@ #include "Tracker.h" -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED // Code to Write and Read packets from AP_Logger log memory @@ -98,13 +98,4 @@ void Tracker::log_init(void) logger.Init(log_structure, ARRAY_SIZE(log_structure)); } -#else // LOGGING_ENABLED - -void Tracker::Log_Write_Attitude(void) {} - -void Tracker::log_init(void) {} -void Tracker::Log_Write_Vehicle_Pos(int32_t lat, int32_t lng, int32_t alt, const Vector3f& vel) {} -void Tracker::Log_Write_Vehicle_Baro(float pressure, float altitude) {} -void Tracker::Log_Write_Vehicle_Startup_Messages() {} - -#endif // LOGGING_ENABLED +#endif // HAL_LOGGING_ENABLED diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 2d20396ecfc694..c21d5dbf4f5d7e 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -505,12 +505,6 @@ const AP_Param::Info Tracker::var_info[] = { GGROUP(pidYaw2Srv, "YAW2SRV_", AC_PID), -#if AP_SCRIPTING_ENABLED - // @Group: SCR_ - // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp - GOBJECT(scripting, "SCR_", AP_Scripting), -#endif - // @Param: CMD_TOTAL // @DisplayName: Number of loaded mission items // @Description: Set to 1 if HOME location has been loaded by the ground station. Do not change this manually. @@ -560,10 +554,6 @@ const AP_Param::Info Tracker::var_info[] = { // @User: Standard GSCALAR(disarm_pwm, "SAFE_DISARM_PWM", 0), - // @Group: STAT - // @Path: ../libraries/AP_Stats/AP_Stats.cpp - GOBJECT(stats, "STAT", AP_Stats), - // @Param: AUTO_OPTIONS // @DisplayName: Auto mode options // @Description: 1: Scan for unknown target @@ -576,9 +566,11 @@ const AP_Param::Info Tracker::var_info[] = { // @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp PARAM_VEHICLE_INFO, +#if HAL_LOGGING_ENABLED // @Group: LOG // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#endif #if HAL_NAVEKF2_AVAILABLE // @Group: EK2_ @@ -615,6 +607,17 @@ void Tracker::load_parameters(void) uint32_t before = AP_HAL::micros(); // Load all auto-loaded EEPROM variables AP_Param::load_all(); + +#if AP_STATS_ENABLED + // PARAMETER_CONVERSION - Added: Jan-2024 + AP_Param::convert_class(g.k_param_stats_old, &stats, stats.var_info, 0, 0, true); +#endif + +#if AP_SCRIPTING_ENABLED + // PARAMETER_CONVERSION - Added: Jan-2024 + AP_Param::convert_class(g.k_param_scripting_old, &scripting, scripting.var_info, 0, 0, true); +#endif + hal.console->printf("load_all took %luus\n", (unsigned long)(AP_HAL::micros() - before)); #if HAL_HAVE_SAFETY_SWITCH diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 972c36f52505e8..1db980b04aaa00 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -44,8 +44,8 @@ class Parameters { k_param_format_version = 0, k_param_software_type, // deprecated - k_param_gcs0 = 100, // stream rates for uartA - k_param_gcs1, // stream rates for uartC + k_param_gcs0 = 100, // stream rates for SERIAL0 + k_param_gcs1, // stream rates for SERIAL1 k_param_sysid_this_mav, k_param_sysid_my_gcs, k_param_serial0_baud, // deprecated @@ -60,7 +60,7 @@ class Parameters { k_param_sitl, k_param_pidPitch_old, // deprecated k_param_pidYaw_old, // deprecated - k_param_gcs2, // stream rates for uartD + k_param_gcs2, // stream rates for SERIAL2 k_param_serial2_baud, // deprecated k_param_yaw_slew_time, @@ -114,8 +114,8 @@ class Parameters { k_param_rc_channels, k_param_servo_channels, - k_param_stats = 218, - k_param_scripting = 219, + k_param_stats_old = 218, + k_param_scripting_old = 219, // // 220: Waypoint data diff --git a/AntennaTracker/Tracker.cpp b/AntennaTracker/Tracker.cpp index fbf3427c501c55..cd4a4eb5777e18 100644 --- a/AntennaTracker/Tracker.cpp +++ b/AntennaTracker/Tracker.cpp @@ -59,8 +59,8 @@ const AP_Scheduler::Task Tracker::scheduler_tasks[] = { SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_receive, 50, 1700, 45), SCHED_TASK_CLASS(GCS, (GCS*)&tracker._gcs, update_send, 50, 3000, 50), SCHED_TASK_CLASS(AP_Baro, &tracker.barometer, accumulate, 50, 900, 55), +#if HAL_LOGGING_ENABLED SCHED_TASK(ten_hz_logging_loop, 10, 300, 60), -#if LOGGING_ENABLED == ENABLED SCHED_TASK_CLASS(AP_Logger, &tracker.logger, periodic_tasks, 50, 300, 65), #endif SCHED_TASK_CLASS(AP_InertialSensor, &tracker.ins, periodic, 50, 50, 70), @@ -107,6 +107,7 @@ void Tracker::one_second_loop() g.pidYaw2Srv.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz()); } +#if HAL_LOGGING_ENABLED void Tracker::ten_hz_logging_loop() { if (should_log(MASK_LOG_IMU)) { @@ -122,6 +123,7 @@ void Tracker::ten_hz_logging_loop() logger.Write_RCOUT(); } } +#endif Mode *Tracker::mode_from_mode_num(const Mode::Number num) { @@ -157,14 +159,15 @@ Mode *Tracker::mode_from_mode_num(const Mode::Number num) */ void Tracker::stats_update(void) { - stats.set_flying(hal.util->get_soft_armed()); - stats.update(); + AP::stats()->set_flying(hal.util->get_soft_armed()); } const AP_HAL::HAL& hal = AP_HAL::get_HAL(); Tracker::Tracker(void) +#if HAL_LOGGING_ENABLED : logger(g.log_bitmask) +#endif { } diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 55c76985b22b55..22c7687ef73ea9 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -40,7 +40,6 @@ #include #include #include -#include // statistics library #include // Battery monitor library // Configuration @@ -54,10 +53,6 @@ #include "AP_Arming.h" -#if AP_SCRIPTING_ENABLED -#include -#endif - #include "mode.h" class Tracker : public AP_Vehicle { @@ -79,7 +74,9 @@ class Tracker : public AP_Vehicle { uint32_t start_time_ms = 0; +#if HAL_LOGGING_ENABLED AP_Logger logger; +#endif /** antenna control channels @@ -96,8 +93,6 @@ class Tracker : public AP_Vehicle { GCS_Tracker _gcs; // avoid using this; use gcs() GCS_Tracker &gcs() { return _gcs; } - AP_Stats stats; - // Battery Sensors AP_BattMonitor battery{MASK_LOG_CURRENT, FUNCTOR_BIND_MEMBER(&Tracker::handle_battery_failsafe, void, const char*, const int8_t), @@ -116,10 +111,6 @@ class Tracker : public AP_Vehicle { ModeServoTest mode_servotest; ModeStop mode_stop; -#if AP_SCRIPTING_ENABLED - AP_Scripting scripting; -#endif - // Vehicle state struct { bool location_valid; // true if we have a valid location for the vehicle diff --git a/AntennaTracker/config.h b/AntennaTracker/config.h index 745f266a1a41c9..fd2932550ddbaa 100644 --- a/AntennaTracker/config.h +++ b/AntennaTracker/config.h @@ -57,9 +57,6 @@ // // Logging control // -#ifndef LOGGING_ENABLED -# define LOGGING_ENABLED ENABLED -#endif // Default logging bitmask #ifndef DEFAULT_LOG_BITMASK @@ -72,3 +69,7 @@ MASK_LOG_COMPASS | \ MASK_LOG_CURRENT #endif + +#ifndef AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED +#define AP_TRACKER_SET_HOME_VIA_MISSION_UPLOAD_ENABLED 1 +#endif diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 3e296c3fa5a77f..db4438ded4c063 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -5,14 +5,6 @@ static const StorageAccess wp_storage(StorageManager::StorageMission); void Tracker::init_ardupilot() { - // initialise stats module - stats.init(); - - BoardConfig.init(); -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - can_mgr.init(); -#endif - // initialise notify notify.init(); AP_Notify::flags.pre_arm_check = true; @@ -32,14 +24,10 @@ void Tracker::init_ardupilot() // try to initialise stream rates in the main loop. gcs().update_send(); -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED log_init(); #endif -#if AP_SCRIPTING_ENABLED - scripting.init(); -#endif // AP_SCRIPTING_ENABLED - // initialise compass AP::compass().set_log_bit(MASK_LOG_COMPASS); AP::compass().init(); @@ -56,8 +44,10 @@ void Tracker::init_ardupilot() barometer.calibrate(); +#if HAL_LOGGING_ENABLED // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&tracker, &Tracker::Log_Write_Vehicle_Startup_Messages, void)); +#endif // initialise rc channels including setting mode rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM); @@ -152,13 +142,17 @@ bool Tracker::set_home(const Location &temp) void Tracker::arm_servos() { hal.util->set_soft_armed(true); +#if HAL_LOGGING_ENABLED logger.set_vehicle_armed(true); +#endif } void Tracker::disarm_servos() { hal.util->set_soft_armed(false); +#if HAL_LOGGING_ENABLED logger.set_vehicle_armed(false); +#endif } /* @@ -189,8 +183,10 @@ void Tracker::set_mode(Mode &newmode, const ModeReason reason) disarm_servos(); } +#if HAL_LOGGING_ENABLED // log mode change logger.Write_Mode((uint8_t)mode->number(), reason); +#endif gcs().send_message(MSG_HEARTBEAT); nav_status.bearing = ahrs.yaw_sensor * 0.01f; @@ -228,6 +224,7 @@ bool Tracker::set_mode(const uint8_t new_mode, const ModeReason reason) return true; } +#if HAL_LOGGING_ENABLED /* should we log a message type now? */ @@ -238,7 +235,7 @@ bool Tracker::should_log(uint32_t mask) } return true; } - +#endif #include #include diff --git a/AntennaTracker/tracking.cpp b/AntennaTracker/tracking.cpp index cd0bf531336ede..facd7141b53ebc 100644 --- a/AntennaTracker/tracking.cpp +++ b/AntennaTracker/tracking.cpp @@ -138,10 +138,12 @@ void Tracker::tracking_update_position(const mavlink_global_position_int_t &msg) vehicle.vel = Vector3f(msg.vx/100.0f, msg.vy/100.0f, msg.vz/100.0f); vehicle.last_update_us = AP_HAL::micros(); vehicle.last_update_ms = AP_HAL::millis(); +#if HAL_LOGGING_ENABLED // log vehicle as GPS2 if (should_log(MASK_LOG_GPS)) { Log_Write_Vehicle_Pos(vehicle.location.lat, vehicle.location.lng, vehicle.location.alt, vehicle.vel); } +#endif } @@ -167,8 +169,10 @@ void Tracker::tracking_update_pressure(const mavlink_scaled_pressure_t &msg) } } +#if HAL_LOGGING_ENABLED // log vehicle baro data Log_Write_Vehicle_Baro(aircraft_pressure, alt_diff); +#endif } /** diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index bdd901706b50de..9b89e93abc030e 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -9,7 +9,6 @@ //#define AC_OAPATHPLANNER_ENABLED DISABLED // disable path planning around obstacles //#define PARACHUTE DISABLED // disable parachute release to save 1k of flash //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands -//#define STATS_ENABLED DISABLED // disable statistics support //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support //#define MODE_AUTO_ENABLED DISABLED // disable auto mode support //#define MODE_BRAKE_ENABLED DISABLED // disable brake mode support diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index d2e4838f8ce30e..30226a9020bbc5 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -398,10 +398,10 @@ bool AP_Arming_Copter::pre_arm_ekf_attitude_check() return filt_status.flags.attitude; } +#if HAL_PROXIMITY_ENABLED // check nothing is too close to vehicle bool AP_Arming_Copter::proximity_checks(bool display_failure) const { -#if HAL_PROXIMITY_ENABLED if (!AP_Arming::proximity_checks(display_failure)) { return false; @@ -425,9 +425,9 @@ bool AP_Arming_Copter::proximity_checks(bool display_failure) const } #endif -#endif return true; } +#endif // HAL_PROXIMITY_ENABLED // performs mandatory gps checks. returns true if passed bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) @@ -694,8 +694,10 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ return false; } +#if HAL_LOGGING_ENABLED // let logger know that we're armed (it may open logs e.g.) AP::logger().set_vehicle_armed(true); +#endif // disable cpu failsafe because initialising everything takes a while copter.failsafe_disable(); @@ -722,7 +724,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ if (!ahrs.home_is_set()) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.resetHeightDatum(); - AP::logger().Write_Event(LogEvent::EKF_ALT_RESET); + LOGGER_WRITE_EVENT(LogEvent::EKF_ALT_RESET); // we have reset height, so arming height is zero copter.arming_altitude_m = 0; @@ -755,8 +757,10 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ // finally actually arm the motors copter.motors->armed(true); +#if HAL_LOGGING_ENABLED // log flight mode in case it was changed while vehicle was disarmed AP::logger().Write_Mode((uint8_t)copter.flightmode->mode_number(), copter.control_mode_reason); +#endif // re-enable failsafe copter.failsafe_enable(); @@ -837,7 +841,9 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che copter.mode_auto.mission.reset(); #endif +#if HAL_LOGGING_ENABLED AP::logger().set_vehicle_armed(false); +#endif hal.util->set_soft_armed(false); diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index 15495469b48840..180525594cdb6f 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -27,7 +27,9 @@ class AP_Arming_Copter : public AP_Arming bool pre_arm_checks(bool display_failure) override; bool pre_arm_ekf_attitude_check(); +#if HAL_PROXIMITY_ENABLED bool proximity_checks(bool display_failure) const override; +#endif bool arm_checks(AP_Arming::Method method) override; // mandatory checks that cannot be bypassed. This function will only be called if ARMING_CHECK is zero or arming forced diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index a3e60e80763791..3fe709b83e756d 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -9,7 +9,7 @@ void Copter::set_auto_armed(bool b) ap.auto_armed = b; if(b){ - AP::logger().Write_Event(LogEvent::AUTO_ARMED); + LOGGER_WRITE_EVENT(LogEvent::AUTO_ARMED); } } @@ -24,17 +24,17 @@ void Copter::set_simple_mode(SimpleMode b) if (simple_mode != b) { switch (b) { case SimpleMode::NONE: - AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF); + LOGGER_WRITE_EVENT(LogEvent::SET_SIMPLE_OFF); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off"); break; case SimpleMode::SIMPLE: - AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON); + LOGGER_WRITE_EVENT(LogEvent::SET_SIMPLE_ON); gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on"); break; case SimpleMode::SUPERSIMPLE: // initialise super simple heading update_super_simple_bearing(true); - AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON); + LOGGER_WRITE_EVENT(LogEvent::SET_SUPERSIMPLE_ON); gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on"); break; } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index c1218651342394..5374c79357765a 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -143,7 +143,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { // camera mount's fast update FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast), #endif +#if HAL_LOGGING_ENABLED FAST_TASK(Log_Video_Stabilisation), +#endif SCHED_TASK(rc_loop, 250, 130, 3), SCHED_TASK(throttle_loop, 50, 75, 6), @@ -188,7 +190,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if FRAME_CONFIG == HELI_FRAME SCHED_TASK(check_dynamic_flight, 50, 75, 72), #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK(loop_rate_logging, LOOP_RATE, 50, 75), #endif SCHED_TASK(one_hz_loop, 1, 100, 81), @@ -209,14 +211,16 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if AP_CAMERA_ENABLED SCHED_TASK_CLASS(AP_Camera, &copter.camera, update, 50, 75, 111), #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK(ten_hz_logging_loop, 10, 350, 114), SCHED_TASK(twentyfive_hz_logging, 25, 110, 117), SCHED_TASK_CLASS(AP_Logger, &copter.logger, periodic_tasks, 400, 300, 120), #endif SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50, 123), +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75, 126), +#endif #if AP_RPM_ENABLED SCHED_TASK_CLASS(AP_RPM, &copter.rpm_sensor, update, 40, 200, 129), #endif @@ -256,9 +260,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_BUTTON_ENABLED SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168), #endif -#if STATS_ENABLED == ENABLED - SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100, 171), -#endif }; void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, @@ -512,6 +513,7 @@ void Copter::update_batt_compass(void) } } +#if HAL_LOGGING_ENABLED // Full rate logging of attitude, rate and pid loops // should be run at loop rate void Copter::loop_rate_logging() @@ -609,6 +611,7 @@ void Copter::twentyfive_hz_logging() } #endif } +#endif // HAL_LOGGING_ENABLED // three_hz_loop - 3hz loop void Copter::three_hz_loop() @@ -638,9 +641,11 @@ void Copter::three_hz_loop() // one_hz_loop - runs at 1Hz void Copter::one_hz_loop() { +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::AP_STATE, ap.value); } +#endif if (!motors->armed()) { update_using_interlock(); @@ -657,8 +662,10 @@ void Copter::one_hz_loop() // update assigned functions and enable auxiliary servos SRV_Channels::enable_aux_servos(); +#if HAL_LOGGING_ENABLED // log terrain data terrain_logging(); +#endif #if HAL_ADSB_ENABLED adsb.set_is_flying(!ap.land_complete); @@ -685,10 +692,12 @@ void Copter::init_simple_bearing() super_simple_cos_yaw = simple_cos_yaw; super_simple_sin_yaw = simple_sin_yaw; +#if HAL_LOGGING_ENABLED // log the simple bearing if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::INIT_SIMPLE_BEARING, ahrs.yaw_sensor); } +#endif } // update_simple_mode - rotates pilot input if we are in simple mode @@ -757,6 +766,7 @@ void Copter::update_altitude() // read in baro altitude read_barometer(); +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); if (!should_log(MASK_LOG_FTN_FAST)) { @@ -766,6 +776,7 @@ void Copter::update_altitude() #endif } } +#endif } // vehicle specific waypoint info helpers @@ -808,7 +819,10 @@ bool Copter::get_rate_ef_targets(Vector3f& rate_ef_targets) const constructor for main Copter class */ Copter::Copter(void) - : logger(g.log_bitmask), + : +#if HAL_LOGGING_ENABLED + logger(g.log_bitmask), +#endif flight_modes(&g.flight_mode1), simple_cos_yaw(1.0f), super_simple_cos_yaw(1.0), @@ -816,7 +830,10 @@ Copter::Copter(void) rc_throttle_control_in_filter(1.0f), inertial_nav(ahrs), param_loader(var_info), - flightmode(&mode_stabilize) + flightmode(&mode_stabilize), + pos_variance_filt(FS_EKF_FILT_DEFAULT), + vel_variance_filt(FS_EKF_FILT_DEFAULT), + hgt_variance_filt(FS_EKF_FILT_DEFAULT) { } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bb468a39939a52..633d413cdc1207 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -47,7 +47,6 @@ #include // Position control library #include // Command model library #include // AP Motors library -#include // statistics library #include // Filter library #include // needed for AHRS build #include // inertial navigation library @@ -252,7 +251,9 @@ class Copter : public AP_Vehicle { RC_Channel *channel_throttle; RC_Channel *channel_yaw; +#if HAL_LOGGING_ENABLED AP_Logger logger; +#endif // flight modes convenience array AP_Int8 *flight_modes; @@ -341,6 +342,14 @@ class Copter : public AP_Vehicle { uint32_t clear_ms; // system time high vibrations stopped } vibration_check; + // EKF variances are unfiltered and are designed to recover very quickly when possible + // thus failsafes should be triggered on filtered values in order to avoid transient errors + LowPassFilterFloat pos_variance_filt; + LowPassFilterFloat vel_variance_filt; + LowPassFilterFloat hgt_variance_filt; + bool variances_valid; + uint32_t last_ekf_check_us; + // takeoff check uint32_t takeoff_check_warning_ms; // system time user was last warned of takeoff check failure @@ -836,6 +845,7 @@ class Copter : public AP_Vehicle { // standby.cpp void standby_update(); +#if HAL_LOGGING_ENABLED // Log.cpp void Log_Write_Control_Tuning(); void Log_Write_Attitude(); @@ -857,6 +867,7 @@ class Copter : public AP_Vehicle { void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); void Log_Write_Vehicle_Startup_Messages(); void log_init(void); +#endif // HAL_LOGGING_ENABLED // mode.cpp bool set_mode(Mode::Number mode, ModeReason reason); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 7f86eabc8ef5ea..c35d96e4ba52c1 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1167,7 +1167,7 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const return true; } -void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) +void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg) { #if MODE_GUIDED_ENABLED == ENABLED // for mavlink SET_POSITION_TARGET messages @@ -1485,13 +1485,6 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } #endif - case MAVLINK_MSG_ID_RADIO: - case MAVLINK_MSG_ID_RADIO_STATUS: // MAV ID: 109 - { - handle_radio_status(msg, copter.should_log(MASK_LOG_PM)); - break; - } - case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: #if AP_TERRAIN_AVAILABLE @@ -1506,7 +1499,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) #endif default: - handle_common_message(msg); + GCS_MAVLINK::handle_message(msg); break; } // end switch } // end handle mavlink diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index d042d3000ebe06..1961916e03bae4 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -2,6 +2,7 @@ #include #include +#include "defines.h" #ifndef AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED #define AC_MAVLINK_SOLO_BUTTON_COMMAND_HANDLING_ENABLED 1 @@ -57,6 +58,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK void handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow) override; +#if HAL_LOGGING_ENABLED + uint32_t log_radio_bit() const override { return MASK_LOG_PM; } +#endif + private: // sanity check velocity or acceleration vector components are numbers @@ -66,7 +71,7 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK MISSION_STATE mission_state(const class AP_Mission &mission) const override; - void handleMessage(const mavlink_message_t &msg) override; + void handle_message(const mavlink_message_t &msg) override; void handle_command_ack(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; bool try_send_message(enum ap_message id) override; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 027544771d9112..4d9e4ef87b01b6 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -1,6 +1,6 @@ #include "Copter.h" -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED // Code to Write and Read packets from AP_Logger log memory // Code to interact with the user to dump or erase logs @@ -576,27 +576,4 @@ void Copter::log_init(void) logger.Init(log_structure, ARRAY_SIZE(log_structure)); } -#else // LOGGING_ENABLED - -void Copter::Log_Write_Control_Tuning() {} -void Copter::Log_Write_Attitude(void) {} -void Copter::Log_Write_EKF_POS() {} -void Copter::Log_Write_Data(LogDataID id, int32_t value) {} -void Copter::Log_Write_Data(LogDataID id, uint32_t value) {} -void Copter::Log_Write_Data(LogDataID id, int16_t value) {} -void Copter::Log_Write_Data(LogDataID id, uint16_t value) {} -void Copter::Log_Write_Data(LogDataID id, float value) {} -void Copter::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} -void Copter::Log_Write_Guided_Position_Target(ModeGuided::SubMode target_type, const Vector3f& pos_target, bool terrain_alt, const Vector3f& vel_target, const Vector3f& accel_target) {} -void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, float roll, float pitch, float yaw, const Vector3f &ang_vel, float thrust, float climb_rate) {} -void Copter::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {} -void Copter::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} -void Copter::Log_Write_Vehicle_Startup_Messages() {} - -#if FRAME_CONFIG == HELI_FRAME -void Copter::Log_Write_Heli() {} -#endif - -void Copter::log_init(void) {} - -#endif // LOGGING_ENABLED +#endif // HAL_LOGGING_ENABLED diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 80a79da030a7c5..35b1780ff9cc1a 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -451,9 +451,9 @@ const AP_Param::Info Copter::var_info[] = { #endif #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif #if PARACHUTE == ENABLED @@ -554,9 +554,11 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(camera_mount, "MNT", AP_Mount), #endif +#if HAL_LOGGING_ENABLED // @Group: LOG // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#endif // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -696,6 +698,20 @@ const AP_Param::Info Copter::var_info[] = { // @Values: 0:Stopped,1:Running // @User: Standard GSCALAR(throw_motor_start, "THROW_MOT_START", (float)ModeThrow::PreThrowMotorState::STOPPED), + + // @Param: THROW_ALT_MIN + // @DisplayName: Throw mode minimum altitude + // @Description: Minimum altitude above which Throw mode will detect a throw or a drop - 0 to disable the check + // @Units: m + // @User: Advanced + GSCALAR(throw_altitude_min, "THROW_ALT_MIN", 0), + + // @Param: THROW_ALT_MAX + // @DisplayName: Throw mode maximum altitude + // @Description: Maximum altitude under which Throw mode will detect a throw or a drop - 0 to disable the check + // @Units: m + // @User: Advanced + GSCALAR(throw_altitude_max, "THROW_ALT_MAX", 0), #endif #if OSD_ENABLED || OSD_PARAM_ENABLED @@ -805,11 +821,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0), -#if STATS_ENABLED == ENABLED - // @Group: STAT - // @Path: ../libraries/AP_Stats/AP_Stats.cpp - AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats), -#endif + // 12 was AP_Stats #if AP_GRIPPER_ENABLED // @Group: GRIP_ @@ -901,11 +913,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPPTR(autotune_ptr, "AUTOTUNE_", 29, ParametersG2, AutoTune), #endif -#if AP_SCRIPTING_ENABLED - // @Group: SCR_ - // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp - AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting), -#endif + // 30 was AP_Scripting // @Param: TUNE_MIN // @DisplayName: Tuning minimum @@ -1226,6 +1234,13 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { AP_GROUPINFO("TKOFF_RPM_MAX", 7, ParametersG2, takeoff_rpm_max, 0), #endif + // @Param: FS_EKF_FILT + // @DisplayName: EKF Failsafe filter cutoff + // @Description: EKF Failsafe filter cutoff frequency. EKF variances are filtered using this value to avoid spurious failsafes from transient high variances. A higher value means the failsafe is more likely to trigger. + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT), + // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND @@ -1366,6 +1381,35 @@ void Copter::load_parameters(void) AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, 0, true); #endif + // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 +#if AP_STATS_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { + return; + } + + const uint16_t old_index = 12; // Old parameter index in g2 + const uint16_t old_top_element = 4044; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(info.old_key, &stats, stats.var_info, old_index, old_top_element, false); + } +#endif + // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 +#if AP_SCRIPTING_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { + return; + } + + const uint16_t old_index = 30; // Old parameter index in g2 + const uint16_t old_top_element = 94; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(info.old_key, &scripting, scripting.var_info, old_index, old_top_element, false); + } +#endif + hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); // setup AP_Param frame type flags diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e59bf2238788b6..a7374f37744d5f 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -383,6 +383,8 @@ class Parameters { // 254,255: reserved k_param_vehicle = 257, // vehicle common block of parameters + k_param_throw_altitude_min, + k_param_throw_altitude_max, // the k_param_* space is 9-bits in size // 511: reserved @@ -463,6 +465,8 @@ class Parameters { #if MODE_THROW_ENABLED == ENABLED AP_Enum throw_motor_start; + AP_Int16 throw_altitude_min; // minimum altitude in m above which a throw can be detected + AP_Int16 throw_altitude_max; // maximum altitude in m below which a throw can be detected #endif AP_Int16 rc_speed; // speed of fast RC Channels in Hz @@ -504,11 +508,6 @@ class ParametersG2 { AP_Button *button_ptr; #endif -#if STATS_ENABLED == ENABLED - // vehicle statistics - AP_Stats stats; -#endif - #if AP_GRIPPER_ENABLED AP_Gripper gripper; #endif @@ -601,10 +600,6 @@ class ParametersG2 { void *autotune_ptr; #endif -#if AP_SCRIPTING_ENABLED - AP_Scripting scripting; -#endif // AP_SCRIPTING_ENABLED - AP_Float tuning_min; AP_Float tuning_max; @@ -686,6 +681,9 @@ class ParametersG2 { AP_Int16 takeoff_rpm_max; #endif + // EKF variance filter cutoff + AP_Float fs_ekf_filt_hz; + #if WEATHERVANE_ENABLED == ENABLED AC_WeatherVane weathervane; #endif diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index f400694b451b38..b613cb141aee43 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -229,7 +229,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi // only altitude will matter to the AP mission script for takeoff. if (copter.mode_auto.mission.add_cmd(cmd)) { // log event - AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); + LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP); } } @@ -247,7 +247,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi // save command if (copter.mode_auto.mission.add_cmd(cmd)) { // log event - AP::logger().Write_Event(LogEvent::SAVEWP_ADD_WP); + LOGGER_WRITE_EVENT(LogEvent::SAVEWP_ADD_WP); } } #endif @@ -276,15 +276,15 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi switch(ch_flag) { case AuxSwitchPos::LOW: copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::OFF); - AP::logger().Write_Event(LogEvent::ACRO_TRAINER_OFF); + LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_OFF); break; case AuxSwitchPos::MIDDLE: copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LEVELING); - AP::logger().Write_Event(LogEvent::ACRO_TRAINER_LEVELING); + LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LEVELING); break; case AuxSwitchPos::HIGH: copter.g.acro_trainer.set((uint8_t)ModeAcro::Trainer::LIMITED); - AP::logger().Write_Event(LogEvent::ACRO_TRAINER_LIMITED); + LOGGER_WRITE_EVENT(LogEvent::ACRO_TRAINER_LIMITED); break; } #endif @@ -540,12 +540,12 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi switch (ch_flag) { case AuxSwitchPos::HIGH: copter.standby_active = true; - AP::logger().Write_Event(LogEvent::STANDBY_ENABLE); + LOGGER_WRITE_EVENT(LogEvent::STANDBY_ENABLE); gcs().send_text(MAV_SEVERITY_INFO, "Stand By Enabled"); break; default: copter.standby_active = false; - AP::logger().Write_Event(LogEvent::STANDBY_DISABLE); + LOGGER_WRITE_EVENT(LogEvent::STANDBY_DISABLE); gcs().send_text(MAV_SEVERITY_INFO, "Stand By Disabled"); break; } @@ -683,7 +683,7 @@ void Copter::save_trim() float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f); float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f); ahrs.add_trim(roll_trim, pitch_trim); - AP::logger().Write_Event(LogEvent::SAVE_TRIM); + LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM); gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); } diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 62b8ca786a0e61..b80cc28e9fa23b 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,6 +1,287 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ -Copter 4.4.4 05-Dec-2023 +Copter 4.5.0-beta1 30-Jan-2025 +Changes from 4.4.4 +1) New autopilots supported + - ACNS-F405AIO + - Airvolute DCS2 onboard FMU + - Aocoda-RC-H743Dual + - BrainFPV RADIX 2 HD + - CAN-Zero + - CM4Pilot + - CubeRed + - Esp32-tomte76, esp32nick, esp32s3devkit + - FlyingMoonH743 + - Flywoo F405 Pro + - FlywooF405S-AIO with alternative IMUs + - Here4 GPS as flight controller + - Holybro 6X revision 6 + - Holybro6X-45686 with 3x ICM45686 IMUs + - JAE JFB110 + - KakuteH7 using ICM42688 + - PixFlamingo (uses STM32L4PLUS CPU) + - PixPilot-C3 + - PixSurveyA1-IND + - QiotekAdeptF407 + - Sierra TrueNavIC + - SPRacing H7RF + - SW-Nav-F405 + - YJUAV_A6 + - YJUAV_A6SE, YJUAV_A6SE_H743 +2) Autopilot specific changes + - 1MB boards lose features to save flash (Payload Place, some battery monitors, NMEA Output, bootloaders, Turtle mode) + - CubeOrangePlus supports IMU high resolution sampling (works with ICM42688, ICM42652, ICM42670, ICM45686 IMUs) + - F4 processors with only 1 IMU gain Triple Harmonic Notch support + - F765-SE bdshot support on 1st 4 pin + - F7 and H7 boards lose DMA on I2C ports (not required, limited DMA better used elsewhere) + - FlyingMoonH743, FlyingMoonF427 2nd and 3rd IMU order swapped + - HEEWING-F405 supports CRSF + - MatekL431-RC bootloader added, DMA used for RC and GPS + - PH4-mini, PH4-mini-bdshot, Swan-K1 and TBS-Colibri-F7 BRD_SER4_RTSCTS param conflict fixed + - Pixhawk6C supports BMI088 baro + - TMotorH743, Heewing-F405 serial parameter definitions fixed +3) AHRS/EKF enhancements and fixes + - AHRS_OPTIONS supports disabling fallback to DCM + - BARO_ALT_OFFSET slews more slowly (was 20sec, now 1min) + - EKF2 removed (can be re-enabled with Custom build server) + - External AHRS support for multiple GPSs + - InertialLabs INS-U external AHRS support + - Lord external AHRS renamed to MicroStrain5 + - MAV_CMD_EXTERNAL_POSITION_ESTIMATE supports setting approximate position during dead-reckoning + - Microstrain7 (aka 3DM-QG7) external AHRS support +4) Driver enhancements + - 3DR Solo supports up to 6S batteries + - Airspeed health checks vs GPS use 3D velocity + - BDshot on the first four channels of boards with F103-based IOMCUs (e.g. Pixhawk 6X) + - Dshot on all IOMCU channels on all boards with an IOMCU (e.g. all CubePilot autopilots) + - Dshot commands (e.g. motor reversal abd beeps) and EDT supported on IOMCU + - DroneCAN battery monitors calculate consumed energy if battery doesn't provide directly + - DroneCAN RC and Ghost RC protocol support + - EFI MAVLink driver + - Extended DShot Telemetry support (requires BLHeli32 ver 32.10 or BlueJay, set SERVO_DSHOT_ESC=3 or 4) + - GPS L5 band health override to enable L5 signal use (see GPS_DRV_OPTIONS) + - GPS-for-yaw works at lower update rate (3hz minimum) + - GSOF GPS supports GPS_COM_PORT parameter + - Hirth ICEngine support + - ICE option to enable/disable starting while disarmed + - ICE support for starter via relay + - IMUDATA serial protocol outputs raw IMU data on serial port (only available using custom build server) + - Innomaker LD06 360deg lidar support + - Intelligent Energy fuel cells new protocol support + - IRC Tramp supports 1G3 bands A and B + - IRC Ghost support + - JAE JRE-30 radar + - KDECAN driver rewrite (params moved to KDE_, works on all vehicles) + - MCP9601 temperature sensor support + - NanoRadar NRA24 rangefinder support + - NeoPixelsRGB support + - NoopLoop TOFSense, TOFSenseF-I2c rangefinder support + - OSD shows flashing horizon when inverted + - OSD2 support (e.g. second OSD) + - QMC5883P compass support + - Relay refactored to support RELAYx_FUNCTION, RELAY_STATUS message support added + - Reventech fuel level support (extends existing analog driver, see BATT_FL_xx parameters) + - RPLidarS1 360 deg lidar support and improved reliability for all RPLidars + - SBF GPS supports yaw from dual antennas + - Temperature sensor using analog voltages supported + - Trimble PX-1 support added as a GPS + - Winch driver enhancements including stuck protection, option for spin-free on startup +5) Control and navigation changes and enhancements + - Auto missions can always be cleared while disarmed (would fail if mission still running) + - DO_ENGINE_CONTROL allows starting engine even when disarmed + - DO_SET_MISSION_CURRENT command can reset mission (see Reset Mission field) + - DO_SET_SERVO, DO_REPEAT_SERVO work with servo outputs set to RCInxScaled + - Fractional Loiter Turn Support in missions + - HarmonicNotch supports up to 16 harmonics + - JUMP command sends improved text msg to pilot (says where will jump to) + - MAV_CMD_AIRFRAME_CONFIGURATION can control landing gear on all vehicles + - MOT_OPTIONS allows voltage compensation to use raw battery voltages (instead of current corrected voltage) + - PID controllers get DFF/D_FF (derivative feed-forward), NTF (target notch filter index) and NEF (error notch filter index) + - PID controllers get PDMX param to limit P+D output (useful for large vehicles and/or slow actuators) + - PID notch filter configured via new filter library using FILT parameters + - RTL mode uses RTL_CLIMB_MIN even if within cone slope (previously always climbed 2m) +6) Copter specific enhancements + - MOT_SPOOL_TIM_DN allows slower spool down of motors + - Precision landing gets fast descent option (see PLND_OPTIONS) + - Throw mode min and max altitude support (see THROW_ALT_MIN/MAX) + - TKOFF_TH_MAX allows lower throttle during takeoff + - ZigZag mode sends position target to GCS +7) TradHeli specific enhancements + - Arming checks improved + - Motor test removed (it was non-functional) + - OSD shows main rotor RPM + - RPM based dynamic notch can track below reference (e.g. below INS_HNTCH_FM_RAT) + - Thrust linearization for DDFP tails + - Heli_idle_control.lua closed loop throttle control Lua script +8) Parameters renamed + - COMPASS_TYPEMASK renamed to COMPASS_DISBLMSK +9) ROS2 / DDS support +10) Camera and gimbal enhancements + - Calculates location where camera gimbal is pointing (see CAMERA_FOV_STATUS) + - CAMx_MNT_INST allows specifying which mount camera is in + - Camera lens (e.g. live video stream) selectable using RC aux function + - Follow mode can point gimbal at lead vehicle + - Circle mode can point gimbal at circle center (see CIRCLE_OPTIONS) + - Interval timer (for taking pictures at timed intervals) + - Image tracking support (ViewPro only) + - MAVLink Gimbal Protocol v2 support for better GCS integration + - MNTx_SYSID_DFLT allows easier pointing gimbal at another vehicle + - MOUNT_CONTROL, MOUNT_CONFIGURE messages deprecated + - RangeFinder support (only logged, only supported on Siyi, Viewpro) + - Pilot's RC input re-takes manual control of gimbal (e.g. switches to RC_TARGETING mode) + - Siyi driver gets Zoom control, sends autopilot attitude and time (reduces leans) + - Video recording may start when armed (see CAMx_OPTIONS) + - ViewPro driver (replaces equivalent Lua driver) + - Xacti camera gimbal support + - Zoom percentage support (for both missions and GCS commands) +11) Logging and reporting changes + - Battery logging (e.g. BAT) includes health, temperature, state-of-health percentage + - CAM and MNT messages contain camera gimbal's desired and actual angles + - INS_RAW_LOG_OPT allows raw, pre-filter and post-filter sensor data logging (alternative to "batch logging", good for filtering analysis) + - PID logging gets reset and I-term-set flags + - Rangefinder logging (e.g. RFND) includes signal quality + - RC aux functions sorted alphabetically for GCS + - RC logging (RCI, RCI2) include valid input and failsafe flags + - RTK GPS logging includes number of fragments used or discarded +12) Scripting enhancements + - Autopilot reboot support + - Baro, Compass, IMU, IOMCU health check support + - Battery cell voltage bindings + - Battery driver support + - BattEsimate.lua applet estimates SoC from voltage + - Camera and Mount bindings improved + - CAN input packet filtering reduces work required by Lua CAN drivers + - DJI RS2/RS3 gimbal driver supports latest DJI firmware version (see mount-djirs2-driver.lua) + - EFI drivers for DLA serial, InnoFlight Inject EFI driver + - EFI bindings improved + - Fence support + - Generator drivers for Halo6000, Zhuhai SVFFI + - GCS failsafe support + - Hobbywing_DataLink driver (see Hobbywing_DataLink.lua) + - is_landing, is_taking_off bindings + - led_on_a_switch.lua sets LED brightness from RC switch + - MAVLink sending and receiving support + - Mission jump_to_landing_sequence binding + - mount-poi.lua upgraded to applet, sends better feedback, can lock onto Location + - Networking/Ethernet support + - Proximity driver support + - Rangefinder drivers can support signal quality + - revert_param.lua applet for quickly reverting params during tuning + - RockBlock.lua applet supports setting mode, fix for battery voltage reporting + - Serial/UART reading performance improvement using readstring binding + - sport_aerobatics.lua rudder control fixed + - Thread priority can be set using SCR_THD_PRIORITY (useful for Lua drivers) + - Wind alignment and head_wind speed bindings +13) Safety related enhancements and fixes + - Arm/Disarmed GPIO may be disabled using BRD_OPTIONS + - Arming check of compass vs world magnetic model to detect metal in ground (see ARMING_MAGTHRESH) + - Arming check of GPIO pin interrupt storm + - Arming check of Lua script CRC + - Arming check of mission loaded from SD card + - Arming check of Relay pin conflicts + - Arming check of emergency stop skipped if emergency stop aux function configured + - Arming failures reported more quickly when changing from success to failed + - ARMING_OPTIONS allows supressing "Armed", "Disarmed" text messages + - BRD_SAFETY_MASK extended to apply to CAN ESCs and servos + - Buzzer noise for gyro calibration and arming checks passed + - Dijkstras object avoidance supports "fast waypoints" (see AVOID_OPTIONS) + - FENCE_OPTIONS supports union OR intersection of all polygon fences + - FLTMODE_GCSBLOCK blocks GCS from changing vehicle to specified modes + - GCS failsafe action to switch to Brake mode (see FS_GCS_ENABLE) + - Main loop lockup recovery by forcing mutex release (only helps if caused by software bug) + - Rally points supports altitude frame (AMSL, Relative or Terrain) + - SERVO_RC_FS_MSK allows outputs using RC passthrough to move to SERVOx_TRIM on RC failsafe + - TKOFF_RPM_MAX aborts takeoff if RPM is too high (for cases where a propeller has come off) +14) System Enhancements + - CAN port can support a second CAN protocol on the same bus (2nd protocol must be 11 bit, see CAN_Dn_PROTOCOL2) + - CAN-FD support (allows faster data transfer rates) + - Crash dump info logged if main thread locksup (helps with root cause analysis) + - Ethernet/Networking support for UDP and TCP server and client (see NET_ENABLED) and PPP (see SERIALx_PROTOCOL) + - Firmware flashing from SD card + - Linux board SBUS input decoding made consistent with ChibiOS + - Linux boards support DroneCAN + - Parameter defaults stored in @ROMFS/defaults.parm + - SD Card formatting supported on all boards + - Second USB endpoint defaults to MAVLink (instead of SLCAN) except on CubePilot boards + - Serial over DroneCAN (see CAN_D1_UC_SER_EN) useful for configuring F9P DroneCAN GPSs using uCenter +15) Custom Build server include/exclude features extended to include + - APJ Tools + - Brake mode + - Bootloader flashing + - Button + - Compass calibration + - DroneCAN GPS + - ExternalAHRS (e.g. MicroStrain, Vectornav) + - Generator + - Highmark Servo + - Hobbywing ESCs + - Kill IMU + - Payload Place + - Precision landing + - Proximity sensor + - RC Protocol + - Relay + - SBUS Output + - ToneAlarm + - Winch +16) Developer specific items + - ChibiOS upgrade to 21.11 + - UAVCAN replaced with DroneCAN + - AUTOPILOT_VERSION_REQUEST message deprecated (use REQUEST_MESSAGE instead) + - PREFLIGHT_SET_SENSOR_OFFSETS support deprecated (was unused by all known ground stations) + - MISSION_SET_CURRENT message deprecated (use DO_SET_MISSION_CURRENT command instead) + - MISSION_CURRENT message sends num commands and stopped/paused/running/complete state + - Python version requirement increased to 3.6.9 + - mavlink_parse.py shows all suported mavlink messages + - COMMAND_INT messages can be used for nearly all commands (previously COMMAND_LONG) +17) Bug fixes: + - 3DR Solo gimbal mavlink routing fixed + - Airspeed health always checked before use (may not have been checked when using "affinity") + - Auto mode fix for DO_CHANGE_SPEED commands placed immediately after TAKEOFF (previously were ignored) + - Bootloop fixed if INS_GYRO_FILTER set too high + - Button Internal Error caused by floating pin or slow device power-up fixed + - CAN Compass order maintained even if compass powered up after autopilot + - Compass device IDs only saved when calibrated to ensure external compasses appear as primary on new boards + - Cut corners more by defaulting WPNAV_ACCEL_C to 2x WPNAV_ACCEL + - Currawong ECU EFI does not send exhaust gas temperature + - DJI RS2/RS3 gimbal reported angle fix + - DO_SET_ROI, ROI_LOCATION, ROI_NONE bug fix that could lead to gimbal pointing at old target + - Generator parameter init fix (defaults might not always have been loaded correctly) + - GPS_TC_BLEND parameter removed (it was unused) + - Guided mode protection against targets with NaN values + - Guided mode yaw fix (vehicle might rotate too slowly) + - Harmonic Notch gets protection against extremely low notch filter frequencies + - IE 650/800 Generators report fuel remaining + - INS calibration prevents unlikely case of two calibrations running at same time + - LPS2XH Baro supported over I2C fixed + - MatekH743 storage eeprom size fixed + - MAVLink routing fix to avoid processing packet meant for another vehicle + - Mount properly enforces user defined angle limits + - MPU6500 IMU filter corrected to 4k + - NMEA output time and altitude fixed + - OSD gets labels for all supported serial protocols + - OSD RF panel format fixed + - RobotisServo initialisation fix + - RPM accuracy and time wrap handling improved + - Sagetech ADSB MXS altitude fix (needs amsl, was sending alt-above-terrain) + - SageTechMXS ADSB climb rate direction fixed + - SBUS out exactly matches SBUS in decoding + - Serial port RTS pins default to pulldown (SiK radios could getting stuck in bootloader mode) + - SERIALx_ parameters removed for ports that can't actually be used + - Servo gimbal attitude reporting fix + - Servo output fix when using scaled RC passthrough (e.g. SERVOx_FUNCTION = RCinXScaled) + - Siyi continuous zoom stutter fixed + - Siyi gimbal upside-down mode fixed (avoid bobbing if upside-down) + - SmartRTL premature "buffer full" failure fixed + - ST24 RC protocol fixed + - STM32L496 CAN2 init fix (AP_Periph only?) + - Tricopter, SingleCopter, CoaxCopter fins centered if using BLHeli/DShot + - VFR_HUD climb rate reports best estimate during high vibration events (previously it would stop updating) + - Visual Odometry healthy check fix in case of out-of-memory + - VTX_MAX_POWER restored (allows setting radio's power) + - Yaw limit calculations fixed +------------------------------------------------------------------ +Copter 4.4.4 19-Dec-2023 / 4.4.4-beta1 05-Dec-2023 Changes from 4.4.3 1) Autopilot related enhancement and fixes - CubeOrange Sim-on-hardware compilation fix diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index 85fee2d7525225..bd541c355117af 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -88,10 +88,12 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O } } +#if HAL_LOGGING_ENABLED if (failsafe_state_change) { AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_ADSB, LogErrorCode(actual_action)); } +#endif // return with action taken return actual_action; @@ -102,8 +104,8 @@ void AP_Avoidance_Copter::handle_recovery(RecoveryAction recovery_action) // check we are coming out of failsafe if (copter.failsafe.adsb) { copter.failsafe.adsb = false; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_ADSB, - LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_ADSB, + LogErrorCode::ERROR_RESOLVED); // restore flight mode if requested and user has not changed mode since if (copter.control_mode_reason == ModeReason::AVOIDANCE) { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 329678e1b889ca..9b134c70e577d3 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -134,6 +134,10 @@ # define EKF_ORIGIN_MAX_ALT_KM 50 // EKF origin and home must be within 50km vertically #endif +#ifndef FS_EKF_FILT_DEFAULT +# define FS_EKF_FILT_DEFAULT 5.0f // frequency cutoff of EKF variance filters +#endif + ////////////////////////////////////////////////////////////////////////////// // Auto Tuning #ifndef AUTOTUNE_ENABLED @@ -239,7 +243,7 @@ ////////////////////////////////////////////////////////////////////////////// // System ID - conduct system identification tests on vehicle #ifndef MODE_SYSTEMID_ENABLED -# define MODE_SYSTEMID_ENABLED ENABLED +# define MODE_SYSTEMID_ENABLED HAL_LOGGING_ENABLED #endif ////////////////////////////////////////////////////////////////////////////// @@ -433,10 +437,6 @@ # define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL #endif -#ifndef RTL_ABS_MIN_CLIMB - # define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb -#endif - #ifndef RTL_CONE_SLOPE_DEFAULT # define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone #endif @@ -533,9 +533,6 @@ ////////////////////////////////////////////////////////////////////////////// // Logging control // -#ifndef LOGGING_ENABLED - # define LOGGING_ENABLED ENABLED -#endif // Default logging bitmask #ifndef DEFAULT_LOG_BITMASK @@ -624,10 +621,6 @@ #error Toy mode is not available on Helicopters #endif -#ifndef STATS_ENABLED - # define STATS_ENABLED ENABLED -#endif - #ifndef OSD_ENABLED #define OSD_ENABLED DISABLED #endif diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 786e56b5a0e522..2ce5d89420cc84 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -88,7 +88,7 @@ void Copter::crash_check() // check if crashing for 2 seconds if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) { - AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming: AngErr=%.0f>%.0f, Accel=%.1f<%.1f", angle_error, CRASH_CHECK_ANGLE_DEVIATION_DEG, filtered_acc, CRASH_CHECK_ACCEL_MAX); // disarm motors @@ -163,7 +163,7 @@ void Copter::thrust_loss_check() if (thrust_loss_counter >= (THRUST_LOSS_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) { // reset counter thrust_loss_counter = 0; - AP::logger().Write_Error(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::THRUST_LOSS_CHECK, LogErrorCode::FAILSAFE_OCCURRED); // send message to gcs gcs().send_text(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)motors->get_lost_motor() + 1); // enable thrust loss handling @@ -322,7 +322,7 @@ void Copter::parachute_check() } else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) { // reset control loss counter control_loss_count = 0; - AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_LOSS_OF_CONTROL); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_LOSS_OF_CONTROL); // release parachute parachute_release(); } @@ -357,7 +357,7 @@ void Copter::parachute_manual_release() if (ap.land_complete) { // warn user of reason for failure gcs().send_text(MAV_SEVERITY_INFO,"Parachute: Landed"); - AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_LANDED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_LANDED); return; } @@ -365,7 +365,7 @@ void Copter::parachute_manual_release() if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) { // warn user of reason for failure gcs().send_text(MAV_SEVERITY_ALERT,"Parachute: Too low"); - AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_TOO_LOW); + LOGGER_WRITE_ERROR(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_TOO_LOW); return; } diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 12a582d71c790a..47d9121be9b742 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -80,7 +80,7 @@ void Copter::ekf_check() // limit count from climbing too high ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.bad_variance = true; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); // send message to gcs if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); @@ -97,7 +97,7 @@ void Copter::ekf_check() // if variances are flagged as bad and the counter reaches zero then clear flag if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { ekf_check_state.bad_variance = false; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); } @@ -113,15 +113,27 @@ void Copter::ekf_check() // ekf_over_threshold - returns true if the ekf's variance are over the tolerance bool Copter::ekf_over_threshold() { - // return false immediately if disabled - if (g.fs_ekf_thresh <= 0.0f) { + // use EKF to get variance + float position_var, vel_var, height_var, tas_variance; + Vector3f mag_variance; + variances_valid = ahrs.get_variances(vel_var, position_var, height_var, mag_variance, tas_variance); + + if (!variances_valid) { return false; } - // use EKF to get variance - float position_variance, vel_variance, height_variance, tas_variance; - Vector3f mag_variance; - if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) { + uint32_t now_us = AP_HAL::micros(); + float dt = (now_us - last_ekf_check_us) / 1000000.0f; + + // always update filtered values as this serves the vibration check as well + position_var = pos_variance_filt.apply(position_var, dt); + vel_var = vel_variance_filt.apply(vel_var, dt); + height_var = hgt_variance_filt.apply(height_var, dt); + + last_ekf_check_us = now_us; + + // return false if disabled + if (g.fs_ekf_thresh <= 0.0f) { return false; } @@ -137,13 +149,13 @@ bool Copter::ekf_over_threshold() #if AP_OPTICALFLOW_ENABLED optflow_healthy = optflow.healthy(); #endif - if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) { + if (!optflow_healthy && (vel_var >= (2.0f * g.fs_ekf_thresh))) { over_thresh_count += 2; - } else if (vel_variance >= g.fs_ekf_thresh) { + } else if (vel_var >= g.fs_ekf_thresh) { over_thresh_count++; } - if ((position_variance >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) { + if ((position_var >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) { return true; } @@ -156,7 +168,7 @@ void Copter::failsafe_ekf_event() { // EKF failsafe event has occurred failsafe.ekf = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); // if disarmed take no action if (!motors->armed()) { @@ -207,7 +219,7 @@ void Copter::failsafe_ekf_off_event(void) AP_Notify::flags.failsafe_ekf = false; gcs().send_text(MAV_SEVERITY_CRITICAL, "EKF Failsafe Cleared"); } - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); } // re-check if the flight mode requires GPS but EKF failsafe is active @@ -232,14 +244,14 @@ void Copter::check_ekf_reset() if (new_ekfYawReset_ms != ekfYawReset_ms) { attitude_control->inertial_frame_reset(); ekfYawReset_ms = new_ekfYawReset_ms; - AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); + LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET); } // check for change in primary EKF, reset attitude target and log. AC_PosControl handles position target adjustment if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) { attitude_control->inertial_frame_reset(); ekf_primary_core = ahrs.get_primary_core_index(); - AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core); } } @@ -264,14 +276,12 @@ void Copter::check_vibration() const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z); // check if vertical velocity variance is at least 1 (NK4.SV >= 1.0) - float position_variance, vel_variance, height_variance, tas_variance; - Vector3f mag_variance; - if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) { + // filtered variances are updated in ekf_check() which runs at the same rate (10Hz) as this check + if (!variances_valid) { innovation_checks_valid = false; } - const bool is_vibration_affected = ahrs.is_vibration_affected(); - const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance > 1.0f)) || is_vibration_affected; + const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance_filt.get() > 1.0f)) || is_vibration_affected; const bool do_bad_vibe_actions = (g2.fs_vibe_enabled == 1) && bad_vibe_detected && motors->armed() && !flightmode->has_manual_throttle(); if (!vibration_check.high_vibes) { @@ -285,7 +295,7 @@ void Copter::check_vibration() vibration_check.clear_ms = 0; vibration_check.high_vibes = true; pos_control->set_vibe_comp(true); - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON"); } } else { @@ -300,7 +310,7 @@ void Copter::check_vibration() vibration_check.high_vibes = false; pos_control->set_vibe_comp(false); vibration_check.clear_ms = 0; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF"); } } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 94077c5088040c..2160e222498b41 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -12,7 +12,7 @@ bool Copter::failsafe_option(FailsafeOption opt) const void Copter::failsafe_radio_on_event() { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); // set desired action based on FS_THR_ENABLE parameter FailsafeAction desired_action; @@ -83,7 +83,7 @@ void Copter::failsafe_radio_off_event() { // no need to do anything except log the error as resolved // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared"); } @@ -98,7 +98,7 @@ void Copter::announce_failsafe(const char *type, const char *action_undertaken) void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); FailsafeAction desired_action = (FailsafeAction)action; @@ -162,7 +162,7 @@ void Copter::failsafe_gcs_check() // failsafe_gcs_on_event - actions to take when GCS contact is lost void Copter::failsafe_gcs_on_event(void) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); RC_Channels::clear_overrides(); // convert the desired failsafe response to the FailsafeAction enum @@ -236,7 +236,7 @@ void Copter::failsafe_gcs_on_event(void) void Copter::failsafe_gcs_off_event(void) { gcs().send_text(MAV_SEVERITY_WARNING, "GCS Failsafe Cleared"); - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); } // executes terrain failsafe if data is missing for longer than a few seconds @@ -251,7 +251,7 @@ void Copter::failsafe_terrain_check() if (trigger_event) { failsafe_terrain_on_event(); } else { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); failsafe.terrain = false; } } @@ -282,7 +282,7 @@ void Copter::failsafe_terrain_on_event() { failsafe.terrain = true; gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing"); - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); if (should_disarm_on_failsafe()) { arming.disarm(AP_Arming::Method::TERRAINFAILSAFE); @@ -306,10 +306,10 @@ void Copter::gpsglitch_check() if (ap.gps_glitching != gps_glitching) { ap.gps_glitching = gps_glitching; if (gps_glitching) { - AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch or Compass error"); } else { - AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL,"Glitch cleared"); } } @@ -361,7 +361,7 @@ void Copter::failsafe_deadreckon_check() if (failsafe.deadreckon && copter.flightmode->requires_GPS()) { // log error - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_DEADRECKON, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_DEADRECKON, LogErrorCode::FAILSAFE_OCCURRED); // immediately disarm while landed if (should_disarm_on_failsafe()) { diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 9adf95db03489b..88ae35572f7939 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -43,7 +43,7 @@ void Copter::failsafe_check() failsafe_last_timestamp = tnow; if (in_failsafe) { in_failsafe = false; - AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED); } return; } @@ -58,7 +58,7 @@ void Copter::failsafe_check() motors->output_min(); } - AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); } if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) { diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 122d49a96544a5..3bfe085c2b8697 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -79,11 +79,11 @@ void Copter::fence_check() } } - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches) { // record clearing of breach - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } } diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index fb7bb9babfcbf7..a9c9fde1ac7cfa 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -184,9 +184,9 @@ void Copter::heli_update_rotor_speed_targets() // when rotor_runup_complete changes to true, log event if (!rotor_runup_complete_last && motors->rotor_runup_complete()) { - AP::logger().Write_Event(LogEvent::ROTOR_RUNUP_COMPLETE); + LOGGER_WRITE_EVENT(LogEvent::ROTOR_RUNUP_COMPLETE); } else if (rotor_runup_complete_last && !motors->rotor_runup_complete() && !heli_flags.in_autorotation) { - AP::logger().Write_Event(LogEvent::ROTOR_SPEED_BELOW_CRITICAL); + LOGGER_WRITE_EVENT(LogEvent::ROTOR_SPEED_BELOW_CRITICAL); } rotor_runup_complete_last = motors->rotor_runup_complete(); } diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index c6b038c4a7c484..1c241f67b79f03 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#include // statistics library + // Code to detect a crash main ArduCopter code #define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing #define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing @@ -139,15 +141,17 @@ void Copter::set_land_complete(bool b) land_detector_count = 0; +#if HAL_LOGGING_ENABLED if(b){ AP::logger().Write_Event(LogEvent::LAND_COMPLETE); } else { AP::logger().Write_Event(LogEvent::NOT_LANDED); } +#endif ap.land_complete = b; -#if STATS_ENABLED == ENABLED - g2.stats.set_flying(!b); +#if AP_STATS_ENABLED + AP::stats()->set_flying(!b); #endif // tell AHRS flying state @@ -170,7 +174,7 @@ void Copter::set_land_complete_maybe(bool b) return; if (b) { - AP::logger().Write_Event(LogEvent::LAND_COMPLETE_MAYBE); + LOGGER_WRITE_EVENT(LogEvent::LAND_COMPLETE_MAYBE); } ap.land_complete_maybe = b; } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 3313ab35509bb9..f235cc22ef3906 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -191,7 +191,7 @@ Mode *Copter::mode_from_mode_num(const Mode::Number mode) void Copter::mode_change_failed(const Mode *mode, const char *reason) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to %s failed: %s", mode->name(), reason); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number())); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode->mode_number())); // make sad noise if (copter.ap.initialised) { AP_Notify::events.user_mode_change_failed = 1; @@ -360,7 +360,9 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) // update flight mode flightmode = new_flightmode; control_mode_reason = reason; +#if HAL_LOGGING_ENABLED logger.Write_Mode((uint8_t)flightmode->mode_number(), reason); +#endif gcs().send_message(MSG_HEARTBEAT); #if HAL_ADSB_ENABLED @@ -700,7 +702,7 @@ void Mode::land_run_horizontal_control() // process pilot inputs if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); @@ -720,7 +722,7 @@ void Mode::land_run_horizontal_control() // record if pilot has overridden roll or pitch if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { - AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); + LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE); } copter.ap.land_repo_active = true; #if AC_PRECLAND_ENABLED @@ -815,7 +817,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos) { if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); @@ -833,7 +835,7 @@ void Mode::precland_retry_position(const Vector3f &retry_pos) // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { if (!copter.ap.land_repo_active) { - AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); + LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE); } // this flag will be checked by prec land state machine later and any further landing retires will be cancelled copter.ap.land_repo_active = true; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index afb348c1dc6526..dd06ee0149c296 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -743,7 +743,9 @@ class AutoTune : public AC_AutoTune_Multi float get_pilot_desired_climb_rate_cms(void) const override; void get_pilot_desired_rp_yrate_cd(float &roll_cd, float &pitch_cd, float &yaw_rate_cds) override; void init_z_limits() override; +#if HAL_LOGGING_ENABLED void log_pids() override; +#endif }; class ModeAutoTune : public Mode { diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index c33c9d98d7ecba..6a2b336b82650c 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -167,7 +167,9 @@ void ModeAuto::run() if (auto_RTL && (!(mission.get_in_landing_sequence_flag() || mission.state() == AP_Mission::mission_state::MISSION_COMPLETE))) { auto_RTL = false; // log exit from Auto RTL +#if HAL_LOGGING_ENABLED copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), ModeReason::AUTO_RTL_EXIT); +#endif } } @@ -219,8 +221,10 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) // if not already in auto switch to auto if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) { auto_RTL = true; +#if HAL_LOGGING_ENABLED // log entry into AUTO RTL copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason); +#endif // make happy noise if (copter.ap.initialised) { @@ -236,7 +240,7 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found"); } - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL)); // make sad noise if (copter.ap.initialised) { AP_Notify::events.user_mode_change_failed = 1; @@ -338,7 +342,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) // get altitude target above EKF origin if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) { // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); // fall back to altitude above current altitude alt_target_cm = current_alt_cm + dest.alt; } @@ -618,10 +622,12 @@ bool ModeAuto::set_speed_down(float speed_down_cms) // start_command - this function will be called when the ap_mission lib wishes to start a new command bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) { +#if HAL_LOGGING_ENABLED // To-Do: logging when new commands start/end if (copter.should_log(MASK_LOG_CMD)) { copter.logger.Write_Mission_Cmd(mission, cmd); } +#endif switch(cmd.id) { @@ -1577,7 +1583,7 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) // this can only fail due to missing terrain database alt or rangefinder alt // use current alt-above-home and report error target_loc.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME); - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); gcs().send_text(MAV_SEVERITY_CRITICAL, "Land: no terrain data, using alt-above-home"); } @@ -1954,7 +1960,7 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) // this can only fail due to missing terrain database alt or rangefinder alt // use current alt-above-home and report error target_loc.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME); - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); gcs().send_text(MAV_SEVERITY_CRITICAL, "PayloadPlace: no terrain data, using alt-above-home"); } if (!wp_start(target_loc)) { @@ -2184,8 +2190,10 @@ bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) return false; } + const float turns = cmd.get_loiter_turns(); + // check if we have completed circling - return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= LOWBYTE(cmd.p1); + return fabsf(copter.circle_nav->get_angle_total()/float(M_2PI)) >= turns; } // verify_spline_wp - check if we have reached the next way point using spline diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 73699663a35926..0a7da76b07932f 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -98,12 +98,14 @@ void AutoTune::init_z_limits() copter.pos_control->set_correction_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z); } +#if HAL_LOGGING_ENABLED void AutoTune::log_pids() { copter.logger.Write_PID(LOG_PIDR_MSG, copter.attitude_control->get_rate_roll_pid().get_pid_info()); copter.logger.Write_PID(LOG_PIDP_MSG, copter.attitude_control->get_rate_pitch_pid().get_pid_info()); copter.logger.Write_PID(LOG_PIDY_MSG, copter.attitude_control->get_rate_yaw_pid().get_pid_info()); } +#endif /* check if we have a good position estimate diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index bade71e5e95264..938b44db7db10e 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -76,7 +76,7 @@ bool ModeFlip::init(bool ignore_checks) } // log start of flip - AP::logger().Write_Event(LogEvent::FLIP_START); + LOGGER_WRITE_EVENT(LogEvent::FLIP_START); // capture current attitude which will be used during the FlipState::Recovery stage const float angle_max = copter.aparm.angle_max; @@ -194,7 +194,7 @@ void ModeFlip::run() copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN); } // log successful completion - AP::logger().Write_Event(LogEvent::FLIP_END); + LOGGER_WRITE_EVENT(LogEvent::FLIP_END); } break; @@ -206,7 +206,7 @@ void ModeFlip::run() copter.set_mode(Mode::Number::STABILIZE, ModeReason::UNKNOWN); } // log abandoning flip - AP::logger().Write_Error(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIP, LogErrorCode::FLIP_ABANDONED); break; } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 91814ab45f3469..ddb55d3d6d6adf 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -202,6 +202,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) bf_angles.x = constrain_float(bf_angles.x, -copter.aparm.angle_max, copter.aparm.angle_max); bf_angles.y = constrain_float(bf_angles.y, -copter.aparm.angle_max, copter.aparm.angle_max); +#if HAL_LOGGING_ENABLED // @LoggerMessage: FHLD // @Description: FlowHold mode messages // @URL: https://ardupilot.org/copter/docs/flowhold-mode.html @@ -222,6 +223,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) (double)quality_filtered, (double)xy_I.x, (double)xy_I.y); } +#endif // HAL_LOGGING_ENABLED } // flowhold_run - runs the flowhold controller @@ -478,6 +480,7 @@ void ModeFlowHold::update_height_estimate(void) // new height estimate for logging height_estimate = ins_height + height_offset; +#if HAL_LOGGING_ENABLED // @LoggerMessage: FHXY // @Description: Height estimation using optical flow sensor // @Field: TimeUS: Time since system startup @@ -504,6 +507,8 @@ void ModeFlowHold::update_height_estimate(void) (double)ins_height, (double)last_ins_height, dt_ms); +#endif + gcs().send_named_float("HEST", height_estimate); delta_velocity_ne.zero(); last_ins_height = ins_height; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 9c2914de8b7bf4..ba9a52f60ed269 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -332,7 +332,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa // reject destination if outside the fence const Location dest_loc(destination, terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -351,8 +351,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa // no need to check return status because terrain data is not used wp_nav->set_wp_destination(destination, terrain_alt); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, destination, terrain_alt, Vector3f(), Vector3f()); +#endif send_notification = true; return true; } @@ -391,8 +393,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa guided_accel_target_cmss.zero(); update_time_ms = millis(); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); +#endif send_notification = true; @@ -424,7 +428,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // reject destination outside the fence. // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails if (!copter.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -438,7 +442,7 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y if (!wp_nav->set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } @@ -446,8 +450,11 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y // set yaw state set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f()); +#endif + send_notification = true; return true; } @@ -493,7 +500,9 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y update_time_ms = millis(); // log target +#if HAL_LOGGING_ENABLED copter.Log_Write_Guided_Position_Target(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); +#endif send_notification = true; @@ -518,10 +527,12 @@ void ModeGuided::set_accel(const Vector3f& acceleration, bool use_yaw, float yaw guided_accel_target_cmss = acceleration; update_time_ms = millis(); +#if HAL_LOGGING_ENABLED // log target if (log_request) { copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); } +#endif } // set_velocity - sets guided mode's target velocity @@ -548,10 +559,12 @@ void ModeGuided::set_velaccel(const Vector3f& velocity, const Vector3f& accelera guided_accel_target_cmss = acceleration; update_time_ms = millis(); +#if HAL_LOGGING_ENABLED // log target if (log_request) { copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); } +#endif } // set_destination_posvel - set guided mode position and velocity target @@ -567,7 +580,7 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); if (!copter.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -587,8 +600,10 @@ bool ModeGuided::set_destination_posvelaccel(const Vector3f& destination, const guided_vel_target_cms = velocity; guided_accel_target_cmss = acceleration; +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_cm.tofloat(), guided_pos_terrain_alt, guided_vel_target_cms, guided_accel_target_cmss); +#endif return true; } @@ -648,8 +663,10 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_ float roll_rad, pitch_rad, yaw_rad; attitude_quat.to_euler(roll_rad, pitch_rad, yaw_rad); +#if HAL_LOGGING_ENABLED // log target copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel, guided_angle_state.thrust, guided_angle_state.climb_rate_cms * 0.01); +#endif } // takeoff_run - takeoff in guided mode diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 377bef5ee9253b..1f34e20fbf3380 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -102,7 +102,7 @@ void ModeLand::nogps_run() // process pilot inputs if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 7c74838a3fe742..b8a6cd9bbe7af5 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -39,7 +39,9 @@ bool ModeRTL::init(bool ignore_checks) // re-start RTL with terrain following disabled void ModeRTL::restart_without_terrain() { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL); +#if HAL_LOGGING_ENABLED + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL); +#endif terrain_following_allowed = false; _state = SubMode::STARTING; _state_complete = true; @@ -134,7 +136,7 @@ void ModeRTL::climb_start() if (!wp_nav->set_wp_destination_loc(rtl_path.climb_target) || !wp_nav->set_wp_destination_next_loc(rtl_path.return_target)) { // this should not happen because rtl_build_path will have checked terrain data was available gcs().send_text(MAV_SEVERITY_CRITICAL,"RTL: unexpected error setting climb target"); - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); copter.set_mode(Mode::Number::LAND, ModeReason::TERRAIN_FAILSAFE); return; } @@ -275,7 +277,7 @@ void ModeRTL::descent_run() // process pilot's input if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ - AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT); + LOGGER_WRITE_EVENT(LogEvent::LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!copter.set_mode(Mode::Number::LOITER, ModeReason::THROTTLE_LAND_ESCAPE)) { copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::THROTTLE_LAND_ESCAPE); @@ -292,7 +294,7 @@ void ModeRTL::descent_run() // record if pilot has overridden roll or pitch if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { - AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); + LOGGER_WRITE_EVENT(LogEvent::LAND_REPO_ACTIVE); } copter.ap.land_repo_active = true; } @@ -426,7 +428,7 @@ void ModeRTL::compute_return_target() switch (wp_nav->get_terrain_source()) { case AC_WPNav::TerrainSource::TERRAIN_UNAVAILABLE: alt_type = ReturnTargetAltType::RELATIVE; - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home"); break; case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER: @@ -447,7 +449,7 @@ void ModeRTL::compute_return_target() // fallback to relative alt and warn user alt_type = ReturnTargetAltType::RELATIVE; gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: rangefinder unhealthy, using alt-above-home"); - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::RTL_MISSING_RNGFND); } } @@ -463,7 +465,7 @@ void ModeRTL::compute_return_target() } else { // fallback to relative alt and warn user alt_type = ReturnTargetAltType::RELATIVE; - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); gcs().send_text(MAV_SEVERITY_CRITICAL, "RTL: no terrain data, using alt-above-home"); } } @@ -483,14 +485,14 @@ void ModeRTL::compute_return_target() int32_t target_alt = MAX(rtl_path.return_target.alt, 0); // increase target to maximum of current altitude + climb_min and rtl altitude - target_alt = MAX(target_alt, curr_alt + MAX(0, g.rtl_climb_min)); - target_alt = MAX(target_alt, MAX(g.rtl_altitude, RTL_ALT_MIN)); + const float min_rtl_alt = MAX(RTL_ALT_MIN, curr_alt + MAX(0, g.rtl_climb_min)); + target_alt = MAX(target_alt, MAX(g.rtl_altitude, min_rtl_alt)); // reduce climb if close to return target float rtl_return_dist_cm = rtl_path.return_target.get_distance(rtl_path.origin_point) * 100.0f; // don't allow really shallow slopes if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { - target_alt = MAX(curr_alt, MIN(target_alt, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB))); + target_alt = MIN(target_alt, MAX(rtl_return_dist_cm * g.rtl_cone_slope, min_rtl_alt)); } // set returned target alt to new target_alt (don't change altitude type) diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index 15dc1eba4f0ede..801f903205deb2 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -99,7 +99,9 @@ bool ModeSystemId::init(bool ignore_checks) gcs().send_text(MAV_SEVERITY_INFO, "SystemID Starting: axis=%d", (unsigned)axis); +#if HAL_LOGGING_ENABLED copter.Log_Write_SysID_Setup(axis, waveform_magnitude, frequency_start, frequency_stop, time_fade_in, time_const_freq, time_record, time_fade_out); +#endif return true; } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index c9ff702e3fd887..884b7a22be88d5 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -200,6 +200,7 @@ void ModeThrow::run() break; } +#if HAL_LOGGING_ENABLED // log at 10hz or if stage changes uint32_t now = AP_HAL::millis(); if ((stage != prev_stage) || (now - last_log_ms) > 100) { @@ -213,7 +214,7 @@ void ModeThrow::run() const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good(); const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good(); const bool pos_ok = (stage > Throw_PosHold) || throw_position_good(); - + // @LoggerMessage: THRO // @Description: Throw Mode messages // @URL: https://ardupilot.org/copter/docs/throw-mode.html @@ -227,7 +228,7 @@ void ModeThrow::run() // @Field: AttOk: True if the vehicle is upright // @Field: HgtOk: True if the vehicle is within 50cm of the demanded height // @Field: PosOk: True if the vehicle is within 50cm of the demanded horizontal position - + AP::logger().WriteStreaming( "THRO", "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk", @@ -245,6 +246,7 @@ void ModeThrow::run() height_ok, pos_ok); } +#endif // HAL_LOGGING_ENABLED } bool ModeThrow::throw_detected() @@ -272,8 +274,21 @@ bool ModeThrow::throw_detected() // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS; - // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release - bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action; + // fetch the altitude above home + float altitude_above_home; // Use altitude above home if it is set, otherwise relative to EKF origin + if (ahrs.home_is_set()) { + ahrs.get_relative_position_D_home(altitude_above_home); + altitude_above_home = -altitude_above_home; // altitude above home is returned as negative + } else { + altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters + } + + // Check that the altitude is within user defined limits + const bool height_within_params = (g.throw_altitude_min == 0 || altitude_above_home > g.throw_altitude_min) && (g.throw_altitude_max == 0 || (altitude_above_home < g.throw_altitude_max)); + + // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release + bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action && height_within_params; + // Record time and vertical velocity when we detect the possible throw if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) { diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 2952158ec615a6..fbf43aa50ed458 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -171,12 +171,12 @@ void ModeZigZag::save_or_move_to_destination(Destination ab_dest) // store point A dest_A = curr_pos; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point A stored"); - AP::logger().Write_Event(LogEvent::ZIGZAG_STORE_A); + LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_A); } else { // store point B dest_B = curr_pos; gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: point B stored"); - AP::logger().Write_Event(LogEvent::ZIGZAG_STORE_B); + LOGGER_WRITE_EVENT(LogEvent::ZIGZAG_STORE_B); } // if both A and B have been stored advance state if (!dest_A.is_zero() && !dest_B.is_zero() && !is_zero((dest_B - dest_A).length_squared())) { diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 84ab92733927ff..34a3ff201b6ce3 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -165,10 +165,10 @@ void Copter::motors_output() bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop(); if (!motors->get_interlock() && interlock) { motors->set_interlock(true); - AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED); + LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_ENABLED); } else if (motors->get_interlock() && !interlock) { motors->set_interlock(false); - AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED); + LOGGER_WRITE_EVENT(LogEvent::MOTORS_INTERLOCK_DISABLED); } if (ap.motor_test) { diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 57043fba918396..430bed7e93836b 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -121,7 +121,7 @@ void Copter::read_radio() } // Log an error and enter failsafe. - AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); + LOGGER_WRITE_ERROR(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); set_failsafe_radio(true); } diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 4c740a2e3b3e1c..31b96628245b64 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -15,17 +15,6 @@ static void failsafe_check_static() void Copter::init_ardupilot() { - -#if STATS_ENABLED == ENABLED - // initialise stats module - g2.stats.init(); -#endif - - BoardConfig.init(); -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - can_mgr.init(); -#endif - // init cargo gripper #if AP_GRIPPER_ENABLED g2.gripper.init(); @@ -55,7 +44,7 @@ void Copter::init_ardupilot() osd.init(); #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED log_init(); #endif @@ -183,15 +172,13 @@ void Copter::init_ardupilot() g2.smart_rtl.init(); #endif +#if HAL_LOGGING_ENABLED // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); +#endif startup_INS_ground(); -#if AP_SCRIPTING_ENABLED - g2.scripting.init(); -#endif // AP_SCRIPTING_ENABLED - #if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED custom_control.init(); #endif @@ -207,12 +194,16 @@ void Copter::init_ardupilot() motors->output_min(); // output lowest possible value to motors - // attempt to set the intial_mode, else set to STABILIZE + // attempt to set the initial_mode, else set to STABILIZE if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { // set mode to STABILIZE will trigger mode change notification to pilot set_mode(Mode::Number::STABILIZE, ModeReason::UNAVAILABLE); } + pos_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); + vel_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); + hgt_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); + // flag that initialisation has completed ap.initialised = true; } @@ -352,18 +343,16 @@ void Copter::update_auto_armed() } } +#if HAL_LOGGING_ENABLED /* should we log a message type now? */ bool Copter::should_log(uint32_t mask) { -#if LOGGING_ENABLED == ENABLED ap.logging_started = logger.logging_started(); return logger.should_log(mask); -#else - return false; -#endif } +#endif /* allocate the motors class diff --git a/ArduCopter/terrain.cpp b/ArduCopter/terrain.cpp index 77765bb61b86b6..5933de460ee752 100644 --- a/ArduCopter/terrain.cpp +++ b/ArduCopter/terrain.cpp @@ -17,6 +17,7 @@ void Copter::terrain_update() #endif } +#if HAL_LOGGING_ENABLED // log terrain data - should be called at 1hz void Copter::terrain_logging() { @@ -26,3 +27,4 @@ void Copter::terrain_logging() } #endif } +#endif diff --git a/ArduCopter/toy_mode.cpp b/ArduCopter/toy_mode.cpp index d212cb43358817..c1cea4d4cc3328 100644 --- a/ArduCopter/toy_mode.cpp +++ b/ArduCopter/toy_mode.cpp @@ -991,6 +991,7 @@ void ToyMode::thrust_limiting(float *thrust, uint8_t num_motors) uint16_t pwm[4]; hal.rcout->read(pwm, 4); +#if HAL_LOGGING_ENABLED // @LoggerMessage: THST // @Description: Maximum thrust limitation based on battery voltage in Toy Mode // @Field: TimeUS: Time since system startup @@ -1008,7 +1009,7 @@ void ToyMode::thrust_limiting(float *thrust, uint8_t num_motors) (double)thrust_mul, pwm[0], pwm[1], pwm[2], pwm[3]); } - +#endif } #if ENABLE_LOAD_TEST diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index db7f15c8899a21..babc53b9ff2672 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -28,7 +28,9 @@ void Copter::tuning() const uint16_t radio_in = rc6->get_radio_in(); float tuning_value = linear_interpolate(g2.tuning_min, g2.tuning_max, radio_in, rc6->get_radio_min(), rc6->get_radio_max()); +#if HAL_LOGGING_ENABLED Log_Write_Parameter_Tuning(g.radio_tuning, tuning_value, g2.tuning_min, g2.tuning_max); +#endif switch(g.radio_tuning) { diff --git a/ArduCopter/version.h b/ArduCopter/version.h index aca789abdc19fa..6fb6ed396aa735 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduCopter V4.5.0-dev" +#define THISFIRMWARE "ArduCopter V4.6.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 6 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index b4e868b2ff288c..d7ecaa03fad0af 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -66,31 +66,33 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) // call parent class checks bool ret = AP_Arming::pre_arm_checks(display_failure); +#if AP_AIRSPEED_ENABLED // Check airspeed sensor ret &= AP_Arming::airspeed_checks(display_failure); +#endif if (plane.g.fs_timeout_long < plane.g.fs_timeout_short && plane.g.fs_action_short != FS_ACTION_SHORT_DISABLED) { check_failed(display_failure, "FS_LONG_TIMEOUT < FS_SHORT_TIMEOUT"); ret = false; } - if (plane.aparm.roll_limit_cd < 300) { - check_failed(display_failure, "LIM_ROLL_CD too small (%u)", (unsigned)plane.aparm.roll_limit_cd); + if (plane.aparm.roll_limit < 3) { + check_failed(display_failure, "ROLL_LIMIT_DEG too small (%.1f)", plane.aparm.roll_limit.get()); ret = false; } - if (plane.aparm.pitch_limit_max_cd < 300) { - check_failed(display_failure, "LIM_PITCH_MAX too small (%u)", (unsigned)plane.aparm.pitch_limit_max_cd); + if (plane.aparm.pitch_limit_max < 3) { + check_failed(display_failure, "PTCH_LIM_MAX_DEG too small (%.1f)", plane.aparm.pitch_limit_max.get()); ret = false; } - if (plane.aparm.pitch_limit_min_cd > -300) { - check_failed(display_failure, "LIM_PITCH_MIN too large (%u)", (unsigned)plane.aparm.pitch_limit_min_cd); + if (plane.aparm.pitch_limit_min > -3) { + check_failed(display_failure, "PTCH_LIM_MIN_DEG too large (%.1f)", plane.aparm.pitch_limit_min.get()); ret = false; } if (plane.aparm.airspeed_min < MIN_AIRSPEED_MIN) { - check_failed(display_failure, "ARSPD_FBW_MIN too low (%i < %i)", plane.aparm.airspeed_min.get(), MIN_AIRSPEED_MIN); + check_failed(display_failure, "AIRSPEED_MIN too low (%i < %i)", plane.aparm.airspeed_min.get(), MIN_AIRSPEED_MIN); ret = false; } @@ -292,7 +294,7 @@ void AP_Arming_Plane::change_arm_state(void) { update_soft_armed(); #if HAL_QUADPLANE_ENABLED - plane.quadplane.set_armed(is_armed_and_safety_off()); + plane.quadplane.set_armed(hal.util->get_soft_armed()); #endif } @@ -395,7 +397,9 @@ void AP_Arming_Plane::update_soft_armed() #endif hal.util->set_soft_armed(_armed); +#if HAL_LOGGING_ENABLED AP::logger().set_vehicle_armed(hal.util->get_soft_armed()); +#endif // update delay_arming oneshot if (delay_arming && @@ -450,7 +454,7 @@ bool AP_Arming_Plane::mission_checks(bool report) prev_cmd.id == MAV_CMD_NAV_WAYPOINT) { const float dist = cmd.content.location.get_distance(prev_cmd.content.location); const float tecs_land_speed = plane.TECS_controller.get_land_airspeed(); - const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise_cm*0.01; + const float landing_speed = is_positive(tecs_land_speed)?tecs_land_speed:plane.aparm.airspeed_cruise; const float min_dist = 0.75 * plane.quadplane.stopping_distance(sq(landing_speed)); if (dist < min_dist) { ret = false; diff --git a/ArduPlane/AP_ExternalControl_Plane.cpp b/ArduPlane/AP_ExternalControl_Plane.cpp new file mode 100644 index 00000000000000..c5afabf5843216 --- /dev/null +++ b/ArduPlane/AP_ExternalControl_Plane.cpp @@ -0,0 +1,22 @@ +/* + external control library for plane + */ + + +#include "AP_ExternalControl_Plane.h" +#if AP_EXTERNAL_CONTROL_ENABLED + +#include "Plane.h" + +/* + Sets the target global position for a loiter point. +*/ +bool AP_ExternalControl_Plane::set_global_position(const Location& loc) +{ + + // set_target_location already checks if plane is ready for external control. + // It doesn't check if flying or armed, just that it's in guided mode. + return plane.set_target_location(loc); +} + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduPlane/AP_ExternalControl_Plane.h b/ArduPlane/AP_ExternalControl_Plane.h new file mode 100644 index 00000000000000..71308780d55fdb --- /dev/null +++ b/ArduPlane/AP_ExternalControl_Plane.h @@ -0,0 +1,21 @@ + +/* + external control library for plane + */ +#pragma once + +#include + +#if AP_EXTERNAL_CONTROL_ENABLED + +#include + +class AP_ExternalControl_Plane : public AP_ExternalControl { +public: + /* + Sets the target global position for a loiter point. + */ + bool set_global_position(const Location& loc) override WARN_IF_UNUSED; +}; + +#endif // AP_EXTERNAL_CONTROL_ENABLED diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 36bc4f5b53e091..bab97f46e259c6 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -104,11 +104,15 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { #if AP_CAMERA_ENABLED SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108), #endif // CAMERA == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111), +#endif SCHED_TASK(compass_save, 0.1, 200, 114), +#if HAL_LOGGING_ENABLED SCHED_TASK(Log_Write_FullRate, 400, 300, 117), SCHED_TASK(update_logging10, 10, 300, 120), SCHED_TASK(update_logging25, 25, 300, 123), +#endif #if HAL_SOARING_ENABLED SCHED_TASK(update_soaring, 50, 400, 126), #endif @@ -117,7 +121,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132), #endif // AP_TERRAIN_AVAILABLE SCHED_TASK(update_is_flying_5Hz, 5, 100, 135), -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400, 138), #endif SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141), @@ -128,9 +132,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { #if HAL_BUTTON_ENABLED SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150), #endif -#if STATS_ENABLED == ENABLED - SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100, 153), -#endif #if AP_GRIPPER_ENABLED SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75, 156), #endif @@ -161,13 +162,15 @@ void Plane::ahrs_update() ahrs.update(); +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_IMU)) { AP::ins().Write_IMU(); } +#endif // calculate a scaled roll limit based on current pitch - roll_limit_cd = aparm.roll_limit_cd; - pitch_limit_min_cd = aparm.pitch_limit_min_cd; + roll_limit_cd = aparm.roll_limit*100; + pitch_limit_min = aparm.pitch_limit_min; bool rotate_limits = true; #if HAL_QUADPLANE_ENABLED @@ -177,7 +180,7 @@ void Plane::ahrs_update() #endif if (rotate_limits) { roll_limit_cd *= ahrs.cos_pitch(); - pitch_limit_min_cd *= fabsf(ahrs.cos_roll()); + pitch_limit_min *= fabsf(ahrs.cos_roll()); } // updated the summed gyro used for ground steering and @@ -194,9 +197,11 @@ void Plane::ahrs_update() quadplane.inertial_nav.update(); #endif +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_VIDEO_STABILISATION)) { ahrs.write_video_stabilisation(); } +#endif } /* @@ -234,6 +239,7 @@ void Plane::update_compass(void) compass.read(); } +#if HAL_LOGGING_ENABLED /* do 10Hz logging */ @@ -286,7 +292,7 @@ void Plane::update_logging25(void) if (should_log(MASK_LOG_IMU)) AP::ins().Write_Vibration(); } - +#endif // HAL_LOGGING_ENABLED /* check for AFS failsafe check @@ -333,7 +339,7 @@ void Plane::one_second_loop() AP_Notify::flags.pre_arm_gps_check = true; AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO; -#if AP_TERRAIN_AVAILABLE +#if AP_TERRAIN_AVAILABLE && HAL_LOGGING_ENABLED if (should_log(MASK_LOG_GPS)) { terrain.log_terrain_data(); } @@ -409,8 +415,8 @@ void Plane::airspeed_ratio_update(void) return; } if (labs(ahrs.roll_sensor) > roll_limit_cd || - ahrs.pitch_sensor > aparm.pitch_limit_max_cd || - ahrs.pitch_sensor < pitch_limit_min_cd) { + ahrs.pitch_sensor > aparm.pitch_limit_max*100 || + ahrs.pitch_sensor < pitch_limit_min*100) { // don't calibrate when going beyond normal flight envelope return; } @@ -528,7 +534,9 @@ void Plane::set_flight_stage(AP_FixedWing::FlightStage fs) } flight_stage = fs; +#if HAL_LOGGING_ENABLED Log_Write_Status(); +#endif } void Plane::update_alt() @@ -798,8 +806,8 @@ bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const return true; } -#if AP_SCRIPTING_ENABLED -// set target location (for use by scripting) +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +// set target location (for use by external control and scripting) bool Plane::set_target_location(const Location &target_loc) { Location loc{target_loc}; @@ -816,7 +824,9 @@ bool Plane::set_target_location(const Location &target_loc) plane.set_guided_WP(loc); return true; } +#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +#if AP_SCRIPTING_ENABLED // set target location (for use by scripting) bool Plane::get_target_location(Location& target_loc) { @@ -904,7 +914,7 @@ bool Plane::is_taking_off() const return control_mode->is_taking_off(); } -// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL +// correct AHRS pitch for PTCH_TRIM_DEG in non-VTOL modes, and return VTOL view in VTOL void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const { #if HAL_QUADPLANE_ENABLED @@ -914,10 +924,10 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const return; } #endif - pitch = ahrs.pitch; - roll = ahrs.roll; - if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH_CD))) { // correct for TRIM_PITCH_CD - pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD; + pitch = ahrs.get_pitch(); + roll = ahrs.get_roll(); + if (!(flight_option_enabled(FlightOptions::OSD_REMOVE_TRIM_PITCH))) { // correct for PTCH_TRIM_DEG + pitch -= g.pitch_trim * DEG_TO_RAD; } } diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 3c59d8a31522bc..b566d4ca730bf9 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -202,14 +202,14 @@ float Plane::stabilize_pitch_get_pitch_out() return pitch_out; } #endif - // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set + // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_DEG if flight option FORCE_FLARE_ATTITUDE is set #if HAL_QUADPLANE_ENABLED const bool quadplane_in_transition = quadplane.in_transition(); #else const bool quadplane_in_transition = false; #endif - int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; + int32_t demanded_pitch = nav_pitch_cd + int32_t(g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch; bool disable_integrator = false; if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) { disable_integrator = true; @@ -310,11 +310,11 @@ void Plane::stabilize_stick_mixing_fbw() pitch_input = -pitch_input; } if (pitch_input > 0) { - nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd; + nav_pitch_cd += pitch_input * aparm.pitch_limit_max*100; } else { - nav_pitch_cd += -(pitch_input * pitch_limit_min_cd); + nav_pitch_cd += -(pitch_input * pitch_limit_min*100); } - nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); + nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min*100, aparm.pitch_limit_max.get()*100); } @@ -585,7 +585,7 @@ int16_t Plane::calc_nav_yaw_ground(void) void Plane::calc_nav_pitch() { int32_t commanded_pitch = TECS_controller.get_pitch_demand(); - nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); + nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min*100, aparm.pitch_limit_max.get()*100); } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 9427b99e0cdac9..c8ee1e7a20c379 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -129,12 +129,12 @@ void GCS_MAVLINK_Plane::send_attitude() const { const AP_AHRS &ahrs = AP::ahrs(); - float r = ahrs.roll; - float p = ahrs.pitch; - float y = ahrs.yaw; + float r = ahrs.get_roll(); + float p = ahrs.get_pitch(); + float y = ahrs.get_yaw(); - if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH_CD))) { - p -= radians(plane.g.pitch_trim_cd * 0.01f); + if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH))) { + p -= radians(plane.g.pitch_trim); } #if HAL_QUADPLANE_ENABLED @@ -1095,6 +1095,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_comma } #if HAL_QUADPLANE_ENABLED +#if AP_MAVLINK_COMMAND_LONG_ENABLED void GCS_MAVLINK_Plane::convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out) { // convert to MAV_FRAME_LOCAL_OFFSET_NED, "NED local tangent frame @@ -1115,7 +1116,6 @@ void GCS_MAVLINK_Plane::convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink out.z = -in.param7; // up -> down } -#if AP_MAVLINK_COMMAND_LONG_ENABLED void GCS_MAVLINK_Plane::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame) { switch (in.command) { @@ -1223,17 +1223,10 @@ void GCS_MAVLINK_Plane::handle_manual_control_axes(const mavlink_manual_control_ manual_override(plane.channel_rudder, packet.r, 1000, 2000, tnow); } -void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) +void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_RADIO: - case MAVLINK_MSG_ID_RADIO_STATUS: - { - handle_radio_status(msg, plane.should_log(MASK_LOG_PM)); - break; - } - case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: #if AP_TERRAIN_AVAILABLE @@ -1242,6 +1235,24 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) break; case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_set_attitude_target(msg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_set_position_target_local_ned(msg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: + handle_set_position_target_global_int(msg); + break; + + default: + GCS_MAVLINK::handle_message(msg); + break; + } // end switch +} // end handle mavlink + +void GCS_MAVLINK_Plane::handle_set_attitude_target(const mavlink_message_t &msg) { // Only allow companion computer (or other external controller) to // control attitude in GUIDED mode. We DON'T want external control @@ -1249,7 +1260,7 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) // computer control is more safe (even more so when using // FENCE_ACTION = 4 for geofence failures). if (plane.control_mode != &plane.mode_guided) { // don't screw up failsafes - break; + return; } mavlink_set_attitude_target_t att_target; @@ -1307,11 +1318,9 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) // Update timer for external throttle plane.guided_state.last_forced_throttle_ms = now; } - - break; } - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: +void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_message_t &msg) { // decode packet mavlink_set_position_target_local_ned_t packet; @@ -1319,23 +1328,21 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) // exit if vehicle is not in Guided mode if (plane.control_mode != &plane.mode_guided) { - break; + return; } // only local moves for now if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) { - break; + return; } // just do altitude for now plane.next_WP_loc.alt += -packet.z*100.0; gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f", (double)((plane.next_WP_loc.alt - plane.home.alt)*0.01)); - - break; } - case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: +void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_message_t &msg) { // Only want to allow companion computer position control when // in a certain mode to avoid inadvertently sending these @@ -1345,7 +1352,7 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) // one uses the FENCE_ACTION = 4 (RTL) for geofence failures). if (plane.control_mode != &plane.mode_guided) { //don't screw up failsafes - break; + return; } mavlink_set_position_target_global_int_t pos_target; @@ -1385,16 +1392,8 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) handle_change_alt_request(cmd); } } // end if alt_mask - - break; } - default: - handle_common_message(msg); - break; - } // end switch -} // end handle mavlink - MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet) { const MAV_RESULT result = GCS_MAVLINK::handle_command_do_set_mission_current(packet); diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 41cb8e6b123f5f..5920a354028bf0 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -4,6 +4,7 @@ #include #include #include "quadplane.h" +#include "defines.h" class GCS_MAVLINK_Plane : public GCS_MAVLINK { @@ -18,6 +19,10 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK uint32_t telem_delay() const override; +#if HAL_LOGGING_ENABLED + uint32_t log_radio_bit() const override { return MASK_LOG_PM; } +#endif + #if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED void handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg) override; #endif @@ -48,9 +53,9 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK private: - void send_pid_info(const AP_PIDInfo *pid_info, const uint8_t axis, const float achieved); + void send_pid_info(const struct AP_PIDInfo *pid_info, const uint8_t axis, const float achieved); - void handleMessage(const mavlink_message_t &msg) override; + void handle_message(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); @@ -61,6 +66,10 @@ class GCS_MAVLINK_Plane : public GCS_MAVLINK MAV_RESULT handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet); MAV_RESULT handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet); + void handle_set_position_target_global_int(const mavlink_message_t &msg); + void handle_set_position_target_local_ned(const mavlink_message_t &msg); + void handle_set_attitude_target(const mavlink_message_t &msg); + #if HAL_QUADPLANE_ENABLED #if AP_MAVLINK_COMMAND_LONG_ENABLED void convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out); diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 55d5ab4c46b495..2df80fbbc1415c 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -1,6 +1,6 @@ #include "Plane.h" -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED // Write an attitude packet void Plane::Log_Write_Attitude(void) @@ -33,6 +33,9 @@ void Plane::Log_Write_Attitude(void) logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); logger.Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); logger.Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() ); + + // Write tailsitter specific log at same rate as PIDs + quadplane.tailsitter.write_log(); } if (quadplane.in_vtol_mode() && quadplane.pos_control->is_active_xy()) { logger.Write_PID(LOG_PIDN_MSG, quadplane.pos_control->get_vel_xy_pid().get_pid_info_x()); @@ -383,16 +386,21 @@ const struct LogStructure Plane::log_structure[] = { // @Field: DCRt: desired climb rate // @Field: CRt: climb rate // @Field: TMix: transition throttle mix value -// @Field: Sscl: speed scalar for tailsitter control surfaces // @Field: Trn: Transition state: 0-AirspeedWait,1-Timer,2-Done / TailSitter: 0-FW Wait,1-VTOL Wait,2-Done // @Field: Ast: Q assist active #if HAL_QUADPLANE_ENABLED { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning), - "QTUN", "QffffffeccffBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Sscl,Trn,Ast", "s----mmmnn----", "F----00000-0--" , true }, + "QTUN", "QffffffeccfBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Trn,Ast", "s----mmmnn---", "F----00000---" , true }, #endif -// @LoggerMessage: PIQR,PIQP,PIQY,PIQA -// @Description: QuadPlane Proportional/Integral/Derivative gain values for Roll/Pitch/Yaw/Z +// @LoggerMessage: PIQR +// @Description: QuadPlane Proportional/Integral/Derivative gain values for Roll rate +// @LoggerMessage: PIQP +// @Description: QuadPlane Proportional/Integral/Derivative gain values for Pitch rate +// @LoggerMessage: PIQY +// @Description: QuadPlane Proportional/Integral/Derivative gain values for Yaw rate +// @LoggerMessage: PIQA +// @Description: QuadPlane Proportional/Integral/Derivative gain values for vertical acceleration // @Field: TimeUS: Time since system startup // @Field: Tar: desired value // @Field: Act: achieved value @@ -406,6 +414,7 @@ const struct LogStructure Plane::log_structure[] = { // @Field: SRate: slew rate // @Field: Flags: bitmask of PID state flags // @FieldBitmaskEnum: Flags: log_PID_Flags +#if HAL_QUADPLANE_ENABLED { LOG_PIQR_MSG, sizeof(log_PID), "PIQR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, { LOG_PIQP_MSG, sizeof(log_PID), @@ -414,6 +423,18 @@ const struct LogStructure Plane::log_structure[] = { "PIQY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, { LOG_PIQA_MSG, sizeof(log_PID), "PIQA", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS , true }, +#endif + +// @LoggerMessage: TSIT +// @Description: tailsitter speed scailing values +// @Field: TimeUS: Time since system startup +// @Field: Ts: throttle scailing used for tilt motors +// @Field: Ss: speed scailing used for control surfaces method from Q_TAILSIT_GSCMSK +// @Field: Tmin: minimum output throttle caculated from disk thoery gain scale with Q_TAILSIT_MIN_VO +#if HAL_QUADPLANE_ENABLED + { LOG_TSIT_MSG, sizeof(Tailsitter::log_tailsitter), + "TSIT", "Qfff", "TimeUS,Ts,Ss,Tmin", "s---", "F---" , true }, +#endif // @LoggerMessage: PIDG // @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control. @@ -485,18 +506,4 @@ void Plane::log_init(void) logger.Init(log_structure, ARRAY_SIZE(log_structure)); } -#else // LOGGING_ENABLED - -void Plane::Log_Write_Attitude(void) {} -void Plane::Log_Write_Fast(void) {} -void Plane::Log_Write_Control_Tuning() {} -void Plane::Log_Write_OFG_Guided() {} -void Plane::Log_Write_Nav_Tuning() {} -void Plane::Log_Write_Status() {} -void Plane::Log_Write_Guided(void) {} -void Plane::Log_Write_RC(void) {} -void Plane::Log_Write_Vehicle_Startup_Messages() {} - -void Plane::log_init(void) {} - -#endif // LOGGING_ENABLED +#endif // HAL_LOGGING_ENABLED diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index fbecd0ee40b6c4..639aa73039e6a2 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -41,9 +41,9 @@ const AP_Param::Info Plane::var_info[] = { // @Param: AUTOTUNE_OPTIONS // @DisplayName: Autotune options bitmask - // @Description: Autotune specific options - // @Bitmask: 0: Disable FLTD update - // @Bitmask: 1: Disable FLTT update + // @Description: Fixed Wing Autotune specific options. Useful on QuadPlanes with higher INS_GYRO_FILTER settings to prevent these filter values from being set too agressively during Fixed Wing Autotune. + // @Bitmask: 0: Disable FLTD update by Autotune + // @Bitmask: 1: Disable FLTT update by Autotune // @User: Advanced ASCALAR(autotune_options, "AUTOTUNE_OPTIONS", 0), @@ -267,28 +267,35 @@ const AP_Param::Info Plane::var_info[] = { // @Param: STALL_PREVENTION // @DisplayName: Enable stall prevention - // @Description: Enables roll limits at low airspeed in roll limiting flight modes. Roll limits based on aerodynamic load factor in turns and scale on ARSPD_FBW_MIN that must be set correctly. Without airspeed sensor, uses synthetic airspeed from wind speed estimate that may both be inaccurate. + // @Description: Enables roll limits at low airspeed in roll limiting flight modes. Roll limits based on aerodynamic load factor in turns and scale on AIRSPEED_MIN that must be set correctly. Without airspeed sensor, uses synthetic airspeed from wind speed estimate that may both be inaccurate. // @Values: 0:Disabled,1:Enabled // @User: Standard ASCALAR(stall_prevention, "STALL_PREVENTION", 1), - // @Param: ARSPD_FBW_MIN + // @Param: AIRSPEED_CRUISE + // @DisplayName: Target cruise airspeed + // @Description: Target cruise airspeed in m/s in automatic throttle modes. Value is as an indicated (calibrated/apparent) airspeed. + // @Units: m/s + // @User: Standard + ASCALAR(airspeed_cruise, "AIRSPEED_CRUISE", AIRSPEED_CRUISE), + + // @Param: AIRSPEED_MIN // @DisplayName: Minimum Airspeed // @Description: Minimum airspeed demanded in automatic throttle modes. Should be set to 20% higher than level flight stall speed. // @Units: m/s // @Range: 5 100 // @Increment: 1 // @User: Standard - ASCALAR(airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN), + ASCALAR(airspeed_min, "AIRSPEED_MIN", AIRSPEED_FBW_MIN), - // @Param: ARSPD_FBW_MAX + // @Param: AIRSPEED_MAX // @DisplayName: Maximum Airspeed - // @Description: Maximum airspeed demanded in automatic throttle modes. Should be set slightly less than level flight speed at THR_MAX and also at least 50% above ARSPD_FBW_MIN to allow for accurate TECS altitude control. + // @Description: Maximum airspeed demanded in automatic throttle modes. Should be set slightly less than level flight speed at THR_MAX and also at least 50% above AIRSPEED_MIN to allow for accurate TECS altitude control. // @Units: m/s // @Range: 5 100 // @Increment: 1 // @User: Standard - ASCALAR(airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX), + ASCALAR(airspeed_max, "AIRSPEED_MAX", AIRSPEED_FBW_MAX), // @Param: FBWB_ELEV_REV // @DisplayName: Fly By Wire elevator reverse @@ -401,7 +408,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: TRIM_THROTTLE // @DisplayName: Throttle cruise percentage - // @Description: Target percentage of throttle to apply for flight in automatic throttle modes and throttle percentage that maintains TRIM_ARSPD_CM. Caution: low battery voltages at the end of flights may require higher throttle to maintain airspeed. + // @Description: Target percentage of throttle to apply for flight in automatic throttle modes and throttle percentage that maintains AIRSPEED_CRUISE. Caution: low battery voltages at the end of flights may require higher throttle to maintain airspeed. // @Units: % // @Range: 0 100 // @Increment: 1 @@ -410,14 +417,14 @@ const AP_Param::Info Plane::var_info[] = { // @Param: THROTTLE_NUDGE // @DisplayName: Throttle nudge enable - // @Description: When enabled, this uses the throttle input in auto-throttle modes to 'nudge' the throttle or airspeed to higher or lower values. When you have an airspeed sensor the nudge affects the target airspeed, so that throttle inputs above 50% will increase the target airspeed from TRIM_ARSPD_CM up to a maximum of ARSPD_FBW_MAX. When no airspeed sensor is enabled the throttle nudge will push up the target throttle for throttle inputs above 50%. + // @Description: When enabled, this uses the throttle input in auto-throttle modes to 'nudge' the throttle or airspeed to higher or lower values. When you have an airspeed sensor the nudge affects the target airspeed, so that throttle inputs above 50% will increase the target airspeed from AIRSPEED_CRUISE up to a maximum of AIRSPEED_MAX. When no airspeed sensor is enabled the throttle nudge will push up the target throttle for throttle inputs above 50%. // @Values: 0:Disabled,1:Enabled // @User: Standard GSCALAR(throttle_nudge, "THROTTLE_NUDGE", 1), // @Param: FS_SHORT_ACTN // @DisplayName: Short failsafe action - // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event. A short failsafe event can be triggered either by loss of RC control (see THR_FS_VALUE) or by loss of GCS control (see FS_GCS_ENABL). If in CIRCLE or RTL mode this parameter is ignored. A short failsafe event in stabilization and manual modes will cause a change to CIRCLE mode if FS_SHORT_ACTN is 0 or 1, a change to FBWA mode with zero throttle if FS_SHORT_ACTN is 2, and a change to FBWB mode if FS_SHORT_ACTN is 4. In all other modes (AUTO, GUIDED and LOITER) a short failsafe event will cause no mode change if FS_SHORT_ACTN is set to 0, will cause a change to CIRCLE mode if set to 1, will change to FBWA mode with zero throttle if set to 2, or will change to FBWB if set to 4. Please see the documentation for FS_LONG_ACTN for the behaviour after FS_LONG_TIMEOUT seconds of failsafe. + // @Description: The action to take on a short (FS_SHORT_TIMEOUT) failsafe event. A short failsafe event can be triggered either by loss of RC control (see THR_FS_VALUE) or by loss of GCS control (see FS_GCS_ENABL). If in CIRCLE or RTL mode this parameter is ignored. A short failsafe event in stabilization and manual modes will cause a change to CIRCLE mode if FS_SHORT_ACTN is 0 or 1, a change to FBWA mode with zero throttle if FS_SHORT_ACTN is 2, and a change to FBWB mode if FS_SHORT_ACTN is 4. In all other modes (AUTO, GUIDED and LOITER) a short failsafe event will cause no mode change if FS_SHORT_ACTN is set to 0, will cause a change to CIRCLE mode if set to 1, will change to FBWA mode with zero throttle if set to 2, or will change to FBWB if set to 4. Please see the documentation for FS_LONG_ACTN for the behaviour after FS_LONG_TIMEOUT seconds of failsafe. This parameter only applies to failsafes during fixed wing modes. Quadplane modes will switch to QLAND unless Q_OPTIONS bit 5(QRTL) or 20(RTL) are set. // @Values: 0:CIRCLE/no change(if already in AUTO|GUIDED|LOITER),1:CIRCLE,2:FBWA at zero throttle,3:Disable,4:FBWB // @User: Standard GSCALAR(fs_action_short, "FS_SHORT_ACTN", FS_ACTION_SHORT_BESTGUESS), @@ -433,7 +440,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: FS_LONG_ACTN // @DisplayName: Long failsafe action - // @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2. If FS_LONG_ACTN is set to 3, the parachute will be deployed (make sure the chute is configured and enabled). If FS_LONG_ACTN is set to 4 the aircraft will switch to mode AUTO with the current waypoint if it is not already in mode AUTO, unless it is in the middle of a landing sequence. + // @Description: The action to take on a long (FS_LONG_TIMEOUT seconds) failsafe event. If the aircraft was in a stabilization or manual mode when failsafe started and a long failsafe occurs then it will change to RTL mode if FS_LONG_ACTN is 0 or 1, and will change to FBWA if FS_LONG_ACTN is set to 2. If the aircraft was in an auto mode (such as AUTO or GUIDED) when the failsafe started then it will continue in the auto mode if FS_LONG_ACTN is set to 0, will change to RTL mode if FS_LONG_ACTN is set to 1 and will change to FBWA mode if FS_LONG_ACTN is set to 2. If FS_LONG_ACTN is set to 3, the parachute will be deployed (make sure the chute is configured and enabled). If FS_LONG_ACTN is set to 4 the aircraft will switch to mode AUTO with the current waypoint if it is not already in mode AUTO, unless it is in the middle of a landing sequence. This parameter only applies to failsafes during fixed wing modes. Quadplane modes will switch to QLAND unless Q_OPTIONS bit 5 (QRTL) or 20(RTL) are set. // @Values: 0:Continue,1:ReturnToLaunch,2:Glide,3:Deploy Parachute,4:Auto // @User: Standard GSCALAR(fs_action_long, "FS_LONG_ACTN", FS_ACTION_LONG_CONTINUE), @@ -505,32 +512,32 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), - // @Param: LIM_ROLL_CD + // @Param: ROLL_LIMIT_DEG // @DisplayName: Maximum Bank Angle // @Description: Maximum bank angle commanded in modes with stabilized limits. Increase this value for sharper turns, but decrease to prevent accelerated stalls. - // @Units: cdeg - // @Range: 0 9000 - // @Increment: 10 + // @Units: deg + // @Range: 0 90 + // @Increment: 0.1 // @User: Standard - ASCALAR(roll_limit_cd, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE), + ASCALAR(roll_limit, "ROLL_LIMIT_DEG", ROLL_LIMIT_DEG), - // @Param: LIM_PITCH_MAX + // @Param: PTCH_LIM_MAX_DEG // @DisplayName: Maximum Pitch Angle // @Description: Maximum pitch up angle commanded in modes with stabilized limits. - // @Units: cdeg - // @Range: 0 9000 + // @Units: deg + // @Range: 0 90 // @Increment: 10 // @User: Standard - ASCALAR(pitch_limit_max_cd, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE), + ASCALAR(pitch_limit_max, "PTCH_LIM_MAX_DEG", PITCH_MAX), - // @Param: LIM_PITCH_MIN + // @Param: PTCH_LIM_MIN_DEG // @DisplayName: Minimum Pitch Angle // @Description: Maximum pitch down angle commanded in modes with stabilized limits // @Units: cdeg - // @Range: -9000 0 + // @Range: -90 0 // @Increment: 10 // @User: Standard - ASCALAR(pitch_limit_min_cd, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE), + ASCALAR(pitch_limit_min, "PTCH_LIM_MIN_DEG", PITCH_MIN), // @Param: ACRO_ROLL_RATE // @DisplayName: ACRO mode roll rate @@ -621,13 +628,6 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), - // @Param: TRIM_ARSPD_CM - // @DisplayName: Target airspeed - // @Description: Target airspeed in cm/s in automatic throttle modes. Value is as an indicated (calibrated/apparent) airspeed. - // @Units: cm/s - // @User: Standard - ASCALAR(airspeed_cruise_cm, "TRIM_ARSPD_CM", AIRSPEED_CRUISE_CM), - // @Param: SCALING_SPEED // @DisplayName: speed used for speed scaling calculations // @Description: Airspeed in m/s to use when calculating surface speed scaling. Note that changing this value will affect all PID values @@ -637,35 +637,34 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced GSCALAR(scaling_speed, "SCALING_SPEED", SCALING_SPEED), - // @Param: MIN_GNDSPD_CM + // @Param: MIN_GROUNDSPEED // @DisplayName: Minimum ground speed // @Description: Minimum ground speed in cm/s when under airspeed control - // @Units: cm/s + // @Units: m/s // @User: Advanced - ASCALAR(min_gndspeed_cm, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM), + ASCALAR(min_groundspeed, "MIN_GROUNDSPEED", MIN_GROUNDSPEED), - // @Param: TRIM_PITCH_CD + // @Param: PTCH_TRIM_DEG // @DisplayName: Pitch angle offset - // @Description: Offset applied to AHRS pitch used for in-flight pitch trimming. Correct ground leveling is better than changing this parameter. - // @Units: cdeg - // @Range: -4500 4500 - // @Increment: 10 - // @User: Advanced - GSCALAR(pitch_trim_cd, "TRIM_PITCH_CD", 0), + // @Description: Offset in degrees used for in-flight pitch trimming for level flight. Correct ground leveling is an alternative to changing this parameter. + // @Units: deg + // @Range: -45 45 + // @User: Standard + GSCALAR(pitch_trim, "PTCH_TRIM_DEG", 0.0f), - // @Param: ALT_HOLD_RTL + // @Param: RTL_ALTITUDE // @DisplayName: RTL altitude // @Description: Target altitude above home for RTL mode. Maintains current altitude if set to -1. Rally point altitudes are used if plane does not return to home. - // @Units: cm + // @Units: m // @User: Standard - GSCALAR(RTL_altitude_cm, "ALT_HOLD_RTL", ALT_HOLD_HOME_CM), + GSCALAR(RTL_altitude, "RTL_ALTITUDE", ALT_HOLD_HOME), - // @Param: ALT_HOLD_FBWCM - // @DisplayName: Minimum altitude for FBWB mode - // @Description: This is the minimum altitude in centimeters (above home) that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. It will also force a climb to this altitude if below in these modes. A value of zero means no limit. - // @Units: cm + // @Param: CRUISE_ALT_FLOOR + // @DisplayName: Minimum altitude for FBWB and CRUISE mode + // @Description: This is the minimum altitude in meters (above home) that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. It will also force a climb to this altitude if below in these modes. A value of zero means no limit. + // @Units: m // @User: Standard - GSCALAR(FBWB_min_altitude_cm, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM), + GSCALAR(cruise_alt_floor, "CRUISE_ALT_FLOOR", CRUISE_ALT_FLOOR), // @Param: FLAP_1_PERCNT // @DisplayName: Flap 1 percentage @@ -757,9 +756,9 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(arming, "ARMING_", AP_Arming_Plane), #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif #if PARACHUTE == ENABLED @@ -909,9 +908,11 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(camera_mount, "MNT", AP_Mount), #endif +#if HAL_LOGGING_ENABLED // @Group: LOG // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#endif // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -1031,11 +1032,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Values: 0:NotEnforced,1:Enforced // @User: Advanced AP_GROUPINFO("SYSID_ENFORCE", 4, ParametersG2, sysid_enforce, 0), -#if STATS_ENABLED == ENABLED - // @Group: STAT - // @Path: ../libraries/AP_Stats/AP_Stats.cpp - AP_SUBGROUPINFO(stats, "STAT", 5, ParametersG2, AP_Stats), -#endif + + // AP_Stats was 5 // @Group: SERVO // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp @@ -1089,12 +1087,12 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 1: Use centered throttle in Cruise or FBWB to indicate trim airspeed // @Bitmask: 2: Disable attitude check for takeoff arming // @Bitmask: 3: Force target airspeed to trim airspeed in Cruise or FBWB - // @Bitmask: 4: Climb to ALT_HOLD_RTL before turning for RTL + // @Bitmask: 4: Climb to RTL_ALTITUDE before turning for RTL // @Bitmask: 5: Enable yaw damper in acro mode // @Bitmask: 6: Supress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airspeed sensor. // @Bitmask: 7: EnableDefaultAirspeed for takeoff - // @Bitmask: 8: Remove the TRIM_PITCH_CD on the GCS horizon - // @Bitmask: 9: Remove the TRIM_PITCH_CD on the OSD horizon + // @Bitmask: 8: Remove the PTCH_TRIM_DEG on the GCS horizon + // @Bitmask: 9: Remove the PTCH_TRIM_DEG on the OSD horizon // @Bitmask: 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode // @Bitmask: 12: Enable FBWB style loiter altitude control @@ -1102,11 +1100,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), -#if AP_SCRIPTING_ENABLED - // @Group: SCR_ - // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp - AP_SUBGROUPINFO(scripting, "SCR_", 14, ParametersG2, AP_Scripting), -#endif + // 14 was AP_Scripting // @Param: TKOFF_ACCEL_CNT // @DisplayName: Takeoff throttle acceleration count @@ -1175,7 +1169,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Units: V // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("FWD_BAT_VOLT_MAX", 23, ParametersG2, fwd_thr_batt_voltage_max, 0.0f), + AP_GROUPINFO("FWD_BAT_VOLT_MAX", 23, ParametersG2, fwd_batt_cmp.batt_voltage_max, 0.0f), // @Param: FWD_BAT_VOLT_MIN // @DisplayName: Forward throttle battery voltage compensation minimum voltage @@ -1184,14 +1178,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Units: V // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("FWD_BAT_VOLT_MIN", 24, ParametersG2, fwd_thr_batt_voltage_min, 0.0f), + AP_GROUPINFO("FWD_BAT_VOLT_MIN", 24, ParametersG2, fwd_batt_cmp.batt_voltage_min, 0.0f), // @Param: FWD_BAT_IDX // @DisplayName: Forward throttle battery compensation index // @Description: Which battery monitor should be used for doing compensation for the forward throttle // @Values: 0:First battery, 1:Second battery // @User: Advanced - AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0), + AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_batt_cmp.batt_idx, 0), // @Param: FS_EKF_THRESH // @DisplayName: EKF failsafe variance threshold @@ -1312,7 +1306,6 @@ static const AP_Param::ConversionInfo conversion_table[] = { { Parameters::k_param_land_slope_recalc_shallow_threshold,0,AP_PARAM_FLOAT, "LAND_SLOPE_RCALC" }, { Parameters::k_param_land_slope_recalc_steep_threshold_to_abort,0,AP_PARAM_FLOAT, "LAND_ABORT_DEG" }, - { Parameters::k_param_land_pitch_cd, 0, AP_PARAM_INT16, "LAND_PITCH_CD" }, { Parameters::k_param_land_flare_alt, 0, AP_PARAM_FLOAT, "LAND_FLARE_ALT" }, { Parameters::k_param_land_flare_sec, 0, AP_PARAM_FLOAT, "LAND_FLARE_SEC" }, { Parameters::k_param_land_pre_flare_sec, 0, AP_PARAM_FLOAT, "LAND_PF_SEC" }, @@ -1543,6 +1536,48 @@ void Plane::load_parameters(void) #if AP_FENCE_ENABLED AP_Param::convert_class(g.k_param_fence, &fence, fence.var_info, 0, 0, true); #endif + + // PARAMETER_CONVERSION - Added: Dec 2023 + // Convert _CM (centimeter) parameters to meters and _CD (centidegrees) parameters to meters + g.pitch_trim.convert_centi_parameter(AP_PARAM_INT16); + aparm.airspeed_cruise.convert_centi_parameter(AP_PARAM_INT32); + aparm.min_groundspeed.convert_centi_parameter(AP_PARAM_INT32); + g.RTL_altitude.convert_centi_parameter(AP_PARAM_INT32); + g.cruise_alt_floor.convert_centi_parameter(AP_PARAM_INT16); + aparm.pitch_limit_max.convert_centi_parameter(AP_PARAM_INT16); + aparm.pitch_limit_min.convert_centi_parameter(AP_PARAM_INT16); + aparm.roll_limit.convert_centi_parameter(AP_PARAM_INT16); + + landing.convert_parameters(); + + // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 +#if AP_STATS_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { + return; + } + + const uint16_t old_index = 5; // Old parameter index in g2 + const uint16_t old_top_element = 4037; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(info.old_key, &stats, stats.var_info, old_index, old_top_element, false); + } +#endif + // PARAMETER_CONVERSION - Added: Jan-2024 for Plane-4.6 +#if AP_SCRIPTING_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { + return; + } + + const uint16_t old_index = 14; // Old parameter index in g2 + const uint16_t old_top_element = 78; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(info.old_key, &scripting, scripting.var_info, old_index, old_top_element, false); + } +#endif hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7210bfe7771267..ad78f76de7e216 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -56,7 +56,7 @@ class Parameters { // k_param_auto_trim = 10, // unused k_param_log_bitmask_old, // unused - k_param_pitch_trim_cd, + k_param_pitch_trim, k_param_mix_mode, k_param_reverse_elevons, // unused k_param_reverse_ch1_elevon, // unused @@ -159,14 +159,14 @@ class Parameters { // 110: Telemetry control // - k_param_gcs0 = 110, // stream rates for uartA - k_param_gcs1, // stream rates for uartC + k_param_gcs0 = 110, // stream rates for SERIAL0 + k_param_gcs1, // stream rates for SERIAL1 k_param_sysid_this_mav, k_param_sysid_my_gcs, k_param_serial1_baud_old, // deprecated k_param_telem_delay, k_param_serial0_baud_old, // deprecated - k_param_gcs2, // stream rates for uartD + k_param_gcs2, // stream rates for SERIAL2 k_param_serial2_baud_old, // deprecated k_param_serial2_protocol, // deprecated @@ -174,7 +174,7 @@ class Parameters { // k_param_airspeed_min = 120, k_param_airspeed_max, - k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm) + k_param_cruise_alt_floor, k_param_flybywire_elev_reverse, k_param_alt_control_algorithm, // unused k_param_flybywire_climb_rate, @@ -213,13 +213,13 @@ class Parameters { // k_param_crosstrack_gain = 150, // unused k_param_crosstrack_entry_angle, // unused - k_param_roll_limit_cd, - k_param_pitch_limit_max_cd, - k_param_pitch_limit_min_cd, - k_param_airspeed_cruise_cm, - k_param_RTL_altitude_cm, + k_param_roll_limit, + k_param_pitch_limit_max, + k_param_pitch_limit_min, + k_param_airspeed_cruise, + k_param_RTL_altitude, k_param_inverted_flight_ch_unused, // unused - k_param_min_gndspeed_cm, + k_param_min_groundspeed, k_param_crosstrack_use_wind, // unused @@ -436,9 +436,9 @@ class Parameters { AP_Int16 mixing_offset; AP_Int16 dspoiler_rud_rate; AP_Int32 log_bitmask; - AP_Int32 RTL_altitude_cm; - AP_Int16 pitch_trim_cd; - AP_Int16 FBWB_min_altitude_cm; + AP_Float RTL_altitude; + AP_Float pitch_trim; + AP_Float cruise_alt_floor; AP_Int8 flap_1_percent; AP_Int8 flap_1_speed; @@ -484,11 +484,6 @@ class ParametersG2 { AP_Button *button_ptr; #endif -#if STATS_ENABLED == ENABLED - // vehicle statistics - AP_Stats stats; -#endif - #if AP_ICENGINE_ENABLED // internal combustion engine control AP_ICEngine ice_control; @@ -524,10 +519,6 @@ class ParametersG2 { AP_Int32 flight_options; -#if AP_SCRIPTING_ENABLED - AP_Scripting scripting; -#endif // AP_SCRIPTING_ENABLED - AP_Int8 takeoff_throttle_accel_count; AP_Int8 takeoff_timeout; @@ -542,9 +533,26 @@ class ParametersG2 { AP_Int8 crow_flap_aileron_matching; // Forward throttle battery voltage compensation - AP_Float fwd_thr_batt_voltage_max; - AP_Float fwd_thr_batt_voltage_min; - AP_Int8 fwd_thr_batt_idx; + class FWD_BATT_CMP { + public: + // Calculate the throttle scale to compensate for battery voltage drop + void update(); + + // Apply throttle scale to min and max limits + void apply_min_max(int8_t &min_throttle, int8_t &max_throttle) const; + + // Apply throttle scale to throttle demand + float apply_throttle(float throttle) const; + + AP_Float batt_voltage_max; + AP_Float batt_voltage_min; + AP_Int8 batt_idx; + + private: + bool enabled; + float ratio; + } fwd_batt_cmp; + #if OFFBOARD_GUIDED == ENABLED // guided yaw heading PID diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index dff3166b02ea12..07055294b523a0 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -24,7 +24,9 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); constructor for main Plane class */ Plane::Plane(void) +#if HAL_LOGGING_ENABLED : logger(g.log_bitmask) +#endif { // C++11 doesn't allow in-class initialisation of bitfields auto_state.takeoff_complete = true; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 80e3cb8636ce1e..3769e44a543c5f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -44,7 +44,6 @@ #include // Photo or video camera #include #include -#include // statistics library #include #include @@ -85,7 +84,7 @@ #include #include #if AP_EXTERNAL_CONTROL_ENABLED -#include +#include "AP_ExternalControl_Plane.h" #endif #include "GCS_Mavlink.h" @@ -167,6 +166,10 @@ class Plane : public AP_Vehicle { friend class ModeThermal; friend class ModeLoiterAltQLand; +#if AP_EXTERNAL_CONTROL_ENABLED + friend class AP_ExternalControl_Plane; +#endif + Plane(void); private: @@ -189,11 +192,13 @@ class Plane : public AP_Vehicle { RC_Channel *channel_flap; RC_Channel *channel_airbrake; +#if HAL_LOGGING_ENABLED AP_Logger logger; +#endif // scaled roll limit based on pitch int32_t roll_limit_cd; - int32_t pitch_limit_min_cd; + float pitch_limit_min; // flight modes convenience array AP_Int8 *flight_modes = &g.flight_mode1; @@ -776,9 +781,9 @@ class Plane : public AP_Vehicle { AP_Param param_loader {var_info}; - // dummy implementation of external control + // external control library #if AP_EXTERNAL_CONTROL_ENABLED - AP_ExternalControl external_control; + AP_ExternalControl_Plane external_control; #endif static const AP_Scheduler::Task scheduler_tasks[]; @@ -1084,6 +1089,7 @@ class Plane : public AP_Vehicle { int8_t takeoff_tail_hold(void); int16_t get_takeoff_pitch_min_cd(void); void landing_gear_update(void); + bool check_takeoff_timeout(void); // avoidance_adsb.cpp void avoidance_adsb_update(void); @@ -1091,7 +1097,8 @@ class Plane : public AP_Vehicle { // servos.cpp void set_servos_idle(void); void set_servos(); - void set_servos_controlled(void); + float apply_throttle_limits(float throttle_in); + void set_throttle(void); void set_takeoff_expected(void); void set_servos_old_elevons(void); void set_servos_flaps(void); @@ -1103,7 +1110,6 @@ class Plane : public AP_Vehicle { void servos_auto_trim(void); void servos_twin_engine_mix(); void force_flare(); - void throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) const; void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func); bool suppress_throttle(void); @@ -1241,8 +1247,10 @@ class Plane : public AP_Vehicle { void failsafe_check(void); bool is_landing() const override; bool is_taking_off() const override; -#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED bool set_target_location(const Location& target_loc) override; +#endif //AP_SCRIPTING_ENABLED || AP_EXTERNAL_CONTROL_ENABLED +#if AP_SCRIPTING_ENABLED bool get_target_location(Location& target_loc) override; bool update_target_location(const Location &old_loc, const Location &new_loc) override; bool set_velocity_match(const Vector2f &velocity) override; diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 7c1d1f296ad642..bff21a6845c3db 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,6 +1,322 @@ -Release 4.4.4-beta1 5th December 2023 +Release 4.5.0-beta1 30th January 2024 ------------------------------------- +Changes from 4.4.4 + +1) New autopilots supported + - ACNS-F405AIO + - Airvolute DCS2 onboard FMU + - Aocoda-RC-H743Dual + - BrainFPV RADIX 2 HD + - CAN-Zero + - CM4Pilot + - CubeRed + - Esp32-tomte76, esp32nick, esp32s3devkit + - FlyingMoonH743 + - Flywoo F405 Pro + - FlywooF405S-AIO with alternative IMUs + - Here4 GPS as flight controller + - Holybro 6X revision 6 + - Holybro6X-45686 with 3x ICM45686 IMUs + - JAE JFB110 + - KakuteH7 using ICM42688 + - PixFlamingo (uses STM32L4PLUS CPU) + - PixPilot-C3 + - PixSurveyA1-IND + - QiotekAdeptF407 + - Sierra TrueNavIC + - SPRacing H7RF + - SW-Nav-F405 + - YJUAV_A6 + - YJUAV_A6SE, YJUAV_A6SE_H743 Plane +2) Autopilot specific changes + - 1MB boards lose features to save flash (Payload Place, some battery monitors, NMEA Output, bootloaders, Turtle mode) + - CubeOrangePlus supports IMU high resolution sampling (works with ICM42688, ICM42652, ICM42670, ICM45686 IMUs) + - F4 processors with only 1 IMU gain Triple Harmonic Notch support + - F765-SE bdshot support on 1st 4 pin + - F7 and H7 boards lose DMA on I2C ports (not required, limited DMA better used elsewhere) + - FlyingMoonH743, FlyingMoonF427 2nd and 3rd IMU order swapped + - HEEWING-F405 supports CRSF + - MatekL431-RC bootloader added, DMA used for RC and GPS + - PH4-mini, PH4-mini-bdshot, Swan-K1 and TBS-Colibri-F7 BRD_SER4_RTSCTS param conflict fixed + - Pixhawk6C supports BMI088 baro + - TMotorH743, Heewing-F405 serial parameter definitions fixed +3) AHRS/EKF enhancements and fixes + - AHRS_OPTIONS supports disabling fallback to DCM + - BARO_ALT_OFFSET slews more slowly (was 20sec, now 1min) + - EKF2 removed (can be re-enabled with Custom build server) + - External AHRS support for multiple GPSs + - InertialLabs INS-U external AHRS support + - Lord external AHRS renamed to MicroStrain5 + - MAV_CMD_EXTERNAL_POSITION_ESTIMATE supports setting approximate position during dead-reckoning + - Microstrain7 (aka 3DM-QG7) external AHRS support +4) Driver enhancements + - 3DR Solo supports up to 6S batteries + - Airspeed health checks vs GPS use 3D velocity + - BDshot on the first four channels of boards with F103-based IOMCUs (e.g. Pixhawk 6X) + - Dshot on all IOMCU channels on all boards with an IOMCU (e.g. all CubePilot autopilots) + - Dshot commands (e.g. motor reversal abd beeps) and EDT supported on IOMCU + - DroneCAN battery monitors calculate consumed energy if battery doesn't provide directly + - DroneCAN RC and Ghost RC protocol support + - EFI MAVLink driver + - Extended DShot Telemetry support (requires BLHeli32 ver 32.10 or BlueJay, set SERVO_DSHOT_ESC=3 or 4) + - GPS L5 band health override to enable L5 signal use (see GPS_DRV_OPTIONS) + - GPS-for-yaw works at lower update rate (3hz minimum) + - GSOF GPS supports GPS_COM_PORT parameter + - Hirth ICEngine support + - ICE option to enable/disable starting while disarmed + - ICE support for starter via relay + - IMUDATA serial protocol outputs raw IMU data on serial port (only available using custom build server) + - Innomaker LD06 360deg lidar support + - Intelligent Energy fuel cells new protocol support + - IRC Tramp supports 1G3 bands A and B + - IRC Ghost support + - JAE JRE-30 radar + - KDECAN driver rewrite (params moved to KDE_, works on all vehicles) + - MCP9601 temperature sensor support + - NanoRadar NRA24 rangefinder support + - NeoPixelsRGB support + - NoopLoop TOFSense, TOFSenseF-I2c rangefinder support + - OSD shows flashing horizon when inverted + - OSD2 support (e.g. second OSD) + - QMC5883P compass support + - Relay refactored to support RELAYx_FUNCTION, RELAY_STATUS message support added + - Reventech fuel level support (extends existing analog driver, see BATT_FL_xx parameters) + - RPLidarS1 360 deg lidar support and improved reliability for all RPLidars + - SBF GPS supports yaw from dual antennas + - Temperature sensor using analog voltages supported + - Trimble PX-1 support added as a GPS + - Winch driver enhancements including stuck protection, option for spin-free on startup +5) Control and navigation changes and enhancements + - Auto missions can always be cleared while disarmed (would fail if mission still running) + - DO_ENGINE_CONTROL allows starting engine even when disarmed + - DO_SET_MISSION_CURRENT command can reset mission (see Reset Mission field) + - DO_SET_SERVO, DO_REPEAT_SERVO work with servo outputs set to RCInxScaled + - Fractional Loiter Turn Support in missions + - HarmonicNotch supports up to 16 harmonics + - JUMP command sends improved text msg to pilot (says where will jump to) + - MAV_CMD_AIRFRAME_CONFIGURATION can control landing gear on all vehicles + - MOT_OPTIONS allows voltage compensation to use raw battery voltages (instead of current corrected voltage) + - PID controllers get DFF/D_FF (derivative feed-forward), NTF (target notch filter index) and NEF (error notch filter index) + - PID controllers get PDMX param to limit P+D output (useful for large vehicles and/or slow actuators) + - PID notch filter configured via new filter library using FILT parameters + - VTOLs send ATTITUDE_TARGET messages to GCS +6) Plane specific enhancements + - AUTOTUNE_OPTIONS allows disabling filter updates + - Harmonic Notch frequencies can be logged at full rate + - L1 controller checks heading and ground track agree to improve strong headwind edge case + - Land airspeed default is halfway between min and cruise + - LoiterAltQLand mode re-uses Loiter point if available + - AUTO mode landing abort aux switch renamed + - Q_M_SPOOL_TIM_DN allows slower spool down of motors + - Quadplane use of forward throttle in VTOL modes improved + - Tailsitters use motor I term for pitch control if no pitch surfaces are setup + - Takeoff mode holds down elevator on taildraggers + - Transition time may be no less than 2 seconds (see TRANSITION_MS) + - VTOL angle controller gets feed-forward scaling (see Q_OPTIONS) +7) Parameters renamed and rescaled + - COMPASS_TYPEMASK renamed to COMPASS_DISBLMSK + - SYS_NUM_RESETS replaced by STAT_BOOTCNT + - AIRSPEED_CRUISE replaces TRIM_ARSPD_CM + - AIRSPEED_MIN replaces ARSPD_FBW_MIN + - AIRSPEED_MAX replaces ARSPD_FBW_MAX + - CRUISE_ALT_FLOOR replaces ALT_HOLD_FBWCM + - LAND_FINAL_SPD replaces LAND_SPEED + - LAND_PITCH_DEG replaces LAND_PITCH_CD + - MIN_GROUNDSPEED replaces MIN_GNDSPD_CM + - PTCH_LIM_MAX_DEG replaces LIM_PITCH_MAX + - PTCH_LIM_MIN_DEG replaces LIM_PITCH_MIN + - PTCH_TRIM_DEG replaces TRIM_PITCH_CD + - Q_LAND_FINAL_SPD replaces Q_LAND_SPEED + - Q_PILOT_ACCEL_Z replaces Q_ACCEL_Z + - Q_PILOT_SPD_UP replaces Q_VELZ_MAX + - Q_PILOT_SPD_DN replaces Q_VELZ_MAX_DN + - ROLL_LIMIT_DEG replaces LIM_ROLL_CD + - RTL_ALTITUDE replaces ALT_HOLD_RTL +8) ROS2 / DDS support +9) Camera and gimbal enhancements + - Calculates location where camera gimbal is pointing (see CAMERA_FOV_STATUS) + - CAMx_MNT_INST allows specifying which mount camera is in + - Camera lens (e.g. live video stream) selectable using RC aux function + - Interval timer (for taking pictures at timed intervals) + - Image tracking support (ViewPro only) + - MAVLink Gimbal Protocol v2 support for better GCS integration + - MNTx_SYSID_DFLT allows easier pointing gimbal at another vehicle + - MOUNT_CONTROL, MOUNT_CONFIGURE messages deprecated + - RangeFinder support (only logged, only supported on Siyi, Viewpro) + - Pilot's RC input re-takes manual control of gimbal (e.g. switches to RC_TARGETING mode) + - Siyi driver gets Zoom control, sends autopilot attitude and time (reduces leans) + - Video recording may start when armed (see CAMx_OPTIONS) + - ViewPro driver (replaces equivalent Lua driver) + - Xacti camera gimbal support + - Zoom percentage support (for both missions and GCS commands) +10) Logging and reporting changes + - Battery logging (e.g. BAT) includes health, temperature, state-of-health percentage + - CAM and MNT messages contain camera gimbal's desired and actual angles + - CTUN includes airspeed estimate type (e.g. sensor, EKF3 estimate) + - INS_RAW_LOG_OPT allows raw, pre-filter and post-filter sensor data logging (alternative to "batch logging", good for filtering analysis) + - PID logging gets reset and I-term-set flags + - Rangefinder logging (e.g. RFND) includes signal quality + - RC aux functions sorted alphabetically for GCS + - RC logging (RCI, RCI2) include valid input and failsafe flags + - RTK GPS logging includes number of fragments used or discarded + - TEC2 provides extended TECS controller logging + - TSIT provides tail sitter speed scaling values +11) Scripting enhancements + - Autopilot reboot support + - Baro, Compass, IMU, IOMCU health check support + - Battery cell voltage bindings + - Battery driver support + - BattEsimate.lua applet estimates SoC from voltage + - Camera and Mount bindings improved + - CAN input packet filtering reduces work required by Lua CAN drivers + - DJI RS2/RS3 gimbal driver supports latest DJI firmware version (see mount-djirs2-driver.lua) + - EFI drivers for DLA serial, InnoFlight Inject EFI driver + - EFI bindings improved + - Fence support + - Generator drivers for Halo6000, Zhuhai SVFFI + - GCS failsafe support + - Hobbywing_DataLink driver (see Hobbywing_DataLink.lua) + - is_landing, is_taking_off bindings + - led_on_a_switch.lua sets LED brightness from RC switch + - MAVLink sending and receiving support + - Mission jump_to_landing_sequence binding + - mount-poi.lua upgraded to applet, sends better feedback, can lock onto Location + - Networking/Ethernet support + - Plane dual-aircraft synchronised aerobatics + - Proximity driver support + - Rangefinder drivers can support signal quality + - revert_param.lua applet for quickly reverting params during tuning + - RockBlock.lua applet supports setting mode, fix for battery voltage reporting + - Serial/UART reading performance improvement using readstring binding + - sport_aerobatics.lua rudder control fixed + - Thread priority can be set using SCR_THD_PRIORITY (useful for Lua drivers) + - Wind alignment and head_wind speed bindings +12) Safety related enhancements and fixes + - Advanced GCS failsafe action to switch to Auto mode + - Advanced GCS failsafe timeout configurable (see AFS_GCS_TIMEOUT) + - Arm in AUTO/TAKEOFF modes only after stick returns to center + - Arm/Disarmed GPIO may be disabled using BRD_OPTIONS + - Arming allowed with Fence enabled but without a compass (previously failed) + - Arming check of compass vs world magnetic model to detect metal in ground (see ARMING_MAGTHRESH) + - Arming check of GPIO pin interrupt storm + - Arming check of Lua script CRC + - Arming check of mission loaded from SD card + - Arming check of Relay pin conflicts + - Arming check to allow Tricopter-Plane with no yaw servo + - Arming check of emergency stop skipped if emergency stop aux function configured + - Arming failures reported more quickly when changing from success to failed + - ARMING_OPTIONS allows supressing "Armed", "Disarmed" text messages + - BRD_SAFETY_MASK extended to apply to CAN ESCs and servos + - Buzzer noise for gyro calibration and arming checks passed + - FENCE_OPTIONS supports union OR intersection of all polygon fences + - FLTMODE_GCSBLOCK blocks GCS from changing vehicle to specified modes + - Long failsafe action to switch to Auto mode (see FS_LONG_ACTN) + - Main loop lockup recovery by forcing mutex release (only helps if caused by software bug) + - Parachute releases causes disarm and ICE shutoff + - Rally points supports altitude frame (AMSL, Relative or Terrain) + - RC failsafe does not trigger until RC has been received at least once + - SERVO_RC_FS_MSK allows outputs using RC passthrough to move to SERVOx_TRIM on RC failsafe + - Takeoff mode gets failsafe protections +13) System Enhancements + - CAN port can support a second CAN protocol on the same bus (2nd protocol must be 11 bit, see CAN_Dn_PROTOCOL2) + - CAN-FD support (allows faster data transfer rates) + - Crash dump info logged if main thread locksup (helps with root cause analysis) + - Ethernet/Networking support for UDP and TCP server and client (see NET_ENABLED) and PPP (see SERIALx_PROTOCOL) + - Firmware flashing from SD card + - Linux board SBUS input decoding made consistent with ChibiOS + - Linux boards support DroneCAN + - Parameter defaults stored in @ROMFS/defaults.parm + - SD Card formatting supported on all boards + - Second USB endpoint defaults to MAVLink (instead of SLCAN) except on CubePilot boards + - Serial over DroneCAN (see CAN_D1_UC_SER_EN) useful for configuring F9P DroneCAN GPSs using uCenter +14) Custom Build server include/exclude features extended to include + - APJ Tools + - Bootloader flashing + - Button + - Compass calibration + - DroneCAN GPS + - ExternalAHRS (e.g. MicroStrain, Vectornav) + - Generator + - Highmark Servo + - Hobbywing ESCs + - Kill IMU + - Payload Place + - Plane BlackBox arming allows Plane to be used as logger (see ARMING_BBOX_SPD) + - Plane's TX Tuning + - Precision landing + - Proximity sensor + - RC Protocol + - Relay + - SBUS Output + - ToneAlarm + - Winch +15) Developer specific items + - ChibiOS upgrade to 21.11 + - UAVCAN replaced with DroneCAN + - AUTOPILOT_VERSION_REQUEST message deprecated (use REQUEST_MESSAGE instead) + - PREFLIGHT_SET_SENSOR_OFFSETS support deprecated (was unused by all known ground stations) + - MISSION_SET_CURRENT message deprecated (use DO_SET_MISSION_CURRENT command instead) + - MISSION_CURRENT message sends num commands and stopped/paused/running/complete state + - Python version requirement increased to 3.6.9 + - mavlink_parse.py shows all suported mavlink messages + - COMMAND_INT messages can be used for nearly all commands (previously COMMAND_LONG) +16) Bug fixes: + - 3DR Solo gimbal mavlink routing fixed + - Airbrakes auxiliary function fixed + - Airspeed health always checked before use (may not have been checked when using "affinity") + - always ignore invalid pilot input throttle + - Bootloop fixed if INS_GYRO_FILTER set too high + - Button Internal Error caused by floating pin or slow device power-up fixed + - CAN Compass order maintained even if compass powered up after autopilot + - Compass device IDs only saved when calibrated to ensure external compasses appear as primary on new boards + - Cruise mode locks in heading only once moving forwards (improves VTOL transition reliability in high winds) + - Currawong ECU EFI does not send exhaust gas temperature + - DO_REPOSITION interprets NaN as zero + - DCM fallback in order to get better GPS is disabled if GPS is not used + - DJI RS2/RS3 gimbal reported angle fix + - DO_SET_ROI, ROI_LOCATION, ROI_NONE bug fix that could lead to gimbal pointing at old target + - Fix throttle going bellow min in fbwa RC failsafe + - Generator parameter init fix (defaults might not always have been loaded correctly) + - GPS_TC_BLEND parameter removed (it was unused) + - Ground speed undershoot correction during loss of GPS fixed + - Guided mode heading control anti windup fix + - Harmonic Notch gets protection against extremely low notch filter frequencies + - Home altitude change while navigating handled correctly (previously led to sudden demanded height change) + - IE 650/800 Generators report fuel remaining + - INS calibration prevents unlikely case of two calibrations running at same time + - LPS2XH Baro supported over I2C fixed + - MatekH743 storage eeprom size fixed + - MAVLink routing fix to avoid processing packet meant for another vehicle + - Mount properly enforces user defined angle limits + - MPU6500 IMU filter corrected to 4k + - nav_roll (aka target roll) calculation improved + - NMEA output time and altitude fixed + - OSD gets labels for all supported serial protocols + - OSD RF panel format fixed + - Reset mission if in landing sequence while also disarmed and on the ground (avoids pre-arm check) + - RTL_AUTOLAND with rally points fix (could skip climb to rally point's altitude) + - RobotisServo initialisation fix + - RPM accuracy and time wrap handling improved + - Sagetech ADSB MXS altitude fix (needs amsl, was sending alt-above-terrain) + - SageTechMXS ADSB climb rate direction fixed + - SBUS out exactly matches SBUS in decoding + - Serial port RTS pins default to pulldown (SiK radios could getting stuck in bootloader mode) + - SERIALx_ parameters removed for ports that can't actually be used + - Servo gimbal attitude reporting fix + - Servo output fix when using scaled RC passthrough (e.g. SERVOx_FUNCTION = RCinXScaled) + - Siyi continuous zoom stutter fixed + - Siyi gimbal upside-down mode fixed (avoid bobbing if upside-down) + - TECS's max deceleration scales properly with vehicle velocity + - ST24 RC protocol fixed + - STM32L496 CAN2 init fix (AP_Periph only?) + - VFR_HUD climb rate reports best estimate during high vibration events (previously it would stop updating) + - Visual Odometry healthy check fix in case of out-of-memory + - VTX_MAX_POWER restored (allows setting radio's power) + +Release 4.4.4 19th December 2023 +-------------------------------- + Changes from 4.4.3: - CubeOrange Sim-on-hardware compilation fix @@ -4553,10 +4869,10 @@ enabled. When in stabilised manual throttle modes this option has the effect of limiting how much bank angle you can demand when close to the -configured minimum airspeed (from ARSPD_FBW_MIN). That means when in -FBWA mode if you try to turn hard while close to ARSPD_FBW_MIN it will +configured minimum airspeed (from AIRSPEED_MIN). That means when in +FBWA mode if you try to turn hard while close to AIRSPEED_MIN it will limit the bank angle to an amount that will keep the speed above -ARSPD_FBW_MIN times the aerodynamic load factor. It will always allow +AIRSPEED_MIN times the aerodynamic load factor. It will always allow you at bank at least 25 degrees however, to ensure you keep some maneuverability if the airspeed estimate is incorrect. @@ -4569,7 +4885,7 @@ handle the demanded turn. After the turn is complete the minimum airspeed will drop back to the normal level. This change won't completely eliminate stalls of course, but it should -make them less likely if you properly configure ARSPD_FBW_MIN for your +make them less likely if you properly configure AIRSPEED_MIN for your aircraft. PX4IO based RC override code diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 4c3a35f4097cad..04ca8440570745 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -103,10 +103,10 @@ void Plane::setup_glide_slope(void) */ int32_t Plane::get_RTL_altitude_cm() const { - if (g.RTL_altitude_cm < 0) { + if (g.RTL_altitude < 0) { return current_loc.alt; } - return g.RTL_altitude_cm + home.alt; + return g.RTL_altitude*100 + home.alt; } /* @@ -337,7 +337,7 @@ int32_t Plane::calc_altitude_error_cm(void) } /* - check for FBWB_min_altitude_cm and fence min/max altitude + check for cruise_alt_floor and fence min/max altitude */ void Plane::check_fbwb_altitude(void) { @@ -359,9 +359,9 @@ void Plane::check_fbwb_altitude(void) } #endif - if (g.FBWB_min_altitude_cm != 0) { + if (g.cruise_alt_floor > 0) { // FBWB min altitude exists - min_alt_cm = MAX(min_alt_cm, plane.g.FBWB_min_altitude_cm); + min_alt_cm = MAX(min_alt_cm, plane.g.cruise_alt_floor*100.0); should_check_min = true; } diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index e0dabe8af0323c..13c0dfe4d67120 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -180,9 +180,9 @@ bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle new_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX return true; - } else if (plane.current_loc.alt > plane.g.RTL_altitude_cm) { + } else if (plane.current_loc.alt > plane.g.RTL_altitude*100) { // should descend while above RTL alt - // TODO: consider using a lower altitude than RTL_altitude_cm since it's default (100m) is quite high + // TODO: consider using a lower altitude than RTL_altitude since it's default (100m) is quite high new_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX return true; } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 823f1e4da93bb5..7c50d9ab492498 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -12,10 +12,12 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) plane.target_altitude.terrain_following_pending = false; #endif +#if HAL_LOGGING_ENABLED // log when new commands start if (should_log(MASK_LOG_CMD)) { logger.Write_Mission_Cmd(mission, cmd); } +#endif // special handling for nav vs non-nav commands if (AP_Mission::is_nav_cmd(cmd)) { @@ -348,6 +350,7 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) next_WP_loc = calc_best_rally_or_home_location(current_loc, rtl_altitude_AMSL_cm); setup_terrain_target_alt(next_WP_loc); set_target_altitude_location(next_WP_loc); + plane.altitude_error_cm = calc_altitude_error_cm(); if (aparm.loiter_radius < 0) { loiter.direction = -1; @@ -358,7 +361,9 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) setup_glide_slope(); setup_turn_angle(); +#if HAL_LOGGING_ENABLED logger.Write_Mode(control_mode->mode_number(), control_mode_reason); +#endif } Location Plane::calc_best_rally_or_home_location(const Location &_current_loc, float rtl_home_alt_amsl_cm) const @@ -471,8 +476,9 @@ void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd) cmdloc.sanitize(current_loc); set_next_WP(cmdloc); loiter_set_direction_wp(cmd); + const float turns = cmd.get_loiter_turns(); - loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL; + loiter.total_cd = (uint32_t)(turns * 36000UL); condition_value = 1; // used to signify primary turns goal not yet met } @@ -589,22 +595,8 @@ bool Plane::verify_takeoff() } // check for optional takeoff timeout - if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { - const float ground_speed = gps.ground_speed(); - const float takeoff_min_ground_speed = 4; - if (!arming.is_armed_and_safety_off()) { - return false; - } - if (ground_speed >= takeoff_min_ground_speed) { - takeoff_state.start_time_ms = 0; - } else { - uint32_t now = AP_HAL::millis(); - if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) { - gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout at %.1f m/s", ground_speed); - plane.arming.disarm(AP_Arming::Method::TAKEOFFTIMEOUT); - mission.reset(); - } - } + if (plane.check_takeoff_timeout()) { + mission.reset(); } // see if we have reached takeoff altitude @@ -972,7 +964,7 @@ bool Plane::do_change_speed(uint8_t speedtype, float speed_target_ms, float thro break; case 1: // Ground speed gcs().send_text(MAV_SEVERITY_INFO, "Set groundspeed %u", (unsigned)speed_target_ms); - aparm.min_gndspeed_cm.set(speed_target_ms * 100); + aparm.min_groundspeed.set(speed_target_ms); return true; } @@ -1089,7 +1081,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd) const float breakout_direction_rad = radians(vtol_approach_s.approach_direction_deg + (direction > 0 ? 270 : 90)); // breakout when within 5 degrees of the opposite direction - if (fabsF(wrap_PI(ahrs.yaw - breakout_direction_rad)) < radians(5.0f)) { + if (fabsF(wrap_PI(ahrs.get_yaw() - breakout_direction_rad)) < radians(5.0f)) { gcs().send_text(MAV_SEVERITY_INFO, "Starting VTOL land approach path"); vtol_approach_s.approach_stage = APPROACH_LINE; set_next_WP(cmd.content.location); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index a8c6c890c96cb0..254165a0027e60 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -114,16 +114,15 @@ #ifndef AIRSPEED_CRUISE # define AIRSPEED_CRUISE 12 // 12 m/s #endif -#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100 + ////////////////////////////////////////////////////////////////////////////// -// MIN_GNDSPEED +// MIN_GROUNDSPEED // -#ifndef MIN_GNDSPEED - # define MIN_GNDSPEED 0 // m/s (0 disables) +#ifndef MIN_GROUNDSPEED + # define MIN_GROUNDSPEED 0 // m/s (0 disables) #endif -#define MIN_GNDSPEED_CM MIN_GNDSPEED*100 ////////////////////////////////////////////////////////////////////////////// @@ -136,10 +135,9 @@ # define AIRSPEED_FBW_MAX 22 #endif -#ifndef ALT_HOLD_FBW - # define ALT_HOLD_FBW 0 +#ifndef CRUISE_ALT_FLOOR + # define CRUISE_ALT_FLOOR 0 #endif -#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100 ////////////////////////////////////////////////////////////////////////////// @@ -158,8 +156,8 @@ ////////////////////////////////////////////////////////////////////////////// // Autopilot control limits // -#ifndef HEAD_MAX - # define HEAD_MAX 45 +#ifndef ROLL_LIMIT_DEG + # define ROLL_LIMIT_DEG 45 #endif #ifndef PITCH_MAX # define PITCH_MAX 20 @@ -167,9 +165,6 @@ #ifndef PITCH_MIN # define PITCH_MIN -25 #endif -#define HEAD_MAX_CENTIDEGREE HEAD_MAX * 100 -#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100 -#define PITCH_MIN_CENTIDEGREE PITCH_MIN * 100 #ifndef RUDDER_MIX # define RUDDER_MIX 0.5f @@ -186,10 +181,6 @@ // Logging control // -#ifndef LOGGING_ENABLED - # define LOGGING_ENABLED ENABLED -#endif - #define DEFAULT_LOG_BITMASK 0xffff @@ -207,7 +198,6 @@ #ifndef ALT_HOLD_HOME # define ALT_HOLD_HOME 100 #endif -#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100 ////////////////////////////////////////////////////////////////////////////// // Developer Items @@ -229,10 +219,6 @@ #define PARACHUTE HAL_PARACHUTE_ENABLED #endif -#ifndef STATS_ENABLED - # define STATS_ENABLED ENABLED -#endif - #ifndef OSD_ENABLED #define OSD_ENABLED DISABLED #endif diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index c7389daf49bdca..fac041ad667e41 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -102,6 +102,7 @@ enum log_messages { LOG_PIDG_MSG, LOG_AETR_MSG, LOG_OFG_MSG, + LOG_TSIT_MSG, }; #define MASK_LOG_ATTITUDE_FAST (1<<0) @@ -160,8 +161,8 @@ enum FlightOptions { ACRO_YAW_DAMPER = (1 << 5), SURPRESS_TKOFF_SCALING = (1<<6), ENABLE_DEFAULT_AIRSPEED = (1<<7), - GCS_REMOVE_TRIM_PITCH_CD = (1 << 8), - OSD_REMOVE_TRIM_PITCH_CD = (1 << 9), + GCS_REMOVE_TRIM_PITCH = (1 << 8), + OSD_REMOVE_TRIM_PITCH = (1 << 9), CENTER_THROTTLE_TRIM = (1<<10), DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), diff --git a/ArduPlane/ekf_check.cpp b/ArduPlane/ekf_check.cpp index e36e4ef1abcdf7..ae8f2415bbf7c1 100644 --- a/ArduPlane/ekf_check.cpp +++ b/ArduPlane/ekf_check.cpp @@ -74,7 +74,7 @@ void Plane::ekf_check() // limit count from climbing too high ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.bad_variance = true; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); // send message to gcs if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); @@ -91,7 +91,7 @@ void Plane::ekf_check() // if compass is flagged as bad and the counter reaches zero then clear flag if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { ekf_check_state.bad_variance = false; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); } @@ -155,7 +155,7 @@ void Plane::failsafe_ekf_event() // EKF failsafe event has occurred ekf_check_state.failsafe_on = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); // if not in a VTOL mode requiring position, then nothing needs to be done #if HAL_QUADPLANE_ENABLED @@ -182,5 +182,5 @@ void Plane::failsafe_ekf_off_event(void) } ekf_check_state.failsafe_on = false; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); } diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 640ad65869926d..33062460df6b0e 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -83,7 +83,7 @@ void Plane::fence_check() loc.alt = home.alt + 100.0f * fence.get_return_altitude(); } else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) { // invalid min/max, use RTL_altitude - loc.alt = home.alt + g.RTL_altitude_cm; + loc.alt = home.alt + g.RTL_altitude*100; } else { // fly to the return point, with an altitude half way between // min and max @@ -114,10 +114,10 @@ void Plane::fence_check() break; } - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches) { // record clearing of breach - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } } diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index ffe578736aff14..979a640a69ee4a 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -1,5 +1,7 @@ #include "Plane.h" +#include // statistics library + /* is_flying and crash detection logic */ @@ -18,7 +20,7 @@ void Plane::update_is_flying_5Hz(void) bool is_flying_bool = false; uint32_t now_ms = AP_HAL::millis(); - uint32_t ground_speed_thresh_cm = (aparm.min_gndspeed_cm > 0) ? ((uint32_t)(aparm.min_gndspeed_cm*0.9f)) : GPS_IS_FLYING_SPEED_CMS; + uint32_t ground_speed_thresh_cm = (aparm.min_groundspeed > 0) ? ((uint32_t)(aparm.min_groundspeed*(100*0.9))) : GPS_IS_FLYING_SPEED_CMS; bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) && (gps.ground_speed_cm() >= ground_speed_thresh_cm); @@ -163,14 +165,16 @@ void Plane::update_is_flying_5Hz(void) #if PARACHUTE == ENABLED parachute.set_is_flying(new_is_flying); #endif -#if STATS_ENABLED == ENABLED - g2.stats.set_flying(new_is_flying); +#if AP_STATS_ENABLED + AP::stats()->set_flying(new_is_flying); #endif AP_Notify::flags.flying = new_is_flying; crash_detection_update(); +#if HAL_LOGGING_ENABLED Log_Write_Status(); +#endif // tell AHRS flying state set_likely_flying(new_is_flying); diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index b6c3a2b44fc863..30ec5fc5b0fb1b 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -135,6 +135,9 @@ bool Mode::enter() GCS_SEND_TEXT(MAV_SEVERITY_INFO, "In landing sequence: mission reset"); plane.mission.reset(); } + + // Make sure the flight stage is correct for the new mode + plane.update_flight_stage(); } return enter_result; @@ -255,3 +258,82 @@ void Mode::output_rudder_and_steering(float val) SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, val); SRV_Channels::set_output_scaled(SRV_Channel::k_steering, val); } + +// Output pilot throttle, this is used in stabilized modes without auto throttle control +// Direct mapping if THR_PASS_STAB is set +// Otherwise apply curve for trim correction if configured +void Mode::output_pilot_throttle() +{ + if (plane.g.throttle_passthru_stabilize) { + // THR_PASS_STAB set, direct mapping + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true)); + return; + } + + // get throttle, but adjust center to output TRIM_THROTTLE if flight option set + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_adjusted_throttle_input(true)); +} + +// true if throttle min/max limits should be applied +bool Mode::use_throttle_limits() const +{ +#if AP_SCRIPTING_ENABLED + if (plane.nav_scripting_active()) { + return false; + } +#endif + + if (this == &plane.mode_stabilize || + this == &plane.mode_training || + this == &plane.mode_acro || + this == &plane.mode_fbwa || + this == &plane.mode_autotune) { + // a manual throttle mode + return !plane.g.throttle_passthru_stabilize; + } + + if (is_guided_mode() && plane.guided_throttle_passthru) { + // manual pass through of throttle while in GUIDED + return false; + } + +#if HAL_QUADPLANE_ENABLED + if (quadplane.in_vtol_mode()) { + return quadplane.allow_forward_throttle_in_vtol_mode(); + } +#endif + + return true; +} + +// true if voltage correction should be applied to throttle +bool Mode::use_battery_compensation() const +{ +#if AP_SCRIPTING_ENABLED + if (plane.nav_scripting_active()) { + return false; + } +#endif + + if (this == &plane.mode_stabilize || + this == &plane.mode_training || + this == &plane.mode_acro || + this == &plane.mode_fbwa || + this == &plane.mode_autotune) { + // a manual throttle mode + return false; + } + + if (is_guided_mode() && plane.guided_throttle_passthru) { + // manual pass through of throttle while in GUIDED + return false; + } + +#if HAL_QUADPLANE_ENABLED + if (quadplane.in_vtol_mode()) { + return false; + } +#endif + + return true; +} diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 2f745074c6f270..b7f4732e924fe7 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -135,6 +135,11 @@ class Mode // true if is taking virtual bool is_taking_off() const; + // true if throttle min/max limits should be applied + bool use_throttle_limits() const; + + // true if voltage correction should be applied to throttle + bool use_battery_compensation() const; protected: @@ -150,6 +155,9 @@ class Mode // Helper to output to both k_rudder and k_steering servo functions void output_rudder_and_steering(float val); + // Output pilot throttle, this is used in stabilized modes without auto throttle control + void output_pilot_throttle(); + #if HAL_QUADPLANE_ENABLED // References for convenience, used by QModes AC_PosControl*& pos_control; @@ -257,6 +265,8 @@ class ModeAutoTune : public Mode bool mode_allows_autotuning() const override { return true; } + void run() override; + protected: bool _enter() override; @@ -488,6 +498,8 @@ class ModeFBWA : public Mode bool mode_allows_autotuning() const override { return true; } + void run() override; + }; class ModeFBWB : public Mode @@ -783,7 +795,7 @@ class ModeTakeoff: public Mode AP_Int16 target_dist; AP_Int8 level_pitch; - bool takeoff_started; + bool takeoff_mode_setup; Location start_loc; bool _enter() override; diff --git a/ArduPlane/mode_acro.cpp b/ArduPlane/mode_acro.cpp index c15970b9bdecec..7093fd96b8cd23 100644 --- a/ArduPlane/mode_acro.cpp +++ b/ArduPlane/mode_acro.cpp @@ -26,6 +26,8 @@ void ModeAcro::update() void ModeAcro::run() { + output_pilot_throttle(); + if (plane.g.acro_locking == 2 && plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) { // we can do 3D acro locking diff --git a/ArduPlane/mode_autotune.cpp b/ArduPlane/mode_autotune.cpp index 395a6d6ef531cb..cbbf5a22bcddbb 100644 --- a/ArduPlane/mode_autotune.cpp +++ b/ArduPlane/mode_autotune.cpp @@ -14,3 +14,10 @@ void ModeAutoTune::update() plane.mode_fbwa.update(); } +void ModeAutoTune::run() +{ + // Run base class function and then output throttle + Mode::run(); + + output_pilot_throttle(); +} diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp index 2d8c4c428c2424..3e16928f15bcab 100644 --- a/ArduPlane/mode_cruise.cpp +++ b/ArduPlane/mode_cruise.cpp @@ -61,7 +61,7 @@ void ModeCruise::navigate() // check if we are moving in the direction of the front of the vehicle const int32_t ground_course_cd = plane.gps.ground_course_cd(); - const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.yaw)) < M_PI_2; + const bool moving_forwards = fabsf(wrap_PI(radians(ground_course_cd * 0.01) - plane.ahrs.get_yaw())) < M_PI_2; if (!locked_heading && plane.channel_roll->get_control_in() == 0 && diff --git a/ArduPlane/mode_fbwa.cpp b/ArduPlane/mode_fbwa.cpp index 35b97ebbe06e80..ff53a835ee49ac 100644 --- a/ArduPlane/mode_fbwa.cpp +++ b/ArduPlane/mode_fbwa.cpp @@ -8,12 +8,12 @@ void ModeFBWA::update() plane.update_load_factor(); float pitch_input = plane.channel_pitch->norm_input(); if (pitch_input > 0) { - plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max_cd; + plane.nav_pitch_cd = pitch_input * plane.aparm.pitch_limit_max*100; } else { - plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min_cd); + plane.nav_pitch_cd = -(pitch_input * plane.pitch_limit_min*100); } plane.adjust_nav_pitch_throttle(); - plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); if (plane.fly_inverted()) { plane.nav_pitch_cd = -plane.nav_pitch_cd; } @@ -35,3 +35,11 @@ void ModeFBWA::update() } } } + +void ModeFBWA::run() +{ + // Run base class function and then output throttle + Mode::run(); + + output_pilot_throttle(); +} diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index a523ddf87065ed..85462166d1bc7f 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -52,7 +52,7 @@ void ModeGuided::update() float error = 0.0f; if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) { - error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().yaw); + error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().get_yaw()); } else { Vector2f groundspeed = AP::ahrs().groundspeed_vector(); error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI); @@ -77,18 +77,26 @@ void ModeGuided::update() if (plane.guided_state.last_forced_rpy_ms.y > 0 && millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) { - plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); } else { plane.calc_nav_pitch(); } - // Received an external msg that guides throttle in the last 3 seconds? - if (plane.aparm.throttle_cruise > 1 && + // Throttle output + if (plane.guided_throttle_passthru) { + // manual passthrough of throttle in fence breach + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true)); + + } else if (plane.aparm.throttle_cruise > 1 && plane.guided_state.last_forced_throttle_ms > 0 && millis() - plane.guided_state.last_forced_throttle_ms < 3000) { + // Received an external msg that guides throttle in the last 3 seconds? SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle); + } else { + // TECS control plane.calc_throttle(); + } } diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index fac2147f93c455..e9e4ede8e62c0f 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -6,8 +6,8 @@ bool ModeQHover::_enter() { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); - pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); + pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); quadplane.set_climb_rate_cms(0); quadplane.init_throttle_wait(); diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 2eea970c58be36..fdadd0e2553951 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -10,8 +10,8 @@ bool ModeQLoiter::_enter() loiter_nav->init_target(); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); - pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); + pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); quadplane.init_throttle_wait(); @@ -66,7 +66,7 @@ void ModeQLoiter::run() quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, quadplane.pilot_accel_z); + pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); // process pilot's roll and pitch input float target_roll_cd, target_pitch_cd; diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 9be1c962bd6a4b..7de44dbc9e030c 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -199,9 +199,9 @@ void ModeQRTL::update_target_altitude() giving time to lose speed before we transition */ const float radius = MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius))); - const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt); + const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude - plane.quadplane.qrtl_alt); const float sink_time = rtl_alt_delta / MAX(0.6*plane.TECS_controller.get_max_sinkrate(), 1); - const float sink_dist = plane.aparm.airspeed_cruise_cm * 0.01 * sink_time; + const float sink_dist = plane.aparm.airspeed_cruise * sink_time; const float dist = plane.auto_state.wp_distance; const float rad_min = 2*radius; const float rad_max = 20*radius; diff --git a/ArduPlane/mode_qstabilize.cpp b/ArduPlane/mode_qstabilize.cpp index 2f78a210fb3c2d..eda421b610c23f 100644 --- a/ArduPlane/mode_qstabilize.cpp +++ b/ArduPlane/mode_qstabilize.cpp @@ -87,12 +87,12 @@ void ModeQStabilize::set_tailsitter_roll_pitch(const float roll_input, const flo void ModeQStabilize::set_limited_roll_pitch(const float roll_input, const float pitch_input) { plane.nav_roll_cd = roll_input * MIN(plane.roll_limit_cd, plane.quadplane.aparm.angle_max); - // pitch is further constrained by LIM_PITCH_MIN/MAX which may impose + // pitch is further constrained by PTCH_LIM_MIN/MAX which may impose // tighter (possibly asymmetrical) limits than Q_ANGLE_MAX if (pitch_input > 0) { - plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max_cd, plane.quadplane.aparm.angle_max); + plane.nav_pitch_cd = pitch_input * MIN(plane.aparm.pitch_limit_max*100, plane.quadplane.aparm.angle_max); } else { - plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min_cd, plane.quadplane.aparm.angle_max); + plane.nav_pitch_cd = pitch_input * MIN(-plane.pitch_limit_min*100, plane.quadplane.aparm.angle_max); } } diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index 417eb6170f22ef..ee851089e20879 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -48,7 +48,7 @@ void ModeRTL::update() bool alt_threshold_reached = false; if (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)) { - // Climb to ALT_HOLD_RTL before turning. This overrides RTL_CLIMB_MIN. + // Climb to RTL_ALTITUDE before turning. This overrides RTL_CLIMB_MIN. alt_threshold_reached = plane.current_loc.alt > plane.next_WP_loc.alt; } else if (plane.g2.rtl_climb_min > 0) { /* diff --git a/ArduPlane/mode_stabilize.cpp b/ArduPlane/mode_stabilize.cpp index b73dfb6fc26506..ee0f7626de8295 100644 --- a/ArduPlane/mode_stabilize.cpp +++ b/ArduPlane/mode_stabilize.cpp @@ -13,4 +13,6 @@ void ModeStabilize::run() plane.stabilize_pitch(); stabilize_stick_mixing_direct(); plane.stabilize_yaw(); + + output_pilot_throttle(); } diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index db212361c398e8..2f70bef07115a3 100644 --- a/ArduPlane/mode_takeoff.cpp +++ b/ArduPlane/mode_takeoff.cpp @@ -17,7 +17,7 @@ const AP_Param::GroupInfo ModeTakeoff::var_info[] = { // @Param: LVL_ALT // @DisplayName: Takeoff mode altitude level altitude - // @Description: This is the altitude below which the wings are held level for TAKEOFF and AUTO modes. Below this altitude, roll demand is restricted to LEVEL_ROLL_LIMIT. Normal-flight roll restriction resumes above TKOFF_LVL_ALT*2 or TKOFF_ALT, whichever is lower. Roll limits are scaled while between those altitudes for a smooth transition. + // @Description: This is the altitude below which the wings are held level for TAKEOFF and AUTO modes. Below this altitude, roll demand is restricted to LEVEL_ROLL_LIMIT. Normal-flight roll restriction resumes above TKOFF_LVL_ALT*3 or TKOFF_ALT, whichever is lower. Roll limits are scaled while between TKOFF_LVL_ALT and those altitudes for a smooth transition. // @Range: 0 50 // @Increment: 1 // @Units: m @@ -62,7 +62,7 @@ ModeTakeoff::ModeTakeoff() : bool ModeTakeoff::_enter() { - takeoff_started = false; + takeoff_mode_setup = false; return true; } @@ -79,28 +79,27 @@ void ModeTakeoff::update() const float alt = target_alt; const float dist = target_dist; - if (!takeoff_started) { + if (!takeoff_mode_setup) { const uint16_t altitude = plane.relative_ground_altitude(false,true); - const float direction = degrees(ahrs.yaw); + const float direction = degrees(ahrs.get_yaw()); // see if we will skip takeoff as already flying if (plane.is_flying() && (millis() - plane.started_flying_ms > 10000U) && ahrs.groundspeed() > 3) { if (altitude >= alt) { gcs().send_text(MAV_SEVERITY_INFO, "Above TKOFF alt - loitering"); plane.next_WP_loc = plane.current_loc; - takeoff_started = true; + takeoff_mode_setup = true; plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } else { gcs().send_text(MAV_SEVERITY_INFO, "Climbing to TKOFF alt then loitering"); plane.next_WP_loc = plane.current_loc; plane.next_WP_loc.alt += ((alt - altitude) *100); plane.next_WP_loc.offset_bearing(direction, dist); - takeoff_started = true; + takeoff_mode_setup = true; plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); } // not flying so do a full takeoff sequence } else { // setup target waypoint and alt for loiter at dist and alt from start - start_loc = plane.current_loc; plane.prev_WP_loc = plane.current_loc; plane.next_WP_loc = plane.current_loc; @@ -116,11 +115,19 @@ void ModeTakeoff::update() if (!plane.throttle_suppressed) { gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg", alt, dist, direction); - takeoff_started = true; + plane.takeoff_state.start_time_ms = millis(); + takeoff_mode_setup = true; + } } } + // check for optional takeoff timeout + if (plane.check_takeoff_timeout()) { + plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL); + takeoff_mode_setup = false; + } + // we finish the initial level takeoff if we climb past // TKOFF_LVL_ALT or we pass the target location. The check for // target location prevents us flying forever if we can't climb diff --git a/ArduPlane/mode_training.cpp b/ArduPlane/mode_training.cpp index 23c81fbcc288f8..eb7040ce6fadea 100644 --- a/ArduPlane/mode_training.cpp +++ b/ArduPlane/mode_training.cpp @@ -20,10 +20,10 @@ void ModeTraining::update() // if the pitch is past the set pitch limits, then // we set target pitch to the limit - if (ahrs.pitch_sensor >= plane.aparm.pitch_limit_max_cd) { - plane.nav_pitch_cd = plane.aparm.pitch_limit_max_cd; - } else if (ahrs.pitch_sensor <= plane.pitch_limit_min_cd) { - plane.nav_pitch_cd = plane.pitch_limit_min_cd; + if (ahrs.pitch_sensor >= plane.aparm.pitch_limit_max*100) { + plane.nav_pitch_cd = plane.aparm.pitch_limit_max*100; + } else if (ahrs.pitch_sensor <= plane.pitch_limit_min*100) { + plane.nav_pitch_cd = plane.pitch_limit_min*100; } else { plane.training_manual_pitch = true; plane.nav_pitch_cd = 0; @@ -66,4 +66,6 @@ void ModeTraining::run() // Always manual rudder control output_rudder_and_steering(plane.rudder_in_expo(false)); + output_pilot_throttle(); + } diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 76846b0fe6ed64..762b190ffee722 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -123,7 +123,7 @@ float Plane::mode_auto_target_airspeed_cm() return land_airspeed * 100; } // fallover to normal airspeed - return aparm.airspeed_cruise_cm; + return aparm.airspeed_cruise*100; } if (quadplane.in_vtol_land_approach()) { return quadplane.get_land_airspeed() * 100; @@ -137,7 +137,7 @@ float Plane::mode_auto_target_airspeed_cm() } // fallover to normal airspeed - return aparm.airspeed_cruise_cm; + return aparm.airspeed_cruise*100; } void Plane::calc_airspeed_errors() @@ -160,7 +160,7 @@ void Plane::calc_airspeed_errors() // FBW_B/cruise airspeed target if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) { if (flight_option_enabled(FlightOptions::CRUISE_TRIM_AIRSPEED)) { - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } else if (flight_option_enabled(FlightOptions::CRUISE_TRIM_THROTTLE)) { float control_min = 0.0f; float control_mid = 0.0f; @@ -175,11 +175,11 @@ void Plane::calc_airspeed_errors() break; } if (control_in <= control_mid) { - target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise_cm, + target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise*100, control_in, control_min, control_mid); } else { - target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise_cm, aparm.airspeed_max * 100, + target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise*100, aparm.airspeed_max * 100, control_in, control_mid, control_max); } @@ -213,7 +213,7 @@ void Plane::calc_airspeed_errors() if (arspd > 0) { target_airspeed_cm = arspd * 100; } else { - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } } else if (control_mode == &mode_auto) { float arspd = g2.soaring_controller.get_cruising_target_airspeed(); @@ -221,7 +221,7 @@ void Plane::calc_airspeed_errors() if (arspd > 0) { target_airspeed_cm = arspd * 100; } else { - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } } #endif @@ -239,7 +239,7 @@ void Plane::calc_airspeed_errors() #endif } else { // Normal airspeed target for all other cases - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; } // Set target to current airspeed + ground speed undershoot, @@ -286,8 +286,8 @@ void Plane::calc_gndspeed_undershoot() if (!yawVect.is_zero()) { yawVect.normalize(); float gndSpdFwd = yawVect * velNED.xy(); - groundspeed_undershoot_is_valid = aparm.min_gndspeed_cm > 0; - groundspeed_undershoot = groundspeed_undershoot_is_valid ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0; + groundspeed_undershoot_is_valid = aparm.min_groundspeed > 0; + groundspeed_undershoot = groundspeed_undershoot_is_valid ? (aparm.min_groundspeed*100 - gndSpdFwd*100) : 0; } } else { groundspeed_undershoot_is_valid = false; diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 141ae621d88bfc..67db319a0e95c9 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -43,14 +43,15 @@ void QAutoTune::init_z_limits() { // set vertical speed and acceleration limits plane.quadplane.pos_control->set_max_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), - plane.quadplane.pilot_velocity_z_max_up, - plane.quadplane.pilot_accel_z); + plane.quadplane.pilot_speed_z_max_up*100, + plane.quadplane.pilot_accel_z*100); plane.quadplane.pos_control->set_correction_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), - plane.quadplane.pilot_velocity_z_max_up, - plane.quadplane.pilot_accel_z); + plane.quadplane.pilot_speed_z_max_up*100, + plane.quadplane.pilot_accel_z*100); } +#if HAL_LOGGING_ENABLED // log VTOL PIDs for during twitch void QAutoTune::log_pids(void) { @@ -58,6 +59,7 @@ void QAutoTune::log_pids(void) AP::logger().Write_PID(LOG_PIQP_MSG, plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); AP::logger().Write_PID(LOG_PIQY_MSG, plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); } +#endif #endif // QAUTOTUNE_ENABLED diff --git a/ArduPlane/qautotune.h b/ArduPlane/qautotune.h index 3ec890ac3dc3f9..938c8e47e43323 100644 --- a/ArduPlane/qautotune.h +++ b/ArduPlane/qautotune.h @@ -26,7 +26,9 @@ class QAutoTune : public AC_AutoTune_Multi float get_pilot_desired_climb_rate_cms(void) const override; void get_pilot_desired_rp_yrate_cd(float &roll_cd, float &pitch_cd, float &yaw_rate_cds) override; void init_z_limits() override; +#if HAL_LOGGING_ENABLED void log_pids() override; +#endif }; #endif // QAUTOTUNE_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 48259c89a4462a..1a4fefe7dc64fe 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -43,32 +43,32 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl), - // @Param: VELZ_MAX + // @Param: PILOT_SPD_UP // @DisplayName: Pilot maximum vertical speed up - // @Description: The maximum ascending vertical velocity the pilot may request in cm/s - // @Units: cm/s - // @Range: 50 500 - // @Increment: 10 + // @Description: The maximum ascending vertical velocity the pilot may request in m/s + // @Units: m/s + // @Range: 0.5 5 + // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("VELZ_MAX", 18, QuadPlane, pilot_velocity_z_max_up, 250), + AP_GROUPINFO("PILOT_SPD_UP", 18, QuadPlane, pilot_speed_z_max_up, 2.50), - // @Param: VELZ_MAX_DN + // @Param: PILOT_SPD_DN // @DisplayName: Pilot maximum vertical speed down - // @Description: The maximum vertical velocity the pilot may request in cm/s going down. If 0, uses Q_VELZ_MAX value. - // @Units: cm/s - // @Range: 50 500 - // @Increment: 10 + // @Description: The maximum vertical velocity the pilot may request in m/s going down. If 0, uses Q_PILOT_SPD_UP value. + // @Units: m/s + // @Range: 0.5 5 + // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("VELZ_MAX_DN", 60, QuadPlane, pilot_velocity_z_max_dn, 0), + AP_GROUPINFO("PILOT_SPD_DN", 60, QuadPlane, pilot_speed_z_max_dn, 0), - // @Param: ACCEL_Z + // @Param: PILOT_ACCEL_Z // @DisplayName: Pilot vertical acceleration // @Description: The vertical acceleration used when pilot is controlling the altitude - // @Units: cm/s/s - // @Range: 50 500 - // @Increment: 10 + // @Units: m/s/s + // @Range: 0.5 5 + // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("ACCEL_Z", 19, QuadPlane, pilot_accel_z, 250), + AP_GROUPINFO("PILOT_ACCEL_Z", 19, QuadPlane, pilot_accel_z, 2.5), // @Group: WP_ // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp @@ -120,14 +120,14 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // YAW_RATE_MAX index 25 - // @Param: LAND_SPEED - // @DisplayName: Land speed - // @Description: The descent speed for the final stage of landing in cm/s - // @Units: cm/s - // @Range: 30 200 - // @Increment: 10 + // @Param: LAND_FINAL_SPD + // @DisplayName: Land final speed + // @Description: The descent speed for the final stage of landing in m/s + // @Units: m/s + // @Range: 0.3 2 + // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("LAND_SPEED", 26, QuadPlane, land_speed_cms, 50), + AP_GROUPINFO("LAND_FINAL_SPD", 26, QuadPlane, land_final_speed, 0.5), // @Param: LAND_FINAL_ALT // @DisplayName: Land final altitude @@ -310,7 +310,7 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = { // @Param: TRIM_PITCH // @DisplayName: Quadplane AHRS trim pitch - // @Description: This sets the compensation for the pitch angle trim difference between calibrated AHRS level and vertical flight pitch. NOTE! this is relative to calibrated AHRS trim, not forward flight trim which includes TRIM_PITCH_CD. For tailsitters, this is relative to a baseline of 90 degrees in AHRS. + // @Description: This sets the compensation for the pitch angle trim difference between calibrated AHRS level and vertical flight pitch. NOTE! this is relative to calibrated AHRS trim, not forward flight trim which includes TRIM_PITCH. For tailsitters, this is relative to a baseline of 90 degrees in AHRS. // @Units: deg // @Range: -10 +10 // @Increment: 0.1 @@ -823,6 +823,12 @@ bool QuadPlane::setup(void) AP_Param::convert_old_parameters(&q_conversion_table[0], ARRAY_SIZE(q_conversion_table)); + // centi-conversions added January 2024 + land_final_speed.convert_centi_parameter(AP_PARAM_INT16); + pilot_speed_z_max_up.convert_centi_parameter(AP_PARAM_INT16); + pilot_speed_z_max_dn.convert_centi_parameter(AP_PARAM_INT16); + pilot_accel_z.convert_centi_parameter(AP_PARAM_INT16); + tailsitter.setup(); tiltrotor.setup(); @@ -1018,7 +1024,7 @@ void QuadPlane::run_z_controller(void) } if ((now - last_pidz_active_ms) > 20 || !pos_control->is_active_z()) { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); // initialise the vertical position controller if (!tailsitter.enabled()) { @@ -1053,7 +1059,7 @@ void QuadPlane::check_yaw_reset(void) if (new_ekfYawReset_ms != ekfYawReset_ms) { attitude_control->inertial_frame_reset(); ekfYawReset_ms = new_ekfYawReset_ms; - AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); + LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET); } } @@ -1071,7 +1077,7 @@ void QuadPlane::hold_hover(float target_climb_rate_cms) set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); // call attitude controller multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false)); @@ -1246,7 +1252,7 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground) height_above_ground = MIN(height_above_ground, land_final_alt); } const float max_climb_speed = wp_nav->get_default_speed_up(); - float ret = linear_interpolate(land_speed_cms, wp_nav->get_default_speed_down(), + float ret = linear_interpolate(land_final_speed*100, wp_nav->get_default_speed_down(), height_above_ground, land_final_alt, land_final_alt+6); @@ -1357,7 +1363,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const uint16_t dead_zone = plane.channel_throttle->get_dead_zone(); uint16_t trim = (plane.channel_throttle->get_radio_max() + plane.channel_throttle->get_radio_min())/2; const float throttle_request = plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) *0.01f; - return throttle_request * (throttle_request > 0.0f ? pilot_velocity_z_max_up : get_pilot_velocity_z_max_dn()); + return throttle_request * (throttle_request > 0.0f ? pilot_speed_z_max_up*100 : get_pilot_velocity_z_max_dn()); } @@ -1405,7 +1411,7 @@ float QuadPlane::assist_climb_rate_cms(void) const climb_rate = plane.altitude_error_cm * 0.1f; } else { // otherwise estimate from pilot input - climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd); + climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(plane.aparm.pitch_limit_max*100)); climb_rate *= plane.get_throttle_input(); } climb_rate = constrain_float(climb_rate, -wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); @@ -1496,7 +1502,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) // we've been below assistant alt for Q_ASSIST_DELAY seconds if (!in_alt_assist) { in_alt_assist = true; - gcs().send_text(MAV_SEVERITY_INFO, "Alt assist %.1fm", height_above_ground); + gcs().send_text(MAV_SEVERITY_WARNING, "Alt assist %.1fm", height_above_ground); } return true; } @@ -1517,9 +1523,9 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) */ const uint16_t allowed_envelope_error_cd = 500U; - if (labs(ahrs.roll_sensor) <= plane.aparm.roll_limit_cd+allowed_envelope_error_cd && - ahrs.pitch_sensor < plane.aparm.pitch_limit_max_cd+allowed_envelope_error_cd && - ahrs.pitch_sensor > -(allowed_envelope_error_cd-plane.aparm.pitch_limit_min_cd)) { + if (labs(ahrs.roll_sensor) <= plane.aparm.roll_limit*100 + allowed_envelope_error_cd && + ahrs.pitch_sensor < plane.aparm.pitch_limit_max*100+allowed_envelope_error_cd && + ahrs.pitch_sensor > -(allowed_envelope_error_cd-plane.aparm.pitch_limit_min*100)) { // we are inside allowed attitude envelope in_angle_assist = false; angle_error_start_ms = 0; @@ -1541,7 +1547,7 @@ bool QuadPlane::should_assist(float aspeed, bool have_airspeed) bool ret = (now - angle_error_start_ms) >= assist_delay*1000; if (ret && !in_angle_assist) { in_angle_assist = true; - gcs().send_text(MAV_SEVERITY_INFO, "Angle assist r=%d p=%d", + gcs().send_text(MAV_SEVERITY_WARNING, "Angle assist r=%d p=%d", (int)(ahrs.roll_sensor/100), (int)(ahrs.pitch_sensor/100)); } @@ -1621,7 +1627,7 @@ void SLT_Transition::update() gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit"); quadplane.transition_failure.warned = true; } - // if option is set and ground speed> 1/2 arspd_fbw_min for non-tiltrotors, then complete transition, otherwise QLAND. + // if option is set and ground speed> 1/2 AIRSPEED_MIN for non-tiltrotors, then complete transition, otherwise QLAND. // tiltrotors will immediately transition const bool tiltrotor_with_ground_speed = quadplane.tiltrotor.enabled() && (plane.ahrs.groundspeed() > plane.aparm.airspeed_min * 0.5); if (quadplane.option_is_set(QuadPlane::OPTION::TRANS_FAIL_TO_FW) && tiltrotor_with_ground_speed) { @@ -1866,6 +1872,16 @@ void QuadPlane::update(void) tiltrotor.update(); + if (in_vtol_mode()) { + // if enabled output forward throttle else 0 + float fwd_thr = 0; + if (allow_forward_throttle_in_vtol_mode()) { + fwd_thr = forward_throttle_pct(); + } + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr); + } + +#if HAL_LOGGING_ENABLED // motors logging if (motors->armed()) { const bool motors_active = in_vtol_mode() || assisted_flight; @@ -1886,7 +1902,9 @@ void QuadPlane::update(void) Log_Write_QControl_Tuning(); } } - +#else + (void)now; +#endif // HAL_LOGGING_ENABLED } /* @@ -2296,6 +2314,7 @@ void QuadPlane::poscontrol_init_approach(void) poscontrol.slow_descent = false; } +#if HAL_LOGGING_ENABLED /* log the QPOS message */ @@ -2309,6 +2328,7 @@ void QuadPlane::log_QPOS(void) poscontrol.target_accel, poscontrol.overshoot); } +#endif /* change position control state @@ -2346,9 +2366,13 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s) qp.landing_detect.lower_limit_start_ms = 0; } // double log to capture the state change +#if HAL_LOGGING_ENABLED qp.log_QPOS(); +#endif state = s; +#if HAL_LOGGING_ENABLED qp.log_QPOS(); +#endif last_log_ms = now; overshoot = false; } @@ -2447,7 +2471,7 @@ void QuadPlane::vtol_position_controller(void) // use TECS for pitch int32_t commanded_pitch = plane.TECS_controller.get_pitch_demand(); - plane.nav_pitch_cd = constrain_int32(commanded_pitch, plane.pitch_limit_min_cd, plane.aparm.pitch_limit_max_cd.get()); + plane.nav_pitch_cd = constrain_int32(commanded_pitch, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100); if (poscontrol.get_state() == QPOS_AIRBRAKE) { // don't allow down pitch in airbrake plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, 0); @@ -2636,7 +2660,7 @@ void QuadPlane::vtol_position_controller(void) target_speed_xy_cms = diff_wp_norm * target_speed * 100; target_accel_cms = diff_wp_norm * (-target_accel*100); target_yaw_deg = degrees(diff_wp_norm.angle()); - const float yaw_err_deg = wrap_180(target_yaw_deg - degrees(plane.ahrs.yaw)); + const float yaw_err_deg = wrap_180(target_yaw_deg - degrees(plane.ahrs.get_yaw())); bool overshoot = (closing_groundspeed < 0 || fabsf(yaw_err_deg) > 60); if (overshoot && !poscontrol.overshoot) { gcs().send_text(MAV_SEVERITY_INFO,"VTOL Overshoot d=%.1f cs=%.1f yerr=%.1f", @@ -2926,11 +2950,13 @@ void QuadPlane::vtol_position_controller(void) run_z_controller(); } +#if HAL_LOGGING_ENABLED if (now_ms - poscontrol.last_log_ms >= 40) { // log poscontrol at 25Hz poscontrol.last_log_ms = now_ms; log_QPOS(); } +#endif } /* @@ -3025,6 +3051,7 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { // acceleration capability. const float nav_pitch_lower_limit_cd = - (int32_t)((float)aparm.angle_max * (1.0f - fwd_thr_scaler) + q_fwd_pitch_lim_cd * fwd_thr_scaler); +#if HAL_LOGGING_ENABLED // Diagnostics logging - remove when feature is fully flight tested. AP::logger().WriteStreaming("FWDT", "TimeUS,fts,qfplcd,npllcd,npcd,qft", // labels @@ -3035,6 +3062,7 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { (double)nav_pitch_lower_limit_cd, (double)plane.nav_pitch_cd, (double)q_fwd_throttle); +#endif plane.nav_pitch_cd = MAX(plane.nav_pitch_cd, (int32_t)nav_pitch_lower_limit_cd); } @@ -3046,7 +3074,7 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) { */ float QuadPlane::get_scaled_wp_speed(float target_bearing_deg) const { - const float yaw_difference = fabsf(wrap_180(degrees(plane.ahrs.yaw) - target_bearing_deg)); + const float yaw_difference = fabsf(wrap_180(degrees(plane.ahrs.get_yaw()) - target_bearing_deg)); const float wp_speed = wp_nav->get_default_speed_xy() * 0.01; if (yaw_difference > 20) { // this gives a factor of 2x reduction in max speed when @@ -3080,8 +3108,8 @@ void QuadPlane::setup_target_position(void) poscontrol.target_cm.z = plane.next_WP_loc.alt - origin.alt; // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); - pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); } /* @@ -3330,8 +3358,8 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) throttle_wait = false; // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); - pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); // initialise the vertical position controller pos_control->init_z_controller(); @@ -3348,8 +3376,8 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) // t_{constant} = \frac{d_{remaining}}{V_z} // t = max(t_{accel}, 0) + max(t_{constant}, 0) const float d_total = (plane.next_WP_loc.alt - plane.current_loc.alt) * 0.01f; - const float accel_m_s_s = MAX(10, pilot_accel_z) * 0.01f; - const float vel_max = MAX(10, pilot_velocity_z_max_up) * 0.01f; + const float accel_m_s_s = MAX(0.1, pilot_accel_z); + const float vel_max = MAX(0.1, pilot_speed_z_max_up); const float vel_z = inertial_nav.get_velocity_z_up_cms() * 0.01f; const float t_accel = (vel_max - vel_z) / accel_m_s_s; const float d_accel = vel_z * t_accel + 0.5f * accel_m_s_s * sq(t_accel); @@ -3640,6 +3668,7 @@ bool QuadPlane::verify_vtol_land(void) return false; } +#if HAL_LOGGING_ENABLED // Write a control tuning packet void QuadPlane::Log_Write_QControl_Tuning() { @@ -3663,7 +3692,6 @@ void QuadPlane::Log_Write_QControl_Tuning() target_climb_rate : target_climb_rate_cms, climb_rate : int16_t(inertial_nav.get_velocity_z_up_cms()), throttle_mix : attitude_control->get_throttle_mix(), - speed_scaler : tailsitter.log_spd_scaler, transition_state : transition->get_log_transition_state(), assist : assisted_flight, }; @@ -3672,6 +3700,7 @@ void QuadPlane::Log_Write_QControl_Tuning() // write multicopter position control message pos_control->write_log(); } +#endif /* @@ -3754,7 +3783,7 @@ float QuadPlane::forward_throttle_pct() // add in a component from our current pitch demand. This tends to // move us to zero pitch. Assume that LIM_PITCH would give us the // WP nav speed. - fwd_vel_error -= (wp_nav->get_default_speed_xy() * 0.01f) * plane.nav_pitch_cd / (float)plane.aparm.pitch_limit_max_cd; + fwd_vel_error -= (wp_nav->get_default_speed_xy() * 0.01f) * plane.nav_pitch_cd / (plane.aparm.pitch_limit_max*100); if (should_relax() && vel_ned.length() < 1) { // we may be landed @@ -4034,7 +4063,7 @@ float QuadPlane::stopping_distance(void) float QuadPlane::transition_threshold(void) { // 1.5 times stopping distance for cruise speed - return 1.5 * stopping_distance(sq(plane.aparm.airspeed_cruise_cm*0.01)); + return 1.5 * stopping_distance(sq(plane.aparm.airspeed_cruise)); } #define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing @@ -4197,13 +4226,16 @@ bool SLT_Transition::show_vtol_view() const return quadplane.in_vtol_mode(); } -// return the PILOT_VELZ_MAX_DN value if non zero, otherwise returns the PILOT_VELZ_MAX value. +/* + return the PILOT_VELZ_MAX_DN value if non zero, otherwise returns the PILOT_VELZ_MAX value. + return is in cm/s +*/ uint16_t QuadPlane::get_pilot_velocity_z_max_dn() const { - if (pilot_velocity_z_max_dn == 0) { - return abs(pilot_velocity_z_max_up); + if (is_zero(pilot_speed_z_max_dn)) { + return abs(pilot_speed_z_max_up*100); } - return abs(pilot_velocity_z_max_dn); + return abs(pilot_speed_z_max_dn*100); } /* @@ -4256,7 +4288,7 @@ Vector2f QuadPlane::landing_desired_closing_velocity() // don't let the target speed go above landing approach speed const float eas2tas = plane.ahrs.get_EAS2TAS(); - float land_speed = plane.aparm.airspeed_cruise_cm * 0.01; + float land_speed = plane.aparm.airspeed_cruise; float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); if (is_positive(tecs_land_airspeed)) { land_speed = tecs_land_airspeed; @@ -4280,7 +4312,7 @@ float QuadPlane::get_land_airspeed(void) const auto qstate = poscontrol.get_state(); if (qstate == QPOS_APPROACH || plane.control_mode == &plane.mode_rtl) { - const float cruise_speed = plane.aparm.airspeed_cruise_cm * 0.01; + const float cruise_speed = plane.aparm.airspeed_cruise; float approach_speed = cruise_speed; float tecs_land_airspeed = plane.TECS_controller.get_land_airspeed(); if (is_positive(tecs_land_airspeed)) { @@ -4446,7 +4478,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ } // we limit pitch during initial transition - const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max,plane.aparm.pitch_limit_max_cd), + const float max_limit_cd = linear_interpolate(MAX(last_fw_nav_pitch_cd,0), MIN(angle_max,plane.aparm.pitch_limit_max*100), dt, 0, limit_time_ms); @@ -4466,7 +4498,7 @@ bool SLT_Transition::set_VTOL_roll_pitch_limit(int32_t& roll_cd, int32_t& pitch_ to prevent inability to progress to position if moving from a loiter to landing */ - const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max,plane.aparm.pitch_limit_min_cd), + const float min_limit_cd = linear_interpolate(MIN(last_fw_nav_pitch_cd,0), MAX(-angle_max,plane.aparm.pitch_limit_min*100), dt, 0, limit_time_ms); @@ -4641,7 +4673,7 @@ bool QuadPlane::landing_with_fixed_wing_spiral_approach(void) const setup scaling of roll and pitch angle P gains to match fixed wing gains we setup the angle P gain to match fixed wing at high speed (above - ARSPD_FBW_MIN) where fixed wing surfaces are presumed to + AIRSPEED_MIN) where fixed wing surfaces are presumed to dominate. At lower speeds we use the multicopter angle P gains. */ void QuadPlane::setup_rp_fw_angle_gains(void) @@ -4743,4 +4775,10 @@ float QuadPlane::get_throttle_input() const return ret; } +// return true if forward throttle from forward_throttle_pct() should be used +bool QuadPlane::allow_forward_throttle_in_vtol_mode() const +{ + return in_vtol_mode() && motors->armed() && (motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN); +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ad5e6251af2df8..63a0cb235a2725 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -156,7 +156,6 @@ class QuadPlane int16_t target_climb_rate; int16_t climb_rate; float throttle_mix; - float speed_scaler; uint8_t transition_state; uint8_t assist; }; @@ -214,11 +213,11 @@ class QuadPlane AC_Loiter *loiter_nav; // maximum vertical velocity the pilot may request - AP_Int16 pilot_velocity_z_max_up; - AP_Int16 pilot_velocity_z_max_dn; + AP_Float pilot_speed_z_max_up; + AP_Float pilot_speed_z_max_dn; // vertical acceleration the pilot may request - AP_Int16 pilot_accel_z; + AP_Float pilot_accel_z; // air mode state: OFF, ON, ASSISTED_FLIGHT_ONLY AirMode air_mode; @@ -350,8 +349,8 @@ class QuadPlane uint32_t alt_error_start_ms; bool in_alt_assist; - // landing speed in cm/s - AP_Int16 land_speed_cms; + // landing speed in m/s + AP_Float land_final_speed; // QRTL start altitude, meters AP_Int16 qrtl_alt; @@ -730,6 +729,11 @@ class QuadPlane */ void setup_rp_fw_angle_gains(void); + /* + return true if forward throttle from forward_throttle_pct() should be used + */ + bool allow_forward_throttle_in_vtol_mode() const; + public: void motor_test_output(); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 68ee2cb9986f42..6f5ea8daeb62bb 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -197,7 +197,7 @@ void Plane::read_radio() && stickmixing) { float nudge = (channel_throttle->get_control_in() - 50) * 0.02f; if (ahrs.using_airspeed_sensor()) { - airspeed_nudge_cm = (aparm.airspeed_max * 100 - aparm.airspeed_cruise_cm) * nudge; + airspeed_nudge_cm = (aparm.airspeed_max - aparm.airspeed_cruise) * nudge * 100; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; } diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 6048aed8042a02..c0cab9f9de4f41 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -384,23 +384,26 @@ void Plane::set_servos_idle(void) /* - Scale the throttle to conpensate for battery voltage drop + Calculate the throttle scale to compensate for battery voltage drop */ -void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) const +void ParametersG2::FWD_BATT_CMP::update() { + // Assume disabled + enabled = false; + // return if not enabled, or setup incorrectly - if (!is_positive(g2.fwd_thr_batt_voltage_min) || g2.fwd_thr_batt_voltage_min >= g2.fwd_thr_batt_voltage_max) { + if (!is_positive(batt_voltage_min) || batt_voltage_min >= batt_voltage_max) { return; } - float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(g2.fwd_thr_batt_idx); + float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(batt_idx); // Return for a very low battery - if (batt_voltage_resting_estimate < 0.25f * g2.fwd_thr_batt_voltage_min) { + if (batt_voltage_resting_estimate < 0.25f * batt_voltage_min) { return; } // constrain read voltage to min and max params - batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate,g2.fwd_thr_batt_voltage_min,g2.fwd_thr_batt_voltage_max); + batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate, batt_voltage_min, batt_voltage_max); // don't apply compensation if the voltage is excessively low if (batt_voltage_resting_estimate < 1) { @@ -409,14 +412,38 @@ void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) co // Scale the throttle up to compensate for voltage drop // Ratio = 1 when voltage = voltage max, ratio increases as voltage drops - const float ratio = g2.fwd_thr_batt_voltage_max / batt_voltage_resting_estimate; + ratio = batt_voltage_max / batt_voltage_resting_estimate; + + // Got this far then ratio is valid + enabled = true; +} + +// Apply throttle scale to min and max limits +void ParametersG2::FWD_BATT_CMP::apply_min_max(int8_t &min_throttle, int8_t &max_throttle) const +{ + // return if not enabled + if (!enabled) { + return; + } // Scale the throttle limits to prevent subsequent clipping + // Ratio will always be >= 1, ensure still within max limits min_throttle = int8_t(MAX((ratio * (float)min_throttle), -100)); max_throttle = int8_t(MIN((ratio * (float)max_throttle), 100)); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100)); +} + +// Apply throttle scale to throttle demand +float ParametersG2::FWD_BATT_CMP::apply_throttle(float throttle) const +{ + // return if not enabled + if (!enabled) { + return throttle; + } + + // Ratio will always be >= 1, ensure still within max limits + return constrain_float(throttle * ratio, -100, 100); + } /* @@ -468,17 +495,12 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) } } #endif // #if AP_BATTERY_WATT_MAX_ENABLED - + /* - setup output channels all non-manual modes + Apply min/max limits to throttle */ -void Plane::set_servos_controlled(void) +float Plane::apply_throttle_limits(float throttle_in) { - if (flight_stage == AP_FixedWing::FlightStage::LAND) { - // allow landing to override servos if it would like to - landing.override_servos(); - } - // convert 0 to 100% (or -100 to +100) into PWM int8_t min_throttle = aparm.throttle_min.get(); int8_t max_throttle = aparm.throttle_max.get(); @@ -509,13 +531,22 @@ void Plane::set_servos_controlled(void) } // compensate for battery voltage drop - throttle_voltage_comp(min_throttle, max_throttle); + g2.fwd_batt_cmp.apply_min_max(min_throttle, max_throttle); #if AP_BATTERY_WATT_MAX_ENABLED // apply watt limiter throttle_watt_limiter(min_throttle, max_throttle); #endif + return constrain_float(throttle_in, min_throttle, max_throttle); +} + +/* + setup output channels all non-manual modes + */ +void Plane::set_throttle(void) +{ + if (!arming.is_armed_and_safety_off()) { // Always set 0 scaled even if overriding to zero pwm. // This ensures slew limits and other functions using the scaled value pick up in the correct place @@ -528,7 +559,10 @@ void Plane::set_servos_controlled(void) SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); } - } else if (suppress_throttle()) { + return; + } + + if (suppress_throttle()) { if (g.throttle_suppress_manual) { // manual pass through of throttle while throttle is suppressed SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); @@ -542,47 +576,29 @@ void Plane::set_servos_controlled(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); } + return; + } + + // Update voltage scaling + g2.fwd_batt_cmp.update(); + #if AP_SCRIPTING_ENABLED - } else if (nav_scripting_active()) { + if (nav_scripting_active()) { SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.nav_scripting.throttle_pct); + } #endif - } else if (control_mode == &mode_stabilize || - control_mode == &mode_training || - control_mode == &mode_acro || - control_mode == &mode_fbwa || - control_mode == &mode_autotune) { - // a manual throttle mode - if (g.throttle_passthru_stabilize) { - // manual pass through of throttle while in FBWA or - // STABILIZE mode with THR_PASS_STAB set - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); - } else { - // get throttle, but adjust center to output TRIM_THROTTLE if flight option set - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_int16(get_adjusted_throttle_input(true), min_throttle, max_throttle)); - } - } else if (control_mode->is_guided_mode() && - guided_throttle_passthru) { - // manual pass through of throttle while in GUIDED - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true)); -#if HAL_QUADPLANE_ENABLED - } else if (quadplane.in_vtol_mode()) { - float fwd_thr = 0; - // if armed and not spooled down ask quadplane code for forward throttle - if (quadplane.motors->armed() && - quadplane.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::SHUT_DOWN) { - - fwd_thr = constrain_float(quadplane.forward_throttle_pct(), min_throttle, max_throttle); - } - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr); -#endif // HAL_QUADPLANE_ENABLED - } else { - // Apply min/max limits to throttle output from flight mode - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, - constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); + if (control_mode->use_battery_compensation()) { + // Apply voltage compensation to throttle output from flight mode + const float throttle = g2.fwd_batt_cmp.apply_throttle(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle); } + if (control_mode->use_throttle_limits()) { + // Apply min/max throttle limits + const float limited_throttle = apply_throttle_limits(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle)); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, limited_throttle); + } } /* @@ -829,11 +845,18 @@ void Plane::set_servos(void) quadplane.update(); #endif + if (flight_stage == AP_FixedWing::FlightStage::LAND) { + // allow landing to override servos if it would like to + landing.override_servos(); + } + if (control_mode != &mode_manual) { - set_servos_controlled(); - set_takeoff_expected(); + set_throttle(); } + // Warn AHRS if we might take off soon + set_takeoff_expected(); + // setup flap outputs set_servos_flaps(); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 7c7066bef06b60..56998c367904db 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -17,20 +17,8 @@ static void failsafe_check_static() void Plane::init_ardupilot() { -#if STATS_ENABLED == ENABLED - // initialise stats module - g2.stats.init(); -#endif - ins.set_log_raw_bit(MASK_LOG_IMU_RAW); - // setup any board specific drivers - BoardConfig.init(); - -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - can_mgr.init(); -#endif - rollController.convert_pid(); pitchController.convert_pid(); @@ -76,7 +64,7 @@ void Plane::init_ardupilot() osd.init(); #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED log_init(); #endif @@ -182,16 +170,12 @@ void Plane::startup_ground(void) mission.init(); // initialise AP_Logger library -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED logger.setVehicle_Startup_Writer( FUNCTOR_BIND(&plane, &Plane::Log_Write_Vehicle_Startup_Messages, void) ); #endif -#if AP_SCRIPTING_ENABLED - g2.scripting.init(); -#endif // AP_SCRIPTING_ENABLED - // reset last heartbeat time, so we don't trigger failsafe on slow // startup gcs().sysid_myggcs_seen(AP_HAL::millis()); @@ -341,7 +325,9 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason) old_mode.exit(); // log and notify mode change +#if HAL_LOGGING_ENABLED logger.Write_Mode(control_mode->mode_number(), control_mode_reason); +#endif notify_mode(*control_mode); gcs().send_message(MSG_HEARTBEAT); @@ -466,17 +452,15 @@ void Plane::notify_mode(const Mode& mode) notify.set_flight_mode_str(mode.name4()); } +#if HAL_LOGGING_ENABLED /* should we log a message type now? */ bool Plane::should_log(uint32_t mask) { -#if LOGGING_ENABLED == ENABLED return logger.should_log(mask); -#else - return false; -#endif } +#endif /* return throttle percentage from 0 to 100 for normal use and -100 to 100 when using reverse thrust diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index f40fa33d1f26eb..4e28d534440a3e 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -177,8 +177,8 @@ static const struct AP_Param::defaults_table_struct defaults_table_tailsitter[] { "Q_A_RAT_YAW_FF", 0.2 }, { "Q_A_RAT_YAW_I", 0.18 }, { "Q_A_ANGLE_BOOST", 0 }, - { "LIM_PITCH_MAX", 3000 }, - { "LIM_PITCH_MIN", -3000 }, + { "PTCH_LIM_MAX_DEG", 30 }, + { "PTCH_LIM_MIN_DEG", -30 }, { "MIXING_GAIN", 1.0 }, { "RUDD_DT_GAIN", 10 }, { "Q_TRANSITION_MS", 2000 }, @@ -755,9 +755,6 @@ void Tailsitter::speed_scaling(void) spd_scaler /= plane.barometer.get_air_density_ratio(); } - // record for QTUN log - log_spd_scaler = spd_scaler; - const SRV_Channel::Aux_servo_function_t functions[] = { SRV_Channel::Aux_servo_function_t::k_aileron, SRV_Channel::Aux_servo_function_t::k_elevator, @@ -778,7 +775,32 @@ void Tailsitter::speed_scaling(void) if (tailsitter_motors != nullptr) { tailsitter_motors->set_min_throttle(disk_loading_min_throttle); } + + // Record for log + log_data.throttle_scaler = throttle_scaler; + log_data.speed_scaler = spd_scaler; + log_data.min_throttle = disk_loading_min_throttle; + +} + +#if HAL_LOGGING_ENABLED +// Write tailsitter specific log +void Tailsitter::write_log() +{ + if (!enabled()) { + return; + } + + struct log_tailsitter pkt = { + LOG_PACKET_HEADER_INIT(LOG_TSIT_MSG), + time_us : AP_HAL::micros64(), + throttle_scaler : log_data.throttle_scaler, + speed_scaler : log_data.speed_scaler, + min_throttle : log_data.min_throttle, + }; + plane.logger.WriteBlock(&pkt, sizeof(pkt)); } +#endif // HAL_LOGGING_ENABLED // return true if pitch control should be relaxed // on vectored belly sitters the pitch control is not relaxed in order to keep motors pointing and avoid risk of props hitting the ground diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h index 4f2ff33c3ba370..fbf26702f8ca9f 100644 --- a/ArduPlane/tailsitter.h +++ b/ArduPlane/tailsitter.h @@ -17,6 +17,7 @@ #include #include "transition.h" #include +#include class QuadPlane; class AP_MotorsMulticopter; @@ -66,9 +67,11 @@ friend class Plane; // return true if pitch control should be relaxed bool relax_pitch(); + // Write tailsitter specific log + void write_log(); + // tailsitter speed scaler float last_spd_scaler = 1.0f; // used to slew rate limiting with TAILSITTER_GSCL_ATT_THR option - float log_spd_scaler; // for QTUN log static const struct AP_Param::GroupInfo var_info[]; @@ -112,6 +115,23 @@ friend class Plane; private: + // Tailsitter specific log message + struct PACKED log_tailsitter { + LOG_PACKET_HEADER; + uint64_t time_us; + float throttle_scaler; + float speed_scaler; + float min_throttle; + }; + + // Data to be logged + struct { + float throttle_scaler; + float speed_scaler; + float min_throttle; + } log_data; + + bool setup_complete; // true when flying a tilt-vectored tailsitter diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index ce2264f8c800cc..faa220412fd914 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -158,7 +158,7 @@ void Plane::takeoff_calc_roll(void) takeoff_roll_limit_cd = g.level_roll_limit * 100; } else { // lim1 - below altitude TKOFF_LVL_ALT, restrict roll to LEVEL_ROLL_LIMIT - // lim2 - above altitude (TKOFF_LVL_ALT * 3) allow full flight envelope of LIM_ROLL_CD + // lim2 - above altitude (TKOFF_LVL_ALT * 3) allow full flight envelope of ROLL_LIMIT_DEG // In between lim1 and lim2 use a scaled roll limit. // The *3 scheme should scale reasonably with both small and large aircraft const float lim1 = MAX(mode_takeoff.level_alt, 0); @@ -194,7 +194,7 @@ void Plane::takeoff_calc_pitch(void) } else { if (g.takeoff_rotate_speed > 0) { // Rise off ground takeoff so delay rotation until ground speed indicates adequate airspeed - nav_pitch_cd = ((gps.ground_speed()*100) / (float)aparm.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd; + nav_pitch_cd = (gps.ground_speed() / (float)aparm.airspeed_cruise) * auto_state.takeoff_pitch_cd; nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd); } else { // Doing hand or catapult launch so need at least 5 deg pitch to prevent initial height loss @@ -305,3 +305,29 @@ void Plane::landing_gear_update(void) g2.landing_gear.update(relative_ground_altitude(g.rangefinder_landing)); } #endif + +/* + check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout +*/ + +bool Plane::check_takeoff_timeout(void) +{ + if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) { + const float ground_speed = AP::gps().ground_speed(); + const float takeoff_min_ground_speed = 4; + if (ground_speed >= takeoff_min_ground_speed) { + takeoff_state.start_time_ms = 0; + return false; + } else { + uint32_t now = AP_HAL::millis(); + if (now - takeoff_state.start_time_ms > (uint32_t)(1000U * g2.takeoff_timeout)) { + gcs().send_text(MAV_SEVERITY_INFO, "Takeoff timeout: %.1f m/s speed < 4m/s", ground_speed); + arming.disarm(AP_Arming::Method::TAKEOFFTIMEOUT); + takeoff_state.start_time_ms = 0; + return true; + } + } + } + return false; +} + diff --git a/ArduPlane/version.h b/ArduPlane/version.h index cdf466f7155fb6..b4c2e5cad65941 100644 --- a/ArduPlane/version.h +++ b/ArduPlane/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduPlane V4.5.0-dev" +#define THISFIRMWARE "ArduPlane V4.6.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 6 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/ArduSub/AP_Arming_Sub.cpp b/ArduSub/AP_Arming_Sub.cpp index 66dfecff2b37bd..463d78a4eaabcf 100644 --- a/ArduSub/AP_Arming_Sub.cpp +++ b/ArduSub/AP_Arming_Sub.cpp @@ -91,8 +91,10 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) return false; } +#if HAL_LOGGING_ENABLED // let logger know that we're armed (it may open logs e.g.) AP::logger().set_vehicle_armed(true); +#endif // disable cpu failsafe because initialising everything takes a while sub.mainloop_failsafe_disable(); @@ -133,8 +135,10 @@ bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks) // finally actually arm the motors sub.motors.armed(true); +#if HAL_LOGGING_ENABLED // log flight mode in case it was changed while vehicle was disarmed AP::logger().Write_Mode((uint8_t)sub.control_mode, sub.control_mode_reason); +#endif // reenable failsafe sub.mainloop_failsafe_enable(); @@ -182,7 +186,9 @@ bool AP_Arming_Sub::disarm(const AP_Arming::Method method, bool do_disarm_checks // reset the mission sub.mission.reset(); +#if HAL_LOGGING_ENABLED AP::logger().set_vehicle_armed(false); +#endif hal.util->set_soft_armed(false); diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index e8012d2e91f612..f75ff5f9dc87dc 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -89,11 +89,15 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { #if AP_CAMERA_ENABLED SCHED_TASK_CLASS(AP_Camera, &sub.camera, update, 50, 75, 48), #endif +#if HAL_LOGGING_ENABLED SCHED_TASK(ten_hz_logging_loop, 10, 350, 51), SCHED_TASK(twentyfive_hz_logging, 25, 110, 54), SCHED_TASK_CLASS(AP_Logger, &sub.logger, periodic_tasks, 400, 300, 57), +#endif SCHED_TASK_CLASS(AP_InertialSensor, &sub.ins, periodic, 400, 50, 60), +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Scheduler, &sub.scheduler, update_logging, 0.1, 75, 63), +#endif #if AP_RPM_ENABLED SCHED_TASK_CLASS(AP_RPM, &sub.rpm_sensor, update, 10, 200, 66), #endif @@ -101,7 +105,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = { #if AP_GRIPPER_ENABLED SCHED_TASK_CLASS(AP_Gripper, &sub.g2.gripper, update, 10, 75, 75), #endif -#if STATS_ENABLED == ENABLED +#if AP_STATS_ENABLED SCHED_TASK(stats_update, 1, 200, 76), #endif #ifdef USERHOOK_FASTLOOP @@ -177,6 +181,7 @@ void Sub::update_batt_compass() } } +#if HAL_LOGGING_ENABLED // ten_hz_logging_loop // should be run at 10hz void Sub::ten_hz_logging_loop() @@ -237,6 +242,7 @@ void Sub::twentyfive_hz_logging() AP::ins().Write_IMU(); } } +#endif // HAL_LOGGING_ENABLED // three_hz_loop - 3.3hz loop void Sub::three_hz_loop() @@ -274,9 +280,11 @@ void Sub::one_hz_loop() AP_Notify::flags.pre_arm_gps_check = position_ok(); AP_Notify::flags.flying = motors.armed(); +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::AP_STATE, ap.value); } +#endif if (!motors.armed()) { motors.update_throttle_range(); @@ -285,8 +293,10 @@ void Sub::one_hz_loop() // update assigned functions and enable auxiliary servos SRV_Channels::enable_aux_servos(); +#if HAL_LOGGING_ENABLED // log terrain data terrain_logging(); +#endif // need to set "likely flying" when armed to allow for compass // learning to run @@ -311,6 +321,7 @@ void Sub::update_altitude() // read in baro altitude read_barometer(); +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_CTUN)) { Log_Write_Control_Tuning(); AP::ins().write_notch_log_messages(); @@ -318,6 +329,7 @@ void Sub::update_altitude() gyro_fft.write_log_messages(); #endif } +#endif // HAL_LOGGING_ENABLED } bool Sub::control_check_barometer() @@ -358,14 +370,13 @@ bool Sub::get_wp_crosstrack_error_m(float &xtrack_error) const return true; } -#if STATS_ENABLED == ENABLED +#if AP_STATS_ENABLED /* update AP_Stats */ void Sub::stats_update(void) { - g2.stats.set_flying(motors.armed()); - g2.stats.update(); + AP::stats()->set_flying(motors.armed()); } #endif diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index aa4c39b3268a73..3b5fb95d79fd49 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -568,7 +568,7 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_i return MAV_RESULT_ACCEPTED; } -void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) +void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg) { switch (msg.msgid) { @@ -817,7 +817,7 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) break; default: - handle_common_message(msg); + GCS_MAVLINK::handle_message(msg); break; } // end switch } // end handle mavlink diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 91c62ddff5278a..4b49f2246e64ca 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -41,7 +41,7 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK { private: - void handleMessage(const mavlink_message_t &msg) override; + void handle_message(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; bool try_send_message(enum ap_message id) override; diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index f2649bc8417a92..63d3feb8ff222d 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -1,6 +1,6 @@ #include "Sub.h" -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED // Code to Write and Read packets from AP_Logger log memory // Code to interact with the user to dump or erase logs @@ -289,18 +289,4 @@ void Sub::log_init() logger.Init(log_structure, ARRAY_SIZE(log_structure)); } -#else // LOGGING_ENABLED - -void Sub::Log_Write_Control_Tuning() {} -void Sub::Log_Write_Attitude(void) {} -void Sub::Log_Write_Data(LogDataID id, int32_t value) {} -void Sub::Log_Write_Data(LogDataID id, uint32_t value) {} -void Sub::Log_Write_Data(LogDataID id, int16_t value) {} -void Sub::Log_Write_Data(LogDataID id, uint16_t value) {} -void Sub::Log_Write_Data(LogDataID id, float value) {} -void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} -void Sub::Log_Write_Vehicle_Startup_Messages() {} - -void Sub::log_init(void) {} - -#endif // LOGGING_ENABLED +#endif // HAL_LOGGING_ENABLED diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index d43b149820f740..168990b01f4c82 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -476,9 +476,9 @@ const AP_Param::Info Sub::var_info[] = { #endif #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif // @Group: COMPASS_ @@ -561,9 +561,11 @@ const AP_Param::Info Sub::var_info[] = { GOBJECT(camera_mount, "MNT", AP_Mount), #endif +#if HAL_LOGGING_ENABLED // @Group: LOG // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#endif // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -688,11 +690,9 @@ const AP_Param::Info Sub::var_info[] = { 2nd group of parameters */ const AP_Param::GroupInfo ParametersG2::var_info[] = { -#if STATS_ENABLED == ENABLED - // @Group: STAT - // @Path: ../libraries/AP_Stats/AP_Stats.cpp - AP_SUBGROUPINFO(stats, "STAT", 1, ParametersG2, AP_Stats), -#endif + + // 1 was AP_Stats + #if HAL_PROXIMITY_ENABLED // @Group: PRX // @Path: ../libraries/AP_Proximity/AP_Proximity.cpp @@ -713,11 +713,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/RC_Channel/RC_Channels_VarInfo.h AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels), -#if AP_SCRIPTING_ENABLED - // @Group: SCR_ - // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp - AP_SUBGROUPINFO(scripting, "SCR_", 18, ParametersG2, AP_Scripting), -#endif + // 18 was scripting // 19 was airspeed @@ -789,6 +785,35 @@ void Sub::load_parameters() #if AP_FENCE_ENABLED AP_Param::convert_class(g.k_param_fence_old, &fence, fence.var_info, 0, 0, true); #endif + + // PARAMETER_CONVERSION - Added: Jan-2024 +#if AP_STATS_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo stats_info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, stats_info.old_key)) { + return; + } + + const uint16_t stats_old_index = 1; // Old parameter index in g2 + const uint16_t stats_old_top_element = 4033; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(stats_info.old_key, &stats, stats.var_info, stats_old_index, stats_old_top_element, false); + } +#endif + // PARAMETER_CONVERSION - Added: Jan-2024 +#if AP_SCRIPTING_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo scripting_info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, scripting_info.old_key)) { + return; + } + + const uint16_t scripting_old_index = 18; // Old parameter index in g2 + const uint16_t scripting_old_top_element = 82; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(scripting_info.old_key, &scripting, scripting.var_info, scripting_old_index, scripting_old_top_element, false); + } +#endif } void Sub::convert_old_parameters() diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 95ca7923ab86fd..f8fd851fbf5e11 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -5,13 +5,8 @@ #include #include -#include #include -#if AP_SCRIPTING_ENABLED -#include -#endif - // Global parameter class. // class Parameters { @@ -354,10 +349,6 @@ class Parameters { class ParametersG2 { public: ParametersG2(void); -#if STATS_ENABLED == ENABLED - // vehicle statistics - AP_Stats stats; -#endif // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -377,9 +368,6 @@ class ParametersG2 { // control over servo output ranges SRV_Channels servo_channels; -#if AP_SCRIPTING_ENABLED - AP_Scripting scripting; -#endif // AP_SCRIPTING_ENABLED }; extern const AP_Param::Info var_info[]; diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 2a90c7f3a685e3..304ed327249e55 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -24,7 +24,10 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); constructor for main Sub class */ Sub::Sub() - : logger(g.log_bitmask), + : +#if HAL_LOGGING_ENABLED + logger(g.log_bitmask), +#endif control_mode(Mode::Number::MANUAL), motors(MAIN_LOOP_RATE), auto_mode(Auto_WP), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 482aabfc388920..53de38af092b79 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -62,6 +62,7 @@ #include // Joystick/gamepad button function assignment #include // Leak detector #include +#include // Local modules #include "defines.h" @@ -145,7 +146,9 @@ class Sub : public AP_Vehicle { RC_Channel *channel_forward; RC_Channel *channel_lateral; +#if HAL_LOGGING_ENABLED AP_Logger logger; +#endif AP_LeakDetector leak_detector; @@ -399,6 +402,7 @@ class Sub : public AP_Vehicle { float get_pilot_desired_climb_rate(float throttle_control); float get_surface_tracking_climb_rate(int16_t target_rate, float current_alt_target, float dt); void rotate_body_frame_to_NE(float &x, float &y); +#if HAL_LOGGING_ENABLED void Log_Write_Control_Tuning(); void Log_Write_Attitude(); void Log_Write_Data(LogDataID id, int32_t value); @@ -408,6 +412,7 @@ class Sub : public AP_Vehicle { void Log_Write_Data(LogDataID id, float value); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_Vehicle_Startup_Messages(); +#endif void load_parameters(void) override; void userhook_init(); void userhook_FastLoop(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 681022a3dd93bf..40a6b70f4d0ab6 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -7,10 +7,12 @@ static enum AutoSurfaceState auto_surface_state = AUTO_SURFACE_STATE_GO_TO_LOCAT // start_command - this function will be called when the ap_mission lib wishes to start a new command bool Sub::start_command(const AP_Mission::Mission_Command& cmd) { +#if HAL_LOGGING_ENABLED // To-Do: logging when new commands start/end if (should_log(MASK_LOG_CMD)) { logger.Write_Mission_Cmd(mission, cmd); } +#endif const Location &target_loc = cmd.content.location; @@ -347,7 +349,7 @@ void Sub::do_circle(const AP_Mission::Mission_Command& cmd) } else { // default to current altitude above origin circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame()); - AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); + LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); } } @@ -537,9 +539,10 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd) } return false; } + const float turns = cmd.get_loiter_turns(); // check if we have completed circling - return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1); + return fabsf(sub.circle_nav.get_angle_total()/M_2PI) >= turns; } #if NAV_GUIDED == ENABLED diff --git a/ArduSub/config.h b/ArduSub/config.h index 4a517afe13adbb..d4cdc55bf93d5c 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -186,15 +186,6 @@ ////////////////////////////////////////////////////////////////////////////// // Logging control // -#ifndef LOGGING_ENABLED -# define LOGGING_ENABLED ENABLED -#endif - -// Statistics -#ifndef STATS_ENABLED - # define STATS_ENABLED (AP_STATS_ENABLED ? ENABLED : DISABLED) -#endif - // Default logging bitmask #ifndef DEFAULT_LOG_BITMASK diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp index 5c0d08d32499cf..80c74ed45829d1 100644 --- a/ArduSub/failsafe.cpp +++ b/ArduSub/failsafe.cpp @@ -37,7 +37,7 @@ void Sub::mainloop_failsafe_check() failsafe_last_timestamp = tnow; if (in_failsafe) { in_failsafe = false; - AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_RESOLVED); } return; } @@ -51,7 +51,7 @@ void Sub::mainloop_failsafe_check() if (motors.armed()) { motors.output_min(); } - AP::logger().Write_Error(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU,LogErrorCode::FAILSAFE_OCCURRED); } if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) { @@ -73,7 +73,7 @@ void Sub::failsafe_sensors_check() // We need a depth sensor to do any sort of auto z control if (sensor_health.depth) { if (failsafe.sensor_health) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::ERROR_RESOLVED); failsafe.sensor_health = false; } return; @@ -86,7 +86,7 @@ void Sub::failsafe_sensors_check() failsafe.sensor_health = true; gcs().send_text(MAV_SEVERITY_CRITICAL, "Depth sensor error!"); - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_SENSORS, LogErrorCode::BAD_DEPTH); if (control_mode == Mode::Number::ALT_HOLD || control_mode == Mode::Number::SURFACE || sub.flightmode->requires_GPS()) { // This should always succeed @@ -137,7 +137,7 @@ void Sub::failsafe_ekf_check() failsafe.ekf = true; AP_Notify::flags.ekf_bad = true; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); if (AP_HAL::millis() > failsafe.last_ekf_warn_ms + 20000) { failsafe.last_ekf_warn_ms = AP_HAL::millis(); @@ -152,7 +152,7 @@ void Sub::failsafe_ekf_check() // Battery failsafe handler void Sub::handle_battery_failsafe(const char* type_str, const int8_t action) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); switch((Failsafe_Action)action) { case Failsafe_Action_Surface: @@ -187,7 +187,7 @@ void Sub::failsafe_pilot_input_check() failsafe.pilot_input = true; - AP::logger().Write_Error(LogErrorSubsystem::PILOT_INPUT, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::PILOT_INPUT, LogErrorCode::FAILSAFE_OCCURRED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Lost manual control"); set_neutral_controls(); @@ -270,7 +270,7 @@ void Sub::failsafe_leak_check() // Do nothing if we are dry, or if leak failsafe action is disabled if (status == false || g.failsafe_leak == FS_LEAK_DISABLED) { if (failsafe.leak) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_RESOLVED); } AP_Notify::flags.leak_detected = false; failsafe.leak = false; @@ -295,7 +295,7 @@ void Sub::failsafe_leak_check() failsafe.leak = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_LEAK, LogErrorCode::FAILSAFE_OCCURRED); // Handle failsafe action if (failsafe.leak && g.failsafe_leak == FS_LEAK_SURFACE && motors.armed()) { @@ -324,7 +324,7 @@ void Sub::failsafe_gcs_check() if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) { // Log event if we are recovering from previous gcs failsafe if (failsafe.gcs) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED); } failsafe.gcs = false; return; @@ -346,7 +346,7 @@ void Sub::failsafe_gcs_check() } failsafe.gcs = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_OCCURRED); // handle failsafe action if (g.failsafe_gcs == FS_GCS_DISARM) { @@ -412,7 +412,7 @@ void Sub::failsafe_crash_check() } failsafe.crash = true; - AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_CRASH); // disarm motors if (g.fs_crash_check == FS_CRASH_DISARM) { @@ -435,7 +435,7 @@ void Sub::failsafe_terrain_check() gcs().send_text(MAV_SEVERITY_CRITICAL,"Failsafe terrain triggered"); failsafe_terrain_on_event(); } else { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::ERROR_RESOLVED); failsafe.terrain = false; } } @@ -467,7 +467,7 @@ void Sub::failsafe_terrain_set_status(bool data_ok) void Sub::failsafe_terrain_on_event() { failsafe.terrain = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED); // If rangefinder is enabled, we can recover from this failsafe if (!rangefinder_state.enabled || !sub.mode_auto.auto_terrain_recover_start()) { diff --git a/ArduSub/fence.cpp b/ArduSub/fence.cpp index c39e84aebf5319..7e895c20da429e 100644 --- a/ArduSub/fence.cpp +++ b/ArduSub/fence.cpp @@ -41,10 +41,10 @@ void Sub::fence_check() // } } - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches) { // record clearing of breach - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } } diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index 323836c2f94abb..9a945d85e49eb1 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -86,7 +86,7 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason) if (new_flightmode->requires_GPS() && !sub.position_ok()) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name()); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } @@ -96,13 +96,13 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason) flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle()) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } if (!new_flightmode->init(false)) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name()); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } @@ -116,7 +116,9 @@ bool Sub::set_mode(Mode::Number mode, ModeReason reason) flightmode = new_flightmode; control_mode = mode; control_mode_reason = reason; +#if HAL_LOGGING_ENABLED logger.Write_Mode((uint8_t)control_mode, reason); +#endif gcs().send_message(MSG_HEARTBEAT); // update notify object diff --git a/ArduSub/mode_guided.cpp b/ArduSub/mode_guided.cpp index 25857bf5c94e64..ff1bb07df5681a 100644 --- a/ArduSub/mode_guided.cpp +++ b/ArduSub/mode_guided.cpp @@ -170,7 +170,7 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination) // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); if (!sub.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -179,8 +179,11 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination) // no need to check return status because terrain data is not used sub.wp_nav.set_wp_destination(destination, false); +#if HAL_LOGGING_ENABLED // log target sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f()); +#endif + return true; } @@ -198,7 +201,7 @@ bool ModeGuided::guided_set_destination(const Location& dest_loc) // reject destination outside the fence. // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails if (!sub.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -206,13 +209,16 @@ bool ModeGuided::guided_set_destination(const Location& dest_loc) if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) { // failure to set destination can only be because of missing terrain data - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION); // failure is propagated to GCS with NAK return false; } +#if HAL_LOGGING_ENABLED // log target sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f()); +#endif + return true; } @@ -230,7 +236,7 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_ya // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); if (!sub.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -244,8 +250,11 @@ bool ModeGuided::guided_set_destination(const Vector3f& destination, bool use_ya // no need to check return status because terrain data is not used sub.wp_nav.set_wp_destination(destination, false); +#if HAL_LOGGING_ENABLED // log target sub.Log_Write_GuidedTarget(sub.guided_mode, destination, Vector3f()); +#endif + return true; } @@ -293,7 +302,7 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); if (!sub.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -308,8 +317,11 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); posvel_pos_target_cm.z = dz; +#if HAL_LOGGING_ENABLED // log target sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); +#endif + return true; } @@ -325,7 +337,7 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons // reject destination if outside the fence const Location dest_loc(destination, Location::AltFrame::ABOVE_ORIGIN); if (!sub.fence.check_destination_within_fence(dest_loc)) { - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE); // failure is propagated to GCS with NAK return false; } @@ -344,8 +356,11 @@ bool ModeGuided::guided_set_destination_posvel(const Vector3f& destination, cons position_control->input_pos_vel_accel_z(dz, posvel_vel_target_cms.z, 0); posvel_pos_target_cm.z = dz; +#if HAL_LOGGING_ENABLED // log target sub.Log_Write_GuidedTarget(sub.guided_mode, destination, velocity); +#endif + return true; } diff --git a/ArduSub/surface_bottom_detector.cpp b/ArduSub/surface_bottom_detector.cpp index 0338e77c99d4c3..ca1b58646e337b 100644 --- a/ArduSub/surface_bottom_detector.cpp +++ b/ArduSub/surface_bottom_detector.cpp @@ -90,9 +90,9 @@ void Sub::set_surfaced(bool at_surface) surface_detector_count = 0; if (ap.at_surface) { - AP::logger().Write_Event(LogEvent::SURFACED); + LOGGER_WRITE_EVENT(LogEvent::SURFACED); } else { - AP::logger().Write_Event(LogEvent::NOT_SURFACED); + LOGGER_WRITE_EVENT(LogEvent::NOT_SURFACED); } } @@ -108,8 +108,8 @@ void Sub::set_bottomed(bool at_bottom) bottom_detector_count = 0; if (ap.at_bottom) { - AP::logger().Write_Event(LogEvent::BOTTOMED); + LOGGER_WRITE_EVENT(LogEvent::BOTTOMED); } else { - AP::logger().Write_Event(LogEvent::NOT_BOTTOMED); + LOGGER_WRITE_EVENT(LogEvent::NOT_BOTTOMED); } } diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 487abe625e063a..f86637c5a143ab 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -14,16 +14,6 @@ static void failsafe_check_static() void Sub::init_ardupilot() { - BoardConfig.init(); -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - can_mgr.init(); -#endif - -#if STATS_ENABLED == ENABLED - // initialise stats module - g2.stats.init(); -#endif - // init cargo gripper #if AP_GRIPPER_ENABLED g2.gripper.init(); @@ -62,7 +52,7 @@ void Sub::init_ardupilot() // setup telem slots with serial ports gcs().setup_uarts(); -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED log_init(); #endif @@ -161,16 +151,12 @@ void Sub::init_ardupilot() mission.init(); // initialise AP_Logger library -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&sub, &Sub::Log_Write_Vehicle_Startup_Messages, void)); #endif startup_INS_ground(); -#if AP_SCRIPTING_ENABLED - g2.scripting.init(); -#endif // AP_SCRIPTING_ENABLED - // enable CPU failsafe mainloop_failsafe_enable(); @@ -264,18 +250,16 @@ bool Sub::optflow_position_ok() return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode); } +#if HAL_LOGGING_ENABLED /* should we log a message type now? */ bool Sub::should_log(uint32_t mask) { -#if LOGGING_ENABLED == ENABLED ap.logging_started = logger.logging_started(); return logger.should_log(mask); -#else - return false; -#endif } +#endif #include #include diff --git a/ArduSub/terrain.cpp b/ArduSub/terrain.cpp index 30b7351ce4b8c4..50249ade7b3322 100644 --- a/ArduSub/terrain.cpp +++ b/ArduSub/terrain.cpp @@ -17,6 +17,7 @@ void Sub::terrain_update() #endif } +#if HAL_LOGGING_ENABLED // log terrain data - should be called at 1hz void Sub::terrain_logging() { @@ -26,4 +27,5 @@ void Sub::terrain_logging() } #endif } +#endif // HAL_LOGGING_ENABLED diff --git a/ArduSub/turn_counter.cpp b/ArduSub/turn_counter.cpp index f2260aa12e47f8..52df84fecb7486 100644 --- a/ArduSub/turn_counter.cpp +++ b/ArduSub/turn_counter.cpp @@ -8,11 +8,11 @@ void Sub::update_turn_counter() // Determine state // 0: 0-90 deg, 1: 90-180 deg, 2: -180--90 deg, 3: -90--0 deg uint8_t turn_state; - if (ahrs.yaw >= 0.0f && ahrs.yaw < radians(90)) { + if (ahrs.get_yaw() >= 0.0f && ahrs.get_yaw() < radians(90)) { turn_state = 0; - } else if (ahrs.yaw > radians(90)) { + } else if (ahrs.get_yaw() > radians(90)) { turn_state = 1; - } else if (ahrs.yaw < -radians(90)) { + } else if (ahrs.get_yaw() < -radians(90)) { turn_state = 2; } else { turn_state = 3; diff --git a/BUILD.md b/BUILD.md index ae32c0a9107e7a..5e11d2c83808ee 100644 --- a/BUILD.md +++ b/BUILD.md @@ -102,6 +102,11 @@ list some basic and more used commands as example. Cleaning the build is very often not necessary and discouraged. We do incremental builds reducing the build time by orders of magnitude. + If submodules are failing to be synchronized, `submodulesync` may be used + to resync the submodules. This is usually necessary when shifting development + between stable releases or a stable release and the master branch. + + In some some cases `submodule_force_clean` may be necessary. This removes all submodules and then performs a `submodulesync`. (Note whitelisted modules like esp_idf is not removed.) * **Upload or install** diff --git a/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index 23ba7790f9a175..2011502b2f98df 100644 --- a/Blimp/AP_Arming.cpp +++ b/Blimp/AP_Arming.cpp @@ -29,8 +29,10 @@ bool AP_Arming_Blimp::run_pre_arm_checks(bool display_failure) return mandatory_checks(display_failure); } - return fence_checks(display_failure) - & parameter_checks(display_failure) + return parameter_checks(display_failure) +#if AP_FENCE_ENABLED + & fence_checks(display_failure) +#endif & motor_checks(display_failure) & gcs_failsafe_check(display_failure) & alt_checks(display_failure) @@ -302,8 +304,10 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c return false; } +#if HAL_LOGGING_ENABLED // let logger know that we're armed (it may open logs e.g.) AP::logger().set_vehicle_armed(true); +#endif // notify that arming will occur (we do this early to give plenty of warning) AP_Notify::flags.armed = true; @@ -321,7 +325,7 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c if (!ahrs.home_is_set()) { // Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home) ahrs.resetHeightDatum(); - AP::logger().Write_Event(LogEvent::EKF_ALT_RESET); + LOGGER_WRITE_EVENT(LogEvent::EKF_ALT_RESET); // we have reset height, so arming height is zero blimp.arming_altitude_m = 0; @@ -340,8 +344,10 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c // finally actually arm the motors blimp.motors->armed(true); +#if HAL_LOGGING_ENABLED // log flight mode in case it was changed while vehicle was disarmed AP::logger().Write_Mode((uint8_t)blimp.control_mode, blimp.control_mode_reason); +#endif // perf monitor ignores delay due to arming AP::scheduler().perf_info.ignore_this_loop(); @@ -389,7 +395,9 @@ bool AP_Arming_Blimp::disarm(const AP_Arming::Method method, bool do_disarm_chec // send disarm command to motors blimp.motors->armed(false); +#if HAL_LOGGING_ENABLED AP::logger().set_vehicle_armed(false); +#endif hal.util->set_soft_armed(false); diff --git a/Blimp/AP_State.cpp b/Blimp/AP_State.cpp index 47214730d99b8e..5dcebd68952333 100644 --- a/Blimp/AP_State.cpp +++ b/Blimp/AP_State.cpp @@ -10,7 +10,7 @@ void Blimp::set_auto_armed(bool b) ap.auto_armed = b; if (b) { - AP::logger().Write_Event(LogEvent::AUTO_ARMED); + LOGGER_WRITE_EVENT(LogEvent::AUTO_ARMED); } } diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index fc03cc4c8fb04b..3231580eaf619c 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -76,7 +76,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_ServoRelayEvents, &blimp.ServoRelayEvents, update_events, 50, 75, 27), #endif SCHED_TASK_CLASS(AP_Baro, &blimp.barometer, accumulate, 50, 90, 30), -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK(full_rate_logging, 50, 50, 33), #endif SCHED_TASK_CLASS(AP_Notify, &blimp.notify, update, 50, 90, 36), @@ -86,15 +86,14 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = { SCHED_TASK(gpsglitch_check, 10, 50, 48), SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_receive, 400, 180, 51), SCHED_TASK_CLASS(GCS, (GCS*)&blimp._gcs, update_send, 400, 550, 54), -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK(ten_hz_logging_loop, 10, 350, 57), SCHED_TASK(twentyfive_hz_logging, 25, 110, 60), SCHED_TASK_CLASS(AP_Logger, &blimp.logger, periodic_tasks, 400, 300, 63), #endif SCHED_TASK_CLASS(AP_InertialSensor, &blimp.ins, periodic, 400, 50, 66), +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Scheduler, &blimp.scheduler, update_logging, 0.1, 75, 69), -#if STATS_ENABLED == ENABLED - SCHED_TASK_CLASS(AP_Stats, &blimp.g2.stats, update, 1, 100, 75), #endif }; @@ -141,6 +140,7 @@ void Blimp::update_batt_compass(void) } } +#if HAL_LOGGING_ENABLED // Full rate logging of attitude, rate and pid loops void Blimp::full_rate_logging() { @@ -190,6 +190,7 @@ void Blimp::twentyfive_hz_logging() AP::ins().Write_IMU(); } } +#endif // HAL_LOGGING_ENABLED // three_hz_loop - 3.3hz loop void Blimp::three_hz_loop() @@ -201,9 +202,11 @@ void Blimp::three_hz_loop() // one_hz_loop - runs at 1Hz void Blimp::one_hz_loop() { +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_ANY)) { Log_Write_Data(LogDataID::AP_STATE, ap.value); } +#endif // update assigned functions and enable auxiliary servos SRV_Channels::enable_aux_servos(); @@ -226,6 +229,7 @@ void Blimp::read_AHRS(void) vel_ned_filtd = {vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)}; vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw); +#if HAL_LOGGING_ENABLED AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff", AP_HAL::micros64(), vel_ned.x, @@ -240,6 +244,7 @@ void Blimp::read_AHRS(void) pos_ned.y, pos_ned.z, blimp.ahrs.get_yaw()); +#endif } // read baro and log control tuning @@ -248,12 +253,14 @@ void Blimp::update_altitude() // read in baro altitude read_barometer(); +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_CTUN)) { AP::ins().write_notch_log_messages(); #if HAL_GYROFFT_ENABLED gyro_fft.write_log_messages(); #endif } +#endif } //Conversions are in 2D so that up remains up in world frame when the blimp is not exactly level. @@ -278,7 +285,10 @@ void Blimp::rotate_NE_to_BF(Vector2f &vec) constructor for main Blimp class */ Blimp::Blimp(void) - : logger(g.log_bitmask), + : +#if HAL_LOGGING_ENABLED + logger(g.log_bitmask), +#endif flight_modes(&g.flight_mode1), control_mode(Mode::Number::MANUAL), rc_throttle_control_in_filter(1.0f), diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index 32ac39b5bddf4f..63f5eb888fa54a 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -39,7 +39,6 @@ // #include // interface and maths for accelerometer calibration // #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library #include -#include // statistics library #include // Filter library #include // needed for AHRS build #include // inertial navigation library @@ -114,7 +113,9 @@ class Blimp : public AP_Vehicle RC_Channel *channel_up; RC_Channel *channel_yaw; +#if HAL_LOGGING_ENABLED AP_Logger logger; +#endif // flight modes convenience array AP_Int8 *flight_modes; @@ -352,6 +353,7 @@ class Blimp : public AP_Vehicle // landing_gear.cpp void landinggear_update(); +#if HAL_LOGGING_ENABLED // Log.cpp void Log_Write_Performance(); void Log_Write_Attitude(); @@ -370,6 +372,7 @@ class Blimp : public AP_Vehicle void log_init(void); void Write_FINI(float right, float front, float down, float yaw); void Write_FINO(float *amp, float *off); +#endif // mode.cpp bool set_mode(Mode::Number mode, ModeReason reason); diff --git a/Blimp/Fins.cpp b/Blimp/Fins.cpp index 0247a5795fbc0c..8e1eade2a2e6b7 100644 --- a/Blimp/Fins.cpp +++ b/Blimp/Fins.cpp @@ -80,7 +80,9 @@ void Fins::output() yaw_out = 0; } +#if HAL_LOGGING_ENABLED blimp.Write_FINI(right_out, front_out, down_out, yaw_out); +#endif //Constrain after logging so as to still show when sub-optimal tuning is causing massive overshoots. right_out = constrain_float(right_out, -1, 1); @@ -130,7 +132,9 @@ void Fins::output() SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX); } +#if HAL_LOGGING_ENABLED blimp.Write_FINO(_amp, _off); +#endif } void Fins::output_min() diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 8eaf6473dd9738..17193438c1dd67 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -407,15 +407,6 @@ const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = { MAV_STREAM_TERMINATOR // must have this at end of stream_entries }; -bool GCS_MAVLINK_Blimp::handle_guided_request(AP_Mission::Mission_Command &cmd) -{ - // #if MODE_AUTO_ENABLED == ENABLED - // // return blimp.mode_auto.do_guided(cmd); - // #else - return false; - // #endif -} - void GCS_MAVLINK_Blimp::packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) { @@ -512,22 +503,16 @@ bool GCS_MAVLINK_Blimp::mav_frame_for_command_long(MAV_FRAME &frame, MAV_CMD pac } #endif -void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) +void GCS_MAVLINK_Blimp::handle_message(const mavlink_message_t &msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_RADIO: - case MAVLINK_MSG_ID_RADIO_STATUS: { // MAV ID: 109 - handle_radio_status(msg, blimp.should_log(MASK_LOG_PM)); - break; - } - case MAVLINK_MSG_ID_TERRAIN_DATA: case MAVLINK_MSG_ID_TERRAIN_CHECK: break; default: - handle_common_message(msg); + GCS_MAVLINK::handle_message(msg); break; } // end switch } // end handle mavlink diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 756dddc3fcf24e..71f10a414e7c34 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -2,6 +2,8 @@ #include +#include "defines.h" + class GCS_MAVLINK_Blimp : public GCS_MAVLINK { @@ -44,10 +46,13 @@ class GCS_MAVLINK_Blimp : public GCS_MAVLINK }; virtual MAV_LANDED_STATE landed_state() const override; +#if HAL_LOGGING_ENABLED + uint32_t log_radio_bit() const override { return MASK_LOG_PM; } +#endif + private: - void handleMessage(const mavlink_message_t &msg) override; - bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; + void handle_message(const mavlink_message_t &msg) override; bool try_send_message(enum ap_message id) override; void packetReceived(const mavlink_status_t &status, diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index 17c3161faba385..7d62fc9d5ebc6b 100644 --- a/Blimp/Log.cpp +++ b/Blimp/Log.cpp @@ -1,6 +1,6 @@ #include "Blimp.h" -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED // Code to Write and Read packets from AP_Logger log memory // Code to interact with the user to dump or erase logs @@ -407,21 +407,4 @@ void Blimp::log_init(void) logger.Init(log_structure, ARRAY_SIZE(log_structure)); } -#else // LOGGING_ENABLED - -void Blimp::Log_Write_Performance() {} -void Blimp::Log_Write_Attitude(void) {} -void Blimp::Log_Write_PIDs(void) {} -void Blimp::Log_Write_EKF_POS() {} -void Blimp::Log_Write_Data(LogDataID id, int32_t value) {} -void Blimp::Log_Write_Data(LogDataID id, uint32_t value) {} -void Blimp::Log_Write_Data(LogDataID id, int16_t value) {} -void Blimp::Log_Write_Data(LogDataID id, uint16_t value) {} -void Blimp::Log_Write_Data(LogDataID id, float value) {} -void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} -void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} -void Blimp::Log_Write_Vehicle_Startup_Messages() {} - -void Blimp::log_init(void) {} - -#endif // LOGGING_ENABLED +#endif // HAL_LOGGING_ENABLED diff --git a/Blimp/Loiter.cpp b/Blimp/Loiter.cpp index 8c6b5d5e7aea7c..52e85f1b536990 100644 --- a/Blimp/Loiter.cpp +++ b/Blimp/Loiter.cpp @@ -25,10 +25,12 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled } scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO; +#if HAL_LOGGING_ENABLED AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn", "Qffff", AP_HAL::micros64(), scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n); +#endif float yaw_ef = blimp.ahrs.get_yaw(); Vector3f err_xyz = target_pos - blimp.pos_ned; @@ -120,9 +122,11 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled blimp.motors->yaw_out = act_yaw; } +#if HAL_LOGGING_ENABLED AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); +#endif } void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled) @@ -196,7 +200,9 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax blimp.motors->yaw_out = act_yaw; } +#if HAL_LOGGING_ENABLED AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); +#endif } diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 593e9f26687f74..f0641e0cf28229 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -341,9 +341,11 @@ const AP_Param::Info Blimp::var_info[] = { // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp GOBJECT(ahrs, "AHRS_", AP_AHRS), +#if HAL_LOGGING_ENABLED // @Group: LOG // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#endif // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -778,11 +780,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("SYSID_ENFORCE", 11, ParametersG2, sysid_enforce, 0), -#if STATS_ENABLED == ENABLED - // @Group: STAT - // @Path: ../libraries/AP_Stats/AP_Stats.cpp - AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats), -#endif + // 12 was AP_Stats // @Param: FRAME_CLASS // @DisplayName: Frame Class @@ -809,11 +807,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0), -#if AP_SCRIPTING_ENABLED - // @Group: SCR_ - // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp - AP_SUBGROUPINFO(scripting, "SCR_", 30, ParametersG2, AP_Scripting), -#endif + // 30 was AP_Scripting // @Param: FS_VIBE_ENABLE // @DisplayName: Vibration Failsafe enable @@ -872,6 +866,35 @@ void Blimp::load_parameters(void) // Load all auto-loaded EEPROM variables AP_Param::load_all(); + // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 +#if AP_STATS_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { + return; + } + + const uint16_t old_index = 12; // Old parameter index in g2 + const uint16_t old_top_element = 4044; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(info.old_key, &stats, stats.var_info, old_index, old_top_element, false); + } +#endif + // PARAMETER_CONVERSION - Added: Jan-2024 for Copter-4.6 +#if AP_SCRIPTING_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, info.old_key)) { + return; + } + + const uint16_t old_index = 30; // Old parameter index in g2 + const uint16_t old_top_element = 94; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(info.old_key, &scripting, scripting.var_info, old_index, old_top_element, false); + } +#endif + hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before)); // setup AP_Param frame type flags diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index 8ec466d62c0c6d..769e9d2df7a4ad 100644 --- a/Blimp/Parameters.h +++ b/Blimp/Parameters.h @@ -281,11 +281,6 @@ class ParametersG2 // altitude at which nav control can start in takeoff AP_Float wp_navalt_min; -#if STATS_ENABLED == ENABLED - // vehicle statistics - AP_Stats stats; -#endif - // whether to enforce acceptance of packets only from sysid_my_gcs AP_Int8 sysid_enforce; @@ -310,11 +305,6 @@ class ParametersG2 // Land alt final stage AP_Int16 land_alt_low; - -#if AP_SCRIPTING_ENABLED - AP_Scripting scripting; -#endif // AP_SCRIPTING_ENABLED - // vibration failsafe enable/disable AP_Int8 fs_vibe_enabled; diff --git a/Blimp/RC_Channel.cpp b/Blimp/RC_Channel.cpp index 7709867cf25c4a..ba94d82a173df4 100644 --- a/Blimp/RC_Channel.cpp +++ b/Blimp/RC_Channel.cpp @@ -132,7 +132,7 @@ void Blimp::save_trim() float roll_trim = ToRad((float)channel_right->get_control_in()/100.0f); float pitch_trim = ToRad((float)channel_front->get_control_in()/100.0f); ahrs.add_trim(roll_trim, pitch_trim); - AP::logger().Write_Event(LogEvent::SAVE_TRIM); + LOGGER_WRITE_EVENT(LogEvent::SAVE_TRIM); gcs().send_text(MAV_SEVERITY_INFO, "Trim saved"); } diff --git a/Blimp/config.h b/Blimp/config.h index 9c5908f65fed98..2aca22ae839cae 100644 --- a/Blimp/config.h +++ b/Blimp/config.h @@ -111,13 +111,6 @@ # define AUTO_DISARMING_DELAY 10 #endif -////////////////////////////////////////////////////////////////////////////// -// Logging control -// -#ifndef LOGGING_ENABLED -# define LOGGING_ENABLED ENABLED -#endif - // Default logging bitmask #ifndef DEFAULT_LOG_BITMASK # define DEFAULT_LOG_BITMASK \ @@ -141,10 +134,6 @@ # define CH_MODE_DEFAULT 5 #endif -#ifndef STATS_ENABLED -# define STATS_ENABLED ENABLED -#endif - #ifndef HAL_FRAME_TYPE_DEFAULT #define HAL_FRAME_TYPE_DEFAULT Fins::MOTOR_FRAME_TYPE_AIRFISH #endif diff --git a/Blimp/ekf_check.cpp b/Blimp/ekf_check.cpp index fd524b99ab91a5..6b91eb6ec8ab1c 100644 --- a/Blimp/ekf_check.cpp +++ b/Blimp/ekf_check.cpp @@ -69,7 +69,7 @@ void Blimp::ekf_check() // limit count from climbing too high ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.bad_variance = true; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); // send message to gcs if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF variance"); @@ -86,7 +86,7 @@ void Blimp::ekf_check() // if compass is flagged as bad and the counter reaches zero then clear flag if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { ekf_check_state.bad_variance = false; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); } @@ -145,7 +145,7 @@ void Blimp::failsafe_ekf_event() // EKF failsafe event has occurred failsafe.ekf = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); // does this mode require position? if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_MANUAL)) { @@ -171,7 +171,7 @@ void Blimp::failsafe_ekf_off_event(void) } failsafe.ekf = false; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); } // check for ekf yaw reset and adjust target heading, also log position reset @@ -182,13 +182,13 @@ void Blimp::check_ekf_reset() uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad); if (new_ekfYawReset_ms != ekfYawReset_ms) { ekfYawReset_ms = new_ekfYawReset_ms; - AP::logger().Write_Event(LogEvent::EKF_YAW_RESET); + LOGGER_WRITE_EVENT(LogEvent::EKF_YAW_RESET); } // check for change in primary EKF, reset attitude target and log. AC_PosControl handles position target adjustment if ((ahrs.get_primary_core_index() != ekf_primary_core) && (ahrs.get_primary_core_index() != -1)) { ekf_primary_core = ahrs.get_primary_core_index(); - AP::logger().Write_Error(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(ekf_primary_core)); gcs().send_text(MAV_SEVERITY_WARNING, "EKF primary changed:%d", (unsigned)ekf_primary_core); } } @@ -232,7 +232,7 @@ void Blimp::check_vibration() // restore ekf gains, reset timers and update user vibration_check.high_vibes = false; vibration_check.clear_ms = 0; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation OFF"); } } @@ -252,7 +252,7 @@ void Blimp::check_vibration() if (!vibration_check.high_vibes) { // switch ekf to use resistant gains vibration_check.high_vibes = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_VIBE, LogErrorCode::FAILSAFE_OCCURRED); gcs().send_text(MAV_SEVERITY_CRITICAL, "Vibration compensation ON"); } } diff --git a/Blimp/events.cpp b/Blimp/events.cpp index 452f624c5b879f..d7d7f7f52c33df 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -14,7 +14,7 @@ bool Blimp::failsafe_option(FailsafeOption opt) const void Blimp::failsafe_radio_on_event() { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_OCCURRED); // set desired action based on FS_THR_ENABLE parameter Failsafe_Action desired_action; @@ -59,13 +59,13 @@ void Blimp::failsafe_radio_off_event() { // no need to do anything except log the error as resolved // user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_RADIO, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_WARNING, "Radio Failsafe Cleared"); } void Blimp::handle_battery_failsafe(const char *type_str, const int8_t action) { - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_BATT, LogErrorCode::FAILSAFE_OCCURRED); Failsafe_Action desired_action = (Failsafe_Action)action; @@ -168,10 +168,10 @@ void Blimp::gpsglitch_check() if (ap.gps_glitching != gps_glitching) { ap.gps_glitching = gps_glitching; if (gps_glitching) { - AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::GPS_GLITCH); gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch"); } else { - AP::logger().Write_Error(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::GPS, LogErrorCode::ERROR_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL,"GPS Glitch cleared"); } } diff --git a/Blimp/failsafe.cpp b/Blimp/failsafe.cpp index 0c05d3ea9183c0..3ea0d1199c43e5 100644 --- a/Blimp/failsafe.cpp +++ b/Blimp/failsafe.cpp @@ -43,7 +43,7 @@ void Blimp::failsafe_check() failsafe_last_timestamp = tnow; if (in_failsafe) { in_failsafe = false; - AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_RESOLVED); } return; } @@ -59,7 +59,7 @@ void Blimp::failsafe_check() //TODO: this may not work correctly. } - AP::logger().Write_Error(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CPU, LogErrorCode::FAILSAFE_OCCURRED); } if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) { diff --git a/Blimp/mode.cpp b/Blimp/mode.cpp index b7d80bd906100f..9165036cb7639e 100644 --- a/Blimp/mode.cpp +++ b/Blimp/mode.cpp @@ -78,7 +78,7 @@ bool Blimp::set_mode(Mode::Number mode, ModeReason reason) new_flightmode->requires_GPS() && !blimp.position_ok()) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s requires position", new_flightmode->name()); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } @@ -89,13 +89,13 @@ bool Blimp::set_mode(Mode::Number mode, ModeReason reason) flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle()) { gcs().send_text(MAV_SEVERITY_WARNING, "Mode change failed: %s need alt estimate", new_flightmode->name()); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } if (!new_flightmode->init(ignore_checks)) { gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed %s", new_flightmode->name()); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); return false; } @@ -109,7 +109,9 @@ bool Blimp::set_mode(Mode::Number mode, ModeReason reason) flightmode = new_flightmode; control_mode = mode; control_mode_reason = reason; +#if HAL_LOGGING_ENABLED logger.Write_Mode((uint8_t)control_mode, reason); +#endif gcs().send_message(MSG_HEARTBEAT); // update notify object diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index 861c057820a5fc..02a42672e1843e 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -93,7 +93,7 @@ void Blimp::read_radio() } // Nobody ever talks to us. Log an error and enter failsafe. - AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); + LOGGER_WRITE_ERROR(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME); set_failsafe_radio(true); } @@ -155,4 +155,4 @@ void Blimp::set_throttle_zero_flag(int16_t throttle_control) ap.throttle_zero = true; } //TODO: This may not be needed -} \ No newline at end of file +} diff --git a/Blimp/system.cpp b/Blimp/system.cpp index f3d8bdc08e69f1..5bde10d45bb60f 100644 --- a/Blimp/system.cpp +++ b/Blimp/system.cpp @@ -14,15 +14,6 @@ static void failsafe_check_static() void Blimp::init_ardupilot() { - -#if STATS_ENABLED == ENABLED - // initialise stats module - g2.stats.init(); -#endif - - BoardConfig.init(); - - // initialise notify system notify.init(); notify_flight_mode(); @@ -38,7 +29,7 @@ void Blimp::init_ardupilot() // setup telem slots with serial ports gcs().setup_uarts(); -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED log_init(); #endif @@ -81,15 +72,13 @@ void Blimp::init_ardupilot() barometer.set_log_baro_bit(MASK_LOG_IMU); barometer.calibrate(); +#if HAL_LOGGING_ENABLED // initialise AP_Logger library logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&blimp, &Blimp::Log_Write_Vehicle_Startup_Messages, void)); +#endif startup_INS_ground(); -#if AP_SCRIPTING_ENABLED - g2.scripting.init(); -#endif // AP_SCRIPTING_ENABLED - ins.set_log_raw_bit(MASK_LOG_IMU_RAW); // setup fin output @@ -224,18 +213,16 @@ void Blimp::update_auto_armed() } } +#if HAL_LOGGING_ENABLED /* should we log a message type now? */ bool Blimp::should_log(uint32_t mask) { -#if LOGGING_ENABLED == ENABLED ap.logging_started = logger.logging_started(); return logger.should_log(mask); -#else - return false; -#endif } +#endif // return MAV_TYPE corresponding to frame class MAV_TYPE Blimp::get_frame_mav_type() diff --git a/README.md b/README.md index 7b603fc1632425..e015b750ff32b0 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ [![Test AP_Periph](https://github.com/ArduPilot/ardupilot/workflows/test%20ap_periph/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_sitl_periph.yml) [![Test Chibios](https://github.com/ArduPilot/ardupilot/workflows/test%20chibios/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_chibios.yml) [![Test Linux SBC](https://github.com/ArduPilot/ardupilot/workflows/test%20Linux%20SBC/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_linux_sbc.yml) [![Test Replay](https://github.com/ArduPilot/ardupilot/workflows/test%20replay/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_replay.yml) -[![Test Unit Tests](https://github.com/ArduPilot/ardupilot/workflows/test%20unit%20tests/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_unit_tests.yml) [![test size](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml) +[![Test Unit Tests](https://github.com/ArduPilot/ardupilot/workflows/test%20unit%20tests%20and%20sitl%20building/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_unit_tests.yml)[![test size](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml/badge.svg)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_size.yml) [![Test Environment Setup](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml/badge.svg?branch=master)](https://github.com/ArduPilot/ardupilot/actions/workflows/test_environment.yml) diff --git a/Rover/AP_Arming.cpp b/Rover/AP_Arming.cpp index 02744eacd19190..36a28aea8780d4 100644 --- a/Rover/AP_Arming.cpp +++ b/Rover/AP_Arming.cpp @@ -106,7 +106,9 @@ void AP_Arming_Rover::update_soft_armed() { hal.util->set_soft_armed(is_armed() && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED); +#if HAL_LOGGING_ENABLED AP::logger().set_vehicle_armed(hal.util->get_soft_armed()); +#endif } /* diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index e13e35bebc255c..efe14de0780a30 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -777,7 +777,7 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_com return MAV_RESULT_ACCEPTED; } -void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg) +void GCS_MAVLINK_Rover::handle_message(const mavlink_message_t &msg) { switch (msg.msgid) { @@ -793,13 +793,8 @@ void GCS_MAVLINK_Rover::handleMessage(const mavlink_message_t &msg) handle_set_position_target_global_int(msg); break; - case MAVLINK_MSG_ID_RADIO: - case MAVLINK_MSG_ID_RADIO_STATUS: - handle_radio(msg); - break; - default: - handle_common_message(msg); + GCS_MAVLINK::handle_message(msg); break; } } @@ -1063,11 +1058,6 @@ void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_mess } } -void GCS_MAVLINK_Rover::handle_radio(const mavlink_message_t &msg) -{ - handle_radio_status(msg, rover.should_log(MASK_LOG_PM)); -} - /* handle a LANDING_TARGET command. The timestamp has been jitter corrected */ diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 050109f5ccc3b3..048bb86969a4e5 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -7,6 +7,8 @@ #define AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED 1 #endif +#include "defines.h" + class GCS_MAVLINK_Rover : public GCS_MAVLINK { public: @@ -36,9 +38,13 @@ class GCS_MAVLINK_Rover : public GCS_MAVLINK void send_nav_controller_output() const override; void send_pid_tuning() override; +#if HAL_LOGGING_ENABLED + uint32_t log_radio_bit() const override { return MASK_LOG_PM; } +#endif + private: - void handleMessage(const mavlink_message_t &msg) override; + void handle_message(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; bool try_send_message(enum ap_message id) override; diff --git a/Rover/Log.cpp b/Rover/Log.cpp index 36403f66d253ed..f102fa95c0ca41 100644 --- a/Rover/Log.cpp +++ b/Rover/Log.cpp @@ -2,7 +2,7 @@ #include -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED // Write an attitude packet void Rover::Log_Write_Attitude() diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 5d4a84fc9b08b6..1c7ae1a07b2a65 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -223,9 +223,9 @@ const AP_Param::Info Rover::var_info[] = { GOBJECT(barometer, "BARO", AP_Baro), #if AP_RELAY_ENABLED - // @Group: RELAY_ + // @Group: RELAY // @Path: ../libraries/AP_Relay/AP_Relay.cpp - GOBJECT(relay, "RELAY_", AP_Relay), + GOBJECT(relay, "RELAY", AP_Relay), #endif // @Group: RCMAP_ @@ -316,9 +316,11 @@ const AP_Param::Info Rover::var_info[] = { // @Path: ../libraries/AP_Arming/AP_Arming.cpp GOBJECT(arming, "ARMING_", AP_Arming), +#if HAL_LOGGING_ENABLED // @Group: LOG // @Path: ../libraries/AP_Logger/AP_Logger.cpp GOBJECT(logger, "LOG", AP_Logger), +#endif // @Group: BATT // @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -402,11 +404,8 @@ const AP_Param::Info Rover::var_info[] = { 2nd group of parameters */ const AP_Param::GroupInfo ParametersG2::var_info[] = { -#if STATS_ENABLED == ENABLED - // @Group: STAT - // @Path: ../libraries/AP_Stats/AP_Stats.cpp - AP_SUBGROUPINFO(stats, "STAT", 1, ParametersG2, AP_Stats), -#endif + // 1 was AP_Stats + // @Param: SYSID_ENFORCE // @DisplayName: GCS sysid enforcement // @Description: This controls whether packets from other than the expected GCS system ID will be accepted @@ -602,11 +601,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("BAL_PITCH_TRIM", 40, ParametersG2, bal_pitch_trim, 0), -#if AP_SCRIPTING_ENABLED - // @Group: SCR_ - // @Path: ../libraries/AP_Scripting/AP_Scripting.cpp - AP_SUBGROUPINFO(scripting, "SCR_", 41, ParametersG2, AP_Scripting), -#endif + // 41 was Scripting // @Param: STICK_MIXING // @DisplayName: Stick Mixing @@ -919,4 +914,34 @@ void Rover::load_parameters(void) #if AP_FENCE_ENABLED AP_Param::convert_class(info.old_key, &fence, fence.var_info, 17, 4049, false); #endif + + // PARAMETER_CONVERSION - Added: Jan-2024 for Rover-4.6 +#if AP_STATS_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo stats_info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, stats_info.old_key)) { + return; + } + + const uint16_t stats_old_index = 1; // Old parameter index in g2 + const uint16_t stats_old_top_element = 4033; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(stats_info.old_key, &stats, stats.var_info, stats_old_index, stats_old_top_element, false); + } +#endif + // PARAMETER_CONVERSION - Added: Jan-2024 for Rover-4.6 +#if AP_SCRIPTING_ENABLED + { + // Find G2's Top Level Key + AP_Param::ConversionInfo scripting_info; + if (!AP_Param::find_top_level_key_by_pointer(&g2, scripting_info.old_key)) { + return; + } + + const uint16_t scripting_old_index = 41; // Old parameter index in g2 + const uint16_t scripting_old_top_element = 105; // Old group element in the tree for the first subgroup element (see AP_PARAM_KEY_DUMP) + AP_Param::convert_class(scripting_info.old_key, &scripting, scripting.var_info, scripting_old_index, scripting_old_top_element, false); + } +#endif + } diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 035d16f4d0ffb4..ef8d6194706ac3 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -79,15 +79,15 @@ class Parameters { // 110: Telemetry control // - k_param_gcs0 = 110, // stream rates for uartA - k_param_gcs1, // stream rates for uartC + k_param_gcs0 = 110, // stream rates for SERIAL0 + k_param_gcs1, // stream rates for SERIAL1 k_param_sysid_this_mav, k_param_sysid_my_gcs, k_param_serial0_baud_old, // unused k_param_serial1_baud_old, // unused k_param_telem_delay, k_param_skip_gyro_cal, // unused - k_param_gcs2, // stream rates for uartD + k_param_gcs2, // stream rates for SERIAL2 k_param_serial2_baud_old, // unused k_param_serial2_protocol, // deprecated, can be deleted k_param_serial_manager, // serial manager library @@ -293,11 +293,6 @@ class ParametersG2 { // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; -#if STATS_ENABLED == ENABLED - // vehicle statistics - AP_Stats stats; -#endif - // whether to enforce acceptance of packets only from sysid_my_gcs AP_Int8 sysid_enforce; @@ -401,10 +396,6 @@ class ParametersG2 { // stick mixing for auto modes AP_Int8 stick_mixing; -#if AP_SCRIPTING_ENABLED - AP_Scripting scripting; -#endif // AP_SCRIPTING_ENABLED - // waypoint navigation AR_WPNav_OA wp_nav; diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 82b7f46e371397..4d8c3e3fd29da9 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -89,8 +89,10 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_WindVane, &rover.g2.windvane, update, 20, 100, 30), SCHED_TASK(update_wheel_encoder, 50, 200, 36), SCHED_TASK(update_compass, 10, 200, 39), +#if HAL_LOGGING_ENABLED SCHED_TASK(update_logging1, 10, 200, 45), SCHED_TASK(update_logging2, 10, 200, 48), +#endif SCHED_TASK_CLASS(GCS, (GCS*)&rover._gcs, update_receive, 400, 500, 51), SCHED_TASK_CLASS(GCS, (GCS*)&rover._gcs, update_send, 400, 1000, 54), SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&rover.g2.rc_channels, read_mode_switch, 7, 200, 57), @@ -123,16 +125,15 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK_CLASS(AC_Sprayer, &rover.g2.sprayer, update, 3, 90, 99), #endif SCHED_TASK(compass_save, 0.1, 200, 105), -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Logger, &rover.logger, periodic_tasks, 50, 300, 108), #endif SCHED_TASK_CLASS(AP_InertialSensor, &rover.ins, periodic, 400, 200, 111), +#if HAL_LOGGING_ENABLED SCHED_TASK_CLASS(AP_Scheduler, &rover.scheduler, update_logging, 0.1, 200, 114), +#endif #if HAL_BUTTON_ENABLED SCHED_TASK_CLASS(AP_Button, &rover.button, update, 5, 200, 117), -#endif -#if STATS_ENABLED == ENABLED - SCHED_TASK(stats_update, 1, 200, 120), #endif SCHED_TASK(crash_check, 10, 200, 123), SCHED_TASK(cruise_learn_update, 50, 200, 126), @@ -156,7 +157,9 @@ constexpr int8_t Rover::_failsafe_priorities[7]; Rover::Rover(void) : AP_Vehicle(), param_loader(var_info), +#if HAL_LOGGING_ENABLED logger{g.log_bitmask}, +#endif modes(&g.mode1), control_mode(&mode_initializing) { @@ -296,14 +299,13 @@ void Rover::nav_script_time_done(uint16_t id) } #endif // AP_SCRIPTING_ENABLED -#if STATS_ENABLED == ENABLED +#if AP_STATS_ENABLED /* update AP_Stats */ void Rover::stats_update(void) { - g2.stats.set_flying(g2.motors.active()); - g2.stats.update(); + AP::stats()->set_flying(g2.motors.active()); } #endif @@ -336,6 +338,7 @@ void Rover::ahrs_update() ground_speed = ahrs.groundspeed(); } +#if HAL_LOGGING_ENABLED if (should_log(MASK_LOG_ATTITUDE_FAST)) { Log_Write_Attitude(); Log_Write_Sail(); @@ -348,6 +351,7 @@ void Rover::ahrs_update() if (should_log(MASK_LOG_VIDEO_STABILISATION)) { ahrs.write_video_stabilisation(); } +#endif } /* @@ -376,6 +380,7 @@ void Rover::gcs_failsafe_check(void) failsafe_trigger(FAILSAFE_EVENT_GCS, "GCS", do_failsafe); } +#if HAL_LOGGING_ENABLED /* log some key data - 10Hz */ @@ -435,7 +440,7 @@ void Rover::update_logging2(void) } #endif } - +#endif // HAL_LOGGING_ENABLED /* once a second events diff --git a/Rover/Rover.h b/Rover/Rover.h index c6aa6db49702cc..cbc508cd2c0a84 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -136,7 +136,9 @@ class Rover : public AP_Vehicle { RC_Channel *channel_pitch; RC_Channel *channel_walking_height; +#if HAL_LOGGING_ENABLED AP_Logger logger; +#endif // flight modes convenience array AP_Int8 *modes; @@ -226,7 +228,9 @@ class Rover : public AP_Vehicle { static const AP_Scheduler::Task scheduler_tasks[]; static const AP_Param::Info var_info[]; +#if HAL_LOGGING_ENABLED static const LogStructure log_structure[]; +#endif // time that rudder/steering arming has been running uint32_t rudder_arm_timer; diff --git a/Rover/config.h b/Rover/config.h index 7a280e88f3497a..03ddc70945998e 100644 --- a/Rover/config.h +++ b/Rover/config.h @@ -45,13 +45,6 @@ #define CRUISE_SPEED 2 // in m/s #endif -////////////////////////////////////////////////////////////////////////////// -// Logging control -// -#ifndef LOGGING_ENABLED - #define LOGGING_ENABLED ENABLED -#endif - #define DEFAULT_LOG_BITMASK 0xffff ////////////////////////////////////////////////////////////////////////////// @@ -83,10 +76,6 @@ #define ADVANCED_FAILSAFE DISABLED #endif -#ifndef STATS_ENABLED - # define STATS_ENABLED ENABLED -#endif - #ifndef OSD_ENABLED #define OSD_ENABLED DISABLED #endif diff --git a/Rover/crash_check.cpp b/Rover/crash_check.cpp index 284c125a4b9533..061e57ae9ce639 100644 --- a/Rover/crash_check.cpp +++ b/Rover/crash_check.cpp @@ -20,7 +20,7 @@ void Rover::crash_check() } // Crashed if pitch/roll > crash_angle - if ((g2.crash_angle != 0) && ((fabsf(ahrs.pitch) > radians(g2.crash_angle)) || (fabsf(ahrs.roll) > radians(g2.crash_angle)))) { + if ((g2.crash_angle != 0) && ((fabsf(ahrs.get_pitch()) > radians(g2.crash_angle)) || (fabsf(ahrs.get_roll()) > radians(g2.crash_angle)))) { crashed = true; } @@ -45,8 +45,8 @@ void Rover::crash_check() } if (crashed) { - AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, - LogErrorCode::CRASH_CHECK_CRASH); + LOGGER_WRITE_ERROR(LogErrorSubsystem::CRASH_CHECK, + LogErrorCode::CRASH_CHECK_CRASH); if (is_balancebot()) { // send message to gcs diff --git a/Rover/cruise_learn.cpp b/Rover/cruise_learn.cpp index 55d0e338ca155d..e844011296e2cc 100644 --- a/Rover/cruise_learn.cpp +++ b/Rover/cruise_learn.cpp @@ -15,7 +15,9 @@ void Rover::cruise_learn_start() cruise_learn.throttle_filt.reset(g2.motors.get_throttle()); cruise_learn.learn_start_ms = AP_HAL::millis(); cruise_learn.log_count = 0; +#if HAL_LOGGING_ENABLED log_write_cruise_learn(); +#endif gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning started"); } @@ -29,10 +31,12 @@ void Rover::cruise_learn_update() cruise_learn.speed_filt.apply(speed, 0.02f); cruise_learn.throttle_filt.apply(g2.motors.get_throttle(), 0.02f); // 10Hz logging +#if HAL_LOGGING_ENABLED if (cruise_learn.log_count % 5 == 0) { log_write_cruise_learn(); } cruise_learn.log_count += 1; +#endif // check how long it took to learn if (AP_HAL::millis() - cruise_learn.learn_start_ms >= 2000) { cruise_learn_complete(); @@ -56,10 +60,13 @@ void Rover::cruise_learn_complete() gcs().send_text(MAV_SEVERITY_CRITICAL, "Cruise Learning failed"); } cruise_learn.learn_start_ms = 0; +#if HAL_LOGGING_ENABLED log_write_cruise_learn(); +#endif } } +#if HAL_LOGGING_ENABLED // logging for cruise learn void Rover::log_write_cruise_learn() const { @@ -77,3 +84,4 @@ void Rover::log_write_cruise_learn() const cruise_learn.speed_filt.get(), cruise_learn.throttle_filt.get()); } +#endif diff --git a/Rover/ekf_check.cpp b/Rover/ekf_check.cpp index 7f6d04b6376979..cf20b559703d73 100644 --- a/Rover/ekf_check.cpp +++ b/Rover/ekf_check.cpp @@ -53,7 +53,7 @@ void Rover::ekf_check() ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX; ekf_check_state.bad_variance = true; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_BAD_VARIANCE); // send message to gcs if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) { @@ -71,7 +71,7 @@ void Rover::ekf_check() // if variance is flagged as bad and the counter reaches zero then clear flag if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) { ekf_check_state.bad_variance = false; - AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, + LOGGER_WRITE_ERROR(LogErrorSubsystem::EKFCHECK, LogErrorCode::EKFCHECK_VARIANCE_CLEARED); // clear failsafe failsafe_ekf_off_event(); @@ -156,7 +156,7 @@ void Rover::failsafe_ekf_event() // EKF failsafe event has occurred failsafe.ekf = true; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); // does this mode require position? @@ -189,7 +189,7 @@ void Rover::failsafe_ekf_off_event(void) } failsafe.ekf = false; - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF failsafe cleared"); } diff --git a/Rover/fence.cpp b/Rover/fence.cpp index 7150c88c467ccc..4c29b48d243dcd 100644 --- a/Rover/fence.cpp +++ b/Rover/fence.cpp @@ -51,11 +51,11 @@ void Rover::fence_check() set_mode(mode_hold, ModeReason::FENCE_BREACHED); } } - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode(new_breaches)); } else if (orig_breaches) { // record clearing of breach - AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_FENCE, + LOGGER_WRITE_ERROR(LogErrorSubsystem::FAILSAFE_FENCE, LogErrorCode::ERROR_RESOLVED); } #endif // AP_FENCE_ENABLED diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 842789c1006698..6173ac337dcdbe 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -290,7 +290,7 @@ void Mode::calc_throttle(float target_speed, bool avoidance_enabled) // apply object avoidance to desired speed using half vehicle's maximum deceleration if (avoidance_enabled) { - g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.yaw, target_speed, rover.G_Dt); + g2.avoid.adjust_speed(0.0f, 0.5f * attitude_control.get_decel_max(), ahrs.get_yaw(), target_speed, rover.G_Dt); if (g2.sailboat.tack_enabled() && g2.avoid.limits_active()) { // we are a sailboat trying to avoid fence, try a tack if (rover.control_mode != &rover.mode_acro) { diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index 5d2029b8201d25..dd99f363a15198 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -512,10 +512,12 @@ void ModeAuto::send_guided_position_target() /********************************************************************************/ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) { +#if HAL_LOGGING_ENABLED // log when new commands start if (rover.should_log(MASK_LOG_CMD)) { rover.logger.Write_Mission_Cmd(mission, cmd); } +#endif switch (cmd.id) { case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint @@ -943,8 +945,9 @@ bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd) bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) { + const float turns = cmd.get_loiter_turns(); // check if we have completed circling - return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= LOWBYTE(cmd.p1)); + return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= turns); } /********************************************************************************/ diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp index 42d6f63833b79b..86ba7dbdae1193 100644 --- a/Rover/mode_circle.cpp +++ b/Rover/mode_circle.cpp @@ -116,7 +116,7 @@ void ModeCircle::init_target_yaw_rad() // if no position estimate use vehicle yaw Vector2f curr_pos_NE; if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) { - target.yaw_rad = AP::ahrs().yaw; + target.yaw_rad = AP::ahrs().get_yaw(); return; } @@ -126,7 +126,7 @@ void ModeCircle::init_target_yaw_rad() // if current position is exactly at the center of the circle return vehicle yaw if (is_zero(dist_m)) { - target.yaw_rad = AP::ahrs().yaw; + target.yaw_rad = AP::ahrs().get_yaw(); } else { target.yaw_rad = center_to_veh.angle(); } diff --git a/Rover/mode_dock.cpp b/Rover/mode_dock.cpp index 4d52cb2af8ee03..89f8d89af7361a 100644 --- a/Rover/mode_dock.cpp +++ b/Rover/mode_dock.cpp @@ -171,6 +171,7 @@ void ModeDock::update() calc_steering_from_turn_rate(desired_turn_rate); calc_throttle(desired_speed, true); +#if HAL_LOGGING_ENABLED // @LoggerMessage: DOCK // @Description: Dock mode target information // @Field: TimeUS: Time since system startup @@ -196,6 +197,7 @@ void ModeDock::update() target_cm.y, desired_speed, desired_turn_rate); +#endif } float ModeDock::apply_slowdown(float desired_speed) diff --git a/Rover/release-notes.txt b/Rover/release-notes.txt index 96c3a1ca40427a..5cae4fdfc24358 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,6 +1,271 @@ Rover Release Notes: ------------------------------------------------------------------ -Rover 4.4.0-beta11 05-Dec-2023 +Rover 4.5.0-beta1 30-Jan-2025 +Changes from 4.4.4 +1) New autopilots supported + - ACNS-F405AIO + - Airvolute DCS2 onboard FMU + - Aocoda-RC-H743Dual + - BrainFPV RADIX 2 HD + - CAN-Zero + - CM4Pilot + - CubeRed + - Esp32-tomte76, esp32nick, esp32s3devkit + - FlyingMoonH743 + - Flywoo F405 Pro + - FlywooF405S-AIO with alternative IMUs + - Here4 GPS as flight controller + - Holybro 6X revision 6 + - Holybro6X-45686 with 3x ICM45686 IMUs + - JAE JFB110 + - KakuteH7 using ICM42688 + - PixFlamingo (uses STM32L4PLUS CPU) + - PixPilot-C3 + - PixSurveyA1-IND + - QiotekAdeptF407 + - Sierra TrueNavIC + - SPRacing H7RF + - SW-Nav-F405 + - YJUAV_A6 + - YJUAV_A6SE, YJUAV_A6SE_H743 +2) Autopilot specific changes + - 1MB boards lose features to save flash (Payload Place, some battery monitors, NMEA Output, bootloaders, Turtle mode) + - CubeOrangePlus supports IMU high resolution sampling (works with ICM42688, ICM42652, ICM42670, ICM45686 IMUs) + - F4 processors with only 1 IMU gain Triple Harmonic Notch support + - F765-SE bdshot support on 1st 4 pin + - F7 and H7 boards lose DMA on I2C ports (not required, limited DMA better used elsewhere) + - FlyingMoonH743, FlyingMoonF427 2nd and 3rd IMU order swapped + - HEEWING-F405 supports CRSF + - MatekL431-RC bootloader added, DMA used for RC and GPS + - PH4-mini, PH4-mini-bdshot, Swan-K1 and TBS-Colibri-F7 BRD_SER4_RTSCTS param conflict fixed + - Pixhawk6C supports BMI088 baro + - TMotorH743, Heewing-F405 serial parameter definitions fixed +3) AHRS/EKF enhancements and fixes + - AHRS_OPTIONS supports disabling fallback to DCM + - BARO_ALT_OFFSET slews more slowly (was 20sec, now 1min) + - EKF2 removed (can be re-enabled with Custom build server) + - External AHRS support for multiple GPSs + - InertialLabs INS-U external AHRS support + - Lord external AHRS renamed to MicroStrain5 + - MAV_CMD_EXTERNAL_POSITION_ESTIMATE supports setting approximate position during dead-reckoning + - Microstrain7 (aka 3DM-QG7) external AHRS support +4) Driver enhancements + - 3DR Solo supports up to 6S batteries + - Airspeed health checks vs GPS use 3D velocity + - BDshot on the first four channels of boards with F103-based IOMCUs (e.g. Pixhawk 6X) + - Dshot on all IOMCU channels on all boards with an IOMCU (e.g. all CubePilot autopilots) + - Dshot commands (e.g. motor reversal abd beeps) and EDT supported on IOMCU + - DroneCAN battery monitors calculate consumed energy if battery doesn't provide directly + - DroneCAN RC and Ghost RC protocol support + - EFI MAVLink driver + - Extended DShot Telemetry support (requires BLHeli32 ver 32.10 or BlueJay, set SERVO_DSHOT_ESC=3 or 4) + - GPS L5 band health override to enable L5 signal use (see GPS_DRV_OPTIONS) + - GPS-for-yaw works at lower update rate (3hz minimum) + - GSOF GPS supports GPS_COM_PORT parameter + - Hirth ICEngine support + - ICE option to enable/disable starting while disarmed + - ICE support for starter via relay + - IMUDATA serial protocol outputs raw IMU data on serial port (only available using custom build server) + - Innomaker LD06 360deg lidar support + - Intelligent Energy fuel cells new protocol support + - IRC Tramp supports 1G3 bands A and B + - IRC Ghost support + - JAE JRE-30 radar + - KDECAN driver rewrite (params moved to KDE_, works on all vehicles) + - MCP9601 temperature sensor support + - NanoRadar NRA24 rangefinder support + - NeoPixelsRGB support + - NoopLoop TOFSense, TOFSenseF-I2c rangefinder support + - OSD shows flashing horizon when inverted + - OSD2 support (e.g. second OSD) + - QMC5883P compass support + - Relay refactored to support RELAYx_FUNCTION, RELAY_STATUS message support added + - Reventech fuel level support (extends existing analog driver, see BATT_FL_xx parameters) + - RPLidarS1 360 deg lidar support and improved reliability for all RPLidars + - SBF GPS supports yaw from dual antennas + - Temperature sensor using analog voltages supported + - Trimble PX-1 support added as a GPS + - Winch driver enhancements including stuck protection, option for spin-free on startup +5) Control and navigation changes and enhancements + - Auto missions can always be cleared while disarmed (would fail if mission still running) + - DO_ENGINE_CONTROL allows starting engine even when disarmed + - DO_SET_MISSION_CURRENT command can reset mission (see Reset Mission field) + - DO_SET_SERVO, DO_REPEAT_SERVO work with servo outputs set to RCInxScaled + - Fractional Loiter Turn Support in missions + - HarmonicNotch supports up to 16 harmonics + - JUMP command sends improved text msg to pilot (says where will jump to) + - MAV_CMD_AIRFRAME_CONFIGURATION can control landing gear on all vehicles + - MAV_CMD_NAV_SET_YAW_SPEED deprecated + - MOT_OPTIONS allows voltage compensation to use raw battery voltages (instead of current corrected voltage) + - PID controllers get DFF/D_FF (derivative feed-forward), NTF (target notch filter index) and NEF (error notch filter index) + - PID controllers get PDMX param to limit P+D output (useful for large vehicles and/or slow actuators) + - PID notch filter configured via new filter library using FILT parameters +6) Parameters renamed + - COMPASS_TYPEMASK renamed to COMPASS_DISBLMSK +7) ROS2 / DDS support +8) Camera and gimbal enhancements + - Calculates location where camera gimbal is pointing (see CAMERA_FOV_STATUS) + - CAMx_MNT_INST allows specifying which mount camera is in + - Camera lens (e.g. live video stream) selectable using RC aux function + - Interval timer (for taking pictures at timed intervals) + - Image tracking support (ViewPro only) + - MAVLink Gimbal Protocol v2 support for better GCS integration + - MNTx_SYSID_DFLT allows easier pointing gimbal at another vehicle + - MOUNT_CONTROL, MOUNT_CONFIGURE messages deprecated + - RangeFinder support (only logged, only supported on Siyi, Viewpro) + - Pilot's RC input re-takes manual control of gimbal (e.g. switches to RC_TARGETING mode) + - Siyi driver gets Zoom control, sends autopilot attitude and time (reduces leans) + - Video recording may start when armed (see CAMx_OPTIONS) + - ViewPro driver (replaces equivalent Lua driver) + - Xacti camera gimbal support + - Zoom percentage support (for both missions and GCS commands) +9) Logging and reporting changes + - Battery logging (e.g. BAT) includes health, temperature, state-of-health percentage + - CAM and MNT messages contain camera gimbal's desired and actual angles + - INS_RAW_LOG_OPT allows raw, pre-filter and post-filter sensor data logging (alternative to "batch logging", good for filtering analysis) + - PID logging gets reset and I-term-set flags + - Rangefinder logging (e.g. RFND) includes signal quality + - RC aux functions sorted alphabetically for GCS + - RC logging (RCI, RCI2) include valid input and failsafe flags + - RTK GPS logging includes number of fragments used or discarded +10) Scripting enhancements + - Autopilot reboot support + - Baro, Compass, IMU, IOMCU health check support + - Battery cell voltage bindings + - Battery driver support + - BattEsimate.lua applet estimates SoC from voltage + - Camera and Mount bindings improved + - CAN input packet filtering reduces work required by Lua CAN drivers + - DJI RS2/RS3 gimbal driver supports latest DJI firmware version (see mount-djirs2-driver.lua) + - EFI drivers for DLA serial, InnoFlight Inject EFI driver + - EFI bindings improved + - Fence support + - Generator drivers for Halo6000, Zhuhai SVFFI + - GCS failsafe support + - Hobbywing_DataLink driver (see Hobbywing_DataLink.lua) + - is_landing, is_taking_off bindings + - led_on_a_switch.lua sets LED brightness from RC switch + - MAVLink sending and receiving support + - Mission jump_to_landing_sequence binding + - mount-poi.lua upgraded to applet, sends better feedback, can lock onto Location + - Networking/Ethernet support + - Plane dual-aircraft synchronised aerobatics + - Proximity driver support + - Rangefinder drivers can support signal quality + - revert_param.lua applet for quickly reverting params during tuning + - RockBlock.lua applet supports setting mode, fix for battery voltage reporting + - Serial/UART reading performance improvement using readstring binding + - sport_aerobatics.lua rudder control fixed + - Thread priority can be set using SCR_THD_PRIORITY (useful for Lua drivers) + - Wind alignment and head_wind speed bindings +11) Safety related enhancements and fixes + - Arm/Disarmed GPIO may be disabled using BRD_OPTIONS + - Arming check that Auto mode requires mission + - Arming check of compass vs world magnetic model to detect metal in ground (see ARMING_MAGTHRESH) + - Arming check of GPIO pin interrupt storm + - Arming check of Lua script CRC + - Arming check of mission loaded from SD card + - Arming check of Relay pin conflicts + - Arming check of emergency stop skipped if emergency stop aux function configured + - Arming check that at least some outputs are configured + - Arming failures reported more quickly when changing from success to failed + - ARMING_OPTIONS allows supressing "Armed", "Disarmed" text messages + - BRD_SAFETY_MASK extended to apply to CAN ESCs and servos + - Buzzer noise for gyro calibration and arming checks passed + - Dijkstras object avoidance supports "fast waypoints" (see AVOID_OPTIONS) + - Fence breach failsafe gets disarm action + - FENCE_OPTIONS supports union OR intersection of all polygon fences + - FLTMODE_GCSBLOCK blocks GCS from changing vehicle to specified modes + - Main loop lockup recovery by forcing mutex release (only helps if caused by software bug) + - Mandatory arming checks cannot be skipped (previously setting ARMING_CHECKS=0 skipped all) + - Rally points supports altitude frame (AMSL, Relative or Terrain) + - SERVO_RC_FS_MSK allows outputs using RC passthrough to move to SERVOx_TRIM on RC failsafe +12) System Enhancements + - CAN port can support a second CAN protocol on the same bus (2nd protocol must be 11 bit, see CAN_Dn_PROTOCOL2) + - CAN-FD support (allows faster data transfer rates) + - Crash dump info logged if main thread locksup (helps with root cause analysis) + - Ethernet/Networking support for UDP and TCP server and client (see NET_ENABLED) and PPP (see SERIALx_PROTOCOL) + - Firmware flashing from SD card + - Linux board SBUS input decoding made consistent with ChibiOS + - Linux boards support DroneCAN + - Parameter defaults stored in @ROMFS/defaults.parm + - SD Card formatting supported on all boards + - Second USB endpoint defaults to MAVLink (instead of SLCAN) except on CubePilot boards + - Serial over DroneCAN (see CAN_D1_UC_SER_EN) useful for configuring F9P DroneCAN GPSs using uCenter +13) Custom Build server include/exclude features extended to include + - APJ Tools + - Bootloader flashing + - Button + - Compass calibration + - DroneCAN GPS + - ExternalAHRS (e.g. MicroStrain, Vectornav) + - Generator + - Highmark Servo + - Hobbywing ESCs + - Kill IMU + - Payload Place + - Plane BlackBox arming allows Plane to be used as logger (see ARMING_BBOX_SPD) + - Plane's TX Tuning + - Precision landing + - Proximity sensor + - RC Protocol + - Relay + - SBUS Output + - ToneAlarm + - Winch +14) Developer specific items + - ChibiOS upgrade to 21.11 + - UAVCAN replaced with DroneCAN + - AUTOPILOT_VERSION_REQUEST message deprecated (use REQUEST_MESSAGE instead) + - PREFLIGHT_SET_SENSOR_OFFSETS support deprecated (was unused by all known ground stations) + - MISSION_SET_CURRENT message deprecated (use DO_SET_MISSION_CURRENT command instead) + - MISSION_CURRENT message sends num commands and stopped/paused/running/complete state + - Python version requirement increased to 3.6.9 + - mavlink_parse.py shows all suported mavlink messages + - COMMAND_INT messages can be used for nearly all commands (previously COMMAND_LONG) +15) Bug fixes: + - 3DR Solo gimbal mavlink routing fixed + - Bootloop fixed if INS_GYRO_FILTER set too high + - Button Internal Error caused by floating pin or slow device power-up fixed + - CAN Compass order maintained even if compass powered up after autopilot + - Compass device IDs only saved when calibrated to ensure external compasses appear as primary on new boards + - Currawong ECU EFI does not send exhaust gas temperature + - DCM fallback in order to get better GPS is disabled if GPS is not used + - DJI RS2/RS3 gimbal reported angle fix + - DO_SET_ROI, ROI_LOCATION, ROI_NONE bug fix that could lead to gimbal pointing at old target + - Generator parameter init fix (defaults might not always have been loaded correctly) + - GPS_TC_BLEND parameter removed (it was unused) + - Harmonic Notch gets protection against extremely low notch filter frequencies + - IE 650/800 Generators report fuel remaining + - INS calibration prevents unlikely case of two calibrations running at same time + - LPS2XH Baro supported over I2C fixed + - MatekH743 storage eeprom size fixed + - MAVLink routing fix to avoid processing packet meant for another vehicle + - Mount properly enforces user defined angle limits + - MPU6500 IMU filter corrected to 4k + - NMEA output time and altitude fixed + - OSD gets labels for all supported serial protocols + - OSD RF panel format fixed + - Omni vehicle support fixed + - RobotisServo initialisation fix + - RPM accuracy and time wrap handling improved + - Sagetech ADSB MXS altitude fix (needs amsl, was sending alt-above-terrain) + - SageTechMXS ADSB climb rate direction fixed + - SBUS out exactly matches SBUS in decoding + - Serial port RTS pins default to pulldown (SiK radios could getting stuck in bootloader mode) + - SERIALx_ parameters removed for ports that can't actually be used + - Servo gimbal attitude reporting fix + - Servo output fix when using scaled RC passthrough (e.g. SERVOx_FUNCTION = RCinXScaled) + - Siyi continuous zoom stutter fixed + - Siyi gimbal upside-down mode fixed (avoid bobbing if upside-down) + - SmartRTL premature "buffer full" failure fixed + - ST24 RC protocol fixed + - STM32L496 CAN2 init fix (AP_Periph only?) + - VFR_HUD climb rate reports best estimate during high vibration events (previously it would stop updating) + - Visual Odometry healthy check fix in case of out-of-memory + - VTX_MAX_POWER restored (allows setting radio's power) +------------------------------------------------------------------ +Rover 4.4.0 19-Dec-2023 / Rover 4.4.0-beta11 05-Dec-2023 Changes from 4.4.0-beta10 1) Autopilot related enhancement and fixes - CubeOrange Sim-on-hardware compilation fix diff --git a/Rover/sailboat.cpp b/Rover/sailboat.cpp index c547a9cb1fa36c..23f72e8a56af24 100644 --- a/Rover/sailboat.cpp +++ b/Rover/sailboat.cpp @@ -329,7 +329,7 @@ float Sailboat::get_VMG() const return speed; } - return (speed * cosf(wrap_PI(radians(rover.g2.wp_nav.wp_bearing_cd() * 0.01f) - rover.ahrs.yaw))); + return (speed * cosf(wrap_PI(radians(rover.g2.wp_nav.wp_bearing_cd() * 0.01f) - rover.ahrs.get_yaw()))); } // handle user initiated tack while in acro mode @@ -340,7 +340,7 @@ void Sailboat::handle_tack_request_acro() } // set tacking heading target to the current angle relative to the true wind but on the new tack currently_tacking = true; - tack_heading_rad = wrap_2PI(rover.ahrs.yaw + 2.0f * wrap_PI((rover.g2.windvane.get_true_wind_direction_rad() - rover.ahrs.yaw))); + tack_heading_rad = wrap_2PI(rover.ahrs.get_yaw() + 2.0f * wrap_PI((rover.g2.windvane.get_true_wind_direction_rad() - rover.ahrs.get_yaw()))); tack_request_ms = AP_HAL::millis(); } @@ -348,7 +348,7 @@ void Sailboat::handle_tack_request_acro() // return target heading in radians when tacking (only used in acro) float Sailboat::get_tack_heading_rad() { - if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.yaw)) < radians(SAILBOAT_TACKING_ACCURACY_DEG) || + if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw())) < radians(SAILBOAT_TACKING_ACCURACY_DEG) || ((AP_HAL::millis() - tack_request_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS)) { clear_tack(); } @@ -494,7 +494,7 @@ float Sailboat::calc_heading(float desired_heading_cd) // if we are tacking we maintain the target heading until the tack completes or times out if (currently_tacking) { // check if we have reached target - if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.yaw)) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) { + if (fabsf(wrap_PI(tack_heading_rad - rover.ahrs.get_yaw())) <= radians(SAILBOAT_TACKING_ACCURACY_DEG)) { clear_tack(); } else if ((now - auto_tack_start_ms) > SAILBOAT_AUTO_TACKING_TIMEOUT_MS) { // tack has taken too long diff --git a/Rover/system.cpp b/Rover/system.cpp index 492463e8a2345a..67f0a8ebdfcbb3 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -14,16 +14,6 @@ static void failsafe_check_static() void Rover::init_ardupilot() { -#if STATS_ENABLED == ENABLED - // initialise stats module - g2.stats.init(); -#endif - - BoardConfig.init(); -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - can_mgr.init(); -#endif - // init gripper #if AP_GRIPPER_ENABLED g2.gripper.init(); @@ -54,7 +44,7 @@ void Rover::init_ardupilot() osd.init(); #endif -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED log_init(); #endif @@ -179,15 +169,11 @@ void Rover::startup_ground(void) mode_auto.mission.init(); // initialise AP_Logger library -#if LOGGING_ENABLED == ENABLED +#if HAL_LOGGING_ENABLED logger.setVehicle_Startup_Writer( FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void) ); #endif - -#if AP_SCRIPTING_ENABLED - g2.scripting.init(); -#endif // AP_SCRIPTING_ENABLED } // update the ahrs flyforward setting which can allow @@ -259,8 +245,8 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) Mode &old_mode = *control_mode; if (!new_mode.enter()) { // Log error that we failed to enter desired flight mode - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, - LogErrorCode(new_mode.mode_number())); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, + LogErrorCode(new_mode.mode_number())); gcs().send_text(MAV_SEVERITY_WARNING, "Flight mode change failed"); return false; } @@ -281,7 +267,9 @@ bool Rover::set_mode(Mode &new_mode, ModeReason reason) old_mode.exit(); control_mode_reason = reason; +#if HAL_LOGGING_ENABLED logger.Write_Mode((uint8_t)control_mode->mode_number(), control_mode_reason); +#endif gcs().send_message(MSG_HEARTBEAT); notify_mode(control_mode); @@ -340,6 +328,7 @@ uint8_t Rover::check_digital_pin(uint8_t pin) return hal.gpio->read(pin); } +#if HAL_LOGGING_ENABLED /* should we log a message type now? */ @@ -347,6 +336,7 @@ bool Rover::should_log(uint32_t mask) { return logger.should_log(mask); } +#endif // returns true if vehicle is a boat // this affects whether the vehicle tries to maintain position after reaching waypoints diff --git a/Rover/version.h b/Rover/version.h index 11b034f62f7d68..a8cceba51d4c9e 100644 --- a/Rover/version.h +++ b/Rover/version.h @@ -6,13 +6,13 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduRover V4.5.0-dev" +#define THISFIRMWARE "ArduRover V4.6.0-dev" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 4,6,0,FIRMWARE_VERSION_TYPE_DEV #define FW_MAJOR 4 -#define FW_MINOR 5 +#define FW_MINOR 6 #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index f42271208073ba..da268f948f6249 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -37,6 +37,7 @@ #include #endif #include +#include "network.h" extern "C" { int main(void); @@ -61,10 +62,22 @@ struct boardinfo board_info = { AP_FlashIface_JEDEC ext_flash; #endif +#if AP_BOOTLOADER_NETWORK_ENABLED +static BL_Network network; +#endif + int main(void) { +#ifdef AP_BOOTLOADER_CUSTOM_HERE4 custom_startup(); +#endif + + flash_init(); +#ifdef STM32H7 + check_ecc_errors(); +#endif + #ifdef STM32F427xx if (BOARD_FLASH_SIZE > 1024 && check_limit_flash_1M()) { board_info.fw_size = (1024 - (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB))*1024; @@ -86,6 +99,10 @@ int main(void) stm32_flash_unprotect_flash(); #endif +#if AP_BOOTLOADER_NETWORK_ENABLED + network.save_comms_ip(); +#endif + #if AP_FASTBOOT_ENABLED enum rtc_boot_magic m = check_fast_reboot(); bool was_watchdog = stm32_was_watchdog_reset(); @@ -109,6 +126,7 @@ int main(void) try_boot = false; timeout = 0; } +#if AP_CHECK_FIRMWARE_ENABLED const auto ok = check_good_firmware(); if (ok != check_fw_result_t::CHECK_FW_OK) { // bad firmware CRC, don't try and boot @@ -116,6 +134,7 @@ int main(void) try_boot = false; led_set(LED_BAD_FW); } +#endif // AP_CHECK_FIRMWARE_ENABLED #ifndef BOOTLOADER_DEV_LIST else if (timeout != 0) { // fast boot for good firmware @@ -181,8 +200,10 @@ int main(void) #if HAL_USE_CAN == TRUE || HAL_NUM_CAN_IFACES can_start(); #endif - flash_init(); +#if AP_BOOTLOADER_NETWORK_ENABLED + network.init(); +#endif #if AP_BOOTLOADER_FLASH_FROM_SD_ENABLED if (flash_from_sd()) { @@ -196,7 +217,7 @@ int main(void) jump_to_app(); } #else - // CAN only + // CAN and network only while (true) { uint32_t t0 = AP_HAL::millis(); while (timeout == 0 || AP_HAL::millis() - t0 <= timeout) { diff --git a/Tools/AP_Bootloader/AP_Bootloader_config.h b/Tools/AP_Bootloader/AP_Bootloader_config.h index 459fca73cd89eb..869ebf16301cb6 100644 --- a/Tools/AP_Bootloader/AP_Bootloader_config.h +++ b/Tools/AP_Bootloader/AP_Bootloader_config.h @@ -1,7 +1,12 @@ #pragma once #include "hwdef.h" +#include #ifndef AP_BOOTLOADER_FLASH_FROM_SD_ENABLED #define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 0 #endif + +#ifndef AP_BOOTLOADER_NETWORK_ENABLED +#define AP_BOOTLOADER_NETWORK_ENABLED AP_NETWORKING_ENABLED +#endif diff --git a/Tools/AP_Bootloader/Web/index.html b/Tools/AP_Bootloader/Web/index.html new file mode 100644 index 00000000000000..f287a58c94dfd4 --- /dev/null +++ b/Tools/AP_Bootloader/Web/index.html @@ -0,0 +1,51 @@ + + + AP_Bootloader + + + + +

Bootloader

+ + + + + +
Board Type{BOARD_NAME}
Board ID{BOARD_ID}
Flash Size{FLASH_SIZE}
+ +

Firmware Update

+ +
+ + + +
+ +
+ +
+ + + diff --git a/Tools/AP_Bootloader/app_comms.h b/Tools/AP_Bootloader/app_comms.h index 04c0dcb0b98e76..2a03f9cc277eb6 100644 --- a/Tools/AP_Bootloader/app_comms.h +++ b/Tools/AP_Bootloader/app_comms.h @@ -10,7 +10,10 @@ struct app_bootloader_comms { uint32_t magic; - uint32_t reserved[4]; + uint32_t ip; + uint32_t netmask; + uint32_t gateway; + uint32_t reserved; uint8_t server_node_id; uint8_t my_node_id; uint8_t path[201]; diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index 5bc0df7b2877f7..9b505664d6c9a5 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -422,23 +422,23 @@ static void test_flash() } uint32_t num_writes = flash_func_sector_size(page) / sizeof(v); uprintf("page %u size %u addr=0x%08x v=0x%08x\n", - page, flash_func_sector_size(page), addr, v[0]); delay(10); + unsigned(page), unsigned(flash_func_sector_size(page)), unsigned(addr), unsigned(v[0])); delay(10); if (init_done) { for (uint32_t j=0; j +#include + +#include +#include +#include +#include +#if LWIP_DHCP +#include +#endif +#include +#include "../../modules/ChibiOS/os/various/evtimer.h" +#include +#include +#include +#include "support.h" +#include "bl_protocol.h" +#include +#include "app_comms.h" +#include "can.h" +#include +#include +#include + +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR +#define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "C2:AF:51:03:CF:46" +#endif + +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_IP +#define AP_NETWORKING_BOOTLOADER_DEFAULT_IP "192.168.1.100" +#endif + +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY +#define AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY "192.168.1.1" +#endif + +#ifndef AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK +#define AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK "255.255.255.0" +#endif + +#ifndef AP_BOOTLOADER_NETWORK_USE_DHCP +#define AP_BOOTLOADER_NETWORK_USE_DHCP 0 +#endif + +#define LWIP_SEND_TIMEOUT_MS 50 +#define LWIP_NETIF_MTU 1500 +#define LWIP_LINK_POLL_INTERVAL TIME_S2I(5) + +#define PERIODIC_TIMER_ID 1 +#define FRAME_RECEIVED_ID 2 + +#define MIN(a,b) ((a)<(b)?(a):(b)) + +void BL_Network::link_up_cb(void *p) +{ + auto *driver = (BL_Network *)p; +#if AP_BOOTLOADER_NETWORK_USE_DHCP + dhcp_start(driver->thisif); +#endif + char ipstr[IP4_STR_LEN]; + can_printf("IP %s", SocketAPM::inet_addr_to_str(ntohl(driver->thisif->ip_addr.addr), ipstr, sizeof(ipstr))); +} + +void BL_Network::link_down_cb(void *p) +{ +#if AP_BOOTLOADER_NETWORK_USE_DHCP + auto *driver = (BL_Network *)p; + dhcp_stop(driver->thisif); +#endif +} + +/* + * This function does the actual transmission of the packet. The packet is + * contained in the pbuf that is passed to the function. This pbuf + * might be chained. + * + * @param netif the lwip network interface structure for this ethernetif + * @param p the MAC packet to send (e.g. IP packet including MAC addresses and type) + * @return ERR_OK if the packet could be sent + * an err_t value if the packet couldn't be sent + * + * @note Returning ERR_MEM here if a DMA queue of your MAC is full can lead to + * strange results. You might consider waiting for space in the DMA queue + * to become available since the stack doesn't retry to send a packet + * dropped because of memory failure (except for the TCP timers). + */ +int8_t BL_Network::low_level_output(struct netif *netif, struct pbuf *p) +{ + struct pbuf *q; + MACTransmitDescriptor td; + (void)netif; + + if (macWaitTransmitDescriptor(ÐD1, &td, TIME_MS2I(LWIP_SEND_TIMEOUT_MS)) != MSG_OK) { + return ERR_TIMEOUT; + } + +#if ETH_PAD_SIZE + pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ +#endif + + /* Iterates through the pbuf chain. */ + for(q = p; q != NULL; q = q->next) { + macWriteTransmitDescriptor(&td, (uint8_t *)q->payload, (size_t)q->len); + } + macReleaseTransmitDescriptorX(&td); + +#if ETH_PAD_SIZE + pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ +#endif + + return ERR_OK; +} + +/* + * Receives a frame. + * Allocates a pbuf and transfers the bytes of the incoming + * packet from the interface into the pbuf. + * + * @param netif the lwip network interface structure for this ethernetif + * @return a pbuf filled with the received packet (including MAC header) + * NULL on memory error + */ +bool BL_Network::low_level_input(struct netif *netif, struct pbuf **pbuf) +{ + MACReceiveDescriptor rd; + struct pbuf *q; + u16_t len; + + (void)netif; + + if (macWaitReceiveDescriptor(ÐD1, &rd, TIME_IMMEDIATE) != MSG_OK) { + return false; + } + + len = (u16_t)rd.size; + +#if ETH_PAD_SIZE + len += ETH_PAD_SIZE; /* allow room for Ethernet padding */ +#endif + + /* We allocate a pbuf chain of pbufs from the pool. */ + *pbuf = pbuf_alloc(PBUF_RAW, len, PBUF_POOL); + + if (*pbuf != nullptr) { +#if ETH_PAD_SIZE + pbuf_header(*pbuf, -ETH_PAD_SIZE); /* drop the padding word */ +#endif + + /* Iterates through the pbuf chain. */ + for(q = *pbuf; q != NULL; q = q->next) { + macReadReceiveDescriptor(&rd, (uint8_t *)q->payload, (size_t)q->len); + } + macReleaseReceiveDescriptorX(&rd); + +#if ETH_PAD_SIZE + pbuf_header(*pbuf, ETH_PAD_SIZE); /* reclaim the padding word */ +#endif + } else { + macReleaseReceiveDescriptorX(&rd); // Drop packet + } + + return true; +} + +int8_t BL_Network::ethernetif_init(struct netif *netif) +{ + netif->state = NULL; + netif->name[0] = 'm'; + netif->name[1] = 's'; + netif->output = etharp_output; + netif->linkoutput = low_level_output; + + /* set MAC hardware address length */ + netif->hwaddr_len = ETHARP_HWADDR_LEN; + + /* maximum transfer unit */ + netif->mtu = LWIP_NETIF_MTU; + + /* device capabilities */ + netif->flags = NETIF_FLAG_BROADCAST | NETIF_FLAG_ETHARP; + +#if LWIP_IGMP + // also enable multicast + netif->flags |= NETIF_FLAG_IGMP; +#endif + + return ERR_OK; +} + +/* + networking thread +*/ +void BL_Network::net_thread() +{ + /* start tcpip thread */ + tcpip_init(NULL, NULL); + + thread_sleep_ms(100); + + AP_Networking::convert_str_to_macaddr(AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR, thisif->hwaddr); + + ip4_addr_t ip, gateway, netmask; + if (addr.ip == 0) { + // no IP from AP_Periph, use defaults + ip.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_IP)); + gateway.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_GATEWAY)); + netmask.addr = htonl(SocketAPM::inet_str_to_addr(AP_NETWORKING_BOOTLOADER_DEFAULT_NETMASK)); + } else { + // use addresses from AP_Periph + ip.addr = htonl(addr.ip); + netmask.addr = htonl(addr.netmask); + gateway.addr = htonl(addr.gateway); + } + + const MACConfig mac_config = {thisif->hwaddr}; + macStart(ÐD1, &mac_config); + + /* Add interface. */ + + auto result = netifapi_netif_add(thisif, &ip, &netmask, &gateway, NULL, ethernetif_init, tcpip_input); + if (result != ERR_OK) { + AP_HAL::panic("Failed to initialise netif"); + } + + netifapi_netif_set_default(thisif); + netifapi_netif_set_up(thisif); + + /* Setup event sources.*/ + event_timer_t evt; + event_listener_t el0, el1; + + evtObjectInit(&evt, LWIP_LINK_POLL_INTERVAL); + evtStart(&evt); + chEvtRegisterMask(&evt.et_es, &el0, PERIODIC_TIMER_ID); + chEvtRegisterMaskWithFlags(macGetEventSource(ÐD1), &el1, + FRAME_RECEIVED_ID, MAC_FLAGS_RX); + chEvtAddEvents(PERIODIC_TIMER_ID | FRAME_RECEIVED_ID); + + while (true) { + eventmask_t mask = chEvtWaitAny(ALL_EVENTS); + if (mask & PERIODIC_TIMER_ID) { + bool current_link_status = macPollLinkStatus(ÐD1); + if (current_link_status != netif_is_link_up(thisif)) { + if (current_link_status) { + tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_up, thisif, 0); + tcpip_callback_with_block(link_up_cb, this, 0); + } + else { + tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_down, thisif, 0); + tcpip_callback_with_block(link_down_cb, this, 0); + } + } + } + + if (mask & FRAME_RECEIVED_ID) { + struct pbuf *p; + while (low_level_input(thisif, &p)) { + if (p != NULL) { + struct eth_hdr *ethhdr = (struct eth_hdr *)p->payload; + switch (htons(ethhdr->type)) { + /* IP or ARP packet? */ + case ETHTYPE_IP: + case ETHTYPE_ARP: + /* full packet send to tcpip_thread to process */ + if (thisif->input(p, thisif) == ERR_OK) { + break; + } + /* Falls through */ + default: + pbuf_free(p); + } + } + } + } + } +} + +void BL_Network::net_thread_trampoline(void *ctx) +{ + auto *net = (BL_Network *)ctx; + net->net_thread(); +} + +void BL_Network::web_server_trampoline(void *ctx) +{ + auto *net = (BL_Network *)ctx; + net->web_server(); +} + +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + +BL_Network::web_var BL_Network::variables[] = { + { "BOARD_NAME", CHIBIOS_BOARD_NAME }, + { "BOARD_ID", STR(APJ_BOARD_ID) }, + { "FLASH_SIZE", STR(BOARD_FLASH_SIZE) }, +}; + +/* + substitute variables of the form {VARNAME} + */ +char *BL_Network::substitute_vars(const char *str, uint32_t size) +{ + // assume 1024 is enough room for new variables + char *result = (char *)malloc(strlen(str) + 1024); + if (result == nullptr) { + return nullptr; + } + char *p = result; + const char *str0 = str; + while (*str && str-str0recv(&c, 1, 100); + if (n != 1) { + break; + } + *p++ = c; + if (p-ret >= 4 && strcmp(p-4, "\r\n\r\n") == 0) { + break; + } + } + return ret; +} + +/* + handle an incoming HTTP POST request + */ +void BL_Network::handle_post(SocketAPM *sock, uint32_t content_length) +{ + /* + skip over form boundary. We don't care about the filename as we + only support one file + */ + uint8_t state = 0; + while (true) { + char c; + if (sock->recv(&c, 1, 100) != 1) { + return; + } + content_length--; + // absorb up to \r\n\r\n + if (c == '\r') { + if (state == 2) { + state = 3; + } else { + state = 1; + } + } else if (c == '\n') { + if (state == 1 || state == 3) { + state++; + } else { + state = 0; + } + if (state == 4) { + break; + } + } else { + state = 0; + } + } + /* + erase all of flash + */ + status_printf("Erasing ..."); + flash_set_keep_unlocked(true); + uint32_t sec=0; + while (flash_func_sector_size(sec) != 0 && + flash_func_erase_sector(sec)) { + thread_sleep_ms(10); + sec++; + if (stm32_flash_getpageaddr(sec) - stm32_flash_getpageaddr(0) >= content_length) { + break; + } + } + /* + receive file and write to flash + */ + uint32_t buf[128]; + uint32_t ofs = 0; + + // must be multiple of 4 + content_length &= ~3; + + const uint32_t max_ofs = MIN(BOARD_FLASH_SIZE*1024, content_length); + uint8_t last_pct = 0; + while (ofs < max_ofs) { + const uint32_t needed = MIN(sizeof(buf), max_ofs-ofs); + auto n = sock->recv((void*)buf, needed, 10000); + if (n <= 0) { + break; + } + // we need a whole number of words + if (n % 4 != 0 && n < needed) { + auto n2 = sock->recv(((uint8_t*)buf)+n, 4 - n%4, 10000); + if (n2 > 0) { + n += n2; + } + } + flash_write_buffer(ofs, buf, n/4); + ofs += n; + uint8_t pct = ofs*100/max_ofs; + if (pct % 10 == 0 && last_pct != pct) { + last_pct = pct; + status_printf("Flashing %u%%", unsigned(pct)); + } + } + if (ofs % 32 != 0) { + // pad to 32 bytes + memset(buf, 0xff, sizeof(buf)); + flash_write_buffer(ofs, buf, (32 - ofs%32)/4); + } + flash_write_flush(); + flash_set_keep_unlocked(false); + const auto ok = check_good_firmware(); + if (ok == check_fw_result_t::CHECK_FW_OK) { + need_launch = true; + status_printf("Flash done: OK"); + const char *str = "Flash OK, booting"; + sock->send(str, strlen(str)); + } else { + status_printf("Flash done: ERR:%u", unsigned(ok)); + } +} + +/* + handle an incoming HTTP request + */ +void BL_Network::handle_request(SocketAPM *sock) +{ + /* + read HTTP headers + */ + char *headers = read_headers(sock); + + const char *header = "HTTP/1.1 200 OK\r\n" + "Content-Type: text/html\r\n" + "Connection: close\r\n" + "\r\n"; + sock->send(header, strlen(header)); + + if (strncmp(headers, "POST / ", 7) == 0) { + const char *clen = "\r\nContent-Length:"; + const char *p = strstr(headers, clen); + if (p != nullptr) { + p += strlen(clen); + const uint32_t content_length = atoi(p); + handle_post(sock, content_length); + delete headers; + delete sock; + return; + } + } + + /* + check for async status + */ + const char *get_status = "GET /bootloader_status.html"; + if (strncmp(headers, get_status, strlen(get_status)) == 0) { + { + WITH_SEMAPHORE(status_mtx); + sock->send(bl_status, strlen(bl_status)); + } + delete headers; + delete sock; + return; + } + + const char *get_reboot = "GET /REBOOT"; + if (strncmp(headers, get_reboot, strlen(get_reboot)) == 0) { + need_reboot = true; + } + + uint32_t size = 0; + const auto *msg = AP_ROMFS::find_decompress("index.html", size); + if (need_reboot) { + const char *str = ""; + sock->send(str, strlen(str)); + } else { + char *msg2 = substitute_vars((const char *)msg, size); + sock->send(msg2, strlen(msg2)); + delete msg2; + } + delete headers; + delete sock; + AP_ROMFS::free(msg); +} + +struct req_context { + BL_Network *driver; + SocketAPM *sock; +}; + +void BL_Network::net_request_trampoline(void *ctx) +{ + auto *req = (req_context *)ctx; + req->driver->handle_request(req->sock); + + auto *driver = req->driver; + auto *thd = chThdGetSelfX(); + delete req; + + WITH_SEMAPHORE(driver->web_delete_mtx); + thd->delete_next = driver->web_delete_list; + driver->web_delete_list = thd; +} + +/* + web server thread + */ +void BL_Network::web_server(void) +{ + auto *listen_socket = new SocketAPM(0); + listen_socket->bind("0.0.0.0", 80); + listen_socket->listen(20); + + while (true) { + auto *sock = listen_socket->accept(20); + if (need_reboot) { + need_reboot = false; + NVIC_SystemReset(); + } + if (need_launch) { + need_launch = false; + thread_sleep_ms(1000); + jump_to_app(); + } + if (sock == nullptr) { + continue; + } + // a new thread for each connection to allow for AJAX + auto *req = new req_context; + req->driver = this; + req->sock = sock; + thread_create_alloc(THD_WORKING_AREA_SIZE(2048), + "web_request", + 60, + net_request_trampoline, + req); + + // cleanup any finished threads + WITH_SEMAPHORE(web_delete_mtx); + while (web_delete_list != nullptr) { + auto *thd = web_delete_list; + web_delete_list = thd->delete_next; + chThdRelease(thd); + } + } +} + +/* + initialise bootloader networking + */ +void BL_Network::init() +{ + AP_Networking_ChibiOS::allocate_buffers(); + + macInit(); + + thisif = new netif; + + net_thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(2048), + "network", + 60, + net_thread_trampoline, + this); + + thread_create_alloc(THD_WORKING_AREA_SIZE(2048), + "web_server", + 60, + web_server_trampoline, + this); +} + +/* + save IP address from AP_Periph + */ +void BL_Network::save_comms_ip(void) +{ + struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START; + if (comms->magic == APP_BOOTLOADER_COMMS_MAGIC && comms->ip != 0) { + addr.ip = comms->ip; + addr.netmask = comms->netmask; + addr.gateway = comms->gateway; + } +} + +/* + update status message + */ +void BL_Network::status_printf(const char *fmt, ...) +{ + WITH_SEMAPHORE(status_mtx); + va_list ap; + va_start(ap, fmt); + vsnprintf(bl_status, sizeof(bl_status), fmt, ap); + va_end(ap); + can_printf("%s", bl_status); +} + +#endif // AP_BOOTLOADER_NETWORK_ENABLED + diff --git a/Tools/AP_Bootloader/network.h b/Tools/AP_Bootloader/network.h new file mode 100644 index 00000000000000..f7f55eebebd52b --- /dev/null +++ b/Tools/AP_Bootloader/network.h @@ -0,0 +1,60 @@ +/* + support for networking enabled bootloader + */ + +#include "AP_Bootloader_config.h" + +#if AP_BOOTLOADER_NETWORK_ENABLED + +#include + +class SocketAPM; + +class BL_Network { +public: + void init(void); + void save_comms_ip(void); + +private: + struct netif *thisif; + thread_t *net_thread_ctx; + + HAL_Semaphore web_delete_mtx; + thread_t *web_delete_list; + + static void net_thread_trampoline(void*); + static void web_server_trampoline(void*); + + void net_thread(void); + void web_server(void); + static void net_request_trampoline(void *); + void handle_request(SocketAPM *); + void handle_post(SocketAPM *, uint32_t content_length); + char *read_headers(SocketAPM *); + + static void link_up_cb(void *p); + static void link_down_cb(void *p); + static int8_t low_level_output(struct netif *netif, struct pbuf *p); + static bool low_level_input(struct netif *netif, struct pbuf **pbuf); + static int8_t ethernetif_init(struct netif *netif); + + static char *substitute_vars(const char *msg, uint32_t size); + static struct web_var { + const char *name; + const char *value; + } variables[]; + + struct { + uint32_t ip, gateway, netmask; + } addr; + + bool need_reboot; + bool need_launch; + HAL_Semaphore status_mtx; + char bl_status[256]; + + void status_printf(const char *fmt, ...); +}; + +#endif // AP_BOOTLOADER_NETWORK_ENABLED + diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index ddf1041da48e37..86d51d35f2a75e 100644 --- a/Tools/AP_Bootloader/support.cpp +++ b/Tools/AP_Bootloader/support.cpp @@ -22,6 +22,10 @@ // optional uprintf() code for debug // #define BOOTLOADER_DEBUG SD1 +#ifndef AP_BOOTLOADER_ALWAYS_ERASE +#define AP_BOOTLOADER_ALWAYS_ERASE 0 +#endif + #if defined(BOOTLOADER_DEV_LIST) static BaseChannel *uarts[] = { BOOTLOADER_DEV_LIST }; #if HAL_USE_SERIAL == TRUE @@ -34,10 +38,6 @@ static uint8_t last_uart; #define BOOTLOADER_BAUDRATE 115200 #endif -#ifndef AP_BOOTLOADER_ALWAYS_ERASE -#define AP_BOOTLOADER_ALWAYS_ERASE 0 -#endif - // #pragma GCC optimize("O0") static bool cin_data(uint8_t *data, uint8_t len, unsigned timeout_ms) @@ -353,7 +353,7 @@ void uprintf(const char *fmt, ...) #endif } -static void thread_sleep_ms(uint32_t ms) +void thread_sleep_ms(uint32_t ms) { while (ms > 0) { // don't sleep more than 65 at a time, to cope with 16 bit @@ -486,3 +486,44 @@ void port_setbaud(uint32_t baudrate) } #endif // BOOTLOADER_DEV_LIST +#ifdef STM32H7 +/* + check if flash has any ECC errors and if it does then erase all of + flash + */ +void check_ecc_errors(void) +{ + __disable_fault_irq(); + auto *dma = dmaStreamAlloc(STM32_DMA_STREAM_ID(1, 1), 0, nullptr, nullptr); + uint32_t buf[32]; + uint32_t ofs = 0; + while (ofs < BOARD_FLASH_SIZE*1024) { + if (FLASH->SR1 != 0) { + break; + } +#if BOARD_FLASH_SIZE > 1024 + if (FLASH->SR2 != 0) { + break; + } +#endif + dmaStartMemCopy(dma, + STM32_DMA_CR_PL(0) | STM32_DMA_CR_PSIZE_BYTE | + STM32_DMA_CR_MSIZE_BYTE, + ofs+(uint8_t*)FLASH_BASE, buf, sizeof(buf)); + dmaWaitCompletion(dma); + ofs += sizeof(buf); + } + dmaStreamFree(dma); + + if (ofs < BOARD_FLASH_SIZE*1024) { + // we must have ECC errors in flash + flash_set_keep_unlocked(true); + for (uint32_t i=0; i= 0) { serial_manager.set_protocol_and_baud(g.gps_port, AP_SerialManager::SerialProtocol_GPS, AP_SERIALMANAGER_GPS_BAUD); @@ -281,6 +285,10 @@ void AP_Periph_FW::init() notify.init(); #endif +#ifdef HAL_PERIPH_ENABLE_RELAY + relay.init(); +#endif + #if AP_SCRIPTING_ENABLED scripting.init(); #endif @@ -586,6 +594,14 @@ void AP_Periph_FW::prepare_reboot() hal.scheduler->delay(40); } +/* + reboot, optionally holding in bootloader. For scripting + */ +void AP_Periph_FW::reboot(bool hold_in_bootloader) +{ + hal.scheduler->reboot(hold_in_bootloader); +} + AP_Periph_FW *AP_Periph_FW::_singleton; AP_Periph_FW& AP::periph() diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index b0e0839d6f08c9..424e341c1c8c14 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -32,10 +32,32 @@ #if HAL_WITH_ESC_TELEM #include #endif +#ifdef HAL_PERIPH_ENABLE_RTC +#include +#endif #include #include "rc_in.h" #include "batt_balance.h" #include "networking.h" +#include "serial_options.h" +#if AP_SIM_ENABLED +#include +#endif +#include + +#ifdef HAL_PERIPH_ENABLE_RELAY +#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT + #error "Relay and PWM_HARDPOINT both use hardpoint message" +#endif +#include +#if !AP_RELAY_ENABLED + #error "HAL_PERIPH_ENABLE_RELAY requires AP_RELAY_ENABLED" +#endif +#endif + +#if defined(HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE) && !AP_TEMPERATURE_SENSOR_ENABLED + #error "HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE requires AP_TEMPERATURE_SENSOR_ENABLED" +#endif #include #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) @@ -331,9 +353,18 @@ class AP_Periph_FW { void batt_balance_update(); BattBalance battery_balance; #endif + +#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS + SerialOptions serial_options; +#endif #if AP_TEMPERATURE_SENSOR_ENABLED AP_TemperatureSensor temperature_sensor; +#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE + void temperature_sensor_update(); + uint32_t temperature_last_send_ms; + uint8_t temperature_last_sent_index; +#endif #endif #if defined(HAL_PERIPH_ENABLE_NOTIFY) || defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) @@ -375,6 +406,11 @@ class AP_Periph_FW { #if HAL_GCS_ENABLED GCS_Periph _gcs; #endif + +#ifdef HAL_PERIPH_ENABLE_RELAY + AP_Relay relay; +#endif + // setup the var_info table AP_Param param_loader{var_info}; @@ -424,8 +460,16 @@ class AP_Periph_FW { uint16_t data_type_id, uint8_t priority, const void* payload, - uint16_t payload_len); - + uint16_t payload_len, + uint8_t iface_mask=0); + + bool canard_respond(CanardInstance* canard_instance, + CanardRxTransfer* transfer, + uint64_t data_type_signature, + uint16_t data_type_id, + const uint8_t *payload, + uint16_t payload_len); + void onTransferReceived(CanardInstance* canard_instance, CanardRxTransfer* transfer); bool shouldAcceptTransfer(const CanardInstance* canard_instance, @@ -433,7 +477,10 @@ class AP_Periph_FW { uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id); - + + // reboot the peripheral, optionally holding in bootloader + void reboot(bool hold_in_bootloader); + #if AP_UART_MONITOR_ENABLED void handle_tunnel_Targetted(CanardInstance* canard_instance, CanardRxTransfer* transfer); void send_serial_monitor_data(); @@ -466,6 +513,7 @@ class AP_Periph_FW { void handle_beep_command(CanardInstance* canard_instance, CanardRxTransfer* transfer); void handle_lightscommand(CanardInstance* canard_instance, CanardRxTransfer* transfer); void handle_notify_state(CanardInstance* canard_instance, CanardRxTransfer* transfer); + void handle_hardpoint_command(CanardInstance* canard_instance, CanardRxTransfer* transfer); void process1HzTasks(uint64_t timestamp_usec); void processTx(void); @@ -473,7 +521,7 @@ class AP_Periph_FW { #if HAL_PERIPH_CAN_MIRROR void processMirror(void); #endif // HAL_PERIPH_CAN_MIRROR - void cleanup_stale_transactions(uint64_t ×tamp_usec); + void cleanup_stale_transactions(uint64_t timestamp_usec); void update_rx_protocol_stats(int16_t res); void node_status_send(void); bool can_do_dna(); diff --git a/Tools/AP_Periph/GCS_MAVLink.h b/Tools/AP_Periph/GCS_MAVLink.h index ce1f17b1ba2ed7..c4ec45bb290f0e 100644 --- a/Tools/AP_Periph/GCS_MAVLink.h +++ b/Tools/AP_Periph/GCS_MAVLink.h @@ -29,7 +29,6 @@ class GCS_MAVLINK_Periph : public GCS_MAVLINK private: uint32_t telem_delay() const override { return 0; } - void handleMessage(const mavlink_message_t &msg) override { handle_common_message(msg); } bool handle_guided_request(AP_Mission::Mission_Command &cmd) override { return true; } MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; uint8_t sysid_my_gcs() const override; diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 27c985a3bf6218..3b00ce8bafc85e 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -610,6 +610,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(battery_balance, "BAL", BattBalance), #endif +#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS + // @Group: UART + // @Path: serial_options.cpp + GOBJECT(serial_options, "UART", SerialOptions), +#endif + // NOTE: sim parameters should go last #if AP_SIM_ENABLED // @Group: SIM_ @@ -638,6 +644,23 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { GOBJECT(rtc, "RTC", AP_RTC), #endif +#ifdef HAL_PERIPH_ENABLE_RELAY + // @Group: RELAY + // @Path: ../libraries/AP_Relay/AP_Relay.cpp + GOBJECT(relay, "RELAY", AP_Relay), +#endif + +#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE + // @Param: TEMP_MSG_RATE + // @DisplayName: Temperature sensor message rate + // @Description: This is the rate Temperature sensor data is sent in Hz. Zero means no send. Each sensor with source DroneCAN is sent in turn. + // @Units: Hz + // @Range: 0 200 + // @Increment: 1 + // @User: Standard + GSCALAR(temperature_msg_rate, "TEMP_MSG_RATE", 0), +#endif + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index ff64f16d1e9508..94ac49b39ac05c 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -88,6 +88,9 @@ class Parameters { k_param_can_terminate0, k_param_can_terminate1, k_param_can_terminate2, + k_param_serial_options, + k_param_relay, + k_param_temperature_msg_rate, }; AP_Int16 format_version; @@ -201,7 +204,11 @@ class Parameters { #if HAL_PERIPH_CAN_MIRROR AP_Int8 can_mirror_ports; #endif // HAL_PERIPH_CAN_MIRROR - + +#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE + AP_Int8 temperature_msg_rate; +#endif + #if HAL_CANFD_SUPPORTED AP_Int8 can_fdmode; AP_Int8 can_fdbaudrate[HAL_NUM_CAN_IFACES]; diff --git a/Tools/AP_Periph/Web/scripts/pppgw_webui.lua b/Tools/AP_Periph/Web/scripts/pppgw_webui.lua new file mode 100644 index 00000000000000..3390a89dc4bbdb --- /dev/null +++ b/Tools/AP_Periph/Web/scripts/pppgw_webui.lua @@ -0,0 +1,994 @@ +--[[ + example script to test lua socket API +--]] + +PARAM_TABLE_KEY = 47 +PARAM_TABLE_PREFIX = "WEB_" + +-- 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 Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'net_test: could not add param table') + +--[[ + // @Param: WEB_ENABLE + // @DisplayName: enable web server + // @Description: enable web server + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local WEB_ENABLE = bind_add_param('ENABLE', 1, 1) + +--[[ + // @Param: WEB_BIND_PORT + // @DisplayName: web server TCP port + // @Description: web server TCP port + // @Range: 1 65535 + // @User: Standard +--]] +local WEB_BIND_PORT = bind_add_param('BIND_PORT', 2, 80) + +--[[ + // @Param: WEB_DEBUG + // @DisplayName: web server debugging + // @Description: web server debugging + // @Values: 0:Disabled,1:Enabled + // @User: Advanced +--]] +local WEB_DEBUG = bind_add_param('DEBUG', 3, 0) + +--[[ + // @Param: WEB_BLOCK_SIZE + // @DisplayName: web server block size + // @Description: web server block size for download + // @Range: 1 65535 + // @User: Advanced +--]] +local WEB_BLOCK_SIZE = bind_add_param('BLOCK_SIZE', 4, 10240) + +--[[ + // @Param: WEB_TIMEOUT + // @DisplayName: web server timeout + // @Description: timeout for inactive connections + // @Units: s + // @Range: 0.1 60 + // @User: Advanced +--]] +local WEB_TIMEOUT = bind_add_param('TIMEOUT', 5, 2.0) + +--[[ + // @Param: WEB_SENDFILE_MIN + // @DisplayName: web server minimum file size for sendfile + // @Description: sendfile is an offloading mechanism for faster file download. If this is non-zero and the file is larger than this size then sendfile will be used for file download + // @Range: 0 10000000 + // @User: Advanced +--]] +local WEB_SENDFILE_MIN = bind_add_param('SENDFILE_MIN', 6, 100000) + +if WEB_ENABLE:get() ~= 1 then + periph:can_printf("WebServer: disabled") + return +end + +periph:can_printf(string.format("WebServer: starting on port %u", WEB_BIND_PORT:get())) + +local sock_listen = Socket(0) +local clients = {} + +local DOCTYPE = "" +local SERVER_VERSION = "net_webserver 1.0" +local CONTENT_TEXT_HTML = "text/html;charset=UTF-8" +local CONTENT_OCTET_STREAM = "application/octet-stream" + +local HIDDEN_FOLDERS = { "@SYS", "@ROMFS", "@MISSION", "@PARAM" } + +local MNT_PREFIX = "/mnt" +local MNT_PREFIX2 = MNT_PREFIX .. "/" + +local MIME_TYPES = { + ["apj"] = CONTENT_OCTET_STREAM, + ["dat"] = CONTENT_OCTET_STREAM, + ["o"] = CONTENT_OCTET_STREAM, + ["obj"] = CONTENT_OCTET_STREAM, + ["lua"] = "text/x-lua", + ["py"] = "text/x-python", + ["shtml"] = CONTENT_TEXT_HTML, + ["js"] = "text/javascript", + -- thanks to https://developer.mozilla.org/en-US/docs/Web/HTTP/Basics_of_HTTP/MIME_types/Common_types + ["aac"] = "audio/aac", + ["abw"] = "application/x-abiword", + ["arc"] = "application/x-freearc", + ["avif"] = "image/avif", + ["avi"] = "video/x-msvideo", + ["azw"] = "application/vnd.amazon.ebook", + ["bin"] = "application/octet-stream", + ["bmp"] = "image/bmp", + ["bz"] = "application/x-bzip", + ["bz2"] = "application/x-bzip2", + ["cda"] = "application/x-cdf", + ["csh"] = "application/x-csh", + ["css"] = "text/css", + ["csv"] = "text/csv", + ["doc"] = "application/msword", + ["docx"] = "application/vnd.openxmlformats-officedocument.wordprocessingml.document", + ["eot"] = "application/vnd.ms-fontobject", + ["epub"] = "application/epub+zip", + ["gz"] = "application/gzip", + ["gif"] = "image/gif", + ["htm"] = CONTENT_TEXT_HTML, + ["html"] = CONTENT_TEXT_HTML, + ["ico"] = "image/vnd.microsoft.icon", + ["ics"] = "text/calendar", + ["jar"] = "application/java-archive", + ["jpeg"] = "image/jpeg", + ["json"] = "application/json", + ["jsonld"] = "application/ld+json", + ["mid"] = "audio/x-midi", + ["mjs"] = "text/javascript", + ["mp3"] = "audio/mpeg", + ["mp4"] = "video/mp4", + ["mpeg"] = "video/mpeg", + ["mpkg"] = "application/vnd.apple.installer+xml", + ["odp"] = "application/vnd.oasis.opendocument.presentation", + ["ods"] = "application/vnd.oasis.opendocument.spreadsheet", + ["odt"] = "application/vnd.oasis.opendocument.text", + ["oga"] = "audio/ogg", + ["ogv"] = "video/ogg", + ["ogx"] = "application/ogg", + ["opus"] = "audio/opus", + ["otf"] = "font/otf", + ["png"] = "image/png", + ["pdf"] = "application/pdf", + ["php"] = "application/x-httpd-php", + ["ppt"] = "application/vnd.ms-powerpoint", + ["pptx"] = "application/vnd.openxmlformats-officedocument.presentationml.presentation", + ["rar"] = "application/vnd.rar", + ["rtf"] = "application/rtf", + ["sh"] = "application/x-sh", + ["svg"] = "image/svg+xml", + ["tar"] = "application/x-tar", + ["tif"] = "image/tiff", + ["tiff"] = "image/tiff", + ["ts"] = "video/mp2t", + ["ttf"] = "font/ttf", + ["txt"] = "text/plain", + ["vsd"] = "application/vnd.visio", + ["wav"] = "audio/wav", + ["weba"] = "audio/webm", + ["webm"] = "video/webm", + ["webp"] = "image/webp", + ["woff"] = "font/woff", + ["woff2"] = "font/woff2", + ["xhtml"] = "application/xhtml+xml", + ["xls"] = "application/vnd.ms-excel", + ["xlsx"] = "application/vnd.openxmlformats-officedocument.spreadsheetml.sheet", + ["xml"] = "default.", + ["xul"] = "application/vnd.mozilla.xul+xml", + ["zip"] = "application/zip", + ["3gp"] = "video", + ["3g2"] = "video", + ["7z"] = "application/x-7z-compressed", +} + +--[[ + builtin dynamic pages +--]] +local DYNAMIC_PAGES = { + +-- main home page +["/"] = [[ + + + + + + ArduPilot + + + +

ArduPilot PPP Gateway

+ + + +

Controller Status

+
+ + +]], + +-- board status section on front page +["@DYNAMIC/board_status.shtml"] = [[ + + + + + + + +
Firmware
GIT Hash
Uptime
IP
Netmask
Gateway
+]] +} + +reboot_counter = 0 + +local ACTION_PAGES = { + ["/?FWUPDATE"] = function() + periph:can_printf("Rebooting for firmware update") + reboot_counter = 50 + end +} + +--[[ + builtin javascript library functions +--]] +JS_LIBRARY = { + ["dynamic_load"] = [[ + function dynamic_load(div_id, uri, period_ms) { + var xhr = new XMLHttpRequest(); + xhr.open('GET', uri); + + xhr.setRequestHeader("Cache-Control", "no-cache, no-store, max-age=0"); + xhr.setRequestHeader("Expires", "Tue, 01 Jan 1980 1:00:00 GMT"); + xhr.setRequestHeader("Pragma", "no-cache"); + + xhr.onload = function () { + if (xhr.status === 200) { + var output = document.getElementById(div_id); + if (uri.endsWith('.shtml') || uri.endsWith('.html')) { + output.innerHTML = xhr.responseText; + } else { + output.textContent = xhr.responseText; + } + } + setTimeout(function() { dynamic_load(div_id,uri, period_ms); }, period_ms); + } + xhr.send(); + } +]] +} + +if not sock_listen:bind("0.0.0.0", WEB_BIND_PORT:get()) then + periph:can_printf(string.format("WebServer: failed to bind to TCP %u", WEB_BIND_PORT:get())) + return +end + +if not sock_listen:listen(20) then + periph:can_printf("WebServer: failed to listen") + return +end + +function hms_uptime() + local s = (millis()/1000):toint() + local min = math.floor(s / 60) % 60 + local hr = math.floor(s / 3600) + return string.format("%u hours %u minutes %u seconds", hr, min, s%60) +end + +--[[ + split string by pattern +--]] +local function split(str, pattern) + local ret = {} + for s in string.gmatch(str, pattern) do + table.insert(ret, s) + end + return ret +end + +--[[ + return true if a string ends in the 2nd string +--]] +local function endswith(str, s) + local len1 = #str + local len2 = #s + return string.sub(str,1+len1-len2,len1) == s +end + +--[[ + return true if a string starts with the 2nd string +--]] +local function startswith(str, s) + return string.sub(str,1,#s) == s +end + +local debug_count=0 + +function DEBUG(txt) + if WEB_DEBUG:get() ~= 0 then + periph:can_printf(txt .. string.format(" [%u]", debug_count)) + debug_count = debug_count + 1 + end +end + +--[[ + return index of element in a table +--]] +function table_index(t,el) + for i,v in ipairs(t) do + if v == el then + return i + end + end + return nil +end + +--[[ + return true if a table contains a given element +--]] +function table_contains(t,el) + local i = table_index(t, el) + return i ~= nil +end + +function is_hidden_dir(path) + return table_contains(HIDDEN_FOLDERS, path) +end + +local DAYS = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat" } +local MONTHS = { "Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec" } + +function isdirectory(path) + local s = fs:stat(path) + return s and s:is_directory() +end + +--[[ + time string for directory listings +--]] +function file_timestring(path) + local s = fs:stat(path) + if not s then + return "" + end + local mtime = s:mtime() + local year, month, day, hour, min, sec, _ = rtc:clock_s_to_date_fields(mtime) + if not year then + return "" + end + return string.format("%04u-%02u-%02u %02u:%02u", year, month+1, day, hour, min, sec) +end + +--[[ + time string for Last-Modified +--]] +function file_timestring_http(mtime) + local year, month, day, hour, min, sec, wday = rtc:clock_s_to_date_fields(mtime) + if not year then + return "" + end + return string.format("%s, %02u %s %u %02u:%02u:%02u GMT", + DAYS[wday+1], + day, + MONTHS[month+1], + year, + hour, + min, + sec) +end + +--[[ + parse a http time string to a uint32_t seconds timestamp +--]] +function file_timestring_http_parse(tstring) + local dayname, day, monthname, year, hour, min, sec = + string.match(tstring, + '(%w%w%w), (%d+) (%w%w%w) (%d%d%d%d) (%d%d):(%d%d):(%d%d) GMT') + if not dayname then + return nil + end + local mon = table_index(MONTHS, monthname) + return rtc:date_fields_to_clock_s(year, mon-1, day, hour, min, sec) +end + +--[[ + return true if path exists and is not a directory +--]] +function file_exists(path) + local s = fs:stat(path) + if not s then + return false + end + return not s:is_directory() +end + +--[[ + substitute variables of form {xxx} from a table + from http://lua-users.org/wiki/StringInterpolation +--]] +function substitute_vars(s, vars) + s = (string.gsub(s, "({([^}]+)})", + function(whole,i) + return vars[i] or whole + end)) + return s +end + +--[[ + lat or lon as a string, working around limited type in ftoa_engine +--]] +function latlon_str(ll) + local ipart = tonumber(string.match(tostring(ll*1.0e-7), '(.*[.]).*')) + local fpart = math.abs(ll - ipart*10000000) + return string.format("%d.%u", ipart, fpart, ipart*10000000, ll) +end + +--[[ + location string for home page +--]] +function location_string(loc) + return substitute_vars([[{lat} {lon} {alt}]], + { ["lat"] = latlon_str(loc:lat()), + ["lon"] = latlon_str(loc:lng()), + ["alt"] = string.format("%.1fm", loc:alt()*1.0e-2) }) +end + +--[[ + client class for open connections +--]] +local function Client(_sock, _idx) + local self = {} + + self.closed = false + + local sock = _sock + local idx = _idx + local have_header = false + local header = "" + local header_lines = {} + local header_vars = {} + local run = nil + local protocol = nil + local file = nil + local start_time = millis() + local offset = 0 + + function self.read_header() + local s = sock:recv(2048) + if not s then + local now = millis() + if not sock:is_connected() or now - start_time > WEB_TIMEOUT:get()*1000 then + -- EOF while looking for header + DEBUG(string.format("%u: EOF", idx)) + self.remove() + return false + end + return false + end + if not s or #s == 0 then + return false + end + header = header .. s + local eoh = string.find(s, '\r\n\r\n') + if eoh then + DEBUG(string.format("%u: got header", idx)) + have_header = true + header_lines = split(header, "[^\r\n]+") + -- blocking for reply + sock:set_blocking(true) + return true + end + return false + end + + function self.sendstring(s) + sock:send(s, #s) + end + + function self.sendline(s) + self.sendstring(s .. "\r\n") + end + + --[[ + send a string with variable substitution using {varname} + --]] + function self.sendstring_vars(s, vars) + self.sendstring(substitute_vars(s, vars)) + end + + function self.send_header(code, codestr, vars) + self.sendline(string.format("%s %u %s", protocol, code, codestr)) + self.sendline(string.format("Server: %s", SERVER_VERSION)) + for k,v in pairs(vars) do + self.sendline(string.format("%s: %s", k, v)) + end + self.sendline("Connection: close") + self.sendline("") + end + + -- get size of a file + function self.file_size(fname) + local s = fs:stat(fname) + if not s then + return 0 + end + local ret = s:size():toint() + DEBUG(string.format("%u: size of '%s' -> %u", idx, fname, ret)) + return ret + end + + + --[[ + return full path with .. resolution + --]] + function self.full_path(path, name) + DEBUG(string.format("%u: full_path(%s,%s)", idx, path, name)) + local ret = path + if path == "/" and startswith(name,"@") then + return name + end + if name == ".." then + if path == "/" then + return "/" + end + if endswith(path,"/") then + path = string.sub(path, 1, #path-1) + end + local dir, _ = string.match(path, '(.*/)(.*)') + if not dir then + return path + end + return dir + end + if not endswith(ret, "/") then + ret = ret .. "/" + end + ret = ret .. name + DEBUG(string.format("%u: full_path(%s,%s) -> %s", idx, path, name, ret)) + return ret + end + + function self.directory_list(path) + sock:set_blocking(true) + if startswith(path, "/@") then + path = string.sub(path, 2, #path-1) + end + DEBUG(string.format("%u: directory_list(%s)", idx, path)) + local dlist = dirlist(path) + if not dlist then + dlist = {} + end + if not table_contains(dlist, "..") then + -- on ChibiOS we don't get .. + table.insert(dlist, "..") + end + if path == "/" then + for _,v in ipairs(HIDDEN_FOLDERS) do + table.insert(dlist, v) + end + end + + table.sort(dlist) + self.send_header(200, "OK", {["Content-Type"]=CONTENT_TEXT_HTML}) + self.sendline(DOCTYPE) + self.sendstring_vars([[ + + + Index of {path} + + +

Index of {path}

+ + +]], {path=path}) + for _,d in ipairs(dlist) do + local skip = d == "." + if not skip then + local fullpath = self.full_path(path, d) + local name = d + local sizestr = "0" + local stat = fs:stat(fullpath) + local size = stat and stat:size() or 0 + if is_hidden_dir(fullpath) or (stat and stat:is_directory()) then + name = name .. "/" + elseif size >= 100*1000*1000 then + sizestr = string.format("%uM", (size/(1000*1000)):toint()) + else + sizestr = tostring(size) + end + local modtime = file_timestring(fullpath) + self.sendstring_vars([[ +]], { name=name, size=sizestr, modtime=modtime }) + end + end + self.sendstring([[ +
NameLast modifiedSize
{name}{modtime}{size}
+ + +]]) + end + + -- send file content + function self.send_file() + if not sock:pollout(0) then + return + end + local chunk = WEB_BLOCK_SIZE:get() + local b = file:read(chunk) + sock:set_blocking(true) + if b and #b > 0 then + local sent = sock:send(b, #b) + if sent == -1 then + run = nil + self.remove() + return + end + if sent < #b then + file:seek(offset+sent) + end + offset = offset + sent + end + if not b or #b < chunk then + -- EOF + DEBUG(string.format("%u: sent file", idx)) + run = nil + self.remove() + return + end + end + + --[[ + load whole file as a string + --]] + function self.load_file() + local chunk = WEB_BLOCK_SIZE:get() + local ret = "" + while true do + local b = file:read(chunk) + if not b or #b == 0 then + break + end + ret = ret .. b + end + return ret + end + + --[[ + evaluate some lua code and return as a string + --]] + function self.evaluate(code) + local eval_code = "function eval_func()\n" .. code .. "\nend\n" + local f, errloc, err = load(eval_code, "eval_func", "t", _ENV) + if not f then + DEBUG(string.format("load failed: err=%s errloc=%s", err, errloc)) + return nil + end + local success, err2 = pcall(f) + if not success then + DEBUG(string.format("pcall failed: err=%s", err2)) + return nil + end + local ok, s2 = pcall(eval_func) + eval_func = nil + if ok then + return s2 + end + return nil + end + + --[[ + process a file as a lua CGI + --]] + function self.send_cgi() + sock:set_blocking(true) + local contents = self.load_file() + local s = self.evaluate(contents) + if s then + self.sendstring(s) + end + self.remove() + end + + --[[ + send file content with server side processsing + files ending in .shtml can have embedded lua lika this: + + + + Using 'lstr' a return tostring(yourcode) is added to the code + automatically + --]] + function self.send_processed_file(dynamic_page) + sock:set_blocking(true) + local contents + if dynamic_page then + contents = file + else + contents = self.load_file() + end + while #contents > 0 do + local pat1 = "(.-)[<][?]lua[ \n](.-)[?][>](.*)" + local pat2 = "(.-)[<][?]lstr[ \n](.-)[?][>](.*)" + local p1, p2, p3 = string.match(contents, pat1) + if not p1 then + p1, p2, p3 = string.match(contents, pat2) + if not p1 then + break + end + p2 = "return tostring(" .. p2 .. ")" + end + self.sendstring(p1) + local s2 = self.evaluate(p2) + if s2 then + self.sendstring(s2) + end + contents = p3 + end + self.sendstring(contents) + self.remove() + end + + -- return a content type + function self.content_type(path) + if path == "/" then + return MIME_TYPES["html"] + end + local _, ext = string.match(path, '(.*[.])(.*)') + ext = string.lower(ext) + local ret = MIME_TYPES[ext] + if not ret then + return CONTENT_OCTET_STREAM + end + return ret + end + + -- perform a file download + function self.file_download(path) + if startswith(path, "/@") then + path = string.sub(path, 2, #path) + end + DEBUG(string.format("%u: file_download(%s)", idx, path)) + file = DYNAMIC_PAGES[path] + dynamic_page = file ~= nil + if not dynamic_page then + file = io.open(path,"rb") + if not file then + DEBUG(string.format("%u: Failed to open '%s'", idx, path)) + return false + end + end + local vars = {["Content-Type"]=self.content_type(path)} + local cgi_processing = startswith(path, "/cgi-bin/") and endswith(path, ".lua") + local server_side_processing = endswith(path, ".shtml") + local stat = fs:stat(path) + if not startswith(path, "@") and + not server_side_processing and + not cgi_processing and stat and + not dynamic_page then + local fsize = stat:size() + local mtime = stat:mtime() + vars["Content-Length"]= tostring(fsize) + local modtime = file_timestring_http(mtime) + if modtime then + vars["Last-Modified"] = modtime + end + local if_modified_since = header_vars['If-Modified-Since'] + if if_modified_since then + local tsec = file_timestring_http_parse(if_modified_since) + if tsec and tsec >= mtime then + DEBUG(string.format("%u: Not modified: %s %s", idx, modtime, if_modified_since)) + self.send_header(304, "Not Modified", vars) + return true + end + end + end + self.send_header(200, "OK", vars) + if server_side_processing or dynamic_page then + DEBUG(string.format("%u: shtml processing %s", idx, path)) + run = self.send_processed_file(dynamic_page) + elseif cgi_processing then + DEBUG(string.format("%u: CGI processing %s", idx, path)) + run = self.send_cgi + elseif stat and + WEB_SENDFILE_MIN:get() > 0 and + stat:size() >= WEB_SENDFILE_MIN:get() and + sock:sendfile(file) then + return true + else + run = self.send_file + end + return true + end + + function self.not_found() + self.send_header(404, "Not found", {}) + end + + function self.moved_permanently(relpath) + if not startswith(relpath, "/") then + relpath = "/" .. relpath + end + local location = string.format("http://%s%s", header_vars['Host'], relpath) + DEBUG(string.format("%u: Redirect -> %s", idx, location)) + self.send_header(301, "Moved Permanently", {["Location"]=location}) + end + + -- process a single request + function self.process_request() + local h1 = header_lines[1] + if not h1 or #h1 == 0 then + DEBUG(string.format("%u: empty request", idx)) + return + end + local cmd = split(header_lines[1], "%S+") + if not cmd or #cmd < 3 then + DEBUG(string.format("bad request: %s", header_lines[1])) + return + end + if cmd[1] ~= "GET" then + DEBUG(string.format("bad op: %s", cmd[1])) + return + end + protocol = cmd[3] + if protocol ~= "HTTP/1.0" and protocol ~= "HTTP/1.1" then + DEBUG(string.format("bad protocol: %s", protocol)) + return + end + local path = cmd[2] + DEBUG(string.format("%u: path='%s'", idx, path)) + + -- extract header variables + for i = 2,#header_lines do + local key, var = string.match(header_lines[i], '(.*): (.*)') + if key then + header_vars[key] = var + end + end + + if ACTION_PAGES[path] ~= nil then + DEBUG(string.format("Running ACTION %s", path)) + local fn = ACTION_PAGES[path] + self.send_header(200, "OK", {["Content-Type"]=CONTENT_TEXT_HTML}) + self.sendstring([[ + + + + + +]]) + fn() + return + end + + if DYNAMIC_PAGES[path] ~= nil then + self.file_download(path) + return + end + + if path == MNT_PREFIX then + path = "/" + end + if startswith(path, MNT_PREFIX2) then + path = string.sub(path,#MNT_PREFIX2,#path) + end + + if isdirectory(path) and + not endswith(path,"/") and + header_vars['Host'] and + not is_hidden_dir(path) then + self.moved_permanently(path .. "/") + return + end + + if path ~= "/" and endswith(path,"/") then + path = string.sub(path, 1, #path-1) + end + + if startswith(path,"/@") then + path = string.sub(path, 2, #path) + end + + -- see if we have an index file + if isdirectory(path) and file_exists(path .. "/index.html") then + DEBUG(string.format("%u: found index.html", idx)) + if self.file_download(path .. "/index.html") then + return + end + end + + -- see if it is a directory + if (path == "/" or + DYNAMIC_PAGES[path] == nil) and + (endswith(path,"/") or + isdirectory(path) or + is_hidden_dir(path)) then + self.directory_list(path) + return + end + + -- or a file + if self.file_download(path) then + return + end + self.not_found(path) + end + + -- update the client + function self.update() + if run then + run() + return + end + if not have_header then + if not self.read_header() then + return + end + end + self.process_request() + if not run then + -- nothing more to do + self.remove() + end + end + + function self.remove() + DEBUG(string.format("%u: removing client OFFSET=%u", idx, offset)) + sock:close() + self.closed = true + end + + -- return the instance + return self +end + +--[[ + see if any new clients want to connect +--]] +local function check_new_clients() + while sock_listen:pollin(0) do + local sock = sock_listen:accept() + if not sock then + return + end + -- non-blocking for header read + sock:set_blocking(false) + -- find free client slot + for i = 1, #clients+1 do + if clients[i] == nil then + local idx = i + local client = Client(sock, idx) + DEBUG(string.format("%u: New client", idx)) + clients[idx] = client + end + end + end +end + +--[[ + check for client activity +--]] +local function check_clients() + for idx,client in ipairs(clients) do + if not client.closed then + client.update() + end + if client.closed then + table.remove(clients,idx) + end + end +end + +local function update() + check_new_clients() + check_clients() + if reboot_counter then + reboot_counter = reboot_counter - 1 + if reboot_counter == 0 then + periph:can_printf("Rebooting") + periph:reboot(true) + end + end + return update,5 +end + +return update,100 diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 89611c5cac5477..1c658463a6e84a 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -34,6 +34,7 @@ #include #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL #include +#include #endif #define IFACE_ALL ((1U<<(HAL_NUM_CAN_IFACES))-1U) @@ -62,6 +63,9 @@ extern AP_Periph_FW periph; #endif #endif +// timeout all frames at 1s +#define CAN_FRAME_TIMEOUT 1000000ULL + #define DEBUG_PKTS 0 #if HAL_PERIPH_CAN_MIRROR @@ -144,6 +148,10 @@ HALSITL::CANIface* AP_Periph_FW::can_iface_periph[HAL_NUM_CAN_IFACES]; SLCAN::CANIface AP_Periph_FW::slcan_interface; #endif +#ifdef EXT_FLASH_SIZE_MB +static_assert(EXT_FLASH_SIZE_MB == 0, "DroneCAN bootloader cannot support external flash"); +#endif + /* * Node status variables */ @@ -207,25 +215,12 @@ void AP_Periph_FW::handle_get_node_info(CanardInstance* canard_instance, uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer, !canfdout()); - const int16_t resp_res = canardRequestOrRespond(canard_instance, - transfer->source_node_id, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, - UAVCAN_PROTOCOL_GETNODEINFO_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size -#if CANARD_MULTI_IFACE - , IFACE_ALL -#endif -#if HAL_CANFD_SUPPORTED - , canfdout() -#endif -); - if (resp_res <= 0) { - printf("Could not respond to GetNodeInfo: %d\n", resp_res); - } + canard_respond(canard_instance, + transfer, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &buffer[0], + total_size); } /* @@ -316,23 +311,12 @@ void AP_Periph_FW::handle_param_getset(CanardInstance* canard_instance, CanardRx uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {}; uint16_t total_size = uavcan_protocol_param_GetSetResponse_encode(&pkt, buffer, !canfdout()); - canardRequestOrRespond(canard_instance, - transfer->source_node_id, - UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, - UAVCAN_PROTOCOL_PARAM_GETSET_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size -#if CANARD_MULTI_IFACE - , IFACE_ALL -#endif -#if HAL_CANFD_SUPPORTED - ,canfdout() -#endif -); - + canard_respond(canard_instance, + transfer, + UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_GETSET_ID, + &buffer[0], + total_size); } /* @@ -376,30 +360,22 @@ void AP_Periph_FW::handle_param_executeopcode(CanardInstance* canard_instance, C uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE] {}; uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer, !canfdout()); - canardRequestOrRespond(canard_instance, - transfer->source_node_id, - UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, - UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size -#if CANARD_MULTI_IFACE - , IFACE_ALL -#endif -#if HAL_CANFD_SUPPORTED - ,canfdout() -#endif -); + canard_respond(canard_instance, + transfer, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, + &buffer[0], + total_size); } void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, CanardRxTransfer* transfer) { #if HAL_RAM_RESERVE_START >= 256 // setup information on firmware request at start of ram - struct app_bootloader_comms *comms = (struct app_bootloader_comms *)HAL_RAM0_START; - memset(comms, 0, sizeof(struct app_bootloader_comms)); + auto *comms = (struct app_bootloader_comms *)HAL_RAM0_START; + if (comms->magic != APP_BOOTLOADER_COMMS_MAGIC) { + memset(comms, 0, sizeof(*comms)); + } comms->magic = APP_BOOTLOADER_COMMS_MAGIC; uavcan_protocol_file_BeginFirmwareUpdateRequest req; @@ -419,22 +395,12 @@ void AP_Periph_FW::handle_begin_firmware_update(CanardInstance* canard_instance, reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer, !canfdout()); - canardRequestOrRespond(canard_instance, - transfer->source_node_id, - UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, - UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size -#if CANARD_MULTI_IFACE - ,IFACE_ALL -#endif -#if HAL_CANFD_SUPPORTED - ,canfdout() -#endif -); + canard_respond(canard_instance, + transfer, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, + &buffer[0], + total_size); uint8_t count = 50; while (count--) { processTx(); @@ -889,6 +855,13 @@ void AP_Periph_FW::onTransferReceived(CanardInstance* canard_instance, handle_notify_state(canard_instance, transfer); break; #endif + +#ifdef HAL_PERIPH_ENABLE_RELAY + case UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID: + handle_hardpoint_command(canard_instance, transfer); + break; +#endif + } } @@ -997,6 +970,11 @@ bool AP_Periph_FW::shouldAcceptTransfer(const CanardInstance* canard_instance, case ARDUPILOT_INDICATION_NOTIFYSTATE_ID: *out_data_type_signature = ARDUPILOT_INDICATION_NOTIFYSTATE_SIGNATURE; return true; +#endif +#ifdef HAL_PERIPH_ENABLE_RELAY + case UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID: + *out_data_type_signature = UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE; + return true; #endif default: break; @@ -1015,7 +993,7 @@ static bool shouldAcceptTransfer_trampoline(const CanardInstance* canard_instanc return fw->shouldAcceptTransfer(canard_instance, out_data_type_signature, data_type_id, transfer_type, source_node_id); } -void AP_Periph_FW::cleanup_stale_transactions(uint64_t ×tamp_usec) +void AP_Periph_FW::cleanup_stale_transactions(uint64_t timestamp_usec) { canardCleanupStaleTransfers(&dronecan.canard, timestamp_usec); } @@ -1058,9 +1036,11 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void* payload, - uint16_t payload_len) + uint16_t payload_len, + uint8_t iface_mask) { - if (canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) { + const bool is_dna = data_type_id == UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID; + if (!is_dna && canardGetLocalNodeID(&dronecan.canard) == CANARD_BROADCAST_NODE_ID) { return false; } @@ -1069,21 +1049,69 @@ bool AP_Periph_FW::canard_broadcast(uint64_t data_type_signature, return false; } - const int16_t res = canardBroadcast(&dronecan.canard, - data_type_signature, - data_type_id, - tid_ptr, - priority, - payload, - payload_len + // create transfer object + CanardTxTransfer transfer_object = { + .transfer_type = CanardTransferTypeBroadcast, + .data_type_signature = data_type_signature, + .data_type_id = data_type_id, + .inout_transfer_id = tid_ptr, + .priority = priority, + .payload = (uint8_t*)payload, + .payload_len = payload_len, +#if CANARD_ENABLE_CANFD + .canfd = is_dna? false : canfdout(), +#endif + .deadline_usec = AP_HAL::micros64()+CAN_FRAME_TIMEOUT, #if CANARD_MULTI_IFACE - , IFACE_ALL // send over all ifaces + .iface_mask = iface_mask==0 ? uint8_t(IFACE_ALL) : iface_mask, #endif -#if HAL_CANFD_SUPPORTED - , canfdout() + }; + const int16_t res = canardBroadcastObj(&dronecan.canard, &transfer_object); + +#if DEBUG_PKTS + if (res < 0) { + can_printf("Tx error %d\n", res); + } +#endif +#if HAL_ENABLE_SENDING_STATS + if (res <= 0) { + protocol_stats.tx_errors++; + } else { + protocol_stats.tx_frames += res; + } #endif - ); + return res > 0; +} +/* + send a response + */ +bool AP_Periph_FW::canard_respond(CanardInstance* canard_instance, + CanardRxTransfer *transfer, + uint64_t data_type_signature, + uint16_t data_type_id, + const uint8_t *payload, + uint16_t payload_len) +{ + CanardTxTransfer transfer_object = { + .transfer_type = CanardTransferTypeResponse, + .data_type_signature = data_type_signature, + .data_type_id = data_type_id, + .inout_transfer_id = &transfer->transfer_id, + .priority = transfer->priority, + .payload = payload, + .payload_len = payload_len, +#if CANARD_ENABLE_CANFD + .canfd = canfdout(), +#endif + .deadline_usec = AP_HAL::micros64()+CAN_FRAME_TIMEOUT, +#if CANARD_MULTI_IFACE + .iface_mask = IFACE_ALL, +#endif + }; + const auto res = canardRequestOrRespondObj(canard_instance, + transfer->source_node_id, + &transfer_object); #if DEBUG_PKTS if (res < 0) { can_printf("Tx error %d\n", res); @@ -1151,15 +1179,17 @@ void AP_Periph_FW::processTx(void) canardPopTxQueue(&dronecan.canard); dronecan.tx_fail_count = 0; } else { - // just exit and try again later. If we fail 8 times in a row - // then start discarding to prevent the pool filling up + // exit and try again later. If we fail 8 times in a row + // then cleanup any stale transfers to keep the queue from + // filling if (dronecan.tx_fail_count < 8) { dronecan.tx_fail_count++; } else { #if HAL_ENABLE_SENDING_STATS protocol_stats.tx_errors++; #endif - canardPopTxQueue(&dronecan.canard); + dronecan.tx_fail_count = 0; + cleanup_stale_transactions(now_us); } break; } @@ -1214,19 +1244,19 @@ void AP_Periph_FW::update_rx_protocol_stats(int16_t res) void AP_Periph_FW::processRx(void) { AP_HAL::CANFrame rxmsg; - for (auto &ins : instances) { - if (ins.iface == NULL) { + for (auto &instance : instances) { + if (instance.iface == NULL) { continue; } #if HAL_NUM_CAN_IFACES >= 2 - if (can_protocol_cached[ins.index] != AP_CAN::Protocol::DroneCAN) { + if (can_protocol_cached[instance.index] != AP_CAN::Protocol::DroneCAN) { continue; } #endif while (true) { bool read_select = true; bool write_select = false; - ins.iface->select(read_select, write_select, nullptr, 0); + instance.iface->select(read_select, write_select, nullptr, 0); if (!read_select) { // No data pending break; } @@ -1235,7 +1265,7 @@ void AP_Periph_FW::processRx(void) //palToggleLine(HAL_GPIO_PIN_LED); uint64_t timestamp; AP_HAL::CANIface::CanIOFlags flags; - if (ins.iface->receive(rxmsg, timestamp, flags) <= 0) { + if (instance.iface->receive(rxmsg, timestamp, flags) <= 0) { break; } #if HAL_PERIPH_CAN_MIRROR @@ -1256,7 +1286,7 @@ void AP_Periph_FW::processRx(void) #endif rx_frame.id = rxmsg.id; #if CANARD_MULTI_IFACE - rx_frame.iface_id = ins.index; + rx_frame.iface_id = instance.index; #endif const int16_t res = canardHandleRxFrame(&dronecan.canard, &rx_frame, timestamp); @@ -1330,7 +1360,7 @@ void AP_Periph_FW::node_status_send(void) uint8_t buffer[UAVCAN_PROTOCOL_NODESTATUS_MAX_SIZE]; node_status.uptime_sec = AP_HAL::millis() / 1000U; - node_status.vendor_specific_status_code = hal.util->available_memory(); + node_status.vendor_specific_status_code = MIN(hal.util->available_memory(), unsigned(UINT16_MAX)); uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer, !canfdout()); @@ -1351,14 +1381,14 @@ void AP_Periph_FW::node_status_send(void) buffer, len); } - for (auto &ins : instances) { + for (auto &instance : instances) { uint8_t buffer[DRONECAN_PROTOCOL_CANSTATS_MAX_SIZE]; dronecan_protocol_CanStats can_stats; - const AP_HAL::CANIface::bus_stats_t *bus_stats = ins.iface->get_statistics(); + const AP_HAL::CANIface::bus_stats_t *bus_stats = instance.iface->get_statistics(); if (bus_stats == nullptr) { return; } - can_stats.interface = ins.index; + can_stats.interface = instance.index; can_stats.tx_requests = bus_stats->tx_requests; can_stats.tx_rejected = bus_stats->tx_rejected; can_stats.tx_overflow = bus_stats->tx_overflow; @@ -1490,8 +1520,6 @@ bool AP_Periph_FW::can_do_dna() const uint32_t now = AP_HAL::millis(); - static uint8_t node_id_allocation_transfer_id = 0; - if (AP_Periph_FW::no_iface_finished_dna) { printf("Waiting for dynamic node ID allocation %x... (pool %u)\n", IFACE_ALL, pool_peak_percent()); } @@ -1525,24 +1553,11 @@ bool AP_Periph_FW::can_do_dna() memmove(&allocation_request[1], &my_unique_id[dronecan.node_id_allocation_unique_id_offset], uid_size); // Broadcasting the request - const int16_t bcast_res = canardBroadcast(&dronecan.canard, - UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, - UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, - &node_id_allocation_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - &allocation_request[0], - (uint16_t) (uid_size + 1) -#if CANARD_MULTI_IFACE - ,(1U << dronecan.dna_interface) -#endif -#if HAL_CANFD_SUPPORTED - // always send allocation request as non-FD - ,false -#endif - ); - if (bcast_res < 0) { - printf("Could not broadcast ID allocation req; error %d\n", bcast_res); - } + canard_broadcast(UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &allocation_request[0], + (uint16_t) (uid_size + 1)); // Preparing for timeout; if response is received, this value will be updated from the callback. dronecan.node_id_allocation_unique_id_offset = 0; @@ -1812,6 +1827,9 @@ void AP_Periph_FW::can_update() #ifdef HAL_PERIPH_ENABLE_EFI can_efi_update(); #endif +#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE + temperature_sensor_update(); +#endif } const uint32_t now_us = AP_HAL::micros(); while ((AP_HAL::micros() - now_us) < 1000) { diff --git a/Tools/AP_Periph/gps.cpp b/Tools/AP_Periph/gps.cpp index e8262e2efb4345..029676f26d1b8a 100644 --- a/Tools/AP_Periph/gps.cpp +++ b/Tools/AP_Periph/gps.cpp @@ -163,10 +163,10 @@ void AP_Periph_FW::can_gps_update(void) uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout()); canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, - UAVCAN_EQUIPMENT_GNSS_FIX2_ID, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size); + UAVCAN_EQUIPMENT_GNSS_FIX2_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); } /* @@ -258,34 +258,18 @@ void AP_Periph_FW::send_moving_baseline_msg() uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE] {}; const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout()); -#if HAL_NUM_CAN_IFACES >= 2 + uint8_t iface_mask = 0; +#if HAL_NUM_CAN_IFACES >= 2 && CANARD_MULTI_IFACE if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) { - uint8_t *tid_ptr = get_tid_ptr(MAKE_TRANSFER_DESCRIPTOR(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, 0, CANARD_BROADCAST_NODE_ID)); - canardBroadcast(&dronecan.canard, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE, - ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID, - tid_ptr, - CANARD_TRANSFER_PRIORITY_LOW, - &buffer[0], - total_size -#if CANARD_MULTI_IFACE - ,(1U< + extern const AP_HAL::HAL& hal; #define TELEM_HEADER 0x9B diff --git a/Tools/AP_Periph/networking.cpp b/Tools/AP_Periph/networking.cpp index 8ac256850cd9b9..ae82c3fd676aea 100644 --- a/Tools/AP_Periph/networking.cpp +++ b/Tools/AP_Periph/networking.cpp @@ -137,12 +137,32 @@ const AP_Param::GroupInfo Networking_Periph::var_info[] { AP_SUBGROUPINFO(passthru[8], "PASS9_", 19, Networking_Periph, Passthru), #endif +#if AP_NETWORKING_BACKEND_PPP + // @Param: PPP_PORT + // @DisplayName: PPP serial port + // @Description: PPP serial port + // @Range: -1 10 + AP_GROUPINFO("PPP_PORT", 20, Networking_Periph, ppp_port, AP_PERIPH_NET_PPP_PORT_DEFAULT), + + // @Param: PPP_BAUD + // @DisplayName: PPP serial baudrate + // @Description: PPP serial baudrate + // @CopyFieldsFrom: SERIAL1_BAUD + AP_GROUPINFO("PPP_BAUD", 21, Networking_Periph, ppp_baud, AP_PERIPH_NET_PPP_BAUD_DEFAULT), +#endif + AP_GROUPEND }; void Networking_Periph::init(void) { +#if AP_NETWORKING_BACKEND_PPP + if (ppp_port >= 0) { + AP::serialmanager().set_protocol_and_baud(ppp_port, AP_SerialManager::SerialProtocol_PPP, ppp_baud.get()); + } +#endif + networking_lib.init(); #if HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 @@ -161,6 +181,20 @@ void Networking_Periph::update(void) p.update(); } #endif + +#if HAL_RAM_RESERVE_START >= 256 + if (!got_addresses && networking_lib.get_ip_active() != 0) { + got_addresses = true; + auto *comms = (struct app_bootloader_comms *)HAL_RAM0_START; + if (comms->magic != APP_BOOTLOADER_COMMS_MAGIC) { + memset(comms, 0, sizeof(*comms)); + } + comms->magic = APP_BOOTLOADER_COMMS_MAGIC; + comms->ip = networking_lib.get_ip_active(); + comms->netmask = networking_lib.get_netmask_active(); + comms->gateway = networking_lib.get_gateway_active(); + } +#endif // HAL_RAM_RESERVE_START } #endif // HAL_PERIPH_ENABLE_NETWORKING diff --git a/Tools/AP_Periph/networking.h b/Tools/AP_Periph/networking.h index 6b7f857fdcde8c..2392764e2e1f62 100644 --- a/Tools/AP_Periph/networking.h +++ b/Tools/AP_Periph/networking.h @@ -10,6 +10,14 @@ #define HAL_PERIPH_NETWORK_NUM_PASSTHRU 2 #endif +#ifndef AP_PERIPH_NET_PPP_PORT_DEFAULT +#define AP_PERIPH_NET_PPP_PORT_DEFAULT -1 +#endif + +#ifndef AP_PERIPH_NET_PPP_BAUD_DEFAULT +#define AP_PERIPH_NET_PPP_BAUD_DEFAULT 12500000 +#endif + class Networking_Periph { public: Networking_Periph() { @@ -54,7 +62,12 @@ class Networking_Periph { #endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU AP_Networking networking_lib; -}; + bool got_addresses; -#endif // HAL_PERIPH_ENABLE_BATTERY_BALANCE +#if AP_NETWORKING_BACKEND_PPP + AP_Int8 ppp_port; + AP_Int32 ppp_baud; +#endif +}; +#endif // HAL_PERIPH_ENABLE_NETWORKING diff --git a/Tools/AP_Periph/relay.cpp b/Tools/AP_Periph/relay.cpp new file mode 100644 index 00000000000000..925211c2003fce --- /dev/null +++ b/Tools/AP_Periph/relay.cpp @@ -0,0 +1,38 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_RELAY + +#include + +void AP_Periph_FW::handle_hardpoint_command(CanardInstance* canard_instance, CanardRxTransfer* transfer) +{ + uavcan_equipment_hardpoint_Command cmd {}; + if (uavcan_equipment_hardpoint_Command_decode(transfer, &cmd)) { + // Failed to decode + return; + } + + // Command must be 0 or 1, other values may be supported in the future + // rejecting them now ensures no change in behaviour + if ((cmd.command != 0) && (cmd.command != 1)) { + return; + } + + // Translate hardpoint ID to relay function + AP_Relay_Params::FUNCTION fun; + switch (cmd.hardpoint_id) { + case 0 ... 15: + // 0 to 15 are continuous + fun = AP_Relay_Params::FUNCTION(cmd.hardpoint_id + (uint8_t)AP_Relay_Params::FUNCTION::DroneCAN_HARDPOINT_0); + break; + + default: + // ID not supported + return; + } + + // Set relay + relay.set(fun, cmd.command); + +} +#endif diff --git a/Tools/AP_Periph/serial_options.cpp b/Tools/AP_Periph/serial_options.cpp new file mode 100644 index 00000000000000..cc480eee5b7f77 --- /dev/null +++ b/Tools/AP_Periph/serial_options.cpp @@ -0,0 +1,110 @@ +/* + 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 . + */ +/* + serial options support, for serial over DroneCAN + */ + +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS + +#include "serial_options.h" +#include + +extern const AP_HAL::HAL &hal; + +const AP_Param::GroupInfo SerialOptions::var_info[] { + +#if HAL_HAVE_SERIAL0 + // @Group: 0_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[0], "0_", 1, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL1 + // @Group: 1_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[1], "1_", 2, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL2 + // @Group: 2_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[2], "2_", 3, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL3 + // @Group: 3_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[3], "3_", 4, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL4 + // @Group: 4_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[4], "4_", 5, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL5 + // @Group: 5_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[5], "5_", 6, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL6 + // @Group: 6_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[6], "6_", 7, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL7 + // @Group: 7_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[7], "7_", 8, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL8 + // @Group: 8_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[8], "8_", 9, SerialOptions, SerialOptionsDev), +#endif + +#if HAL_HAVE_SERIAL9 + // @Group: 9_ + // @Path: serial_options_dev.cpp + AP_SUBGROUPINFO(devs[9], "9_", 10, SerialOptions, SerialOptionsDev), +#endif + + AP_GROUPEND +}; + +SerialOptions::SerialOptions(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +void SerialOptions::init(void) +{ + for (uint8_t i=0; iset_options(d.options); + uart->set_flow_control(AP_HAL::UARTDriver::flow_control(d.rtscts.get())); + } + } +} + +#endif // HAL_PERIPH_ENABLE_SERIAL_OPTIONS diff --git a/Tools/AP_Periph/serial_options.h b/Tools/AP_Periph/serial_options.h new file mode 100644 index 00000000000000..d71bdf43138117 --- /dev/null +++ b/Tools/AP_Periph/serial_options.h @@ -0,0 +1,30 @@ +#pragma once + +#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS + +#ifndef HAL_UART_NUM_SERIAL_PORTS +#define HAL_UART_NUM_SERIAL_PORTS AP_HAL::HAL::num_serial +#endif + +class SerialOptionsDev { +public: + SerialOptionsDev(void); + static const struct AP_Param::GroupInfo var_info[]; + AP_Int32 options; + AP_Int8 rtscts; +}; + +class SerialOptions { +public: + friend class AP_Periph_FW; + SerialOptions(void); + void init(void); + + static const struct AP_Param::GroupInfo var_info[]; + +private: + SerialOptionsDev devs[HAL_UART_NUM_SERIAL_PORTS]; +}; + + +#endif // HAL_PERIPH_ENABLE_SERIAL_OPTIONS diff --git a/Tools/AP_Periph/serial_options_dev.cpp b/Tools/AP_Periph/serial_options_dev.cpp new file mode 100644 index 00000000000000..26ba2bb137453c --- /dev/null +++ b/Tools/AP_Periph/serial_options_dev.cpp @@ -0,0 +1,47 @@ +/* + 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 . + */ +/* + serial options support, for serial over DroneCAN + */ + +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS + +#include "serial_options.h" + +SerialOptionsDev::SerialOptionsDev(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +const AP_Param::GroupInfo SerialOptionsDev::var_info[] { + + // @Param: OPTIONS + // @DisplayName: Serial options + // @Description: Control over UART options. The InvertRX option controls invert of the receive pin. The InvertTX option controls invert of the transmit pin. The HalfDuplex option controls half-duplex (onewire) mode, where both transmit and receive is done on the transmit wire. The Swap option allows the RX and TX pins to be swapped on STM32F7 based boards. + // @Bitmask: 0:InvertRX, 1:InvertTX, 2:HalfDuplex, 3:SwapTXRX, 4: RX_PullDown, 5: RX_PullUp, 6: TX_PullDown, 7: TX_PullUp, 8: RX_NoDMA, 9: TX_NoDMA, 10: Don't forward mavlink to/from, 11: DisableFIFO, 12: Ignore Streamrate + AP_GROUPINFO("OPTIONS", 1, SerialOptionsDev, options, 0), + + // @Param: RTSCTS + // @DisplayName: Serial1 flow control + // @Description: Enable flow control. You must have the RTS and CTS pins available on the port. If this is set to 2 then flow control will be auto-detected by checking for the output buffer filling on startup. + // @Values: 0:Disabled,1:Enabled,2:Auto + AP_GROUPINFO("RTSCTS", 2, SerialOptionsDev, rtscts, float(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE)), + + AP_GROUPEND +}; + +#endif // HAL_PERIPH_ENABLE_SERIAL_OPTIONS diff --git a/Tools/AP_Periph/temperature.cpp b/Tools/AP_Periph/temperature.cpp new file mode 100644 index 00000000000000..3377e2b63594e7 --- /dev/null +++ b/Tools/AP_Periph/temperature.cpp @@ -0,0 +1,54 @@ +#include "AP_Periph.h" + +#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE + +#include + +// Send temperature message occasionally +void AP_Periph_FW::temperature_sensor_update(void) +{ + if (g.temperature_msg_rate <= 0) { + return; + } + + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - temperature_last_send_ms < (1000U / g.temperature_msg_rate)) { + return; + } + temperature_last_send_ms = now_ms; + + { + const uint8_t num_sensors = temperature_sensor.num_instances(); + for (uint8_t i = 0; i < num_sensors; i++) { + // Send each sensor in turn + const uint8_t index = (temperature_last_sent_index + 1 + i) % num_sensors; + + float temp_deg = 0.0; + if ((temperature_sensor.get_source(index) != AP_TemperatureSensor_Params::Source::DroneCAN) || + !temperature_sensor.get_temperature(temp_deg, index)) { + // not configured to send or Unhealthy + continue; + } + + uavcan_equipment_device_Temperature pkt {}; + pkt.temperature = C_TO_KELVIN(temp_deg); + + // Use source ID from temperature lib + pkt.device_id = temperature_sensor.get_source_id(index); + + uint8_t buffer[UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE] {}; + const uint16_t total_size = uavcan_equipment_device_Temperature_encode(&pkt, buffer, !canfdout()); + + canard_broadcast(UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE, + UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID, + CANARD_TRANSFER_PRIORITY_LOW, + &buffer[0], + total_size); + + temperature_last_sent_index = index; + break; + } + } +} + +#endif // HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE diff --git a/Tools/AP_Periph/version.cpp b/Tools/AP_Periph/version.cpp index f557ce9f89ca4e..e38ffded0c65ad 100644 --- a/Tools/AP_Periph/version.cpp +++ b/Tools/AP_Periph/version.cpp @@ -1,4 +1,3 @@ #define FORCE_VERSION_H_INCLUDE #include "version.h" -#include -#undef FORCE_VERSION_H_INCLUDE \ No newline at end of file +#undef FORCE_VERSION_H_INCLUDE diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index 090841e1d57fdc..6166fba32d1459 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -1,9 +1,19 @@ #pragma once -#include +#ifndef FORCE_VERSION_H_INCLUDE +#error version.h should never be included directly. You probably want to include AP_Common/AP_FWVersion.h +#endif + +#include "ap_version.h" #define THISFIRMWARE "AP_Periph V1.7.0-dev" +// defines needed due to lack of GCS includes +#define FIRMWARE_VERSION_TYPE_DEV 0 +#define FIRMWARE_VERSION_TYPE_BETA 255 +#define FIRMWARE_VERSION_TYPE_OFFICIAL 255 + + // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 1,7,0,FIRMWARE_VERSION_TYPE_DEV @@ -12,7 +22,4 @@ #define FW_PATCH 0 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV - - - - +#include diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index da728bdcf4e282..a3b92b0150a111 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -81,6 +81,7 @@ def build(bld): 'AP_RobotisServo', 'AP_FETtecOneWire', 'GCS_MAVLink', + 'AP_Relay' ] bld.ap_stlib( diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index a20db4924d95d6..3e99d61982abad 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -10,12 +10,17 @@ #include #include #include +#include #include #include "EKF_Maths.h" -#if HAL_WITH_DSP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#if HAL_WITH_DSP #include #endif +#include +#include +#endif // HAL_BOARD_CHIBIOS void setup(); void loop(); @@ -41,7 +46,9 @@ static uint32_t sysclk = 0; static EKF_Maths ekf; HAL_Semaphore sem; +#if HAL_WITH_ESC_TELEM AP_ESC_Telem telem; +#endif void setup() { #ifdef DISABLE_CACHES @@ -129,6 +136,13 @@ static void show_timings(void) TIMEIT("millis16()", AP_HAL::millis16(), 200); TIMEIT("micros64()", AP_HAL::micros64(), 200); +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + TIMEIT("hrt_micros32()", hrt_micros32(), 200); + TIMEIT("hrt_micros64()", hrt_micros64(), 200); + TIMEIT("hrt_millis32()", hrt_millis32(), 200); + TIMEIT("hrt_millis64()", hrt_millis64(), 200); +#endif + TIMEIT("fadd", v_out += v_f, 100); TIMEIT("fsub", v_out -= v_f, 100); TIMEIT("fmul", v_out *= v_f, 100); @@ -194,11 +208,51 @@ static void show_timings(void) TIMEIT("SEM", { WITH_SEMAPHORE(sem); v_out_32 += v_32;}, 100); } +static void test_div1000(void) +{ + hal.console->printf("Testing div1000\n"); + 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"); + 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", + (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2); + return; + } + } +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + // test from locked context + 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"); + break; + } + chSysLock(); + uint64_t v1 = v / 1000ULL; + uint64_t v2 = uint64_div1000(v); + chSysUnlock(); + if (v1 != v2) { + AP_HAL::panic("ERROR: 0x%llx v1=0x%llx v2=0x%llx\n", + (unsigned long long)v, (unsigned long long)v1, (unsigned long long)v2); + return; + } + } +#endif + hal.console->printf("div1000 OK\n"); +} + void loop() { show_sizes(); hal.console->printf("\n"); show_timings(); + test_div1000(); hal.console->printf("\n"); hal.scheduler->delay(3000); } diff --git a/Tools/Frame_params/EFlight_Convergence.param b/Tools/Frame_params/EFlight_Convergence.param index a8ab7be9a0504b..9d9decd5429a8f 100644 --- a/Tools/Frame_params/EFlight_Convergence.param +++ b/Tools/Frame_params/EFlight_Convergence.param @@ -6,7 +6,7 @@ BATT_MONITOR,4 BATT_VOLT_MULT,12 BRD_SAFETY_DEFLT,0 FORMAT_VERSION,13 -LIM_PITCH_MAX,3500 +PTCH_LIM_MAX_DEG,35.00 MIXING_GAIN,0.8 PTCH_RATE_D,0.000000 PTCH_RATE_FF,0.455000 diff --git a/Tools/Frame_params/Holybro-QAV250-bdshot.param b/Tools/Frame_params/Holybro-QAV250-bdshot.param new file mode 100644 index 00000000000000..5ee424b27b2c96 --- /dev/null +++ b/Tools/Frame_params/Holybro-QAV250-bdshot.param @@ -0,0 +1,55 @@ +ANGLE_MAX 7500 +ATC_ACCEL_P_MAX 247932.2 +ATC_ACCEL_R_MAX 280459.7 +ATC_ACCEL_Y_MAX 70449.61 +ATC_ANG_PIT_P 20.62971 +ATC_ANG_RLL_P 21.77228 +ATC_ANG_YAW_P 12.07662 +ATC_RAT_PIT_D 0.001443983 +ATC_RAT_PIT_FLTD 40 +ATC_RAT_PIT_FLTT 40 +ATC_RAT_PIT_I 0.05448329 +ATC_RAT_PIT_P 0.05448329 +ATC_RAT_PIT_SMAX 30 +ATC_RAT_RLL_D 0.001218464 +ATC_RAT_RLL_FLTD 40 +ATC_RAT_RLL_FLTT 40 +ATC_RAT_RLL_I 0.03652912 +ATC_RAT_RLL_P 0.03652912 +ATC_RAT_RLL_SMAX 30 +ATC_RAT_YAW_D 0.004205064 +ATC_RAT_YAW_FLTD 20 +ATC_RAT_YAW_FLTE 2 +ATC_RAT_YAW_I 0.02155114 +ATC_RAT_YAW_P 0.2155114 +ATC_RAT_YAW_SMAX 30 +INS_HNTC2_ATT 40 +INS_HNTC2_BW 8 +INS_HNTC2_ENABLE 1 +INS_HNTC2_FM_RAT 1 +INS_HNTC2_FREQ 47 +INS_HNTC2_HMNCS 3 +INS_HNTC2_MODE 1 +INS_HNTC2_OPTS 16 +INS_HNTC2_REF 1 +INS_HNTCH_ATT 40 +INS_HNTCH_BW 20 +INS_HNTCH_ENABLE 1 +INS_HNTCH_FM_RAT 1 +INS_HNTCH_FREQ 80 +INS_HNTCH_HMNCS 1 +INS_HNTCH_MODE 3 +INS_HNTCH_OPTS 22 +INS_HNTCH_REF 1 +MOT_BAT_VOLT_MAX 16.8 +MOT_BAT_VOLT_MIN 13.2 +MOT_PWM_TYPE 6 +MOT_SPIN_ARM 0.06 +MOT_SPIN_MIN 0.08 +MOT_THST_EXPO 0.55 +MOT_THST_HOVER 0.125 +PSC_ACCZ_I 0.168 +PSC_ACCZ_P 0.084 +SERVO_BLH_AUTO 1 +SERVO_DSHOT_ESC 2 +SERVO_DSHOT_RATE 3 diff --git a/Tools/Frame_params/Holybro-QAV250.param b/Tools/Frame_params/Holybro-QAV250.param new file mode 100644 index 00000000000000..af5072de6d9038 --- /dev/null +++ b/Tools/Frame_params/Holybro-QAV250.param @@ -0,0 +1,54 @@ +ANGLE_MAX 7500 +ATC_ACCEL_P_MAX 278477.8 +ATC_ACCEL_R_MAX 344115.3 +ATC_ACCEL_Y_MAX 51274.09 +ATC_ANG_PIT_P 25.66075 +ATC_ANG_RLL_P 28.50734 +ATC_ANG_YAW_P 8.258418 +ATC_RAT_PIT_D 0.001949416 +ATC_RAT_PIT_FLTD 40 +ATC_RAT_PIT_FLTT 40 +ATC_RAT_PIT_I 0.07421205 +ATC_RAT_PIT_P 0.07421205 +ATC_RAT_PIT_SMAX 30 +ATC_RAT_RLL_D 0.00120043 +ATC_RAT_RLL_FLTD 40 +ATC_RAT_RLL_FLTT 40 +ATC_RAT_RLL_I 0.04598495 +ATC_RAT_RLL_P 0.04598495 +ATC_RAT_RLL_SMAX 30 +ATC_RAT_YAW_D 0.009076501 +ATC_RAT_YAW_FLTD 40 +ATC_RAT_YAW_FLTE 1.3176 +ATC_RAT_YAW_I 0.02884508 +ATC_RAT_YAW_P 0.2884507 +ATC_RAT_YAW_SMAX 30 +INS_HNTC2_ATT 40 +INS_HNTC2_BW 8 +INS_HNTC2_ENABLE 1 +INS_HNTC2_FM_RAT 1 +INS_HNTC2_FREQ 47 +INS_HNTC2_HMNCS 1 +INS_HNTC2_MODE 1 +INS_HNTC2_OPTS 16 +INS_HNTC2_REF 1 +INS_HNTCH_ATT 40 +INS_HNTCH_BW 75 +INS_HNTCH_ENABLE 1 +INS_HNTCH_FM_RAT 0.7 +INS_HNTCH_FREQ 150 +INS_HNTCH_HMNCS 3 +INS_HNTCH_MODE 1 +INS_HNTCH_OPTS 0 +INS_HNTCH_REF 0.125 +MOT_BAT_VOLT_MAX 16.8 +MOT_BAT_VOLT_MIN 13.2 +MOT_PWM_TYPE 5 +MOT_SPIN_ARM 0.06 +MOT_SPIN_MIN 0.08 +MOT_THST_EXPO 0.55 +MOT_THST_HOVER 0.125 +PSC_ACCZ_I 0.168 +PSC_ACCZ_P 0.084 +SERVO_DSHOT_ESC 2 +SERVO_DSHOT_RATE 3 diff --git a/Tools/Frame_params/Holybro-X500v2-bdshot.param b/Tools/Frame_params/Holybro-X500v2-bdshot.param new file mode 100644 index 00000000000000..51f8733d21b439 --- /dev/null +++ b/Tools/Frame_params/Holybro-X500v2-bdshot.param @@ -0,0 +1,44 @@ +ANGLE_MAX 3500 +ATC_ACCEL_P_MAX 165167.5 +ATC_ACCEL_R_MAX 176726.4 +ATC_ACCEL_Y_MAX 35955.46 +ATC_ANG_PIT_P 21.2236 +ATC_ANG_RLL_P 26.35878 +ATC_ANG_YAW_P 8.53887 +ATC_RAT_PIT_D 0.005201788 +ATC_RAT_PIT_FLTD 21 +ATC_RAT_PIT_FLTT 21 +ATC_RAT_PIT_I 0.1466683 +ATC_RAT_PIT_P 0.1466683 +ATC_RAT_RLL_D 0.004905871 +ATC_RAT_RLL_FLTD 21 +ATC_RAT_RLL_FLTT 21 +ATC_RAT_RLL_I 0.1255951 +ATC_RAT_RLL_P 0.1255951 +ATC_RAT_YAW_D 0.02224096 +ATC_RAT_YAW_FLTD 10 +ATC_RAT_YAW_FLTE 1.3176 +ATC_RAT_YAW_FLTT 21 +ATC_RAT_YAW_I 0.1204564 +ATC_RAT_YAW_P 1.204564 +INS_HNTCH_ATT 40 +INS_HNTCH_BW 40 +INS_HNTCH_ENABLE 1 +INS_HNTCH_FM_RAT 1 +INS_HNTCH_FREQ 80 +INS_HNTCH_HMNCS 3 +INS_HNTCH_MODE 3 +INS_HNTCH_OPTS 22 +INS_HNTCH_REF 1 +MOT_BAT_VOLT_MAX 16.8 +MOT_BAT_VOLT_MIN 13.2 +MOT_SPIN_ARM 0.07 +MOT_SPIN_MIN 0.09 +MOT_THST_HOVER 0.2905434 +PSC_ACCZ_I 0.6 +PSC_ACCZ_P 0.3 +SERVO_BLH_AUTO 1 +SERVO_BLH_MASK 3840 +SERVO_BLH_OTYPE 5 +SERVO_DSHOT_ESC 2 +TKOFF_RPM_MIN 1000 diff --git a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param index ceba5d41ac0e1b..0d26d2c7c84cf5 100644 --- a/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param +++ b/Tools/Frame_params/Parrot_Disco/Parrot_Disco.param @@ -1,7 +1,7 @@ -ALT_HOLD_RTL 6000 +RTL_ALTITUDE 60.00 ARMING_RUDDER 2 -ARSPD_FBW_MAX 17 -ARSPD_FBW_MIN 12 +AIRSPEED_MAX 17 +AIRSPEED_MIN 12 ARSPD_PSI_RANGE 0.05 ARSPD_TUBE_ORDR 2 ARSPD_USE 1 @@ -24,9 +24,9 @@ INS_ACCEL_FILTER 10 INS_GYRO_FILTER 5 LAND_FLARE_ALT 3 LAND_FLARE_SEC 2 -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -2500 -LIM_ROLL_CD 5500 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -25.00 +ROLL_LIMIT_DEG 55.00 NAVL1_PERIOD 16 PTCH_RATE_D 0.000000 PTCH_RATE_I 0.240000 @@ -56,7 +56,7 @@ THR_FS_VALUE 950 THR_MAX 100 THR_MIN 0 TKOFF_THR_MINACC 4 -TRIM_ARSPD_CM 1400 +AIRSPEED_CRUISE 14.00 TRIM_THROTTLE 35 WP_LOITER_RAD 30 WP_RADIUS 90 diff --git a/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param b/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param index 9fe63b03f76c2b..adb4b650bf1ab1 100644 --- a/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param +++ b/Tools/Frame_params/QuadPlanes/Aerofox_AYK320.param @@ -1,9 +1,9 @@ # parameters for Foxtech AYK320 # airspeed -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 18 -TRIM_ARSPD_CM 2300 +AIRSPEED_MAX 35 +AIRSPEED_MIN 18 +AIRSPEED_CRUISE 23.00 TRIM_THROTTLE 60 # notch filter @@ -17,8 +17,8 @@ INS_HNTCH_MODE 1.000000 INS_HNTCH_REF 0.337000 # fixed wing limits -LIM_PITCH_MIN -2000 -LIM_ROLL_CD 4000 +PTCH_LIM_MIN_DEG -20.00 +ROLL_LIMIT_DEG 40.00 LEVEL_ROLL_LIMIT 12 FBWB_CLIMB_RATE 5 @@ -80,7 +80,7 @@ Q_A_RAT_YAW_FLTT 12.5 Q_A_RAT_YAW_P 1 Q_A_SLEW_YAW 3000 Q_LAND_FINAL_ALT 12 -Q_LAND_SPEED 40 +Q_LAND_FINAL_SPD 0.40 Q_M_BAT_IDX 1 Q_M_BAT_VOLT_MAX 50.4 Q_M_BAT_VOLT_MIN 40 @@ -95,8 +95,8 @@ Q_P_VELXY_P 0.7 Q_RTL_ALT 40 Q_TRANS_DECEL 1.5 Q_TRANS_FAIL 20 -Q_VELZ_MAX 200 -Q_VELZ_MAX_DN 130 +Q_PILOT_SPD_UP 2.00 +Q_PILOT_SPD_UP 1.30 Q_VFWD_ALT 6 Q_VFWD_GAIN 0.05 Q_WVANE_GAIN 0.3 diff --git a/Tools/Frame_params/QuadPlanes/Foxtech_Altair370.parm b/Tools/Frame_params/QuadPlanes/Foxtech_Altair370.parm index 666842efaf7f13..309910a3732805 100644 --- a/Tools/Frame_params/QuadPlanes/Foxtech_Altair370.parm +++ b/Tools/Frame_params/QuadPlanes/Foxtech_Altair370.parm @@ -7,14 +7,14 @@ # the rear two elevons are setup for roll only (ailerons) # setup airspeed range -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 22 -TRIM_ARSPD_CM 2700 +AIRSPEED_MAX 35 +AIRSPEED_MIN 22 +AIRSPEED_CRUISE 27.00 # fixed wing limits -LIM_PITCH_MAX 1500 -LIM_PITCH_MIN -1500 -LIM_ROLL_CD 2500 +PTCH_LIM_MAX_DEG 15.00 +PTCH_LIM_MIN_DEG -15.00 +ROLL_LIMIT_DEG 25.00 # setup elevons as 75% elevator and 25% aileron MIXING_GAIN 0.25 diff --git a/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param b/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param index c3098e966c4668..60804701731244 100644 --- a/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param +++ b/Tools/Frame_params/QuadPlanes/Foxtech_GreatShark.param @@ -3,9 +3,9 @@ # https://www.foxtechfpv.com/foxtech-great-shark-330-vtol.html # airspeed -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 18 -TRIM_ARSPD_CM 2300 +AIRSPEED_MAX 35 +AIRSPEED_MIN 18 +AIRSPEED_CRUISE 23.00 TRIM_THROTTLE 60 # notch filter @@ -19,8 +19,8 @@ INS_HNTCH_MODE 1.000000 INS_HNTCH_REF 0.337000 # fixed wing limits -LIM_PITCH_MIN -2000 -LIM_ROLL_CD 4000 +PTCH_LIM_MIN_DEG -20.00 +ROLL_LIMIT_DEG 40.00 LEVEL_ROLL_LIMIT 12 FBWB_CLIMB_RATE 5 @@ -82,7 +82,7 @@ Q_A_RAT_YAW_FLTT 12.5 Q_A_RAT_YAW_P 1 Q_A_SLEW_YAW 3000 Q_LAND_FINAL_ALT 12 -Q_LAND_SPEED 40 +Q_LAND_FINAL_SPD 0.40 Q_M_BAT_IDX 1 Q_M_BAT_VOLT_MAX 50.4 Q_M_BAT_VOLT_MIN 40 @@ -97,8 +97,8 @@ Q_P_VELXY_P 0.7 Q_RTL_ALT 40 Q_TRANS_DECEL 1.5 Q_TRANS_FAIL 20 -Q_VELZ_MAX 200 -Q_VELZ_MAX_DN 130 +Q_PILOT_SPD_UP 2.00 +Q_PILOT_SPD_UP 1.30 Q_VFWD_ALT 6 Q_VFWD_GAIN 0.05 Q_WVANE_GAIN 0.3 diff --git a/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param b/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param index a52093b3efdb80..18bbf11b867760 100644 --- a/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param +++ b/Tools/Frame_params/QuadPlanes/MFD_Crosswind_VTOL.param @@ -3,16 +3,16 @@ # http://myflydream.com/products/c-0/369.html # airspeed -ARSPD_FBW_MAX 29 -ARSPD_FBW_MIN 16 -TRIM_ARSPD_CM 2000 +AIRSPEED_MAX 29 +AIRSPEED_MIN 16 +AIRSPEED_CRUISE 20.00 TRIM_THROTTLE 52 # fixed wing limits -LIM_PITCH_MAX 2200 -LIM_ROLL_CD 3500 +PTCH_LIM_MAX_DEG 22.00 +ROLL_LIMIT_DEG 35.00 NAVL1_PERIOD 24 -ALT_HOLD_RTL 9000 +RTL_ALTITUDE 90.00 FBWB_CLIMB_RATE 5 WP_LOITER_RAD 150 WP_RADIUS 150 diff --git a/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param b/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param index feac862bc69b2d..5b322c05e2c776 100644 --- a/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param +++ b/Tools/Frame_params/QuadPlanes/MFE_StriverMini.param @@ -6,7 +6,7 @@ SCALING_SPEED 15 TECS_CLMB_MAX 3 TECS_LAND_ARSPD 20 TKOFF_THR_MAX 85 -TRIM_PITCH_CD 100 +PTCH_TRIM_DEG 1.00 TRIM_THROTTLE 40 WP_LOITER_RAD 120 WP_RADIUS 110 @@ -21,14 +21,14 @@ INS_HNTCH_MODE 1 INS_HNTCH_REF 0.26 # airspeed -TRIM_ARSPD_CM 2300 -ARSPD_FBW_MAX 30 -ARSPD_FBW_MIN 15 +AIRSPEED_CRUISE 23.00 +AIRSPEED_MAX 30 +AIRSPEED_MIN 15 # limits -LIM_PITCH_MAX 2000 -LIM_PITCH_MIN -2000 -LIM_ROLL_CD 5500 +PTCH_LIM_MAX_DEG 20.00 +PTCH_LIM_MIN_DEG -20.00 +ROLL_LIMIT_DEG 55.00 # pitch tuning PTCH_RATE_D 0.017718 @@ -100,7 +100,7 @@ Q_M_THST_EXPO 0.650000 Q_M_THST_HOVER 0.247719 Q_RTL_ALT 30 Q_TRANS_DECEL 1.700000 -Q_VELZ_MAX 150 +Q_PILOT_SPD_UP 1.50 Q_VFWD_ALT 5 Q_VFWD_GAIN 0.050000 Q_WVANE_ENABLE 1 diff --git a/Tools/Frame_params/QuadPlanes/Mugin_EV350.param b/Tools/Frame_params/QuadPlanes/Mugin_EV350.param index 3797d770109710..3279f50b12aee3 100644 --- a/Tools/Frame_params/QuadPlanes/Mugin_EV350.param +++ b/Tools/Frame_params/QuadPlanes/Mugin_EV350.param @@ -2,10 +2,10 @@ # https://www.muginuav.com/product/mugin-ev350-carbon-fiber-full-electric-vtol-uav-platform/ -ALT_HOLD_RTL 8000 -LIM_PITCH_MAX 2000 -LIM_PITCH_MIN -2000 -LIM_ROLL_CD 4000 +RTL_ALTITUDE 80.00 +PTCH_LIM_MAX_DEG 20.00 +PTCH_LIM_MIN_DEG -20.00 +ROLL_LIMIT_DEG 40.00 NAVL1_PERIOD 17 PTCH2SRV_TCONST 0.75 RTL_RADIUS 180 @@ -24,9 +24,9 @@ INS_HNTCH_MODE 1 INS_HNTCH_REF 0.245000 # airspeed -TRIM_ARSPD_CM 2300 -ARSPD_FBW_MAX 28 -ARSPD_FBW_MIN 18 +AIRSPEED_CRUISE 23.00 +AIRSPEED_MAX 28 +AIRSPEED_MIN 18 # FW pitch tuning PTCH_RATE_D 0.021381 @@ -93,7 +93,7 @@ Q_A_THR_MIX_MAX 0.9 Q_FRAME_CLASS 1 Q_FRAME_TYPE 1 Q_LAND_FINAL_ALT 12 -Q_LAND_SPEED 40 +Q_LAND_FINAL_SPD 0.40 Q_LOIT_ANG_MAX 20 Q_LOIT_BRK_ACCEL 40 Q_LOIT_BRK_JERK 200 @@ -113,8 +113,8 @@ Q_RTL_ALT 40 Q_THROTTLE_EXPO 0.2 Q_TRANS_DECEL 1.5 Q_TRAN_PIT_MAX 7 -Q_VELZ_MAX 200 -Q_VELZ_MAX_DN 130 +Q_PILOT_SPD_UP 2.00 +Q_PILOT_SPD_UP 1.30 Q_VFWD_ALT 5 Q_VFWD_GAIN 0.05 Q_WP_RADIUS 300 diff --git a/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param b/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param index c661c0c3c60492..901b3ac166fd3d 100644 --- a/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param +++ b/Tools/Frame_params/QuadPlanes/SparkleTech-EagleHero.param @@ -3,13 +3,13 @@ # http://www.sparkletech.hk/%E9%B9%B0%E7%9C%BC-%E6%97%A0%E4%BA%BA%E6%9C%BA/electric-power/eagle-plus-vtol/ # airspeed -ARSPD_FBW_MAX 31 -ARSPD_FBW_MIN 22 -TRIM_ARSPD_CM 2400 +AIRSPEED_MAX 31 +AIRSPEED_MIN 22 +AIRSPEED_CRUISE 24.00 SCALING_SPEED 20 TRIM_THROTTLE 55 -LIM_ROLL_CD 5000 +ROLL_LIMIT_DEG 50.00 # fixed wing tuning RLL2SRV_RMAX 75 diff --git a/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm b/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm new file mode 100644 index 00000000000000..7c038475871b9d --- /dev/null +++ b/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm @@ -0,0 +1,130 @@ +STAB_PITCH_DOWN,4 +TKOFF_TDRAG_ELEV,30 +TKOFF_TDRAG_SPD1,20 +TKOFF_ROTATE_SPD,30 +USE_REV_THRUST,1075 +WP_RADIUS,400 +WP_LOITER_RAD,1000 +RTL_RADIUS,500 +AIRSPEED_MIN,30 +AIRSPEED_MAX,65 +TERRAIN_FOLLOW,1 +THR_MIN,-25 +THR_SLEWRATE,30 +TRIM_THROTTLE,60 +GROUND_STEER_ALT,10 +AIRSPEED_CRUISE,52.00 +SCALING_SPEED,60 +PTCH_TRIM_DEG,4.0 +RTL_ALTITUDE,250.00 +FLAP_1_PERCNT,50 +FLAP_1_SPEED,30 +RTL_AUTOLAND,2 +RELAY1_FUNCTION,1 +Q_ENABLE,1 +Q_M_THST_EXPO,0 +Q_M_SPIN_MIN,0.25 +Q_M_THST_HOVER,0.66 +Q_ANGLE_MAX,1500 +Q_TRANSITION_MS,10000 +Q_P_POSZ_P,0.5 +Q_P_VELZ_P,2.5 +Q_P_POSXY_P,0.1 +Q_P_VELXY_P,0.2 +Q_P_VELXY_I,0.1 +Q_P_VELXY_D,0.059 +Q_PILOT_SPD_UP,5.00 +Q_WP_SPEED,1500 +Q_WP_SPEED_DN,500 +Q_WP_ACCEL,200 +Q_ASSIST_SPEED,25 +Q_LAND_FINAL_SPD,1.00 +Q_LAND_FINAL_ALT,25 +Q_FRAME_TYPE,3 +Q_VFWD_GAIN,0.3 +Q_RTL_ALT,50 +Q_TRANS_DECEL,0.69 +Q_LOIT_SPEED,1200 +Q_LOIT_ACC_MAX,150 +Q_LOIT_BRK_DELAY,3 +Q_A_SLEW_YAW,2000 +Q_A_ACCEL_Y_MAX,2500 +Q_A_ACCEL_R_MAX,6000 +Q_A_ACCEL_P_MAX,9000 +Q_A_ANG_RLL_P,0.5 +Q_A_ANG_PIT_P,0.25 +Q_A_ANG_YAW_P,0.5 +Q_A_RATE_R_MAX,35 +Q_A_RATE_P_MAX,35 +Q_A_RATE_Y_MAX,35 +Q_A_RAT_RLL_P,0.7835410833358765 +Q_A_RAT_RLL_I,0.7835410833358765 +Q_A_RAT_RLL_D,0.05042541027069092 +Q_A_RAT_RLL_FLTT,10 +Q_A_RAT_PIT_P,1.0936822891235352 +Q_A_RAT_PIT_I,1.0936822891235352 +Q_A_RAT_PIT_D,0.03247206285595894 +Q_A_RAT_PIT_FLTT,10 +Q_A_RAT_YAW_P,2.0000014305114746 +Q_A_RAT_YAW_I,0.20000013709068298 +Q_A_RAT_YAW_D,0.03000001423060894 +Q_A_RAT_YAW_FLTT,10 +Q_A_RAT_YAW_FLTE,2 +Q_A_RAT_YAW_FLTD,10 +RLL2SRV_TCONST,1 +RLL2SRV_RMAX,75 +RLL_RATE_P,0.6642029881477356 +RLL_RATE_I,0.6642029881477356 +RLL_RATE_D,0.019881000742316246 +RLL_RATE_FF,1.2974460124969482 +RLL_RATE_FLTT,0.7957746982574463 +RLL_RATE_FLTD,10 +PTCH2SRV_TCONST,0.75 +PTCH2SRV_RMAX_UP,75 +PTCH2SRV_RMAX_DN,75 +PTCH_RATE_P,0.6644459962844849 +PTCH_RATE_I,0.5014650225639343 +PTCH_RATE_D,0.017854999750852585 +PTCH_RATE_FF,0.5014650225639343 +PTCH_RATE_FLTT,2.122066020965576 +PTCH_RATE_FLTD,10 +SCHED_LOOP_RATE,100 +NAVL1_PERIOD,20 +TECS_CLMB_MAX,15 +TECS_TIME_CONST,8 +TECS_THR_DAMP,2 +TECS_SINK_MAX,15 +TECS_LAND_ARSPD,40 +SIM_OH_RELAY_MSK,-1 +SIM_MAG1_OFS_X,5 +SIM_MAG1_OFS_Y,13 +SIM_MAG1_OFS_Z,-18 +SIM_MAG2_OFS_X,5 +SIM_MAG2_OFS_Y,13 +SIM_MAG2_OFS_Z,-18 +SIM_MAG3_OFS_X,5 +SIM_MAG3_OFS_Y,13 +SIM_MAG3_OFS_Z,-18 +EK3_ENABLE,0 +SERVO1_MIN,1000 +SERVO1_MAX,1700 +SERVO1_TRIM,1374 +SERVO2_MIN,1000 +SERVO2_MAX,2000 +SERVO2_TRIM,1516 +SERVO3_MIN,500 +SERVO3_MAX,2000 +SERVO3_TRIM,982 +SERVO4_MIN,1000 +SERVO4_MAX,2000 +SERVO5_MIN,1500 +SERVO5_FUNCTION,33 +SERVO6_MIN,1000 +SERVO6_FUNCTION,34 +SERVO9_MIN,1300 +SERVO9_MAX,2000 +SERVO9_TRIM,1601 +SERVO9_FUNCTION,4 +SERVO_AUTO_TRIM,1 +LGR_ENABLE,1 +LAND_FLAP_PERCNT,100 diff --git a/Tools/Frame_params/SkyWalkerX8.param b/Tools/Frame_params/SkyWalkerX8.param index bd7915109a95ac..f78cf5c500fb4c 100644 --- a/Tools/Frame_params/SkyWalkerX8.param +++ b/Tools/Frame_params/SkyWalkerX8.param @@ -22,15 +22,15 @@ LAND_FLARE_SEC,0.1 LAND_PF_ALT,8 LAND_PF_ARSPD,12 LAND_PF_SEC,0 -LAND_PITCH_CD,-450 +LAND_PITCH_DEG,-4.50 LAND_SLOPE_RCALC,2 LAND_THEN_NEUTRL,1 LAND_THR_SLEW,70 LEVEL_ROLL_LIMIT,15 -LIM_PITCH_MAX,1500 -LIM_PITCH_MIN,-2500 -LIM_ROLL_CD,3500 -MIN_GNDSPD_CM,600 +PTCH_LIM_MAX_DEG,15.00 +PTCH_LIM_MIN_DEG,-25.00 +ROLL_LIMIT_DEG,35.00 +MIN_GROUNDSPEED,6 MIXING_GAIN,0.5 NAVL1_DAMPING,0.75 NAVL1_PERIOD,17 @@ -105,9 +105,9 @@ TKOFF_THR_MAX,0 TKOFF_THR_MINACC,12 TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,100 -TRIM_ARSPD_CM,1600 +AIRSPEED_CRUISE,16.00 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PTCH_TRIM_DEG,0.00 TRIM_RC_AT_START,0 TRIM_THROTTLE,55 USE_REV_THRUST,0 diff --git a/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param b/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param index cb831736576390..853b9c533bda2d 100644 --- a/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param +++ b/Tools/Frame_params/SkyWalkerX8_ReverseThrust.param @@ -22,15 +22,15 @@ LAND_FLARE_SEC,0.1 LAND_PF_ALT,8 LAND_PF_ARSPD,12 LAND_PF_SEC,0 -LAND_PITCH_CD,-450 +LAND_PITCH_DEG,-4.50 LAND_SLOPE_RCALC,2 LAND_THEN_NEUTRL,1 LAND_THR_SLEW,70 LEVEL_ROLL_LIMIT,15 -LIM_PITCH_MAX,1500 -LIM_PITCH_MIN,-2500 -LIM_ROLL_CD,3500 -MIN_GNDSPD_CM,600 +PTCH_LIM_MAX_DEG,15.00 +PTCH_LIM_MIN_DEG,-25.00 +ROLL_LIMIT_DEG,35.00 +MIN_GROUNDSPEED,6 MIXING_GAIN,0.5 NAVL1_DAMPING,0.75 NAVL1_PERIOD,17 @@ -105,9 +105,9 @@ TKOFF_THR_MAX,0 TKOFF_THR_MINACC,12 TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,100 -TRIM_ARSPD_CM,1600 +AIRSPEED_CRUISE,16.00 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PTCH_TRIM_DEG,0.00 TRIM_RC_AT_START,0 TRIM_THROTTLE,55 USE_REV_THRUST,2 diff --git a/Tools/Frame_params/Solo_Copter-3.5_GreenCube.param b/Tools/Frame_params/Solo_Copter-3.5_GreenCube.param index 894d482c3361da..0f443af4bcabab 100644 --- a/Tools/Frame_params/Solo_Copter-3.5_GreenCube.param +++ b/Tools/Frame_params/Solo_Copter-3.5_GreenCube.param @@ -38,7 +38,7 @@ COMPASS_ORIENT,38 COMPASS_ORIENT2,0 COMPASS_ORIENT3,0 COMPASS_PRIMARY,0 -COMPASS_TYPEMASK,15392 +COMPASS_DISBLMSK,15392 COMPASS_USE,1 COMPASS_USE2,1 COMPASS_USE3,0 diff --git a/Tools/Frame_params/Solo_Copter-3.6_GreenCube.param b/Tools/Frame_params/Solo_Copter-3.6_GreenCube.param index 61863f1579034b..d80e6e9001fffb 100644 --- a/Tools/Frame_params/Solo_Copter-3.6_GreenCube.param +++ b/Tools/Frame_params/Solo_Copter-3.6_GreenCube.param @@ -42,7 +42,7 @@ BRD_SAFETYENABLE,0 BRD_SAFETY_MASK,16368 CIRCLE_RADIUS,3000 COMPASS_ORIENT,38 -COMPASS_TYPEMASK,15392 +COMPASS_DISBLMSK,15392 COMPASS_USE3,0 DISARM_DELAY,10 EK2_ENABLE,0 diff --git a/Tools/Frame_params/Sub/bluerov2-4_0_0.params b/Tools/Frame_params/Sub/bluerov2-4_0_0.params index 7704b35a136900..a5a21c5343711f 100644 --- a/Tools/Frame_params/Sub/bluerov2-4_0_0.params +++ b/Tools/Frame_params/Sub/bluerov2-4_0_0.params @@ -169,7 +169,7 @@ COMPASS_ORIENT2,0 COMPASS_ORIENT3,0 COMPASS_PMOT_EN,0 COMPASS_PRIMARY,0 -COMPASS_TYPEMASK,0 +COMPASS_DISBLMSK,0 COMPASS_USE,1 COMPASS_USE2,1 COMPASS_USE3,1 diff --git a/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params b/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params index 1cc38ff762b4db..88d0826b3424a6 100644 --- a/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params +++ b/Tools/Frame_params/Sub/bluerov2-heavy-4_0_0.params @@ -169,7 +169,7 @@ COMPASS_ORIENT2,0 COMPASS_ORIENT3,0 COMPASS_PMOT_EN,0 COMPASS_PRIMARY,0 -COMPASS_TYPEMASK,0 +COMPASS_DISBLMSK,0 COMPASS_USE,1 COMPASS_USE2,1 COMPASS_USE3,1 diff --git a/Tools/Frame_params/hexsoon-edu650.param b/Tools/Frame_params/hexsoon-edu650.param new file mode 100644 index 00000000000000..330ffba32feed4 --- /dev/null +++ b/Tools/Frame_params/hexsoon-edu650.param @@ -0,0 +1,42 @@ +#NOTE: Hexsoon EDU650 params for ArduPilot Copter 4.5 (and higher) +ANGLE_MAX,3000 +ACRO_YAW_P,2.6 +ATC_ACCEL_P_MAX,86000 +ATC_ACCEL_R_MAX,86000 +ATC_ACCEL_Y_MAX,15000 +ATC_RAT_PIT_FLTD,10 +ATC_RAT_PIT_FLTT,10 +ATC_RAT_PIT_D,0.0056 +ATC_RAT_PIT_I,0.11 +ATC_RAT_PIT_P,0.11 +ATC_RAT_RLL_D,0.0048 +ATC_RAT_RLL_FLTD,10 +ATC_RAT_RLL_FLTT,10 +ATC_RAT_RLL_I,0.10 +ATC_RAT_RLL_P,0.10 +ATC_RAT_YAW_FLTD,10 +ATC_RAT_YAW_FLTE,2 +ATC_RAT_YAW_FLTT,10 +BATT_MONITOR,3 +BATT_VOLT_MULT,12.02 +FRAME_CLASS,1 +FRAME_TYPE,1 +INS_GYRO_FILTER,40 +INS_HNTCH_ATT,40 +INS_HNTCH_BW,28 +INS_HNTCH_ENABLE,1 +INS_HNTCH_FM_RAT,0.7 +INS_HNTCH_FREQ,63 +INS_HNTCH_HMNCS,7 +INS_HNTCH_MODE,1 +INS_HNTCH_OPTS,20 +INS_HNTCH_REF,0.202 +MOT_BAT_VOLT_MAX,25.2 +MOT_BAT_VOLT_MIN,19.8 +MOT_SPIN_ARM,0.16 +MOT_SPIN_MIN,0.18 +MOT_THST_EXPO,0.70 +MOT_THST_HOVER,0.20 +PILOT_Y_RATE,60 +PSC_ACCZ_I,0.5 +PSC_ACCZ_P,0.3 diff --git a/Tools/Frame_params/hexsoon-td650.param b/Tools/Frame_params/hexsoon-td650.param deleted file mode 100644 index 34a31ee3c71480..00000000000000 --- a/Tools/Frame_params/hexsoon-td650.param +++ /dev/null @@ -1,29 +0,0 @@ -#NOTE: Hexsoon TD650 params for ArduPilot Copter 4.1 -ANGLE_MAX,3500 -ACRO_YAW_P,2.6 -ATC_ACCEL_P_MAX,86000 -ATC_ACCEL_R_MAX,86000 -ATC_ACCEL_Y_MAX,23400 -ATC_RAT_PIT_FLTD,17 -ATC_RAT_PIT_FLTT,17 -ATC_RAT_PIT_D,0.0034 -ATC_RAT_PIT_I,0.12 -ATC_RAT_PIT_P,0.12 -ATC_RAT_RLL_D,0.0034 -ATC_RAT_RLL_FLTD,17 -ATC_RAT_RLL_FLTT,17 -ATC_RAT_RLL_I,0.12 -ATC_RAT_RLL_P,0.12 -ATC_RAT_YAW_FLTE,2 -BATT_MONITOR,4 -BATT_VOLT_MULT,12.02 -FRAME_CLASS,1 -FRAME_TYPE,1 -INS_GYRO_FILTER,34 -MOT_BAT_VOLT_MAX,25.2 -MOT_BAT_VOLT_MIN,19.8 -MOT_THST_EXPO,0.68 -MOT_THST_HOVER,0.2288489 -PSC_ACCZ_I,0.5 -PSC_ACCZ_P,0.3 - diff --git a/Tools/GIT_Test/GIT_Success.txt b/Tools/GIT_Test/GIT_Success.txt index 9f00eea02a121e..82f59f568a52ef 100644 --- a/Tools/GIT_Test/GIT_Success.txt +++ b/Tools/GIT_Test/GIT_Success.txt @@ -177,4 +177,7 @@ Torre Orazio seba czapnik Ramy Gad Matthew R. Cunningham +Mehmet Keten prathap devendran (Tamil) india, 11-23 +Taqwaisneeded +FreeName diff --git a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin index 83a7e2b6299695..5934290788ad07 100755 Binary files a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin and b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin index dbc520edd07a61..c65751281d15ac 100755 Binary files a/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin index a722bc5bd2ee92..adbf98f090cde5 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin index cdb40f2fab870f..fce01bb26c29e6 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin index 3d993264ce8699..2ba418de0788fe 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin index ef5755a62abef3..1a0426c5761b65 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_highpolh.bin index 89d0449fb12566..af9d068cd5ea57 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_highpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin index 2179b5cbc5efb9..8db3cfaad172de 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_highpolh.bin b/Tools/IO_Firmware/iofirmware_highpolh.bin index 4e2b62dec6a670..988375fa5d73c2 100755 Binary files a/Tools/IO_Firmware/iofirmware_highpolh.bin and b/Tools/IO_Firmware/iofirmware_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_lowpolh.bin b/Tools/IO_Firmware/iofirmware_lowpolh.bin index 3ba40363bcd4a8..059048670ef3f5 100755 Binary files a/Tools/IO_Firmware/iofirmware_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_lowpolh.bin differ diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 75c9975b826bd7..2beb1c5478349c 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -238,6 +238,7 @@ def ap_get_all_libraries(bld): continue libraries.append(name) libraries.extend(['AP_HAL', 'AP_HAL_Empty']) + libraries.append('AP_PiccoloCAN/piccolo_protocol') return libraries @conf diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index c7644558e979a4..35f3ec24b80b4f 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -148,6 +148,9 @@ def srcpath(path): ) cfg.msg("Enabled custom controller", 'no', color='YELLOW') + if cfg.options.enable_ppp: + env.CXXFLAGS += ['-DAP_NETWORKING_BACKEND_PPP=1'] + if cfg.options.disable_networking: env.CXXFLAGS += ['-DAP_NETWORKING_ENABLED=0'] @@ -438,22 +441,24 @@ def configure_env(self, cfg, env): '-Wl,--gc-sections', ] - if self.with_can and not cfg.env.AP_PERIPH: - env.AP_LIBRARIES += [ - 'AP_DroneCAN', - 'modules/DroneCAN/libcanard/*.c', - ] - if cfg.options.enable_dronecan_tests: + if self.with_can: + # for both AP_Perip and main fw enable deadlines + env.DEFINES.update(CANARD_ENABLE_DEADLINE = 1) + + if not cfg.env.AP_PERIPH: + env.AP_LIBRARIES += [ + 'AP_DroneCAN', + 'modules/DroneCAN/libcanard/*.c', + ] + if cfg.options.enable_dronecan_tests: + env.DEFINES.update(AP_TEST_DRONECAN_DRIVERS = 1) + env.DEFINES.update( - AP_TEST_DRONECAN_DRIVERS = 1 + DRONECAN_CXX_WRAPPERS = 1, + USE_USER_HELPERS = 1, + CANARD_ALLOCATE_SEM=1 ) - env.DEFINES.update( - DRONECAN_CXX_WRAPPERS = 1, - USE_USER_HELPERS = 1, - CANARD_ENABLE_DEADLINE = 1, - CANARD_ALLOCATE_SEM=1 - ) if cfg.options.build_dates: @@ -644,6 +649,8 @@ def configure_env(self, cfg, env): AP_BARO_PROBE_EXTERNAL_I2C_BUSES = 1, ) + env.BOARD_CLASS = "SITL" + cfg.define('AP_SIM_ENABLED', 1) cfg.define('HAL_WITH_SPI', 1) cfg.define('HAL_WITH_RAMTRON', 1) @@ -680,7 +687,8 @@ def configure_env(self, cfg, env): '-Werror=missing-declarations', ] - if not cfg.options.disable_networking: + if not cfg.options.disable_networking and not 'clang' in cfg.env.COMPILER_CC: + # lwip doesn't build with clang env.CXXFLAGS += ['-DAP_NETWORKING_ENABLED=1'] if cfg.options.ubsan or cfg.options.ubsan_abort: @@ -829,14 +837,65 @@ def get_name(self): return self.__class__.__name__ -class sitl_periph_gps(sitl): +class sitl_periph(sitl): def configure_env(self, cfg, env): cfg.env.AP_PERIPH = 1 - super(sitl_periph_gps, self).configure_env(cfg, env) + super(sitl_periph, self).configure_env(cfg, env) env.DEFINES.update( HAL_BUILD_AP_PERIPH = 1, PERIPH_FW = 1, - CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph"', + HAL_RAM_RESERVE_START = 0, + + CANARD_ENABLE_CANFD = 1, + CANARD_ENABLE_TAO_OPTION = 1, + CANARD_MULTI_IFACE = 1, + + # FIXME: SITL library should not be using AP_AHRS: + AP_AHRS_ENABLED = 1, + AP_AHRS_BACKEND_DEFAULT_ENABLED = 0, + AP_AHRS_DCM_ENABLED = 1, # need a default backend + HAL_EXTERNAL_AHRS_ENABLED = 0, + + HAL_MAVLINK_BINDINGS_ENABLED = 1, + + AP_AIRSPEED_AUTOCAL_ENABLE = 0, + AP_CAN_SLCAN_ENABLED = 0, + AP_ICENGINE_ENABLED = 0, + AP_MISSION_ENABLED = 0, + AP_RCPROTOCOL_ENABLED = 0, + AP_RTC_ENABLED = 0, + AP_SCHEDULER_ENABLED = 0, + AP_SCRIPTING_ENABLED = 0, + AP_STATS_ENABLED = 0, + AP_UART_MONITOR_ENABLED = 1, + COMPASS_CAL_ENABLED = 0, + COMPASS_LEARN_ENABLED = 0, + COMPASS_MOT_ENABLED = 0, + HAL_CAN_DEFAULT_NODE_ID = 0, + HAL_CANMANAGER_ENABLED = 0, + HAL_GCS_ENABLED = 0, + HAL_GENERATOR_ENABLED = 0, + HAL_LOGGING_ENABLED = 0, + HAL_LOGGING_MAVLINK_ENABLED = 0, + HAL_PROXIMITY_ENABLED = 0, + HAL_RALLY_ENABLED = 0, + HAL_SUPPORT_RCOUT_SERIAL = 0, + AP_TERRAIN_AVAILABLE = 0, + ) + + try: + env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=1') + except ValueError: + pass + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] + +class sitl_periph_universal(sitl_periph): + def configure_env(self, cfg, env): + super(sitl_periph_universal, self).configure_env(cfg, env) + env.DEFINES.update( + CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_universal"', + APJ_BOARD_ID = 100, + HAL_PERIPH_ENABLE_GPS = 1, HAL_PERIPH_ENABLE_AIRSPEED = 1, HAL_PERIPH_ENABLE_MAG = 1, @@ -847,53 +906,33 @@ def configure_env(self, cfg, env): HAL_PERIPH_ENABLE_RPM = 1, HAL_PERIPH_ENABLE_RC_OUT = 1, HAL_PERIPH_ENABLE_ADSB = 1, - AP_ICENGINE_ENABLED = 0, + HAL_PERIPH_ENABLE_SERIAL_OPTIONS = 1, AP_AIRSPEED_ENABLED = 1, - AP_AIRSPEED_AUTOCAL_ENABLE = 0, - AP_AHRS_ENABLED = 1, - AP_UART_MONITOR_ENABLED = 1, - HAL_CAN_DEFAULT_NODE_ID = 0, - HAL_RAM_RESERVE_START = 0, - APJ_BOARD_ID = 100, - HAL_GCS_ENABLED = 0, - HAL_MAVLINK_BINDINGS_ENABLED = 1, - HAL_LOGGING_ENABLED = 0, - HAL_LOGGING_MAVLINK_ENABLED = 0, - AP_MISSION_ENABLED = 0, - HAL_RALLY_ENABLED = 0, - AP_SCHEDULER_ENABLED = 0, - CANARD_ENABLE_TAO_OPTION = 1, - AP_RCPROTOCOL_ENABLED = 0, - CANARD_ENABLE_CANFD = 1, - CANARD_MULTI_IFACE = 1, - HAL_CANMANAGER_ENABLED = 0, - COMPASS_CAL_ENABLED = 0, - COMPASS_MOT_ENABLED = 0, - COMPASS_LEARN_ENABLED = 0, AP_BATTERY_ESC_ENABLED = 1, - HAL_EXTERNAL_AHRS_ENABLED = 0, - HAL_GENERATOR_ENABLED = 0, - AP_STATS_ENABLED = 0, - HAL_SUPPORT_RCOUT_SERIAL = 0, - AP_CAN_SLCAN_ENABLED = 0, - HAL_PROXIMITY_ENABLED = 0, - AP_SCRIPTING_ENABLED = 0, - HAL_NAVEKF3_AVAILABLE = 0, HAL_PWM_COUNT = 32, HAL_WITH_ESC_TELEM = 1, - AP_RTC_ENABLED = 0, + AP_TERRAIN_AVAILABLE = 1, ) - try: - env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=1') - except ValueError: - pass - env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] +class sitl_periph_gps(sitl_periph): + def configure_env(self, cfg, env): + cfg.env.AP_PERIPH = 1 + super(sitl_periph_gps, self).configure_env(cfg, env) + env.DEFINES.update( + HAL_BUILD_AP_PERIPH = 1, + PERIPH_FW = 1, + CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_gps"', + APJ_BOARD_ID = 101, + + HAL_PERIPH_ENABLE_GPS = 1, + ) class esp32(Board): abstract = True toolchain = 'xtensa-esp32-elf' def configure_env(self, cfg, env): + env.BOARD_CLASS = "ESP32" + def expand_path(p): print("USING EXPRESSIF IDF:"+str(env.idf)) return cfg.root.find_dir(env.IDF+p).abspath() @@ -906,7 +945,6 @@ def expand_path(p): cfg.load('esp32') env.DEFINES.update( CONFIG_HAL_BOARD = 'HAL_BOARD_ESP32', - AP_SIM_ENABLED = 0, ) tt = self.name[5:] #leave off 'esp32' so we just get 'buzz','diy','icarus, etc @@ -918,6 +956,15 @@ def expand_path(p): HAL_HAVE_HARDWARE_DOUBLE = '1', ) + if self.name.endswith("empty"): + # for empty targets build as SIM-on-HW + env.DEFINES.update(AP_SIM_ENABLED = 1) + env.AP_LIBRARIES += [ + 'SITL', + ] + else: + env.DEFINES.update(AP_SIM_ENABLED = 0) + env.AP_LIBRARIES += [ 'AP_HAL_ESP32', ] @@ -925,6 +972,7 @@ def expand_path(p): env.CFLAGS += [ '-fno-inline-functions', '-mlongcalls', + '-fsingle-precision-constant', ] env.CFLAGS.remove('-Werror=undef') @@ -940,6 +988,7 @@ def expand_path(p): '-Wno-sign-compare', '-fno-inline-functions', '-mlongcalls', + '-fsingle-precision-constant', # force const vals to be float , not double. so 100.0 means 100.0f '-fno-threadsafe-statics', '-DCYGWIN_BUILD'] env.CXXFLAGS.remove('-Werror=undef') @@ -985,6 +1034,7 @@ def configure_env(self, cfg, env): cfg.load('chibios') env.BOARD = self.name + env.BOARD_CLASS = "ChibiOS" env.DEFINES.update( CONFIG_HAL_BOARD = 'HAL_BOARD_CHIBIOS', @@ -1150,7 +1200,7 @@ def configure_env(self, cfg, env): env.INCLUDES += [ cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath(), - cfg.srcnode.find_dir('modules/ChibiOS/ext/lwip/src/include/compat/posix').abspath() + cfg.srcnode.find_dir('modules/lwip/src/include/compat/posix').abspath() ] # whitelist of compilers which we should build with -Werror @@ -1226,6 +1276,8 @@ def configure_env(self, cfg, env): self.with_can = True super(linux, self).configure_env(cfg, env) + env.BOARD_CLASS = "LINUX" + env.DEFINES.update( CONFIG_HAL_BOARD = 'HAL_BOARD_LINUX', CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_NONE', diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index a827af26f777f7..a14e3f9b8e6f1a 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -461,13 +461,17 @@ def setup_canmgr_build(cfg): 'CANARD_ALLOCATE_SEM=1' ] - if cfg.env.HAL_CANFD_SUPPORTED: - env.DEFINES += ['UAVCAN_SUPPORT_CANFD=1'] - else: - env.DEFINES += ['UAVCAN_SUPPORT_CANFD=0'] - cfg.get_board().with_can = True +def setup_canperiph_build(cfg): + '''enable CAN build for peripherals''' + env = cfg.env + env.DEFINES += [ + 'CANARD_ENABLE_DEADLINE=1', + ] + + cfg.get_board().with_can = True + def load_env_vars(env): '''optionally load extra environment variables from env.py in the build directory''' print("Checking for env.py") @@ -578,6 +582,8 @@ def bldpath(path): load_env_vars(cfg.env) if env.HAL_NUM_CAN_IFACES and not env.AP_PERIPH: setup_canmgr_build(cfg) + if env.HAL_NUM_CAN_IFACES and env.AP_PERIPH and not env.BOOTLOADER: + setup_canperiph_build(cfg) if env.HAL_NUM_CAN_IFACES and env.AP_PERIPH and int(env.HAL_NUM_CAN_IFACES)>1 and not env.BOOTLOADER: env.DEFINES += [ 'CANARD_MULTI_IFACE=1' ] setup_optimization(cfg.env) @@ -715,7 +721,7 @@ def build(bld): # errors if we accidentially try to use one of those functions either # directly or via another libc call wraplist = ['sscanf', 'fprintf', 'snprintf', 'vsnprintf','vasprintf','asprintf','vprintf','scanf', - 'fiprintf','printf', + 'printf', 'fopen', 'fflush', 'fwrite', 'fread', 'fputs', 'fgets', 'clearerr', 'fseek', 'ferror', 'fclose', 'tmpfile', 'getc', 'ungetc', 'feof', 'ftell', 'freopen', 'remove', 'vfprintf', 'fscanf', diff --git a/Tools/ardupilotwaf/embed.py b/Tools/ardupilotwaf/embed.py index ccc9673985ea10..1ca4c8f9966b60 100755 --- a/Tools/ardupilotwaf/embed.py +++ b/Tools/ardupilotwaf/embed.py @@ -7,7 +7,7 @@ May 2017 ''' -import os, sys, tempfile, gzip +import os, sys, zlib def write_encode(out, s): out.write(s.encode()) @@ -19,48 +19,36 @@ def embed_file(out, f, idx, embedded_name, uncompressed): except Exception: raise Exception("Failed to embed %s" % f) - pad = 0 if embedded_name.endswith("bootloader.bin"): # round size to a multiple of 32 bytes for bootloader, this ensures # it can be flashed on a STM32H7 chip blen = len(contents) pad = (32 - (blen % 32)) % 32 if pad != 0: - if sys.version_info[0] >= 3: - contents += bytes([0xff]*pad) - else: - for i in range(pad): - contents += bytes(chr(0xff)) + contents += bytes([0xff]*pad) print("Padded %u bytes for %s to %u" % (pad, embedded_name, len(contents))) - crc = crc32(bytearray(contents)) + crc = crc32(contents) write_encode(out, '__EXTFLASHFUNC__ static const uint8_t ap_romfs_%u[] = {' % idx) - compressed = tempfile.NamedTemporaryFile() if uncompressed: - # ensure nul termination - if sys.version_info[0] >= 3: - nul = bytearray(0) - else: - nul = chr(0) - if contents[-1] != nul: - contents += nul - compressed.write(contents) + # terminate if there's not already an existing null. we don't add it to + # the contents to avoid storing the wrong length + null_terminate = 0 not in contents + b = contents else: - # compress it - f = open(compressed.name, "wb") - with gzip.GzipFile(fileobj=f, mode='wb', filename='', compresslevel=9, mtime=0) as g: - g.write(contents) - f.close() - - compressed.seek(0) - b = bytearray(compressed.read()) - compressed.close() - - for c in b: - write_encode(out, '%u,' % c) + # compress it (max level, max window size, raw stream, max mem usage) + z = zlib.compressobj(level=9, method=zlib.DEFLATED, wbits=-15, memLevel=9) + b = z.compress(contents) + b += z.flush() + # decompressed data will be null terminated at runtime, nothing to do here + null_terminate = False + + write_encode(out, ",".join(str(c) for c in b)) + if null_terminate: + write_encode(out, ",0") write_encode(out, '};\n\n'); - return crc + return crc, len(contents) def crc32(bytes, crc=0): '''crc32 equivalent to crc32_small() from AP_Math/crc.cpp''' @@ -81,10 +69,11 @@ def create_embedded_h(filename, files, uncompressed=False): # remove duplicates and sort files = sorted(list(set(files))) crc = {} + decompressed_size = {} for i in range(len(files)): (name, filename) = files[i] try: - crc[filename] = embed_file(out, filename, i, name, uncompressed) + crc[filename], decompressed_size[filename] = embed_file(out, filename, i, name, uncompressed) except Exception as e: print(e) return False @@ -98,7 +87,8 @@ def create_embedded_h(filename, files, uncompressed=False): else: ustr = '' print("Embedding file %s:%s%s" % (name, filename, ustr)) - write_encode(out, '{ "%s", sizeof(ap_romfs_%u), 0x%08x, ap_romfs_%u },\n' % (name, i, crc[filename], i)) + write_encode(out, '{ "%s", sizeof(ap_romfs_%u), %d, 0x%08x, ap_romfs_%u },\n' % ( + name, i, decompressed_size[filename], crc[filename], i)) write_encode(out, '};\n') out.close() return True diff --git a/Tools/ardupilotwaf/esp32.py b/Tools/ardupilotwaf/esp32.py index 20a9528860de18..5e30e39ce3a8ef 100644 --- a/Tools/ardupilotwaf/esp32.py +++ b/Tools/ardupilotwaf/esp32.py @@ -62,6 +62,7 @@ def pre_build(self): """Configure esp-idf as lib target""" lib_vars = OrderedDict() lib_vars['ARDUPILOT_CMD'] = self.cmd + lib_vars['WAF_BUILD_TARGET'] = self.targets lib_vars['ARDUPILOT_LIB'] = self.bldnode.find_or_declare('lib/').abspath() lib_vars['ARDUPILOT_BIN'] = self.bldnode.find_or_declare('lib/bin').abspath() target = self.env.ESP32_TARGET diff --git a/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt new file mode 100644 index 00000000000000..ab2be0d1a45470 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/InertialLabsEAHRS/ap1.txt @@ -0,0 +1,7 @@ +QGC WPL 110 +0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.090027 1 +1 0 3 22 0.000000 0.000000 0.000000 0.000000 -35.361553 149.163956 20.000000 1 +2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364540 149.162857 50.000000 1 +3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.367333 149.163164 28.000000 1 +4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.366814 149.165878 28.000000 1 +5 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.362947 149.165179 0.000000 1 diff --git a/Tools/autotest/antennatracker.py b/Tools/autotest/antennatracker.py index b06df26492126c..438e6d7ce66587 100644 --- a/Tools/autotest/antennatracker.py +++ b/Tools/autotest/antennatracker.py @@ -162,6 +162,13 @@ def SCAN(self): timeout=90, comparator=operator.le) + def BaseMessageSet(self): + '''ensure we're getting messages we expect''' + self.set_parameter('BATT_MONITOR', 4) + self.reboot_sitl() + for msg in 'BATTERY_STATUS', : + self.assert_receive_message(msg) + def disabled_tests(self): return { "ArmFeatures": "See https://github.com/ArduPilot/ardupilot/issues/10652", @@ -178,5 +185,6 @@ def tests(self): self.MAV_CMD_MISSION_START, self.NMEAOutput, self.SCAN, + self.BaseMessageSet, ]) return ret diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 449b09064e06a2..d89ffa43f5ea63 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2160,8 +2160,10 @@ def configure_EKFs_to_use_optical_flow_instead_of_GPS(self): "EK3_SRC1_VELZ": 0, }) - def OpticalFlow(self): - '''test optical flow works''' + def OpticalFlowLocation(self): + '''test optical flow doesn't supply location''' + + self.context_push() self.assert_sensor_state(mavutil.mavlink.MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, False, False, False, verbose=True) @@ -2179,6 +2181,65 @@ def OpticalFlow(self): self.delay_sim_time(5) self.wait_statustext("Need Position Estimate", timeout=300) + self.context_pop() + + self.reboot_sitl() + + def OpticalFlow(self): + '''test OpticalFlow in flight''' + self.start_subtest("Make sure no crash if no rangefinder") + + self.context_push() + + self.set_parameter("SIM_FLOW_ENABLE", 1) + self.set_parameter("FLOW_TYPE", 10) + + self.set_analog_rangefinder_parameters() + + self.reboot_sitl() + + self.change_mode('LOITER') + + # ensure OPTICAL_FLOW message is reasonable: + global flow_rate_rads + global rangefinder_distance + global gps_speed + global last_debug_time + flow_rate_rads = 0 + rangefinder_distance = 0 + gps_speed = 0 + last_debug_time = 0 + + def check_optical_flow(mav, m): + global flow_rate_rads + global rangefinder_distance + global gps_speed + global last_debug_time + m_type = m.get_type() + if m_type == "OPTICAL_FLOW": + flow_rate_rads = math.sqrt(m.flow_comp_m_x**2+m.flow_comp_m_y**2) + elif m_type == "RANGEFINDER": + rangefinder_distance = m.distance + elif m_type == "GPS_RAW_INT": + gps_speed = m.vel/100.0 # cm/s -> m/s + of_speed = flow_rate_rads * rangefinder_distance + if abs(of_speed - gps_speed) > 3: + raise NotAchievedException("gps=%f vs of=%f mismatch" % + (gps_speed, of_speed)) + + now = self.get_sim_time_cached() + if now - last_debug_time > 5: + last_debug_time = now + self.progress("gps=%f of=%f" % (gps_speed, of_speed)) + + self.install_message_hook_context(check_optical_flow) + + self.fly_generic_mission("CMAC-copter-navtest.txt") + + self.context_pop() + + self.reboot_sitl() + def OpticalFlowLimits(self): '''test EKF navigation limiting''' ex = None @@ -3052,7 +3113,7 @@ def VisionPosition(self): """Disable GPS navigation, enable Vicon input.""" # scribble down a location we can set origin to: - self.customise_SITL_commandline(["--uartF=sim:vicon:"]) + self.customise_SITL_commandline(["--serial5=sim:vicon:"]) self.progress("Waiting for location") self.change_mode('LOITER') self.wait_ready_to_arm() @@ -3153,7 +3214,7 @@ def BodyFrameOdom(self): # only tested on this EKF return - self.customise_SITL_commandline(["--uartF=sim:vicon:"]) + self.customise_SITL_commandline(["--serial5=sim:vicon:"]) if self.current_onboard_log_contains_message("XKFD"): raise NotAchievedException("Found unexpected XKFD message") @@ -3293,7 +3354,7 @@ def InvalidJumpTags(self): def GPSViconSwitching(self): """Fly GPS and Vicon switching test""" - self.customise_SITL_commandline(["--uartF=sim:vicon:"]) + self.customise_SITL_commandline(["--serial5=sim:vicon:"]) """Setup parameters including switching to EKF3""" self.context_push() @@ -3623,6 +3684,15 @@ def fly_mission(self, filename, strict=True): self.wait_waypoint(num_wp-1, num_wp-1) self.wait_disarmed() + def fly_generic_mission(self, filename, strict=True): + num_wp = self.load_generic_mission(filename, strict=strict) + self.set_parameter("AUTO_OPTIONS", 3) + self.change_mode('AUTO') + self.wait_ready_to_arm() + self.arm_vehicle() + self.wait_waypoint(num_wp-1, num_wp-1) + self.wait_disarmed() + def SurfaceTracking(self): '''Test Surface Tracking''' ex = None @@ -5858,8 +5928,8 @@ def DynamicRpmNotches(self): freq, hover_throttle, peakdb2 = self.hover_and_check_matched_frequency_with_fft(-15, 20, 350, reverse=True) - # notch-per-motor should do better, but check for within 5% - if peakdb2 * 1.05 > peakdb1: + # notch-per-motor should do better, but check for within 10%. ( its mostly within 5%, but does vary a bit) + if peakdb2 * 1.10 > peakdb1: raise NotAchievedException( "Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" % (peakdb2, peakdb1)) @@ -7193,7 +7263,7 @@ def ProximitySensors(self): self.start_subtest("Testing %s" % name) self.set_parameter("PRX1_TYPE", prx_type) self.customise_SITL_commandline([ - "--uartF=sim:%s:" % name, + "--serial5=sim:%s:" % name, "--home", home_string, ]) self.wait_ready_to_arm() @@ -7630,7 +7700,7 @@ def RichenPower(self): }) self.reboot_sitl() self.set_rc(9, 1000) # remember this is a switch position - stop - self.customise_SITL_commandline(["--uartF=sim:richenpower"]) + self.customise_SITL_commandline(["--serial5=sim:richenpower"]) self.wait_statustext("requested state is not RUN", timeout=60) self.set_message_rate_hz("GENERATOR_STATUS", 10) @@ -7697,7 +7767,7 @@ def run_IE24(self, proto_ver): "LOG_DISARMED": 1, }) - self.customise_SITL_commandline(["--uartF=sim:ie24"]) + self.customise_SITL_commandline(["--serial5=sim:ie24"]) self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for electrical generator message looks right" % proto_ver) self.start_subsubtest("Protocol %i: Checking original voltage (electrical)" % proto_ver) @@ -8182,7 +8252,7 @@ def fly_rangefinder_mavlink(self): "SERIAL5_PROTOCOL": 1, "RNGFND1_TYPE": 10, }) - self.customise_SITL_commandline(['--uartF=sim:rf_mavlink']) + self.customise_SITL_commandline(['--serial5=sim:rf_mavlink']) self.change_mode('GUIDED') self.wait_ready_to_arm() @@ -8329,17 +8399,16 @@ def RangeFinderDrivers(self): drivers = drivers[3:] command_line_args = [] self.context_push() - for (offs, cmdline_argument, serial_num) in [(0, '--uartE', 4), - (1, '--uartF', 5), - (2, '--uartG', 6)]: + for offs in range(3): + serial_num = offs + 4 if len(do_drivers) > offs: if len(do_drivers[offs]) > 2: (sim_name, rngfnd_param_value, kwargs) = do_drivers[offs] else: (sim_name, rngfnd_param_value) = do_drivers[offs] kwargs = {} - command_line_args.append("%s=sim:%s" % - (cmdline_argument, sim_name)) + command_line_args.append("--serial%s=sim:%s" % + (serial_num, sim_name)) sets = { "SERIAL%u_PROTOCOL" % serial_num: 9, # rangefinder "RNGFND%u_TYPE" % (offs+1): rngfnd_param_value, @@ -8380,7 +8449,7 @@ def RangeFinderDriversMaxAlt(self): "WPNAV_SPEED_UP": 1000, # cm/s }) self.customise_SITL_commandline([ - "--uartE=sim:lightwareserial", + "--serial4=sim:lightwareserial", ]) self.takeoff(95, mode='GUIDED', timeout=240, max_err=0.5) self.assert_rangefinder_distance_between(90, 100) @@ -8834,7 +8903,7 @@ def test_replay_bit(self, bit): ok = check_replay.check_log(replay_log_filepath, self.progress, verbose=True) if not ok: - raise NotAchievedException("check_replay failed") + raise NotAchievedException("check_replay (%s) failed" % current_log_filepath) def DefaultIntervalsFromFiles(self): '''Test setting default mavlink message intervals from files''' @@ -9776,7 +9845,7 @@ def FETtecESC(self): "SERVO8_FUNCTION": 36, "SIM_ESC_TELEM": 0, }) - self.customise_SITL_commandline(["--uartF=sim:fetteconewireesc"]) + self.customise_SITL_commandline(["--serial5=sim:fetteconewireesc"]) self.FETtecESC_safety_switch() self.FETtecESC_esc_power_checks() self.FETtecESC_btw_mask_checks() @@ -10068,6 +10137,7 @@ def tests1d(self): self.ModeCircle, self.MagFail, self.OpticalFlow, + self.OpticalFlowLocation, self.OpticalFlowLimits, self.OpticalFlowCalibration, self.MotorFail, @@ -10251,7 +10321,7 @@ def test_rplidar(self, sim_device_name, expected_distances): "PRX1_TYPE": 5, }) self.customise_SITL_commandline([ - "--uartF=sim:%s:" % sim_device_name, + "--serial5=sim:%s:" % sim_device_name, "--home", "51.8752066,14.6487840,0,0", # SITL has "posts" here ]) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index fb78e2c186b536..ce476165310b47 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -844,8 +844,8 @@ def context_push_do_change_speed(self): # - DO_CHANGE_AIRSPEED is a permanent vehicle change! self.context_push() self.set_parameters({ - "TRIM_ARSPD_CM": self.get_parameter("TRIM_ARSPD_CM"), - "MIN_GNDSPD_CM": self.get_parameter("MIN_GNDSPD_CM"), + "AIRSPEED_CRUISE": self.get_parameter("AIRSPEED_CRUISE"), + "MIN_GROUNDSPEED": self.get_parameter("MIN_GROUNDSPEED"), "TRIM_THROTTLE": self.get_parameter("TRIM_THROTTLE"), }) @@ -886,7 +886,7 @@ def DO_CHANGE_SPEED_mission(self): self.change_mode('AUTO') checks = [ - (1, self.get_parameter("TRIM_ARSPD_CM") * 0.01), + (1, self.get_parameter("AIRSPEED_CRUISE")), (3, 10), (5, 20), (7, 15), @@ -1124,9 +1124,12 @@ def TestFlaps(self): def TestRCRelay(self): '''Test Relay RC Channel Option''' - self.set_parameter("RC12_OPTION", 28) # Relay On/Off + self.set_parameters({ + "RELAY1_FUNCTION": 1, # Enable relay as a standard relay pin + "RC12_OPTION": 28 # Relay On/Off + }) self.set_rc(12, 1000) - self.reboot_sitl() # needed for RC12_OPTION to take effect + self.reboot_sitl() # needed for RC12_OPTION and RELAY1_FUNCTION to take effect off = self.get_parameter("SIM_PIN_MASK") if off: @@ -2271,7 +2274,7 @@ def ClimbBeforeTurn(self): self.wait_ready_to_arm() self.set_parameters({ "FLIGHT_OPTIONS": 0, - "ALT_HOLD_RTL": 8000, + "RTL_ALTITUDE": 80, "RTL_AUTOLAND": 1, }) takeoff_alt = 10 @@ -2279,7 +2282,7 @@ def ClimbBeforeTurn(self): self.change_mode("CRUISE") self.wait_distance_to_home(500, 1000, timeout=60) self.change_mode("RTL") - expected_alt = self.get_parameter("ALT_HOLD_RTL") / 100.0 + expected_alt = self.get_parameter("RTL_ALTITUDE") home = self.home_position_as_mav_location() distance = self.get_distance(home, self.mav.location()) @@ -2303,7 +2306,7 @@ def ClimbBeforeTurn(self): self.wait_ready_to_arm() self.set_parameters({ "FLIGHT_OPTIONS": 16, - "ALT_HOLD_RTL": 10000, + "RTL_ALTITUDE": 100, }) self.takeoff(alt=takeoff_alt) self.change_mode("CRUISE") @@ -2336,7 +2339,7 @@ def RTL_CLIMB_MIN(self): self.wait_distance_to_home(1000, 1500, timeout=60) post_cruise_alt = self.get_altitude(relative=True) self.change_mode('RTL') - expected_alt = self.get_parameter("ALT_HOLD_RTL")/100.0 + expected_alt = self.get_parameter("RTL_ALTITUDE") if expected_alt == -1: expected_alt = self.get_altitude(relative=True) @@ -2919,8 +2922,8 @@ def SpeedToFly(self): trim_airspeed = m.airspeed self.progress("Using trim_airspeed=%f" % (trim_airspeed,)) - min_airspeed = self.get_parameter("ARSPD_FBW_MIN") - max_airspeed = self.get_parameter("ARSPD_FBW_MAX") + min_airspeed = self.get_parameter("AIRSPEED_MIN") + max_airspeed = self.get_parameter("AIRSPEED_MAX") if trim_airspeed > max_airspeed: raise NotAchievedException("trim airspeed > max_airspeed (%f>%f)" % @@ -3160,7 +3163,7 @@ def TerrainLoiter(self): def fly_external_AHRS(self, sim, eahrs_type, mission): """Fly with external AHRS (VectorNav)""" - self.customise_SITL_commandline(["--uartE=sim:%s" % sim]) + self.customise_SITL_commandline(["--serial4=sim:%s" % sim]) self.set_parameters({ "EAHRS_TYPE": eahrs_type, @@ -3279,6 +3282,10 @@ def MicroStrainEAHRS7(self): '''Test MicroStrain EAHRS series 7 support''' self.fly_external_AHRS("MicroStrain7", 7, "ap1.txt") + def InertialLabsEAHRS(self): + '''Test InertialLabs EAHRS support''' + self.fly_external_AHRS("ILabs", 5, "ap1.txt") + def get_accelvec(self, m): return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81 @@ -4214,7 +4221,7 @@ def EFITest(self, efi_type, name, sim_name, check_fuel_flow=True): }) self.customise_SITL_commandline( - ["--uartF=sim:%s" % sim_name, + ["--serial5=sim:%s" % sim_name, ], ) self.wait_ready_to_arm() @@ -4374,8 +4381,8 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1): seq = 0 items = [] tests = [ - (self.home_relative_loc_ne(50, -50), 100), - (self.home_relative_loc_ne(100, 50), 1005), + (self.home_relative_loc_ne(50, -50), 100, 0.3), + (self.home_relative_loc_ne(100, 50), 1005, 3), ] # add a home position: items.append(self.mav.mav.mission_item_int_encode( @@ -4416,7 +4423,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1): seq += 1 # add circles - for (loc, radius) in tests: + for (loc, radius, turn) in tests: items.append(self.mav.mav.mission_item_int_encode( target_system, target_component, @@ -4425,7 +4432,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1): mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS, 0, # current 0, # autocontinue - 3, # p1 + turn, # p1 0, # p2 radius, # p3 0, # p4 @@ -4458,7 +4465,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1): downloaded_items = self.download_using_mission_protocol(mavutil.mavlink.MAV_MISSION_TYPE_MISSION) ofs = 2 self.progress("Checking downloaded mission is as expected") - for (loc, radius) in tests: + for (loc, radius, turn) in tests: downloaded = downloaded_items[ofs] if radius > 255: # ArduPilot only stores % 10 @@ -4467,6 +4474,13 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1): raise NotAchievedException( "Did not get expected radius for item %u; want=%f got=%f" % (ofs, radius, downloaded.param3)) + if turn > 0 and turn < 1: + # ArduPilot stores fractions in 8 bits (*256) and unpacks it (/256) + turn = int(turn*256) / 256.0 + if downloaded.param1 != turn: + raise NotAchievedException( + "Did not get expected turn for item %u; want=%f got=%f" % + (ofs, turn, downloaded.param1)) ofs += 1 self.change_mode('AUTO') @@ -4476,7 +4490,7 @@ def MAV_CMD_NAV_LOITER_TURNS(self, target_system=1, target_component=1): self.wait_current_waypoint(2) - for (loc, expected_radius) in tests: + for (loc, expected_radius, _) in tests: self.wait_circling_point_with_radius( loc, expected_radius, @@ -4515,7 +4529,7 @@ def AerobaticsScripting(self): self.customise_SITL_commandline( [], model=model, - defaults_filepath="", + defaults_filepath="Tools/autotest/models/plane-3d.parm", wipe=True) self.context_push() @@ -5358,6 +5372,7 @@ def tests(self): self.VectorNavEAHRS, self.MicroStrainEAHRS5, self.MicroStrainEAHRS7, + self.InertialLabsEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, @@ -5384,7 +5399,6 @@ def tests(self): self.GlideSlopeThresh, self.HIGH_LATENCY2, self.MidAirDisarmDisallowed, - self.EmbeddedParamParser, self.AerobaticsScripting, self.MANUAL_CONTROL, self.RunMissionScript, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index 40eb081cfb3825..6d234c8ebfd669 100644 --- a/Tools/autotest/ardusub.py +++ b/Tools/autotest/ardusub.py @@ -166,6 +166,44 @@ def AltitudeHold(self): self.watch_altitude_maintained() self.disarm_vehicle() + def RngfndQuality(self): + """Check lua Range Finder quality information flow""" + self.context_push() + self.context_collect('STATUSTEXT') + + ex = None + try: + self.set_parameters({ + "SCR_ENABLE": 1, + "RNGFND1_TYPE": 36, + "RNGFND1_ORIENT": 25, + "RNGFND1_MIN_CM": 10, + "RNGFND1_MAX_CM": 5000, + }) + + self.install_example_script_context("rangefinder_quality_test.lua") + + # These string must match those sent by the lua test script. + complete_str = "#complete#" + failure_str = "!!failure!!" + + self.reboot_sitl() + + self.wait_statustext(complete_str, timeout=20, check_context=True) + found_failure = self.statustext_in_collections(failure_str) + + if found_failure is not None: + raise NotAchievedException("RngfndQuality test failed: " + found_failure.text) + + except Exception as e: + self.print_exception_caught(e) + ex = e + + self.context_pop() + + if ex: + raise ex + def ModeChanges(self, delta=0.2): """Check if alternating between ALTHOLD, STABILIZE and POSHOLD affects altitude""" self.wait_ready_to_arm() @@ -524,6 +562,7 @@ def tests(self): ret.extend([ self.DiveManual, self.AltitudeHold, + self.RngfndQuality, self.PositionHold, self.ModeChanges, self.DiveMission, diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 616411b7165a8a..52bb3aac1e95e2 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -286,7 +286,7 @@ def should_run_step(step): "Blimp": "blimp", "BalanceBot": "ardurover", "Sailboat": "ardurover", - "SITLPeriphGPS": "sitl_periph_gp.AP_Periph", + "SITLPeriphUniversal": ("sitl_periph_universal", "AP_Periph"), "CAN": "arducopter", } @@ -298,17 +298,16 @@ def binary_path(step, debug=False): except Exception: return None - if vehicle in __bin_names: - if len(__bin_names[vehicle].split(".")) == 2: - config_name = __bin_names[vehicle].split(".")[0] - binary_name = __bin_names[vehicle].split(".")[1] - else: - config_name = 'sitl' - binary_name = __bin_names[vehicle] - else: + if vehicle not in __bin_names: # cope with builds that don't have a specific binary return None + try: + (config_name, binary_name) = __bin_names[vehicle] + except ValueError: + config_name = "sitl" + binary_name = __bin_names[vehicle] + binary = util.reltopdir(os.path.join('build', config_name, 'bin', @@ -362,8 +361,8 @@ def find_specific_test_to_run(step): } supplementary_test_binary_map = { - "test.CAN": ["sitl_periph_gps:AP_Periph:0:Tools/autotest/default_params/periph.parm,Tools/autotest/default_params/quad-periph.parm", # noqa: E501 - "sitl_periph_gps:AP_Periph:1:Tools/autotest/default_params/periph.parm"], + "test.CAN": ["sitl_periph_universal:AP_Periph:0:Tools/autotest/default_params/periph.parm,Tools/autotest/default_params/quad-periph.parm", # noqa: E501 + "sitl_periph_universal:AP_Periph:1:Tools/autotest/default_params/periph.parm"], } @@ -420,6 +419,7 @@ def run_step(step): build_opts = build_opts vehicle_binary = None + board = "sitl" if step == 'build.Plane': vehicle_binary = 'bin/arduplane' @@ -441,8 +441,9 @@ def run_step(step): if step == 'build.Sub': vehicle_binary = 'bin/ardusub' - if step == 'build.SITLPeriphGPS': - vehicle_binary = 'sitl_periph_gps.bin/AP_Periph' + if step == 'build.SITLPeriphUniversal': + vehicle_binary = 'bin/AP_Periph' + board = 'sitl_periph_universal' if step == 'build.Replay': return util.build_replay(board='SITL') @@ -453,14 +454,11 @@ def run_step(step): os.unlink(binary) except (FileNotFoundError, ValueError): pass - if len(vehicle_binary.split(".")) == 1: - return util.build_SITL(vehicle_binary, **build_opts) - else: - return util.build_SITL( - vehicle_binary.split(".")[1], - board=vehicle_binary.split(".")[0], - **build_opts - ) + return util.build_SITL( + vehicle_binary, + board=board, + **build_opts + ) binary = binary_path(step, debug=opts.debug) @@ -1085,7 +1083,7 @@ def format_epilog(self, formatter): 'build.Blimp', 'test.Blimp', - 'build.SITLPeriphGPS', + 'build.SITLPeriphUniversal', 'test.CAN', # convertgps disabled as it takes 5 hours diff --git a/Tools/autotest/default_params/firefly.parm b/Tools/autotest/default_params/firefly.parm index 01f95bfb3cc051..07c681d8b6de3e 100644 --- a/Tools/autotest/default_params/firefly.parm +++ b/Tools/autotest/default_params/firefly.parm @@ -8,8 +8,8 @@ SERVO2_MAX = 2000 SERVO2_MIN = 1000 AHRS_EKF_TYPE 2 ARMING_RUDDER 2 -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 6 +AIRSPEED_MAX 35 +AIRSPEED_MIN 6 ARSPD_USE 1 AUTOTUNE_LEVEL 8 COMPASS_OFS2_X -0.420265 @@ -45,9 +45,9 @@ INS_ACCSCAL_Y 1.001000 INS_ACCSCAL_Z 1.001000 INS_GYR_CAL 0 KFF_RDDRMIX 0.500000 -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -3000 -LIM_ROLL_CD 6500 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -30.00 +ROLL_LIMIT_DEG 65.00 NAVL1_PERIOD 8 PTCH_RATE_D 0.000000 PTCH_RATE_I 0.212500 @@ -71,7 +71,7 @@ RLL_RATE_IMAX 0.888889 RLL_RATE_P 0.141009 SCHED_LOOP_RATE 300 THR_MAX 100 -TRIM_ARSPD_CM 2500 +AIRSPEED_CRUISE 25.00 LOG_BITMASK 65534 # map channel 13 out for tilt Q_ENABLE 1 diff --git a/Tools/autotest/default_params/plane-jet.parm b/Tools/autotest/default_params/plane-jet.parm index 93a58ac6d1932c..13580dbc662349 100644 --- a/Tools/autotest/default_params/plane-jet.parm +++ b/Tools/autotest/default_params/plane-jet.parm @@ -1,11 +1,11 @@ -ALT_HOLD_RTL 20000.0000 -ARSPD_FBW_MAX 80.0000 -ARSPD_FBW_MIN 25.0000 +RTL_ALTITUDE 200.00 +AIRSPEED_MAX 80.0000 +AIRSPEED_MIN 25.0000 FBWB_CLIMB_RATE 5.0000 LAND_FLARE_SEC 2.0000 -LIM_PITCH_MAX 2000.0000 -LIM_PITCH_MIN -2500.0000 -LIM_ROLL_CD 7000.0000 +PTCH_LIM_MAX_DEG 20.00 +PTCH_LIM_MIN_DEG -25.00 +ROLL_LIMIT_DEG 70.00 NAVL1_PERIOD 17.0000 PTCH_RATE_D 0.000000 PTCH_RATE_FF 0.690000 @@ -26,7 +26,7 @@ TECS_SINK_MIN 10.0000 THR_SLEWRATE 30.0000 TKOFF_ROTATE_SPD 20.0000 TKOFF_THR_MAX 100.0000 -TRIM_ARSPD_CM 4000.0000 +AIRSPEED_CRUISE 40.00 TRIM_THROTTLE 36.0000 WP_LOITER_RAD 180.0000 WP_RADIUS 140.0000 diff --git a/Tools/autotest/default_params/plane-jsbsim.parm b/Tools/autotest/default_params/plane-jsbsim.parm index a77ec1084ef7d1..70cff97a7e25d8 100644 --- a/Tools/autotest/default_params/plane-jsbsim.parm +++ b/Tools/autotest/default_params/plane-jsbsim.parm @@ -1,18 +1,18 @@ EK2_ENABLE 1 BATT_MONITOR 4 LOG_BITMASK 65535 -TRIM_ARSPD_CM 2200 -TRIM_PITCH_CD 0 +AIRSPEED_CRUISE 22.00 +PTCH_TRIM_DEG 0.00 TRIM_THROTTLE 50 -LIM_PITCH_MIN -2000 -LIM_PITCH_MAX 2500 -LIM_ROLL_CD 6500 +PTCH_LIM_MIN_DEG -20.00 +PTCH_LIM_MAX_DEG 25.00 +ROLL_LIMIT_DEG 65.00 LAND_DISARMDELAY 3 -LAND_PITCH_CD 100 +LAND_PITCH_DEG 1.00 LAND_FLARE_SEC 3 ARSPD_USE 1 -ARSPD_FBW_MAX 30 -ARSPD_FBW_MIN 10 +AIRSPEED_MAX 30 +AIRSPEED_MIN 10 KFF_RDDRMIX 0.5 THR_MAX 100 RC2_REVERSED 1 diff --git a/Tools/autotest/default_params/plane-soaring.parm b/Tools/autotest/default_params/plane-soaring.parm index 4723eaf42ff6a2..a7f1aa65d47e35 100644 --- a/Tools/autotest/default_params/plane-soaring.parm +++ b/Tools/autotest/default_params/plane-soaring.parm @@ -1,5 +1,5 @@ -ARSPD_FBW_MAX 20 -ARSPD_FBW_MIN 8 +AIRSPEED_MAX 20 +AIRSPEED_MIN 8 FLIGHT_OPTIONS 2 @@ -28,7 +28,7 @@ SOAR_VSPEED 0.700000 TECS_SPDWEIGHT 2.000000 -TRIM_ARSPD_CM 900.000000 +AIRSPEED_CRUISE 9.00 NAVL1_LIM_BANK 60.000000 NAVL1_PERIOD 10.000 diff --git a/Tools/autotest/default_params/plane-tailsitter.parm b/Tools/autotest/default_params/plane-tailsitter.parm index b920f96f64c8bf..1db32c8f92ba6f 100644 --- a/Tools/autotest/default_params/plane-tailsitter.parm +++ b/Tools/autotest/default_params/plane-tailsitter.parm @@ -1,12 +1,12 @@ BATT_MONITOR 4 -TRIM_ARSPD_CM 2200 +AIRSPEED_CRUISE 22.00 TRIM_THROTTLE 50 -LIM_PITCH_MIN -2000 -LIM_PITCH_MAX 2500 -LIM_ROLL_CD 6500 +PTCH_LIM_MIN_DEG -20.00 +PTCH_LIM_MAX_DEG 25.00 +ROLL_LIMIT_DEG 65.00 ARSPD_USE 1 -ARSPD_FBW_MAX 30 -ARSPD_FBW_MIN 10 +AIRSPEED_MAX 30 +AIRSPEED_MIN 10 THR_MAX 100 KFF_RDDRMIX 1 RC1_MAX 2000 diff --git a/Tools/autotest/default_params/quadplane-cl84.parm b/Tools/autotest/default_params/quadplane-cl84.parm index 9d988cd89e5a0d..493d3059581730 100644 --- a/Tools/autotest/default_params/quadplane-cl84.parm +++ b/Tools/autotest/default_params/quadplane-cl84.parm @@ -1,7 +1,7 @@ ARMING_RUDDER 2 ARSPD_TYPE 0 -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 6 +AIRSPEED_MAX 35 +AIRSPEED_MIN 6 AUTOTUNE_LEVEL 8 COMPASS_OFS2_X -0.420265 COMPASS_OFS2_Y -0.726942 @@ -34,9 +34,9 @@ INS_ACCSCAL_Y 1.001000 INS_ACCSCAL_Z 1.001000 INS_GYR_CAL 0 KFF_RDDRMIX 0.500000 -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -3000 -LIM_ROLL_CD 6500 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -30.00 +ROLL_LIMIT_DEG 65.00 NAVL1_PERIOD 14 PTCH_RATE_D 0.000000 PTCH_RATE_I 0.212500 @@ -51,13 +51,13 @@ RLL_RATE_I 0.212500 RLL_RATE_IMAX 0.888889 RLL_RATE_P 0.141009 THR_MAX 100 -TRIM_ARSPD_CM 2500 +AIRSPEED_CRUISE 25.00 SERVO12_FUNCTION 41 SERVO12_MIN 1000 SERVO12_MAX 2000 SERVO11_TRIM 1500 RTL_RADIUS 80 -ALT_HOLD_RTL 2000 +RTL_ALTITUDE 20.00 Q_ENABLE 1 Q_FRAME_CLASS 7 Q_ANGLE_MAX 4500 diff --git a/Tools/autotest/default_params/quadplane-tilthvec.parm b/Tools/autotest/default_params/quadplane-tilthvec.parm index 336f5209c5a447..f7fe478d55e8fe 100644 --- a/Tools/autotest/default_params/quadplane-tilthvec.parm +++ b/Tools/autotest/default_params/quadplane-tilthvec.parm @@ -1,5 +1,5 @@ -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -3000 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -30.00 Q_ENABLE 1 Q_FRAME_CLASS 1 Q_FRAME_TYPE 3 diff --git a/Tools/autotest/default_params/quadplane-tilttri.parm b/Tools/autotest/default_params/quadplane-tilttri.parm index 05fdeb052dfc3a..f4a499185163cd 100644 --- a/Tools/autotest/default_params/quadplane-tilttri.parm +++ b/Tools/autotest/default_params/quadplane-tilttri.parm @@ -1,7 +1,7 @@ AHRS_EKF_TYPE 2 ARMING_RUDDER 2 -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 6 +AIRSPEED_MAX 35 +AIRSPEED_MIN 6 ARSPD_USE 1 AUTOTUNE_LEVEL 8 COMPASS_OFS2_X -0.420265 @@ -37,9 +37,9 @@ INS_ACCSCAL_Y 1.001000 INS_ACCSCAL_Z 1.001000 INS_GYR_CAL 0 KFF_RDDRMIX 0.500000 -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -3000 -LIM_ROLL_CD 6500 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -30.00 +ROLL_LIMIT_DEG 65.00 NAVL1_PERIOD 14 PTCH_RATE_D 0.000000 PTCH_RATE_I 0.212500 @@ -67,7 +67,7 @@ RLL_RATE_IMAX 0.888889 RLL_RATE_P 0.141009 SCHED_LOOP_RATE 300 THR_MAX 100 -TRIM_ARSPD_CM 2500 +AIRSPEED_CRUISE 25.00 LOG_BITMASK 65534 Q_TILT_MASK 3 Q_VFWD_GAIN 0.1 @@ -76,6 +76,6 @@ SERVO12_MIN 1000 SERVO12_MAX 2000 RTL_RADIUS 80 Q_RTL_MODE 1 -ALT_HOLD_RTL 2000 +RTL_ALTITUDE 20.00 PTCH_RATE_FF 1.407055 RLL_RATE_FF 0.552741 diff --git a/Tools/autotest/default_params/quadplane-tilttrivec.parm b/Tools/autotest/default_params/quadplane-tilttrivec.parm index e1d20acab5dbe3..1efb903f2ca0b4 100644 --- a/Tools/autotest/default_params/quadplane-tilttrivec.parm +++ b/Tools/autotest/default_params/quadplane-tilttrivec.parm @@ -1,7 +1,7 @@ AHRS_EKF_TYPE 2 ARMING_RUDDER 2 -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 6 +AIRSPEED_MAX 35 +AIRSPEED_MIN 6 ARSPD_USE 1 AUTOTUNE_LEVEL 8 COMPASS_OFS2_X -0.420265 @@ -37,9 +37,9 @@ INS_ACCSCAL_Y 1.001000 INS_ACCSCAL_Z 1.001000 INS_GYR_CAL 0 KFF_RDDRMIX 0.500000 -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -3000 -LIM_ROLL_CD 6500 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -30.00 +ROLL_LIMIT_DEG 65.00 NAVL1_PERIOD 14 PTCH_RATE_D 0.000000 PTCH_RATE_I 0.212500 @@ -67,7 +67,7 @@ RLL_RATE_IMAX 0.888889 RLL_RATE_P 0.141009 SCHED_LOOP_RATE 300 THR_MAX 100 -TRIM_ARSPD_CM 2500 +AIRSPEED_CRUISE 25.00 LOG_BITMASK 65534 Q_TILT_MASK 3 Q_VFWD_GAIN 0.1 @@ -79,7 +79,7 @@ SERVO12_MIN 1000 SERVO12_MAX 2000 RTL_RADIUS 80 Q_RTL_MODE 1 -ALT_HOLD_RTL 2000 +RTL_ALTITUDE 20.00 Q_TILT_TYPE 2 Q_TILT_YAW_ANGLE 10 PTCH_RATE_FF 1.407055 diff --git a/Tools/autotest/default_params/quadplane-tri.parm b/Tools/autotest/default_params/quadplane-tri.parm index 1bec298bb93794..c567bf4dba5d0d 100644 --- a/Tools/autotest/default_params/quadplane-tri.parm +++ b/Tools/autotest/default_params/quadplane-tri.parm @@ -1,7 +1,7 @@ AHRS_EKF_TYPE 2 ARMING_RUDDER 2 -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 13 +AIRSPEED_MAX 35 +AIRSPEED_MIN 13 ARSPD_USE 1 AUTOTUNE_LEVEL 8 COMPASS_OFS2_X -0.420265 @@ -37,9 +37,9 @@ INS_ACCSCAL_Y 1.001000 INS_ACCSCAL_Z 1.001000 INS_GYR_CAL 0 KFF_RDDRMIX 0.500000 -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -3000 -LIM_ROLL_CD 6500 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -30.00 +ROLL_LIMIT_DEG 65.00 NAVL1_PERIOD 14 PTCH_RATE_D 0.000000 PTCH_RATE_I 0.212500 @@ -67,7 +67,7 @@ RLL_RATE_IMAX 0.888889 RLL_RATE_P 0.141009 SCHED_LOOP_RATE 300 THR_MAX 100 -TRIM_ARSPD_CM 2500 +AIRSPEED_CRUISE 25.00 LOG_BITMASK 65534 PTCH_RATE_FF 1.407055 RLL_RATE_FF 0.552741 diff --git a/Tools/autotest/default_params/quadplane.parm b/Tools/autotest/default_params/quadplane.parm index 92a58bb015fdfa..62c5225d052d52 100644 --- a/Tools/autotest/default_params/quadplane.parm +++ b/Tools/autotest/default_params/quadplane.parm @@ -1,6 +1,6 @@ ARMING_RUDDER 2 -ARSPD_FBW_MAX 35 -ARSPD_FBW_MIN 13 +AIRSPEED_MAX 35 +AIRSPEED_MIN 13 ARSPD_USE 1 AUTOTUNE_LEVEL 8 COMPASS_OFS2_X -0.420265 @@ -35,9 +35,9 @@ INS_ACCSCAL_Y 1.001000 INS_ACCSCAL_Z 1.001000 INS_GYR_CAL 0 KFF_RDDRMIX 0.500000 -LIM_PITCH_MAX 3000 -LIM_PITCH_MIN -3000 -LIM_ROLL_CD 6500 +PTCH_LIM_MAX_DEG 30.00 +PTCH_LIM_MIN_DEG -30.00 +ROLL_LIMIT_DEG 65.00 NAVL1_PERIOD 14 PTCH_RATE_D 0.000000 PTCH_RATE_I 0.212500 @@ -63,7 +63,7 @@ RLL_RATE_IMAX 0.888889 RLL_RATE_P 0.141009 SCHED_LOOP_RATE 300 THR_MAX 100 -TRIM_ARSPD_CM 2500 +AIRSPEED_CRUISE 25.00 LOG_BITMASK 65534 Q_M_THST_EXPO 0.65 Q_M_PWM_MIN 1000 @@ -74,6 +74,6 @@ Q_A_RAT_RLL_P 0.15 Q_A_RAT_PIT_P 0.15 Q_A_RAT_RLL_D 0.002 Q_A_RAT_PIT_D 0.002 -ALT_HOLD_RTL 2000 +RTL_ALTITUDE 20.00 PTCH_RATE_FF 1.407055 RLL_RATE_FF 0.552741 diff --git a/Tools/autotest/default_params/rover.parm b/Tools/autotest/default_params/rover.parm index 9095f40720d5d8..58e064d81b3f5c 100644 --- a/Tools/autotest/default_params/rover.parm +++ b/Tools/autotest/default_params/rover.parm @@ -24,8 +24,8 @@ RC1_MAX 2000 RC1_MIN 1000 RC3_MAX 2000 RC3_MIN 1000 -RELAY_PIN 1 -RELAY_PIN2 2 +RELAY1_PIN 1 +RELAY2_PIN 2 SERVO1_MIN 1000 SERVO1_MAX 2000 SERVO3_MAX 2000 diff --git a/Tools/autotest/default_params/vee-gull 005.param b/Tools/autotest/default_params/vee-gull 005.param index 2ae334ae8b7fc1..710889e2d7b0d8 100644 --- a/Tools/autotest/default_params/vee-gull 005.param +++ b/Tools/autotest/default_params/vee-gull 005.param @@ -15,8 +15,8 @@ AHRS_TRIM_Y,0 AHRS_TRIM_Z,0 AHRS_WIND_MAX,0 AHRS_YAW_P,0.2 -ALT_HOLD_FBWCM,0 -ALT_HOLD_RTL,10000 +CRUISE_ALT_FLOOR,0.00 +RTL_ALTITUDE,100.00 ALT_OFFSET,0 ARMING_ACCTHRESH,0.75 ARMING_CHECK,0 @@ -26,8 +26,8 @@ ARMING_REQUIRE,0 ARMING_RUDDER,1 ARSPD_AUTOCAL,0 ARSPD_BUS,1 -ARSPD_FBW_MAX,15 -ARSPD_FBW_MIN,10 +AIRSPEED_MAX,15 +AIRSPEED_MIN,10 ARSPD_OFFSET,2015.877 ARSPD_PIN,1 ARSPD_PSI_RANGE,1 @@ -330,22 +330,22 @@ LAND_FLARE_SEC,2 LAND_PF_ALT,10 LAND_PF_ARSPD,0 LAND_PF_SEC,6 -LAND_PITCH_CD,0 +LAND_PITCH_DEG,0.00 LAND_SLOPE_RCALC,2 LAND_THEN_NEUTRL,0 LAND_THR_SLEW,0 LAND_TYPE,0 LEVEL_ROLL_LIMIT,5 -LIM_PITCH_MAX,2000 -LIM_PITCH_MIN,-2500 -LIM_ROLL_CD,4500 +PTCH_LIM_MAX_DEG,20.00 +PTCH_LIM_MIN_DEG,-25.00 +ROLL_LIMIT_DEG,45.00 LOG_BACKEND_TYPE,1 LOG_BITMASK,65535 LOG_DISARMED,0 LOG_FILE_BUFSIZE,16 LOG_FILE_DSRMROT,0 LOG_REPLAY,0 -MIN_GNDSPD_CM,0 +MIN_GROUNDSPEED,0 MIS_RESTART,0 MIS_TOTAL,7 MIXING_GAIN,0.5 @@ -456,10 +456,7 @@ RCMAP_ROLL,1 RCMAP_THROTTLE,3 RCMAP_YAW,4 RELAY_DEFAULT,0 -RELAY_PIN,13 -RELAY_PIN2,-1 -RELAY_PIN3,-1 -RELAY_PIN4,-1 +RELAY1_PIN,13 RLL_RATE_D,0.000000 RLL_RATE_FF,0.255000 RLL_RATE_I,0.050000 @@ -804,9 +801,9 @@ TKOFF_THR_MAX,0 TKOFF_THR_MINACC,0 TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,0 -TRIM_ARSPD_CM,1200 +AIRSPEED_CRUISE,12.00 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PTCH_TRIM_DEG,0.00 TRIM_RC_AT_START,0 TRIM_THROTTLE,45 TUNE_CHAN,0 diff --git a/Tools/autotest/logger_metadata/emit_html.py b/Tools/autotest/logger_metadata/emit_html.py index 08f16160f0c4a2..506dfbc104565b 100644 --- a/Tools/autotest/logger_metadata/emit_html.py +++ b/Tools/autotest/logger_metadata/emit_html.py @@ -37,14 +37,24 @@ def emit(self, doccos, enumerations): print('

%s

' % docco.description, file=self.fh) print(' ', file=self.fh) - print(" ", + print(" ", file=self.fh) for f in docco.fields_order: if "description" in docco.fields[f]: fdesc = docco.fields[f]["description"] else: fdesc = "" - print(' ' % (f, fdesc), + if "units" in docco.fields[f] and docco.fields[f]["units"]!="": + ftypeunits = docco.fields[f]["units"] + elif "fmt" in docco.fields[f] and "char" in docco.fields[f]["fmt"]: + ftypeunits = docco.fields[f]["fmt"] + elif "bitmaskenum" in docco.fields[f]: + ftypeunits = "bitmask" + elif "valueenum" in docco.fields[f]: + ftypeunits = "enum" + else: + ftypeunits = "" + print(' ' % (f, ftypeunits, fdesc), file=self.fh) print('
FieldNameDescription
FieldNameUnits/TypeDescription
%s%s
%s%s%s
', file=self.fh) diff --git a/Tools/autotest/logger_metadata/emit_md.py b/Tools/autotest/logger_metadata/emit_md.py index d6d1cb78778ba4..f9ae44492b5744 100644 --- a/Tools/autotest/logger_metadata/emit_md.py +++ b/Tools/autotest/logger_metadata/emit_md.py @@ -67,13 +67,23 @@ def emit(self, doccos, enumerations=None): if docco.url is not None: desc += f' ([Read more...]({docco.url}))' print(desc, file=self.fh) - print("\n|FieldName|Description|\n|---|---|", file=self.fh) + print("\n|FieldName|Units/Type|Description|\n|---|---|---|", file=self.fh) for f in docco.fields_order: if "description" in docco.fields[f]: fdesc = docco.fields[f]["description"] else: fdesc = "" - print(f'|{f}|{fdesc}|', file=self.fh) + if "units" in docco.fields[f] and docco.fields[f]["units"]!="": + ftypeunits = docco.fields[f]["units"] + elif "fmt" in docco.fields[f] and "char" in docco.fields[f]["fmt"]: + ftypeunits = docco.fields[f]["fmt"] + elif "bitmaskenum" in docco.fields[f]: + ftypeunits = "bitmask" + elif "valueenum" in docco.fields[f]: + ftypeunits = "enum" + else: + ftypeunits = "" + print(f'|{f}|{ftypeunits}|{fdesc}|', file=self.fh) print("", file=self.fh) self.stop() diff --git a/Tools/autotest/logger_metadata/emit_rst.py b/Tools/autotest/logger_metadata/emit_rst.py index 6b48ca5b315e36..dfaa8fea159ce1 100644 --- a/Tools/autotest/logger_metadata/emit_rst.py +++ b/Tools/autotest/logger_metadata/emit_rst.py @@ -37,17 +37,23 @@ def emit(self, doccos, enumerations): rows = [] for f in docco.fields_order: + # Populate the description column if "description" in docco.fields[f]: fdesc = docco.fields[f]["description"] else: fdesc = "" + # Initialise Type/Unit and check for enum/bitfields + ftypeunit = "" fieldnamething = None if "bitmaskenum" in docco.fields[f]: fieldnamething = "bitmaskenum" table_label = "Bitmask values" + ftypeunit = "bitmask" elif "valueenum" in docco.fields[f]: fieldnamething = "valueenum" table_label = "Values" + ftypeunit = "enum" + # If an enum/bitmask is defined, build the table if fieldnamething is not None: enum_name = docco.fields[f][fieldnamething] if enum_name not in enumerations: @@ -62,9 +68,16 @@ def emit(self, doccos, enumerations): comment = "" bitmaskrows.append([enumentry.name, str(enumentry.value), comment]) fdesc += "\n%s:\n\n%s" % (table_label, self.tablify(bitmaskrows)) - rows.append([f, fdesc]) - - print(self.tablify(rows), file=self.fh) + # Populate the Type/Units column + if "units" in docco.fields[f] and docco.fields[f]["units"] != "": + ftypeunit = docco.fields[f]["units"] + elif "fmt" in docco.fields[f] and "char" in docco.fields[f]["fmt"]: + ftypeunit = docco.fields[f]["fmt"] + # Add the new row + rows.append([f, ftypeunit, fdesc]) + + if rows: + print(self.tablify(rows), file=self.fh) print("", file=self.fh) self.stop() diff --git a/Tools/autotest/logger_metadata/emit_xml.py b/Tools/autotest/logger_metadata/emit_xml.py index c0fc39b0c0dc83..05073364c91aa4 100644 --- a/Tools/autotest/logger_metadata/emit_xml.py +++ b/Tools/autotest/logger_metadata/emit_xml.py @@ -31,24 +31,37 @@ def emit(self, doccos, enumerations): xml_fields = etree.SubElement(xml_logformat, 'fields') for f in docco.fields_order: - xml_field = etree.SubElement(xml_fields, 'field', name=f) + units = docco.fields[f]['units'] if "units" in docco.fields[f] else "" + fmt = docco.fields[f]['fmt'] if "fmt" in docco.fields[f] else "" + xml_field = etree.SubElement(xml_fields, 'field', name=f, units=units, type=fmt) if "description" in docco.fields[f]: xml_description2 = etree.SubElement(xml_field, 'description') xml_description2.text = docco.fields[f]["description"] + # Check for enum/bitfield + fieldnamething = None if "bitmaskenum" in docco.fields[f]: - enum_name = docco.fields[f]["bitmaskenum"] + fieldnamething = "bitmaskenum" + xmlenumtag = "bitmask" + xmlentrytag = "bit" + elif "valueenum" in docco.fields[f]: + fieldnamething = "valueenum" + xmlenumtag = "enum" + xmlentrytag = "element" + # If an enum/bitmask is defined, include this in the XML + if fieldnamething is not None: + enum_name = docco.fields[f][fieldnamething] if enum_name not in enumerations: raise Exception("Unknown enum (%s) (have %s)" % (enum_name, "\n".join(sorted(enumerations.keys())))) - bit_mask = enumerations[enum_name] - xml_bitmask = etree.SubElement(xml_field, 'bitmask') - for bit in bit_mask.entries: - xml_bitmask_bit = etree.SubElement(xml_bitmask, 'bit', name=bit.name) - xml_bitmask_bit_value = etree.SubElement(xml_bitmask_bit, 'value') - xml_bitmask_bit_value.text = str(bit.value) - if bit.comment is not None: - xml_bitmask_bit_comment = etree.SubElement(xml_bitmask_bit, 'description') - xml_bitmask_bit_comment.text = bit.comment + enum = enumerations[enum_name] + xml_enum = etree.SubElement(xml_field, xmlenumtag, name=enum_name) + for entry in enum.entries: + xml_enum_entry = etree.SubElement(xml_enum, xmlentrytag, name=entry.name) + xml_enum_entry_value = etree.SubElement(xml_enum_entry, 'value') + xml_enum_entry_value.text = str(entry.value) + if entry.comment is not None: + xml_enum_entry_comment = etree.SubElement(xml_enum_entry, 'description') + xml_enum_entry_comment.text = entry.comment if xml_fields.text is None and not len(xml_fields): xml_fields.text = '\n' # add on next line in case of empty element. self.stop() diff --git a/Tools/autotest/logger_metadata/enum_parse.py b/Tools/autotest/logger_metadata/enum_parse.py index 50c026a21651f3..99227a211e9766 100755 --- a/Tools/autotest/logger_metadata/enum_parse.py +++ b/Tools/autotest/logger_metadata/enum_parse.py @@ -18,6 +18,7 @@ class EnumDocco(object): "Copter": "ArduCopter", "Plane": "ArduPlane", "Tracker": "AntennaTracker", + "Blimp": "Blimp", } def __init__(self, vehicle): @@ -34,7 +35,7 @@ def match_enum_line(self, line): # attempts to extract name, value and comment from line. # Match: " FRED, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+)\s*,? *(?:// *(.*) *)?$", line) + m = re.match("\s*([A-Z0-9_a-z]+)\s*,? *(?://[/>]* *(.*) *)?$", line) if m is not None: return (m.group(1), None, m.group(2)) @@ -44,7 +45,7 @@ def match_enum_line(self, line): return (m.group(1), None, m.group(2)) # Match: " FRED = 17, // optional comment" - m = re.match("\s*([A-Z0-9_a-z]+)\s*=\s*([-0-9]+)\s*,?(?:\s*//\s*(.*) *)?$", + m = re.match("\s*([A-Z0-9_a-z]+)\s*=\s*([-0-9]+)\s*,?(?:\s*//[/<]*\s*(.*) *)?$", line) if m is not None: return (m.group(1), m.group(2), m.group(3)) @@ -91,6 +92,10 @@ def match_enum_line(self, line): raise ValueError("Failed to match (%s)" % line) def enumerations_from_file(self, source_file): + def debug(x): + pass + # if source_file == "/home/pbarker/rc/ardupilot/libraries/AP_HAL/AnalogIn.h": + # debug = print state_outside = "outside" state_inside = "inside" @@ -102,6 +107,7 @@ def enumerations_from_file(self, source_file): in_class = None while True: line = f.readline() + # debug(f"{state} line: {line}") if line == "": break line = line.rstrip() @@ -112,7 +118,7 @@ def enumerations_from_file(self, source_file): if re.match("class .*;", line) is not None: # forward-declaration of a class continue - m = re.match("class *(\w+)", line) + m = re.match("class *([:\w]+)", line) if m is not None: in_class = m.group(1) continue @@ -124,6 +130,7 @@ def enumerations_from_file(self, source_file): if m is not None: # all one one line! Thanks! enum_name = m.group(2) + debug("ol: %s: %s" % (source_file, enum_name)) entries_string = m.group(3) entry_names = [x.strip() for x in entries_string.split(",")] count = 0 @@ -138,7 +145,7 @@ def enumerations_from_file(self, source_file): m = re.match(".*enum\s*(class)? *([\w]+)\s*(?::.*_t)? *{", line) if m is not None: enum_name = m.group(2) - # print("%s: %s" % (source_file, enum_name)) + debug("%s: %s" % (source_file, enum_name)) entries = [] last_value = None state = state_inside @@ -171,7 +178,7 @@ def enumerations_from_file(self, source_file): if name is None: skip_enumeration = True continue - # print(" name=(%s) value=(%s) comment=(%s)\n" % (name, value, comment)) + debug(" name=(%s) value=(%s) comment=(%s)\n" % (name, value, comment)) if value is None: if last_value is None: value = 0 @@ -189,6 +196,7 @@ def enumerations_from_file(self, source_file): class Enumeration(object): def __init__(self, name, entries): + # print("creating enum %s" % name) self.name = name self.entries = entries @@ -245,4 +253,4 @@ def run(self): sys.exit(1) s.run() - print("Enumerations: %s" % s.enumerations) +# print("Enumerations: %s" % s.enumerations) diff --git a/Tools/autotest/logger_metadata/parse.py b/Tools/autotest/logger_metadata/parse.py index ea58198ea2bcc8..2f7517d2182f46 100755 --- a/Tools/autotest/logger_metadata/parse.py +++ b/Tools/autotest/logger_metadata/parse.py @@ -19,6 +19,7 @@ topdir = os.path.join(os.path.dirname(os.path.realpath(__file__)), '../../../') topdir = os.path.realpath(topdir) +# Regular expressions for finding message information in code comments re_loggermessage = re.compile(r"@LoggerMessage\s*:\s*([\w,]+)", re.MULTILINE) re_commentline = re.compile(r"\s*//") re_description = re.compile(r"\s*//\s*@Description\s*:\s*(.*)") @@ -29,9 +30,39 @@ re_fieldvalueenum = re.compile(r"\s*//\s*@FieldValueEnum\s*:\s*(\w+):\s*(.*)") re_vehicles = re.compile(r"\s*//\s*@Vehicles\s*:\s*(.*)") +# Regular expressions for finding message definitions in structure format +re_start_messagedef = re.compile(r"^\s*{?\s*LOG_[A-Z0-9_]+_[MSGTA]+[A-Z0-9_]*\s*,") +re_deffield = r'[\s\\]*"?([\w\-#?%]+)"?\s*' +re_full_messagedef = re.compile(r'\s*LOG_\w+\s*,\s*\w+\([^)]+\)[\s\\]*,' + f'{re_deffield},{re_deffield},' + r'[\s\\]*"?([\w,]+)"?[\s\\]*,' + f'{re_deffield},{re_deffield}' , re.MULTILINE) +re_fmt_define = re.compile(r'#define\s+(\w+_FMT)\s+"([\w\-#?%]+)"') +re_units_define = re.compile(r'#define\s+(\w+_UNITS)\s+"([\w\-#?%]+)"') +re_mults_define = re.compile(r'#define\s+(\w+_MULTS)\s+"([\w\-#?%]+)"') + +# Regular expressions for finding message definitions in Write calls +re_start_writecall = re.compile(r"\s*[AP:]*logger[\(\)]*.Write[StreamingCrcl]*\(") +re_writefield = r'\s*"([\w\-#?%,]+)"\s*' +re_full_writecall = re.compile(r'\s*[AP:]*logger[\(\)]*.Write[StreamingCrcl]*\(' + f'{re_writefield},{re_writefield},{re_writefield},({re_writefield},{re_writefield})?' , re.MULTILINE) + +# Regular expression for extracting unit and multipliers from structure +re_units_mults_struct = re.compile(r"^\s*{\s*'([\w\-#?%!/])',"+r'\s*"?([\w\-#?%./]*)"?\s*}') + # TODO: validate URLS actually return 200 -# TODO: augment with other information from log definitions; type and units... +# Lookup tables are populated by reading LogStructure.h +log_fmt_lookup = {} +log_units_lookup = {} +log_mult_lookup = {} + +# Lookup table to convert multiplier to prefix +mult_prefix_lookup = { + 0: "", + 1: "", + 1e-1: "d", # deci- + 1e-2: "c", # centi- + 1e-3: "m", # milli- + 1e-6: "μ", # micro- + 1e-9: "n" # nano- +} class LoggerDocco(object): @@ -53,20 +84,48 @@ def __init__(self, vehicle): emit_xml.XMLEmitter(), emit_md.MDEmitter(), ] + self.msg_fmts_list = {} + self.msg_units_list = {} + self.msg_mults_list = {} class Docco(object): def __init__(self, name): self.name = name self.url = None - self.description = None + if isinstance(name,list): + self.description = [None] * len(name) + else: + self.description = None self.fields = {} self.fields_order = [] self.vehicles = None self.bits_enums = [] + def add_name(self, name): + # If self.name/description aren't lists, convert them + if isinstance(self.name,str): + self.name = [self.name] + self.description = [self.description] + # Replace any existing empty descriptions with empty strings + for i in range(0,len(self.description)): + if self.description[i] is None: + self.description[i] = "" + # Extend the name and description lists + if isinstance(name,list): + self.name.extend(name) + self.description.extend([None] * len(name)) + else: + self.name.append(name) + self.description.append(None) + def set_description(self, desc): - self.description = desc + if isinstance(self.description,list): + for i in range(0,len(self.description)): + if self.description[i] is None: + self.description[i] = desc + else: + self.description = desc def set_url(self, url): self.url = url @@ -99,13 +158,107 @@ def set_fieldbitmaskenum(self, field, bits): self.ensure_field(field) self.fields[field]["bitmaskenum"] = bits - def set_fieldbitmaskvalue(self, field, bits): + def set_fieldvalueenum(self, field, bits): self.ensure_field(field) self.fields[field]["valueenum"] = bits def set_vehicles(self, vehicles): self.vehicles = vehicles + def set_fmts(self, fmts): + # If no fields are defined, do nothing + if len(self.fields_order)==0: + return + # Make sure lengths match up + if len(fmts) != len(self.fields_order): + print(f"Number of fmts don't match fields: msg={self.name} fmts={fmts} num_fields={len(self.fields_order)}") + return + # Loop through the list + for idx in range(0,len(fmts)): + if fmts[idx] in log_fmt_lookup: + self.fields[self.fields_order[idx]]["fmt"] = log_fmt_lookup[fmts[idx]] + else: + print(f"Unrecognised format character: {fmts[idx]} in message {self.name}") + + def set_units(self, units, mults): + # If no fields are defined, do nothing + if len(self.fields_order)==0: + return + # Make sure lengths match up + if len(units) != len(self.fields_order) or len(units) != len(mults): + print(f"Number of units/mults/fields don't match: msg={self.name} units={units} mults={mults} num_fields={len(self.fields_order)}") + return + # Loop through the list + for idx in range(0,len(units)): + # Get the index into fields from field_order + f = self.fields_order[idx] + # Convert unit char to base unit + if units[idx] in log_units_lookup: + baseunit = log_units_lookup[units[idx]] + else: + print(f"Unrecognised units character: {units[idx]} in message {self.name}") + continue + # Do nothing if this field has no unit defined + if baseunit == "": + continue + # Convert mult char to value + if mults[idx] in log_mult_lookup: + mult = log_mult_lookup[mults[idx]] + mult_num = float(mult) + else: + print(f"Unrecognised multiplier character: {mults[idx]} in message {self.name}") + continue + # Check if the defined format for this field contains its own multiplier + # If so, the presented value will be the base-unit directly + if 'fmt' in self.fields[f] and self.fields[f]['fmt'].endswith("* 100"): + self.fields[f]["units"] = baseunit + elif 'fmt' in self.fields[f] and "latitude/longitude" in self.fields[f]['fmt']: + self.fields[f]["units"] = baseunit + # Check if we have a defined prefix for this multiplier + elif mult_num in mult_prefix_lookup: + self.fields[f]["units"] = f"{mult_prefix_lookup[mult_num]}{baseunit}" + # If all else fails, set the unit as the multipler and base unit together + else: + self.fields[f]["units"] = f"{mult} {baseunit}" + + def populate_lookups(self): + # Initialise the lookup tables + # Read the contents of the LogStructure.h file + structfile = os.path.join(topdir, "libraries", "AP_Logger", "LogStructure.h") + with open(structfile) as f: + lines = f.readlines() + f.close() + # Initialise current section to none + section = "none" + # Loop through the lines in the file + for line in lines: + # Look for the start of fmt/unit/mult info + if line.startswith("Format characters"): + section = "fmt" + elif line.startswith("const struct UnitStructure"): + section = "units" + elif line.startswith("const struct MultiplierStructure"): + section = "mult" + # Read formats from code comment, e.g.: + # b : int8_t + elif section == "fmt": + if "*/" in line: + section = "none" + else: + parts = line.split(":") + log_fmt_lookup[parts[0].strip()] = parts[1].strip() + # Read units or multipliers from C struct definition, e.g.: + # { '2', 1e2 }, or { 'J', "W.s" }, + elif section != "none": + if "};" in line: + section = "none" + else: + u = re_units_mults_struct.search(line) + if u is not None and section == "units": + log_units_lookup[u.group(1)] = u.group(2) + if u is not None and section == "mult": + log_mult_lookup[u.group(1)] = u.group(2) + def search_for_files(self, dirs_to_search): _next = [] for _dir in dirs_to_search: @@ -122,17 +275,91 @@ def search_for_files(self, dirs_to_search): if len(_next): self.search_for_files(_next) + def parse_messagedef(self,messagedef): + # Merge concatinated strings and remove comments + messagedef = re.sub(r'"\s+"', '', messagedef) + messagedef = re.sub(r'//[^\n]*', '', messagedef) + # Extract details from a structure definition + d = re_full_messagedef.search(messagedef) + if d is not None: + self.msg_fmts_list[d.group(1)] = d.group(2) + self.msg_units_list[d.group(1)] = d.group(4) + self.msg_mults_list[d.group(1)] = d.group(5) + return + # Extract details from a WriteStreaming call + d = re_full_writecall.search(messagedef) + if d is not None: + if d.group(1) in self.msg_fmts_list: + return + if d.group(5) is None: + self.msg_fmts_list[d.group(1)] = d.group(3) + else: + self.msg_fmts_list[d.group(1)] = d.group(6) + self.msg_units_list[d.group(1)] = d.group(3) + self.msg_mults_list[d.group(1)] = d.group(5) + return + # Didn't parse + #print(f"Unable to parse: {messagedef}") + + def search_messagedef_start(self,line,prevmessagedef=""): + # Look for the start of a structure definition + d = re_start_messagedef.search(line) + if d is not None: + messagedef = line + if "}" in line: + self.parse_messagedef(messagedef) + return "" + else: + return messagedef + # Look for a new call to WriteStreaming + d = re_start_writecall.search(line) + if d is not None: + messagedef = line + if ";" in line: + self.parse_messagedef(messagedef) + return "" + else: + return messagedef + # If we didn't find a new one, continue with any previous state + return prevmessagedef + def parse_file(self, filepath): with open(filepath) as f: - # print("Opened (%s)" % filepath) +# print("Opened (%s)" % filepath) lines = f.readlines() f.close() - state_outside = "outside" - state_inside = "inside" - state = state_outside - docco = None + def debug(x): + pass +# if filepath == "/home/pbarker/rc/ardupilot/libraries/AP_HAL/AnalogIn.h": +# debug = print + state_outside = "outside" + state_inside = "inside" + messagedef = "" + state = state_outside + docco = None for line in lines: + debug(f"{state}: {line}") + if messagedef: + messagedef = messagedef + line + if "}" in line or ";" in line: + self.parse_messagedef(messagedef) + messagedef = "" if state == state_outside: + # Check for start of a message definition + messagedef = self.search_messagedef_start(line,messagedef) + + # Check for fmt/unit/mult #define + u = re_fmt_define.search(line) + if u is not None: + self.msg_fmts_list[u.group(1)] = u.group(2) + u = re_units_define.search(line) + if u is not None: + self.msg_units_list[u.group(1)] = u.group(2) + u = re_mults_define.search(line) + if u is not None: + self.msg_mults_list[u.group(1)] = u.group(2) + + # Check for the @LoggerMessage tag indicating the start of the docco block m = re_loggermessage.search(line) if m is None: continue @@ -142,11 +369,22 @@ def parse_file(self, filepath): state = state_inside docco = LoggerDocco.Docco(name) elif state == state_inside: + # If this line is not a comment, then this is the end of the docco block if not re_commentline.match(line): state = state_outside if docco.vehicles is None or self.vehicle in docco.vehicles: self.finalise_docco(docco) + messagedef = self.search_messagedef_start(line) continue + # Check for an multiple @LoggerMessage lines in this docco block + m = re_loggermessage.search(line) + if m is not None: + name = m.group(1) + if "," in name: + name = name.split(",") + docco.add_name(name) + continue + # Find and extract data from the various docco fields m = re_description.match(line) if m is not None: docco.set_description(m.group(1)) @@ -169,7 +407,7 @@ def parse_file(self, filepath): continue m = re_fieldvalueenum.match(line) if m is not None: - docco.set_fieldbitmaskvalue(m.group(1), m.group(2)) + docco.set_fieldvalueenum(m.group(1), m.group(2)) continue m = re_vehicles.match(line) if m is not None: @@ -187,14 +425,48 @@ def emit_output(self): new_doccos = [] for docco in self.doccos: if isinstance(docco.name, list): - for name in docco.name: + for name,desc in zip(docco.name, docco.description): tmpdocco = copy.copy(docco) tmpdocco.name = name + tmpdocco.description = desc new_doccos.append(tmpdocco) else: new_doccos.append(docco) new_doccos = sorted(new_doccos, key=lambda x : x.name) + # Try to attach the formats/units/multipliers + for docco in new_doccos: + # Apply the Formats to the docco + if docco.name in self.msg_fmts_list: + if "FMT" in self.msg_fmts_list[docco.name]: + if self.msg_fmts_list[docco.name] in self.msg_fmts_list: + docco.set_fmts(self.msg_fmts_list[self.msg_fmts_list[docco.name]]) + else: + docco.set_fmts(self.msg_fmts_list[docco.name]) + else: + print(f"No formats found for message {docco.name}") + # Get the Units + units = None + if docco.name in self.msg_units_list: + if "UNITS" in self.msg_units_list[docco.name]: + if self.msg_units_list[docco.name] in self.msg_units_list: + units = self.msg_units_list[self.msg_units_list[docco.name]] + else: + units = self.msg_units_list[docco.name] + # Get the Multipliers + mults = None + if docco.name in self.msg_mults_list: + if "MULTS" in self.msg_mults_list[docco.name]: + if self.msg_mults_list[docco.name] in self.msg_mults_list: + mults = self.msg_mults_list[self.msg_mults_list[docco.name]] + else: + mults = self.msg_mults_list[docco.name] + # Apply the units/mults to the docco + if units is not None and mults is not None: + docco.set_units(units,mults) + elif units is not None or mults is not None: + print(f"Cannot find matching units/mults for message {docco.name}") + enums_by_name = {} for enum in self.enumerations: enums_by_name[enum.name] = enum @@ -202,6 +474,7 @@ def emit_output(self): emitter.emit(new_doccos, enums_by_name) def run(self): + self.populate_lookups() self.enumerations = enum_parse.EnumDocco(self.vehicle).get_enumerations() self.files = [] self.search_for_files([self.vehicle_map[self.vehicle], "libraries"]) diff --git a/Tools/autotest/models/plane-3d.parm b/Tools/autotest/models/plane-3d.parm index c2d467b347252a..afb92f7c2da3b5 100644 --- a/Tools/autotest/models/plane-3d.parm +++ b/Tools/autotest/models/plane-3d.parm @@ -28,9 +28,9 @@ ACRO_LOCKING 2 AHRS_EKF_TYPE 10 TRIM_THROTTLE 30 NAVL1_PERIOD 7 -TRIM_ARSPD_CM 2400 -ARSPD_FBW_MAX 30 -LIM_ROLL_CD 7000 +AIRSPEED_CRUISE 24.00 +AIRSPEED_MAX 30 +ROLL_LIMIT_DEG 70.00 # setup for scripting SCR_ENABLE 1 SCR_HEAP_SIZE 350000 diff --git a/Tools/autotest/models/plane.parm b/Tools/autotest/models/plane.parm index 0650cecf61b1ab..5fb0dbea02d4c6 100644 --- a/Tools/autotest/models/plane.parm +++ b/Tools/autotest/models/plane.parm @@ -1,17 +1,17 @@ EK2_ENABLE 1 BATT_MONITOR 4 LOG_BITMASK 65535 -TRIM_ARSPD_CM 2200 -TRIM_PITCH_CD 0 +AIRSPEED_CRUISE 22.00 +PTCH_TRIM_DEG 0.00 TRIM_THROTTLE 50 -LIM_PITCH_MIN -2000 -LIM_PITCH_MAX 2500 -LIM_ROLL_CD 6500 -LAND_PITCH_CD 100 +PTCH_LIM_MIN_DEG -20.00 +PTCH_LIM_MAX_DEG 25.00 +ROLL_LIMIT_DEG 65.00 +LAND_PITCH_DEG 1.00 LAND_FLARE_SEC 3 ARSPD_USE 1 -ARSPD_FBW_MAX 30 -ARSPD_FBW_MIN 10 +AIRSPEED_MAX 30 +AIRSPEED_MIN 10 KFF_RDDRMIX 0.5 THR_MAX 100 RC1_MAX 2000 diff --git a/Tools/autotest/param_metadata/htmlemit.py b/Tools/autotest/param_metadata/htmlemit.py index 1d9e76534168a7..201a13ef9a85b6 100644 --- a/Tools/autotest/param_metadata/htmlemit.py +++ b/Tools/autotest/param_metadata/htmlemit.py @@ -65,7 +65,7 @@ def emit(self, g): t += "
    \n" for field in param.__dict__.keys(): - if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields: + if field not in ['name', 'DisplayName', 'Description', 'User', 'SortValues'] and field in known_param_fields: if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]): values = (param.__dict__[field]).split(',') t += "\n" diff --git a/Tools/autotest/param_metadata/jsonemit.py b/Tools/autotest/param_metadata/jsonemit.py index 9d50c908db6ee8..8352b684951cb7 100644 --- a/Tools/autotest/param_metadata/jsonemit.py +++ b/Tools/autotest/param_metadata/jsonemit.py @@ -55,9 +55,12 @@ def emit(self, g): if ':' in name: name = name.split(':')[1] - # Remove real_path key - if 'real_path' in param.__dict__: - param.__dict__.pop('real_path') + # Remove various unwanted keys + for key in 'real_path', 'SortValues', '__field_text': + try: + param.__dict__.pop(key) + except KeyError: + pass # Get range section if available range_json = {} diff --git a/Tools/autotest/param_metadata/mdemit.py b/Tools/autotest/param_metadata/mdemit.py index d97ed1f3e69dcb..e6edba803bd8c0 100644 --- a/Tools/autotest/param_metadata/mdemit.py +++ b/Tools/autotest/param_metadata/mdemit.py @@ -90,7 +90,7 @@ def emit(self, g): t += "\n\n%s" % param.Description for field in param.__dict__.keys(): - if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields: + if field not in ['name', 'DisplayName', 'Description', 'User', 'SortValues'] and field in known_param_fields: if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]): values = (param.__dict__[field]).split(',') t += "\n\n|Value|Meaning|" diff --git a/Tools/autotest/param_metadata/param.py b/Tools/autotest/param_metadata/param.py index 3cc5266a47ecc4..818a0f9b97f971 100644 --- a/Tools/autotest/param_metadata/param.py +++ b/Tools/autotest/param_metadata/param.py @@ -51,6 +51,7 @@ def has_param(self, pname): 'ReadOnly', 'Calibration', 'Vector3Parameter', + 'SortValues' ] # Follow SI units conventions from: diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index fdb1465e29cc4d..60466c75ec9234 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -22,7 +22,6 @@ from xmlemit import XmlEmit from mdemit import MDEmit from jsonemit import JSONEmit -from xmlemit_mp import XmlEmitMP parser = ArgumentParser(description="Parse ArduPilot parameters.") parser.add_argument("-v", "--verbose", dest='verbose', action='store_true', default=False, help="show debugging output") @@ -36,7 +35,7 @@ dest='output_format', action='store', default='all', - choices=['all', 'html', 'rst', 'rstlatexpdf', 'wiki', 'xml', 'json', 'edn', 'md', 'xml_mp'], + choices=['all', 'html', 'rst', 'rstlatexpdf', 'wiki', 'xml', 'json', 'edn', 'md'], help="what output format to use") args = parser.parse_args() @@ -66,7 +65,7 @@ def find_vehicle_parameter_filepath(vehicle_name): "Sub": "ArduSub", } - # first try ArduCopter/Parmameters.cpp + # first try ArduCopter/Parameters.cpp for top_dir in apm_path, apm_tools_path: path = os.path.join(top_dir, vehicle_name, "Parameters.cpp") if os.path.exists(path): @@ -631,7 +630,6 @@ def validate(param, is_library=False): 'rst': RSTEmit, 'rstlatexpdf': RSTLATEXPDFEmit, 'md': MDEmit, - 'xml_mp': XmlEmitMP, } try: diff --git a/Tools/autotest/param_metadata/rstemit.py b/Tools/autotest/param_metadata/rstemit.py index d81f8d6b237434..4fa423b69089b3 100755 --- a/Tools/autotest/param_metadata/rstemit.py +++ b/Tools/autotest/param_metadata/rstemit.py @@ -263,7 +263,7 @@ def emit(self, g): headings = [] row = [] for field in sorted(param.__dict__.keys()): - if (field not in ['name', 'DisplayName', 'Description', 'User', 'RebootRequired'] and + if (field not in ['name', 'DisplayName', 'Description', 'User', 'SortValues', 'RebootRequired'] and field in known_param_fields): headings.append(field) if field in field_table_info and Emit.prog_values_field.match(param.__dict__[field]): diff --git a/Tools/autotest/param_metadata/xmlemit.py b/Tools/autotest/param_metadata/xmlemit.py index c9262590af094f..7d19234baa5021 100644 --- a/Tools/autotest/param_metadata/xmlemit.py +++ b/Tools/autotest/param_metadata/xmlemit.py @@ -2,6 +2,7 @@ from emit import Emit from param import known_param_fields, known_units +import re # Emit ArduPilot documentation in an machine readable XML format class XmlEmit(Emit): @@ -30,6 +31,18 @@ def emit_comment(self, s): def start_libraries(self): self.current_element = self.libraries + def sorted_Values_keys(self, nv_pairs, zero_at_top=False): + '''sorts name/value pairs derived from items in @Values. Sorts by + value, with special attention paid to common "Do nothing" values''' + keys = nv_pairs.keys() + def sort_key(value): + description = nv_pairs[value] + if zero_at_top and value == "0": + # make sure this item goes at the top of the list: + return "AAAAAAA" + return description + return sorted(keys, key=sort_key) + def emit(self, g): xml_parameters = etree.SubElement(self.current_element, 'parameters', name=g.reference) # i.e. ArduPlane @@ -50,17 +63,36 @@ def emit(self, g): # Add values as chidren of this node for field in param.__dict__.keys(): - if field not in ['name', 'DisplayName', 'Description', 'User'] and field in known_param_fields: + if field not in ['name', 'DisplayName', 'Description', 'User', 'SortValues'] and field in known_param_fields: if field == 'Values' and Emit.prog_values_field.match(param.__dict__[field]): xml_values = etree.SubElement(xml_param, 'values') values = (param.__dict__[field]).split(',') + nv_unsorted = {} for value in values: v = value.split(':') if len(v) != 2: raise ValueError("Bad value (%s)" % v) # i.e. numeric value, string label - xml_value = etree.SubElement(xml_values, 'value', code=v[0]) - xml_value.text = v[1] + if v[0] in nv_unsorted: + raise ValueError("%s already exists" % v[0]) + nv_unsorted[v[0]] = v[1] + + all_keys = nv_unsorted.keys() + if hasattr(param, 'SortValues'): + sort = getattr(param, 'SortValues').lower() + zero_at_top = False + if sort == 'alphabeticalzeroattop': + zero_at_top = True + else: + raise ValueError("Unknown sort (%s)" % sort) + + all_keys = self.sorted_Values_keys(nv_unsorted, zero_at_top=zero_at_top) + + for key in all_keys: + value = nv_unsorted[key] + + xml_value = etree.SubElement(xml_values, 'value', code=key) + xml_value.text = value elif field == 'Units': abreviated_units = param.__dict__[field] diff --git a/Tools/autotest/param_metadata/xmlemit_mp.py b/Tools/autotest/param_metadata/xmlemit_mp.py deleted file mode 100644 index 846993ee59e67f..00000000000000 --- a/Tools/autotest/param_metadata/xmlemit_mp.py +++ /dev/null @@ -1,91 +0,0 @@ -from xml.sax.saxutils import escape, quoteattr - -from emit import Emit -from param import known_param_fields, known_units -from lxml import etree - -# Emit ArduPilot documentation in an machine readable XML format for Mission Planner -class XmlEmitMP(Emit): - def __init__(self, *args, **kwargs): - Emit.__init__(self, *args, **kwargs) - self.mp_fname = 'ParameterMetaData.xml' - self.f = open(self.mp_fname, mode='w') - self.preamble = '''\n''' - self.f.write(self.preamble) - self.f.write('\n') - self.gname = None - self.skip_name = False - - def close(self): - self.f.write(' \n' % self.gname) - self.f.write('''\n''') - self.f.close() - # sort and reformat XML - parser = etree.XMLParser(remove_blank_text=True) - tree = etree.parse(self.mp_fname, parser) - root = tree.getroot() - vehicle = tree.find(self.gname) - sort_xml_node(vehicle) - sorted_unicode = etree.tostring(root, pretty_print=True, encoding='unicode') - f = open(self.mp_fname, mode='w') - f.write(self.preamble) - f.write(sorted_unicode) - f.close() - - def emit_comment(self, s): - self.f.write("") - - def start_libraries(self): - self.skip_name = True - - def emit(self, g): - t = "" - if not self.skip_name: - self.gname = g.reference - if self.gname == "ArduCopter": - self.gname = "ArduCopter2" - if self.gname == "APMrover2" or self.gname == "Rover": - self.gname = "ArduRover" - t = ' <%s>\n' % self.gname - - for param in g.params: - # Begin our parameter node - # Get param name and and remove key - name = param.name.split(':')[-1] - - t += ' <%s>\n' % name - - if hasattr(param, 'DisplayName'): - t += ' %s\n' % param.DisplayName - - if hasattr(param, 'Description'): - t += ' %s\n' % escape(param.Description) # i.e. parameter docs - if hasattr(param, 'User'): - t += ' %s\n' % param.User # i.e. Standard or Advanced - - # not used yet - # if hasattr(param, 'Calibration'): - # t += ' %s. + +""" +Run a single-waypoint mission on Plane. + +Warning - This is NOT production code; it's a simple demo of capability. +""" + +import math +import rclpy +import time +import errno + +from rclpy.node import Node +from builtin_interfaces.msg import Time +from ardupilot_msgs.msg import GlobalPosition +from geographic_msgs.msg import GeoPoseStamped +from geopy import distance +from geopy import point +from ardupilot_msgs.srv import ArmMotors +from ardupilot_msgs.srv import ModeSwitch + + +PLANE_MODE_TAKEOFF = 13 +PLANE_MODE_GUIDED = 15 + +FRAME_GLOBAL_INT = 5 + +# Hard code some known locations +# Note - Altitude in geopy is in km! +GRAYHOUND_TRACK = point.Point(latitude=-35.345996, longitude=149.159017, altitude=0.575) +CMAC = point.Point(latitude=-35.3627010, longitude=149.1651513, altitude=0.585) + + +class PlaneWaypointFollower(Node): + """Plane follow waypoints using guided control.""" + + def __init__(self): + """Initialise the node.""" + super().__init__("plane_waypoint_follower") + + self.declare_parameter("arm_topic", "/ap/arm_motors") + self._arm_topic = self.get_parameter("arm_topic").get_parameter_value().string_value + self._client_arm = self.create_client(ArmMotors, self._arm_topic) + while not self._client_arm.wait_for_service(timeout_sec=1.0): + self.get_logger().info('arm service not available, waiting again...') + + self.declare_parameter("mode_topic", "/ap/mode_switch") + self._mode_topic = self.get_parameter("mode_topic").get_parameter_value().string_value + self._client_mode_switch = self.create_client(ModeSwitch, self._mode_topic) + while not self._client_mode_switch.wait_for_service(timeout_sec=1.0): + self.get_logger().info('mode switch service not available, waiting again...') + + self.declare_parameter("global_position_topic", "/ap/cmd_gps_pose") + self._global_pos_topic = self.get_parameter("global_position_topic").get_parameter_value().string_value + self._global_pos_pub = self.create_publisher(GlobalPosition, self._global_pos_topic, 1) + + # Create subscriptions after services are up + self.declare_parameter("geopose_topic", "/ap/geopose/filtered") + self._geopose_topic = self.get_parameter("geopose_topic").get_parameter_value().string_value + qos = rclpy.qos.QoSProfile( + reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, durability=rclpy.qos.DurabilityPolicy.VOLATILE, depth=1 + ) + + self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos) + self._cur_geopose = GeoPoseStamped() + + def geopose_cb(self, msg: GeoPoseStamped): + """Process a GeoPose message.""" + stamp = msg.header.stamp + if stamp.sec: + self.get_logger().info("From AP : Geopose [sec:{}, nsec: {}]".format(stamp.sec, stamp.nanosec)) + + # Store current state + self._cur_geopose = msg + + def arm(self): + req = ArmMotors.Request() + req.arm = True + future = self._client_arm.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result() + + def arm_with_timeout(self, timeout: rclpy.duration.Duration): + """Try to arm. Returns true on success, or false if arming fails or times out.""" + armed = False + start = self.get_clock().now() + while not armed and self.get_clock().now() - start < timeout: + armed = self.arm().result + time.sleep(1) + return armed + + def switch_mode(self, mode): + req = ModeSwitch.Request() + assert mode in [PLANE_MODE_TAKEOFF, PLANE_MODE_GUIDED] + req.mode = mode + future = self._client_mode_switch.call_async(req) + rclpy.spin_until_future_complete(self, future) + return future.result() + + def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration): + """Try to switch mode. Returns true on success, or false if mode switch fails or times out.""" + is_in_desired_mode = False + start = self.get_clock().now() + while not is_in_desired_mode: + result = self.switch_mode(desired_mode) + # Handle successful switch or the case that the vehicle is already in expected mode + is_in_desired_mode = result.status or result.curr_mode == desired_mode + time.sleep(1) + + return is_in_desired_mode + + def get_cur_geopose(self): + """Return latest geopose.""" + return self._cur_geopose + + def send_goal_position(self, goal_global_pos): + """Send goal position. Must be in guided for this to work.""" + self._global_pos_pub.publish(goal_global_pos) + + +def achieved_goal(goal_global_pos, cur_geopose): + """Return true if the current position (LLH) is close enough to the goal (within the orbit radius).""" + # Use 3D geopy distance calculation + # https://geopy.readthedocs.io/en/stable/#module-geopy.distance + goal_lat = goal_global_pos + + p1 = (goal_global_pos.latitude, goal_global_pos.longitude, goal_global_pos.altitude) + cur_pos = cur_geopose.pose.position + p2 = (cur_pos.latitude, cur_pos.longitude, cur_pos.altitude) + + flat_distance = distance.distance(p1[:2], p2[:2]).m + euclidian_distance = math.sqrt(flat_distance**2 + (p2[2] - p1[2]) ** 2) + print(f"Goal is {euclidian_distance} meters away") + return euclidian_distance < 150 + + +def main(args=None): + """Node entry point.""" + rclpy.init(args=args) + node = PlaneWaypointFollower() + try: + # Block till armed, which will wait for EKF3 to initialize + if not node.arm_with_timeout(rclpy.duration.Duration(seconds=30)): + raise RuntimeError("Unable to arm") + + # Block till in takeoff + if not node.switch_mode_with_timeout(PLANE_MODE_TAKEOFF, rclpy.duration.Duration(seconds=20)): + raise RuntimeError("Unable to switch to takeoff mode") + + is_ascending_to_takeoff_alt = True + while is_ascending_to_takeoff_alt: + rclpy.spin_once(node) + time.sleep(1.0) + + # Hard code waiting in takeoff to reach operating altitude of 630m + # This is just a hack because geopose is reported with absolute rather than relative altitude, + # and this node doesn't have access to the terrain data + is_ascending_to_takeoff_alt = node.get_cur_geopose().pose.position.altitude < CMAC.altitude * 1000 + 45 + + if is_ascending_to_takeoff_alt: + raise RuntimeError("Failed to reach takeoff altitude") + + if not node.switch_mode_with_timeout(PLANE_MODE_GUIDED, rclpy.duration.Duration(seconds=20)): + raise RuntimeError("Unable to switch to guided mode") + + # Send a guided WP with location, frame ID, alt frame + goal_pos = GlobalPosition() + goal_pos.latitude = GRAYHOUND_TRACK.latitude + goal_pos.longitude = GRAYHOUND_TRACK.longitude + DESIRED_AGL = 60 + goal_pos.altitude = GRAYHOUND_TRACK.altitude * 1000 + DESIRED_AGL + goal_pos.coordinate_frame = FRAME_GLOBAL_INT + goal_pos.header.frame_id = "map" + + node.send_goal_position(goal_pos) + + start = node.get_clock().now() + has_achieved_goal = False + while not has_achieved_goal and node.get_clock().now() - start < rclpy.duration.Duration(seconds=120): + rclpy.spin_once(node) + has_achieved_goal = achieved_goal(goal_pos, node.get_cur_geopose()) + time.sleep(1.0) + + if not has_achieved_goal: + raise RuntimeError("Unable to achieve goal location") + + print("Goal achieved") + + except KeyboardInterrupt: + pass + + # Destroy the node explicitly. + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/Tools/ros2/ardupilot_dds_tests/package.xml b/Tools/ros2/ardupilot_dds_tests/package.xml index 7b44fbffdcbf94..b4cc9fa2a0e358 100644 --- a/Tools/ros2/ardupilot_dds_tests/package.xml +++ b/Tools/ros2/ardupilot_dds_tests/package.xml @@ -8,11 +8,13 @@ GLP-3.0 ament_index_python + ardupilot_msgs ardupilot_sitl launch launch_pytest launch_ros micro_ros_msgs + python3-geopy python3-pytest rclpy socat @@ -23,6 +25,7 @@ ament_uncrustify ament_xmllint ament_lint_auto + ardupilot_msgs ardupilot_sitl launch launch_pytest diff --git a/Tools/ros2/ardupilot_dds_tests/setup.py b/Tools/ros2/ardupilot_dds_tests/setup.py index 3d2039beb5c0fd..bbe11e6a5c8123 100644 --- a/Tools/ros2/ardupilot_dds_tests/setup.py +++ b/Tools/ros2/ardupilot_dds_tests/setup.py @@ -25,6 +25,7 @@ entry_points={ 'console_scripts': [ "time_listener = ardupilot_dds_tests.time_listener:main", + "plane_waypoint_follower = ardupilot_dds_tests.plane_waypoint_follower:main", ], }, ) diff --git a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/conftest.py b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/conftest.py index 66a71779fa28ff..f1a1a4532ea243 100644 --- a/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/conftest.py +++ b/Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/conftest.py @@ -145,7 +145,7 @@ def sitl_copter_dds_serial(device_dir, virtual_ports, micro_ros_agent_serial, ma "speedup": "10", "slave": "0", "instance": "0", - "uartC": f"uart:{str(tty1)}", + "serial1": f"uart:{str(tty1)}", "defaults": str( Path( get_package_share_directory("ardupilot_sitl"), diff --git a/Tools/ros2/ardupilot_msgs/CMakeLists.txt b/Tools/ros2/ardupilot_msgs/CMakeLists.txt index 8096431370687c..d066a2d32b2e43 100644 --- a/Tools/ros2/ardupilot_msgs/CMakeLists.txt +++ b/Tools/ros2/ardupilot_msgs/CMakeLists.txt @@ -5,14 +5,18 @@ project(ardupilot_msgs) # Find dependencies. find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) # --------------------------------------------------------------------------- # # Generate and export message interfaces. rosidl_generate_interfaces(${PROJECT_NAME} + "msg/GlobalPosition.msg" "srv/ArmMotors.srv" "srv/ModeSwitch.srv" + DEPENDENCIES geometry_msgs std_msgs ADD_LINTER_TESTS ) diff --git a/Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg b/Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg new file mode 100644 index 00000000000000..f34b84c3ea34e8 --- /dev/null +++ b/Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg @@ -0,0 +1,30 @@ +# Experimental REP-147 Goal Interface +# https://ros.org/reps/rep-0147.html#goal-interface + +std_msgs/Header header + +uint8 coordinate_frame +uint8 FRAME_GLOBAL_INT = 5 +uint8 FRAME_GLOBAL_REL_ALT = 6 +uint8 FRAME_GLOBAL_TERRAIN_ALT = 11 + +uint16 type_mask +uint16 IGNORE_LATITUDE = 1 # Position ignore flags +uint16 IGNORE_LONGITUDE = 2 +uint16 IGNORE_ALTITUDE = 4 +uint16 IGNORE_VX = 8 # Velocity vector ignore flags +uint16 IGNORE_VY = 16 +uint16 IGNORE_VZ = 32 +uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags +uint16 IGNORE_AFY = 128 +uint16 IGNORE_AFZ = 256 +uint16 FORCE = 512 # Force in af vector flag +uint16 IGNORE_YAW = 1024 +uint16 IGNORE_YAW_RATE = 2048 + +float64 latitude +float64 longitude +float32 altitude # in meters, AMSL or above terrain +geometry_msgs/Twist velocity +geometry_msgs/Twist acceleration_or_force +float32 yaw diff --git a/Tools/ros2/ardupilot_msgs/package.xml b/Tools/ros2/ardupilot_msgs/package.xml index 08e92bf1df7d99..2e2a118fa5ae2c 100644 --- a/Tools/ros2/ardupilot_msgs/package.xml +++ b/Tools/ros2/ardupilot_msgs/package.xml @@ -11,6 +11,9 @@ ament_cmake rosidl_default_generators + std_msgs + geometry_msgs + rosidl_default_runtime ament_cmake_copyright diff --git a/Tools/ros2/ardupilot_sitl/config/copter_quad.yaml b/Tools/ros2/ardupilot_sitl/config/copter_quad.yaml index 02ad121f3a9c95..9fa079957a9f8a 100644 --- a/Tools/ros2/ardupilot_sitl/config/copter_quad.yaml +++ b/Tools/ros2/ardupilot_sitl/config/copter_quad.yaml @@ -12,8 +12,6 @@ # rate: 400 # console: true # home: [lat, lon, alt, yaw] - # uartA: /dev/tty0 - # uartB: /dev/tty1 # serial0: /dev/usb0 # serial1: /dev/usb1 # sim_port_in: 5501 diff --git a/Tools/ros2/ardupilot_sitl/package.xml b/Tools/ros2/ardupilot_sitl/package.xml index d0851a191e3a13..5cc0abfd72a73c 100644 --- a/Tools/ros2/ardupilot_sitl/package.xml +++ b/Tools/ros2/ardupilot_sitl/package.xml @@ -22,6 +22,7 @@ ament_cmake_uncrustify ament_cmake_xmllint + ardupilot_msgs ament_cmake diff --git a/Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py b/Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py index cb6aecaf3d2f8e..10203e148bf530 100644 --- a/Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py +++ b/Tools/ros2/ardupilot_sitl/src/ardupilot_sitl/launch.py @@ -142,6 +142,8 @@ def generate_action(context: LaunchContext, *args, **kwargs) -> ExecuteProcess: transport, "--middleware", middleware, + "--verbose", + verbose, ] if transport in ["udp4", "udp6", "tcp4", "tcp6"]: diff --git a/Tools/scripts/CAN/CAN_playback.py b/Tools/scripts/CAN/CAN_playback.py new file mode 100755 index 00000000000000..bdd4825f4fa221 --- /dev/null +++ b/Tools/scripts/CAN/CAN_playback.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +''' +playback a set of CAN frames from libraries/AP_Scripting/examples/CAN_logger.lua onto a CAN bus +''' + +import dronecan +import time +import sys +import threading +from pymavlink import mavutil +from dronecan.driver.common import CANFrame + + +# get command line arguments +from argparse import ArgumentParser +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") + +args = parser.parse_args() + +print("Connecting to %s" % args.canport) +driver = dronecan.driver.make_driver(args.canport) + +print("Opening %s" % args.logfile) +mlog = mavutil.mavlink_connection(args.logfile) + +tstart = time.time() +first_tstamp = None + +while True: + m = mlog.recv_match(type='CANF') + + if m is None: + print("Rewinding") + mlog.rewind() + tstart = time.time() + first_tstamp = None + 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] + fid = m.Id + is_extended = (fid & (1<<31)) != 0 + driver.send(fid, data, extended=is_extended, canfd=False) + print(m) diff --git a/Tools/scripts/apj_tool.py b/Tools/scripts/apj_tool.py index 55f6a9496f059c..e52e178fc439dd 100755 --- a/Tools/scripts/apj_tool.py +++ b/Tools/scripts/apj_tool.py @@ -232,7 +232,7 @@ def defaults_contents(firmware, ofs, length): have_defaults = defaults.find() if not have_defaults and not args.extract: - print("Error: Param defaults support not found in firmware") + print("Error: Param defaults support not found in firmware; see https://ardupilot.org/copter/docs/common-oem-customizations.html for embedding defaults.parm") sys.exit(1) if have_defaults: diff --git a/Tools/scripts/board_list.py b/Tools/scripts/board_list.py index ab92794673821a..1891c8225855c3 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -16,7 +16,7 @@ def __init__(self, name): self.name = name self.is_ap_periph = False self.autobuild_targets = [ - 'Tracker', + 'AntennaTracker', 'Blimp', 'Copter', 'Heli', diff --git a/Tools/scripts/build_binaries.py b/Tools/scripts/build_binaries.py index 4d0c1c02f2bc27..6e85a9839a95ef 100755 --- a/Tools/scripts/build_binaries.py +++ b/Tools/scripts/build_binaries.py @@ -493,7 +493,7 @@ def build_vehicle(self, tag, vehicle, boards, vehicle_binaries_subdir, "".join([binaryname, framesuffix])) files_to_copy = [] extensions = [".apj", ".abin", "_with_bl.hex", ".hex"] - if vehicle == 'AP_Periph': + if vehicle == 'AP_Periph' or board == "Here4FC": # need bin file for uavcan-gui-tool and MissionPlanner extensions.append('.bin') for extension in extensions: diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index 6223c2c0b09cf3..4dbc66c4bff61b 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -132,7 +132,7 @@ for t in $CI_BUILD_TARGET; do echo "Building SITL Periph GPS" $waf configure --board sitl $waf copter - run_autotest "Copter" "build.SITLPeriphGPS" "test.CAN" + run_autotest "Copter" "build.SITLPeriphUniversal" "test.CAN" continue fi if [ "$t" == "sitltest-plane" ]; then @@ -144,6 +144,8 @@ for t in $CI_BUILD_TARGET; do continue fi if [ "$t" == "sitltest-rover" ]; then + sudo apt-get update || /bin/true + sudo apt-get install -y ppp || /bin/true run_autotest "Rover" "build.Rover" "test.Rover" continue fi @@ -325,6 +327,31 @@ for t in $CI_BUILD_TARGET; do continue fi + if [ "$t" == "CubeOrange-PPP" ]; then + echo "Building CubeOrange-PPP" + $waf configure --board CubeOrange --enable-ppp + $waf clean + $waf copter + continue + fi + + if [ "$t" == "CubeOrange-SOHW" ]; then + echo "Building CubeOrange-SOHW" + Tools/scripts/sitl-on-hardware/sitl-on-hw.py --vehicle plane --simclass Plane --board CubeOrange + continue + fi + + if [ "$t" == "Pixhawk6X-PPPGW" ]; then + echo "Building Pixhawk6X-PPPGW" + $waf configure --board Pixhawk6X-PPPGW + $waf clean + $waf AP_Periph + $waf configure --board Pixhawk6X-PPPGW --bootloader + $waf clean + $waf bootloader + continue + fi + if [ "$t" == "dds-stm32h7" ]; then echo "Building with DDS support on a STM32H7" $waf configure --board Durandal --enable-dds diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 7dc8f69cfe6a8d..3cf264ce3ec993 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -72,6 +72,7 @@ def __init__(self, Feature('Telemetry', 'FrSky D', 'AP_FRSKY_D_TELEM_ENABLED', 'Enable FrSkyD Telemetry', 0, 'FrSky'), Feature('Telemetry', 'FrSky SPort', 'AP_FRSKY_SPORT_TELEM_ENABLED', 'Enable FrSkySPort Telemetry', 0, 'FrSky'), # noqa Feature('Telemetry', 'FrSky SPort PassThrough', 'AP_FRSKY_SPORT_PASSTHROUGH_ENABLED', 'Enable FrSkySPort PassThrough Telemetry', 0, 'FrSky SPort,FrSky'), # noqa + Feature('Telemetry', 'GHST', 'AP_GHST_TELEM_ENABLED', 'Enable Ghost Telemetry', 0, "RC_GHST"), # noqa Feature('Notify', 'PLAY_TUNE', 'AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', 'Enable MAVLink Play Tune', 0, None), # noqa Feature('Notify', 'TONEALARM', 'AP_NOTIFY_TONEALARM_ENABLED', 'Enable ToneAlarm on PWM', 0, None), # noqa @@ -131,7 +132,7 @@ def __init__(self, Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam Control', 0, None), Feature('Copter', 'MODE_ZIGZAG', 'MODE_ZIGZAG_ENABLED', 'Enable Mode ZigZag', 0, None), - Feature('Copter', 'MODE_SYSTEMID', 'MODE_SYSTEMID_ENABLED', 'Enable Mode SystemID', 0, None), + Feature('Copter', 'MODE_SYSTEMID', 'MODE_SYSTEMID_ENABLED', 'Enable Mode SystemID', 0, 'Logging'), Feature('Copter', 'MODE_SPORT', 'MODE_SPORT_ENABLED', 'Enable Mode Sport', 0, None), Feature('Copter', 'MODE_FOLLOW', 'MODE_FOLLOW_ENABLED', 'Enable Mode Follow', 0, 'AC_AVOID'), Feature('Copter', 'MODE_TURTLE', 'MODE_TURTLE_ENABLED', 'Enable Mode Turtle', 0, None), @@ -159,6 +160,8 @@ def __init__(self, Feature('Compass', 'QMC5883L', 'AP_COMPASS_QMC5883L_ENABLED', 'Enable QMC5883L compasses', 1, None), Feature('Compass', 'RM3100', 'AP_COMPASS_RM3100_ENABLED', 'Enable RM3100 compasses', 1, None), Feature('Compass', 'DRONECAN_COMPASS', 'AP_COMPASS_DRONECAN_ENABLED', 'Enable DroneCAN compasses', 0, None), + Feature('Compass', 'FixedYawCal', 'AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'Enable Fixed-Yaw Compass Calibration', 1, None), # noqa + Feature('Compass', 'CompassLearn', 'COMPASS_LEARN_ENABLED', 'Enable In-Flight Compass Learning', 1, "FixedYawCal"), Feature('Gimbal', 'MOUNT', 'HAL_MOUNT_ENABLED', 'Enable Mount', 0, None), Feature('Gimbal', 'ALEXMOS', 'HAL_MOUNT_ALEXMOS_ENABLED', 'Enable Alexmos Gimbal', 0, "MOUNT"), @@ -203,6 +206,7 @@ def __init__(self, Feature('RC', 'RC_SRXL2', 'AP_RCPROTOCOL_SRXL2_ENABLED', "Enable SRXL2 RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_ST24', 'AP_RCPROTOCOL_ST24_ENABLED', "Enable ST24 Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('RC', 'RC_SUMD', 'AP_RCPROTOCOL_SUMD_ENABLED', "Enable SUMD RC Protocol", 0, "RC_Protocol"), # NOQA: E501 + Feature('RC', 'RC_GHST', 'AP_RCPROTOCOL_GHST_ENABLED', "Enable Ghost RC Protocol", 0, "RC_Protocol"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER', 'AP_RANGEFINDER_ENABLED', "Enable Rangefinders", 0, None), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_ANALOG', 'AP_RANGEFINDER_ANALOG_ENABLED', "Enable Rangefinder - Analog", 0, "RANGEFINDER"), # NOQA: E501 @@ -309,6 +313,8 @@ def __init__(self, Feature('Other', 'SDCARD_MISSION', 'AP_SDCARD_STORAGE_ENABLED', 'Enable storing mission on microSD cards', 0, None), Feature('Other', 'COMPASS_CAL', 'COMPASS_CAL_ENABLED', 'Enable "tumble" compass calibration', 0, None), Feature('Other', 'DRONECAN_SERIAL', 'AP_DRONECAN_SERIAL_ENABLED', 'Enable DroneCAN virtual serial ports', 0, None), + Feature('Other', 'Buttons', 'HAL_BUTTON_ENABLED', 'Enable Buttons', 0, None), + Feature('Other', 'Logging', 'HAL_LOGGING_ENABLED', 'Enable Logging', 0, None), # MAVLink section for mavlink features and/or message handling, # rather than for e.g. mavlink-based sensor drivers @@ -373,6 +379,9 @@ def __init__(self, # Feature('Filesystem', 'FILESYSTEM_POSIX', 'AP_FILESYSTEM_POSIX_ENABLED', 'Enable POSIX filesystem', 0, None), Feature('Filesystem', 'FILESYSTEM_ROMFS', 'AP_FILESYSTEM_ROMFS_ENABLED', 'Enable @ROMFS/ filesystem', 0, None), Feature('Filesystem', 'FILESYSTEM_SYS', 'AP_FILESYSTEM_SYS_ENABLED', 'Enable @SYS/ filesystem', 0, None), + Feature('Filesystem', 'APJ_TOOL_PARAMETERS', 'FORCE_APJ_DEFAULT_PARAMETERS', 'Enable apj_tool parameter area', 0, None), + + Feature('Networking', 'PPP Support', 'AP_NETWORKING_BACKEND_PPP', 'Enable PPP networking', 0, None), ] BUILD_OPTIONS.sort(key=lambda x: (x.category + x.label)) diff --git a/Tools/scripts/build_sizes/build_sizes.py b/Tools/scripts/build_sizes/build_sizes.py index d347278f17f7e8..76cbb7af746e29 100755 --- a/Tools/scripts/build_sizes/build_sizes.py +++ b/Tools/scripts/build_sizes/build_sizes.py @@ -23,6 +23,8 @@ warning_flash_free = 5000 warning_build_days = 3 +LINUX_BINARIES = ["arduplane", "arducopter", "arducopter-heli", "ardurover", "ardusub", "antennatracker"] + class APJInfo: def __init__(self, vehicle, board, githash, mtime, flash_free): @@ -34,20 +36,29 @@ def __init__(self, vehicle, board, githash, mtime, flash_free): self.warning = 0 -def apj_list(basedir): +def firmware_list(basedir): '''list of APJInfo for one directory''' boards = [] for root, subdirs, files in os.walk(basedir): for f in files: - if not fnmatch.fnmatch(f, "*.apj"): + if not fnmatch.fnmatch(f, "*.apj") and f not in LINUX_BINARIES: continue fname = os.path.join(root, f) board = os.path.basename(root) vehicle = fname.split('/')[-4] - fw_json = json.load(open(fname, "r")) - githash = fw_json['git_identity'] - flash_free = fw_json.get('flash_free', -1) mtime = os.stat(fname).st_mtime + if f in LINUX_BINARIES: + git_version = os.path.join(root, "git-version.txt") + try: + line = open(git_version, 'r').readline() + githash = line.split()[1][:8] + except OSError: + githash = "unknown" + flash_free = 999999 + else: + fw_json = json.load(open(fname, "r")) + githash = fw_json['git_identity'] + flash_free = fw_json.get('flash_free', -1) apjinfo = APJInfo(vehicle, board, githash, mtime, flash_free) boards.append(apjinfo) return boards @@ -117,7 +128,7 @@ def write_table(h, build_type): boards = [] for build in builds: - boards.extend(apj_list(os.path.join(args.basedir, build, build_type))) + boards.extend(firmware_list(os.path.join(args.basedir, build, build_type))) max_mtime = 0 for apjinfo in boards: diff --git a/Tools/scripts/convert_param_scale.py b/Tools/scripts/convert_param_scale.py new file mode 100755 index 00000000000000..ad0f9a04cdb655 --- /dev/null +++ b/Tools/scripts/convert_param_scale.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +''' +tool to convert parameter names and scales, useful for conversion for cm -> m and cdeg -> deg + +this looks for files called *.param or *.parm + +example: + Tools/scripts/convert_param_scale.py --scale 0.1 TRIM_ARSPD_CM AIRSPEED_CRUISE +''' + +import os +import sys + +from argparse import ArgumentParser + +parser = ArgumentParser(description="parameter conversion tool") + +parser.add_argument("--scale", default=1.0, type=float, help="scale factor") +parser.add_argument("--directory", default=".", help="directory to search") +parser.add_argument("old_name", default=None, help="old parameter name") +parser.add_argument("new_name", default=None, help="new parameter name") + +args = parser.parse_args() + +def process_file(fname): + needs_write = False + with open(fname, "r") as f: + lines = f.readlines() + for i in range(len(lines)): + line = lines[i] + if line.startswith(args.old_name): + a = line.split() + if len(a) == 2: + sep = ' ' + else: + a = line.split(',') + if len(a) == 2: + sep = ',' + else: + continue + v = float(a[1]) + v *= args.scale + lines[i] = "%s%s%.2f\n" % (args.new_name, sep, v) + needs_write = True + if not needs_write: + return + print("Updating %s" % fname) + with open(fname, "w") as f: + for line in lines: + f.write(line) + +for root, dirs, files in os.walk(args.directory): + for file in files: + if file.endswith(".parm") or file.endswith(".param"): + process_file(os.path.join(root, file)) diff --git a/Tools/scripts/decode_ICSR.py b/Tools/scripts/decode_ICSR.py index 4e705a058b3c6c..0f7e785197527d 100755 --- a/Tools/scripts/decode_ICSR.py +++ b/Tools/scripts/decode_ICSR.py @@ -28,7 +28,7 @@ def __init__(self): ("25", "PENDSTCLR", self.decoder_m4_pendstclr), ("26", "PENDSTSET", self.decoder_m4_pendstset), ("27", "PENDSVCLR", self.decoder_m4_pendsvclr), - ("28", "PENDSVSET", self.decoder_m4_pendstset), + ("28", "PENDSVSET", self.decoder_m4_pendsvset), ("29-30", "RESERVED4", None), ("31", "NMIPENDSET", self.decoder_m4_nmipendset), ] diff --git a/Tools/scripts/esp32_get_idf.sh b/Tools/scripts/esp32_get_idf.sh index 39bf8dae7dcc65..4ad61e15e9d360 100755 --- a/Tools/scripts/esp32_get_idf.sh +++ b/Tools/scripts/esp32_get_idf.sh @@ -1,4 +1,5 @@ -#!/bin/bash +#!/bin/bash + # if you have modules/esp_idf setup as a submodule, then leave it as a submodule and switch branches if [ ! -d modules ]; then echo "this script needs to be run from the root of your repo, sorry, giving up." @@ -25,26 +26,31 @@ else # add esp_idf as almost submodule, depths uses less space #git clone -b v4.4 --single-branch --depth 10 https://github.com/espressif/esp-idf.git esp_idf git clone -b 'release/v4.4' https://github.com/espressif/esp-idf.git esp_idf + git checkout 6d853f # check if we've got v4.4 checked out, only this version of esp_idf is tested and works? fi fi + + echo "inspecting possible IDF... " cd esp_idf echo `git rev-parse HEAD` # these are a selection of possible specific commit/s that represent v4.4 branch of the esp_idf -if [ `git rev-parse HEAD` == 'f98ec313f2a9bc50151349c404a8f2f10ed99649' ]; then +if [ `git rev-parse HEAD` == '6d853f0525b003afaeaed4fb59a265c8522c2da9' ]; then echo "IDF version 'release/4.4' found OK, great."; else - echo "looks like an idf, but not v4.4 branch, trying to switch branch and reflect upstream"; + echo "looks like an idf, but not v4.4 branch, or wrong commit , trying to switch branch and reflect upstream"; ../../Tools/gittools/submodule-sync.sh >/dev/null git fetch ; git checkout -f release/v4.4 + git checkout 6d853f # retry same as above echo `git rev-parse HEAD` - if [ `git rev-parse HEAD` == 'f98ec313f2a9bc50151349c404a8f2f10ed99649' ]; then + if [ `git rev-parse HEAD` == '6d853f0525b003afaeaed4fb59a265c8522c2da9' ]; then echo "IDF version 'release/4.4' found OK, great."; + git checkout 6d853f fi fi cd ../.. diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index fa2b0378710eb6..5a8665bf7faf7c 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -110,6 +110,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('HAL_MOUNT_STORM32MAVLINK_ENABLED', 'AP_Mount_SToRM32::init',), ('HAL_{type}_TELEM_ENABLED', r'AP_(?P.*)_Telem::init',), + ('AP_{type}_TELEM_ENABLED', r'AP_(?P.*)_Telem::init',), ('HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED', 'AP_CRSF_Telem::calc_text_selection',), ('AP_LTM_TELEM_ENABLED', 'AP_LTM_Telem::init',), ('HAL_HIGH_LATENCY2_ENABLED', 'GCS_MAVLINK::handle_control_high_latency',), @@ -237,6 +238,12 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_TUNING_ENABLED', 'AP_Tuning::check_input'), ('AP_DRONECAN_SERIAL_ENABLED', 'AP_DroneCAN_Serial::update'), ('AP_SERIALMANAGER_IMUOUT_ENABLED', 'AP_InertialSensor::send_uart_data'), + ('AP_NETWORKING_BACKEND_PPP', 'AP_Networking_PPP::init'), + ('FORCE_APJ_DEFAULT_PARAMETERS', 'AP_Param::param_defaults_data'), + ('HAL_BUTTON_ENABLED', 'AP_Button::update'), + ('HAL_LOGGING_ENABLED', 'AP_Logger::Init'), + ('AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED', 'AP_Compass::mag_cal_fixed_yaw'), + ('COMPASS_LEARN_ENABLED', 'CompassLearn::update'), ] def progress(self, msg): diff --git a/Tools/scripts/generate_mp_paramfile.sh b/Tools/scripts/generate_mp_paramfile.sh deleted file mode 100755 index 753d14bff6c891..00000000000000 --- a/Tools/scripts/generate_mp_paramfile.sh +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env bash - -set -e -set -x - -echo "Remove previous param file" -rm -f ParameterMetaDataBackup.xml -rm -f ParameterMetaData.xml - -echo "Create first parameter file" -./Tools/autotest/param_metadata/param_parse.py --vehicle ArduCopter --format xml_mp -echo "Remove the last line" -sed -i -e '$d' ParameterMetaData.xml -echo "Copy parameters to the complete file" -cp ParameterMetaData.xml ParameterMetaDataBackup.xml - -echo "Create the second parameter file" -./Tools/autotest/param_metadata/param_parse.py --vehicle ArduPlane --format xml_mp -echo "Remove the two first lines and the last one" -sed -i -e '1d' -e '2d' -e '$d' ParameterMetaData.xml -echo "Append parameters to the complete file" -cat ParameterMetaData.xml >> ParameterMetaDataBackup.xml - -./Tools/autotest/param_metadata/param_parse.py --vehicle Rover --format xml_mp -echo "Remove the two first lines and the last one" -sed -i -e '1d' -e '2d' -e '$d' ParameterMetaData.xml -echo "Append parameters to the complete file" -cat ParameterMetaData.xml >> ParameterMetaDataBackup.xml - -./Tools/autotest/param_metadata/param_parse.py --vehicle ArduSub --format xml_mp -echo "Remove the two first lines and the last one" -sed -i -e '1d' -e '2d' -e '$d' ParameterMetaData.xml -echo "Append parameters to the complete file" -cat ParameterMetaData.xml >> ParameterMetaDataBackup.xml - -./Tools/autotest/param_metadata/param_parse.py --vehicle AntennaTracker --format xml_mp -echo "Remove the two first lines" -sed -i -e '1d' -e '2d' ParameterMetaData.xml -echo "Append parameters to the complete file" -cat ParameterMetaData.xml >> ParameterMetaDataBackup.xml - -echo "Remove vehile param file" -rm -f ParameterMetaData.xml -echo "Rename complete param file" -mv ParameterMetaDataBackup.xml ParameterMetaData.xml diff --git a/Tools/scripts/run_coverage.py b/Tools/scripts/run_coverage.py index ee6e4c24a34bc6..00036982128b03 100755 --- a/Tools/scripts/run_coverage.py +++ b/Tools/scripts/run_coverage.py @@ -238,8 +238,8 @@ def update_stats(self) -> None: root_dir + "/build/linux/modules/*", root_dir + "/build/sitl/libraries/*", root_dir + "/build/sitl/modules/*", - root_dir + "/build/sitl_periph_gps/libraries/*", - root_dir + "/build/sitl_periph_gps/modules/*", + root_dir + "/build/sitl_periph_universal/libraries/*", + root_dir + "/build/sitl_periph_universal/modules/*", root_dir + "/libraries/*/examples/*", root_dir + "/libraries/*/tests/*", "-o", self.INFO_FILE diff --git a/Tools/scripts/run_flake8.py b/Tools/scripts/run_flake8.py index acfde4e4e52f63..22df522e00199d 100755 --- a/Tools/scripts/run_flake8.py +++ b/Tools/scripts/run_flake8.py @@ -25,6 +25,8 @@ def progress(self, string): print("****** %s" % (string,)) def check(self): + if len(self.files_to_check) == 0: + return for path in self.files_to_check: self.progress("Checking (%s)" % path) ret = subprocess.run(["flake8", "--show-source"] + self.files_to_check, diff --git a/Tools/scripts/sitl-on-hardware/README.md b/Tools/scripts/sitl-on-hardware/README.md index 081b78c8494212..26135aac74d1c2 100644 --- a/Tools/scripts/sitl-on-hardware/README.md +++ b/Tools/scripts/sitl-on-hardware/README.md @@ -7,21 +7,21 @@ Run the sitl-on-hw.sh script to compile and flash for MatekH743. Adjust for you :: cd $HOME/ardupilot - ./libraries/SITL/examples/on-hardware/sitl-on-hw.py --board MatekH743 --vehicle copter + ./Tools/scripts/sitl-on-hardware/sitl-on-hw.py --board MatekH743 --vehicle copter Plane can also be simulated: :: cd $HOME/ardupilot - ./libraries/SITL/examples/on-hardware/sitl-on-hw.py --board MatekH743 --vehicle plane + ./Tools/scripts/sitl-on-hardware/sitl-on-hw.py --board MatekH743 --vehicle plane and quadplane: :: cd $HOME/ardupilot - ./libraries/SITL/examples/on-hardware/sitl-on-hw.py --board MatekH743 --vehicle plane --simclass QuadPlane + ./Tools/scripts/sitl-on-hardware/sitl-on-hw.py --board MatekH743 --vehicle plane --simclass QuadPlane ## Configuring diff --git a/Tools/scripts/size_compare_branches.py b/Tools/scripts/size_compare_branches.py index 41e93bc7a5c5ca..a7321448c1da15 100755 --- a/Tools/scripts/size_compare_branches.py +++ b/Tools/scripts/size_compare_branches.py @@ -164,6 +164,7 @@ def __init__(self, 'Pixhawk1-bdshot', 'SITL_arm_linux_gnueabihf', 'RADIX2HD', + 'canzero', ]) # blacklist all linux boards for bootloader build: @@ -197,6 +198,7 @@ def linux_board_names(self): 'rst_zynq', 'obal', 'SITL_x86_64_linux_gnu', + 'canzero', ] def esp32_board_names(self): @@ -206,6 +208,7 @@ def esp32_board_names(self): 'esp32tomte76', 'esp32nick', 'esp32s3devkit', + 'esp32s3empty', 'esp32icarous', 'esp32diy', ] @@ -264,6 +267,12 @@ def run_program(self, prefix, cmd_list, show_output=True, env=None, show_output_ print(output) self.progress("Process failed (%s)" % str(returncode)) + try: + path = pathlib.Path(self.tmpdir, f"process-failure-{int(time.time())}") + path.write_text(output) + self.progress("Wrote process failure file (%s)" % path) + except Exception: + self.progress("Writing process failure file failed") raise subprocess.CalledProcessError( returncode, cmd_list) return output @@ -413,7 +422,7 @@ def parallel_thread_main(self, thread_number): break jobs = None if self.jobs is not None: - jobs = int(self.jobs / self.num_threads_remaining) + jobs = int(self.jobs / self.n_threads) if jobs <= 0: jobs = 1 try: @@ -433,27 +442,29 @@ def check_result_queue(self): self.failure_exceptions.append(result) def run_build_tasks_in_parallel(self, tasks): - n_threads = self.parallel_copies - if len(tasks) < n_threads: - n_threads = len(tasks) - self.num_threads_remaining = n_threads + self.n_threads = self.parallel_copies # shared list for the threads: self.parallel_tasks = copy.copy(tasks) # make this an argument instead?! threads = [] self.thread_exit_result_queue = Queue.Queue() - for i in range(0, n_threads): - t = threading.Thread( - target=self.parallel_thread_main, - name=f'task-builder-{i}', - args=[i], - ) - t.start() - threads.append(t) tstart = time.time() self.failure_exceptions = [] - while len(threads): + thread_number = 0 + while len(self.parallel_tasks) or len(threads): + if len(self.parallel_tasks) < self.n_threads: + self.n_threads = len(self.parallel_tasks) + while len(threads) < self.n_threads: + self.progress(f"Starting thread {thread_number}") + t = threading.Thread( + target=self.parallel_thread_main, + name=f'task-builder-{thread_number}', + args=[thread_number], + ) + t.start() + threads.append(t) + thread_number += 1 self.check_result_queue() @@ -463,10 +474,9 @@ def run_build_tasks_in_parallel(self, tasks): if thread.is_alive(): new_threads.append(thread) threads = new_threads - self.num_threads_remaining = len(threads) self.progress( f"remaining-tasks={len(self.parallel_tasks)} " + - f"remaining-threads={len(threads)} failed-threads={len(self.failure_exceptions)} elapsed={int(time.time() - tstart)}s") # noqa + f"failed-threads={len(self.failure_exceptions)} elapsed={int(time.time() - tstart)}s") # noqa # write out a progress CSV: task_results = [] diff --git a/Tools/scripts/uploader.py b/Tools/scripts/uploader.py index a4872f2844ff70..b926b26c232795 100755 --- a/Tools/scripts/uploader.py +++ b/Tools/scripts/uploader.py @@ -275,7 +275,7 @@ def __init__(self, self.force_erase = force_erase # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate_bootloader, timeout=2.0) + self.port = serial.Serial(portname, baudrate_bootloader, timeout=2.0, write_timeout=2.0) self.baudrate_bootloader = baudrate_bootloader if baudrate_bootloader_flash is not None: self.baudrate_bootloader_flash = baudrate_bootloader_flash diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 282237fb3a961d..e82241ab3c5d45 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -1160,6 +1160,7 @@ void AC_PosControl::standby_xyz_reset() init_ekf_xy_reset(); } +#if HAL_LOGGING_ENABLED // write PSC and/or PSCZ logs void AC_PosControl::write_log() { @@ -1180,6 +1181,7 @@ void AC_PosControl::write_log() -_accel_desired.z, -get_accel_target_cmss().z, -get_z_accel_cmss()); } } +#endif // HAL_LOGGING_ENABLED /// crosstrack_error - returns horizontal error to the closest point to the current track float AC_PosControl::crosstrack_error() const diff --git a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp index 087f4b2274c51d..7c0e6b81547105 100644 --- a/libraries/AC_AttitudeControl/AC_WeatherVane.cpp +++ b/libraries/AC_AttitudeControl/AC_WeatherVane.cpp @@ -219,7 +219,8 @@ bool AC_WeatherVane::get_yaw_out(float &yaw_output, const int16_t pilot_yaw, con } if (!active_msg_sent) { - gcs().send_text(MAV_SEVERITY_INFO, "Weathervane Active: %s", dir_string); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Weathervane Active: %s", dir_string); + (void)dir_string; // in case GCS is disabled active_msg_sent = true; } diff --git a/libraries/AC_AttitudeControl/ControlMonitor.cpp b/libraries/AC_AttitudeControl/ControlMonitor.cpp index 5efdb1606c56da..1535a9f273a376 100644 --- a/libraries/AC_AttitudeControl/ControlMonitor.cpp +++ b/libraries/AC_AttitudeControl/ControlMonitor.cpp @@ -37,8 +37,9 @@ void AC_AttitudeControl::control_monitor_update(void) control_monitor_filter_pid(iyaw.P + iyaw.D + iyaw.FF, _control_monitor.rms_yaw); } +#if HAL_LOGGING_ENABLED /* - log a CRTL message + log a CTRL message */ void AC_AttitudeControl::control_monitor_log(void) const { @@ -59,6 +60,7 @@ void AC_AttitudeControl::control_monitor_log(void) const (double)safe_sqrt(_control_monitor.rms_yaw)); } +#endif // HAL_LOGGING_ENABLED /* return current controller RMS filter value for roll diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 1e672f2e325505..998297d0219877 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -1,3 +1,7 @@ +#include "AC_AutoTune_config.h" + +#if AC_AUTOTUNE_ENABLED + #include "AC_AutoTune.h" #include @@ -71,7 +75,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold, level_start_time_ms = step_start_time_ms; // reset gains to tuning-start gains (i.e. low I term) load_gains(GAIN_INTRA_TEST); - AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_RESTART); update_gcs(AUTOTUNE_MESSAGE_STARTED); break; @@ -79,7 +83,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold, // we have completed a tune and the pilot wishes to test the new gains load_gains(GAIN_TUNED); update_gcs(AUTOTUNE_MESSAGE_TESTING); - AP::logger().Write_Event(LogEvent::AUTOTUNE_PILOT_TESTING); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_PILOT_TESTING); break; } @@ -98,7 +102,8 @@ void AC_AutoTune::stop() attitude_control->use_sqrt_controller(true); update_gcs(AUTOTUNE_MESSAGE_STOPPED); - AP::logger().Write_Event(LogEvent::AUTOTUNE_OFF); + + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_OFF); // Note: we leave the mode as it was so that we know how the autotune ended // we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch @@ -396,10 +401,12 @@ void AC_AutoTune::control_attitude() step = WAITING_FOR_LEVEL; } +#if HAL_LOGGING_ENABLED // log this iterations lean angle and rotation rate Log_AutoTuneDetails(); ahrs_view->Write_Rate(*motors, *attitude_control, *pos_control); log_pids(); +#endif break; } @@ -408,8 +415,10 @@ void AC_AutoTune::control_attitude() // re-enable rate limits attitude_control->use_sqrt_controller(true); +#if HAL_LOGGING_ENABLED // log the latest gains Log_AutoTune(); +#endif // Announce tune type test results // must be done before updating method because this method changes parameters for next test @@ -515,7 +524,7 @@ void AC_AutoTune::control_attitude() if (complete) { mode = SUCCESS; update_gcs(AUTOTUNE_MESSAGE_SUCCESS); - AP::logger().Write_Event(LogEvent::AUTOTUNE_SUCCESS); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SUCCESS); AP_Notify::events.autotune_complete = true; } else { AP_Notify::events.autotune_next_axis = true; @@ -757,3 +766,4 @@ void AC_AutoTune::next_tune_type(TuneType &curr_tune_type, bool reset) curr_tune_type = tune_seq[tune_seq_curr]; } +#endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AC_AutoTune/AC_AutoTune.h b/libraries/AC_AutoTune/AC_AutoTune.h index e152af499993f8..4c2198c11ac88c 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.h +++ b/libraries/AC_AutoTune/AC_AutoTune.h @@ -18,6 +18,10 @@ */ #pragma once +#include "AC_AutoTune_config.h" + +#if AC_AUTOTUNE_ENABLED + #include #include #include @@ -88,8 +92,10 @@ class AC_AutoTune // init pos controller Z velocity and accel limits virtual void init_z_limits() = 0; +#if HAL_LOGGING_ENABLED // log PIDs at full rate for during twitch virtual void log_pids() = 0; +#endif // // methods to load and save gains @@ -147,9 +153,12 @@ class AC_AutoTune // reverse direction for twitch test virtual bool twitch_reverse_direction() = 0; + +#if HAL_LOGGING_ENABLED virtual void Log_AutoTune() = 0; virtual void Log_AutoTuneDetails() = 0; virtual void Log_AutoTuneSweep() = 0; +#endif // internal init function, should be called from init() bool init_internals(bool use_poshold, @@ -327,3 +336,5 @@ class AC_AutoTune uint32_t last_pilot_override_warning; }; + +#endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index d72a6db6fe4c09..0f972a48938076 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -17,6 +17,10 @@ Converted to a library by Andrew Tridgell, and rewritten to include helicopters by Bill Geyer */ +#include "AC_AutoTune_config.h" + +#if AC_AUTOTUNE_ENABLED + #include "AC_AutoTune_Heli.h" #include @@ -246,12 +250,12 @@ void AC_AutoTune_Heli::test_run(AxisType test_axis, const float dir_sign) if ((tune_type == RP_UP || tune_type == RD_UP) && (max_rate_p.max_allowed <= 0.0f || max_rate_d.max_allowed <= 0.0f)) { gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Max Gain Determination Failed"); mode = FAILED; - AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); update_gcs(AUTOTUNE_MESSAGE_FAILED); } else if ((tune_type == MAX_GAINS || tune_type == RP_UP || tune_type == RD_UP || tune_type == SP_UP) && exceeded_freq_range(start_freq)){ gcs().send_text(MAV_SEVERITY_INFO, "AutoTune: Exceeded frequency range"); mode = FAILED; - AP::logger().Write_Event(LogEvent::AUTOTUNE_FAILED); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_FAILED); update_gcs(AUTOTUNE_MESSAGE_FAILED); } else if (tune_type == TUNE_COMPLETE) { counter = AUTOTUNE_SUCCESS_COUNT; @@ -438,7 +442,7 @@ void AC_AutoTune_Heli::backup_gains_and_initialise() tune_yaw_sp = attitude_control->get_angle_yaw_p().kP(); tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss(); - AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_INITIALISED); } // load_orig_gains - set gains to their original values @@ -656,7 +660,7 @@ void AC_AutoTune_Heli::save_tuning_gains() // update GCS and log save gains event update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); - AP::logger().Write_Event(LogEvent::AUTOTUNE_SAVEDGAINS); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SAVEDGAINS); reset(); } @@ -1162,8 +1166,10 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq, if (dwell_type == DRB) {test_accel_max = freqresp.get_accel_max();} // reset cycle_complete to allow indication of next cycle freqresp.reset_cycle_complete(); +#if HAL_LOGGING_ENABLED // log sweep data Log_AutoTuneSweep(); +#endif } else { dwell_gain = freqresp.get_gain(); dwell_phase = freqresp.get_phase(); @@ -1379,14 +1385,14 @@ void AC_AutoTune_Heli::updating_rate_ff_up(float &tune_ff, float rate_target, fl if (tune_ff <= AUTOTUNE_RFF_MIN) { tune_ff = AUTOTUNE_RFF_MIN; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else if (is_positive(rate_target * meas_rate) && fabsf(meas_rate) < 0.95f * fabsf(rate_target)) { tune_ff = 1.02f * tune_ff; if (tune_ff >= AUTOTUNE_RFF_MAX) { tune_ff = AUTOTUNE_RFF_MAX; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { if (!is_zero(meas_rate)) { @@ -1543,7 +1549,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga // exceeded max response gain at the minimum allowable tuning gain. terminate testing. tune_p = AUTOTUNE_SP_MIN; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } else if (gain[freq_cnt] > gain[freq_cnt_max]) { freq_cnt_max = freq_cnt; phase_max = phase[freq_cnt]; @@ -1575,7 +1581,7 @@ void AC_AutoTune_Heli::updating_angle_p_up(float &tune_p, float *freq, float *ga if (tune_p >= AUTOTUNE_SP_MAX) { tune_p = AUTOTUNE_SP_MAX; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } curr_test.freq = freq[freq_cnt]; @@ -1718,6 +1724,7 @@ void AC_AutoTune_Heli::updating_max_gains(float *freq, float *gain, float *phase } +#if HAL_LOGGING_ENABLED // log autotune summary data void AC_AutoTune_Heli::Log_AutoTune() { @@ -1836,6 +1843,7 @@ void AC_AutoTune_Heli::Log_Write_AutoTuneSweep(float freq, float gain, float pha gain, phase); } +#endif // HAL_LOGGING_ENABLED // reset the test variables for each vehicle void AC_AutoTune_Heli::reset_vehicle_test_variables() @@ -1941,3 +1949,5 @@ bool AC_AutoTune_Heli::exceeded_freq_range(float frequency) } return ret; } + +#endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.h b/libraries/AC_AutoTune/AC_AutoTune_Heli.h index 6329563b02bae7..4890a0da8e2cef 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.h @@ -18,6 +18,10 @@ #pragma once +#include "AC_AutoTune_config.h" + +#if AC_AUTOTUNE_ENABLED + #include "AC_AutoTune.h" #include #include @@ -99,6 +103,7 @@ class AC_AutoTune_Heli : public AC_AutoTune // reverse direction for twitch test bool twitch_reverse_direction() override { return positive_direction; } +#if HAL_LOGGING_ENABLED // methods to log autotune summary data void Log_AutoTune() override; void Log_Write_AutoTune(uint8_t _axis, uint8_t tune_step, float dwell_freq, float meas_gain, float meas_phase, float new_gain_rff, float new_gain_rp, float new_gain_rd, float new_gain_sp, float max_accel); @@ -110,6 +115,7 @@ class AC_AutoTune_Heli : public AC_AutoTune // methods to log autotune frequency response results void Log_AutoTuneSweep() override; void Log_Write_AutoTuneSweep(float freq, float gain, float phase); +#endif // send intermittent updates to user on status of tune void do_gcs_announcements() override; @@ -296,3 +302,5 @@ class AC_AutoTune_Heli : public AC_AutoTune Chirp chirp_input; }; + +#endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index bf988c187ae9f2..f304c3e70acb91 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -1,3 +1,7 @@ +#include "AC_AutoTune_config.h" + +#if AC_AUTOTUNE_ENABLED + #include "AC_AutoTune_Multi.h" #include @@ -188,7 +192,7 @@ void AC_AutoTune_Multi::backup_gains_and_initialise() tune_yaw_sp = attitude_control->get_angle_yaw_p().kP(); tune_yaw_accel = attitude_control->get_accel_yaw_max_cdss(); - AP::logger().Write_Event(LogEvent::AUTOTUNE_INITIALISED); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_INITIALISED); } // load_orig_gains - set gains to their original values @@ -460,7 +464,7 @@ void AC_AutoTune_Multi::save_tuning_gains() // update GCS and log save gains event update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS); - AP::logger().Write_Event(LogEvent::AUTOTUNE_SAVEDGAINS); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_SAVEDGAINS); reset(); } @@ -792,7 +796,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa // We have reached minimum D gain so stop tuning tune_d = tune_d_min; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } } else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { @@ -801,7 +805,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa tune_p += tune_p*tune_p_step_ratio; if (tune_p >= tune_p_max) { tune_p = tune_p_max; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { // we have a good measurement of bounce back @@ -822,7 +826,7 @@ void AC_AutoTune_Multi::updating_rate_d_up(float &tune_d, float tune_d_min, floa if (tune_d >= tune_d_max) { tune_d = tune_d_max; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { ignore_next = false; @@ -847,7 +851,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl // We have reached minimum D so stop tuning tune_d = tune_d_min; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } } else if ((meas_rate_max < rate_target*(1.0f-AUTOTUNE_D_UP_DOWN_MARGIN)) && (tune_p <= tune_p_max)) { @@ -856,7 +860,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl tune_p += tune_p*tune_p_step_ratio; if (tune_p >= tune_p_max) { tune_p = tune_p_max; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { // we have a good measurement of bounce back @@ -880,7 +884,7 @@ void AC_AutoTune_Multi::updating_rate_d_down(float &tune_d, float tune_d_min, fl if (tune_d <= tune_d_min) { tune_d = tune_d_min; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } } @@ -905,14 +909,14 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi // do not decrease the D term past the minimum if (tune_d <= tune_d_min) { tune_d = tune_d_min; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } // decrease P gain to match D gain reduction tune_p -= tune_p*tune_p_step_ratio; // do not decrease the P term past the minimum if (tune_p <= tune_p_min) { tune_p = tune_p_min; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } // cancel change in direction positive_direction = !positive_direction; @@ -928,7 +932,7 @@ void AC_AutoTune_Multi::updating_rate_p_up_d_down(float &tune_d, float tune_d_mi if (tune_p >= tune_p_max) { tune_p = tune_p_max; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { ignore_next = false; @@ -960,8 +964,8 @@ void AC_AutoTune_Multi::updating_angle_p_down(float &tune_p, float tune_p_min, f if (tune_p <= tune_p_min) { tune_p = tune_p_min; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); - } + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); + } } } @@ -987,7 +991,7 @@ void AC_AutoTune_Multi::updating_angle_p_up(float &tune_p, float tune_p_max, flo if (tune_p >= tune_p_max) { tune_p = tune_p_max; counter = AUTOTUNE_SUCCESS_COUNT; - AP::logger().Write_Event(LogEvent::AUTOTUNE_REACHED_LIMIT); + LOGGER_WRITE_EVENT(LogEvent::AUTOTUNE_REACHED_LIMIT); } } else { ignore_next = false; @@ -995,6 +999,7 @@ void AC_AutoTune_Multi::updating_angle_p_up(float &tune_p, float tune_p_max, flo } } +#if HAL_LOGGING_ENABLED void AC_AutoTune_Multi::Log_AutoTune() { if ((tune_type == SP_DOWN) || (tune_type == SP_UP)) { @@ -1089,6 +1094,7 @@ void AC_AutoTune_Multi::Log_Write_AutoTuneDetails(float angle_cd, float rate_cds angle_cd*0.01f, rate_cds*0.01f); } +#endif // HAL_LOGGING_ENABLED void AC_AutoTune_Multi::twitch_test_init() { @@ -1246,3 +1252,5 @@ uint32_t AC_AutoTune_Multi::get_testing_step_timeout_ms() const return AUTOTUNE_TESTING_STEP_TIMEOUT_MS; } + +#endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.h b/libraries/AC_AutoTune/AC_AutoTune_Multi.h index b2a02fcdc6bce8..ac2a0c6892a0e0 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.h +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.h @@ -19,6 +19,10 @@ #pragma once +#include "AC_AutoTune_config.h" + +#if AC_AUTOTUNE_ENABLED + #include "AC_AutoTune.h" class AC_AutoTune_Multi : public AC_AutoTune @@ -104,6 +108,7 @@ class AC_AutoTune_Multi : public AC_AutoTune // reverse direction for twitch test bool twitch_reverse_direction() override { return !positive_direction; } +#if HAL_LOGGING_ENABLED void Log_AutoTune() override; void Log_AutoTuneDetails() override; void Log_AutoTuneSweep() override { @@ -112,6 +117,7 @@ class AC_AutoTune_Multi : public AC_AutoTune } void Log_Write_AutoTune(uint8_t axis, uint8_t tune_step, float meas_target, float meas_min, float meas_max, float new_gain_rp, float new_gain_rd, float new_gain_sp, float new_ddt); void Log_Write_AutoTuneDetails(float angle_cd, float rate_cds); +#endif void set_tune_sequence() override { tune_seq[0] = RD_UP; @@ -168,3 +174,5 @@ class AC_AutoTune_Multi : public AC_AutoTune AP_Float aggressiveness; // aircraft response aggressiveness to be tuned AP_Float min_d; // minimum rate d gain allowed during tuning }; + +#endif // AC_AUTOTUNE_ENABLED diff --git a/libraries/AC_AutoTune/AC_AutoTune_config.h b/libraries/AC_AutoTune/AC_AutoTune_config.h new file mode 100644 index 00000000000000..ae2b22b712205b --- /dev/null +++ b/libraries/AC_AutoTune/AC_AutoTune_config.h @@ -0,0 +1,7 @@ +#pragma once + +#include + +#ifndef AC_AUTOTUNE_ENABLED +#define AC_AUTOTUNE_ENABLED 1 +#endif diff --git a/libraries/AC_Autorotation/AC_Autorotation.cpp b/libraries/AC_Autorotation/AC_Autorotation.cpp index 7ce790517d942b..627dd7894ec317 100644 --- a/libraries/AC_Autorotation/AC_Autorotation.cpp +++ b/libraries/AC_Autorotation/AC_Autorotation.cpp @@ -252,6 +252,7 @@ float AC_Autorotation::get_rpm(bool update_counter) } +#if HAL_LOGGING_ENABLED void AC_Autorotation::Log_Write_Autorotation(void) const { // @LoggerMessage: AROT @@ -289,7 +290,7 @@ void AC_Autorotation::Log_Write_Autorotation(void) const (double)_accel_target, (double)_pitch_target); } - +#endif // HAL_LOGGING_ENABLED // Initialise forward speed controller void AC_Autorotation::init_fwd_spd_controller(void) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 8e5067f3be6935..9745d4abb62b5b 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -259,6 +259,7 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa _last_limit_time = AP_HAL::millis(); } +#if HAL_LOGGING_ENABLED if (limits_active()) { // log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!) uint32_t now = AP_HAL::millis(); @@ -275,6 +276,7 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa _last_log_ms = 0; } } +#endif } /* diff --git a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp index f02ad8a2fd743d..7a6b3354f9b4c0 100644 --- a/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp +++ b/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp @@ -1,3 +1,7 @@ +#include + +#if HAL_LOGGING_ENABLED + #include "AC_Avoid.h" #include "AP_OADijkstra.h" #include "AP_OABendyRuler.h" @@ -76,3 +80,5 @@ void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desire }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 673dd552766971..ed122bff29c414 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.cpp +++ b/libraries/AC_Avoidance/AP_OABendyRuler.cpp @@ -78,8 +78,8 @@ AP_OABendyRuler::AP_OABendyRuler() _bearing_prev = FLT_MAX; } -// run background task to find best path and update avoidance_results -// returns true and updates origin_new and destination_new if a best path has been found +// run background task to find best path +// returns true and updates origin_new and destination_new if a best path has been found. returns false if OA is not required // bendy_type is set to the type of BendyRuler used bool AP_OABendyRuler::update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, OABendyType &bendy_type, bool proximity_only) { @@ -318,7 +318,7 @@ bool AP_OABendyRuler::search_vertical_path(const Location ¤t_loc, const Lo destination_new = current_loc; destination_new.offset_bearing_and_pitch(bearing_to_dest, pitch_delta, distance_to_dest); _current_lookahead = MIN(_lookahead, _current_lookahead * 1.1f); - + Write_OABendyRuler((uint8_t)OABendyType::OA_BENDY_VERTICAL, active, bearing_to_dest, pitch_delta, false, margin, destination, destination_new); return active; } diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.h b/libraries/AC_Avoidance/AP_OABendyRuler.h index e282e647221942..35e9b4a1c38328 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.h +++ b/libraries/AC_Avoidance/AP_OABendyRuler.h @@ -3,6 +3,7 @@ #include #include #include +#include /* * BendyRuler avoidance algorithm for avoiding the polygon and circular fence and dynamic objects detected by the proximity sensor @@ -22,8 +23,9 @@ class AP_OABendyRuler { OA_BENDY_VERTICAL = 2, }; - // run background task to find best path and update avoidance_results - // returns true and populates origin_new and destination_new if OA is required. returns false if OA is not required + // run background task to find best path + // returns true and updates origin_new and destination_new if a best path has been found. returns false if OA is not required + // bendy_type is set to the type of BendyRuler used bool update(const Location& current_loc, const Location& destination, const Vector2f &ground_speed_vec, Location &origin_new, Location &destination_new, OABendyType &bendy_type, bool proximity_only); static const struct AP_Param::GroupInfo var_info[]; @@ -66,7 +68,11 @@ class AP_OABendyRuler { bool calc_margin_from_object_database(const Location &start, const Location &end, float &margin) const; // Logging function +#if HAL_LOGGING_ENABLED void 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; +#else + void 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 {} +#endif // OA common parameters float _margin_max; // object avoidance will ignore objects more than this many meters from vehicle diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 749cb140ee24a1..d40b7a182f207d 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.cpp +++ b/libraries/AC_Avoidance/AP_OADijkstra.cpp @@ -40,19 +40,30 @@ AP_OADijkstra::AP_OADijkstra(AP_Int16 &options) : } // calculate a destination to avoid fences -// returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required -AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t_loc, const Location &destination, Location& origin_new, Location& destination_new) +// returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required +// next_destination_new will be non-zero if there is a next destination +// dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear +AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t_loc, + const Location &destination, + const Location &next_destination, + Location& origin_new, + Location& destination_new, + Location& next_destination_new, + bool& dest_to_next_dest_clear) { WITH_SEMAPHORE(AP::fence()->polyfence().get_loaded_fence_semaphore()); // avoidance is not required if no fences if (!some_fences_enabled()) { + dest_to_next_dest_clear = _dest_to_next_dest_clear = true; Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination); return DIJKSTRA_STATE_NOT_REQUIRED; } // no avoidance required if destination is same as current location if (current_loc.same_latlon_as(destination)) { + // we do not check path to next destination so conservatively set to false + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination); return DIJKSTRA_STATE_NOT_REQUIRED; } @@ -79,43 +90,46 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t } // create inner polygon fence - AP_OADijkstra_Error error_id; if (!_inclusion_polygon_with_margin_ok) { - _inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id); + _inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, _error_id); if (!_inclusion_polygon_with_margin_ok) { - report_error(error_id); - Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; + report_error(_error_id); + Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; } } // create exclusion polygon outer fence if (!_exclusion_polygon_with_margin_ok) { - _exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id); + _exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, _error_id); if (!_exclusion_polygon_with_margin_ok) { - report_error(error_id); - Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; + report_error(_error_id); + Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; } } // create exclusion circle points if (!_exclusion_circle_with_margin_ok) { - _exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, error_id); + _exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, _error_id); if (!_exclusion_circle_with_margin_ok) { - report_error(error_id); - Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; + report_error(_error_id); + Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; } } // create visgraph for all fence (with margin) points if (!_polyfence_visgraph_ok) { - _polyfence_visgraph_ok = create_fence_visgraph(error_id); + _polyfence_visgraph_ok = create_fence_visgraph(_error_id); if (!_polyfence_visgraph_ok) { _shortest_path_ok = false; - report_error(error_id); - Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; + report_error(_error_id); + Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; } // reset logging count to restart logging updated graph @@ -133,27 +147,39 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t } } - // rebuild path if destination has changed - if (!destination.same_latlon_as(_destination_prev)) { + // rebuild path if destination or next_destination has changed + if (!destination.same_latlon_as(_destination_prev) || !next_destination.same_latlon_as(_next_destination_prev)) { _destination_prev = destination; + _next_destination_prev = next_destination; _shortest_path_ok = false; } // calculate shortest path from current_loc to destination if (!_shortest_path_ok) { - _shortest_path_ok = calc_shortest_path(current_loc, destination, error_id); + _shortest_path_ok = calc_shortest_path(current_loc, destination, _error_id); if (!_shortest_path_ok) { - report_error(error_id); - Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)error_id, 0, 0, destination, destination); + dest_to_next_dest_clear = _dest_to_next_dest_clear = false; + report_error(_error_id); + Write_OADijkstra(DIJKSTRA_STATE_ERROR, (uint8_t)_error_id, 0, 0, destination, destination); return DIJKSTRA_STATE_ERROR; } // start from 2nd point on path (first is the original origin) _path_idx_returned = 1; + + // check if path from destination to next_destination intersects with a fence + _dest_to_next_dest_clear = false; + if (!next_destination.is_zero()) { + Vector2f seg_start, seg_end; + if (destination.get_vector_xy_from_origin_NE(seg_start) && next_destination.get_vector_xy_from_origin_NE(seg_end)) { + _dest_to_next_dest_clear = !intersects_fence(seg_start, seg_end); + } + } } // path has been created, return latest point Vector2f dest_pos; - if (get_shortest_path_point(_path_idx_returned, dest_pos)) { + const uint8_t path_length = get_shortest_path_numpoints() > 0 ? (get_shortest_path_numpoints() - 1) : 0; + if ((_path_idx_returned < path_length) && get_shortest_path_point(_path_idx_returned, dest_pos)) { // for the first point return origin as current_loc Vector2f origin_pos; @@ -172,6 +198,23 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t destination_new.lat = temp_loc.lat; destination_new.lng = temp_loc.lng; + // provide next destination to allow smooth cornering + next_destination_new.zero(); + Vector2f next_dest_pos; + if ((_path_idx_returned + 1 < path_length) && get_shortest_path_point(_path_idx_returned + 1, next_dest_pos)) { + // convert offset from ekf origin to Location + Location next_loc(Vector3f{next_dest_pos.x, next_dest_pos.y, 0.0}, Location::AltFrame::ABOVE_ORIGIN); + next_destination_new = destination; + next_destination_new.lat = next_loc.lat; + next_destination_new.lng = next_loc.lng; + } else { + // return destination as next_destination + next_destination_new = destination; + } + + // path to next destination clear state is still valid from previous calcs (was calced along with shortest path) + dest_to_next_dest_clear = _dest_to_next_dest_clear; + // check if we should advance to next point for next iteration const bool near_oa_wp = current_loc.get_distance(destination_new) <= 2.0f; const bool past_oa_wp = current_loc.past_interval_finish_line(origin_new, destination_new); @@ -179,11 +222,13 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t _path_idx_returned++; } // log success - Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, _path_numpoints, destination, destination_new); + Write_OADijkstra(DIJKSTRA_STATE_SUCCESS, 0, _path_idx_returned, get_shortest_path_numpoints(), destination, destination_new); return DIJKSTRA_STATE_SUCCESS; } // we have reached the destination so avoidance is no longer required + // path to next destination clear state is still valid from previous calcs + dest_to_next_dest_clear = _dest_to_next_dest_clear; Write_OADijkstra(DIJKSTRA_STATE_NOT_REQUIRED, 0, 0, 0, destination, destination); return DIJKSTRA_STATE_NOT_REQUIRED; } @@ -934,7 +979,7 @@ bool AP_OADijkstra::calc_shortest_path(const Location &origin, const Location &d } // return point from final path as an offset (in cm) from the ekf origin -bool AP_OADijkstra::get_shortest_path_point(uint8_t point_num, Vector2f& pos) +bool AP_OADijkstra::get_shortest_path_point(uint8_t point_num, Vector2f& pos) const { if ((_path_numpoints == 0) || (point_num >= _path_numpoints)) { return false; diff --git a/libraries/AC_Avoidance/AP_OADijkstra.h b/libraries/AC_Avoidance/AP_OADijkstra.h index 48e91f6cc1269c..f56e940e30c9b8 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -4,6 +4,7 @@ #include #include #include "AP_OAVisGraph.h" +#include /* * Dijkstra's algorithm for path planning around polygon fence @@ -30,8 +31,16 @@ class AP_OADijkstra { }; // calculate a destination to avoid the polygon fence - // returns DIJKSTRA_STATE_SUCCESS and populates origin_new and destination_new if avoidance is required - AP_OADijkstra_State update(const Location ¤t_loc, const Location &destination, Location& origin_new, Location& destination_new); + // returns DIJKSTRA_STATE_SUCCESS and populates origin_new, destination_new and next_destination_new if avoidance is required + // next_destination_new will be non-zero if there is a next destination + // dest_to_next_dest_clear will be set to true if the path from (the input) destination to (input) next_destination is clear + AP_OADijkstra_State update(const Location ¤t_loc, + const Location &destination, + const Location &next_destination, + Location& origin_new, + Location& destination_new, + Location& next_destination_new, + bool& dest_to_next_dest_clear); private: @@ -48,7 +57,7 @@ class AP_OADijkstra { DIJKSTRA_ERROR_TOO_MANY_FENCE_POINTS, DIJKSTRA_ERROR_NO_POSITION_ESTIMATE, DIJKSTRA_ERROR_COULD_NOT_FIND_PATH - }; + } _error_id; // return error message for a given error id const char* get_error_msg(AP_OADijkstra_Error error_id) const; @@ -124,7 +133,9 @@ class AP_OADijkstra { bool _shortest_path_ok; Location _destination_prev; // destination of previous iterations (used to determine if path should be re-calculated) + Location _next_destination_prev;// next_destination of previous iterations (used to determine if path should be re-calculated) uint8_t _path_idx_returned; // index into _path array which gives location vehicle should be currently moving towards + bool _dest_to_next_dest_clear; // true if path from dest to next_dest is clear (i.e. does not intersects a fence) // inclusion polygon (with margin) related variables float _polyfence_margin = 10; // margin around polygon defaults to 10m but is overriden with set_fence_margin @@ -181,8 +192,11 @@ class AP_OADijkstra { Vector2f _path_source; // source point used in shortest path calculations (offset in cm from EKF origin) Vector2f _path_destination; // destination position used in shortest path calculations (offset in cm from EKF origin) + // return number of points on path + uint8_t get_shortest_path_numpoints() const { return _path_numpoints; } + // return point from final path as an offset (in cm) from the ekf origin - bool get_shortest_path_point(uint8_t point_num, Vector2f& pos); + bool get_shortest_path_point(uint8_t point_num, Vector2f& pos) const; // find the position of a node as an offset (in cm) from the ekf origin // returns true if successful and pos is updated @@ -191,9 +205,14 @@ class AP_OADijkstra { AP_OADijkstra_Error _error_last_id; // last error id sent to GCS uint32_t _error_last_report_ms; // last time an error message was sent to GCS +#if HAL_LOGGING_ENABLED // Logging functions void Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const; void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const; +#else + void Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const {} + void Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const {} +#endif uint8_t _log_num_points; uint8_t _log_visgraph_version; diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp index 3913cc71921022..5e2b0f7e6c0026 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.cpp +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.cpp @@ -59,7 +59,7 @@ const AP_Param::GroupInfo AP_OAPathPlanner::var_info[] = { // @DisplayName: Options while recovering from Object Avoidance // @Description: Bitmask which will govern vehicles behaviour while recovering from Obstacle Avoidance (i.e Avoidance is turned off after the path ahead is clear). // @Bitmask{Rover}: 0: Reset the origin of the waypoint to the present location, 1: log Dijkstra points - // @Bitmask{Copter}: 1: log Dijkstra points + // @Bitmask{Copter}: 1:log Dijkstra points, 2:Allow fast waypoints (Dijkastras only) // @User: Standard AP_GROUPINFO("OPTIONS", 5, AP_OAPathPlanner, _options, OA_OPTIONS_DEFAULT), @@ -186,12 +186,17 @@ AP_OAPathPlanner::OAPathPlannerUsed AP_OAPathPlanner::map_bendytype_to_pathplann } // provides an alternative target location if path planning around obstacles is required -// returns true and updates result_loc with an intermediate location +// returns true and updates result_origin, result_destination, result_next_destination with an intermediate path +// result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear (only supported by Dijkstras) +// path_planner_used updated with which path planner produced the result AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location ¤t_loc, const Location &origin, const Location &destination, + const Location &next_destination, Location &result_origin, Location &result_destination, + Location &result_next_destination, + bool &result_dest_to_next_dest_clear, OAPathPlannerUsed &path_planner_used) { // exit immediately if disabled or thread is not running from a failed init @@ -199,27 +204,38 @@ AP_OAPathPlanner::OA_RetState AP_OAPathPlanner::mission_avoidance(const Location return OA_NOT_REQUIRED; } + // check if just activated to avoid initial timeout error const uint32_t now = AP_HAL::millis(); + if (now - _last_update_ms > 200) { + _activated_ms = now; + } + _last_update_ms = now; + WITH_SEMAPHORE(_rsem); // place new request for the thread to work on avoidance_request.current_loc = current_loc; avoidance_request.origin = origin; avoidance_request.destination = destination; + avoidance_request.next_destination = next_destination; avoidance_request.ground_speed_vec = AP::ahrs().groundspeed_vector(); avoidance_request.request_time_ms = now; - // check result's destination matches our request - const bool destination_matches = (destination.lat == avoidance_result.destination.lat) && (destination.lng == avoidance_result.destination.lng); + // check result's destination and next_destination matches our request + // e.g. check this result was using our current inputs and not from an old request + const bool destination_matches = destination.same_latlon_as(avoidance_result.destination); + const bool next_destination_matches = next_destination.same_latlon_as(avoidance_result.next_destination); // check results have not timed out - const bool timed_out = now - avoidance_result.result_time_ms > OA_TIMEOUT_MS; + const bool timed_out = (now - avoidance_result.result_time_ms > OA_TIMEOUT_MS) && (now - _activated_ms > OA_TIMEOUT_MS); // return results from background thread's latest checks - if (destination_matches && !timed_out) { + if (destination_matches && next_destination_matches && !timed_out) { // we have a result from the thread result_origin = avoidance_result.origin_new; result_destination = avoidance_result.destination_new; + result_next_destination = avoidance_result.next_destination_new; + result_dest_to_next_dest_clear = avoidance_result.dest_to_next_dest_clear; path_planner_used = avoidance_result.path_planner_used; return avoidance_result.ret_state; } @@ -264,8 +280,11 @@ void AP_OAPathPlanner::avoidance_thread() _oadatabase.update(); + // values returned by path planners Location origin_new; Location destination_new; + Location next_destination_new; + bool dest_to_next_dest_clear = false; { WITH_SEMAPHORE(_rsem); if (now - avoidance_request.request_time_ms > OA_TIMEOUT_MS) { @@ -276,9 +295,10 @@ void AP_OAPathPlanner::avoidance_thread() // copy request to avoid conflict with main thread avoidance_request2 = avoidance_request; - // store passed in origin and destination so we can return it if object avoidance is not required + // store passed in origin, destination and next_destination so we can return it if object avoidance is not required origin_new = avoidance_request.origin; destination_new = avoidance_request.destination; + next_destination_new = avoidance_request.next_destination; } // run background task looking for best alternative destination @@ -307,7 +327,13 @@ void AP_OAPathPlanner::avoidance_thread() continue; } _oadijkstra->set_fence_margin(_margin_max); - const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new); + const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, + avoidance_request2.destination, + avoidance_request2.next_destination, + origin_new, + destination_new, + next_destination_new, + dest_to_next_dest_clear); switch (dijkstra_state) { case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED: res = OA_NOT_REQUIRED; @@ -348,7 +374,13 @@ void AP_OAPathPlanner::avoidance_thread() } #if AP_FENCE_ENABLED _oadijkstra->set_fence_margin(_margin_max); - const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, avoidance_request2.destination, origin_new, destination_new); + const AP_OADijkstra::AP_OADijkstra_State dijkstra_state = _oadijkstra->update(avoidance_request2.current_loc, + avoidance_request2.destination, + avoidance_request2.next_destination, + origin_new, + destination_new, + next_destination_new, + dest_to_next_dest_clear); switch (dijkstra_state) { case AP_OADijkstra::DIJKSTRA_STATE_NOT_REQUIRED: res = OA_NOT_REQUIRED; @@ -370,9 +402,18 @@ void AP_OAPathPlanner::avoidance_thread() { // give the main thread the avoidance result WITH_SEMAPHORE(_rsem); + + // place the destination and next destination used into the result (used by the caller to verify the result matches their request) avoidance_result.destination = avoidance_request2.destination; + avoidance_result.next_destination = avoidance_request2.next_destination; + avoidance_result.dest_to_next_dest_clear = dest_to_next_dest_clear; + + // fill the result structure with the intermediate path avoidance_result.origin_new = (res == OA_SUCCESS) ? origin_new : avoidance_result.origin_new; avoidance_result.destination_new = (res == OA_SUCCESS) ? destination_new : avoidance_result.destination; + avoidance_result.next_destination_new = (res == OA_SUCCESS) ? next_destination_new : avoidance_result.next_destination; + + // create new avoidance result.dest_to_next_dest_clear field. fill in with results from dijkstras or leave as unknown avoidance_result.result_time_ms = AP_HAL::millis(); avoidance_result.path_planner_used = path_planner_used; avoidance_result.ret_state = res; diff --git a/libraries/AC_Avoidance/AP_OAPathPlanner.h b/libraries/AC_Avoidance/AP_OAPathPlanner.h index ae8b0aff7dc323..3c08dc4b7690d8 100644 --- a/libraries/AC_Avoidance/AP_OAPathPlanner.h +++ b/libraries/AC_Avoidance/AP_OAPathPlanner.h @@ -48,13 +48,17 @@ class AP_OAPathPlanner { }; // provides an alternative target location if path planning around obstacles is required - // returns true and updates result_origin and result_destination with an intermediate path + // returns true and updates result_origin, result_destination and result_next_destination with an intermediate path + // result_dest_to_next_dest_clear is set to true if the path from result_destination to result_next_destination is clear (only supported by Dijkstras) // path_planner_used updated with which path planner produced the result OA_RetState mission_avoidance(const Location ¤t_loc, const Location &origin, const Location &destination, + const Location &next_destination, Location &result_origin, Location &result_destination, + Location &result_next_destination, + bool &result_dest_to_next_dest_clear, OAPathPlannerUsed &path_planner_used) WARN_IF_UNUSED; // enumerations for _TYPE parameter @@ -70,6 +74,7 @@ class AP_OAPathPlanner { OA_OPTION_DISABLED = 0, OA_OPTION_WP_RESET = (1 << 0), OA_OPTION_LOG_DIJKSTRA_POINTS = (1 << 1), + OA_OPTION_FAST_WAYPOINTS = (1 << 2), }; uint16_t get_options() const { return _options;} @@ -90,6 +95,7 @@ class AP_OAPathPlanner { Location current_loc; Location origin; Location destination; + Location next_destination; Vector2f ground_speed_vec; uint32_t request_time_ms; } avoidance_request, avoidance_request2; @@ -97,8 +103,11 @@ class AP_OAPathPlanner { // an avoidance result from the avoidance thread struct { Location destination; // destination vehicle is trying to get to (also used to verify the result matches a recent request) + Location next_destination; // next destination vehicle is trying to get to (also used to verify the result matches a recent request) Location origin_new; // intermediate origin. The start of line segment that vehicle should follow Location destination_new; // intermediate destination vehicle should move towards + Location next_destination_new; // intermediate next destination vehicle should move towards + bool dest_to_next_dest_clear; // true if the path from destination_new to next_destination_new is clear and does not require path planning (only supported by Dijkstras) uint32_t result_time_ms; // system time the result was calculated (used to verify the result is recent) OAPathPlannerUsed path_planner_used; // path planner that produced the result OA_RetState ret_state; // OA_SUCCESS if the vehicle should move along the path from origin_new to destination_new @@ -115,7 +124,9 @@ class AP_OAPathPlanner { AP_OABendyRuler *_oabendyruler; // Bendy Ruler algorithm AP_OADijkstra *_oadijkstra; // Dijkstra's algorithm AP_OADatabase _oadatabase; // Database of dynamic objects to avoid - uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran + uint32_t avoidance_latest_ms; // last time Dijkstra's or BendyRuler algorithms ran (in the avoidance thread) + uint32_t _last_update_ms; // system time that mission_avoidance was called in main thread + uint32_t _activated_ms; // system time that object avoidance was most recently activated (used to avoid timeout error on first run) bool proximity_only = true; static AP_OAPathPlanner *_singleton; diff --git a/libraries/AC_Avoidance/LogStructure.h b/libraries/AC_Avoidance/LogStructure.h index 585eca825182e3..1099d1c0e8024d 100644 --- a/libraries/AC_Avoidance/LogStructure.h +++ b/libraries/AC_Avoidance/LogStructure.h @@ -108,10 +108,10 @@ struct PACKED log_OD_Visgraph { #define LOG_STRUCTURE_FROM_AVOIDANCE \ { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \ - "OABR","QBBHHHBfLLiLLi","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s-bddd-mDUmDUm", "F-------GGBGGB" , true }, \ + "OABR","QBBHHHBfLLiLLi","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s--ddd-mDUmDUm", "F-------GGBGGB" , true }, \ { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \ - "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" , true }, \ + "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "s----DUDU", "F----GGGG" , true }, \ { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \ - "SA", "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "sbnnnnnnb", "F--------", true }, \ + "SA", "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "s-nnnnnn-", "F--------", true }, \ { LOG_OD_VISGRAPH_MSG, sizeof(log_OD_Visgraph), \ "OAVG", "QBBLL", "TimeUS,version,point_num,Lat,Lon", "s--DU", "F--GG", true}, diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index aaec44932bd4ed..ac9d7f50a031ab 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -128,7 +128,7 @@ const AP_Param::GroupInfo AC_Fence::var_info[] = { // @Param{Plane}: AUTOENABLE // @DisplayName: Fence Auto-Enable - // @Description: Auto-enable of fence + // @Description: Auto-enable of fences. AutoEnableOnTakeoff enables all configured fences after autotakeoffs reach altitude. During autolandings the fences will be disabled. AutoEnableDisableFloorOnLanding enables all configured fences after autotakeoffs reach altitude. During autolandings only the Minimum Altitude fence will be disabled. AutoEnableOnlyWhenArmed enables all configured fences, but no fences are disabled during autolandings. However, fence breaches are ignored while executing prior breach recovery actions which may include autolandings. // @Values: 0:AutoEnableOff,1:AutoEnableOnTakeoff,2:AutoEnableDisableFloorOnLanding,3:AutoEnableOnlyWhenArmed // @Range: 0 3 // @Increment: 1 @@ -160,11 +160,13 @@ AC_Fence::AC_Fence() /// enable the Fence code generally; a master switch for all fences void AC_Fence::enable(bool value) { +#if HAL_LOGGING_ENABLED if (_enabled && !value) { AP::logger().Write_Event(LogEvent::FENCE_DISABLE); } else if (!_enabled && value) { AP::logger().Write_Event(LogEvent::FENCE_ENABLE); } +#endif _enabled.set(value); if (!value) { clear_breach(AC_FENCE_TYPE_ALT_MIN | AC_FENCE_TYPE_ALT_MAX | AC_FENCE_TYPE_CIRCLE | AC_FENCE_TYPE_POLYGON); @@ -177,19 +179,23 @@ void AC_Fence::enable(bool value) /// enable/disable fence floor only void AC_Fence::enable_floor() { +#if HAL_LOGGING_ENABLED if (!_floor_enabled) { // Floor is currently disabled, enable it AP::logger().Write_Event(LogEvent::FENCE_FLOOR_ENABLE); } +#endif _floor_enabled = true; } void AC_Fence::disable_floor() { +#if HAL_LOGGING_ENABLED if (_floor_enabled) { // Floor is currently enabled, disable it AP::logger().Write_Event(LogEvent::FENCE_FLOOR_DISABLE); } +#endif _floor_enabled = false; clear_breach(AC_FENCE_TYPE_ALT_MIN); } diff --git a/libraries/AC_Fence/AC_PolyFence_loader.cpp b/libraries/AC_Fence/AC_PolyFence_loader.cpp index 1b42ded2738094..12ddba86bbb5a4 100644 --- a/libraries/AC_Fence/AC_PolyFence_loader.cpp +++ b/libraries/AC_Fence/AC_PolyFence_loader.cpp @@ -1121,8 +1121,10 @@ bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_ gcs().send_text(MAV_SEVERITY_DEBUG, "Fence Indexed OK"); #endif +#if HAL_LOGGING_ENABLED // start logger logging new fence AP::logger().Write_Fence(); +#endif void_index(); diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 557b6b5bfbf146..166be7b7b3122b 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -198,6 +198,7 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit, } // reset input filter to value received + _pid_info.reset = _flags._reset_filter; if (_flags._reset_filter) { _flags._reset_filter = false; _target = target; @@ -319,11 +320,15 @@ void AC_PID::update_i(float dt, bool limit) } _pid_info.I = _integrator; _pid_info.limit = limit; + + // Set I set flag for logging and clear + _pid_info.I_term_set = _flags._I_set; + _flags._I_set = false; } float AC_PID::get_p() const { - return _error * _kp; + return _pid_info.P; } float AC_PID::get_i() const @@ -333,16 +338,17 @@ float AC_PID::get_i() const float AC_PID::get_d() const { - return _kd * _derivative; + return _pid_info.D; } -float AC_PID::get_ff() +float AC_PID::get_ff() const { return _pid_info.FF + _pid_info.DFF; } void AC_PID::reset_I() { + _flags._I_set = true; _integrator = 0.0; } @@ -402,18 +408,9 @@ float AC_PID::get_filt_D_alpha(float dt) const return calc_lowpass_alpha_dt(dt, _filt_D_hz); } -void AC_PID::set_integrator(float target, float measurement, float integrator) -{ - set_integrator(target - measurement, integrator); -} - -void AC_PID::set_integrator(float error, float integrator) -{ - _integrator = constrain_float(integrator - error * _kp, -_kimax, _kimax); -} - void AC_PID::set_integrator(float integrator) { + _flags._I_set = true; _integrator = constrain_float(integrator, -_kimax, _kimax); } @@ -421,6 +418,7 @@ void AC_PID::relax_integrator(float integrator, float dt, float time_constant) { integrator = constrain_float(integrator, -_kimax, _kimax); if (is_positive(dt)) { + _flags._I_set = true; _integrator = _integrator + (integrator - _integrator) * (dt / (dt + time_constant)); } } diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index 0aa7e35e2c9508..fbbebcac7db783 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -72,17 +72,11 @@ class AC_PID { // todo: remove function when it is no longer used. float update_error(float error, float dt, bool limit = false); - // update_i - update the integral - // if the limit flag is set the integral is only allowed to shrink - void update_i(float dt, bool limit); - // get_pid - get results from pid controller - float get_pid() const; - float get_pi() const; float get_p() const; float get_i() const; float get_d() const; - float get_ff(); + float get_ff() const; // reset_I - reset the integrator void reset_I(); @@ -113,9 +107,11 @@ class AC_PID { AP_Float &filt_E_hz() { return _filt_E_hz; } AP_Float &filt_D_hz() { return _filt_D_hz; } AP_Float &slew_limit() { return _slew_rate_max; } + AP_Float &kDff() { return _kdff; } float imax() const { return _kimax.get(); } float pdmax() const { return _kpdmax.get(); } + float get_filt_T_alpha(float dt) const; float get_filt_E_alpha(float dt) const; float get_filt_D_alpha(float dt) const; @@ -131,14 +127,13 @@ class AC_PID { void filt_E_hz(const float v); void filt_D_hz(const float v); void slew_limit(const float v); + void kDff(const float v) { _kdff.set(v); } // set the desired and actual rates (for logging purposes) void set_target_rate(float target) { _pid_info.target = target; } void set_actual_rate(float actual) { _pid_info.actual = actual; } // integrator setting functions - void set_integrator(float target, float measurement, float i); - void set_integrator(float error, float i); void set_integrator(float i); void relax_integrator(float integrator, float dt, float time_constant); @@ -150,20 +145,17 @@ class AC_PID { const AP_PIDInfo& get_pid_info(void) const { return _pid_info; } - AP_Float &kDff() { return _kdff; } - void kDff(const float v) { _kdff.set(v); } void set_notch_sample_rate(float); + // parameter var table static const struct AP_Param::GroupInfo var_info[]; - // the time constant tau is not currently configurable, but is set - // as an AP_Float to make it easy to make it configurable for a - // single user of AC_PID by adding the parameter in the param - // table of the parent class. It is made public for this reason - AP_Float _slew_rate_tau; - protected: + // update_i - update the integral + // if the limit flag is set the integral is only allowed to shrink + void update_i(float dt, bool limit); + // parameters AP_Float _kp; AP_Float _ki; @@ -180,11 +172,19 @@ class AC_PID { AP_Int8 _notch_T_filter; AP_Int8 _notch_E_filter; #endif + + // the time constant tau is not currently configurable, but is set + // as an AP_Float to make it easy to make it configurable for a + // single user of AC_PID by adding the parameter in the param + // table of the parent class. It is made public for this reason + AP_Float _slew_rate_tau; + SlewLimiter _slew_limiter{_slew_rate_max, _slew_rate_tau}; // flags struct ac_pid_flags { bool _reset_filter :1; // true when input filter should be reset during next call to set_input + bool _I_set :1; // true if if the I terms has been set externally including zeroing } _flags; // internal variables diff --git a/libraries/AC_PID/AP_PIDInfo.h b/libraries/AC_PID/AP_PIDInfo.h index 70afb0974e01ce..992a287ec8bc5c 100644 --- a/libraries/AC_PID/AP_PIDInfo.h +++ b/libraries/AC_PID/AP_PIDInfo.h @@ -19,4 +19,6 @@ struct AP_PIDInfo { float slew_rate; bool limit; bool PD_limit; + bool reset; + bool I_term_set; }; diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 57223e1f666c93..49c7a11060a90a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -310,11 +310,13 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) // check the status of the landing target location check_target_status(rangefinder_alt_m, rangefinder_alt_valid); +#if HAL_LOGGING_ENABLED const uint32_t now = AP_HAL::millis(); if (now - last_log_ms > 40) { // 25Hz last_log_ms = now; Write_Precland(); } +#endif } // check the status of the target @@ -740,6 +742,7 @@ void AC_PrecLand::run_output_prediction() _last_valid_target_ms = AP_HAL::millis(); } +#if HAL_LOGGING_ENABLED // Write a precision landing entry void AC_PrecLand::Write_Precland() { @@ -773,6 +776,7 @@ void AC_PrecLand::Write_Precland() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif // singleton instance AC_PrecLand *AC_PrecLand::_singleton; diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index ca804a54916c94..b1f88a40947f3b 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -121,7 +121,7 @@ void AC_Circle::set_center(const Location& center) } else { // failed to convert location so set to current position and log error set_center(_inav.get_position_neu_cm(), false); - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } } else { // convert Location with alt-above-home, alt-above-origin or absolute alt @@ -129,7 +129,7 @@ void AC_Circle::set_center(const Location& center) if (!center.get_vector_from_origin_NEU(circle_center_neu)) { // default to current position and log error circle_center_neu = _inav.get_position_neu_cm(); - AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); + LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } set_center(circle_center_neu, false); } diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 116e85911c0e2f..be5c7ac97873d5 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -94,7 +94,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Param: ACCEL_C // @DisplayName: Waypoint Cornering Acceleration - // @Description: Defines the maximum cornering acceleration in cm/s/s used during missions, zero uses max lean angle. + // @Description: Defines the maximum cornering acceleration in cm/s/s used during missions. If zero uses 2x accel value. // @Units: cm/s/s // @Range: 0 500 // @Increment: 10 @@ -350,6 +350,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) _this_leg_is_spline = false; _scurve_next_leg.init(); + _next_destination.zero(); // clear next destination _flags.fast_waypoint = false; // default waypoint back to slow _flags.reached_destination = false; @@ -380,6 +381,9 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain // next destination provided so fast waypoint _flags.fast_waypoint = true; + // record next destination + _next_destination = destination; + return true; } @@ -623,6 +627,29 @@ bool AC_WPNav::is_active() const return (AP_HAL::millis() - _wp_last_update) < 200; } +// force stopping at next waypoint. Used by Dijkstra's object avoidance when path from destination to next destination is not clear +// only affects regular (e.g. non-spline) waypoints +// returns true if this had any affect on the path +bool AC_WPNav::force_stop_at_next_wp() +{ + // exit immediately if vehicle was going to stop anyway + if (!_flags.fast_waypoint) { + return false; + } + + _flags.fast_waypoint = false; + + // update this_leg's final velocity and next leg's initial velocity to zero + if (!_this_leg_is_spline) { + _scurve_this_leg.set_destination_speed_max(0); + } + if (!_next_leg_is_spline) { + _scurve_next_leg.init(); + } + + return true; +} + // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) bool AC_WPNav::get_terrain_offset(float& offset_cm) { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index e4563f7ce6de16..5e2249597698d6 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -87,8 +87,8 @@ class AC_WPNav /// get_wp_acceleration - returns acceleration in cm/s/s during missions float get_wp_acceleration() const { return (is_positive(_wp_accel_cmss)) ? _wp_accel_cmss : WPNAV_ACCELERATION; } - /// get_wp_acceleration - returns acceleration in cm/s/s during missions - float get_corner_acceleration() const { return (is_positive(_wp_accel_c_cmss)) ? _wp_accel_c_cmss : get_wp_acceleration(); } + /// get_corner_acceleration - returns maximum acceleration in cm/s/s used during cornering in missions + float get_corner_acceleration() const { return (is_positive(_wp_accel_c_cmss)) ? _wp_accel_c_cmss : 2.0 * get_wp_acceleration(); } /// get_wp_destination waypoint using position vector /// x,y are distance from ekf origin in cm @@ -163,6 +163,11 @@ class AC_WPNav // returns true if update_wpnav has been run very recently bool is_active() const; + // force stopping at next waypoint. Used by Dijkstra's object avoidance when path from destination to next destination is not clear + // only affects regular (e.g. non-spline) waypoints + // returns true if this had any affect on the path + bool force_stop_at_next_wp(); + /// /// spline methods /// @@ -273,6 +278,7 @@ class AC_WPNav float _wp_desired_speed_xy_cms; // desired wp speed in cm/sec Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin Vector3f _destination; // target destination in cm from ekf origin + Vector3f _next_destination; // next target destination in cm from ekf origin float _track_scalar_dt; // time compression multiplier to slow the progress along the track float _offset_vel; // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain float _offset_accel; // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index 10e56d51ecfb95..b0ab5502b39a93 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -77,26 +77,60 @@ bool AC_WPNav_OA::update_wpnav() _origin_oabak = _origin; _destination_oabak = _destination; _terrain_alt_oabak = _terrain_alt; + _next_destination_oabak = _next_destination; } - // convert origin and destination to Locations and pass into oa + // convert origin, destination and next_destination to Locations and pass into oa const Location origin_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); const Location destination_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - Location oa_origin_new, oa_destination_new; + const Location next_destination_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location oa_origin_new, oa_destination_new, oa_next_destination_new; + bool dest_to_next_dest_clear = true; AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None; - const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, origin_loc, destination_loc, oa_origin_new, oa_destination_new, path_planner_used); + const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, + origin_loc, + destination_loc, + next_destination_loc, + oa_origin_new, + oa_destination_new, + oa_next_destination_new, + dest_to_next_dest_clear, + path_planner_used); switch (oa_retstate) { case AP_OAPathPlanner::OA_NOT_REQUIRED: if (_oa_state != oa_retstate) { // object avoidance has become inactive so reset target to original destination - set_wp_destination(_destination_oabak, _terrain_alt_oabak); + if (!set_wp_destination(_destination_oabak, _terrain_alt_oabak)) { + // trigger terrain failsafe + return false; + } + + // if path from destination to next_destination is clear + if (dest_to_next_dest_clear && (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS)) { + // set next destination if non-zero + if (!_next_destination_oabak.is_zero()) { + set_wp_destination_next(_next_destination_oabak); + } + } _oa_state = oa_retstate; } + + // ensure we stop at next waypoint + // Note that this check is run on every iteration even if the path planner is not active + if (!dest_to_next_dest_clear) { + force_stop_at_next_wp(); + } break; case AP_OAPathPlanner::OA_PROCESSING: + if (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) { + // if fast waypoint option is set, proceed during processing + break; + } + FALLTHROUGH; + case AP_OAPathPlanner::OA_ERROR: // during processing or in case of error stop the vehicle // by setting the oa_destination to a stopping point @@ -105,6 +139,7 @@ bool AC_WPNav_OA::update_wpnav() Vector3f stopping_point; get_wp_stopping_point(stopping_point); _oa_destination = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN); + _oa_next_destination.zero(); if (set_wp_destination(stopping_point, false)) { _oa_state = oa_retstate; } @@ -127,6 +162,8 @@ bool AC_WPNav_OA::update_wpnav() Location origin_oabak_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); Location destination_oabak_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); + + // set new OA adjusted destination if (!set_wp_destination_loc(oa_destination_new)) { // trigger terrain failsafe return false; @@ -134,6 +171,17 @@ bool AC_WPNav_OA::update_wpnav() // if new target set successfully, update oa state and destination _oa_state = oa_retstate; _oa_destination = oa_destination_new; + + // if a next destination was provided then use it + if ((oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) && !oa_next_destination_new.is_zero()) { + // calculate oa_next_destination_new's altitude using linear interpolation between original origin and destination + // this "next destination" is still an intermediate point between the origin and destination + Location next_destination_oabak_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + oa_next_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); + if (set_wp_destination_next_loc(oa_next_destination_new)) { + _oa_next_destination = oa_next_destination_new; + } + } } break; diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h index cf532de016c124..2687c925521fc9 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.h +++ b/libraries/AC_WPNav/AC_WPNav_OA.h @@ -40,6 +40,8 @@ class AC_WPNav_OA : public AC_WPNav AP_OAPathPlanner::OA_RetState _oa_state; // state of object avoidance, if OA_SUCCESS we use _oa_destination to avoid obstacles Vector3f _origin_oabak; // backup of _origin so it can be restored when oa completes Vector3f _destination_oabak; // backup of _destination so it can be restored when oa completes + Vector3f _next_destination_oabak;// backup of _next_destination so it can be restored when oa completes bool _terrain_alt_oabak; // true if backup origin and destination z-axis are terrain altitudes Location _oa_destination; // intermediate destination during avoidance + Location _oa_next_destination; // intermediate next destination during avoidance }; diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index 87c84ee94a1c34..d1d817cb3d8636 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -208,10 +208,10 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) float att_limit_deg = 0; switch (type) { case AUTOTUNE_ROLL: - att_limit_deg = aparm.roll_limit_cd * 0.01; + att_limit_deg = aparm.roll_limit; break; case AUTOTUNE_PITCH: - att_limit_deg = MIN(abs(aparm.pitch_limit_max_cd),abs(aparm.pitch_limit_min_cd))*0.01; + att_limit_deg = MIN(abs(aparm.pitch_limit_max*100),abs(aparm.pitch_limit_min*100))*0.01; break; case AUTOTUNE_YAW: // arbitrary value for yaw angle @@ -247,6 +247,7 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) const uint32_t now = AP_HAL::millis(); +#if HAL_LOGGING_ENABLED if (now - last_log_ms >= 40) { // log at 25Hz struct log_ATRP pkt = { @@ -269,6 +270,7 @@ void AP_AutoTune::update(AP_PIDInfo &pinfo, float scaler, float angle_err_deg) AP::logger().WriteBlock(&pkt, sizeof(pkt)); last_log_ms = now; } +#endif if (new_state == state) { if (state == ATState::IDLE && diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 1c02fdb0a86bf9..0dda072cdd3302 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -273,7 +273,7 @@ float AP_PitchController::get_rate_out(float desired_rate, float scaler) float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const { float rate_offset; - float bank_angle = AP::ahrs().roll; + float bank_angle = AP::ahrs().get_roll(); // limit bank angle between +- 80 deg if right way up if (fabsf(bank_angle) < radians(90)) { @@ -296,7 +296,7 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv // don't do turn coordination handling when at very high pitch angles rate_offset = 0; } else { - rate_offset = cosf(_ahrs.pitch)*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()), MAX(aparm.airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; + rate_offset = cosf(_ahrs.get_pitch())*fabsf(ToDeg((GRAVITY_MSS / MAX((aspeed * _ahrs.get_EAS2TAS()), MAX(aparm.airspeed_min, 1))) * tanf(bank_angle) * sinf(bank_angle))) * _roll_ff; } if (inverted) { rate_offset = -rate_offset; @@ -361,7 +361,7 @@ float AP_PitchController::get_servo_out(int32_t angle_err, float scaler, bool di if (roll_wrapped > 9000) { roll_wrapped = 18000 - roll_wrapped; } - const float roll_limit_margin = MIN(aparm.roll_limit_cd + 500.0, 8500.0); + const float roll_limit_margin = MIN(aparm.roll_limit*100 + 500.0, 8500.0); if (roll_wrapped > roll_limit_margin && labs(_ahrs.pitch_sensor) < 7000) { float roll_prop = (roll_wrapped - roll_limit_margin) / (float)(9000 - roll_limit_margin); desired_rate *= (1 - roll_prop); diff --git a/libraries/APM_Control/AP_YawController.cpp b/libraries/APM_Control/AP_YawController.cpp index f99333aca1e7e2..15f42c2fb505bd 100644 --- a/libraries/APM_Control/AP_YawController.cpp +++ b/libraries/APM_Control/AP_YawController.cpp @@ -206,7 +206,7 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator) // Calculate yaw rate required to keep up with a constant height coordinated turn float aspeed; float rate_offset; - float bank_angle = AP::ahrs().roll; + float bank_angle = AP::ahrs().get_roll(); // limit bank angle between +- 80 deg if right way up if (fabsf(bank_angle) < 1.5707964f) { bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f); diff --git a/libraries/APM_Control/AR_AttitudeControl.cpp b/libraries/APM_Control/AR_AttitudeControl.cpp index 67f12c20e946af..c537a05868774a 100644 --- a/libraries/APM_Control/AR_AttitudeControl.cpp +++ b/libraries/APM_Control/AR_AttitudeControl.cpp @@ -613,7 +613,7 @@ float AR_AttitudeControl::get_steering_out_heading(float heading_rad, float rate // return a desired turn-rate given a desired heading in radians float AR_AttitudeControl::get_turn_rate_from_heading(float heading_rad, float rate_max_rads) const { - const float yaw_error = wrap_PI(heading_rad - AP::ahrs().yaw); + const float yaw_error = wrap_PI(heading_rad - AP::ahrs().get_yaw()); // Calculate the desired turn rate (in radians) from the angle error (also in radians) float desired_rate = _steer_angle_p.get_p(yaw_error); @@ -884,7 +884,7 @@ float AR_AttitudeControl::get_throttle_out_from_pitch(float desired_pitch, float } // initialise output to feed forward from current pitch angle - const float pitch_rad = AP::ahrs().pitch; + const float pitch_rad = AP::ahrs().get_pitch(); float output = sinf(pitch_rad) * _pitch_to_throttle_ff; // add regular PID control @@ -940,7 +940,7 @@ float AR_AttitudeControl::get_sail_out_from_heel(float desired_heel, float dt) } _heel_controller_last_ms = now; - _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().roll), dt); + _sailboat_heel_pid.update_all(desired_heel, fabsf(AP::ahrs().get_roll()), dt); // get feed-forward const float ff = _sailboat_heel_pid.get_ff(); diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index 4107082558054b..112c508934b609 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -363,6 +363,7 @@ void AR_PosControl::get_srate(float &velocity_srate) velocity_srate = _pid_vel.get_pid_info_x().slew_rate; } +#if HAL_LOGGING_ENABLED // write PSC logs void AR_PosControl::write_log() { @@ -401,6 +402,7 @@ void AR_PosControl::write_log() _accel_target.y * 100.0, // target accel curr_accel_NED.y); // accel } +#endif /// initialise ekf xy position reset check void AR_PosControl::init_ekf_xy_reset() diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index 2acbd81b62189f..5d1508873149ad 100644 --- a/libraries/AP_ADSB/AP_ADSB.cpp +++ b/libraries/AP_ADSB/AP_ADSB.cpp @@ -37,7 +37,7 @@ #include #include #include - +#include #define VEHICLE_TIMEOUT_MS 5000 // if no updates in this time, drop it from the list #define ADSB_SQUAWK_OCTAL_DEFAULT 1200 @@ -365,7 +365,9 @@ void AP_ADSB::update(void) // Altitude difference between sea level pressure and current // pressure (in metres) - loc.baro_alt_press_diff_sea_level = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure()); + if (loc.baro_is_healthy) { + loc.baro_alt_press_diff_sea_level = baro.get_altitude_difference(SSL_AIR_PRESSURE, baro.get_pressure()); + } update(loc); } @@ -679,10 +681,9 @@ void AP_ADSB::handle_out_cfg(const mavlink_uavionix_adsb_out_cfg_t &packet) out_state.cfg.stall_speed_cm = packet.stallSpeed; // guard against string with non-null end char - const char c = out_state.cfg.callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1]; - out_state.cfg.callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = 0; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", (int)out_state.cfg.ICAO_id, out_state.cfg.callsign); - out_state.cfg.callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN-1] = c; + char tmp_callsign[MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN+1] {}; + memcpy(tmp_callsign, out_state.cfg.callsign, MAVLINK_MSG_UAVIONIX_ADSB_OUT_CFG_FIELD_CALLSIGN_LEN); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ADSB: Using ICAO_id %d and Callsign %s", (int)out_state.cfg.ICAO_id, tmp_callsign); // send now out_state.last_config_ms = 0; diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp index c7fc9990b30446..d4964ba131b682 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.cpp @@ -324,38 +324,13 @@ void AP_ADSB_Sagetech_MXS::auto_config_operating() mxs_state.op.altHostAvlbl = false; mxs_state.op.altRes25 = !mxs_state.inst.altRes100; // Host Altitude Resolution from install - int32_t height; - if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) { - mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet - } else { - mxs_state.op.altitude = 0; - } - mxs_state.op.identOn = false; const auto &my_loc = _frontend._my_loc; - float vertRateD; - if (my_loc.get_vert_pos_rate_D(vertRateD)) { - // convert from down to up, and scale appropriately: - mxs_state.op.climbRate = -1 * vertRateD * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN; - mxs_state.op.climbValid = true; - } else { - mxs_state.op.climbValid = false; - mxs_state.op.climbRate = -CLIMB_RATE_LIMIT; - } - const Vector2f speed = my_loc.groundspeed_vector(); - if (!speed.is_nan() && !speed.is_zero()) { - mxs_state.op.headingValid = true; - mxs_state.op.airspdValid = true; - } else { - mxs_state.op.headingValid = false; - mxs_state.op.airspdValid = false; - } - const uint16_t speed_knots = speed.length() * M_PER_SEC_TO_KNOTS; - double heading = wrap_360(degrees(speed.angle())); - mxs_state.op.airspd = speed_knots; - mxs_state.op.heading = heading; + populate_op_altitude(my_loc); + populate_op_climbrate(my_loc); + populate_op_airspeed_and_heading(my_loc); last.msg.type = SG_MSG_TYPE_HOST_OPMSG; @@ -587,35 +562,9 @@ void AP_ADSB_Sagetech_MXS::send_operating_msg() const auto &my_loc = _frontend._my_loc; - int32_t height; - if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) { - mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet - } else { - mxs_state.op.altitude = 0; - } - - float vertRateD; - if (my_loc.get_vert_pos_rate_D(vertRateD)) { - mxs_state.op.climbRate = -1 * vertRateD * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN; - mxs_state.op.climbValid = true; - } else { - mxs_state.op.climbValid = false; - mxs_state.op.climbRate = -CLIMB_RATE_LIMIT; - } - - const Vector2f speed = my_loc.groundspeed_vector(); - if (!speed.is_nan() && !speed.is_zero()) { - mxs_state.op.headingValid = true; - mxs_state.op.airspdValid = true; - } else { - mxs_state.op.headingValid = false; - mxs_state.op.airspdValid = false; - } - const uint16_t speed_knots = (speed.length() * M_PER_SEC_TO_KNOTS); - const double heading = wrap_360(degrees(speed.angle())); - - mxs_state.op.airspd = speed_knots; - mxs_state.op.heading = heading; + populate_op_altitude(my_loc); + populate_op_climbrate(my_loc); + populate_op_airspeed_and_heading(my_loc); mxs_state.op.identOn = _frontend.out_state.ctrl.identActive; _frontend.out_state.ctrl.identActive = false; // only send identButtonActive once per request @@ -692,7 +641,7 @@ void AP_ADSB_Sagetech_MXS::send_gps_msg() } int32_t height; - if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, height)) { + if (_frontend._my_loc.initialised() && _frontend._my_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) { gps.height = height * 0.01; } else { gps.height = 0.0; @@ -785,5 +734,44 @@ sg_airspeed_t AP_ADSB_Sagetech_MXS::convert_airspeed_knots_to_sg(const float max } } +void AP_ADSB_Sagetech_MXS::populate_op_altitude(const AP_ADSB::Loc &loc) +{ + int32_t height; + if (loc.initialised() && loc.get_alt_cm(Location::AltFrame::ABSOLUTE, height)) { + mxs_state.op.altitude = height * SAGETECH_SCALE_CM_TO_FEET; // Height above sealevel in feet + } else { + mxs_state.op.altitude = 0; + } +} + +void AP_ADSB_Sagetech_MXS::populate_op_climbrate(const AP_ADSB::Loc &my_loc) +{ + float vertRateD; + if (my_loc.get_vert_pos_rate_D(vertRateD)) { + // convert from down to up, and scale appropriately: + mxs_state.op.climbRate = -1 * vertRateD * SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN; + mxs_state.op.climbValid = true; + } else { + mxs_state.op.climbValid = false; + mxs_state.op.climbRate = -CLIMB_RATE_LIMIT; + } +} + +void AP_ADSB_Sagetech_MXS::populate_op_airspeed_and_heading(const AP_ADSB::Loc &my_loc) +{ + const Vector2f speed = my_loc.groundspeed_vector(); + if (!speed.is_nan() && !speed.is_zero()) { + mxs_state.op.headingValid = true; + mxs_state.op.airspdValid = true; + } else { + mxs_state.op.headingValid = false; + mxs_state.op.airspdValid = false; + } + const uint16_t speed_knots = speed.length() * M_PER_SEC_TO_KNOTS; + double heading = wrap_360(degrees(speed.angle())); + mxs_state.op.airspd = speed_knots; + mxs_state.op.heading = heading; +} + #endif // HAL_ADSB_SAGETECH_MXS_ENABLED diff --git a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h index 5ba13cfc048735..5e8c841039bfeb 100644 --- a/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h +++ b/libraries/AP_ADSB/AP_ADSB_Sagetech_MXS.h @@ -260,6 +260,12 @@ class AP_ADSB_Sagetech_MXS : public AP_ADSB_Backend { sg_flightid_t fid; sg_ack_t ack; } mxs_state; + + // helper functions for populating the operating message: + void populate_op_altitude(const struct AP_ADSB::Loc &loc); + void populate_op_climbrate(const struct AP_ADSB::Loc &loc); + void populate_op_airspeed_and_heading(const struct AP_ADSB::Loc &loc); + }; #endif // HAL_ADSB_SAGETECH_MXS_ENABLED diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 312471f0e9f5b9..889272b2d438bb 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -18,6 +18,11 @@ * ArduPilot * */ + +#include "AP_AHRS_config.h" + +#if AP_AHRS_ENABLED + #include #include "AP_AHRS.h" #include "AP_AHRS_View.h" @@ -34,6 +39,9 @@ #include #include #include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#endif #define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10) #define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20) @@ -220,12 +228,12 @@ void AP_AHRS::init() } #if !HAL_NAVEKF2_AVAILABLE && HAL_NAVEKF3_AVAILABLE if (_ekf_type.get() == 2) { - _ekf_type.set(3); + _ekf_type.set(EKFType::THREE); EKF3.set_enable(true); } #elif !HAL_NAVEKF3_AVAILABLE && HAL_NAVEKF2_AVAILABLE if (_ekf_type.get() == 3) { - _ekf_type.set(2); + _ekf_type.set(EKFType::TWO); EKF2.set_enable(true); } #endif @@ -234,7 +242,7 @@ void AP_AHRS::init() // a special case to catch users who had AHRS_EKF_TYPE=2 saved and // updated to a version where EK2_ENABLE=0 if (_ekf_type.get() == 2 && !EKF2.get_enable() && EKF3.get_enable()) { - _ekf_type.set(3); + _ekf_type.set(EKFType::THREE); } #endif @@ -244,7 +252,7 @@ void AP_AHRS::init() #if AP_AHRS_DCM_ENABLED dcm.init(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED external.init(); #endif @@ -313,7 +321,7 @@ void AP_AHRS::reset_gyro_drift(void) #if AP_AHRS_DCM_ENABLED dcm.reset_gyro_drift(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED external.reset_gyro_drift(); #endif @@ -392,7 +400,7 @@ void AP_AHRS::update(bool skip_ins_update) update_SITL(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED update_external(); #endif @@ -450,7 +458,7 @@ void AP_AHRS::update(bool skip_ins_update) shortname = "SIM"; break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: shortname = "External"; break; @@ -551,6 +559,7 @@ void AP_AHRS::update_EKF2(void) if (start_time_ms == 0) { start_time_ms = AP_HAL::millis(); } +#if HAL_LOGGING_ENABLED // if we're doing Replay logging then don't allow any data // into the EKF yet. Don't allow it to block us for long. if (!hal.util->was_watchdog_reset()) { @@ -560,6 +569,7 @@ void AP_AHRS::update_EKF2(void) } } } +#endif if (AP_HAL::millis() - start_time_ms > startup_delay_ms) { _ekf2_started = EKF2.InitialiseFilter(); @@ -618,6 +628,7 @@ void AP_AHRS::update_EKF3(void) if (start_time_ms == 0) { start_time_ms = AP_HAL::millis(); } +#if HAL_LOGGING_ENABLED // if we're doing Replay logging then don't allow any data // into the EKF yet. Don't allow it to block us for long. if (!hal.util->was_watchdog_reset()) { @@ -627,6 +638,7 @@ void AP_AHRS::update_EKF3(void) } } } +#endif if (AP_HAL::millis() - start_time_ms > startup_delay_ms) { _ekf3_started = EKF3.InitialiseFilter(); } @@ -677,7 +689,7 @@ void AP_AHRS::update_EKF3(void) } #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED void AP_AHRS::update_external(void) { external.update(); @@ -687,7 +699,7 @@ void AP_AHRS::update_external(void) copy_estimates_from_backend_estimates(external_estimates); } } -#endif // HAL_EXTERNAL_AHRS_ENABLED +#endif // AP_AHRS_EXTERNAL_ENABLED void AP_AHRS::reset() { @@ -701,7 +713,7 @@ void AP_AHRS::reset() sim.reset(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED external.reset(); #endif @@ -746,7 +758,7 @@ bool AP_AHRS::_get_location(Location &loc) const return sim_estimates.get_location(loc); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external_estimates.get_location(loc); #endif @@ -804,7 +816,7 @@ bool AP_AHRS::_wind_estimate(Vector3f &wind) const return EKF3.getWind(wind); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.wind_estimate(wind); #endif @@ -812,6 +824,32 @@ bool AP_AHRS::_wind_estimate(Vector3f &wind) const return false; } + +/* + * Determine how aligned heading_deg is with the wind. Return result + * is 1.0 when perfectly aligned heading into wind, -1 when perfectly + * aligned with-wind, and zero when perfect cross-wind. There is no + * distinction between a left or right cross-wind. Wind speed is ignored + */ +float AP_AHRS::wind_alignment(const float heading_deg) const +{ + Vector3f wind; + if (!wind_estimate(wind)) { + return 0; + } + const float wind_heading_rad = atan2f(-wind.y, -wind.x); + return cosf(wind_heading_rad - radians(heading_deg)); +} + +/* + * returns forward head-wind component in m/s. Negative means tail-wind. + */ +float AP_AHRS::head_wind(void) const +{ + const float alignment = wind_alignment(yaw_sensor*0.01f); + return alignment * wind_estimate().xy().length(); +} + /* return true if the current AHRS airspeed estimate is directly derived from an airspeed sensor */ @@ -908,7 +946,7 @@ bool AP_AHRS::_airspeed_estimate(float &airspeed_ret, AirspeedEstimateType &airs break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #if AP_AHRS_DCM_ENABLED airspeed_estimate_type = AirspeedEstimateType::DCM_SYNTHETIC; @@ -962,7 +1000,7 @@ bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #endif break; @@ -999,7 +1037,7 @@ bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: break; #endif @@ -1031,7 +1069,7 @@ bool AP_AHRS::airspeed_health_data(float &innovation, float &innovationVariance, break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: break; #endif @@ -1074,7 +1112,7 @@ bool AP_AHRS::use_compass(void) return sim.use_compass(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: break; #endif @@ -1121,7 +1159,7 @@ bool AP_AHRS::_get_quaternion(Quaternion &quat) const } break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: // we assume the external AHRS isn't trimmed with the autopilot! return external.get_quaternion(quat); @@ -1172,7 +1210,7 @@ bool AP_AHRS::_get_secondary_attitude(Vector3f &eulers) const return false; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: { // External is secondary eulers[0] = external_estimates.roll_rad; @@ -1233,7 +1271,7 @@ bool AP_AHRS::_get_secondary_quaternion(Quaternion &quat) const return false; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: // External is secondary return external.get_quaternion(quat); @@ -1285,7 +1323,7 @@ bool AP_AHRS::_get_secondary_position(Location &loc) const return false; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: // External is secondary return external_estimates.get_location(loc); @@ -1325,7 +1363,7 @@ Vector2f AP_AHRS::_groundspeed_vector(void) case EKFType::SIM: return sim.groundspeed_vector(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: { return external.groundspeed_vector(); } @@ -1349,7 +1387,7 @@ float AP_AHRS::_groundspeed(void) case EKFType::THREE: #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #endif @@ -1397,7 +1435,7 @@ bool AP_AHRS::set_origin(const Location &loc) // simulation backend return false; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: // don't allow origin set with external AHRS return false; @@ -1451,7 +1489,7 @@ bool AP_AHRS::_get_velocity_NED(Vector3f &vec) const case EKFType::SIM: return sim.get_velocity_NED(vec); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.get_velocity_NED(vec); #endif @@ -1486,7 +1524,7 @@ bool AP_AHRS::get_mag_field_NED(Vector3f &vec) const case EKFType::SIM: return false; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -1518,7 +1556,7 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const case EKFType::SIM: return false; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -1553,7 +1591,7 @@ bool AP_AHRS::get_vert_pos_rate_D(float &velocity) const case EKFType::SIM: return sim.get_vert_pos_rate_D(velocity); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.get_vert_pos_rate_D(velocity); #endif @@ -1584,7 +1622,7 @@ bool AP_AHRS::get_hagl(float &height) const case EKFType::SIM: return sim.get_hagl(height); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: { return false; } @@ -1638,7 +1676,7 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const case EKFType::SIM: return sim.get_relative_position_NED_origin(vec); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: { return external.get_relative_position_NED_origin(vec); } @@ -1691,7 +1729,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const return sim.get_relative_position_NE_origin(posNE); } #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.get_relative_position_NE_origin(posNE); #endif @@ -1746,7 +1784,7 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const case EKFType::SIM: return sim.get_relative_position_D_origin(posD); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.get_relative_position_D_origin(posD); #endif @@ -1798,7 +1836,7 @@ AP_AHRS::EKFType AP_AHRS::ekf_type(void) const case EKFType::SIM: return type; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return type; #endif @@ -1889,7 +1927,7 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const ret = EKFType::SIM; break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: ret = EKFType::EXTERNAL; break; @@ -1898,35 +1936,38 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const #if AP_AHRS_DCM_ENABLED // Handle fallback for fixed wing planes (including VTOL's) and ground vehicles. - if (ret != EKFType::DCM && - (_vehicle_class == VehicleClass::FIXED_WING || - _vehicle_class == VehicleClass::GROUND)) { - + if (_vehicle_class == VehicleClass::FIXED_WING || + _vehicle_class == VehicleClass::GROUND) { bool should_use_gps = true; - nav_filter_status filt_state; + nav_filter_status filt_state {}; + switch (ret) { + case EKFType::DCM: + // already using DCM + break; #if HAL_NAVEKF2_AVAILABLE - if (ret == EKFType::TWO) { + case EKFType::TWO: EKF2.getFilterStatus(filt_state); should_use_gps = EKF2.configuredToUseGPSForPosXY(); - } + break; #endif #if HAL_NAVEKF3_AVAILABLE - if (ret == EKFType::THREE) { + case EKFType::THREE: EKF3.getFilterStatus(filt_state); should_use_gps = EKF3.configuredToUseGPSForPosXY(); - } + break; #endif #if AP_AHRS_SIM_ENABLED - if (ret == EKFType::SIM) { + case EKFType::SIM: get_filter_status(filt_state); - } + break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED - if (ret == EKFType::EXTERNAL) { +#if AP_AHRS_EXTERNAL_ENABLED + case EKFType::EXTERNAL: get_filter_status(filt_state); should_use_gps = true; - } + break; #endif + } // Handle fallback for the case where the DCM or EKF is unable to provide attitude or height data. const bool can_use_dcm = dcm.yaw_source_available() || fly_forward; @@ -1948,6 +1989,7 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const // Handle loss of global position when we still have a GPS fix if (hal.util->get_soft_armed() && + (_gps_use != GPSUse::Disable) && should_use_gps && AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D && (!filt_state.flags.using_gps || !filt_state.flags.horiz_pos_abs)) { @@ -2015,7 +2057,7 @@ AP_AHRS::EKFType AP_AHRS::fallback_active_EKF_type(void) const } #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED if (external.healthy()) { return EKFType::EXTERNAL; } @@ -2026,7 +2068,7 @@ AP_AHRS::EKFType AP_AHRS::fallback_active_EKF_type(void) const return EKFType::THREE; #elif HAL_NAVEKF2_AVAILABLE return EKFType::TWO; -#elif HAL_EXTERNAL_AHRS_ENABLED +#elif AP_AHRS_EXTERNAL_ENABLED return EKFType::EXTERNAL; #endif } @@ -2051,7 +2093,7 @@ bool AP_AHRS::_get_secondary_EKF_type(EKFType &secondary_ekf_type) const return true; } #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED if ((EKFType)_ekf_type.get() == EKFType::EXTERNAL) { secondary_ekf_type = EKFType::EXTERNAL; return true; @@ -2068,7 +2110,7 @@ bool AP_AHRS::_get_secondary_EKF_type(EKFType &secondary_ekf_type) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #endif // DCM is secondary @@ -2133,7 +2175,7 @@ bool AP_AHRS::healthy(void) const case EKFType::SIM: return sim.healthy(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.healthy(); #endif @@ -2154,7 +2196,7 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f ret = false; } -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED // Always check external AHRS if enabled // it is a source for IMU data even if not being used as direct AHRS replacement if (AP::externalAHRS().enabled() || (ekf_type() == EKFType::EXTERNAL)) { @@ -2185,7 +2227,7 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f return dcm.pre_arm_check(requires_position, failure_msg, failure_msg_len) && ret; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.pre_arm_check(requires_position, failure_msg, failure_msg_len); #endif @@ -2239,7 +2281,7 @@ bool AP_AHRS::initialised(void) const case EKFType::SIM: return true; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.initialised(); #endif @@ -2272,7 +2314,7 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const case EKFType::SIM: return sim.get_filter_status(status); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.get_filter_status(status); #endif @@ -2369,7 +2411,7 @@ void AP_AHRS::getControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler sim.get_control_limits(ekfGndSpdLimit, ekfNavVelGainScaler); break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: // no limit on gains, large vel limit ekfGndSpdLimit = 400; @@ -2417,7 +2459,7 @@ bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const case EKFType::SIM: return sim.get_mag_offsets(mag_idx, magOffsets); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -2452,7 +2494,7 @@ void AP_AHRS::_getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const case EKFType::SIM: break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: break; #endif @@ -2596,7 +2638,7 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng) case EKFType::SIM: return sim.getLastYawResetAngle(yawAng); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.getLastYawResetAngle(yawAng); #endif @@ -2629,7 +2671,7 @@ uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos) case EKFType::SIM: return sim.getLastPosNorthEastReset(pos); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return 0; #endif @@ -2662,7 +2704,7 @@ uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const case EKFType::SIM: return sim.getLastVelNorthEastReset(vel); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return 0; #endif @@ -2695,7 +2737,7 @@ uint32_t AP_AHRS::getLastPosDownReset(float &posDelta) case EKFType::SIM: return sim.getLastPosDownReset(posDelta); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return 0; #endif @@ -2746,7 +2788,7 @@ bool AP_AHRS::resetHeightDatum(void) case EKFType::SIM: return sim.resetHeightDatum(); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -2770,7 +2812,7 @@ void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: { external.send_ekf_status_report(link); break; @@ -2813,7 +2855,7 @@ bool AP_AHRS::_get_origin(EKFType type, Location &ret) const case EKFType::SIM: return sim.get_origin(ret); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return external.get_origin(ret); #endif @@ -2858,7 +2900,7 @@ bool AP_AHRS::set_home(const Location &loc) return false; } -#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) +#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) && HAL_LOGGING_ENABLED if (!_home_is_set) { // record home is set AP::logger().Write_Event(LogEvent::SET_HOME); @@ -2868,7 +2910,9 @@ bool AP_AHRS::set_home(const Location &loc) _home = tmp; _home_is_set = true; +#if HAL_LOGGING_ENABLED Log_Write_Home_And_Origin(); +#endif // send new home and ekf origin to GCS GCS_SEND_MESSAGE(MSG_HOME); @@ -2922,7 +2966,7 @@ bool AP_AHRS::get_hgt_ctrl_limit(float& limit) const case EKFType::SIM: return sim.get_hgt_ctrl_limit(limit); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -2989,7 +3033,7 @@ bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f & return sim.get_innovations(velInnov, posInnov, magInnov, tasInnov, yawInnov); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -3015,7 +3059,7 @@ bool AP_AHRS::is_vibration_affected() const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #endif return false; @@ -3057,7 +3101,7 @@ bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3 return sim.get_variances(velVar, posVar, hgtVar, magVar, tasVar); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -3095,7 +3139,7 @@ bool AP_AHRS::get_vel_innovations_and_variances_for_source(uint8_t source, Vecto return false; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: return false; #endif @@ -3156,7 +3200,7 @@ uint8_t AP_AHRS::_get_primary_IMU_index() const case EKFType::SIM: break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: break; #endif @@ -3181,7 +3225,7 @@ int8_t AP_AHRS::_get_primary_core_index() const // we have only one core return 0; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: // we have only one core return 0; @@ -3229,7 +3273,7 @@ void AP_AHRS::check_lane_switch(void) break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: break; #endif @@ -3261,7 +3305,7 @@ void AP_AHRS::request_yaw_reset(void) break; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: break; #endif @@ -3333,7 +3377,7 @@ bool AP_AHRS::using_noncompass_for_yaw(void) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #endif return false; @@ -3360,7 +3404,7 @@ bool AP_AHRS::using_extnav_for_yaw(void) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #endif return false; @@ -3403,7 +3447,7 @@ const EKFGSF_yaw *AP_AHRS::get_yaw_estimator(void) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED case EKFType::EXTERNAL: #endif return nullptr; @@ -3517,3 +3561,5 @@ AP_AHRS &ahrs() } } + +#endif // AP_AHRS_ENABLED diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 0f5c985eeb1a70..31130035514442 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -25,6 +25,7 @@ #include +#include "AP_AHRS_Backend.h" #include #include #include // definitions shared by inertial and ekf nav filters @@ -114,6 +115,15 @@ class AP_AHRS { // return a wind estimation vector, in m/s; returns 0,0,0 on failure bool wind_estimate(Vector3f &wind) const; + // Determine how aligned heading_deg is with the wind. Return result + // is 1.0 when perfectly aligned heading into wind, -1 when perfectly + // aligned with-wind, and zero when perfect cross-wind. There is no + // distinction between a left or right cross-wind. Wind speed is ignored + float wind_alignment(const float heading_deg) const; + + // returns forward head-wind component in m/s. Negative means tail-wind + float head_wind(void) const; + // instruct DCM to update its wind estimate: void estimate_wind() { #if AP_AHRS_DCM_ENABLED @@ -421,6 +431,29 @@ class AP_AHRS { return _ekf_type; } + enum class EKFType : uint8_t { +#if AP_AHRS_DCM_ENABLED + DCM = 0, +#endif +#if HAL_NAVEKF3_AVAILABLE + THREE = 3, +#endif +#if HAL_NAVEKF2_AVAILABLE + TWO = 2, +#endif +#if AP_AHRS_SIM_ENABLED + SIM = 10, +#endif +#if AP_AHRS_EXTERNAL_ENABLED + EXTERNAL = 11, +#endif + }; + + // set the selected ekf type, for RC aux control + void set_ekf_type(EKFType ahrs_type) { + _ekf_type.set(ahrs_type); + } + // these are only out here so vehicles can reference them for parameters #if HAL_NAVEKF2_AVAILABLE NavEKF2 EKF2; @@ -516,10 +549,6 @@ class AP_AHRS { */ // roll/pitch/yaw euler angles, all in radians - float roll; - float pitch; - float yaw; - float get_roll() const { return roll; } float get_pitch() const { return pitch; } float get_yaw() const { return yaw; } @@ -649,6 +678,11 @@ class AP_AHRS { private: + // roll/pitch/yaw euler angles, all in radians + float roll; + float pitch; + float yaw; + // optional view class AP_AHRS_View *_view; @@ -668,7 +702,7 @@ class AP_AHRS { */ AP_Int8 _wind_max; AP_Int8 _board_orientation; - AP_Int8 _ekf_type; + AP_Enum _ekf_type; /* * DCM-backend parameters; it takes references to these @@ -683,23 +717,6 @@ class AP_AHRS { AP_Enum _gps_use; AP_Int8 _gps_minsats; - enum class EKFType { -#if AP_AHRS_DCM_ENABLED - DCM = 0, -#endif -#if HAL_NAVEKF3_AVAILABLE - THREE = 3, -#endif -#if HAL_NAVEKF2_AVAILABLE - TWO = 2, -#endif -#if AP_AHRS_SIM_ENABLED - SIM = 10, -#endif -#if HAL_EXTERNAL_AHRS_ENABLED - EXTERNAL = 11, -#endif - }; EKFType active_EKF_type(void) const { return state.active_EKF; } bool always_use_EKF() const { @@ -784,7 +801,7 @@ class AP_AHRS { void update_SITL(void); #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED void update_external(void); #endif @@ -986,7 +1003,7 @@ class AP_AHRS { struct AP_AHRS_Backend::Estimates sim_estimates; #endif -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED AP_AHRS_External external; struct AP_AHRS_Backend::Estimates external_estimates; #endif diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index 763716e148c694..5075f824ef44dd 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -244,6 +244,7 @@ Vector2f AP_AHRS::body_to_earth2D(const Vector2f &bf) const bf.x * _sin_yaw + bf.y * _cos_yaw); } +#if HAL_LOGGING_ENABLED // log ahrs home and EKF origin void AP_AHRS::Log_Write_Home_And_Origin() { @@ -260,6 +261,7 @@ void AP_AHRS::Log_Write_Home_And_Origin() Write_Origin(LogOriginType::ahrs_home, _home); } } +#endif // get apparent to true airspeed ratio float AP_AHRS_Backend::get_EAS2TAS(void) { diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 0fb02eae9b2761..ae71df9c0a0124 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -76,12 +76,20 @@ class AP_AHRS_Backend // get the index of the current primary accelerometer sensor virtual uint8_t get_primary_accel_index(void) const { +#if AP_INERTIALSENSOR_ENABLED return AP::ins().get_primary_accel(); +#else + return 0; +#endif } // get the index of the current primary gyro sensor virtual uint8_t get_primary_gyro_index(void) const { +#if AP_INERTIALSENSOR_ENABLED return AP::ins().get_primary_gyro(); +#else + return 0; +#endif } // Methods diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5dd7f681ba1951..b26bdeb472714a 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -101,6 +101,22 @@ AP_AHRS_DCM::update() // remember the last origin for fallback support IGNORE_RETURN(AP::ahrs().get_origin(last_origin)); + +#if HAL_LOGGING_ENABLED + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_log_ms >= 100) { + // log DCM at 10Hz + last_log_ms = now_ms; + AP::logger().WriteStreaming("DCM", "TimeUS,Roll,Pitch,Yaw", + "sddd", + "F000", + "Qfff", + AP_HAL::micros64(), + degrees(roll), + degrees(pitch), + wrap_360(degrees(yaw))); + } +#endif // HAL_LOGGING_ENABLED } void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 690ef22810bed0..0d3dedc5b54d96 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -286,6 +286,8 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { // pre-calculated trig cache: float _sin_yaw; float _cos_yaw; + + uint32_t last_log_ms; }; #endif // AP_AHRS_DCM_ENABLED diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp index 0c4be3d5bd614e..8cdf8ecb52bb03 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.cpp +++ b/libraries/AP_AHRS/AP_AHRS_External.cpp @@ -1,6 +1,6 @@ #include "AP_AHRS_External.h" -#if HAL_EXTERNAL_AHRS_ENABLED +#if AP_AHRS_EXTERNAL_ENABLED #include #include diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h index 4d63a0702907de..6ca69f73f8f4ac 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.h +++ b/libraries/AP_AHRS/AP_AHRS_External.h @@ -21,9 +21,11 @@ * */ -#include "AP_AHRS_Backend.h" +#include "AP_AHRS_config.h" + +#if AP_AHRS_EXTERNAL_ENABLED -#if HAL_EXTERNAL_AHRS_ENABLED +#include "AP_AHRS_Backend.h" class AP_AHRS_External : public AP_AHRS_Backend { public: diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 8d602b81cedd10..a5d77476238d53 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -1,3 +1,7 @@ +#include + +#if HAL_LOGGING_ENABLED + #include "AP_AHRS.h" #include @@ -184,3 +188,5 @@ void AP_AHRS_View::Write_Rate(const AP_Motors &motors, const AC_AttitudeControl AP::logger().WriteBlock(&pkt_ATSC, sizeof(pkt_ATSC)); } } + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_AHRS/AP_AHRS_config.h b/libraries/AP_AHRS/AP_AHRS_config.h index 559d0403b0566e..3b27e786065181 100644 --- a/libraries/AP_AHRS/AP_AHRS_config.h +++ b/libraries/AP_AHRS/AP_AHRS_config.h @@ -2,6 +2,7 @@ #include #include +#include #ifndef AP_AHRS_ENABLED #define AP_AHRS_ENABLED 1 @@ -15,20 +16,24 @@ #define AP_AHRS_DCM_ENABLED AP_AHRS_BACKEND_DEFAULT_ENABLED && AP_INERTIALSENSOR_ENABLED #endif +#ifndef AP_AHRS_EXTERNAL_ENABLED +#define AP_AHRS_EXTERNAL_ENABLED AP_AHRS_BACKEND_DEFAULT_ENABLED && HAL_EXTERNAL_AHRS_ENABLED +#endif + #ifndef HAL_NAVEKF2_AVAILABLE // EKF2 slated compiled out by default in 4.5, slated to be removed. #define HAL_NAVEKF2_AVAILABLE 0 #endif #ifndef HAL_NAVEKF3_AVAILABLE -#define HAL_NAVEKF3_AVAILABLE 1 +#define HAL_NAVEKF3_AVAILABLE AP_AHRS_BACKEND_DEFAULT_ENABLED && AP_INERTIALSENSOR_ENABLED #endif #ifndef AP_AHRS_SIM_ENABLED -#define AP_AHRS_SIM_ENABLED AP_SIM_ENABLED +#define AP_AHRS_SIM_ENABLED AP_AHRS_BACKEND_DEFAULT_ENABLED && AP_SIM_ENABLED && AP_INERTIALSENSOR_ENABLED #endif #ifndef AP_AHRS_POSITION_RESET_ENABLED -#define AP_AHRS_POSITION_RESET_ENABLED (BOARD_FLASH_SIZE>1024) +#define AP_AHRS_POSITION_RESET_ENABLED (BOARD_FLASH_SIZE>1024 && AP_AHRS_ENABLED) #endif diff --git a/libraries/AP_AHRS/LogStructure.h b/libraries/AP_AHRS/LogStructure.h index 566334393ba33b..618ade0a5c78cb 100644 --- a/libraries/AP_AHRS/LogStructure.h +++ b/libraries/AP_AHRS/LogStructure.h @@ -195,7 +195,7 @@ struct PACKED log_ATSC { #define LOG_STRUCTURE_FROM_AHRS \ { LOG_AHR2_MSG, sizeof(log_AHRS), \ - "AHR2","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4","sddhmDU????", "FBBB0GG????" , true }, \ + "AHR2","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4","sddhmDU----", "FBBB0GG----" , true }, \ { LOG_AOA_SSA_MSG, sizeof(log_AOA_SSA), \ "AOA", "Qff", "TimeUS,AOA,SSA", "sdd", "F00" , true }, \ { LOG_ATTITUDE_MSG, sizeof(log_Attitude),\ @@ -209,5 +209,5 @@ struct PACKED log_ATSC { { LOG_ATSC_MSG, sizeof(log_ATSC), \ "ATSC", "Qffffff", "TimeUS,AngPScX,AngPScY,AngPScZ,PDScX,PDScY,PDScZ", "s------", "F000000" , true }, \ { LOG_VIDEO_STABILISATION_MSG, sizeof(log_Video_Stabilisation), \ - "VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo????", "F000000????" }, + "VSTB", "Qffffffffff", "TimeUS,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,Q1,Q2,Q3,Q4", "sEEEooo----", "F0000000000" }, diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp index 38a4eaefa2becb..a4138916877693 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp +++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp @@ -84,9 +84,9 @@ void loop(void) hal.console->printf( "r:%4.1f p:%4.1f y:%4.1f " "drift=(%5.1f %5.1f %5.1f) hdg=%.1f rate=%.1f\n", - (double)ToDeg(ahrs.roll), - (double)ToDeg(ahrs.pitch), - (double)ToDeg(ahrs.yaw), + (double)ToDeg(ahrs.get_roll()), + (double)ToDeg(ahrs.get_pitch()), + (double)ToDeg(ahrs.get_yaw()), (double)ToDeg(drift.x), (double)ToDeg(drift.y), (double)ToDeg(drift.z), diff --git a/libraries/AP_AIS/AP_AIS.cpp b/libraries/AP_AIS/AP_AIS.cpp index c08a4dea0318f9..a47c57ee6fc315 100644 --- a/libraries/AP_AIS/AP_AIS.cpp +++ b/libraries/AP_AIS/AP_AIS.cpp @@ -31,6 +31,7 @@ #include #include #include +#include const AP_Param::GroupInfo AP_AIS::var_info[] = { @@ -120,20 +121,26 @@ void AP_AIS::update() if (_incoming.total > AIVDM_BUFFER_SIZE) { // no point in trying to decode it wont fit +#if HAL_LOGGING_ENABLED if (log_all || log_unsupported) { log_raw(&_incoming); } +#endif break; } +#if HAL_LOGGING_ENABLED if (log_all) { log_raw(&_incoming); } +#endif if (_incoming.num == 1 && _incoming.total == 1) { // single part message if (!payload_decode(_incoming.payload) && log_unsupported) { +#if HAL_LOGGING_ENABLED // could not decode so log log_raw(&_incoming); +#endif } } else if (_incoming.num == _incoming.total) { // last part of a multi part message @@ -154,12 +161,14 @@ void AP_AIS::update() // did we find the right number? if (_incoming.num != index) { // could not find all of the message, save messages +#if HAL_LOGGING_ENABLED if (log_unsupported) { for (uint8_t i = 0; i < index; i++) { log_raw(&_AIVDM_buffer[msg_parts[i]]); } log_raw(&_incoming); } +#endif // remove for (uint8_t i = 0; i < index; i++) { buffer_shift(msg_parts[i]); @@ -174,17 +183,23 @@ void AP_AIS::update() strncat(multi,_AIVDM_buffer[msg_parts[i]].payload,sizeof(multi)); } strncat(multi,_incoming.payload,sizeof(multi)); +#if HAL_LOGGING_ENABLED const bool decoded = payload_decode(multi); +#endif for (uint8_t i = 0; i < _incoming.total; i++) { +#if HAL_LOGGING_ENABLED // unsupported type, log and discard if (!decoded && log_unsupported) { log_raw(&_AIVDM_buffer[msg_parts[i]]); } +#endif buffer_shift(msg_parts[i]); } +#if HAL_LOGGING_ENABLED if (!decoded && log_unsupported) { log_raw(&_incoming); } +#endif } else { // multi part message, store in buffer bool fits_in = false; @@ -198,10 +213,12 @@ void AP_AIS::update() } if (!fits_in) { // remove the oldest message +#if HAL_LOGGING_ENABLED if (log_unsupported) { // log the unused message before removing it log_raw(&_AIVDM_buffer[0]); } +#endif buffer_shift(0); _AIVDM_buffer[AIVDM_BUFFER_SIZE - 1] = _incoming; } @@ -393,6 +410,7 @@ bool AP_AIS::decode_position_report(const char *payload, uint8_t type) bool raim = get_bits(payload, 148, 148); uint32_t radio = get_bits(payload, 149, 167); +#if HAL_LOGGING_ENABLED // log the raw infomation if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) { const struct log_AIS_msg1 pkt{ @@ -416,6 +434,19 @@ bool AP_AIS::decode_position_report(const char *payload, uint8_t type) }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#else + (void)repeat; + (void)nav; + (void)rot; + (void)sog; + (void)pos_acc; + (void)cog; + (void)head; + (void)sec_utc; + (void)maneuver; + (void)raim; + (void)radio; +#endif uint16_t index; if (!get_vessel_index(mmsi, index, lat, lon)) { @@ -482,6 +513,7 @@ bool AP_AIS::decode_base_station_report(const char *payload) bool raim = get_bits(payload, 148, 148); uint32_t radio = get_bits(payload, 149, 167); +#if HAL_LOGGING_ENABLED // log the raw infomation if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) { struct log_AIS_msg4 pkt { @@ -504,6 +536,19 @@ bool AP_AIS::decode_base_station_report(const char *payload) }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#else + (void)repeat; + (void)year; + (void)month; + (void)day; + (void)hour; + (void)minute; + (void)second; + (void)fix; + (void)epfd; + (void)raim; + (void)radio; +#endif uint16_t index; if (!get_vessel_index(mmsi, index)) { @@ -548,6 +593,7 @@ bool AP_AIS::decode_static_and_voyage_data(const char *payload) bool dte = get_bits(payload, 422, 422); // 423 - 426: spare +#if HAL_LOGGING_ENABLED // log the raw infomation if ((_log_options & AIS_OPTIONS_LOG_DECODED) != 0) { struct log_AIS_msg5 pkt { @@ -574,6 +620,14 @@ bool AP_AIS::decode_static_and_voyage_data(const char *payload) strncpy(pkt.dest, dest, sizeof(pkt.dest)); AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#else + (void)repeat; + (void)ver; + (void)imo; + (void)fix; + (void)draught; + (void)dte; +#endif uint16_t index; if (!get_vessel_index(mmsi, index)) { @@ -699,6 +753,7 @@ uint8_t AP_AIS::payload_char_decode(const char c) ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Functions for decoding and logging AIVDM NMEA sentence +#if HAL_LOGGING_ENABLED // log a raw AIVDM a message void AP_AIS::log_raw(const AIVDM *msg) { @@ -713,6 +768,7 @@ void AP_AIS::log_raw(const AIVDM *msg) memcpy(pkt.payload, msg->payload, sizeof(pkt.payload)); AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif // add a single character to the buffer and attempt to decode // returns true if a complete sentence was successfully decoded diff --git a/libraries/AP_AccelCal/AP_AccelCal.cpp b/libraries/AP_AccelCal/AP_AccelCal.cpp index 8cf1fc126a3500..d4fa4cba030b4b 100644 --- a/libraries/AP_AccelCal/AP_AccelCal.cpp +++ b/libraries/AP_AccelCal/AP_AccelCal.cpp @@ -44,9 +44,8 @@ void AP_AccelCal::update() if (_started) { update_status(); - AccelCalibrator *cal; uint8_t num_active_calibrators = 0; - for(uint8_t i=0; (cal = get_calibrator(i)); i++) { + for(uint8_t i=0; get_calibrator(i) != nullptr; i++) { num_active_calibrators++; } if (num_active_calibrators != _num_active_calibrators) { @@ -56,6 +55,7 @@ void AP_AccelCal::update() if(_start_collect_sample) { collect_sample(); } + AccelCalibrator *cal; switch(_status) { case ACCEL_CAL_NOT_STARTED: fail(); diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index a13480b531c7e9..a5d94d31aacdfe 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -52,6 +52,7 @@ #include "AP_Airspeed_DroneCAN.h" #include "AP_Airspeed_NMEA.h" #include "AP_Airspeed_MSP.h" +#include "AP_Airspeed_External.h" #include "AP_Airspeed_SITL.h" extern const AP_HAL::HAL &hal; @@ -433,6 +434,11 @@ void AP_Airspeed::allocate() case TYPE_MSP: #if AP_AIRSPEED_MSP_ENABLED sensor[i] = new AP_Airspeed_MSP(*this, i, 0); +#endif + break; + case TYPE_EXTERNAL: +#if AP_AIRSPEED_EXTERNAL_ENABLED + sensor[i] = new AP_Airspeed_External(*this, i); #endif break; } @@ -556,7 +562,7 @@ void AP_Airspeed::update_calibration(uint8_t i, float raw_pressure) // check if new offset differs too greatly from last calibration, indicating pitot uncovered in wind if (fixed_wing_parameters != nullptr) { float airspeed_min = fixed_wing_parameters->airspeed_min.get(); - // use percentage of ARSPD_FBW_MIN as criteria for max allowed change in offset + // use percentage of AIRSPEED_MIN as criteria for max allowed change in offset float max_change = 0.5*(sq((1 + (max_speed_pcnt * 0.01))*airspeed_min) - sq(airspeed_min)); if (max_speed_pcnt > 0 && (abs(calibrated_offset-param[i].offset) > max_change) && (abs(param[i].offset) > 0)) { GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Arspd %d offset change large;cover and recal", i +1); @@ -731,6 +737,25 @@ void AP_Airspeed::handle_msp(const MSP::msp_airspeed_data_message_t &pkt) } #endif +#if AP_AIRSPEED_EXTERNAL_ENABLED +/* + handle airspeed airspeed data + */ +void AP_Airspeed::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) +{ + if (!lib_enabled()) { + return; + } + + for (uint8_t i=0; ihandle_external(pkt); + } + } +} +#endif + +#if HAL_LOGGING_ENABLED // @LoggerMessage: HYGR // @Description: Hygrometer data // @Field: TimeUS: Time since system startup @@ -788,6 +813,7 @@ void AP_Airspeed::Log_Airspeed() #endif } } +#endif bool AP_Airspeed::use(uint8_t i) const { diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 5574f807557a67..8f6666f62b822c 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -10,6 +10,9 @@ #if AP_AIRSPEED_MSP_ENABLED #include #endif +#if AP_AIRSPEED_EXTERNAL_ENABLED +#include +#endif class AP_Airspeed_Backend; @@ -187,6 +190,7 @@ class AP_Airspeed TYPE_NMEA_WATER=13, TYPE_MSP=14, TYPE_I2C_ASP5033=15, + TYPE_EXTERNAL=16, TYPE_SITL=100, }; @@ -208,6 +212,10 @@ class AP_Airspeed void handle_msp(const MSP::msp_airspeed_data_message_t &pkt); #endif +#if AP_AIRSPEED_EXTERNAL_ENABLED + void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt); +#endif + enum class CalibrationState { NOT_STARTED, IN_PROGRESS, diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp index 412b337eeb65c6..fc36421419b867 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.cpp @@ -58,7 +58,7 @@ uint8_t AP_Airspeed_Backend::get_bus(void) const return frontend.param[instance].bus; } -bool AP_Airspeed_Backend::bus_is_confgured(void) const +bool AP_Airspeed_Backend::bus_is_configured(void) const { return frontend.param[instance].bus.configured(); } diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index 2deeb427c09c34..bdb4dded5c562c 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -49,6 +49,9 @@ class AP_Airspeed_Backend { virtual bool get_airspeed(float& airspeed) {return false;} virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {} +#if AP_AIRSPEED_EXTERNAL_ENABLED + virtual void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) {} +#endif #if AP_AIRSPEED_HYGROMETER_ENABLE // optional hygrometer support @@ -59,7 +62,7 @@ class AP_Airspeed_Backend { int8_t get_pin(void) const; float get_psi_range(void) const; uint8_t get_bus(void) const; - bool bus_is_confgured(void) const; + bool bus_is_configured(void) const; uint8_t get_instance(void) const { return instance; } diff --git a/libraries/AP_Airspeed/AP_Airspeed_External.cpp b/libraries/AP_Airspeed/AP_Airspeed_External.cpp new file mode 100644 index 00000000000000..5fc709f069bc9b --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_External.cpp @@ -0,0 +1,58 @@ +#include "AP_Airspeed_External.h" + +#if AP_AIRSPEED_EXTERNAL_ENABLED + +AP_Airspeed_External::AP_Airspeed_External(AP_Airspeed &_frontend, uint8_t _instance) : + AP_Airspeed_Backend(_frontend, _instance) +{ + set_bus_id(AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,0,_instance,0)); +} + +// return the current differential_pressure in Pascal +bool AP_Airspeed_External::get_differential_pressure(float &pressure) +{ + WITH_SEMAPHORE(sem); + if (press_count == 0) { + return false; + } + pressure = sum_pressure/press_count; + press_count = 0; + sum_pressure = 0; + return true; +} + +// get last temperature +bool AP_Airspeed_External::get_temperature(float &temperature) +{ + WITH_SEMAPHORE(sem); + if (temperature_count == 0) { + return false; + } + temperature = sum_temperature/temperature_count; + temperature_count = 0; + sum_temperature = 0; + return true; +} + +void AP_Airspeed_External::handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) +{ + WITH_SEMAPHORE(sem); + + sum_pressure += pkt.differential_pressure; + press_count++; + if (press_count > 100) { + // prevent overflow + sum_pressure /= 2; + press_count /= 2; + } + + sum_temperature += pkt.temperature; + temperature_count++; + if (temperature_count > 100) { + // prevent overflow + sum_temperature /= 2; + temperature_count /= 2; + } +} + +#endif // AP_AIRSPEED_EXTERNAL_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed_External.h b/libraries/AP_Airspeed/AP_Airspeed_External.h new file mode 100644 index 00000000000000..cd27d757f2ce24 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_External.h @@ -0,0 +1,38 @@ +/* + external AHRS airspeed backend + */ +#pragma once + +#include "AP_Airspeed_config.h" + +#if AP_AIRSPEED_EXTERNAL_ENABLED + +#include "AP_Airspeed_Backend.h" +#include + +class AP_Airspeed_External : public AP_Airspeed_Backend +{ +public: + AP_Airspeed_External(AP_Airspeed &airspeed, uint8_t instance); + + bool init(void) override { + return true; + } + + void handle_external(const AP_ExternalAHRS::airspeed_data_message_t &pkt) override; + + // return the current differential_pressure in Pascal + bool get_differential_pressure(float &pressure) override; + + // temperature not available via analog backend + bool get_temperature(float &temperature) override; + +private: + float sum_pressure; + uint8_t press_count; + float sum_temperature; + uint8_t temperature_count; +}; + +#endif // AP_AIRSPEED_EXTERNAL_ENABLED + diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp index d6e4c456a32e61..ec91bf8b4b309c 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.cpp @@ -62,7 +62,7 @@ bool AP_Airspeed_MS4525::probe(uint8_t bus, uint8_t address) bool AP_Airspeed_MS4525::init() { static const uint8_t addresses[] = { MS4525D0_I2C_ADDR1, MS4525D0_I2C_ADDR2, MS4525D0_I2C_ADDR3 }; - if (bus_is_confgured()) { + if (bus_is_configured()) { // the user has configured a specific bus for (uint8_t addr : addresses) { if (probe(get_bus(), addr)) { diff --git a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp index 37c6b69faa262c..749fa9d6121641 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Params.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Params.cpp @@ -46,7 +46,7 @@ const AP_Param::GroupInfo AP_Airspeed_Params::var_info[] = { // @Param: TYPE // @DisplayName: Airspeed type // @Description: Type of airspeed sensor - // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,100:SITL + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,16:ExternalAHRS,100:SITL // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 1, AP_Airspeed_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Airspeed/AP_Airspeed_config.h b/libraries/AP_Airspeed/AP_Airspeed_config.h index 4ba2e4179388a2..f12db5cb16c4e9 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_config.h +++ b/libraries/AP_Airspeed/AP_Airspeed_config.h @@ -3,6 +3,7 @@ #include #include #include +#include #ifndef AP_AIRSPEED_ENABLED #define AP_AIRSPEED_ENABLED 1 @@ -66,3 +67,7 @@ #ifndef AP_AIRSPEED_HYGROMETER_ENABLE #define AP_AIRSPEED_HYGROMETER_ENABLE (AP_AIRSPEED_ENABLED && BOARD_FLASH_SIZE > 1024) #endif + +#ifndef AP_AIRSPEED_EXTERNAL_ENABLED +#define AP_AIRSPEED_EXTERNAL_ENABLED AP_AIRSPEED_ENABLED && HAL_EXTERNAL_AHRS_ENABLED +#endif diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index b617c6d134c491..0b6ae786a2b08f 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -244,6 +244,7 @@ void AP_Arming::check_failed(const enum AP_Arming::ArmingChecks check, bool repo } hal.util->snprintf(taggedfmt, sizeof(taggedfmt), metafmt, fmt); +#if HAL_GCS_ENABLED MAV_SEVERITY severity = MAV_SEVERITY_CRITICAL; if (!check_enabled(check)) { // technically should be NOTICE, but will annoy users at that level: @@ -253,10 +254,12 @@ void AP_Arming::check_failed(const enum AP_Arming::ArmingChecks check, bool repo va_start(arg_list, fmt); gcs().send_textv(severity, taggedfmt, arg_list); va_end(arg_list); +#endif // HAL_GCS_ENABLED } void AP_Arming::check_failed(bool report, const char *fmt, ...) const { +#if HAL_GCS_ENABLED if (!report) { return; } @@ -275,6 +278,7 @@ void AP_Arming::check_failed(bool report, const char *fmt, ...) const va_start(arg_list, fmt); gcs().send_textv(MAV_SEVERITY_CRITICAL, taggedfmt, arg_list); va_end(arg_list); +#endif // HAL_GCS_ENABLED } bool AP_Arming::barometer_checks(bool report) @@ -299,9 +303,9 @@ bool AP_Arming::barometer_checks(bool report) return true; } +#if AP_AIRSPEED_ENABLED bool AP_Arming::airspeed_checks(bool report) { -#if AP_AIRSPEED_ENABLED if (check_enabled(ARMING_CHECK_AIRSPEED)) { const AP_Airspeed *airspeed = AP_Airspeed::get_singleton(); if (airspeed == nullptr) { @@ -315,11 +319,12 @@ bool AP_Arming::airspeed_checks(bool report) } } } -#endif return true; } +#endif // AP_AIRSPEED_ENABLED +#if HAL_LOGGING_ENABLED bool AP_Arming::logging_checks(bool report) { if (check_enabled(ARMING_CHECK_LOGGING)) { @@ -342,6 +347,7 @@ bool AP_Arming::logging_checks(bool report) } return true; } +#endif // HAL_LOGGING_ENABLED #if AP_INERTIALSENSOR_ENABLED bool AP_Arming::ins_accels_consistent(const AP_InertialSensor &ins) @@ -573,6 +579,7 @@ bool AP_Arming::compass_checks(bool report) return false; } +#if AP_AHRS_ENABLED // if ahrs is using compass and we have location, check mag field versus expected earth magnetic model Location ahrs_loc; AP_AHRS &ahrs = AP::ahrs(); @@ -591,11 +598,13 @@ bool AP_Arming::compass_checks(bool report) return false; } } +#endif // AP_AHRS_ENABLED } return true; } +#if AP_GPS_ENABLED bool AP_Arming::gps_checks(bool report) { const AP_GPS &gps = AP::gps(); @@ -686,7 +695,9 @@ bool AP_Arming::gps_checks(bool report) return true; } +#endif // AP_GPS_ENABLED +#if AP_BATTERY_ENABLED bool AP_Arming::battery_checks(bool report) { if (check_enabled(ARMING_CHECK_BATTERY)) { @@ -699,6 +710,7 @@ bool AP_Arming::battery_checks(bool report) } return true; } +#endif // AP_BATTERY_ENABLED bool AP_Arming::hardware_safety_check(bool report) { @@ -714,6 +726,7 @@ bool AP_Arming::hardware_safety_check(bool report) return true; } +#if AP_RC_CHANNEL_ENABLED bool AP_Arming::rc_arm_checks(AP_Arming::Method method) { // don't check the trims if we are in a failsafe @@ -831,7 +844,9 @@ bool AP_Arming::manual_transmitter_checks(bool report) return rc_in_calibration_check(report); } +#endif // AP_RC_CHANNEL_ENABLED +#if AP_MISSION_ENABLED bool AP_Arming::mission_checks(bool report) { AP_Mission *mission = AP::mission(); @@ -894,6 +909,7 @@ bool AP_Arming::mission_checks(bool report) } #endif +#if AP_VEHICLE_ENABLED // do not allow arming if there are no mission items and we are in // (e.g.) AUTO mode if (AP::vehicle()->current_mode_requires_mission() && @@ -901,9 +917,11 @@ bool AP_Arming::mission_checks(bool report) check_failed(ARMING_CHECK_MISSION, report, "Mode requires mission"); return false; } +#endif return true; } +#endif // AP_MISSION_ENABLED bool AP_Arming::rangefinder_checks(bool report) { @@ -1115,6 +1133,7 @@ bool AP_Arming::system_checks(bool report) bool AP_Arming::terrain_database_required() const { +#if AP_MISSION_ENABLED AP_Mission *mission = AP::mission(); if (mission == nullptr) { // no mission support? @@ -1123,6 +1142,7 @@ bool AP_Arming::terrain_database_required() const if (mission->contains_terrain_alt_items()) { return true; } +#endif return false; } @@ -1166,10 +1186,10 @@ bool AP_Arming::terrain_checks(bool report) const } +#if HAL_PROXIMITY_ENABLED // check nothing is too close to vehicle bool AP_Arming::proximity_checks(bool report) const { -#if HAL_PROXIMITY_ENABLED const AP_Proximity *proximity = AP::proximity(); // return true immediately if no sensor present if (proximity == nullptr) { @@ -1180,15 +1200,13 @@ bool AP_Arming::proximity_checks(bool report) const check_failed(report, "%s", buffer); return false; } - return true; -#endif - return true; } +#endif // HAL_PROXIMITY_ENABLED +#if HAL_MAX_CAN_PROTOCOL_DRIVERS && HAL_CANMANAGER_ENABLED bool AP_Arming::can_checks(bool report) { -#if HAL_MAX_CAN_PROTOCOL_DRIVERS && HAL_CANMANAGER_ENABLED if (check_enabled(ARMING_CHECK_SYSTEM)) { char fail_msg[50] = {}; (void)fail_msg; // might be left unused @@ -1226,6 +1244,7 @@ bool AP_Arming::can_checks(bool report) case AP_CAN::Protocol::TOFSenseP: case AP_CAN::Protocol::NanoRadar_NRA24: case AP_CAN::Protocol::Benewake: + case AP_CAN::Protocol::MR72: { for (uint8_t j = i; j; j--) { if (AP::can().get_driver_type(i) == AP::can().get_driver_type(j-1)) { @@ -1245,14 +1264,14 @@ bool AP_Arming::can_checks(bool report) } } } -#endif return true; } +#endif // HAL_MAX_CAN_PROTOCOL_DRIVERS && HAL_CANMANAGER_ENABLED +#if AP_FENCE_ENABLED bool AP_Arming::fence_checks(bool display_failure) { -#if AP_FENCE_ENABLED const AC_Fence *fence = AP::fence(); if (fence == nullptr) { return true; @@ -1271,15 +1290,13 @@ bool AP_Arming::fence_checks(bool display_failure) } return false; -#else - return true; -#endif } +#endif // AP_FENCE_ENABLED +#if HAL_RUNCAM_ENABLED bool AP_Arming::camera_checks(bool display_failure) { if (check_enabled(ARMING_CHECK_CAMERA)) { -#if HAL_RUNCAM_ENABLED AP_RunCam *runcam = AP::runcam(); if (runcam == nullptr) { return true; @@ -1291,14 +1308,14 @@ bool AP_Arming::camera_checks(bool display_failure) check_failed(ARMING_CHECK_CAMERA, display_failure, "%s", fail_msg); return false; } -#endif } return true; } +#endif // HAL_RUNCAM_ENABLED +#if OSD_ENABLED bool AP_Arming::osd_checks(bool display_failure) const { -#if OSD_ENABLED if (check_enabled(ARMING_CHECK_OSD)) { // if no OSD then pass const AP_OSD *osd = AP::osd(); @@ -1312,13 +1329,13 @@ bool AP_Arming::osd_checks(bool display_failure) const return false; } } -#endif return true; } +#endif // OSD_ENABLED +#if HAL_MOUNT_ENABLED bool AP_Arming::mount_checks(bool display_failure) const { -#if HAL_MOUNT_ENABLED if (check_enabled(ARMING_CHECK_CAMERA)) { AP_Mount *mount = AP::mount(); if (mount == nullptr) { @@ -1330,13 +1347,13 @@ bool AP_Arming::mount_checks(bool display_failure) const return false; } } -#endif return true; } +#endif // HAL_MOUNT_ENABLED +#if AP_FETTEC_ONEWIRE_ENABLED bool AP_Arming::fettec_checks(bool display_failure) const { -#if AP_FETTEC_ONEWIRE_ENABLED const AP_FETtecOneWire *f = AP_FETtecOneWire::get_singleton(); if (f == nullptr) { return true; @@ -1348,9 +1365,9 @@ bool AP_Arming::fettec_checks(bool display_failure) const check_failed(ARMING_CHECK_ALL, display_failure, "FETtec: %s", fail_msg); return false; } -#endif return true; } +#endif // AP_FETTEC_ONEWIRE_ENABLED #if AP_ARMING_AUX_AUTH_ENABLED // request an auxiliary authorisation id. This id should be used in subsequent calls to set_aux_auth_passed/failed @@ -1471,9 +1488,9 @@ bool AP_Arming::aux_auth_checks(bool display_failure) } #endif // AP_ARMING_AUX_AUTH_ENABLED +#if HAL_GENERATOR_ENABLED bool AP_Arming::generator_checks(bool display_failure) const { -#if HAL_GENERATOR_ENABLED const AP_Generator *generator = AP::generator(); if (generator == nullptr) { return true; @@ -1483,14 +1500,14 @@ bool AP_Arming::generator_checks(bool display_failure) const check_failed(display_failure, "Generator: %s", failure_msg); return false; } -#endif return true; } +#endif // HAL_GENERATOR_ENABLED +#if AP_OPENDRONEID_ENABLED // OpenDroneID Checks bool AP_Arming::opendroneid_checks(bool display_failure) { -#if AP_OPENDRONEID_ENABLED auto &opendroneid = AP::opendroneid(); char failure_msg[50] {}; @@ -1498,9 +1515,9 @@ bool AP_Arming::opendroneid_checks(bool display_failure) check_failed(display_failure, "OpenDroneID: %s", failure_msg); return false; } -#endif return true; } +#endif // AP_OPENDRONEID_ENABLED //Check for multiple RC in serial protocols bool AP_Arming::serial_protocol_checks(bool display_failure) @@ -1519,6 +1536,7 @@ bool AP_Arming::estop_checks(bool display_failure) // not emergency-stopped, so no prearm failure: return true; } +#if AP_RC_CHANNEL_ENABLED // vehicle is emergency-stopped; if this *appears* to have been done via switch then we do not fail prearms: const RC_Channel *chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ARM_EMERGENCY_STOP); if (chan != nullptr) { @@ -1527,7 +1545,8 @@ bool AP_Arming::estop_checks(bool display_failure) // switch is configured and is in estop position, so likely the reason we are estopped, so no prearm failure return true; // no prearm failure } - } + } +#endif // AP_RC_CHANNEL_ENABLED check_failed(display_failure,"Motors Emergency Stopped"); return false; } @@ -1546,35 +1565,73 @@ bool AP_Arming::pre_arm_checks(bool report) #if HAL_HAVE_IMU_HEATER & heater_min_temperature_checks(report) #endif +#if AP_BARO_ENABLED & barometer_checks(report) +#endif #if AP_INERTIALSENSOR_ENABLED & ins_checks(report) #endif +#if AP_COMPASS_ENABLED & compass_checks(report) +#endif +#if AP_GPS_ENABLED & gps_checks(report) +#endif +#if AP_BATTERY_ENABLED & battery_checks(report) +#endif +#if HAL_LOGGING_ENABLED & logging_checks(report) +#endif +#if AP_RC_CHANNEL_ENABLED & manual_transmitter_checks(report) +#endif +#if AP_MISSION_ENABLED & mission_checks(report) +#endif +#if AP_RANGEFINDER_ENABLED & rangefinder_checks(report) +#endif & servo_checks(report) & board_voltage_checks(report) & system_checks(report) & terrain_checks(report) +#if HAL_MAX_CAN_PROTOCOL_DRIVERS && HAL_CANMANAGER_ENABLED & can_checks(report) +#endif +#if HAL_GENERATOR_ENABLED & generator_checks(report) +#endif +#if HAL_PROXIMITY_ENABLED & proximity_checks(report) +#endif +#if HAL_RUNCAM_ENABLED & camera_checks(report) +#endif +#if OSD_ENABLED & osd_checks(report) +#endif +#if HAL_MOUNT_ENABLED & mount_checks(report) +#endif +#if AP_FETTEC_ONEWIRE_ENABLED & fettec_checks(report) +#endif +#if HAL_VISUALODOM_ENABLED & visodom_checks(report) +#endif #if AP_ARMING_AUX_AUTH_ENABLED & aux_auth_checks(report) #endif +#if AP_RC_CHANNEL_ENABLED & disarm_switch_checks(report) +#endif +#if AP_FENCE_ENABLED & fence_checks(report) +#endif +#if AP_OPENDRONEID_ENABLED & opendroneid_checks(report) +#endif & serial_protocol_checks(report) & estop_checks(report); @@ -1588,11 +1645,13 @@ bool AP_Arming::pre_arm_checks(bool report) bool AP_Arming::arm_checks(AP_Arming::Method method) { +#if AP_RC_CHANNEL_ENABLED if (check_enabled(ARMING_CHECK_RC)) { if (!rc_arm_checks(method)) { return false; } } +#endif // ensure the GPS drivers are ready on any final changes if (check_enabled(ARMING_CHECK_GPS_CONFIG)) { @@ -1608,6 +1667,7 @@ bool AP_Arming::arm_checks(AP_Arming::Method method) // the arming check flag is set - disabling the arming check // should not stop logging from working. +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); if (logger->logging_present()) { // If we're configured to log, prep it @@ -1618,6 +1678,8 @@ bool AP_Arming::arm_checks(AP_Arming::Method method) return false; } } +#endif // HAL_LOGGING_ENABLED + return true; } @@ -1646,17 +1708,21 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks) _last_arm_method = method; +#if HAL_LOGGING_ENABLED Log_Write_Arm(!do_arming_checks, method); // note Log_Write_Armed takes forced not do_arming_checks +#endif } else { +#if HAL_LOGGING_ENABLED AP::logger().arming_failure(); +#endif armed = false; } running_arming_checks = false; if (armed && do_arming_checks && checks_to_perform == 0) { - gcs().send_text(MAV_SEVERITY_WARNING, "Warning: Arming Checks Disabled"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Warning: Arming Checks Disabled"); } #if HAL_GYROFFT_ENABLED @@ -1685,7 +1751,7 @@ bool AP_Arming::arm(AP_Arming::Method method, const bool do_arming_checks) // If a fence is set to auto-enable, turn on the fence if (fence->auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) { fence->enable(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence: auto-enabled"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fence: auto-enabled"); } } } @@ -1705,9 +1771,11 @@ bool AP_Arming::disarm(const AP_Arming::Method method, bool do_disarm_checks) armed = false; _last_disarm_method = method; +#if HAL_LOGGING_ENABLED Log_Write_Disarm(!do_disarm_checks, method); // Log_Write_Disarm takes "force" check_forced_logging(method); +#endif #if HAL_HAVE_SAFETY_SWITCH AP_BoardConfig *board_cfg = AP_BoardConfig::get_singleton(); @@ -1752,7 +1820,7 @@ void AP_Arming::send_arm_disarm_statustext(const char *str) const if (option_enabled(AP_Arming::Option::DISABLE_STATUSTEXT_ON_STATE_CHANGE)) { return; } - gcs().send_text(MAV_SEVERITY_INFO, "%s", str); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", str); } AP_Arming::Required AP_Arming::arming_required() const @@ -1768,6 +1836,7 @@ AP_Arming::Required AP_Arming::arming_required() const return require; } +#if AP_RC_CHANNEL_ENABLED // Copter and sub share the same RC input limits // Copter checks that min and max have been configured by default, Sub does not bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channel *channels[4]) const @@ -1796,7 +1865,9 @@ bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channe } return ret; } +#endif // AP_RC_CHANNEL_ENABLED +#if HAL_VISUALODOM_ENABLED // check visual odometry is working bool AP_Arming::visodom_checks(bool display_failure) const { @@ -1804,7 +1875,6 @@ bool AP_Arming::visodom_checks(bool display_failure) const return true; } -#if HAL_VISUALODOM_ENABLED AP_VisualOdom *visual_odom = AP::visualodom(); if (visual_odom != nullptr) { char fail_msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; @@ -1813,11 +1883,12 @@ bool AP_Arming::visodom_checks(bool display_failure) const return false; } } -#endif return true; } +#endif +#if AP_RC_CHANNEL_ENABLED // check disarm switch is asserted bool AP_Arming::disarm_switch_checks(bool display_failure) const { @@ -1830,7 +1901,9 @@ bool AP_Arming::disarm_switch_checks(bool display_failure) const return true; } +#endif // AP_RC_CHANNEL_ENABLED +#if HAL_LOGGING_ENABLED void AP_Arming::Log_Write_Arm(const bool forced, const AP_Arming::Method method) { const struct log_Arm_Disarm pkt { @@ -1909,6 +1982,7 @@ void AP_Arming::check_forced_logging(const AP_Arming::Method method) return; } } +#endif // HAL_LOGGING_ENABLED AP_Arming *AP_Arming::_singleton = nullptr; diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 603e1b6b85a7a5..8f46bd47e9d77a 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -6,6 +6,7 @@ #include "AP_Arming_config.h" #include "AP_InertialSensor/AP_InertialSensor_config.h" +#include "AP_Proximity/AP_Proximity_config.h" class AP_Arming { public: @@ -235,9 +236,9 @@ class AP_Arming { bool fettec_checks(bool display_failure) const; - bool kdecan_checks(bool display_failure) const; - +#if HAL_PROXIMITY_ENABLED virtual bool proximity_checks(bool report) const; +#endif bool servo_checks(bool report) const; bool rc_checks_copter_sub(bool display_failure, const class RC_Channel *channels[4]) const; diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp index 45f28874fc10c5..14e0d286283e0b 100644 --- a/libraries/AP_Avoidance/AP_Avoidance.cpp +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -396,6 +396,7 @@ MAV_COLLISION_THREAT_LEVEL AP_Avoidance::current_threat_level() const { return _obstacles[_current_most_serious_threat].threat_level; } +#if HAL_GCS_ENABLED void AP_Avoidance::send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour) const { const mavlink_collision_t packet{ @@ -409,6 +410,7 @@ void AP_Avoidance::send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_ }; gcs().send_to_active_channels(MAVLINK_MSG_ID_COLLISION, (const char *)&packet); } +#endif void AP_Avoidance::handle_threat_gcs_notify(AP_Avoidance::Obstacle *threat) { diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 66577622a7cd40..6d944f221bef88 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -134,7 +134,7 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @RebootRequired: True AP_GROUPINFO("3DMASK", 10, AP_BLHeli, channel_reversible_mask, 0), -#ifdef HAL_WITH_BIDIR_DSHOT +#if defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT // @Param: BDMASK // @DisplayName: BLHeli bitmask of bi-directional dshot channels // @Description: Mask of channels which support bi-directional dshot. This is used for ESCs which have firmware that supports bi-directional dshot allowing fast rpm telemetry values to be returned for the harmonic notch. @@ -1409,6 +1409,8 @@ void AP_BLHeli::init(uint32_t mask, AP_HAL::RCOutput::output_mode otype) #ifdef HAL_WITH_BIDIR_DSHOT // possibly enable bi-directional dshot hal.rcout->set_motor_poles(motor_poles); +#endif +#if defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT hal.rcout->set_bidir_dshot_mask(uint32_t(channel_bidir_dshot_mask.get()) & digital_mask); #endif // add motors from channel mask diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index ae583262fdaee3..35f1e5082263bf 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -54,6 +54,8 @@ class AP_BLHeli : public AP_ESC_Telem_Backend { } uint32_t get_bidir_dshot_mask() const { return channel_bidir_dshot_mask.get(); } + uint8_t get_motor_poles() const { return motor_poles.get(); } + uint16_t get_telemetry_rate() const { return telem_rate.get(); } static AP_BLHeli *get_singleton(void) { return _singleton; diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 874f4e054ad405..84bfbadc9fd989 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -874,6 +874,7 @@ void AP_Baro::_probe_i2c_barometers(void) } } +#if HAL_LOGGING_ENABLED bool AP_Baro::should_log() const { AP_Logger *logger = AP_Logger::get_singleton(); @@ -888,6 +889,7 @@ bool AP_Baro::should_log() const } return true; } +#endif /* call update on all drivers @@ -899,7 +901,7 @@ void AP_Baro::update(void) if (fabsf(_alt_offset - _alt_offset_active) > 0.01f) { // If there's more than 1cm difference then slowly slew to it via LPF. // The EKF does not like step inputs so this keeps it happy. - _alt_offset_active = (0.95f*_alt_offset_active) + (0.05f*_alt_offset); + _alt_offset_active = (0.98f*_alt_offset_active) + (0.02f*_alt_offset); } else { _alt_offset_active = _alt_offset; } diff --git a/libraries/AP_Baro/AP_Baro_ICP201XX.cpp b/libraries/AP_Baro/AP_Baro_ICP201XX.cpp index b7b5c6ec899328..6811e8c66b33a4 100644 --- a/libraries/AP_Baro/AP_Baro_ICP201XX.cpp +++ b/libraries/AP_Baro/AP_Baro_ICP201XX.cpp @@ -106,6 +106,7 @@ bool AP_Baro_ICP201XX::init() uint8_t id = 0xFF; uint8_t ver = 0xFF; read_reg(REG_DEVICE_ID, &id); + read_reg(REG_DEVICE_ID, &id); read_reg(REG_VERSION, &ver); if (id != ICP201XX_ID) { diff --git a/libraries/AP_Baro/AP_Baro_Logging.cpp b/libraries/AP_Baro/AP_Baro_Logging.cpp index a697d22661074f..91c622aaa0f207 100644 --- a/libraries/AP_Baro/AP_Baro_Logging.cpp +++ b/libraries/AP_Baro/AP_Baro_Logging.cpp @@ -1,3 +1,7 @@ +#include + +#if HAL_LOGGING_ENABLED + #include "AP_Baro.h" #include @@ -44,3 +48,5 @@ void AP_Baro::Write_Baro(void) Write_Baro_instance(time_us, i); } } + +#endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 45f630ce3bb6f1..d929b8182a06df 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -1,4 +1,9 @@ +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_ENABLED + #include "AP_BattMonitor.h" + #include "AP_BattMonitor_Analog.h" #include "AP_BattMonitor_SMBus.h" #include "AP_BattMonitor_SMBus_Solo.h" @@ -21,6 +26,7 @@ #include "AP_BattMonitor_FuelLevel_Analog.h" #include "AP_BattMonitor_Synthetic_Current.h" #include "AP_BattMonitor_AD7091R5.h" +#include "AP_BattMonitor_Scripting.h" #include @@ -560,6 +566,11 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_AD7091R5(*this, state[instance], _params[instance]); break; #endif// AP_BATTERY_AD7091R5_ENABLED +#if AP_BATTERY_SCRIPTING_ENABLED + case Type::Scripting: + drivers[instance] = new AP_BattMonitor_Scripting(*this, state[instance], _params[instance]); + break; +#endif // AP_BATTERY_SCRIPTING_ENABLED case Type::NONE: default: break; @@ -645,7 +656,7 @@ void AP_BattMonitor::convert_dynamic_param_groups(uint8_t instance) } } -// read - For all active instances read voltage & current; log BAT, BCL, POWR +// read - For all active instances read voltage & current; log BAT, BCL, POWR, MCU void AP_BattMonitor::read() { #if HAL_LOGGING_ENABLED @@ -1050,6 +1061,15 @@ uint32_t AP_BattMonitor::get_mavlink_fault_bitmask(const uint8_t instance) const return drivers[instance]->get_mavlink_fault_bitmask(); } +// return true if state of health (as a percentage) can be provided and fills in soh_pct argument +bool AP_BattMonitor::get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const +{ + if (instance >= _num_instances || drivers[instance] == nullptr) { + return false; + } + return drivers[instance]->get_state_of_health_pct(soh_pct); +} + // Enable/Disable (Turn on/off) MPPT power to all backends who are MPPTs void AP_BattMonitor::MPPT_set_powered_state_to_all(const bool power_on) { @@ -1081,6 +1101,19 @@ bool AP_BattMonitor::healthy() const return true; } +#if AP_BATTERY_SCRIPTING_ENABLED +/* + handle state update from a lua script + */ +bool AP_BattMonitor::handle_scripting(uint8_t idx, const BattMonitorScript_State &_state) +{ + if (idx >= _num_instances) { + return false; + } + return drivers[idx]->handle_scripting(_state); +} +#endif + namespace AP { AP_BattMonitor &battery() @@ -1089,3 +1122,5 @@ AP_BattMonitor &battery() } }; + +#endif // AP_BATTERY_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index dbeed2e470ff18..67c138e5f779fa 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -1,12 +1,15 @@ #pragma once +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_ENABLED + #include #include #include #include #include #include "AP_BattMonitor_Params.h" -#include "AP_BattMonitor_config.h" // maximum number of battery monitors #ifndef AP_BATT_MONITOR_MAX_INSTANCES @@ -45,6 +48,7 @@ class AP_BattMonitor_LTC2946; class AP_BattMonitor_Torqeedo; class AP_BattMonitor_FuelLevel_Analog; class AP_BattMonitor_EFI; +class AP_BattMonitor_Scripting; class AP_BattMonitor @@ -70,6 +74,7 @@ class AP_BattMonitor friend class AP_BattMonitor_Torqeedo; friend class AP_BattMonitor_FuelLevel_Analog; friend class AP_BattMonitor_Synthetic_Current; + friend class AP_BattMonitor_Scripting; public: @@ -109,6 +114,7 @@ class AP_BattMonitor INA239_SPI = 26, EFI = 27, AD7091R5 = 28, + Scripting = 29, }; FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t); @@ -151,6 +157,8 @@ class AP_BattMonitor bool powerOffNotified; // only send powering off notification once uint32_t time_remaining; // remaining battery time bool has_time_remaining; // time_remaining is only valid if this is true + uint8_t state_of_health_pct; // state of health (SOH) in percent + bool has_state_of_health_pct; // state_of_health_pct is only valid if this is true uint8_t instance; // instance number of this backend const struct AP_Param::GroupInfo *var_info; }; @@ -271,8 +279,15 @@ class AP_BattMonitor // Returns mavlink fault state uint32_t get_mavlink_fault_bitmask(const uint8_t instance) const; + // return true if state of health (as a percentage) can be provided and fills in soh_pct argument + bool get_state_of_health_pct(uint8_t instance, uint8_t &soh_pct) const; + static const struct AP_Param::GroupInfo var_info[]; +#if AP_BATTERY_SCRIPTING_ENABLED + bool handle_scripting(uint8_t idx, const struct BattMonitorScript_State &state); +#endif + protected: /// parameters @@ -303,3 +318,5 @@ class AP_BattMonitor namespace AP { AP_BattMonitor &battery(); }; + +#endif // AP_BATTERY_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 1aef15a6efda3b..195dbded04047e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_ENABLED + #include #include #include "AP_BattMonitor.h" @@ -107,6 +111,16 @@ void AP_BattMonitor_Backend::update_resistance_estimate() _state.voltage_resting_estimate = _state.voltage + _state.current_amps * _state.resistance; } +// return true if state of health can be provided and fills in soh_pct argument +bool AP_BattMonitor_Backend::get_state_of_health_pct(uint8_t &soh_pct) const +{ + if (!_state.has_state_of_health_pct) { + return false; + } + soh_pct = _state.state_of_health_pct; + return true; +} + float AP_BattMonitor_Backend::voltage_resting_estimate() const { // resting voltage should always be greater than or equal to the raw voltage @@ -324,3 +338,5 @@ void AP_BattMonitor_Backend::update_consumed(AP_BattMonitor::BattMonitor_State & state.consumed_wh += 0.001 * mah * state.voltage; } } + +#endif // AP_BATTERY_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 85d62127708c82..a33c316d70d7bc 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -14,9 +14,14 @@ */ #pragma once -#include +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_ENABLED + #include "AP_BattMonitor.h" +#include + class AP_BattMonitor_Backend { public: @@ -58,12 +63,15 @@ class AP_BattMonitor_Backend // return true if cycle count can be provided and fills in cycles argument virtual bool get_cycle_count(uint16_t &cycles) const { return false; } + // return true if state of health (as a percentage) can be provided and fills in soh_pct argument + bool get_state_of_health_pct(uint8_t &soh_pct) const; + /// get voltage with sag removed (based on battery current draw and resistance) /// this will always be greater than or equal to the raw voltage float voltage_resting_estimate() const; // update battery resistance estimate and voltage_resting_estimate - void update_resistance_estimate(); + virtual void update_resistance_estimate(); // updates failsafe timers, and returns what failsafes are active virtual AP_BattMonitor::Failsafe update_failsafes(void); @@ -95,6 +103,10 @@ class AP_BattMonitor_Backend bool option_is_set(const AP_BattMonitor_Params::Options option) const { return (uint16_t(_params._options.get()) & uint16_t(option)) != 0; } + +#if AP_BATTERY_SCRIPTING_ENABLED + virtual bool handle_scripting(const BattMonitorScript_State &battmon_state) { return false; } +#endif protected: AP_BattMonitor &_mon; // reference to front-end @@ -114,3 +126,25 @@ class AP_BattMonitor_Backend float _resistance_voltage_ref; // voltage used for maximum resistance calculation float _resistance_current_ref; // current used for maximum resistance calculation }; + +#if AP_BATTERY_SCRIPTING_ENABLED +struct BattMonitorScript_State { + float voltage; // Battery voltage in volts + bool healthy; // True if communicating properly + uint8_t cell_count; // Number of valid cells in state + uint8_t capacity_remaining_pct=UINT8_MAX; // Remaining battery capacity in percent, 255 for invalid + uint16_t cell_voltages[32]; // allow script to have up to 32 cells, will be limited internally + uint16_t cycle_count=UINT16_MAX; // Battery cycle count, 65535 for unavailable + /* + all of the following float variables should be set to NaN by the + script if they are not known. + consumed_mah will auto-integrate if set to NaN + */ + float current_amps=nanf(""); // Battery current in amperes + float consumed_mah=nanf(""); // Total current drawn since start-up in milliampere hours + float consumed_wh=nanf(""); // Total energy drawn since start-up in watt hours + float temperature=nanf(""); // Battery temperature in degrees Celsius +}; +#endif // AP_BATTERY_SCRIPTING_ENABLED + +#endif // AP_BATTERY_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 123287c89a92ce..3c7d35fd9e5b9a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -114,14 +114,20 @@ AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneC void AP_BattMonitor_DroneCAN::handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg) { - update_interim_state(msg.voltage, msg.current, msg.temperature, msg.state_of_charge_pct); + update_interim_state(msg.voltage, msg.current, msg.temperature, msg.state_of_charge_pct, msg.state_of_health_pct); WITH_SEMAPHORE(_sem_battmon); _remaining_capacity_wh = msg.remaining_capacity_wh; _full_charge_capacity_wh = msg.full_charge_capacity_wh; + + // consume state of health + if (msg.state_of_health_pct != UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN) { + _interim_state.state_of_health_pct = msg.state_of_health_pct; + _interim_state.has_state_of_health_pct = true; + } } -void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc) +void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc, uint8_t soh_pct) { WITH_SEMAPHORE(_sem_battmon); @@ -145,6 +151,12 @@ void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const fl update_consumed(_interim_state, dt_us); } + // state of health + if (soh_pct != UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN) { + _interim_state.state_of_health_pct = soh_pct; + _interim_state.has_state_of_health_pct = true; + } + // record time _interim_state.last_time_micros = tnow; _interim_state.healthy = true; @@ -185,7 +197,7 @@ void AP_BattMonitor_DroneCAN::handle_mppt_stream(const mppt_Stream &msg) // convert C to Kelvin const float temperature_K = isnan(msg.temperature) ? 0 : C_TO_KELVIN(msg.temperature); - update_interim_state(voltage, current, temperature_K, soc); + update_interim_state(voltage, current, temperature_K, soc, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN); if (!_mppt.is_detected) { // this is the first time the mppt message has been received @@ -283,6 +295,8 @@ void AP_BattMonitor_DroneCAN::read() _state.time_remaining = _interim_state.time_remaining; _state.has_time_remaining = _interim_state.has_time_remaining; _state.is_powering_off = _interim_state.is_powering_off; + _state.state_of_health_pct = _interim_state.state_of_health_pct; + _state.has_state_of_health_pct = _interim_state.has_state_of_health_pct; memcpy(_state.cell_voltages.cells, _interim_state.cell_voltages.cells, sizeof(_state.cell_voltages)); _has_temperature = (AP_HAL::millis() - _state.temperature_time) <= AP_BATT_MONITOR_TIMEOUT; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index 819ade73164817..a25ef9667855c8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -61,7 +61,7 @@ class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend private: void handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg); void handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg); - void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc); + void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc, uint8_t soh_pct); static bool match_battery_id(uint8_t instance, uint8_t battery_id); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp index cab13b302f191a..94be30abc7b9f2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.cpp @@ -31,8 +31,10 @@ extern const AP_HAL::HAL& hal; #define REG_228_CURRENT 0x07 #define REG_228_MANUFACT_ID 0x3e #define REG_228_DEVICE_ID 0x3f +#define REG_228_DIETEMP 0x06 +#define INA_228_TEMP_C_LSB 7.8125e-3 -// INA238 specific registers +// INA237/INA238 specific registers #define REG_238_CONFIG 0x00 #define REG_238_CONFIG_RESET 0x8000 #define REG_238_ADC_CONFIG 0x01 @@ -41,6 +43,8 @@ extern const AP_HAL::HAL& hal; #define REG_238_CURRENT 0x07 #define REG_238_MANUFACT_ID 0x3e #define REG_238_DEVICE_ID 0x3f +#define REG_238_DIETEMP 0x06 +#define INA_238_TEMP_C_LSB 7.8125e-3 // need to mask bottom 4 bits #ifndef DEFAULT_BATTMON_INA2XX_MAX_AMPS #define DEFAULT_BATTMON_INA2XX_MAX_AMPS 90.0 @@ -262,10 +266,12 @@ bool AP_BattMonitor_INA2XX::detect_device(void) if (read_word16(REG_228_MANUFACT_ID, id) && id == 0x5449 && read_word16(REG_228_DEVICE_ID, id) && (id&0xFFF0) == 0x2280) { + has_temp = true; return configure(DevType::INA228); } if (read_word16(REG_238_MANUFACT_ID, id) && id == 0x5449 && read_word16(REG_238_DEVICE_ID, id) && (id&0xFFF0) == 0x2380) { + has_temp = true; return configure(DevType::INA238); } if (read_word16(REG_226_MANUFACT_ID, id) && id == 0x5449 && @@ -311,8 +317,10 @@ void AP_BattMonitor_INA2XX::timer(void) case DevType::INA228: { int32_t bus_voltage24, current24; + int16_t temp16; if (!read_word24(REG_228_VBUS, bus_voltage24) || - !read_word24(REG_228_CURRENT, current24)) { + !read_word24(REG_228_CURRENT, current24) || + !read_word16(REG_228_DIETEMP, temp16)) { failed_reads++; if (failed_reads > 10) { // device has disconnected, we need to reconfigure it @@ -322,13 +330,15 @@ void AP_BattMonitor_INA2XX::timer(void) } voltage = (bus_voltage24>>4) * voltage_LSB; current = (current24>>4) * current_LSB; + temperature = temp16 * INA_228_TEMP_C_LSB; break; } case DevType::INA238: { - int16_t bus_voltage16, current16; + int16_t bus_voltage16, current16, temp16; if (!read_word16(REG_238_VBUS, bus_voltage16) || - !read_word16(REG_238_CURRENT, current16)) { + !read_word16(REG_238_CURRENT, current16) || + !read_word16(REG_238_DIETEMP, temp16)) { failed_reads++; if (failed_reads > 10) { // device has disconnected, we need to reconfigure it @@ -338,6 +348,7 @@ void AP_BattMonitor_INA2XX::timer(void) } voltage = bus_voltage16 * voltage_LSB; current = current16 * current_LSB; + temperature = (temp16&0xFFF0) * INA_238_TEMP_C_LSB; break; } } @@ -350,4 +361,13 @@ void AP_BattMonitor_INA2XX::timer(void) accumulate.count++; } +/* + get last temperature + */ +bool AP_BattMonitor_INA2XX::get_temperature(float &temp) const +{ + temp = temperature; + return has_temp; +} + #endif // AP_BATTERY_INA2XX_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h index d49e7c82746da3..b23b4420eacfc8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_INA2xx.h @@ -17,9 +17,10 @@ class AP_BattMonitor_INA2XX : public AP_BattMonitor_Backend AP_BattMonitor_Params ¶ms); bool has_cell_voltages() const override { return false; } - bool has_temperature() const override { return false; } + bool has_temperature() const override { return has_temp; } bool has_current() const override { return true; } bool get_cycle_count(uint16_t &cycles) const override { return false; } + bool get_temperature(float &temperature) const override; void init(void) override; void read() override; @@ -63,6 +64,10 @@ class AP_BattMonitor_INA2XX : public AP_BattMonitor_Backend } accumulate; float current_LSB; float voltage_LSB; + + float temperature; + + bool has_temp; }; #endif // AP_BATTERY_INA2XX_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp index f5595d321aca32..c7950b847eac2d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp @@ -1,3 +1,8 @@ +#include "AP_BattMonitor_config.h" +#include + +#if AP_BATTERY_ENABLED && HAL_LOGGING_ENABLED + #include "AP_BattMonitor_Backend.h" #include @@ -16,6 +21,9 @@ void AP_BattMonitor_Backend::Log_Write_BAT(const uint8_t instance, const uint64_ temperature_cd = temperature * 100.0; } + uint8_t soh_pct = 0; + IGNORE_RETURN(get_state_of_health_pct(soh_pct)); + const struct log_BAT pkt{ LOG_PACKET_HEADER_INIT(LOG_BAT_MSG), time_us : time_us, @@ -28,7 +36,8 @@ void AP_BattMonitor_Backend::Log_Write_BAT(const uint8_t instance, const uint64_ temperature : temperature_cd, resistance : _state.resistance, rem_percent : percent, - health : _state.healthy + health : _state.healthy, + state_of_health_pct : soh_pct }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } @@ -79,3 +88,5 @@ void AP_BattMonitor_Backend::Log_Write_BCL(const uint8_t instance, const uint64_ } #endif } + +#endif // AP_BATTERY_ENABLED && HAL_LOGGING_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 5302bf77306a66..60cdf5ad769bc5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -1,3 +1,7 @@ +#include "AP_BattMonitor_config.h" + +#if AP_BATTERY_ENABLED + #include #include #include "AP_BattMonitor_Params.h" @@ -17,7 +21,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: MONITOR // @DisplayName: Battery monitoring // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5 + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE), @@ -99,7 +103,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: CRT_MAH // @DisplayName: Battery critical capacity - // @Description: Battery capacity at which the critical battery failsafe is triggered. Set to 0 to disable battery remaining failsafe. If the battery capacity drops below this level the vehicle will perform the failsafe specified by the @PREFIX@_FS_CRT_ACT parameter. + // @Description: Battery capacity at which the critical battery failsafe is triggered. Set to 0 to disable battery remaining failsafe. If the battery capacity drops below this level the vehicle will perform the failsafe specified by the @PREFIX@FS_CRT_ACT parameter. // @Units: mAh // @Increment: 50 // @User: Standard @@ -172,3 +176,5 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { AP_BattMonitor_Params::AP_BattMonitor_Params(void) { AP_Param::setup_object_defaults(this, var_info); } + +#endif // AP_BATTERY_ENABLED diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp new file mode 100644 index 00000000000000..337c9d9b7ac2c1 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.cpp @@ -0,0 +1,84 @@ +#include "AP_BattMonitor_Scripting.h" + +#if AP_BATTERY_SCRIPTING_ENABLED + +#define AP_BATT_MONITOR_SCRIPTING_TIMEOUT_US 5000000 + +bool AP_BattMonitor_Scripting::capacity_remaining_pct(uint8_t &percentage) const +{ + if (internal_state.capacity_remaining_pct != UINT8_MAX) { + percentage = internal_state.capacity_remaining_pct; + return true; + } + // Fall back to default implementation + return AP_BattMonitor_Backend::capacity_remaining_pct(percentage); +} + +bool AP_BattMonitor_Scripting::get_cycle_count(uint16_t &cycles) const +{ + if (internal_state.cycle_count == UINT16_MAX) { + return false; + } + cycles = internal_state.cycle_count; + return true; +} + +// Called by frontend to update the state. Called at 10Hz +void AP_BattMonitor_Scripting::read() +{ + WITH_SEMAPHORE(sem); + + // Check for timeout, to prevent a faulty script from appearing healthy + if (last_update_us == 0 || AP_HAL::micros() - last_update_us > AP_BATT_MONITOR_SCRIPTING_TIMEOUT_US) { + _state.healthy = false; + return; + } + + if (_state.last_time_micros == last_update_us) { + // No new data + return; + } + + /* + the script can fill in voltages up to 32 cells, for mavlink reporting + the extra cell voltages get distributed over the max of 14 for mavlink + */ + for (uint8_t i = 0; i < MIN(AP_BATT_MONITOR_CELLS_MAX,internal_state.cell_count); i++) { + _state.cell_voltages.cells[i] = internal_state.cell_voltages[i]; + } + _state.voltage = internal_state.voltage; + if (!isnan(internal_state.current_amps)) { + _state.current_amps = internal_state.current_amps; + } + if (!isnan(internal_state.consumed_mah)) { + _state.consumed_mah = internal_state.consumed_mah; + } + // Overide integrated consumed energy with script value if it has been set + if (!isnan(internal_state.consumed_wh)) { + _state.consumed_wh = internal_state.consumed_wh; + } + if (!isnan(internal_state.temperature)) { + _state.temperature = internal_state.temperature; + } + + _state.healthy = internal_state.healthy; + + // Update the timestamp (has to be done after the consumed_mah calculation) + _state.last_time_micros = last_update_us; +} + +bool AP_BattMonitor_Scripting::handle_scripting(const BattMonitorScript_State &battmon_state) +{ + WITH_SEMAPHORE(sem); + internal_state = battmon_state; + const uint32_t now_us = AP_HAL::micros(); + uint32_t dt_us = now_us - last_update_us; + if (last_update_us != 0 && !isnan(internal_state.current_amps) && isnan(internal_state.consumed_mah)) { + AP_BattMonitor_Backend::update_consumed(_state, dt_us); + } + last_update_us = now_us; + return true; +} + +#endif // AP_BATTERY_SCRIPTING_ENABLED + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h new file mode 100644 index 00000000000000..f68f11b9dceb5b --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Scripting.h @@ -0,0 +1,32 @@ +#pragma once + +#include "AP_BattMonitor_Backend.h" + +#if AP_BATTERY_SCRIPTING_ENABLED + +class AP_BattMonitor_Scripting : public AP_BattMonitor_Backend +{ +public: + // Inherit constructor + using AP_BattMonitor_Backend::AP_BattMonitor_Backend; + + bool has_current() const override { return last_update_us != 0 && !isnan(internal_state.current_amps); } + bool has_consumed_energy() const override { return has_current(); } + bool has_cell_voltages() const override { return internal_state.cell_count > 0; } + bool has_temperature() const override { return last_update_us != 0 && !isnan(internal_state.temperature); } + bool capacity_remaining_pct(uint8_t &percentage) const override; + bool get_cycle_count(uint16_t &cycles) const override; + + void read() override; + + bool handle_scripting(const BattMonitorScript_State &battmon_state) override; + +protected: + BattMonitorScript_State internal_state; + uint32_t last_update_us; + + HAL_Semaphore sem; +}; + +#endif // AP_BATTMONITOR_SCRIPTING_ENABLED + diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index 92d5ad074e937f..e15909c1435794 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_config.h @@ -116,3 +116,6 @@ #define AP_BATTERY_SMBUS_ROTOYE_ENABLED AP_BATTERY_SMBUS_GENERIC_ENABLED #endif +#ifndef AP_BATTERY_SCRIPTING_ENABLED +#define AP_BATTERY_SCRIPTING_ENABLED (AP_SCRIPTING_ENABLED && AP_BATTERY_BACKEND_DEFAULT_ENABLED) +#endif diff --git a/libraries/AP_BattMonitor/LogStructure.h b/libraries/AP_BattMonitor/LogStructure.h index fdc97512ef0065..5d0bb2cf130432 100644 --- a/libraries/AP_BattMonitor/LogStructure.h +++ b/libraries/AP_BattMonitor/LogStructure.h @@ -19,6 +19,7 @@ // @Field: Res: estimated battery resistance // @Field: RemPct: remaining percentage // @Field: H: health +// @Field: SH: state of health percentage. 0 if unknown struct PACKED log_BAT { LOG_PACKET_HEADER; uint64_t time_us; @@ -32,6 +33,7 @@ struct PACKED log_BAT { float resistance; uint8_t rem_percent; uint8_t health; + uint8_t state_of_health_pct; }; // @LoggerMessage: BCL @@ -61,6 +63,6 @@ struct PACKED log_BCL { #define LOG_STRUCTURE_FROM_BATTMONITOR \ { LOG_BAT_MSG, sizeof(log_BAT), \ - "BAT", "QBfffffcfBB", "TimeUS,Inst,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res,RemPct,H", "s#vvAaXOw%-", "F-000C0?000" , true }, \ + "BAT", "QBfffffcfBBB", "TimeUS,Inst,Volt,VoltR,Curr,CurrTot,EnrgTot,Temp,Res,RemPct,H,SH", "s#vvAaXOw%-%", "F-000C0?0000" , true }, \ { LOG_BCL_MSG, sizeof(log_BCL), \ "BCL", "QBfHHHHHHHHHHHH", "TimeUS,Instance,Volt,V1,V2,V3,V4,V5,V6,V7,V8,V9,V10,V11,V12", "s#vvvvvvvvvvvvv", "F-0CCCCCCCCCCCC" , true }, diff --git a/libraries/AP_Beacon/AP_Beacon.cpp b/libraries/AP_Beacon/AP_Beacon.cpp index 23c8e217245f22..0a2a04e71cb983 100644 --- a/libraries/AP_Beacon/AP_Beacon.cpp +++ b/libraries/AP_Beacon/AP_Beacon.cpp @@ -391,6 +391,7 @@ bool AP_Beacon::device_ready(void) const return ((_driver != nullptr) && (_type != AP_BeaconType_None)); } +#if HAL_LOGGING_ENABLED // Write beacon sensor (position) data void AP_Beacon::log() { @@ -417,6 +418,7 @@ void AP_Beacon::log() }; AP::logger().WriteBlock(&pkt_beacon, sizeof(pkt_beacon)); } +#endif // singleton instance AP_Beacon *AP_Beacon::_singleton; diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h index 18e6a007e94e1c..47bf754da4653c 100644 --- a/libraries/AP_BoardConfig/AP_BoardConfig.h +++ b/libraries/AP_BoardConfig/AP_BoardConfig.h @@ -68,6 +68,7 @@ class AP_BoardConfig { FMUV6_BOARD_HOLYBRO_6X = 40, FMUV6_BOARD_CUAV_6X = 41, FMUV6_BOARD_HOLYBRO_6X_REV6 = 42, + FMUV6_BOARD_HOLYBRO_6X_45686 = 43, PX4_BOARD_OLDDRIVERS = 100, }; diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index c54f872bb94a80..51f32bf8d7d63f 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -107,6 +107,7 @@ void AP_BoardConfig::board_setup_drivers(void) case PX4_BOARD_MINDPXV2: case FMUV6_BOARD_HOLYBRO_6X: case FMUV6_BOARD_HOLYBRO_6X_REV6: + case FMUV6_BOARD_HOLYBRO_6X_45686: case FMUV6_BOARD_CUAV_6X: break; default: @@ -499,6 +500,11 @@ void AP_BoardConfig::detect_fmuv6_variant() state.board_type.set_and_notify(FMUV6_BOARD_CUAV_6X); DEV_PRINTF("Detected CUAV 6X\n"); AP_Param::load_defaults_file("@ROMFS/param/CUAV_V6X_defaults.parm", false); + } else if (spi_check_register("icm45686-1", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686) && + spi_check_register("icm45686-2", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686) && + spi_check_register("icm45686-3", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686)) { + state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X_45686); + DEV_PRINTF("Detected Holybro 6X_45686\n"); } else if (spi_check_register("iim42652", INV3REG_WHOAMI, INV3_WHOAMI_IIM42652) && spi_check_register("icm45686", INV3REG_456_WHOAMI, INV3_WHOAMI_ICM45686)) { state.board_type.set_and_notify(FMUV6_BOARD_HOLYBRO_6X_REV6); diff --git a/libraries/AP_CANManager/AP_CAN.h b/libraries/AP_CANManager/AP_CAN.h index 53e7ccb14fea94..dff85215c15fa9 100644 --- a/libraries/AP_CANManager/AP_CAN.h +++ b/libraries/AP_CANManager/AP_CAN.h @@ -29,5 +29,6 @@ class AP_CAN { Scripting2 = 12, TOFSenseP = 13, NanoRadar_NRA24 = 14, + MR72 = 15, }; }; diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index 025dfd8036e2f8..8e300eba0921bb 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -127,7 +127,7 @@ bool CANSensor::add_interface(AP_HAL::CANIface* can_iface) return false; } - if (!_can_iface->set_event_handle(&_event_handle)) { + if (!_can_iface->set_event_handle(&sem_handle)) { debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle"); return false; } diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index e340b71facdd09..2cd775f4c39f78 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/libraries/AP_CANManager/AP_CANSensor.h @@ -77,7 +77,7 @@ class CANSensor : public AP_CANDriver { bool is_aux_11bit_driver; AP_CANDriver *_can_driver; - HAL_EventHandle _event_handle; + HAL_BinarySemaphore sem_handle; AP_HAL::CANIface* _can_iface; #ifdef HAL_BUILD_AP_PERIPH diff --git a/libraries/AP_CANManager/AP_SLCANIface.cpp b/libraries/AP_CANManager/AP_SLCANIface.cpp index dcd04dcff12f1a..496e8850ed2c1e 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.cpp +++ b/libraries/AP_CANManager/AP_SLCANIface.cpp @@ -561,11 +561,11 @@ void SLCAN::CANIface::update_slcan_port() } } -bool SLCAN::CANIface::set_event_handle(AP_HAL::EventHandle* evt_handle) +bool SLCAN::CANIface::set_event_handle(AP_HAL::BinarySemaphore *sem_handle) { // When in passthrough mode methods is handled through can iface if (_can_iface) { - return _can_iface->set_event_handle(evt_handle); + return _can_iface->set_event_handle(sem_handle); } return false; } diff --git a/libraries/AP_CANManager/AP_SLCANIface.h b/libraries/AP_CANManager/AP_SLCANIface.h index 2f3f35382cdceb..a15ac4f5886c50 100644 --- a/libraries/AP_CANManager/AP_SLCANIface.h +++ b/libraries/AP_CANManager/AP_SLCANIface.h @@ -111,7 +111,7 @@ class CANIface: public AP_HAL::CANIface void reset_params(); // Overriden methods - bool set_event_handle(AP_HAL::EventHandle* evt_handle) override; + bool set_event_handle(AP_HAL::BinarySemaphore *sem_handle) override; uint16_t getNumFilters() const override; uint32_t getErrorCount() const override; void get_stats(ExpandingString &) override; diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a44243c8caccaa..d16d29e441e89a 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -771,6 +771,28 @@ void AP_Camera::convert_params() } } +#if AP_RELAY_ENABLED +// Return true and the relay index if relay camera backend is selected, used for conversion to relay functions +bool AP_Camera::get_legacy_relay_index(int8_t &index) const +{ + // PARAMETER_CONVERSION - Added: Dec-2023 + + // Note that this assumes that the camera param conversion has already been done + // Copter, Plane, Sub and Rover all have both relay and camera and all init relay first + // This will only be a issue if the relay and camera conversion were done at once, if the user skipped 4.4 + for (uint8_t i = 0; i < AP_CAMERA_MAX_INSTANCES; i++) { +#if AP_CAMERA_RELAY_ENABLED + if ((CameraType)_params[i].type.get() == CameraType::RELAY) { + // Camera was hard coded to relay 0 + index = 0; + return true; + } +#endif + } + return false; +} +#endif + // singleton instance AP_Camera *AP_Camera::_singleton; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 356901ab262816..71ef3ffda4614a 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -182,6 +182,9 @@ class AP_Camera { bool get_state(uint8_t instance, camera_state_t& cam_state); #endif + // Return true and the relay index if relay camera backend is selected, used for conversion to relay functions + bool get_legacy_relay_index(int8_t &index) const; + // allow threads to lock against AHRS update HAL_Semaphore &get_semaphore() { return _rsem; } diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index f6cf2c2d2b6089..4781c4180721a9 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -140,7 +140,9 @@ bool AP_Camera_Backend::take_picture() image_index++; last_picture_time_ms = now_ms; IGNORE_RETURN(AP::ahrs().get_location(last_location)); +#if HAL_LOGGING_ENABLED log_picture(); +#endif return true; } @@ -358,16 +360,20 @@ void AP_Camera_Backend::feedback_pin_timer() void AP_Camera_Backend::check_feedback() { if (feedback_trigger_logged_count != feedback_trigger_count) { +#if HAL_LOGGING_ENABLED const uint32_t timestamp32 = feedback_trigger_timestamp_us; +#endif feedback_trigger_logged_count = feedback_trigger_count; // we should consider doing this inside the ISR and pin_timer prep_mavlink_msg_camera_feedback(feedback_trigger_timestamp_us); +#if HAL_LOGGING_ENABLED // log camera message uint32_t tdiff = AP_HAL::micros() - timestamp32; uint64_t timestamp = AP_HAL::micros64(); Write_Camera(timestamp - tdiff); +#endif } } @@ -386,6 +392,7 @@ void AP_Camera_Backend::prep_mavlink_msg_camera_feedback(uint64_t timestamp_us) GCS_SEND_MESSAGE(MSG_CAMERA_FEEDBACK); } +#if HAL_LOGGING_ENABLED // log picture void AP_Camera_Backend::log_picture() { @@ -404,5 +411,6 @@ void AP_Camera_Backend::log_picture() Write_Trigger(); } } +#endif #endif // AP_CAMERA_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 067fa15a080eaa..a6b4c8a60a3200 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -37,8 +37,7 @@ class AP_Camera_Backend CLASS_NO_COPY(AP_Camera_Backend); enum CAMOPTIONS { - NONE = 0, - REC_ARM_DISARM = 1, // Recording start/stop on Arm/Disarm + REC_ARM_DISARM = 0, // Recording start/stop on Arm/Disarm }; // init - performs any required initialisation diff --git a/libraries/AP_Camera/AP_Camera_Logging.cpp b/libraries/AP_Camera/AP_Camera_Logging.cpp index 903f42a8a8ab58..d49a6200c13d95 100644 --- a/libraries/AP_Camera/AP_Camera_Logging.cpp +++ b/libraries/AP_Camera/AP_Camera_Logging.cpp @@ -1,7 +1,8 @@ #include "AP_Camera_Backend.h" #include +#include -#if AP_CAMERA_ENABLED +#if AP_CAMERA_ENABLED && HAL_LOGGING_ENABLED #include #include @@ -85,4 +86,4 @@ void AP_Camera_Backend::Write_Trigger() Write_CameraInfo(LOG_TRIGGER_MSG, 0); } -#endif +#endif // AP_CAMERA_ENABLED && HAL_LOGGING_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 7b32ca60cf1bb8..7fb915082c991e 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -77,7 +77,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _OPTIONS // @DisplayName: Camera options // @Description: Camera options bitmask - // @Bitmask: 0:None,1: Recording Starts at arming and stops at disarming + // @Bitmask: 0:Recording Starts at arming and stops at disarming // @User: Standard AP_GROUPINFO("_OPTIONS", 10, AP_Camera_Params, options, 0), diff --git a/libraries/AP_Camera/AP_Camera_Relay.cpp b/libraries/AP_Camera/AP_Camera_Relay.cpp index a48adeb348d51d..db8fc2adec4811 100644 --- a/libraries/AP_Camera/AP_Camera_Relay.cpp +++ b/libraries/AP_Camera/AP_Camera_Relay.cpp @@ -14,11 +14,7 @@ void AP_Camera_Relay::update() if (ap_relay == nullptr) { return; } - if (_params.relay_on) { - ap_relay->off(0); - } else { - ap_relay->on(0); - } + ap_relay->set(AP_Relay_Params::FUNCTION::CAMERA, !_params.relay_on); } // call parent update @@ -39,11 +35,7 @@ bool AP_Camera_Relay::trigger_pic() return false; } - if (_params.relay_on) { - ap_relay->on(0); - } else { - ap_relay->off(0); - } + ap_relay->set(AP_Relay_Params::FUNCTION::CAMERA, _params.relay_on); // set counter to move servo to off position after this many iterations of update (assumes 50hz update rate) trigger_counter = constrain_float(_params.trigger_duration * 50, 0, UINT16_MAX); diff --git a/libraries/AP_CheckFirmware/AP_CheckFirmware.h b/libraries/AP_CheckFirmware/AP_CheckFirmware.h index 648994fe292f8f..99189642e983df 100644 --- a/libraries/AP_CheckFirmware/AP_CheckFirmware.h +++ b/libraries/AP_CheckFirmware/AP_CheckFirmware.h @@ -39,7 +39,7 @@ enum class check_fw_result_t : uint8_t { #endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(APJ_BOARD_ID) -// this allows for sitl_periph_gps to build +// this allows for sitl_periph to build #define APJ_BOARD_ID 0 #endif diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index 5949ca14cf63ab..65a5648ad66aa4 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -142,6 +142,11 @@ template struct assert_storage_size { _assert_storage_size _member; }; +#define ASSERT_STORAGE_SIZE_JOIN( name, line ) ASSERT_STORAGE_SIZE_DO_JOIN( name, line ) +#define ASSERT_STORAGE_SIZE_DO_JOIN( name, line ) name ## line +#define ASSERT_STORAGE_SIZE(structure, size) \ + do { assert_storage_size ASSERT_STORAGE_SIZE_JOIN(assert_storage_sizex, __LINE__); (void)ASSERT_STORAGE_SIZE_JOIN(assert_storage_sizex, __LINE__); } while(false) + //////////////////////////////////////////////////////////////////////////////// /// @name Conversions /// diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index f466e99db23353..e1a1ec578c51d3 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -27,6 +27,9 @@ void Location::zero(void) // Construct location using position (NEU) from ekf_origin for the given altitude frame Location::Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame) { + // make sure we know what size the Location object is: + ASSERT_STORAGE_SIZE(Location, 16); + zero(); lat = latitude; lng = longitude; @@ -381,10 +384,6 @@ bool Location::sanitize(const Location &defaultLoc) return has_changed; } -// make sure we know what size the Location object is: -assert_storage_size _assert_storage_size_Location; - - // return bearing in radians from location to loc2, return is 0 to 2*Pi ftype Location::get_bearing(const Location &loc2) const { diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 134a27e590af36..eab834e2d427f3 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -15,9 +15,9 @@ class Location uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location // note that mission storage only stores 24 bits of altitude (~ +/- 83km) - int32_t alt; - int32_t lat; - int32_t lng; + int32_t alt; // in cm + int32_t lat; // in 1E7 degrees + int32_t lng; // in 1E7 degrees /// enumeration of possible altitude types enum class AltFrame { diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 18d2cc9136c76c..45eccb97e73756 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -508,12 +508,12 @@ const AP_Param::GroupInfo Compass::var_info[] = { AP_SUBGROUPINFO(_per_motor, "PMOT", 32, Compass, Compass_PerMotor), #endif - // @Param: TYPEMASK + // @Param: DISBLMSK // @DisplayName: Compass disable driver type mask // @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:DroneCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP,18:ExternalAHRS // @User: Advanced - AP_GROUPINFO("TYPEMASK", 33, Compass, _driver_type_mask, 0), + AP_GROUPINFO("DISBLMSK", 33, Compass, _driver_type_mask, 0), // @Param: FLTR_RNG // @DisplayName: Range in which sample is accepted diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 2c7b1ac7418dfb..f08a63e81c96b6 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/libraries/AP_Compass/AP_Compass.h @@ -39,7 +39,7 @@ #define COMPASS_MOT_ENABLED 1 #endif #ifndef COMPASS_LEARN_ENABLED -#define COMPASS_LEARN_ENABLED 1 +#define COMPASS_LEARN_ENABLED AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED #endif // define default compass calibration fitness and consistency checks @@ -342,12 +342,14 @@ friend class AP_Compass_Backend; uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); } +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* fast compass calibration given vehicle position and yaw */ - MAV_RESULT mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, - float lat_deg, float lon_deg, - bool force_use=false); + bool mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, + float lat_deg, float lon_deg, + bool force_use=false); +#endif #if AP_COMPASS_MSP_ENABLED void handle_msp(const MSP::msp_compass_data_message_t &pkt); @@ -405,12 +407,14 @@ friend class AP_Compass_Backend; // see if we already have probed a i2c driver by bus number and address bool _have_i2c_driver(uint8_t bus_num, uint8_t address) const; +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* get mag field with the effects of offsets, diagonals and off-diagonals removed */ bool get_uncorrected_field(uint8_t instance, Vector3f &field) const; - +#endif + #if COMPASS_CAL_ENABLED //keep track of which calibrators have been saved RestrictIDTypeArray _cal_saved; @@ -422,7 +426,7 @@ friend class AP_Compass_Backend; bool _cal_requires_reboot; bool _cal_has_run; - // enum of drivers for COMPASS_TYPEMASK + // enum of drivers for COMPASS_DISBLMSK enum DriverType { #if AP_COMPASS_HMC5843_ENABLED DRIVER_HMC5843 =0, diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index ed8f338e549be9..b8c498483ac05e 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -504,7 +504,7 @@ bool Compass::get_uncorrected_field(uint8_t instance, Vector3f &field) const This assumes that the compass is correctly scaled in milliGauss */ -MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, +bool Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, float lat_deg, float lon_deg, bool force_use) { _reset_compass_id(); @@ -514,7 +514,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, if (!AP::ahrs().get_location(loc)) { if (AP::gps().status() < AP_GPS::GPS_OK_FIX_3D) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: no position available"); - return MAV_RESULT_FAILED; + return false; } loc = AP::gps().location(); } @@ -528,7 +528,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, float inclination; if (!AP_Declination::get_mag_field_ef(lat_deg, lon_deg, intensity, declination, inclination)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag: WMM table error"); - return MAV_RESULT_FAILED; + return false; } // create a field vector and rotate to the required orientation @@ -538,7 +538,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, field = R * field; Matrix3f dcm; - dcm.from_euler(AP::ahrs().roll, AP::ahrs().pitch, radians(yaw_deg)); + dcm.from_euler(AP::ahrs().get_roll(), AP::ahrs().get_pitch(), radians(yaw_deg)); // Rotate into body frame using provided yaw field = dcm.transposed() * field; @@ -553,13 +553,13 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, } if (!healthy(i)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: unhealthy\n", i); - return MAV_RESULT_FAILED; + return false; } Vector3f measurement; if (!get_uncorrected_field(i, measurement)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mag[%u]: bad uncorrected field", i); - return MAV_RESULT_FAILED; + return false; } Vector3f offsets = field - measurement; @@ -572,7 +572,7 @@ MAV_RESULT Compass::mag_cal_fixed_yaw(float yaw_deg, uint8_t compass_mask, #endif } - return MAV_RESULT_ACCEPTED; + return true; } #endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index a9c3d4b10c1e67..6c8f91764598bc 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -126,6 +126,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::probe(AP_HAL::OwnPtr dev return sensor; } +#if AP_INERTIALSENSOR_ENABLED AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(enum Rotation rotation) { AP_InertialSensor &ins = *AP_InertialSensor::get_singleton(); @@ -145,6 +146,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(enum Rotation rotation) return sensor; } +#endif bool AP_Compass_HMC5843::init() { @@ -488,6 +490,7 @@ AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_HALDevice::register_periodic } +#if AP_INERTIALSENSOR_ENABLED /* HMC5843 on an auxiliary bus of IMU driver */ AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id, uint8_t addr) @@ -496,14 +499,12 @@ AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor * Only initialize members. Fails are handled by configure or while * getting the semaphore */ -#if AP_INERTIALSENSOR_ENABLED _bus = ins.get_auxiliary_bus(backend_id); if (!_bus) { return; } _slave = _bus->request_next_slave(addr); -#endif } AP_HMC5843_BusDriver_Auxiliary::~AP_HMC5843_BusDriver_Auxiliary() @@ -585,5 +586,6 @@ uint32_t AP_HMC5843_BusDriver_Auxiliary::get_bus_id(void) const { return _bus->get_bus_id(); } +#endif // AP_INERTIALSENSOR_ENABLED #endif // AP_COMPASS_HMC5843_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index e3427c2d3d529a..a2750c6ceffff2 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -13,6 +13,7 @@ #include #include "AP_Compass_Backend.h" +#include class AuxiliaryBus; class AuxiliaryBusSlave; @@ -26,7 +27,9 @@ class AP_Compass_HMC5843 : public AP_Compass_Backend bool force_external, enum Rotation rotation); +#if AP_INERTIALSENSOR_ENABLED static AP_Compass_Backend *probe_mpu6000(enum Rotation rotation); +#endif static constexpr const char *name = "HMC5843"; @@ -124,6 +127,7 @@ class AP_HMC5843_BusDriver_HALDevice : public AP_HMC5843_BusDriver AP_HAL::OwnPtr _dev; }; +#if AP_INERTIALSENSOR_ENABLED class AP_HMC5843_BusDriver_Auxiliary : public AP_HMC5843_BusDriver { public: @@ -153,5 +157,6 @@ class AP_HMC5843_BusDriver_Auxiliary : public AP_HMC5843_BusDriver AuxiliaryBusSlave *_slave; bool _started; }; +#endif // AP_INERTIALSENSOR_ENABLED #endif // AP_COMPASS_HMC5843_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_RM3100.cpp b/libraries/AP_Compass/AP_Compass_RM3100.cpp index b191a6b52b7ca4..7e4fd7970b5819 100644 --- a/libraries/AP_Compass/AP_Compass_RM3100.cpp +++ b/libraries/AP_Compass/AP_Compass_RM3100.cpp @@ -61,7 +61,6 @@ #define GAIN_CC50 20.0f // LSB/uT #define GAIN_CC100 38.0f #define GAIN_CC200 75.0f -#define UTESLA_TO_MGAUSS 10.0f // uT to mGauss conversion #define TMRC 0x94 // Update rate 150Hz #define CMM 0x71 // read 3 axes and set data ready if 3 axes are ready diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h index ce8989f2be591f..df53675ea8f1ea 100644 --- a/libraries/AP_Compass/AP_Compass_config.h +++ b/libraries/AP_Compass/AP_Compass_config.h @@ -19,7 +19,7 @@ #endif #ifndef AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED -#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED AP_GPS_ENABLED +#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED AP_COMPASS_ENABLED && AP_GPS_ENABLED #endif #define COMPASS_MAX_SCALE_FACTOR 1.5 diff --git a/libraries/AP_Compass/Compass_learn.cpp b/libraries/AP_Compass/Compass_learn.cpp index fe57f712e41936..12861e3fe29ca3 100644 --- a/libraries/AP_Compass/Compass_learn.cpp +++ b/libraries/AP_Compass/Compass_learn.cpp @@ -60,8 +60,8 @@ void CompassLearn::update(void) return; } - const auto result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U< #include #include +#include #if APM_BUILD_TYPE(APM_BUILD_Replay) #include @@ -37,17 +38,23 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype) _last_imu_time_us = imu_us; // we force write all msgs when logging starts +#if HAL_LOGGING_ENABLED bool logging = AP::logger().logging_started() && AP::logger().allow_start_ekf(); if (logging && !logging_started) { force_write = true; } logging_started = logging; +#endif end_frame(); _RFRF.frame_types = uint8_t(frametype); - + +#if AP_VEHICLE_ENABLED _RFRH.time_flying_ms = AP::vehicle()->get_time_flying_ms(); +#else + _RFRH.time_flying_ms = 0; +#endif _RFRH.time_us = AP_HAL::micros64(); WRITE_REPLAY_BLOCK(RFRH, _RFRH); @@ -275,6 +282,7 @@ uint8_t AP_DAL::logging_core(uint8_t c) const #endif } +#if HAL_LOGGING_ENABLED // write out a DAL log message. If old_msg is non-null, then // only write if the content has changed void AP_DAL::WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size) @@ -296,6 +304,7 @@ void AP_DAL::WriteLogMessage(enum LogMessages msg_type, void *msg, const void *o _end = 0; } } +#endif /* check if we are low on CPU for this core. This needs to capture the diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index 671499b547baa2..d8c92e01b53912 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/libraries/AP_DAL/AP_DAL.h @@ -324,9 +324,11 @@ class AP_DAL { // map core number for replay uint8_t logging_core(uint8_t c) const; +#if HAL_LOGGING_ENABLED // write out a DAL log message. If old_msg is non-null, then // only write if the content has changed static void WriteLogMessage(enum LogMessages msg_type, void *msg, const void *old_msg, uint8_t msg_size); +#endif private: @@ -376,10 +378,15 @@ class AP_DAL { bool init_done; }; +#if HAL_LOGGING_ENABLED #define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end)) #define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { static_assert(sizeof(v) == sizeof(old), "types must match"); \ AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, &old, offsetof(log_ ##sname, _end)); } \ while (0) +#else +#define WRITE_REPLAY_BLOCK(sname,v) do { (void)v; } while (false) +#define WRITE_REPLAY_BLOCK_IFCHANGED(sname,v,old) do { (void)old; } while (false) +#endif namespace AP { AP_DAL &dal(); diff --git a/libraries/AP_DAL/AP_DAL_GPS.cpp b/libraries/AP_DAL/AP_DAL_GPS.cpp index 8246157691602d..c57cc3d0631108 100644 --- a/libraries/AP_DAL/AP_DAL_GPS.cpp +++ b/libraries/AP_DAL/AP_DAL_GPS.cpp @@ -3,9 +3,6 @@ #include #include "AP_DAL.h" -// we use a static here as the "location" accessor wants to be const -static Location tmp_location[GPS_MAX_INSTANCES]; - AP_DAL_GPS::AP_DAL_GPS() { for (uint8_t i=0; i +// These are the Goal Interface constants. Because microxrceddsgen does not expose +// them in the generated code, they are manually maintained +// Position ignore flags +static constexpr uint16_t TYPE_MASK_IGNORE_LATITUDE = 1; +static constexpr uint16_t TYPE_MASK_IGNORE_LONGITUDE = 2; +static constexpr uint16_t TYPE_MASK_IGNORE_ALTITUDE = 4; + +bool AP_DDS_External_Control::handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + + if (strcmp(cmd_pos.header.frame_id, MAP_FRAME) == 0) { + // Narrow the altitude + const int32_t alt_cm = static_cast(cmd_pos.altitude * 100); + + Location::AltFrame alt_frame; + if (!convert_alt_frame(cmd_pos.coordinate_frame, alt_frame)) { + return false; + } + + constexpr uint32_t MASK_POS_IGNORE = + TYPE_MASK_IGNORE_LATITUDE | + TYPE_MASK_IGNORE_LONGITUDE | + TYPE_MASK_IGNORE_ALTITUDE; + + if (!(cmd_pos.type_mask & MASK_POS_IGNORE)) { + Location loc(cmd_pos.latitude * 1E7, cmd_pos.longitude * 1E7, alt_cm, alt_frame); + if (!external_control->set_global_position(loc)) { + return false; // Don't try sending other commands if this fails + } + } + + // TODO add velocity and accel handling + + return true; + } + + return false; +} + bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel) { auto *external_control = AP::externalcontrol(); @@ -40,5 +83,25 @@ bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistSta return false; } +bool AP_DDS_External_Control::convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out) +{ + + // Specified in ROS REP-147; only some are supported. + switch (frame_in) { + case 5: // FRAME_GLOBAL_INT + frame_out = Location::AltFrame::ABSOLUTE; + break; + case 6: // FRAME_GLOBAL_REL_ALT + frame_out = Location::AltFrame::ABOVE_HOME; + break; + case 11: // FRAME_GLOBAL_TERRAIN_ALT + frame_out = Location::AltFrame::ABOVE_TERRAIN; + break; + default: + return false; + } + return true; +} + #endif // AP_DDS_ENABLED \ No newline at end of file diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.h b/libraries/AP_DDS/AP_DDS_ExternalControl.h index dbffafdd81ad1c..3986ef63774aa6 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.h +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.h @@ -1,11 +1,19 @@ #pragma once #if AP_DDS_ENABLED +#include "ardupilot_msgs/msg/GlobalPosition.h" #include "geometry_msgs/msg/TwistStamped.h" +#include + class AP_DDS_External_Control { public: + // REP-147 Goal Interface Global Position Control + // https://ros.org/reps/rep-0147.html#goal-interface + static bool handle_global_position_control(ardupilot_msgs_msg_GlobalPosition& cmd_pos); static bool handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel); +private: + static bool convert_alt_frame(const uint8_t frame_in, Location::AltFrame& frame_out); }; #endif // AP_DDS_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 8ce9507d08005d..8df056d484fc71 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -22,6 +22,7 @@ enum class TopicIndex: uint8_t { JOY_SUB, DYNAMIC_TRANSFORMS_SUB, VELOCITY_CONTROL_SUB, + GLOBAL_POSITION_SUB, }; static inline constexpr uint8_t to_underlying(const TopicIndex index) @@ -142,4 +143,14 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .dw_profile_label = "", .dr_profile_label = "velocitycontrol__dr", }, + { + .topic_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), + .pub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), + .sub_id = to_underlying(TopicIndex::GLOBAL_POSITION_SUB), + .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAWRITER_ID}, + .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GLOBAL_POSITION_SUB), .type=UXR_DATAREADER_ID}, + .topic_profile_label = "globalposcontrol__t", + .dw_profile_label = "", + .dr_profile_label = "globalposcontrol__dr", + }, }; diff --git a/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl new file mode 100644 index 00000000000000..7b6325d9e02019 --- /dev/null +++ b/libraries/AP_DDS/Idl/ardupilot_msgs/msg/GlobalPosition.idl @@ -0,0 +1,60 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from ardupilot_msgs/msg/GlobalPosition.msg +// generated code does not contain a copyright notice + +#include "geometry_msgs/msg/Twist.idl" +#include "std_msgs/msg/Header.idl" + +module ardupilot_msgs { + module msg { + module GlobalPosition_Constants { + const uint8 FRAME_GLOBAL_INT = 5; + const uint8 FRAME_GLOBAL_REL_ALT = 6; + const uint8 FRAME_GLOBAL_TERRAIN_ALT = 11; + @verbatim (language="comment", text= + "Position ignore flags") + const uint16 IGNORE_LATITUDE = 1; + const uint16 IGNORE_LONGITUDE = 2; + const uint16 IGNORE_ALTITUDE = 4; + @verbatim (language="comment", text= + "Velocity vector ignore flags") + const uint16 IGNORE_VX = 8; + const uint16 IGNORE_VY = 16; + const uint16 IGNORE_VZ = 32; + @verbatim (language="comment", text= + "Acceleration/Force vector ignore flags") + const uint16 IGNORE_AFX = 64; + const uint16 IGNORE_AFY = 128; + const uint16 IGNORE_AFZ = 256; + @verbatim (language="comment", text= + "Force in af vector flag") + const uint16 FORCE = 512; + const uint16 IGNORE_YAW = 1024; + const uint16 IGNORE_YAW_RATE = 2048; + }; + @verbatim (language="comment", text= + "Experimental REP-147 Goal Interface" "\n" + "https://ros.org/reps/rep-0147.html#goal-interface") + struct GlobalPosition { + std_msgs::msg::Header header; + + uint8 coordinate_frame; + + uint16 type_mask; + + double latitude; + + double longitude; + + @verbatim (language="comment", text= + "in meters, AMSL or above terrain") + float altitude; + + geometry_msgs::msg::Twist velocity; + + geometry_msgs::msg::Twist acceleration_or_force; + + float yaw; + }; + }; +}; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 1af0af6f6001a1..252a6075fba43c 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -189,7 +189,7 @@ Next, follow the associated section for your chosen transport, and finally you c - Run SITL (remember to kill any terminals running ardupilot SITL beforehand) ```console # assuming we are using /dev/pts/1 for Ardupilot SITL - sim_vehicle.py -v ArduPlane -DG --console --enable-dds -A "--uartC=uart:/dev/pts/1" + sim_vehicle.py -v ArduPlane -DG --console --enable-dds -A "--serial1=uart:/dev/pts/1" ``` ## Use ROS 2 CLI @@ -216,6 +216,7 @@ Published topics: * /rosout [rcl_interfaces/msg/Log] 1 publisher Subscribed topics: + * /ap/cmd_gps_pose [ardupilot_msgs/msg/GlobalPosition] 1 subscriber * /ap/cmd_vel [geometry_msgs/msg/TwistStamped] 1 subscriber * /ap/joy [sensor_msgs/msg/Joy] 1 subscriber * /ap/tf [tf2_msgs/msg/TFMessage] 1 subscriber @@ -311,6 +312,10 @@ cp /opt/ros/humble/share/builtin_interfaces/msg/Time.idl libraries/AP_DDS/Idl/bu # Build the code again with the `--enable-dds` flag as described above ``` +If the message is custom for ardupilot, first create the ROS message in `Tools/ros2/ardupilot_msgs/msg/GlobalPosition.msg`. +Then, build ardupilot_msgs with colcon. +Finally, copy the IDL folder from the install directory into the source tree. + ### Rules for adding topics and services to `dds_xrce_profile.xml` Topics and services available from `AP_DDS` are automatically mapped into ROS 2 diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index abfa47779e9abc..b8e80c6e49e6cc 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -276,4 +276,19 @@ rq/ap/mode_switchRequest rr/ap/mode_switchReply + + rt/ap/cmd_gps_pose + ardupilot_msgs::msg::dds_::GlobalPosition_ + + KEEP_LAST + 5 + + + + + NO_KEY + rt/ap/cmd_gps_pose + ardupilot_msgs::msg::dds_::GlobalPosition_ + + diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp index 86bedca40e680d..ebece8962676ac 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -406,10 +406,9 @@ void CanardInterface::process(uint32_t duration_ms) { WITH_SEMAPHORE(_sem_tx); canardCleanupStaleTransfers(&canard, AP_HAL::micros64()); } - uint64_t now = AP_HAL::micros64(); + const uint64_t now = AP_HAL::micros64(); if (now < deadline) { - _event_handle.wait(MIN(UINT16_MAX - 2U, deadline - now)); - hal.scheduler->delay_microseconds(50); + IGNORE_RETURN(sem_handle.wait(deadline - now)); } else { break; } @@ -436,7 +435,7 @@ bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface) AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Can't alloc uavcan::iface\n"); return false; } - if (!can_iface->set_event_handle(&_event_handle)) { + if (!can_iface->set_event_handle(&sem_handle)) { AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Setting event handle failed\n"); return false; } diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h index ce9b75ee923d79..43d786639fdf05 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.h +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -72,7 +72,7 @@ class CanardInterface : public Canard::Interface { static CanardInterface test_iface; #endif uint8_t num_ifaces; - HAL_EventHandle _event_handle; + HAL_BinarySemaphore sem_handle; bool initialized; HAL_Semaphore _sem_tx; HAL_Semaphore _sem_rx; diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 9f34e7f93b10ef..006a3cfed8e0f3 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -55,6 +55,12 @@ #include "AP_DroneCAN_serial.h" #endif +#if AP_RELAY_DRONECAN_ENABLED +#include +#endif + +#include + extern const AP_HAL::HAL& hal; // setup default pool size @@ -153,6 +159,16 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { // @User: Advanced AP_GROUPINFO("ESC_RV", 9, AP_DroneCAN, _esc_rv, 0), +#if AP_RELAY_DRONECAN_ENABLED + // @Param: RLY_RT + // @DisplayName: DroneCAN relay output rate + // @Description: Maximum transmit rate for relay outputs, note that this rate is per message each message does 1 relay, so if with more relays will take longer to update at the same rate, a extra message will be sent when a relay changes state + // @Range: 0 200 + // @Units: Hz + // @User: Advanced + AP_GROUPINFO("RLY_RT", 23, AP_DroneCAN, _relay.rate_hz, 0), +#endif + #if AP_DRONECAN_SERIAL_ENABLED /* due to the parameter tree depth limitation we can't use a sub-table for the serial parameters @@ -252,6 +268,8 @@ const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { #endif #endif // AP_DRONECAN_SERIAL_ENABLED + // RLY_RT is index 23 but has to be above SER_EN so its not hidden + AP_GROUPEND }; @@ -375,6 +393,9 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) #if HAL_MOUNT_XACTI_ENABLED AP_Mount_Xacti::subscribe_msgs(this); #endif +#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + AP_TemperatureSensor_DroneCAN::subscribe_msgs(this); +#endif act_out_array.set_timeout_ms(5); act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH); @@ -434,6 +455,11 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) xacti_gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW); #endif +#if AP_RELAY_DRONECAN_ENABLED + relay_hardpoint.set_timeout_ms(20); + relay_hardpoint.set_priority(CANARD_TRANSFER_PRIORITY_LOW); +#endif + param_save_client.set_timeout_ms(20); param_save_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW); @@ -449,9 +475,6 @@ void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) can_stats.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST); can_stats.set_timeout_ms(3000); - rgb_led.set_timeout_ms(20); - rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW); - node_info_server.set_timeout_ms(20); // setup node status @@ -532,6 +555,10 @@ void AP_DroneCAN::loop(void) #if AP_DRONECAN_SERIAL_ENABLED serial.update(); #endif + +#if AP_RELAY_DRONECAN_ENABLED + relay_hardpoint_send(); +#endif } } @@ -1212,6 +1239,33 @@ void AP_DroneCAN::safety_state_send() } } +// Send relay outputs with hardpoint msg +#if AP_RELAY_DRONECAN_ENABLED +void AP_DroneCAN::relay_hardpoint_send() +{ + const uint32_t now = AP_HAL::millis(); + if ((_relay.rate_hz == 0) || ((now - _relay.last_send_ms) < uint32_t(1000 / _relay.rate_hz))) { + // Rate limit per user config + return; + } + _relay.last_send_ms = now; + + AP_Relay *relay = AP::relay(); + if (relay == nullptr) { + return; + } + + uavcan_equipment_hardpoint_Command msg {}; + + // Relay will populate the next command to send and update the last index + // This will cycle through each relay in turn + if (relay->dronecan.populate_next_command(_relay.last_index, msg)) { + relay_hardpoint.broadcast(msg); + } + +} +#endif // AP_RELAY_DRONECAN_ENABLED + /* handle Button message */ @@ -1305,6 +1359,7 @@ void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const */ void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg) { +#if HAL_LOGGING_ENABLED // log as CSRV message AP::logger().Write_ServoStatus(AP_HAL::micros64(), msg.actuator_id, @@ -1313,6 +1368,7 @@ void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const msg.speed, msg.power_rating_pct, 0, 0, 0, 0, 0, 0); +#endif } #if AP_DRONECAN_HIMARK_SERVO_SUPPORT @@ -1321,6 +1377,7 @@ void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const */ void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg) { +#if HAL_LOGGING_ENABLED // log as CSRV message AP::logger().Write_ServoStatus(AP_HAL::micros64(), msg.servo_id, @@ -1334,12 +1391,14 @@ void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, cons msg.motor_temp*0.2-40, msg.pcb_temp*0.2-40, msg.error_status); +#endif } #endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT #if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg) { +#if HAL_LOGGING_ENABLED AP::logger().WriteStreaming( "CVOL", "TimeUS,Id,Pos,Cur,V,Pow,T", @@ -1353,6 +1412,7 @@ void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, msg.voltage * 0.2f, msg.motor_pwm * (100.0/255.0), int16_t(msg.motor_temperature) - 50); +#endif } #endif diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index 61f0528bc95ff5..8b37c2528b89b9 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -35,6 +35,7 @@ #include #include #include +#include #ifndef DRONECAN_SRV_NUMBER #define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS @@ -165,6 +166,12 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { Canard::Publisher xacti_gimbal_control_data{canard_iface}; Canard::Publisher xacti_gnss_status{canard_iface}; +#if AP_RELAY_DRONECAN_ENABLED + // Hardpoint for relay + // Needs to be public so relay can edge trigger as well as streaming + Canard::Publisher relay_hardpoint{canard_iface}; +#endif + private: void loop(void); @@ -272,6 +279,15 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { uint32_t _last_notify_state_ms; uavcan_protocol_NodeStatus node_status_msg; +#if AP_RELAY_DRONECAN_ENABLED + void relay_hardpoint_send(); + struct { + AP_Int16 rate_hz; + uint32_t last_send_ms; + uint8_t last_index; + } _relay; +#endif + CanardInterface canard_iface; #if AP_DRONECAN_SERIAL_ENABLED diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp index 02b5ed7453647a..1164d4f7346c5d 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp @@ -352,12 +352,14 @@ void AP_DroneCAN_DNA_Server::verify_nodes() return; } +#if HAL_LOGGING_ENABLED uint8_t log_count = AP::logger().get_log_start_count(); if (log_count != last_logging_count) { last_logging_count = log_count; logged.clearall(); } - +#endif + //Check if we got acknowledgement from previous request //except for requests using our own node_id if (curr_verifying_node == self_node_id) { @@ -438,6 +440,7 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co /* if we haven't logged this node then log it now */ +#if HAL_LOGGING_ENABLED if (!logged.get(transfer.source_node_id) && AP::logger().logging_started()) { logged.set(transfer.source_node_id); uint64_t uid[2]; @@ -462,6 +465,7 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co rsp.software_version.minor, rsp.software_version.vcs_commit); } +#endif if (isNodeIDOccupied(transfer.source_node_id)) { //if node_id already registered, just verify if Unique ID matches as well diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp index 6856c539f2e9cf..f0bbd72884d1ad 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_serial.cpp @@ -74,26 +74,29 @@ void AP_DroneCAN_Serial::update(void) if (p.writebuffer == nullptr || p.node <= 0 || p.idx < 0) { continue; } - WITH_SEMAPHORE(p.sem); - uint32_t avail; - const bool send_keepalive = now_ms - p.last_send_ms > 500; - const auto *ptr = p.writebuffer->readptr(avail); - if (!send_keepalive && (ptr == nullptr || avail <= 0)) { - continue; - } uavcan_tunnel_Targetted pkt {}; - auto n = MIN(avail, sizeof(pkt.buffer.data)); - pkt.target_node = p.node; - pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_UNDEFINED; - pkt.buffer.len = n; - pkt.baudrate = p.baudrate; - pkt.serial_id = p.idx; - pkt.options = UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT; - if (ptr != nullptr) { - memcpy(pkt.buffer.data, ptr, n); + uint32_t n = 0; + { + WITH_SEMAPHORE(p.sem); + uint32_t avail; + const bool send_keepalive = now_ms - p.last_send_ms > 500; + const auto *ptr = p.writebuffer->readptr(avail); + if (!send_keepalive && (ptr == nullptr || avail <= 0)) { + continue; + } + n = MIN(avail, sizeof(pkt.buffer.data)); + pkt.target_node = p.node; + pkt.protocol.protocol = UAVCAN_TUNNEL_PROTOCOL_UNDEFINED; + pkt.buffer.len = n; + pkt.baudrate = p.baudrate; + pkt.serial_id = p.idx; + pkt.options = UAVCAN_TUNNEL_TARGETTED_OPTION_LOCK_PORT; + if (ptr != nullptr) { + memcpy(pkt.buffer.data, ptr, n); + } } - if (targetted->broadcast(pkt)) { + WITH_SEMAPHORE(p.sem); p.writebuffer->advance(n); p.last_send_ms = now_ms; } diff --git a/libraries/AP_EFI/AP_EFI.cpp b/libraries/AP_EFI/AP_EFI.cpp index 3cc4d648ffd68d..79bffff77a9577 100644 --- a/libraries/AP_EFI/AP_EFI.cpp +++ b/libraries/AP_EFI/AP_EFI.cpp @@ -28,6 +28,7 @@ #include #include +#include #if HAL_MAX_CAN_PROTOCOL_DRIVERS #include diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index 0ebc163c2d27ed..4af6e8bd72dcb0 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -23,6 +23,8 @@ #include #include +#include + //#define ESC_TELEM_DEBUG #define ESC_RPM_CHECK_TIMEOUT_US 210000UL // timeout for motor running validity @@ -49,7 +51,9 @@ AP_ESC_Telem::AP_ESC_Telem() AP_HAL::panic("Too many AP_ESC_Telem instances"); } _singleton = this; +#if !defined(IOMCU_FW) AP_Param::setup_object_defaults(this, var_info); +#endif } // return the average motor RPM @@ -416,7 +420,7 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem // can only get slightly more up-to-date information that perhaps they were expecting or might // read data that has just gone stale - both of these are safe and avoid the overhead of locking - if (esc_index >= ESC_TELEM_MAX_ESCS) { + if (esc_index >= ESC_TELEM_MAX_ESCS || data_mask == 0) { return; } @@ -486,11 +490,14 @@ void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, cons void AP_ESC_Telem::update() { +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); +#endif const uint32_t now_us = AP_HAL::micros(); for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { +#if HAL_LOGGING_ENABLED // Push received telemetry data into the logging system if (logger && logger->logging_enabled()) { if (_telem_data[i].last_update_ms != _last_telem_log_ms[i] @@ -498,12 +505,12 @@ void AP_ESC_Telem::update() float rpm = 0.0f; get_rpm(i, rpm); - float rawrpm = 0.0f; - get_raw_rpm(i, rawrpm); + float raw_rpm = 0.0f; + get_raw_rpm(i, raw_rpm); // Write ESC status messages // id starts from 0 - // rpm is eRPM (rpm * 100) + // rpm, raw_rpm is eRPM (in RPM units) // voltage is in Volt // current is in Ampere // esc_temp is in centi-degrees Celsius @@ -514,8 +521,8 @@ void AP_ESC_Telem::update() LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC_MSG)), time_us : AP_HAL::micros64(), instance : i, - rpm : (int32_t) rpm * 100, - raw_rpm : (int32_t) rawrpm * 100, + rpm : rpm, + raw_rpm : raw_rpm, voltage : _telem_data[i].voltage, current : _telem_data[i].current, esc_temp : _telem_data[i].temperature_cdeg, @@ -528,6 +535,7 @@ void AP_ESC_Telem::update() _last_rpm_log_us[i] = _rpm_data[i].last_update_us; } } +#endif // HAL_LOGGING_ENABLED if ((now_us - _rpm_data[i].last_update_us) > ESC_RPM_DATA_TIMEOUT_US) { _rpm_data[i].data_valid = false; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 503ef43299dfce..ae40ede5970792 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -32,6 +32,11 @@ class AP_ESC_Telem { // get an individual ESC's raw rpm if available bool get_raw_rpm(uint8_t esc_index, float& rpm) const; + // get raw telemetry data, used by IOMCU + const volatile AP_ESC_Telem_Backend::TelemetryData& get_telem_data(uint8_t esc_index) const { + return _telem_data[esc_index]; + } + // return the average motor RPM float get_average_motor_rpm(uint32_t servo_channel_mask) const; diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp index 0048571452f230..46f5aa61459964 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -20,14 +20,19 @@ #if HAL_WITH_ESC_TELEM #include +#include extern const AP_HAL::HAL& hal; AP_ESC_Telem_Backend::AP_ESC_Telem_Backend() { _frontend = AP_ESC_Telem::_singleton; +#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) + // we allow for no frontend in example fw and tools to make it + // possible to run them on hardware with IOMCU if (_frontend == nullptr) { AP_HAL::panic("No ESC frontend"); } +#endif } // callback to update the rpm in the frontend, should be called by the driver when new data is available diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h index 89d0578875e39a..a3c0dc791cd3df 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -21,8 +21,8 @@ struct PACKED log_Esc { LOG_PACKET_HEADER; uint64_t time_us; uint8_t instance; - int32_t rpm; - int32_t raw_rpm; + float rpm; + float raw_rpm; float voltage; float current; int16_t esc_temp; @@ -33,4 +33,4 @@ struct PACKED log_Esc { #define LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_ESC_MSG, sizeof(log_Esc), \ - "ESC", "QBeeffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-BB--BCB-" , true }, + "ESC", "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true }, diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp index f75d6ac1d4fd7d..9b15e6135bfe21 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -25,8 +25,10 @@ #include "AP_ExternalAHRS_VectorNav.h" #include "AP_ExternalAHRS_MicroStrain5.h" #include "AP_ExternalAHRS_MicroStrain7.h" +#include "AP_ExternalAHRS_InertialLabs.h" #include +#include extern const AP_HAL::HAL &hal; @@ -54,7 +56,7 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = { // @Param: _TYPE // @DisplayName: AHRS type // @Description: Type of AHRS device - // @Values: 0:None,1:VectorNav,2:MicroStrain5,7:MicroStrain7 + // @Values: 0:None,1:VectorNav,2:MicroStrain5,5:InertialLabs,7:MicroStrain7 // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_ExternalAHRS, devtype, HAL_EXTERNAL_AHRS_DEFAULT, AP_PARAM_FLAG_ENABLE), @@ -94,21 +96,31 @@ void AP_ExternalAHRS::init(void) case DevType::None: // nothing to do return; + #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED case DevType::VecNav: backend = new AP_ExternalAHRS_VectorNav(this, state); return; #endif + #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED case DevType::MicroStrain5: backend = new AP_ExternalAHRS_MicroStrain5(this, state); return; #endif + #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED case DevType::MicroStrain7: backend = new AP_ExternalAHRS_MicroStrain7(this, state); return; #endif + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + case DevType::InertialLabs: + backend = new AP_ExternalAHRS_InertialLabs(this, state); + return; +#endif + } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unsupported ExternalAHRS type %u", unsigned(devtype)); @@ -166,6 +178,19 @@ bool AP_ExternalAHRS::get_location(Location &loc) } WITH_SEMAPHORE(state.sem); loc = state.location; + + if (state.last_location_update_us != 0 && + state.have_velocity) { + // extrapolate position based on velocity to cope with slow backends + const float dt = (AP_HAL::micros() - state.last_location_update_us)*1.0e-6; + if (dt < 1) { + // only extrapolate for 1s max + Vector3p ofs = state.velocity.topostype(); + ofs *= dt; + loc.offset(ofs); + } + } + return true; } @@ -242,6 +267,20 @@ void AP_ExternalAHRS::update(void) if (backend) { backend->update(); } + + /* + if backend has not supplied an origin and AHRS has an origin + then use that origin so we get a common origin for minimum + disturbance when switching backends + */ + WITH_SEMAPHORE(state.sem); + if (!state.have_origin) { + Location origin; + if (AP::ahrs().get_origin(origin)) { + state.origin = origin; + state.have_origin = true; + } + } } // Get model/type name diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h index c825b0f609b6b8..c904c68072d19b 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -48,16 +48,19 @@ class AP_ExternalAHRS { #endif #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED MicroStrain5 = 2, +#endif +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + InertialLabs = 5, #endif // 3 reserved for AdNav // 4 reserved for CINS - // 5 reserved for InertialLabs // 6 reserved for Trimble #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED MicroStrain7 = 7, #endif // 8 reserved for SBG // 9 reserved for EulerNav + // 10 reserved for Aeron }; static AP_ExternalAHRS *get_singleton(void) { @@ -96,6 +99,8 @@ class AP_ExternalAHRS { bool have_origin; bool have_location; bool have_velocity; + + uint32_t last_location_update_us; } state; // accessors for AP_AHRS @@ -154,6 +159,11 @@ class AP_ExternalAHRS { float temperature; } ins_data_message_t; + typedef struct { + float differential_pressure; // Pa + float temperature; // degC + } airspeed_data_message_t; + protected: enum class OPTIONS { @@ -175,6 +185,11 @@ class AP_ExternalAHRS { bool has_sensor(AvailableSensor sensor) const { return (uint16_t(sensors.get()) & uint16_t(sensor)) != 0; } + + // set default of EAHRS_SENSORS + void set_default_sensors(uint16_t _sensors) { + sensors.set_default(_sensors); + } }; namespace AP { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp new file mode 100644 index 00000000000000..d10ca6997c7cd7 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -0,0 +1,718 @@ +/* + 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 . + */ +/* + support for serial connected InertialLabs INS + */ + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + +#include "AP_ExternalAHRS_InertialLabs.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +// unit status bits +#define ILABS_UNIT_STATUS_ALIGNMENT_FAIL 0x0001 +#define ILABS_UNIT_STATUS_OPERATION_FAIL 0x0002 +#define ILABS_UNIT_STATUS_GYRO_FAIL 0x0004 +#define ILABS_UNIT_STATUS_ACCEL_FAIL 0x0008 +#define ILABS_UNIT_STATUS_MAG_FAIL 0x0010 +#define ILABS_UNIT_STATUS_ELECTRONICS_FAIL 0x0020 +#define ILABS_UNIT_STATUS_GNSS_FAIL 0x0040 +#define ILABS_UNIT_STATUS_RUNTIME_CAL 0x0080 +#define ILABS_UNIT_STATUS_VOLTAGE_LOW 0x0100 +#define ILABS_UNIT_STATUS_VOLTAGE_HIGH 0x0200 +#define ILABS_UNIT_STATUS_X_RATE_HIGH 0x0400 +#define ILABS_UNIT_STATUS_Y_RATE_HIGH 0x0800 +#define ILABS_UNIT_STATUS_Z_RATE_HIGH 0x1000 +#define ILABS_UNIT_STATUS_MAG_FIELD_HIGH 0x2000 +#define ILABS_UNIT_STATUS_TEMP_RANGE_ERR 0x4000 +#define ILABS_UNIT_STATUS_RUNTIME_CAL2 0x8000 + +// unit status2 bits +#define ILABS_UNIT_STATUS2_ACCEL_X_HIGH 0x0001 +#define ILABS_UNIT_STATUS2_ACCEL_Y_HIGH 0x0002 +#define ILABS_UNIT_STATUS2_ACCEL_Z_HIGH 0x0004 +#define ILABS_UNIT_STATUS2_BARO_FAIL 0x0008 +#define ILABS_UNIT_STATUS2_DIFF_PRESS_FAIL 0x0010 +#define ILABS_UNIT_STATUS2_MAGCAL_2D_ACT 0x0020 +#define ILABS_UNIT_STATUS2_MAGCAL_3D_ACT 0x0020 +#define ILABS_UNIT_STATUS2_GNSS_FUSION_OFF 0x0040 +#define ILABS_UNIT_STATUS2_DIFF_PRESS_FUSION_OFF 0x0080 +#define ILABS_UNIT_STATUS2_MAG_FUSION_OFF 0x0100 +#define ILABS_UNIT_STATUS2_GNSS_POS_VALID 0x0200 + +// air data status bits +#define ILABS_AIRDATA_INIT_FAIL 0x0001 +#define ILABS_AIRDATA_DIFF_PRESS_INIT_FAIL 0x0002 +#define ILABS_AIRDATA_STATIC_PRESS_FAIL 0x0004 +#define ILABS_AIRDATA_DIFF_PRESS_FAIL 0x0008 +#define ILABS_AIRDATA_STATIC_PRESS_RANGE_ERR 0x0010 +#define ILABS_AIRDATA_DIFF_PRESS_RANGE_ERR 0x0020 +#define ILABS_AIRDATA_PRESS_ALT_FAIL 0x0040 +#define ILABS_AIRDATA_AIRSPEED_FAIL 0x0080 +#define ILABS_AIRDATA_BELOW_THRESHOLD 0x0100 + + +// constructor +AP_ExternalAHRS_InertialLabs::AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *_frontend, + AP_ExternalAHRS::state_t &_state) : + AP_ExternalAHRS_backend(_frontend, _state) +{ + auto &sm = AP::serialmanager(); + uart = sm.find_serial(AP_SerialManager::SerialProtocol_AHRS, 0); + if (!uart) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "InertialLabs ExternalAHRS no UART"); + return; + } + baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + + // don't offer IMU by default, at 200Hz it is too slow for many aircraft + set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | + uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | + uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_InertialLabs::update_thread, void), "ILabs", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + AP_HAL::panic("InertialLabs Failed to start ExternalAHRS update thread"); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs ExternalAHRS initialised"); +} + +/* + re-sync buffer on parse failure + */ +void AP_ExternalAHRS_InertialLabs::re_sync(void) +{ + if (buffer_ofs > 3) { + /* + look for the 2 byte header and try to sync to that + */ + const uint16_t header = 0x55AA; + const uint8_t *p = (const uint8_t *)memmem(&buffer[1], buffer_ofs-3, &header, sizeof(header)); + if (p != nullptr) { + const uint16_t n = p - &buffer[0]; + memmove(&buffer[0], p, buffer_ofs - n); + buffer_ofs -= n; + } else { + buffer_ofs = 0; + } + } else { + buffer_ofs = 0; + } +} + +// macro for checking we don't run past end of message buffer +#define CHECK_SIZE(d) need_re_sync = (message_ofs + (msg_len=sizeof(d)) > buffer_end); if (need_re_sync) break + +// lookup a message in the msg_types bitmask to see if we received it in this packet +#define GOT_MSG(mtype) msg_types.get(unsigned(MessageType::mtype)) + +/* + check header is valid + */ +bool AP_ExternalAHRS_InertialLabs::check_header(const ILabsHeader *h) const +{ + return h->magic == 0x55AA && + h->msg_type == 1 && + h->msg_id == 0x95 && + h->msg_len <= sizeof(buffer)-2; +} + +/* + check the UART for more data + returns true if we have consumed potentially valid bytes + */ +bool AP_ExternalAHRS_InertialLabs::check_uart() +{ + WITH_SEMAPHORE(state.sem); + + if (!setup_complete) { + return false; + } + // ensure we own the uart + uart->begin(0); + uint32_t n = uart->available(); + if (n == 0) { + return false; + } + if (n + buffer_ofs > sizeof(buffer)) { + n = sizeof(buffer) - buffer_ofs; + } + const ILabsHeader *h = (ILabsHeader *)&buffer[0]; + + if (buffer_ofs < sizeof(ILabsHeader)) { + n = MIN(n, sizeof(ILabsHeader)-buffer_ofs); + } else { + if (!check_header(h)) { + re_sync(); + return false; + } + if (buffer_ofs > h->msg_len+8) { + re_sync(); + return false; + } + n = MIN(n, uint32_t(h->msg_len + 2 - buffer_ofs)); + } + + const ssize_t nread = uart->read(&buffer[buffer_ofs], n); + if (nread != ssize_t(n)) { + re_sync(); + return false; + } + buffer_ofs += n; + + if (buffer_ofs < sizeof(ILabsHeader)) { + return true; + } + + if (!check_header(h)) { + re_sync(); + return false; + } + + if (buffer_ofs < h->msg_len+2) { + /* + see if we can read the rest immediately + */ + const uint16_t needed = h->msg_len+2 - buffer_ofs; + if (uart->available() < needed) { + // need more data + return true; + } + const ssize_t nread2 = uart->read(&buffer[buffer_ofs], needed); + if (nread2 != needed) { + re_sync(); + return false; + } + buffer_ofs += nread2; + } + + // check checksum + const uint16_t crc1 = crc_sum_of_bytes_16(&buffer[2], buffer_ofs-4); + const uint16_t crc2 = le16toh_ptr(&buffer[buffer_ofs-2]); + if (crc1 != crc2) { + re_sync(); + return false; + } + + const uint8_t *buffer_end = &buffer[buffer_ofs]; + const uint16_t payload_size = h->msg_len - 6; + const uint8_t *payload = &buffer[6]; + if (payload_size < 3) { + re_sync(); + return false; + } + const uint8_t num_messages = payload[0]; + if (num_messages == 0 || + num_messages > payload_size-1) { + re_sync(); + return false; + } + const uint8_t *message_ofs = &payload[num_messages+1]; + bool need_re_sync = false; + + // bitmask for what types we get + Bitmask<256> msg_types; + uint32_t now_ms = AP_HAL::millis(); + + for (uint8_t i=0; i= buffer_end) { + re_sync(); + return false; + } + MessageType mtype = (MessageType)payload[1+i]; + ILabsData &u = *(ILabsData*)message_ofs; + uint8_t msg_len = 0; + + msg_types.set(unsigned(mtype)); + + switch (mtype) { + case MessageType::GPS_INS_TIME_MS: { + // this is the GPS tow timestamp in ms for when the IMU data was sampled + CHECK_SIZE(u.gps_time_ms); + state2.gnss_ins_time_ms = u.gps_time_ms; + break; + } + case MessageType::GPS_WEEK: { + CHECK_SIZE(u.gps_week); + gps_data.gps_week = u.gps_week; + break; + } + case MessageType::ACCEL_DATA_HR: { + CHECK_SIZE(u.accel_data_hr); + ins_data.accel = u.accel_data_hr.tofloat().rfu_to_frd()*GRAVITY_MSS*1.0e-6; + break; + } + case MessageType::GYRO_DATA_HR: { + CHECK_SIZE(u.gyro_data_hr); + ins_data.gyro = u.gyro_data_hr.tofloat().rfu_to_frd()*DEG_TO_RAD*1.0e-5; + break; + } + case MessageType::BARO_DATA: { + CHECK_SIZE(u.baro_data); + baro_data.pressure_pa = u.baro_data.pressure_pa2*2; + break; + } + case MessageType::MAG_DATA: { + CHECK_SIZE(u.mag_data); + mag_data.field = u.mag_data.tofloat().rfu_to_frd()*(10*NTESLA_TO_MGAUSS); + break; + } + case MessageType::ORIENTATION_ANGLES: { + CHECK_SIZE(u.orientation_angles); + state.quat.from_euler(radians(u.orientation_angles.roll*0.01), + radians(u.orientation_angles.pitch*0.01), + radians(u.orientation_angles.yaw*0.01)); + state.have_quaternion = true; + if (last_att_ms == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got link"); + } + last_att_ms = now_ms; + break; + } + case MessageType::VELOCITIES: { + CHECK_SIZE(u.velocity); + state.velocity = u.velocity.tofloat().rfu_to_frd()*0.01; + state.have_velocity = true; + last_vel_ms = now_ms; + break; + } + case MessageType::POSITION: { + CHECK_SIZE(u.position); + state.location.lat = u.position.lat; + state.location.lng = u.position.lon; + state.location.alt = u.position.alt; + state.have_location = true; + state.last_location_update_us = AP_HAL::micros(); + last_pos_ms = now_ms; + break; + } + case MessageType::KF_VEL_COVARIANCE: { + CHECK_SIZE(u.kf_vel_covariance); + state2.kf_vel_covariance = u.kf_vel_covariance.tofloat() * 0.001; + break; + } + case MessageType::KF_POS_COVARIANCE: { + CHECK_SIZE(u.kf_pos_covariance); + state2.kf_pos_covariance = u.kf_pos_covariance.tofloat() * 0.001; + break; + } + case MessageType::UNIT_STATUS: { + CHECK_SIZE(u.unit_status); + state2.unit_status = u.unit_status; + break; + } + case MessageType::GNSS_EXTENDED_INFO: { + CHECK_SIZE(u.gnss_extended_info); + gps_data.fix_type = u.gnss_extended_info.fix_type+1; + state2.gnss_extended_info = u.gnss_extended_info; + break; + } + case MessageType::NUM_SATS: { + CHECK_SIZE(u.num_sats); + gps_data.satellites_in_view = u.num_sats; + break; + } + case MessageType::GNSS_POSITION: { + CHECK_SIZE(u.position); + gps_data.latitude = u.position.lat; + gps_data.longitude = u.position.lon; + gps_data.msl_altitude = u.position.alt; + break; + } + case MessageType::GNSS_VEL_TRACK: { + CHECK_SIZE(u.gnss_vel_track); + Vector2f velxy; + velxy.offset_bearing(u.gnss_vel_track.track_over_ground*0.01, u.gnss_vel_track.hor_speed*0.01); + gps_data.ned_vel_north = velxy.x; + gps_data.ned_vel_east = velxy.y; + gps_data.ned_vel_down = -u.gnss_vel_track.ver_speed*0.01; + break; + } + case MessageType::GNSS_POS_TIMESTAMP: { + CHECK_SIZE(u.gnss_pos_timestamp); + gps_data.ms_tow = u.gnss_pos_timestamp; + break; + } + case MessageType::GNSS_INFO_SHORT: { + CHECK_SIZE(u.gnss_info_short); + state2.gnss_info_short = u.gnss_info_short; + break; + } + case MessageType::GNSS_NEW_DATA: { + CHECK_SIZE(u.gnss_new_data); + state2.gnss_new_data = u.gnss_new_data; + break; + } + case MessageType::GNSS_JAM_STATUS: { + CHECK_SIZE(u.gnss_jam_status); + state2.gnss_jam_status = u.gnss_jam_status; + break; + } + case MessageType::DIFFERENTIAL_PRESSURE: { + CHECK_SIZE(u.differential_pressure); + airspeed_data.differential_pressure = u.differential_pressure*1.0e-4; + break; + } + case MessageType::TRUE_AIRSPEED: { + CHECK_SIZE(u.true_airspeed); + state2.true_airspeed = u.true_airspeed*0.01; + break; + } + case MessageType::WIND_SPEED: { + CHECK_SIZE(u.wind_speed); + state2.wind_speed = u.wind_speed.tofloat().rfu_to_frd()*0.01; + break; + } + case MessageType::AIR_DATA_STATUS: { + CHECK_SIZE(u.air_data_status); + state2.air_data_status = u.air_data_status; + break; + } + case MessageType::SUPPLY_VOLTAGE: { + CHECK_SIZE(u.supply_voltage); + state2.supply_voltage = u.supply_voltage*0.01; + break; + } + case MessageType::TEMPERATURE: { + CHECK_SIZE(u.temperature); + // assume same temperature for baro and airspeed + baro_data.temperature = u.temperature*0.1; + airspeed_data.temperature = u.temperature*0.1; + break; + } + case MessageType::UNIT_STATUS2: { + CHECK_SIZE(u.unit_status2); + state2.unit_status2 = u.unit_status2; + break; + } + } + + if (msg_len == 0) { + // got an unknown message + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: unknown msg 0x%02x", unsigned(mtype)); + buffer_ofs = 0; + return false; + } + message_ofs += msg_len; + + if (msg_len == 0 || need_re_sync) { + re_sync(); + return false; + } + } + + if (h->msg_len != message_ofs-buffer) { + // we had stray bytes at the end of the message + re_sync(); + return false; + } + + if (GOT_MSG(ACCEL_DATA_HR) && + GOT_MSG(GYRO_DATA_HR)) { + AP::ins().handle_external(ins_data); + state.accel = ins_data.accel; + state.gyro = ins_data.gyro; + } + if (GOT_MSG(GPS_INS_TIME_MS) && + GOT_MSG(NUM_SATS) && + GOT_MSG(GNSS_POSITION) && + GOT_MSG(GNSS_NEW_DATA) && + GOT_MSG(GNSS_EXTENDED_INFO) && + state2.gnss_new_data != 0) { + uint8_t instance; + if (AP::gps().get_first_external_instance(instance)) { + AP::gps().handle_external(gps_data, instance); + } + if (gps_data.satellites_in_view > 3) { + if (last_gps_ms == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "InertialLabs: got GPS lock"); + if (!state.have_origin) { + state.origin = Location{ + gps_data.latitude, + gps_data.longitude, + gps_data.msl_altitude, + Location::AltFrame::ABSOLUTE}; + state.have_origin = true; + } + } + last_gps_ms = now_ms; + } + } +#if AP_BARO_EXTERNALAHRS_ENABLED + if (GOT_MSG(BARO_DATA) && + GOT_MSG(TEMPERATURE)) { + AP::baro().handle_external(baro_data); + } +#endif + #if AP_COMPASS_EXTERNALAHRS_ENABLED + if (GOT_MSG(MAG_DATA)) { + AP::compass().handle_external(mag_data); + } +#endif +#if AP_AIRSPEED_EXTERNAL_ENABLED && (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)) + // only on plane and copter as others do not link AP_Airspeed + if (GOT_MSG(DIFFERENTIAL_PRESSURE) && + GOT_MSG(TEMPERATURE)) { + auto *arsp = AP::airspeed(); + if (arsp != nullptr) { + arsp->handle_external(airspeed_data); + } + } +#endif // AP_AIRSPEED_EXTERNAL_ENABLED + buffer_ofs = 0; + +#if HAL_LOGGING_ENABLED + if (GOT_MSG(POSITION) && + GOT_MSG(ORIENTATION_ANGLES) && + GOT_MSG(VELOCITIES)) { + + float roll, pitch, yaw; + state.quat.to_euler(roll, pitch, yaw); + uint64_t now_us = AP_HAL::micros64(); + + // @LoggerMessage: ILB1 + // @Description: InertialLabs AHRS data1 + // @Field: TimeUS: Time since system startup + // @Field: Roll: euler roll + // @Field: Pitch: euler pitch + // @Field: Yaw: euler yaw + // @Field: VN: velocity north + // @Field: VE: velocity east + // @Field: VD: velocity down + // @Field: Lat: latitude + // @Field: Lon: longitude + // @Field: Alt: altitude AMSL + + AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt", + "sdddnnnDUm", + "F000000GG0", + "QffffffLLf", + now_us, + degrees(roll), degrees(pitch), degrees(yaw), + state.velocity.x, state.velocity.y, state.velocity.z, + state.location.lat, state.location.lng, state.location.alt*0.01); + + // @LoggerMessage: ILB2 + // @Description: InertialLabs AHRS data2 + // @Field: TimeUS: Time since system startup + // @Field: PosVarN: position variance north + // @Field: PosVarE: position variance east + // @Field: PosVarD: position variance down + // @Field: VelVarN: velocity variance north + // @Field: VelVarE: velocity variance east + // @Field: VelVarD: velocity variance down + + AP::logger().WriteStreaming("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD", + "smmmnnn", + "F000000", + "Qffffff", + now_us, + state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z, + state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z); + + // @LoggerMessage: ILB3 + // @Description: InertialLabs AHRS data3 + // @Field: TimeUS: Time since system startup + // @Field: Stat1: unit status1 + // @Field: Stat2: unit status2 + // @Field: FType: fix type + // @Field: SpStat: spoofing status + // @Field: GI1: GNSS Info1 + // @Field: GI2: GNSS Info2 + // @Field: GJS: GNSS jamming status + // @Field: TAS: true airspeed + // @Field: WVN: Wind velocity north + // @Field: WVE: Wind velocity east + // @Field: WVD: Wind velocity down + + AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD", + "s-----------", + "F-----------", + "QHHBBBBBffff", + now_us, + state2.unit_status, state2.unit_status2, + state2.gnss_extended_info.fix_type, state2.gnss_extended_info.spoofing_status, + state2.gnss_info_short.info1, state2.gnss_info_short.info2, + state2.gnss_jam_status, + state2.true_airspeed, + state2.wind_speed.x, state2.wind_speed.y, state2.wind_speed.z); + } +#endif // HAL_LOGGING_ENABLED + + return true; +} + +void AP_ExternalAHRS_InertialLabs::update_thread() +{ + // Open port in the thread + uart->begin(baudrate, 1024, 512); + + /* + we assume the user has already configured the device + */ + + setup_complete = true; + while (true) { + if (!check_uart()) { + hal.scheduler->delay_microseconds(250); + } + } +} + +// get serial port number for the uart +int8_t AP_ExternalAHRS_InertialLabs::get_port(void) const +{ + if (!uart) { + return -1; + } + return port_num; +}; + +// accessors for AP_AHRS +bool AP_ExternalAHRS_InertialLabs::healthy(void) const +{ + WITH_SEMAPHORE(state.sem); + return AP_HAL::millis() - last_att_ms < 100; +} + +bool AP_ExternalAHRS_InertialLabs::initialised(void) const +{ + if (!setup_complete) { + return false; + } + return true; +} + +bool AP_ExternalAHRS_InertialLabs::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + if (!setup_complete) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs setup failed"); + return false; + } + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs unhealthy"); + return false; + } + WITH_SEMAPHORE(state.sem); + uint32_t now = AP_HAL::millis(); + if (now - last_att_ms > 10 || + now - last_pos_ms > 10 || + now - last_vel_ms > 10) { + hal.util->snprintf(failure_msg, failure_msg_len, "InertialLabs not up to date"); + return false; + } + return true; +} + +/* + get filter status. We don't know the meaning of the status bits yet, + so assume all OK if we have GPS lock + */ +void AP_ExternalAHRS_InertialLabs::get_filter_status(nav_filter_status &status) const +{ + WITH_SEMAPHORE(state.sem); + uint32_t now = AP_HAL::millis(); + const uint32_t dt_limit = 200; + const uint32_t dt_limit_gps = 500; + memset(&status, 0, sizeof(status)); + const bool init_ok = (state2.unit_status & (ILABS_UNIT_STATUS_ALIGNMENT_FAIL|ILABS_UNIT_STATUS_OPERATION_FAIL))==0; + status.flags.initalized = init_ok; + status.flags.attitude = init_ok && (now - last_att_ms < dt_limit) && init_ok; + status.flags.vert_vel = init_ok && (now - last_vel_ms < dt_limit); + status.flags.vert_pos = init_ok && (now - last_pos_ms < dt_limit); + status.flags.horiz_vel = status.flags.vert_vel; + status.flags.horiz_pos_abs = status.flags.vert_pos; + status.flags.horiz_pos_rel = status.flags.horiz_pos_abs; + status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_abs; + status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; + status.flags.using_gps = (now - last_gps_ms < dt_limit_gps) && + (state2.unit_status & (ILABS_UNIT_STATUS_GNSS_FAIL|ILABS_UNIT_STATUS2_GNSS_FUSION_OFF)) == 0; + status.flags.gps_quality_good = (now - last_gps_ms < dt_limit_gps) && + (state2.unit_status2 & ILABS_UNIT_STATUS2_GNSS_POS_VALID) != 0 && + (state2.unit_status & ILABS_UNIT_STATUS_GNSS_FAIL) == 0; + status.flags.rejecting_airspeed = (state2.air_data_status & ILABS_AIRDATA_AIRSPEED_FAIL); +} + +// send an EKF_STATUS message to GCS +void AP_ExternalAHRS_InertialLabs::send_status_report(GCS_MAVLINK &link) const +{ + // prepare flags + uint16_t flags = 0; + nav_filter_status filterStatus; + get_filter_status(filterStatus); + if (filterStatus.flags.attitude) { + flags |= EKF_ATTITUDE; + } + if (filterStatus.flags.horiz_vel) { + flags |= EKF_VELOCITY_HORIZ; + } + if (filterStatus.flags.vert_vel) { + flags |= EKF_VELOCITY_VERT; + } + if (filterStatus.flags.horiz_pos_rel) { + flags |= EKF_POS_HORIZ_REL; + } + if (filterStatus.flags.horiz_pos_abs) { + flags |= EKF_POS_HORIZ_ABS; + } + if (filterStatus.flags.vert_pos) { + flags |= EKF_POS_VERT_ABS; + } + if (filterStatus.flags.terrain_alt) { + flags |= EKF_POS_VERT_AGL; + } + if (filterStatus.flags.const_pos_mode) { + flags |= EKF_CONST_POS_MODE; + } + if (filterStatus.flags.pred_horiz_pos_rel) { + flags |= EKF_PRED_POS_HORIZ_REL; + } + if (filterStatus.flags.pred_horiz_pos_abs) { + flags |= EKF_PRED_POS_HORIZ_ABS; + } + if (!filterStatus.flags.initalized) { + flags |= EKF_UNINITIALIZED; + } + + // send message + const float vel_gate = 5; + const float pos_gate = 5; + const float hgt_gate = 5; + const float mag_var = 0; + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, + state2.kf_vel_covariance.length()/vel_gate, + state2.kf_pos_covariance.xy().length()/pos_gate, + state2.kf_pos_covariance.z/hgt_gate, + mag_var, 0, 0); +} + +#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h new file mode 100644 index 00000000000000..bee7d5c28e0cce --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.h @@ -0,0 +1,226 @@ +/* + 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 . + */ +/* + support for serial connected InertialLabs INS system + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + +#include "AP_ExternalAHRS_backend.h" + +class AP_ExternalAHRS_InertialLabs : public AP_ExternalAHRS_backend { + +public: + AP_ExternalAHRS_InertialLabs(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); + + // get serial port number, -1 for not enabled + int8_t get_port(void) const override; + + // accessors for AP_AHRS + bool healthy(void) const override; + bool initialised(void) const override; + bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override; + void get_filter_status(nav_filter_status &status) const override; + void send_status_report(class GCS_MAVLINK &link) const override; + + // check for new data + void update() override { + check_uart(); + } + + // Get model/type name + const char* get_name() const override { + return "ILabs"; + } + + enum class MessageType : uint8_t { + GPS_INS_TIME_MS = 0x01, + GPS_WEEK = 0x3C, + ACCEL_DATA_HR = 0x23, + GYRO_DATA_HR = 0x21, + BARO_DATA = 0x25, + MAG_DATA = 0x24, + ORIENTATION_ANGLES = 0x07, + VELOCITIES = 0x12, + POSITION = 0x10, + KF_VEL_COVARIANCE = 0x58, + KF_POS_COVARIANCE = 0x57, + UNIT_STATUS = 0x53, + GNSS_EXTENDED_INFO = 0x4A, + NUM_SATS = 0x3B, + GNSS_POSITION = 0x30, + GNSS_VEL_TRACK = 0x32, + GNSS_POS_TIMESTAMP = 0x3E, + GNSS_INFO_SHORT = 0x36, + GNSS_NEW_DATA = 0x41, + GNSS_JAM_STATUS = 0xC0, + DIFFERENTIAL_PRESSURE = 0x28, + TRUE_AIRSPEED = 0x86, + WIND_SPEED = 0x8A, + AIR_DATA_STATUS = 0x8D, + SUPPLY_VOLTAGE = 0x50, + TEMPERATURE = 0x52, + UNIT_STATUS2 = 0x5A, + }; + + /* + packets consist of: + ILabsHeader + list of MessageType + sequence of ILabsData + checksum + */ + struct PACKED ILabsHeader { + uint16_t magic; // 0x55AA + uint8_t msg_type; // always 1 for INS data + uint8_t msg_id; // always 0x95 + uint16_t msg_len; // msg_len+2 is total packet length + }; + + struct PACKED vec3_16_t { + int16_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + struct PACKED vec3_32_t { + int32_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + struct PACKED vec3_u8_t { + uint8_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + struct PACKED vec3_u16_t { + uint16_t x,y,z; + Vector3f tofloat(void) { + return Vector3f(x,y,z); + } + }; + + struct gnss_extended_info_t { + uint8_t fix_type; + uint8_t spoofing_status; + }; + + struct gnss_info_short_t { + uint8_t info1; + uint8_t info2; + }; + + union PACKED ILabsData { + uint32_t gps_time_ms; // ms since start of GPS week + uint16_t gps_week; + vec3_32_t accel_data_hr; // g * 1e6 + vec3_32_t gyro_data_hr; // deg/s * 1e5 + struct PACKED { + uint16_t pressure_pa2; // Pascals/2 + int32_t baro_alt; // meters*100 + } baro_data; + vec3_16_t mag_data; // nT/10 + struct PACKED { + int16_t yaw; // deg*100 + int16_t pitch; // deg*100 + int16_t roll; // deg*100 + } orientation_angles; // 321 euler order? + vec3_32_t velocity; // m/s * 100 + struct PACKED { + int32_t lat; // deg*1e7 + int32_t lon; // deg*1e7 + int32_t alt; // m*100, AMSL + } position; + vec3_u8_t kf_vel_covariance; // mm/s + vec3_u16_t kf_pos_covariance; + uint16_t unit_status; // set ILABS_UNIT_STATUS_* + gnss_extended_info_t gnss_extended_info; + uint8_t num_sats; + struct PACKED { + int32_t hor_speed; // m/s*100 + uint16_t track_over_ground; // deg*100 + int32_t ver_speed; // m/s*100 + } gnss_vel_track; + uint32_t gnss_pos_timestamp; // ms + gnss_info_short_t gnss_info_short; + uint8_t gnss_new_data; + uint8_t gnss_jam_status; + int32_t differential_pressure; // mbar*1e4 + int16_t true_airspeed; // m/s*100 + vec3_16_t wind_speed; // m/s*100 + uint16_t air_data_status; + uint16_t supply_voltage; // V*100 + int16_t temperature; // degC*10 + uint16_t unit_status2; + }; + + AP_ExternalAHRS::gps_data_message_t gps_data; + AP_ExternalAHRS::mag_data_message_t mag_data; + AP_ExternalAHRS::baro_data_message_t baro_data; + AP_ExternalAHRS::ins_data_message_t ins_data; + AP_ExternalAHRS::airspeed_data_message_t airspeed_data; + + uint16_t buffer_ofs; + uint8_t buffer[256]; // max for normal message set is 167+8 + +private: + AP_HAL::UARTDriver *uart; + int8_t port_num; + uint32_t baudrate; + bool setup_complete; + + void update_thread(); + bool check_uart(); + bool check_header(const ILabsHeader *h) const; + + // re-sync on header bytes + void re_sync(void); + + static const struct MessageLength { + MessageType mtype; + uint8_t length; + } message_lengths[]; + + struct { + Vector3f kf_vel_covariance; + Vector3f kf_pos_covariance; + uint32_t gnss_ins_time_ms; + uint16_t unit_status; + uint16_t unit_status2; + gnss_extended_info_t gnss_extended_info; + gnss_info_short_t gnss_info_short; + uint8_t gnss_new_data; + uint8_t gnss_jam_status; + float differential_pressure; + float true_airspeed; + Vector3f wind_speed; + uint16_t air_data_status; + float supply_voltage; + } state2; + + uint32_t last_att_ms; + uint32_t last_vel_ms; + uint32_t last_pos_ms; + uint32_t last_gps_ms; +}; + +#endif // AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED + diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp index 141b4ff857e7c9..ed6304429c28d8 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -163,6 +163,7 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const state.location = Location{filter_data.lat, filter_data.lon, gnss_data[gnss_instance].msl_altitude, Location::AltFrame::ABSOLUTE}; state.have_location = true; + state.last_location_update_us = AP_HAL::micros(); } AP_ExternalAHRS::gps_data_message_t gps { diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp index 1abf9436355e03..09762f67d6aa88 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -16,7 +16,7 @@ $ sim_vehicle.py -v Plane -A "--serial3=uart:/dev/3dm-gq7" --console --map -DG $ ./Tools/autotest/sim_vehicle.py -v Plane -A "--serial3=uart:/dev/3dm-gq7" -DG param set AHRS_EKF_TYPE 11 - param set EAHRS_TYPE 4 + param set EAHRS_TYPE 7 param set GPS_TYPE 21 param set SERIAL3_BAUD 115 param set SERIAL3_PROTOCOL 36 @@ -65,6 +65,11 @@ AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *_fro AP_BoardConfig::allocation_error("MicroStrain7 failed to allocate ExternalAHRS update thread"); } + // don't offer IMU by default, at 100Hz it is too slow for many aircraft + set_default_sensors(uint16_t(AP_ExternalAHRS::AvailableSensor::GPS) | + uint16_t(AP_ExternalAHRS::AvailableSensor::BARO) | + uint16_t(AP_ExternalAHRS::AvailableSensor::COMPASS)); + hal.scheduler->delay(5000); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain7 ExternalAHRS initialised"); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 8e8515b70aa17d..26f379868a2dff 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp @@ -208,6 +208,8 @@ bool AP_ExternalAHRS_VectorNav::check_uart() return false; } WITH_SEMAPHORE(state.sem); + // ensure we own the uart + uart->begin(0); uint32_t n = uart->available(); if (n == 0) { return false; @@ -475,6 +477,7 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) int32_t(pkt1.positionLLA[1] * 1.0e7), int32_t(pkt1.positionLLA[2] * 1.0e2), Location::AltFrame::ABSOLUTE}; + state.last_location_update_us = AP_HAL::micros(); state.have_location = true; } @@ -510,6 +513,7 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) } +#if HAL_LOGGING_ENABLED // @LoggerMessage: EAH1 // @Description: External AHRS data // @Field: TimeUS: Time since system startup @@ -538,6 +542,7 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b) float(pkt1.positionLLA[2]), pkt1.posU, pkt1.velU, pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]); +#endif // HAL_LOGGING_ENABLED } /* @@ -648,6 +653,7 @@ void AP_ExternalAHRS_VectorNav::process_packet_VN_100(const uint8_t *b) AP::ins().handle_external(ins); } +#if HAL_LOGGING_ENABLED // @LoggerMessage: EAH3 // @Description: External AHRS data // @Field: TimeUS: Time since system startup @@ -678,7 +684,7 @@ void AP_ExternalAHRS_VectorNav::process_packet_VN_100(const uint8_t *b) state.accel[0], state.accel[1], state.accel[2], state.gyro[0], state.gyro[1], state.gyro[2], state.quat[0], state.quat[1], state.quat[2], state.quat[3]); - +#endif // HAL_LOGGING_ENABLED } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index 07436b7ae43888..02eb1b5319b7d0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -53,6 +53,11 @@ class AP_ExternalAHRS_backend { uint16_t get_rate(void) const; bool option_is_set(AP_ExternalAHRS::OPTIONS option) const; + // set default of EAHRS_SENSORS + void set_default_sensors(uint16_t sensors) { + frontend.set_default_sensors(sensors); + } + private: AP_ExternalAHRS &frontend; }; diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h index 3b2caa23870462..75e8e5228021bb 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -19,9 +19,13 @@ #endif #ifndef AP_MICROSTRAIN_ENABLED -#define AP_MICROSTRAIN_ENABLED AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED +#define AP_MICROSTRAIN_ENABLED AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED || AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED #endif #ifndef AP_EXTERNAL_AHRS_VECTORNAV_ENABLED #define AP_EXTERNAL_AHRS_VECTORNAV_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED +#define AP_EXTERNAL_AHRS_INERTIAL_LABS_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp index 66d324df99554a..19a6e22509c7e8 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.cpp +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.cpp @@ -11,7 +11,7 @@ along with this program. If not, see . */ /* - support for MicroStrain CX5/GX5-45 serially connected AHRS Systems + support for MicroStrain MIP parsing */ #define ALLOW_DOUBLE_MATH_FUNCTIONS diff --git a/libraries/AP_ExternalControl/AP_ExternalControl.h b/libraries/AP_ExternalControl/AP_ExternalControl.h index b72190d9c4da81..34228e2b7ab4ff 100644 --- a/libraries/AP_ExternalControl/AP_ExternalControl.h +++ b/libraries/AP_ExternalControl/AP_ExternalControl.h @@ -8,6 +8,7 @@ #if AP_EXTERNAL_CONTROL_ENABLED +#include #include class AP_ExternalControl @@ -24,9 +25,18 @@ class AP_ExternalControl return false; } + /* + Sets the target global position with standard guided mode behavior. + */ + virtual bool set_global_position(const Location& loc) WARN_IF_UNUSED { + return false; + } + static AP_ExternalControl *get_singleton(void) WARN_IF_UNUSED { return singleton; } +protected: + ~AP_ExternalControl() {} private: static AP_ExternalControl *singleton; diff --git a/libraries/AP_FETtecOneWire/README.md b/libraries/AP_FETtecOneWire/README.md index 482670b0fbf832..7f0d2549f02f44 100644 --- a/libraries/AP_FETtecOneWire/README.md +++ b/libraries/AP_FETtecOneWire/README.md @@ -30,7 +30,7 @@ For purchase, connection and configuration information please see the [ArduPilot - check that the ESCs are periodically sending telemetry data - re-init and configure an ESC(s) if not armed (motors not spinning) when - telemetry communication with the ESC(s) is lost -- adds a serial simulator (--uartF=sim:fetteconewireesc) of FETtec OneWire ESCs +- adds a serial simulator (--serial5=sim:fetteconewireesc) of FETtec OneWire ESCs - adds autotest (using the simulator) to: - simulate telemetry voltage, current, temperature, RPM data using SITL internal variables - test the safety switch functionality diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index c0ab14b9869049..e62ec5920827c3 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -333,6 +333,30 @@ AP_Filesystem_Backend::FormatStatus AP_Filesystem::get_format_status(void) const } #endif +/* + stat wrapper for scripting + */ +bool AP_Filesystem::stat(const char *pathname, stat_t &stbuf) +{ + struct stat st; + if (fs.stat(pathname, &st) != 0) { + return false; + } + stbuf.size = st.st_size; + stbuf.mode = st.st_mode; + // these wrap in 2038 + stbuf.atime = st.st_atime; + stbuf.ctime = st.st_ctime; + stbuf.mtime = st.st_mtime; + return true; +} + +// get_singleton for scripting +AP_Filesystem *AP_Filesystem::get_singleton(void) +{ + return &fs; +} + namespace AP { AP_Filesystem &FS() diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index 585a2f5e2eb2c9..b139713a751f59 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -82,6 +82,20 @@ class AP_Filesystem { int fsync(int fd); int32_t lseek(int fd, int32_t offset, int whence); int stat(const char *pathname, struct stat *stbuf); + + // stat variant for scripting + typedef struct { + uint32_t size; + int32_t mode; + uint32_t mtime; + uint32_t atime; + uint32_t ctime; + bool is_directory(void) const { + return (mode & S_IFMT) == S_IFDIR; + } + } stat_t; + bool stat(const char *pathname, stat_t &stbuf); + int unlink(const char *pathname); int mkdir(const char *pathname); int rename(const char *oldpath, const char *newpath); @@ -121,7 +135,10 @@ class AP_Filesystem { load a full file. Use delete to free the data */ FileData *load_file(const char *filename); - + + // get_singleton for scripting + static AP_Filesystem *get_singleton(void); + private: struct Backend { const char *prefix; diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp index 59cbd357624114..df47485a307218 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #if 0 #define debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) @@ -416,7 +417,10 @@ int32_t AP_Filesystem_FATFS::read(int fd, void *buf, uint32_t count) UINT total = 0; do { UINT size = 0; - UINT n = MIN(bytes, MAX_IO_SIZE); + UINT n = bytes; + if (!mem_is_dma_safe(buf, count, true)) { + n = MIN(bytes, MAX_IO_SIZE); + } res = f_read(fh, (void *)buf, n, &size); if (res != FR_OK) { errno = fatfs_to_errno((FRESULT)res); @@ -460,7 +464,10 @@ int32_t AP_Filesystem_FATFS::write(int fd, const void *buf, uint32_t count) UINT total = 0; do { - UINT n = MIN(bytes, MAX_IO_SIZE); + UINT n = bytes; + if (!mem_is_dma_safe(buf, count, true)) { + n = MIN(bytes, MAX_IO_SIZE); + } UINT size = 0; res = f_write(fh, buf, n, &size); if (res == FR_DISK_ERR && RETRY_ALLOWED()) { diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp index 59103f3f8a2f25..cba03addcdf75b 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp @@ -27,6 +27,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; extern int errno; @@ -211,10 +212,12 @@ int AP_Filesystem_Mission::stat(const char *name, struct stat *stbuf) */ bool AP_Filesystem_Mission::check_file_name(const char *name, enum MAV_MISSION_TYPE &mtype) { +#if AP_MISSION_ENABLED if (strcmp(name, "mission.dat") == 0) { mtype = MAV_MISSION_TYPE_MISSION; return true; } +#endif #if AP_FENCE_ENABLED if (strcmp(name, "fence.dat") == 0) { mtype = MAV_MISSION_TYPE_FENCE; @@ -261,6 +264,7 @@ bool AP_Filesystem_Mission::get_item(uint32_t idx, enum MAV_MISSION_TYPE mtype, uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const { switch (mtype) { +#if AP_MISSION_ENABLED case MAV_MISSION_TYPE_MISSION: { auto *mission = AP::mission(); if (!mission) { @@ -268,18 +272,17 @@ uint32_t AP_Filesystem_Mission::get_num_items(enum MAV_MISSION_TYPE mtype) const } return mission->num_commands(); } - - case MAV_MISSION_TYPE_FENCE: { +#endif + #if AP_FENCE_ENABLED + case MAV_MISSION_TYPE_FENCE: { auto *fence = AP::fence(); if (fence == nullptr) { return 0; } return fence->polyfence().num_stored_items(); -#else - return 0; -#endif } +#endif #if HAL_RALLY_ENABLED case MAV_MISSION_TYPE_RALLY: { @@ -381,6 +384,30 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r) } } + switch (r.mtype) { +#if AP_MISSION_ENABLED + case MAV_MISSION_TYPE_MISSION: + return finish_upload_mission(hdr, r, b); +#endif +#if HAL_RALLY_ENABLED + case MAV_MISSION_TYPE_RALLY: + return finish_upload_rally(hdr, r, b); +#endif +#if AP_FENCE_ENABLED + case MAV_MISSION_TYPE_FENCE: + return finish_upload_fence(hdr, r, b); +#endif + default: + // really should not get here.... + break; + } + + return false; +} + +#if AP_MISSION_ENABLED +bool AP_Filesystem_Mission::finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b) +{ auto *mission = AP::mission(); if (mission == nullptr) { return false; @@ -389,16 +416,17 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r) if ((hdr.options & unsigned(Options::NO_CLEAR)) == 0) { mission->clear(); } - for (uint32_t i=0; i= nitems || cmd.content.jump.target == 0)) { + (cmd.content.jump.target >= hdr.num_items || cmd.content.jump.target == 0)) { return false; } uint16_t idx = i + hdr.start; @@ -414,5 +442,92 @@ bool AP_Filesystem_Mission::finish_upload(const rfile &r) } return true; } +#endif // AP_MISSION_ENABLED + +#if AP_FENCE_ENABLED +bool AP_Filesystem_Mission::finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b) +{ + bool success = false; + + AC_PolyFenceItem *new_items = nullptr; + + auto *fence = AP::fence(); + if (fence == nullptr) { + goto OUT; + } + + if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) { + // Only complete fences can be uploaded for now. + goto OUT; + } + + // passing nullptr and 0 items through to Polyfence loader is + // absolutely OK: + if (hdr.num_items != 0) { + new_items = new AC_PolyFenceItem[hdr.num_items]; + if (new_items == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Out of memory for upload"); + goto OUT; + } + } + + // convert from MISSION_ITEM_INT to AC_PolyFenceItem: + for (uint32_t i=0; ipolyfence().write_fence(new_items, hdr.num_items); + +OUT: + + delete[] new_items; + + return success; +} +#endif // AP_FENCE_ENABLED + +#if HAL_RALLY_ENABLED +bool AP_Filesystem_Mission::finish_upload_rally(const struct header &hdr, const rfile &r, const uint8_t *b) +{ + bool success = false; + + auto *rally = AP::rally(); + if (rally == nullptr) { + goto OUT; + } + + if ((hdr.options & unsigned(Options::NO_CLEAR)) != 0) { + //only complete sets of rally points can be added ATM + goto OUT; + } + + rally->truncate(0); + + for (uint32_t i=0; iappend(cmd)) { + goto OUT; + } + } + success = true; + +OUT: + return success; +} +#endif // HAL_RALLY_ENABLED #endif // AP_FILESYSTEM_MISSION_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.h b/libraries/AP_Filesystem/AP_Filesystem_Mission.h index 5a6e123527651a..88a97781e9c032 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Mission.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.h @@ -72,6 +72,9 @@ class AP_Filesystem_Mission : public AP_Filesystem_Backend // finish loading items bool finish_upload(const rfile &r); + bool finish_upload_mission(const struct header &hdr, const rfile &r, const uint8_t *b); + bool finish_upload_fence(const struct header &hdr, const rfile &r, const uint8_t *b); + bool finish_upload_rally(const struct header &hdr, const rfile &r, const uint8_t *b); // see if a block of memory is all zero bool all_zero(const uint8_t *b, uint8_t size) const; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp index 31d5265f358f3f..9eadd7f607b407 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp @@ -52,6 +52,9 @@ static const SysFileList sysfs_file_list[] = { #endif {"crash_dump.bin"}, {"storage.bin"}, +#if AP_FILESYSTEM_SYS_FLASH_ENABLED + {"flash.bin"}, +#endif }; int8_t AP_Filesystem_Sys::file_in_sysfs(const char *fname) { @@ -152,6 +155,13 @@ int AP_Filesystem_Sys::open(const char *fname, int flags, bool allow_absolute_pa r.str->set_buffer((char*)ptr, size, size); } } +#if AP_FILESYSTEM_SYS_FLASH_ENABLED + if (strcmp(fname, "flash.bin") == 0) { + void *ptr = (void*)0x08000000; + const size_t size = BOARD_FLASH_SIZE*1024; + r.str->set_buffer((char*)ptr, size, size); + } +#endif if (r.str->get_length() == 0) { errno = r.str->has_failed_allocation()?ENOMEM:ENOENT; diff --git a/libraries/AP_Filesystem/AP_Filesystem_config.h b/libraries/AP_Filesystem/AP_Filesystem_config.h index 75f3aa001c224b..63cf31f2084f69 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_config.h +++ b/libraries/AP_Filesystem/AP_Filesystem_config.h @@ -50,3 +50,6 @@ #define AP_FILESYSTEM_FILE_READING_ENABLED (AP_FILESYSTEM_FILE_WRITING_ENABLED || AP_FILESYSTEM_ROMFS_ENABLED) #endif +#ifndef AP_FILESYSTEM_SYS_FLASH_ENABLED +#define AP_FILESYSTEM_SYS_FLASH_ENABLED CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#endif diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 039e0685df98a4..5a6169053472da 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -58,6 +58,12 @@ int AP_Filesystem_Posix::open(const char *fname, int flags, bool allow_absolute_ if (! allow_absolute_paths) { fname = map_filename(fname); } + struct stat st; + if (::stat(fname, &st) == 0 && + ((st.st_mode & S_IFMT) != S_IFREG && (st.st_mode & S_IFMT) != S_IFLNK)) { + // only allow links and files + return -1; + } // we automatically add O_CLOEXEC as we always want it for ArduPilot FS usage return ::open(fname, flags | O_CLOEXEC, 0644); } diff --git a/libraries/AP_Filesystem/posix_compat.cpp b/libraries/AP_Filesystem/posix_compat.cpp index 3ec501ef393bc6..6dee4f1653431f 100644 --- a/libraries/AP_Filesystem/posix_compat.cpp +++ b/libraries/AP_Filesystem/posix_compat.cpp @@ -30,14 +30,6 @@ #include #include -struct apfs_file { - int fd; - bool error; - bool eof; - int16_t unget; - char *tmpfile_name; -}; - #define CHECK_STREAM(stream, ret) while (stream == NULL || stream->fd < 0) { errno = EBADF; return ret; } #define modecmp(str, pat) (strcmp(str, pat) == 0 ? 1: 0) @@ -170,7 +162,8 @@ int apfs_fseek(APFS_FILE *stream, long offset, int whence) { CHECK_STREAM(stream, EOF); stream->eof = false; - return AP::FS().lseek(stream->fd, offset, whence); + AP::FS().lseek(stream->fd, offset, whence); + return 0; } int apfs_ferror(APFS_FILE *stream) diff --git a/libraries/AP_Filesystem/posix_compat.h b/libraries/AP_Filesystem/posix_compat.h index 5fa0fba3caa8f2..4ccf56b5c5f43e 100644 --- a/libraries/AP_Filesystem/posix_compat.h +++ b/libraries/AP_Filesystem/posix_compat.h @@ -20,6 +20,8 @@ #include #include #include +#include +#include #ifdef __cplusplus extern "C" { @@ -29,7 +31,13 @@ extern "C" { these are here to allow lua to build on HAL_ChibiOS */ -typedef struct apfs_file APFS_FILE; +typedef struct apfs_file { + int fd; + bool error; + bool eof; + int16_t unget; + char *tmpfile_name; +} APFS_FILE; APFS_FILE *apfs_fopen(const char *pathname, const char *mode); int apfs_fprintf(APFS_FILE *stream, const char *format, ...); diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index bb7dedc03aae7b..a06b922feaac2c 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -395,8 +395,9 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) break; } } - + if (updated) { +#if HAL_LOGGING_ENABLED // get estimated location and velocity Location loc_estimate{}; Vector3f vel_estimate; @@ -431,6 +432,7 @@ void AP_Follow::handle_msg(const mavlink_message_t &msg) loc_estimate.lng, loc_estimate.alt ); +#endif } } @@ -539,4 +541,4 @@ AP_Follow &follow() } -#endif +#endif // AP_FOLLOW_ENABLED diff --git a/libraries/AP_Follow/AP_Follow_config.h b/libraries/AP_Follow/AP_Follow_config.h index f5aa2dae54d465..2621dd698fc99e 100644 --- a/libraries/AP_Follow/AP_Follow_config.h +++ b/libraries/AP_Follow/AP_Follow_config.h @@ -1,5 +1,7 @@ #pragma once +#include + #ifndef AP_FOLLOW_ENABLED #define AP_FOLLOW_ENABLED 1 #endif diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 371aeffb85bbee..c4dd68e481dcc5 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -369,6 +369,7 @@ bool AP_Frsky_SPortParser::get_packet(AP_Frsky_SPort::sport_packet_t &sport_pack 0x1B // Physical ID 27 - ArduPilot/Betaflight DEFAULT DOWNLINK * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) */ +#undef BIT #define BIT(x, index) (((x) >> index) & 0x01) uint8_t AP_Frsky_SPort::calc_sensor_id(const uint8_t physical_id) { diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index b947175b0bf620..25a790787a8d42 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -347,7 +347,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _DRV_OPTIONS // @DisplayName: driver options // @Description: Additional backend specific options - // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL + // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL, 5:Override GPS satellite health of L5 band from L1 health // @User: Advanced AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), @@ -2254,6 +2254,7 @@ bool AP_GPS::get_undulation(uint8_t instance, float &undulation) const return true; } +#if HAL_LOGGING_ENABLED // Logging support: // Write an GPS packet void AP_GPS::Write_GPS(uint8_t i) @@ -2310,6 +2311,7 @@ void AP_GPS::Write_GPS(uint8_t i) }; AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); } +#endif /* get GPS based yaw diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index eedc1a0c5a43a4..a5ca9cdceaddd1 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -624,6 +624,7 @@ class AP_GPS UBX_Use115200 = (1U << 2U), UAVCAN_MBUseDedicatedBus = (1 << 3U), HeightEllipsoid = (1U << 4), + GPSL5HealthOverride = (1U << 5) }; // check if an option is set diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 2471d3958019f4..5da56758a517c3 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -526,10 +526,12 @@ void AP_GPS_DroneCAN::handle_status_msg(const ardupilot_gnss_Status& msg) healthy = msg.healthy; status_flags = msg.status; if (error_code != msg.error_codes) { +#if HAL_LOGGING_ENABLED AP::logger().Write_MessageF("GPS %d: error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1), error_code, msg.error_codes); +#endif error_code = msg.error_codes; } } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 323a0f0f1cbbb6..f073201373b554 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -50,6 +50,9 @@ // use this to enable debugging of moving baseline configs #define UBLOX_MB_DEBUGGING 0 +// debug VALGET/VALSET configuration +#define UBLOX_CFG_DEBUGGING 0 + extern const AP_HAL::HAL& hal; #if UBLOX_DEBUGGING @@ -78,6 +81,19 @@ extern const AP_HAL::HAL& hal; # define MB_Debug(fmt, args ...) #endif +#if UBLOX_CFG_DEBUGGING +#if defined(HAL_BUILD_AP_PERIPH) + extern "C" { + void can_printf(const char *fmt, ...); + } + # define CFG_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0) +#else + # define CFG_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) +#endif +#else + # define CFG_Debug(fmt, args ...) +#endif + AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port, AP_GPS::GPS_Role _role) : AP_GPS_Backend(_gps, _state, _port), _next_message(STEP_PVT), @@ -241,6 +257,18 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_M10[] { }; +/* + config changes for L5 modules +*/ +const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_ena[] { + {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 1}, + {ConfigKey::CFG_SIGNAL_GPS_L5_ENA, 1}, +}; + +const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_L5_ovrd_dis[] { + {ConfigKey::CFG_SIGNAL_L5_HEALTH_OVRD, 0}, +}; + void AP_GPS_UBLOX::_request_next_config(void) { @@ -436,7 +464,24 @@ AP_GPS_UBLOX::_request_next_config(void) } break; } - + + case STEP_L5: { + if (supports_l5 && option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) { + const config_list *list = config_L5_ovrd_ena; + const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_ena); + if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) { + _next_message--; + } + } else if (supports_l5 && !option_set(AP_GPS::DriverOptions::GPSL5HealthOverride)) { + const config_list *list = config_L5_ovrd_dis; + const uint8_t list_length = ARRAY_SIZE(config_L5_ovrd_dis); + if (!_configure_config_set(list, list_length, CONFIG_L5, UBX_VALSET_LAYER_RAM | UBX_VALSET_LAYER_BBR)) { + _next_message--; + } + } + break; + } + default: // this case should never be reached, do a full reset if it is hit _next_message = STEP_PVT; @@ -1002,13 +1047,13 @@ AP_GPS_UBLOX::_parse_gps(void) likely this device does not support fetching multiple keys at once, go one at a time */ if (active_config.fetch_index == -1) { - Debug("NACK starting %u", unsigned(active_config.count)); + CFG_Debug("NACK starting %u", unsigned(active_config.count)); active_config.fetch_index = 0; } else { // the device does not support the config key we asked for, // consider the bit as done active_config.done_mask |= (1U< #include #include +#include #define GPS_BACKEND_DEBUGGING 0 diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h index ffdbd73db5ef60..5ed1d2794693d3 100644 --- a/libraries/AP_GPS/LogStructure.h +++ b/libraries/AP_GPS/LogStructure.h @@ -19,6 +19,7 @@ // @Field: TimeUS: Time since system startup // @Field: I: GPS instance number // @Field: Status: GPS Fix type; 2D fix, 3D fix etc. +// @FieldValueEnum: Status: AP_GPS::GPS_Status // @Field: GMS: milliseconds since start of GPS Week // @Field: GWk: weeks since 5 Jan 1980 // @Field: NSats: number of satellites visible @@ -204,15 +205,15 @@ struct PACKED log_GPS_RAWS { #define LOG_STRUCTURE_FROM_GPS \ { LOG_GPS_MSG, sizeof(log_GPS), \ - "GPS", "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#---SmDUmnhnh-", "F----0BGGB000--" , true }, \ + "GPS", "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#-s-S-DUmnhnh-", "F--C-0BGGB000--" , true }, \ { LOG_GPA_MSG, sizeof(log_GPA), \ - "GPA", "QBCCCCfBIHfHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTCMFU,RTCMFD", "s#mmmnd-ssm--", "F-BBBB0-CC0--" , true }, \ + "GPA", "QBCCCCfBIHfHH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und,RTCMFU,RTCMFD", "s#-mmnd-ssm--", "F-BBBB0-CC0--" , true }, \ { LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \ "UBX1", "QBHBBHI", "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------" , true }, \ { LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \ "UBX2", "QBbBbB", "TimeUS,Instance,ofsI,magI,ofsQ,magQ", "s#----", "F-----" , true }, \ { LOG_GPS_RAW_MSG, sizeof(log_GPS_RAW), \ - "GRAW", "QIHBBddfBbB", "TimeUS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli", "s--S-------", "F--0-------" , true }, \ + "GRAW", "QIHBBddfBbB", "TimeUS,WkMS,Week,numSV,sv,cpMes,prMes,doMes,mesQI,cno,lli", "ss-S-------", "FC-0-------" , true }, \ { LOG_GPS_RAWH_MSG, sizeof(log_GPS_RAWH), \ "GRXH", "QdHbBB", "TimeUS,rcvTime,week,leapS,numMeas,recStat", "s-----", "F-----" , true }, \ { LOG_GPS_RAWS_MSG, sizeof(log_GPS_RAWS), \ diff --git a/libraries/AP_GPS/LogStructure_SBP.h b/libraries/AP_GPS/LogStructure_SBP.h index c3a427c3ce70f7..9c0b259710e166 100644 --- a/libraries/AP_GPS/LogStructure_SBP.h +++ b/libraries/AP_GPS/LogStructure_SBP.h @@ -74,7 +74,7 @@ struct PACKED log_SbpEvent { { LOG_MSG_SBPHEALTH, sizeof(log_SbpHealth), \ "SBPH", "QIII", "TimeUS,CrcError,LastInject,IARhyp", "s---", "F---" , true }, \ { LOG_MSG_SBPRAWH, sizeof(log_SbpRAWH), \ - "SBRH", "QQQQQQQQ", "TimeUS,msg_flag,1,2,3,4,5,6", "s--b----", "F--0----" , true }, \ + "SBRH", "QQQQQQQQ", "TimeUS,msg_flag,1,2,3,4,5,6", "s-------", "F-------" , true }, \ { LOG_MSG_SBPRAWM, sizeof(log_SbpRAWM), \ "SBRM", "QQQQQQQQQQQQQQQ", "TimeUS,msg_flag,1,2,3,4,5,6,7,8,9,10,11,12,13", "s??????????????", "F??????????????" , true }, \ { LOG_MSG_SBPEVENT, sizeof(log_SbpEvent), \ diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.cpp b/libraries/AP_Generator/AP_Generator_IE_2400.cpp index 84d6a497bfe35a..85ef02dee5bf20 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_2400.cpp @@ -90,7 +90,7 @@ void AP_Generator_IE_2400::assign_measurements(const uint32_t now) // If received 20 valid packets for a single protocol version then lock on if (_last_version_packet_count > 20) { _version = new_version; - gcs().send_text(MAV_SEVERITY_INFO, "Generator: IE using %s protocol", (_version == ProtocolVersion::V2) ? "V2" : "legacy" ); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: IE using %s protocol", (_version == ProtocolVersion::V2) ? "V2" : "legacy" ); } else { // Don't record any data during version detection diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.h b/libraries/AP_Generator/AP_Generator_IE_2400.h index c98238f53f7c53..a52c53cc856166 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.h +++ b/libraries/AP_Generator/AP_Generator_IE_2400.h @@ -4,6 +4,8 @@ #if AP_GENERATOR_IE_2400_ENABLED +#include + class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell { // Inherit constructor diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.cpp b/libraries/AP_Generator/AP_Generator_RichenPower.cpp index 530f8192b371d5..cbfc2fcdeb7799 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.cpp +++ b/libraries/AP_Generator/AP_Generator_RichenPower.cpp @@ -30,6 +30,8 @@ extern const AP_HAL::HAL& hal; // init method; configure communications with the generator void AP_Generator_RichenPower::init() { + ASSERT_STORAGE_SIZE(RichenPacket, 70); + const AP_SerialManager &serial_manager = AP::serialmanager(); uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Generator, 0); diff --git a/libraries/AP_Generator/AP_Generator_RichenPower.h b/libraries/AP_Generator/AP_Generator_RichenPower.h index c5999c0ed61bc4..bd9bfc25d926a8 100644 --- a/libraries/AP_Generator/AP_Generator_RichenPower.h +++ b/libraries/AP_Generator/AP_Generator_RichenPower.h @@ -153,7 +153,6 @@ class AP_Generator_RichenPower : public AP_Generator_Backend uint8_t footermagic1; uint8_t footermagic2; }; - assert_storage_size _assert_storage_size_RichenPacket UNUSED_PRIVATE_MEMBER; union RichenUnion { uint8_t parse_buffer[70]; diff --git a/libraries/AP_Gripper/AP_Gripper_EPM.cpp b/libraries/AP_Gripper/AP_Gripper_EPM.cpp index 5762597f50f010..76c2f67381f900 100644 --- a/libraries/AP_Gripper/AP_Gripper_EPM.cpp +++ b/libraries/AP_Gripper/AP_Gripper_EPM.cpp @@ -66,7 +66,7 @@ void AP_Gripper_EPM::grab() SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm); } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load grabbing"); - AP::logger().Write_Event(LogEvent::GRIPPER_GRAB); + LOGGER_WRITE_EVENT(LogEvent::GRIPPER_GRAB); } // release - move epm pwm output to the release position @@ -90,7 +90,7 @@ void AP_Gripper_EPM::release() SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm); } GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load releasing"); - AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE); + LOGGER_WRITE_EVENT(LogEvent::GRIPPER_RELEASE); } // neutral - return the epm pwm output to the neutral position diff --git a/libraries/AP_Gripper/AP_Gripper_Servo.cpp b/libraries/AP_Gripper/AP_Gripper_Servo.cpp index 0ef9957da0fec9..a2d13897727ba0 100644 --- a/libraries/AP_Gripper/AP_Gripper_Servo.cpp +++ b/libraries/AP_Gripper/AP_Gripper_Servo.cpp @@ -40,7 +40,7 @@ void AP_Gripper_Servo::grab() SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.grab_pwm); _last_grab_or_release = AP_HAL::millis(); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load grabbing"); - AP::logger().Write_Event(LogEvent::GRIPPER_GRAB); + LOGGER_WRITE_EVENT(LogEvent::GRIPPER_GRAB); } void AP_Gripper_Servo::release() @@ -65,7 +65,7 @@ void AP_Gripper_Servo::release() SRV_Channels::set_output_pwm(SRV_Channel::k_gripper, config.release_pwm); _last_grab_or_release = AP_HAL::millis(); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Gripper load releasing"); - AP::logger().Write_Event(LogEvent::GRIPPER_RELEASE); + LOGGER_WRITE_EVENT(LogEvent::GRIPPER_RELEASE); } bool AP_Gripper_Servo::has_state_pwm(const uint16_t pwm) const diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 886baa3749c370..5a1c0e79666842 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -473,7 +473,7 @@ uint16_t AP_GyroFFT::run_cycle() _thread_state._last_output_us[_update_axis] = AP_HAL::micros(); _output_cycle_micros = _thread_state._last_output_us[_update_axis] - now; -#if AP_SIM_ENABLED +#if AP_SIM_ENABLED && HAL_LOGGING_ENABLED // extra logging when running simulations AP::logger().WriteStreaming( "FTN3", @@ -966,6 +966,8 @@ float AP_GyroFFT::calculate_weighted_freq_hz(const Vector3f& energy, const Vecto // @Field: FHZ: FFT health, Z-axis // @Field: Tc: FFT cycle time +#if HAL_LOGGING_ENABLED + // log gyro fft messages void AP_GyroFFT::write_log_messages() { @@ -1048,6 +1050,8 @@ void AP_GyroFFT::log_noise_peak(uint8_t id, FrequencyPeak peak) const get_center_freq_energy(peak).z); } +#endif + // return an average noise bandwidth weighted by bin energy // called from main thread float AP_GyroFFT::get_weighted_noise_center_bandwidth_hz() const diff --git a/libraries/AP_HAL/AP_HAL.h b/libraries/AP_HAL/AP_HAL.h index 64198b63d00e3e..49aab05996f6b9 100644 --- a/libraries/AP_HAL/AP_HAL.h +++ b/libraries/AP_HAL/AP_HAL.h @@ -16,7 +16,6 @@ #include "RCOutput.h" #include "Scheduler.h" #include "Semaphores.h" -#include "EventHandle.h" #include "Util.h" #include "OpticalFlow.h" #include "Flash.h" diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index a6ef45740f161b..254895827266d3 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -68,6 +68,7 @@ #define HAL_BOARD_SUBTYPE_ESP32_TOMTE76 6005 #define HAL_BOARD_SUBTYPE_ESP32_NICK 6006 #define HAL_BOARD_SUBTYPE_ESP32_S3DEVKIT 6007 +#define HAL_BOARD_SUBTYPE_ESP32_S3EMPTY 6008 /* InertialSensor driver types */ #define HAL_INS_NONE 0 @@ -170,8 +171,12 @@ #define HAL_WITH_IO_MCU 0 #endif +#ifndef HAL_WITH_IO_MCU_BIDIR_DSHOT +#define HAL_WITH_IO_MCU_BIDIR_DSHOT 0 +#endif + #ifndef HAL_WITH_IO_MCU_DSHOT -#define HAL_WITH_IO_MCU_DSHOT 0 +#define HAL_WITH_IO_MCU_DSHOT HAL_WITH_IO_MCU_BIDIR_DSHOT #endif // this is used as a general mechanism to make a 'small' build by diff --git a/libraries/AP_HAL/AP_HAL_Macros.h b/libraries/AP_HAL/AP_HAL_Macros.h index 25e124af6665b4..20039cdb2f6a58 100644 --- a/libraries/AP_HAL/AP_HAL_Macros.h +++ b/libraries/AP_HAL/AP_HAL_Macros.h @@ -6,7 +6,7 @@ macros to allow code to build on multiple platforms more easily */ -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || HAL_WITH_EKF_DOUBLE || (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS && AP_SIM_ENABLED) +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX || HAL_WITH_EKF_DOUBLE || AP_SIM_ENABLED /* allow double maths on Linux and SITL to avoid problems with system headers */ diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h index aca8e02293763a..9782b9fb2c4e6c 100644 --- a/libraries/AP_HAL/AP_HAL_Namespace.h +++ b/libraries/AP_HAL/AP_HAL_Namespace.h @@ -27,9 +27,8 @@ namespace AP_HAL { class RCInput; class RCOutput; class Scheduler; - class EventHandle; - class EventSource; class Semaphore; + class BinarySemaphore; class OpticalFlow; class DSP; diff --git a/libraries/AP_HAL/AnalogIn.h b/libraries/AP_HAL/AnalogIn.h index 2a8f72207a5fa5..c07799acc4fabc 100644 --- a/libraries/AP_HAL/AnalogIn.h +++ b/libraries/AP_HAL/AnalogIn.h @@ -42,6 +42,16 @@ class AP_HAL::AnalogIn { // failures can still be diagnosed virtual uint16_t accumulated_power_status_flags(void) const { return 0; } + // this enum class is 1:1 with MAVLink's MAV_POWER_STATUS enumeration! + enum class PowerStatusFlag : uint16_t { + BRICK_VALID = 1, // main brick power supply valid + SERVO_VALID = 2, // main servo power supply valid for FMU + USB_CONNECTED = 4, // USB power is connected + PERIPH_OVERCURRENT = 8, // peripheral supply is in over-current state + PERIPH_HIPOWER_OVERCURRENT = 16, // hi-power peripheral supply is in over-current state + CHANGED = 32, // Power status has changed since boot + }; + #if HAL_WITH_MCU_MONITORING virtual float mcu_temperature(void) { return 0; } virtual float mcu_voltage(void) { return 0; } diff --git a/libraries/AP_HAL/CANIface.h b/libraries/AP_HAL/CANIface.h index 06b4f7dffbf848..1b2e88fb33f236 100644 --- a/libraries/AP_HAL/CANIface.h +++ b/libraries/AP_HAL/CANIface.h @@ -187,7 +187,7 @@ class AP_HAL::CANIface return false; } - virtual bool set_event_handle(EventHandle* evt_handle) + virtual bool set_event_handle(AP_HAL::BinarySemaphore *sem_handle) { return true; } diff --git a/libraries/AP_HAL/EventHandle.cpp b/libraries/AP_HAL/EventHandle.cpp deleted file mode 100644 index d0dab77d9245e1..00000000000000 --- a/libraries/AP_HAL/EventHandle.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "EventHandle.h" -#include - - -bool AP_HAL::EventHandle::register_event(uint32_t evt_mask) -{ - WITH_SEMAPHORE(sem); - evt_mask_ |= evt_mask; - return true; -} - -bool AP_HAL::EventHandle::unregister_event(uint32_t evt_mask) -{ - WITH_SEMAPHORE(sem); - evt_mask_ &= ~evt_mask; - return true; -} - -bool AP_HAL::EventHandle::wait(uint16_t duration_us) -{ - if (evt_src_ == nullptr) { - return false; - } - return evt_src_->wait(duration_us, this); -} - -bool AP_HAL::EventHandle::set_source(AP_HAL::EventSource* src) -{ - WITH_SEMAPHORE(sem); - evt_src_ = src; - evt_mask_ = 0; - return true; -} diff --git a/libraries/AP_HAL/EventHandle.h b/libraries/AP_HAL/EventHandle.h deleted file mode 100644 index c5c676c389a907..00000000000000 --- a/libraries/AP_HAL/EventHandle.h +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include "AP_HAL_Namespace.h" -#include -#include "AP_HAL_Boards.h" - -class AP_HAL::EventSource { -public: - // generate event from thread context - virtual void signal(uint32_t evt_mask) = 0; - - // generate event from interrupt context - virtual void signalI(uint32_t evt_mask) { signal(evt_mask); } - - - // Wait on an Event handle, method for internal use by EventHandle - virtual bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) = 0; -}; - -class AP_HAL::EventHandle { -public: - //Set event source - virtual bool set_source(AP_HAL::EventSource* src); - - AP_HAL::EventSource* get_source() { return evt_src_; } - - // return true if event type was successfully registered - virtual bool register_event(uint32_t evt_mask); - - // return true if event type was successfully unregistered - virtual bool unregister_event(uint32_t evt_mask); - - // return true if event was triggered within the duration - virtual bool wait(uint16_t duration_us); - - virtual uint32_t get_evt_mask() const { return evt_mask_; } - -private: - // Mask of events to be handeled, - // Max 32 events can be handled per event handle - uint32_t evt_mask_; - AP_HAL::EventSource *evt_src_; - HAL_Semaphore sem; -}; diff --git a/libraries/AP_HAL/HAL.cpp b/libraries/AP_HAL/HAL.cpp index a2d1216d2b389e..6f796c3ab9c6a2 100644 --- a/libraries/AP_HAL/HAL.cpp +++ b/libraries/AP_HAL/HAL.cpp @@ -11,16 +11,3 @@ HAL::FunCallbacks::FunCallbacks(void (*setup_fun)(void), void (*loop_fun)(void)) } } - -// access serial ports using SERIALn numbering -AP_HAL::UARTDriver* AP_HAL::HAL::serial(uint8_t sernum) const -{ - UARTDriver **uart_array = const_cast(&uartA); - // this mapping captures the historical use of uartB as SERIAL3 - const uint8_t mapping[] = { 0, 2, 3, 1, 4, 5, 6, 7, 8, 9 }; - static_assert(sizeof(mapping) == num_serial, "num_serial must match mapping"); - if (sernum >= num_serial) { - return nullptr; - } - return uart_array[mapping[sernum]]; -} diff --git a/libraries/AP_HAL/HAL.h b/libraries/AP_HAL/HAL.h index f8827444204f46..cb71ca5ec833f3 100644 --- a/libraries/AP_HAL/HAL.h +++ b/libraries/AP_HAL/HAL.h @@ -20,16 +20,16 @@ class AP_Param; class AP_HAL::HAL { public: - HAL(AP_HAL::UARTDriver* _uartA, // console - AP_HAL::UARTDriver* _uartB, // 1st GPS - AP_HAL::UARTDriver* _uartC, // telem1 - AP_HAL::UARTDriver* _uartD, // telem2 - AP_HAL::UARTDriver* _uartE, // 2nd GPS - AP_HAL::UARTDriver* _uartF, // extra1 - AP_HAL::UARTDriver* _uartG, // extra2 - AP_HAL::UARTDriver* _uartH, // extra3 - AP_HAL::UARTDriver* _uartI, // extra4 - AP_HAL::UARTDriver* _uartJ, // extra5 + HAL(AP_HAL::UARTDriver* _serial0, // console + AP_HAL::UARTDriver* _serial1, // telem1 + AP_HAL::UARTDriver* _serial2, // telem2 + AP_HAL::UARTDriver* _serial3, // 1st GPS + AP_HAL::UARTDriver* _serial4, // 2nd GPS + AP_HAL::UARTDriver* _serial5, // extra1 + AP_HAL::UARTDriver* _serial6, // extra2 + AP_HAL::UARTDriver* _serial7, // extra3 + AP_HAL::UARTDriver* _serial8, // extra4 + AP_HAL::UARTDriver* _serial9, // extra5 AP_HAL::I2CDeviceManager* _i2c_mgr, AP_HAL::SPIDeviceManager* _spi, AP_HAL::WSPIDeviceManager* _wspi, @@ -46,23 +46,26 @@ class AP_HAL::HAL { #if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL class AP_HAL::SIMState* _simstate, #endif +#if HAL_WITH_DSP AP_HAL::DSP* _dsp, +#endif #if HAL_NUM_CAN_IFACES > 0 AP_HAL::CANIface* _can_ifaces[HAL_NUM_CAN_IFACES]) #else AP_HAL::CANIface** _can_ifaces) #endif : - uartA(_uartA), - uartB(_uartB), - uartC(_uartC), - uartD(_uartD), - uartE(_uartE), - uartF(_uartF), - uartG(_uartG), - uartH(_uartH), - uartI(_uartI), - uartJ(_uartJ), + serial_array{ + _serial0, + _serial1, + _serial2, + _serial3, + _serial4, + _serial5, + _serial6, + _serial7, + _serial8, + _serial9}, i2c_mgr(_i2c_mgr), spi(_spi), wspi(_wspi), @@ -75,11 +78,13 @@ class AP_HAL::HAL { scheduler(_scheduler), util(_util), opticalflow(_opticalflow), - flash(_flash), +#if HAL_WITH_DSP + dsp(_dsp), +#endif #if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL simstate(_simstate), #endif - dsp(_dsp) + flash(_flash) { #if HAL_NUM_CAN_IFACES > 0 if (_can_ifaces == nullptr) { @@ -112,19 +117,6 @@ class AP_HAL::HAL { virtual void run(int argc, char * const argv[], Callbacks* callbacks) const = 0; -private: - // the uartX ports must be contiguous in ram for the serial() method to work - AP_HAL::UARTDriver* uartA; - AP_HAL::UARTDriver* uartB UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartC UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartD UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartE UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartF UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartG UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartH UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartI UNUSED_PRIVATE_MEMBER; - AP_HAL::UARTDriver* uartJ UNUSED_PRIVATE_MEMBER; - public: AP_HAL::I2CDeviceManager* i2c_mgr; AP_HAL::SPIDeviceManager* spi; @@ -151,6 +143,11 @@ class AP_HAL::HAL { static constexpr uint8_t num_serial = 10; +private: + // UART drivers in SERIALn_ order + AP_HAL::UARTDriver* serial_array[num_serial]; + +public: #if AP_SIM_ENABLED && CONFIG_HAL_BOARD != HAL_BOARD_SITL AP_HAL::SIMState *simstate; #endif @@ -162,3 +159,12 @@ class AP_HAL::HAL { #endif }; + +// access serial ports using SERIALn numbering +inline AP_HAL::UARTDriver* AP_HAL::HAL::serial(uint8_t sernum) const +{ + if (sernum >= ARRAY_SIZE(serial_array)) { + return nullptr; + } + return serial_array[sernum]; +} diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index bc360932268ecd..c4de0473a96f66 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -154,6 +154,11 @@ class AP_HAL::RCOutput { */ virtual uint16_t get_erpm(uint8_t chan) const { return 0; } virtual float get_erpm_error_rate(uint8_t chan) const { return 100.0f; } + /* + allow all erpm values to be read and for new updates to be detected - primarily for IOMCU + */ + virtual bool new_erpm() { return false; } + virtual uint32_t read_erpm(uint16_t* erpm, uint8_t len) { return 0; } /* enable PX4IO SBUS out at the given rate diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp index 9a270445abf71c..70fcb9abfaca1b 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/libraries/AP_HAL/SIMState.cpp @@ -35,10 +35,16 @@ using namespace AP_HAL; #define AP_SIM_FRAME_CLASS MultiCopter #elif APM_BUILD_TYPE(APM_BUILD_Heli) #define AP_SIM_FRAME_CLASS Helicopter +#elif APM_BUILD_TYPE(APM_BUILD_AntennaTracker) +#define AP_SIM_FRAME_CLASS Tracker #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) #define AP_SIM_FRAME_CLASS Plane #elif APM_BUILD_TYPE(APM_BUILD_Rover) #define AP_SIM_FRAME_CLASS SimRover +#elif APM_BUILD_TYPE(APM_BUILD_Blimp) +#define AP_SIM_FRAME_CLASS Blimp +#elif APM_BUILD_TYPE(APM_BUILD_ArduSub) +#define AP_SIM_FRAME_CLASS Submarine #endif #endif @@ -47,10 +53,16 @@ using namespace AP_HAL; #define AP_SIM_FRAME_STRING "+" #elif APM_BUILD_TYPE(APM_BUILD_Heli) #define AP_SIM_FRAME_STRING "heli" +#elif APM_BUILD_TYPE(APM_BUILD_AntennaTracker) +#define AP_SIM_FRAME_STRING "tracker" #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) #define AP_SIM_FRAME_STRING "plane" #elif APM_BUILD_TYPE(APM_BUILD_Rover) #define AP_SIM_FRAME_STRING "rover" +#elif APM_BUILD_TYPE(APM_BUILD_Blimp) +#define AP_SIM_FRAME_STRING "blimp" +#elif APM_BUILD_TYPE(APM_BUILD_ArduSub) +#define AP_SIM_FRAME_STRING "sub" #endif #endif @@ -230,6 +242,9 @@ void SIMState::fdm_input_local(void) if (microstrain5 != nullptr) { microstrain5->update(); } + if (inertiallabs != nullptr) { + inertiallabs->update(); + } #if HAL_SIM_AIS_ENABLED if (ais != nullptr) { @@ -381,7 +396,7 @@ void SIMState::set_height_agl(void) AP_Terrain *_terrain = AP_Terrain::get_singleton(); if (_terrain != nullptr && _terrain->height_amsl(location, terrain_height_amsl)) { - _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl; + _sitl->state.height_agl = _sitl->state.altitude - terrain_height_amsl; return; } } @@ -389,7 +404,7 @@ void SIMState::set_height_agl(void) if (_sitl != nullptr) { // fall back to flat earth model - _sitl->height_agl = _sitl->state.altitude - home_alt; + _sitl->state.height_agl = _sitl->state.altitude - home_alt; } } diff --git a/libraries/AP_HAL/SIMState.h b/libraries/AP_HAL/SIMState.h index 41505a6263d905..0e1d2c808d0d0d 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -42,8 +43,9 @@ #include #include +#include #include -#include +#include #include @@ -92,7 +94,7 @@ class AP_HAL::SIMState { uint32_t _update_count; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - SocketAPM _sitl_rc_in{true}; + SocketAPM_native _sitl_rc_in{true}; #endif SITL::SIM *_sitl; uint16_t _rcin_port; @@ -198,6 +200,9 @@ class AP_HAL::SIMState { // simulated MicroStrain Series 7 system SITL::MicroStrain7 *microstrain7; + // simulated InertialLabs INS-U + SITL::InertialLabs *inertiallabs; + #if HAL_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend SITL::JSON_Master ride_along; @@ -216,7 +221,7 @@ class AP_HAL::SIMState { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL // output socket for flightgear viewing - SocketAPM fg_socket{true}; + SocketAPM_native fg_socket{true}; #endif const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; diff --git a/libraries/AP_HAL/Scheduler.h b/libraries/AP_HAL/Scheduler.h index eb580ebe0dd714..0f047617f0e408 100644 --- a/libraries/AP_HAL/Scheduler.h +++ b/libraries/AP_HAL/Scheduler.h @@ -115,6 +115,7 @@ class AP_HAL::Scheduler { PRIORITY_UART, PRIORITY_STORAGE, PRIORITY_SCRIPTING, + PRIORITY_NET, }; /* diff --git a/libraries/AP_HAL/Semaphores.cpp b/libraries/AP_HAL/Semaphores.cpp index 65890007e236b7..1ebb9cbe222d18 100644 --- a/libraries/AP_HAL/Semaphores.cpp +++ b/libraries/AP_HAL/Semaphores.cpp @@ -12,14 +12,18 @@ WithSemaphore::WithSemaphore(AP_HAL::Semaphore *mtx, uint32_t line) : WithSemaphore::WithSemaphore(AP_HAL::Semaphore &mtx, uint32_t line) : _mtx(mtx) { +#ifndef HAL_BOOTLOADER_BUILD bool in_main = hal.scheduler->in_main_thread(); if (in_main) { hal.util->persistent_data.semaphore_line = line; } +#endif _mtx.take_blocking(); +#ifndef HAL_BOOTLOADER_BUILD if (in_main) { hal.util->persistent_data.semaphore_line = 0; } +#endif } WithSemaphore::~WithSemaphore() diff --git a/libraries/AP_HAL/Semaphores.h b/libraries/AP_HAL/Semaphores.h index 3c5bca94b13bb3..34e8ff93fb86a5 100644 --- a/libraries/AP_HAL/Semaphores.h +++ b/libraries/AP_HAL/Semaphores.h @@ -55,3 +55,28 @@ class WithSemaphore { #define JOIN( sem, line, counter ) _DO_JOIN( sem, line, counter ) #define _DO_JOIN( sem, line, counter ) WithSemaphore _getsem ## counter(sem, line) + +/* + a binary semaphore + */ +class AP_HAL::BinarySemaphore { +public: + /* + create a binary semaphore. initial_state determines if a wait() + immediately after creation would block. If initial_state is true + then it won't block, if initial_state is false it will block + */ + BinarySemaphore(bool initial_state=false) {} + + // do not allow copying + CLASS_NO_COPY(BinarySemaphore); + + virtual bool wait(uint32_t timeout_us) WARN_IF_UNUSED = 0 ; + virtual bool wait_blocking() = 0; + virtual bool wait_nonblocking() { return wait(0); } + + virtual void signal() = 0; + virtual void signal_ISR() { signal(); } + + virtual ~BinarySemaphore(void) {} +}; diff --git a/libraries/AP_HAL/UARTDriver.cpp b/libraries/AP_HAL/UARTDriver.cpp index 40819c9900c41d..b3d06c4f450ff2 100644 --- a/libraries/AP_HAL/UARTDriver.cpp +++ b/libraries/AP_HAL/UARTDriver.cpp @@ -152,3 +152,14 @@ bool AP_HAL::UARTDriver::discard_input() } return _discard_input(); } + +/* + default implementation of receive_time_constraint_us() will be used + for subclasses that don't implement the call (eg. network + sockets). Best we can do is to use the current timestamp as we don't + know the transport delay + */ +uint64_t AP_HAL::UARTDriver::receive_time_constraint_us(uint16_t nbytes) +{ + return AP_HAL::micros64(); +} diff --git a/libraries/AP_HAL/UARTDriver.h b/libraries/AP_HAL/UARTDriver.h index b3e890b7199d11..d876de6a93d4e6 100644 --- a/libraries/AP_HAL/UARTDriver.h +++ b/libraries/AP_HAL/UARTDriver.h @@ -137,10 +137,8 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { This takes account of the baudrate of the link. For transports that have no baudrate (such as USB) the time estimate may be less accurate. - - A return value of zero means the HAL does not support this API */ - virtual uint64_t receive_time_constraint_us(uint16_t nbytes) { return 0; } + virtual uint64_t receive_time_constraint_us(uint16_t nbytes); virtual uint32_t bw_in_bytes_per_second() const { return 5760; @@ -180,6 +178,13 @@ class AP_HAL::UARTDriver : public AP_HAL::BetterStream { } #endif + /* + return true if the port is currently locked for writing + */ + bool is_write_locked(void) const { + return lock_write_key != 0; + } + protected: // key for a locked port uint32_t lock_write_key; diff --git a/libraries/AP_HAL/Util.h b/libraries/AP_HAL/Util.h index b605fecabad95e..d5bd6bb95cf4ea 100644 --- a/libraries/AP_HAL/Util.h +++ b/libraries/AP_HAL/Util.h @@ -147,7 +147,8 @@ class AP_HAL::Util { // allocate and free DMA-capable memory if possible. Otherwise return normal memory enum Memory_Type { MEM_DMA_SAFE, - MEM_FAST + MEM_FAST, + MEM_FILESYSTEM }; virtual void *malloc_type(size_t size, Memory_Type mem_type) { return calloc(1, size); } virtual void free_type(void *ptr, size_t size, Memory_Type mem_type) { return free(ptr); } diff --git a/libraries/AP_HAL/board/chibios.h b/libraries/AP_HAL/board/chibios.h index b06642742d0fb9..9d1b940f310029 100644 --- a/libraries/AP_HAL/board/chibios.h +++ b/libraries/AP_HAL/board/chibios.h @@ -63,9 +63,7 @@ // allow for static semaphores #include #define HAL_Semaphore ChibiOS::Semaphore - -#include -#define HAL_EventHandle AP_HAL::EventHandle +#define HAL_BinarySemaphore ChibiOS::BinarySemaphore #endif /* string names for well known SPI devices */ diff --git a/libraries/AP_HAL/board/empty.h b/libraries/AP_HAL/board/empty.h index 6e33c9a81b103e..0dc26bdf23c149 100644 --- a/libraries/AP_HAL/board/empty.h +++ b/libraries/AP_HAL/board/empty.h @@ -16,3 +16,4 @@ #define HAL_HAVE_SAFETY_SWITCH 1 #define HAL_Semaphore Empty::Semaphore +#define HAL_BinarySemaphore Empty::BinarySemaphore diff --git a/libraries/AP_HAL/board/esp32.h b/libraries/AP_HAL/board/esp32.h index 02a39cfc7ad537..1fc82f841bdfa7 100644 --- a/libraries/AP_HAL/board/esp32.h +++ b/libraries/AP_HAL/board/esp32.h @@ -15,6 +15,10 @@ #include "esp32nick.h" //Nick K. on discord #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_S3DEVKIT #include "esp32s3devkit.h" //Nick K. on discord +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_S3EMPTY +#include "esp32s3empty.h" +#else +#error "Invalid CONFIG_HAL_BOARD_SUBTYPE for esp32" #endif #define HAL_BOARD_NAME "ESP32" @@ -35,6 +39,7 @@ // allow for static semaphores #include #define HAL_Semaphore ESP32::Semaphore +#define HAL_BinarySemaphore ESP32::BinarySemaphore #endif #define HAL_NUM_CAN_IFACES 0 @@ -110,4 +115,4 @@ // other big things.. #define HAL_QUADPLANE_ENABLED 0 -#define HAL_GYROFFT_ENABLED 0 \ No newline at end of file +#define HAL_GYROFFT_ENABLED 0 diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index 76cea1a112982e..65ce66d2a10e40 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -398,8 +398,7 @@ #ifdef __cplusplus #include #define HAL_Semaphore Linux::Semaphore -#include -#define HAL_EventHandle AP_HAL::EventHandle +#define HAL_BinarySemaphore Linux::BinarySemaphore #endif #ifndef HAL_HAVE_HARDWARE_DOUBLE diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index aa2799f09abc63..a4b3bba71dfbe0 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -54,9 +54,7 @@ // allow for static semaphores #include #define HAL_Semaphore HALSITL::Semaphore - -#include -#define HAL_EventHandle AP_HAL::EventHandle +#define HAL_BinarySemaphore HALSITL::BinarySemaphore #endif #ifndef HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL/examples/BinarySem/BinarySem.cpp b/libraries/AP_HAL/examples/BinarySem/BinarySem.cpp new file mode 100644 index 00000000000000..885933ded9fa9d --- /dev/null +++ b/libraries/AP_HAL/examples/BinarySem/BinarySem.cpp @@ -0,0 +1,91 @@ +/* + test of HAL_BinarySemaphore + */ + +#include +#include + +#include + +void setup(); +void loop(); + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +class BinarySemTest { +public: + HAL_BinarySemaphore sem1{1}; + HAL_BinarySemaphore sem2{0}; + + void setup(void); + void thread1(void); + void thread2(void); + void update(bool ok); + + uint32_t ops, timeouts; + uint32_t last_print_us; + HAL_Semaphore mtx; +}; + +void BinarySemTest::setup(void) +{ + hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&BinarySemTest::thread1, void), "thd1", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0); + hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&BinarySemTest::thread2, void), "thd2", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0); + ::printf("Setup threads\n"); +} + +void BinarySemTest::thread2(void) +{ + while (true) { + bool ok = sem2.wait(50000); + sem1.signal(); + update(ok); + } +} + +void BinarySemTest::thread1(void) +{ + while (true) { + bool ok = sem1.wait(50000); + sem2.signal(); + update(ok); + } +} + +void BinarySemTest::update(bool ok) +{ + WITH_SEMAPHORE(mtx); + if (ok) { + ops++; + } else { + timeouts++; + } + uint32_t now_us = AP_HAL::micros(); + float dt = (now_us - last_print_us)*1.0e-6; + if (dt >= 1.0) { + last_print_us = now_us; + ::printf("tick %u %.3f ops/s %.3f timeouts/s\n", + unsigned(AP_HAL::millis()), + ops/dt, + timeouts/dt); + ops = 0; + timeouts = 0; + } +} + +static BinarySemTest *ct; + +void setup(void) +{ + ct = new BinarySemTest; + ct->setup(); +} + +void loop(void) +{ + hal.scheduler->delay(1000); +} + +AP_HAL_MAIN(); diff --git a/libraries/AP_HAL/examples/BinarySem/wscript b/libraries/AP_HAL/examples/BinarySem/wscript new file mode 100644 index 00000000000000..719ec151ba9d4b --- /dev/null +++ b/libraries/AP_HAL/examples/BinarySem/wscript @@ -0,0 +1,7 @@ +#!/usr/bin/env python +# encoding: utf-8 + +def build(bld): + bld.ap_example( + use='ap', + ) diff --git a/libraries/AP_HAL/examples/UART_chargen/UART_chargen.cpp b/libraries/AP_HAL/examples/UART_chargen/UART_chargen.cpp index 328da1698fac68..7a94258ab14c28 100644 --- a/libraries/AP_HAL/examples/UART_chargen/UART_chargen.cpp +++ b/libraries/AP_HAL/examples/UART_chargen/UART_chargen.cpp @@ -38,7 +38,7 @@ uint16_t buflen = 0; void setup(void) { - hal.scheduler->delay(1000); //Ensure that the uartA can be initialized + hal.scheduler->delay(1000); //Ensure that hal.serial(n) can be initialized #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX uart = hal.serial(0); // console diff --git a/libraries/AP_HAL/examples/UART_test/UART_test.cpp b/libraries/AP_HAL/examples/UART_test/UART_test.cpp index 1af94cb3bdd914..1dd5d5ed011d8f 100644 --- a/libraries/AP_HAL/examples/UART_test/UART_test.cpp +++ b/libraries/AP_HAL/examples/UART_test/UART_test.cpp @@ -30,7 +30,7 @@ void setup(void) start all UARTs at 57600 with default buffer sizes */ - hal.scheduler->delay(1000); //Ensure that the uartA can be initialized + hal.scheduler->delay(1000); //Ensure that hal.serial(n) can be initialized setup_uart(hal.serial(0), "SERIAL0"); // console setup_uart(hal.serial(1), "SERIAL1"); // telemetry 1 diff --git a/libraries/AP_HAL/utility/Socket.cpp b/libraries/AP_HAL/utility/Socket.cpp index a5beacca66d054..841c0c3c928c21 100644 --- a/libraries/AP_HAL/utility/Socket.cpp +++ b/libraries/AP_HAL/utility/Socket.cpp @@ -20,24 +20,51 @@ #include #if AP_NETWORKING_SOCKETS_ENABLED -#include "Socket.h" +#ifndef SOCKET_CLASS_NAME +#define SOCKET_CLASS_NAME SocketAPM +#endif + +#ifndef IN_SOCKET_NATIVE_CPP +#include "Socket.hpp" +#endif + +#if AP_NETWORKING_BACKEND_CHIBIOS || AP_NETWORKING_BACKEND_PPP +#include +#else +// SITL or Linux +#include +#include +#include +#include +#include +#include +#include +#include +#endif + #include -#if AP_NETWORKING_BACKEND_CHIBIOS +#if AP_NETWORKING_BACKEND_CHIBIOS || AP_NETWORKING_BACKEND_PPP #define CALL_PREFIX(x) ::lwip_##x #else #define CALL_PREFIX(x) ::x #endif +#ifndef MSG_NOSIGNAL +#define MSG_NOSIGNAL 0 +#endif + /* constructor */ -SocketAPM::SocketAPM(bool _datagram) : - SocketAPM(_datagram, +SOCKET_CLASS_NAME::SOCKET_CLASS_NAME(bool _datagram) : + SOCKET_CLASS_NAME(_datagram, CALL_PREFIX(socket)(AF_INET, _datagram?SOCK_DGRAM:SOCK_STREAM, 0)) -{} +{ + static_assert(sizeof(SOCKET_CLASS_NAME::last_in_addr) >= sizeof(struct sockaddr_in), "last_in_addr must be at least sockaddr_in size"); +} -SocketAPM::SocketAPM(bool _datagram, int _fd) : +SOCKET_CLASS_NAME::SOCKET_CLASS_NAME(bool _datagram, int _fd) : datagram(_datagram), fd(_fd) { @@ -50,19 +77,17 @@ SocketAPM::SocketAPM(bool _datagram, int _fd) : } } -SocketAPM::~SocketAPM() +SOCKET_CLASS_NAME::~SOCKET_CLASS_NAME() { if (fd != -1) { CALL_PREFIX(close)(fd); - fd = -1; } if (fd_in != -1) { CALL_PREFIX(close)(fd_in); - fd_in = -1; } } -void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr) +void SOCKET_CLASS_NAME::make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr) { memset(&sockaddr, 0, sizeof(sockaddr)); @@ -71,14 +96,18 @@ void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockadd #endif sockaddr.sin_port = htons(port); sockaddr.sin_family = AF_INET; - sockaddr.sin_addr.s_addr = inet_addr(address); + sockaddr.sin_addr.s_addr = htonl(inet_str_to_addr(address)); } +#if !defined(HAL_BOOTLOADER_BUILD) /* connect the socket */ -bool SocketAPM::connect(const char *address, uint16_t port) +bool SOCKET_CLASS_NAME::connect(const char *address, uint16_t port) { + if (fd == -1) { + return false; + } struct sockaddr_in sockaddr; int ret; int one = 1; @@ -157,12 +186,16 @@ bool SocketAPM::connect(const char *address, uint16_t port) fd_in = -1; return false; } +#endif // HAL_BOOTLOADER_BUILD /* connect the socket with a timeout */ -bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t timeout_ms) +bool SOCKET_CLASS_NAME::connect_timeout(const char *address, uint16_t port, uint32_t timeout_ms) { + if (fd == -1) { + return false; + } struct sockaddr_in sockaddr; make_sockaddr(address, port, sockaddr); @@ -182,7 +215,7 @@ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t tim } int sock_error = 0; socklen_t len = sizeof(sock_error); - if (getsockopt(fd, SOL_SOCKET, SO_ERROR, (void*)&sock_error, &len) != 0) { + if (CALL_PREFIX(getsockopt)(fd, SOL_SOCKET, SO_ERROR, (void*)&sock_error, &len) != 0) { return false; } connected = sock_error == 0; @@ -192,11 +225,15 @@ bool SocketAPM::connect_timeout(const char *address, uint16_t port, uint32_t tim /* bind the socket */ -bool SocketAPM::bind(const char *address, uint16_t port) +bool SOCKET_CLASS_NAME::bind(const char *address, uint16_t port) { + if (fd == -1) { + return false; + } struct sockaddr_in sockaddr; make_sockaddr(address, port, sockaddr); + reuseaddress(); if (CALL_PREFIX(bind)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { return false; } @@ -207,8 +244,11 @@ bool SocketAPM::bind(const char *address, uint16_t port) /* set SO_REUSEADDR */ -bool SocketAPM::reuseaddress(void) const +bool SOCKET_CLASS_NAME::reuseaddress(void) const { + if (fd == -1) { + return false; + } int one = 1; return (CALL_PREFIX(setsockopt)(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != -1); } @@ -216,8 +256,11 @@ bool SocketAPM::reuseaddress(void) const /* set blocking state */ -bool SocketAPM::set_blocking(bool blocking) const +bool SOCKET_CLASS_NAME::set_blocking(bool blocking) const { + if (fd == -1) { + return false; + } int fcntl_ret; if (blocking) { fcntl_ret = CALL_PREFIX(fcntl)(fd, F_SETFL, CALL_PREFIX(fcntl)(fd, F_GETFL, 0) & ~O_NONBLOCK); @@ -236,8 +279,11 @@ bool SocketAPM::set_blocking(bool blocking) const /* set cloexec state */ -bool SocketAPM::set_cloexec() const +bool SOCKET_CLASS_NAME::set_cloexec() const { + if (fd == -1) { + return false; + } #ifdef FD_CLOEXEC return (CALL_PREFIX(fcntl)(fd, F_SETFD, FD_CLOEXEC) != -1); #else @@ -248,16 +294,22 @@ bool SocketAPM::set_cloexec() const /* send some data */ -ssize_t SocketAPM::send(const void *buf, size_t size) const +ssize_t SOCKET_CLASS_NAME::send(const void *buf, size_t size) const { - return CALL_PREFIX(send)(fd, buf, size, 0); + if (fd == -1) { + return -1; + } + return CALL_PREFIX(send)(fd, buf, size, MSG_NOSIGNAL); } /* send some data */ -ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uint16_t port) +ssize_t SOCKET_CLASS_NAME::sendto(const void *buf, size_t size, const char *address, uint16_t port) { + if (fd == -1) { + return -1; + } struct sockaddr_in sockaddr; make_sockaddr(address, port, sockaddr); return CALL_PREFIX(sendto)(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); @@ -266,17 +318,21 @@ ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uin /* receive some data */ -ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) +ssize_t SOCKET_CLASS_NAME::recv(void *buf, size_t size, uint32_t timeout_ms) { if (!pollin(timeout_ms)) { errno = EWOULDBLOCK; return -1; } - socklen_t len = sizeof(in_addr); + socklen_t len = sizeof(struct sockaddr_in); int fin = get_read_fd(); ssize_t ret; - ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len); + ret = CALL_PREFIX(recvfrom)(fin, buf, size, MSG_DONTWAIT, (sockaddr *)&last_in_addr[0], &len); if (ret <= 0) { + if (!datagram && connected && ret == 0) { + // remote host has closed connection + connected = false; + } return ret; } if (fd_in != -1) { @@ -288,9 +344,10 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) if (CALL_PREFIX(getsockname)(fd, (struct sockaddr *)&send_addr, &send_len) != 0) { return -1; } - if (in_addr.sin_port == send_addr.sin_port && - in_addr.sin_family == send_addr.sin_family && - in_addr.sin_addr.s_addr == send_addr.sin_addr.s_addr) { + const struct sockaddr_in &sin = *(struct sockaddr_in *)&last_in_addr[0]; + if (sin.sin_port == send_addr.sin_port && + sin.sin_family == send_addr.sin_family && + sin.sin_addr.s_addr == send_addr.sin_addr.s_addr) { // discard packets from ourselves return -1; } @@ -301,27 +358,33 @@ ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) /* return the IP address and port of the last received packet */ -void SocketAPM::last_recv_address(const char *&ip_addr, uint16_t &port) const +void SOCKET_CLASS_NAME::last_recv_address(const char *&ip_addr, uint16_t &port) const { - ip_addr = inet_ntoa(in_addr.sin_addr); - port = ntohs(in_addr.sin_port); + static char buf[IP4_STR_LEN]; + auto *str = last_recv_address(buf, sizeof(buf), port); + ip_addr = str; } /* return the IP address and port of the last received packet, using caller supplied buffer */ -const char *SocketAPM::last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const +const char *SOCKET_CLASS_NAME::last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const { - const char *ret = inet_ntop(AF_INET, (void*)&in_addr.sin_addr, ip_addr_buf, buflen); + const struct sockaddr_in &sin = *(struct sockaddr_in *)&last_in_addr[0]; + + const char *ret = inet_addr_to_str(ntohl(sin.sin_addr.s_addr), ip_addr_buf, buflen); if (ret == nullptr) { return nullptr; } - port = ntohs(in_addr.sin_port); + port = ntohs(sin.sin_port); return ret; } -void SocketAPM::set_broadcast(void) const +void SOCKET_CLASS_NAME::set_broadcast(void) const { + if (fd == -1) { + return; + } int one = 1; CALL_PREFIX(setsockopt)(fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one)); } @@ -329,13 +392,16 @@ void SocketAPM::set_broadcast(void) const /* return true if there is pending data for input */ -bool SocketAPM::pollin(uint32_t timeout_ms) +bool SOCKET_CLASS_NAME::pollin(uint32_t timeout_ms) { fd_set fds; struct timeval tv; FD_ZERO(&fds); int fin = get_read_fd(); + if (fin == -1) { + return false; + } FD_SET(fin, &fds); tv.tv_sec = timeout_ms / 1000; @@ -351,8 +417,11 @@ bool SocketAPM::pollin(uint32_t timeout_ms) /* return true if there is room for output data */ -bool SocketAPM::pollout(uint32_t timeout_ms) +bool SOCKET_CLASS_NAME::pollout(uint32_t timeout_ms) { + if (fd == -1) { + return false; + } fd_set fds; struct timeval tv; @@ -371,8 +440,11 @@ bool SocketAPM::pollout(uint32_t timeout_ms) /* start listening for new tcp connections */ -bool SocketAPM::listen(uint16_t backlog) const +bool SOCKET_CLASS_NAME::listen(uint16_t backlog) const { + if (fd == -1) { + return false; + } return CALL_PREFIX(listen)(fd, (int)backlog) == 0; } @@ -380,24 +452,33 @@ bool SocketAPM::listen(uint16_t backlog) const accept a new connection. Only valid for TCP connections after listen has been used. A new socket is returned */ -SocketAPM *SocketAPM::accept(uint32_t timeout_ms) +SOCKET_CLASS_NAME *SOCKET_CLASS_NAME::accept(uint32_t timeout_ms) { + if (fd == -1) { + return nullptr; + } if (!pollin(timeout_ms)) { return nullptr; } - socklen_t len = sizeof(in_addr); - int newfd = CALL_PREFIX(accept)(fd, (sockaddr *)&in_addr, &len); + struct sockaddr_in &sin = *(struct sockaddr_in *)&last_in_addr[0]; + socklen_t len = sizeof(sin); + int newfd = CALL_PREFIX(accept)(fd, (sockaddr *)&sin, &len); if (newfd == -1) { return nullptr; } - return new SocketAPM(false, newfd); + auto *ret = new SOCKET_CLASS_NAME(false, newfd); + if (ret != nullptr) { + ret->connected = true; + ret->reuseaddress(); + } + return ret; } /* return true if an address is in the multicast range */ -bool SocketAPM::is_multicast_address(struct sockaddr_in &addr) const +bool SOCKET_CLASS_NAME::is_multicast_address(struct sockaddr_in &addr) const { const uint32_t mc_lower = 0xE0000000; // 224.0.0.0 const uint32_t mc_upper = 0xEFFFFFFF; // 239.255.255.255 @@ -405,4 +486,49 @@ bool SocketAPM::is_multicast_address(struct sockaddr_in &addr) const return haddr >= mc_lower && haddr <= mc_upper; } +void SOCKET_CLASS_NAME::close(void) +{ + if (fd != -1) { + CALL_PREFIX(close)(fd); + fd = -1; + } + if (fd_in != -1) { + CALL_PREFIX(close)(fd_in); + fd_in = -1; + } +} + +/* + duplicate a socket, giving a new object with the same contents, + the fd in the old object is set to -1 + */ +SOCKET_CLASS_NAME *SOCKET_CLASS_NAME::duplicate(void) +{ + auto *ret = new SOCKET_CLASS_NAME(datagram, fd); + if (ret == nullptr) { + return nullptr; + } + ret->fd_in = fd_in; + ret->connected = connected; + fd = -1; + fd_in = -1; + return ret; +} + +// access to inet_ntop, takes host order ipv4 as uint32_t +const char *SOCKET_CLASS_NAME::inet_addr_to_str(uint32_t addr, char *dst, uint16_t len) +{ + addr = htonl(addr); + return CALL_PREFIX(inet_ntop)(AF_INET, (void*)&addr, dst, len); +} + +// access to inet_pton, returns host order ipv4 as uint32_t +uint32_t SOCKET_CLASS_NAME::inet_str_to_addr(const char *ipstr) +{ + uint32_t ret = 0; + CALL_PREFIX(inet_pton)(AF_INET, ipstr, &ret); + return ntohl(ret); + +} + #endif // AP_NETWORKING_BACKEND_ANY diff --git a/libraries/AP_HAL/utility/Socket.h b/libraries/AP_HAL/utility/Socket.h index 70e4a1cdea1584..61f5ee7ecf6a3d 100644 --- a/libraries/AP_HAL/utility/Socket.h +++ b/libraries/AP_HAL/utility/Socket.h @@ -1,100 +1,14 @@ /* - 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 . - */ -/* - simple socket handling class for systems with BSD socket API + variant of SocketAPM using either lwip or native sockets */ #pragma once #include -#include -#if AP_NETWORKING_SOCKETS_ENABLED - -#if HAL_OS_SOCKETS -#include -#include -#include -#include -#include -#include -#include -#include -#elif AP_NETWORKING_BACKEND_CHIBIOS -#include -#include +#ifndef SOCKET_CLASS_NAME +#define SOCKET_CLASS_NAME SocketAPM #endif -class SocketAPM { -public: - SocketAPM(bool _datagram); - SocketAPM(bool _datagram, int _fd); - ~SocketAPM(); - - bool connect(const char *address, uint16_t port); - bool connect_timeout(const char *address, uint16_t port, uint32_t timeout_ms); - bool bind(const char *address, uint16_t port); - bool reuseaddress() const; - bool set_blocking(bool blocking) const; - bool set_cloexec() const; - void set_broadcast(void) const; - - ssize_t send(const void *pkt, size_t size) const; - ssize_t sendto(const void *buf, size_t size, const char *address, uint16_t port); - ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms); - - // return the IP address and port of the last received packet - void last_recv_address(const char *&ip_addr, uint16_t &port) const; - - // return the IP address and port of the last received packet, using caller supplied buffer - const char *last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const; - - // return true if there is pending data for input - bool pollin(uint32_t timeout_ms); - - // return true if there is room for output data - bool pollout(uint32_t timeout_ms); - - // start listening for new tcp connections - bool listen(uint16_t backlog) const; - - // accept a new connection. Only valid for TCP connections after - // listen has been used. A new socket is returned - SocketAPM *accept(uint32_t timeout_ms); - - // get a FD suitable for read selection - int get_read_fd(void) const { - return fd_in != -1? fd_in : fd; - } - - bool is_connected(void) const { - return connected; - } - -private: - bool datagram; - struct sockaddr_in in_addr {}; - bool is_multicast_address(struct sockaddr_in &addr) const; - - int fd = -1; - - // fd_in is used for multicast UDP - int fd_in = -1; - - bool connected; - - void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr); -}; +#include "Socket.hpp" -#endif // AP_NETWORKING_SOCKETS_ENABLED +#undef SOCKET_CLASS_NAME diff --git a/libraries/AP_HAL/utility/Socket.hpp b/libraries/AP_HAL/utility/Socket.hpp new file mode 100644 index 00000000000000..8847ce958f55b8 --- /dev/null +++ b/libraries/AP_HAL/utility/Socket.hpp @@ -0,0 +1,106 @@ +/* + 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 . + */ +/* + simple socket handling class for systems with BSD socket API + */ + +#include +#include + +#if AP_NETWORKING_SOCKETS_ENABLED || defined(AP_SOCKET_NATIVE_ENABLED) + +#ifndef SOCKET_CLASS_NAME +#error "Don't include Socket.hpp directly" +#endif + +#define IP4_STR_LEN 16 + +class SOCKET_CLASS_NAME { +public: + SOCKET_CLASS_NAME(bool _datagram); + SOCKET_CLASS_NAME(bool _datagram, int _fd); + ~SOCKET_CLASS_NAME(); + + bool connect(const char *address, uint16_t port); + bool connect_timeout(const char *address, uint16_t port, uint32_t timeout_ms); + bool bind(const char *address, uint16_t port); + bool reuseaddress() const; + bool set_blocking(bool blocking) const; + bool set_cloexec() const; + void set_broadcast(void) const; + + ssize_t send(const void *pkt, size_t size) const; + ssize_t sendto(const void *buf, size_t size, const char *address, uint16_t port); + ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms); + + // return the IP address and port of the last received packet + void last_recv_address(const char *&ip_addr, uint16_t &port) const; + + // return the IP address and port of the last received packet, using caller supplied buffer + const char *last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const; + + // return true if there is pending data for input + bool pollin(uint32_t timeout_ms); + + // return true if there is room for output data + bool pollout(uint32_t timeout_ms); + + // start listening for new tcp connections + bool listen(uint16_t backlog) const; + + // close socket + void close(void); + + // accept a new connection. Only valid for TCP connections after + // listen has been used. A new socket is returned + SOCKET_CLASS_NAME *accept(uint32_t timeout_ms); + + // get a FD suitable for read selection + int get_read_fd(void) const { + return fd_in != -1? fd_in : fd; + } + + // create a new socket with same fd, but new memory + // the old socket gets fd of -1 + SOCKET_CLASS_NAME *duplicate(void); + + bool is_connected(void) const { + return connected; + } + + // access to inet_ntop + static const char *inet_addr_to_str(uint32_t addr, char *dst, uint16_t len); + + // access to inet_pton + static uint32_t inet_str_to_addr(const char *ipstr); + +private: + bool datagram; + // we avoid using struct sockaddr_in here to keep support for + // mixing native sockets and lwip sockets in SITL + uint32_t last_in_addr[4]; + bool is_multicast_address(struct sockaddr_in &addr) const; + + int fd = -1; + + // fd_in is used for multicast UDP + int fd_in = -1; + + bool connected; + + void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr); +}; + +#endif // AP_NETWORKING_SOCKETS_ENABLED diff --git a/libraries/AP_HAL/utility/Socket_native.cpp b/libraries/AP_HAL/utility/Socket_native.cpp new file mode 100644 index 00000000000000..cd6bf2c93715ce --- /dev/null +++ b/libraries/AP_HAL/utility/Socket_native.cpp @@ -0,0 +1,16 @@ +/* + variant of SocketAPM using native sockets (not via lwip) + */ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#include "Socket_native.h" + +#if AP_SOCKET_NATIVE_ENABLED +#undef AP_NETWORKING_BACKEND_PPP +#define IN_SOCKET_NATIVE_CPP +#define SOCKET_CLASS_NAME SocketAPM_native +#include "Socket.cpp" +#endif + +#endif diff --git a/libraries/AP_HAL/utility/Socket_native.h b/libraries/AP_HAL/utility/Socket_native.h new file mode 100644 index 00000000000000..43708cd51b9b18 --- /dev/null +++ b/libraries/AP_HAL/utility/Socket_native.h @@ -0,0 +1,22 @@ +/* + variant of SocketAPM using native sockets (not via lwip) + */ +#pragma once + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#define AP_SOCKET_NATIVE_ENABLED 1 +#define SOCKET_CLASS_NAME SocketAPM_native +#ifndef AP_NETWORKING_SOCKETS_ENABLED +#define AP_NETWORKING_SOCKETS_ENABLED 1 +#endif +#include "Socket.hpp" +#elif !AP_SIM_ENABLED +#error "attempt to use Socket_native.h without native sockets" +#endif + +#undef SOCKET_CLASS_NAME + + + diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h index 9d559a561c4e64..4b62fcb9286a0b 100644 --- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h +++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h @@ -17,7 +17,7 @@ namespace ChibiOS { class RCOutput; class Scheduler; class Semaphore; - class EventSource; + class BinarySemaphore; class SPIBus; class SPIDesc; class SPIDevice; diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index ea188572091a49..0621162132eef5 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -148,6 +148,22 @@ float AnalogSource::_pin_scaler(void) break; } } +#ifdef HAL_ANALOG2_PINS + for (uint8_t i=0; iNBTP = ((timings.sjw << FDCAN_NBTP_NSJW_Pos) | - (timings.bs1 << FDCAN_NBTP_NTSEG1_Pos) | - (timings.bs2 << FDCAN_NBTP_NTSEG2_Pos) | - (timings.prescaler << FDCAN_NBTP_NBRP_Pos)); + can_->NBTP = (((timings.sjw-1) << FDCAN_NBTP_NSJW_Pos) | + ((timings.bs1-1) << FDCAN_NBTP_NTSEG1_Pos) | + ((timings.bs2-1) << FDCAN_NBTP_NTSEG2_Pos) | + ((timings.prescaler-1) << FDCAN_NBTP_NBRP_Pos)); if (fdbitrate) { - if (!computeTimings(fdbitrate, fdtimings)) { + if (!computeFDTimings(fdbitrate, fdtimings)) { can_->CCCR &= ~FDCAN_CCCR_INIT; uint32_t while_start_ms = AP_HAL::millis(); while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { @@ -685,9 +716,10 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper _fdbitrate = fdbitrate; Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n", unsigned(fdtimings.prescaler), unsigned(fdtimings.bs1), unsigned(fdtimings.bs2)); - can_->DBTP = ((fdtimings.bs1 << FDCAN_DBTP_DTSEG1_Pos) | - (fdtimings.bs2 << FDCAN_DBTP_DTSEG2_Pos) | - (fdtimings.prescaler << FDCAN_DBTP_DBRP_Pos)); + can_->DBTP = (((fdtimings.bs1-1) << FDCAN_DBTP_DTSEG1_Pos) | + ((fdtimings.bs2-1) << FDCAN_DBTP_DTSEG2_Pos) | + ((fdtimings.prescaler-1) << FDCAN_DBTP_DBRP_Pos) | + ((fdtimings.sjw-1) << FDCAN_DBTP_DSJW_Pos)); } //RX Config @@ -812,11 +844,9 @@ void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) rx_item.flags = AP_HAL::CANIface::Loopback; add_to_rx_queue(rx_item); } - if (event_handle_ != nullptr) { - stats.num_events++; -#if CH_CFG_USE_EVENTS == TRUE - evt_src_.signalI(1 << self_index_); -#endif + stats.num_events++; + if (sem_handle != nullptr) { + sem_handle->signal_ISR(); } } } @@ -925,11 +955,9 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index) while (readRxFIFO(fifo_index)) { had_activity_ = true; } - if (event_handle_ != nullptr) { - stats.num_events++; -#if CH_CFG_USE_EVENTS == TRUE - evt_src_.signalI(1 << self_index_); -#endif + stats.num_events++; + if (sem_handle != nullptr) { + sem_handle->signal_ISR(); } } @@ -995,16 +1023,11 @@ uint32_t CANIface::getErrorCount() const stats.tx_timedout; } -#if CH_CFG_USE_EVENTS == TRUE -ChibiOS::EventSource CANIface::evt_src_; -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) { - CriticalSectionLocker lock; - event_handle_ = handle; - event_handle_->set_source(&evt_src_); - return event_handle_->register_event(1 << self_index_); + sem_handle = handle; + return true; } -#endif bool CANIface::isRxBufferEmpty() const { @@ -1075,10 +1098,10 @@ bool CANIface::select(bool &read, bool &write, return true; } while (time < blocking_deadline) { - if (event_handle_ == nullptr) { + if (sem_handle == nullptr) { break; } - event_handle_->wait(blocking_deadline - time); // Block until timeout expires or any iface updates + IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates checkAvailable(read, write, pending_tx); // Check what we got if ((read && in_read) || (write && in_write)) { return true; @@ -1120,7 +1143,7 @@ void CANIface::get_stats(ExpandingString &str) unsigned(timings.bs2), timings.sample_point_permill/10.0f, _fdbitrate, unsigned(fdtimings.prescaler), unsigned(fdtimings.sjw), unsigned(fdtimings.bs1), - unsigned(fdtimings.bs2), timings.sample_point_permill/10.0f, + unsigned(fdtimings.bs2), fdtimings.sample_point_permill/10.0f, stats.tx_requests, stats.tx_rejected, stats.tx_overflow, diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 9b214441617207..fb6a9719c7fcb5 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.h +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.h @@ -41,7 +41,6 @@ #pragma once #include "AP_HAL_ChibiOS.h" -#include "EventSource.h" #if HAL_NUM_CAN_IFACES @@ -73,11 +72,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface struct CriticalSectionLocker { CriticalSectionLocker() { - chSysSuspend(); + chSysLock(); } ~CriticalSectionLocker() { - chSysEnable(); + chSysUnlock(); } }; @@ -120,13 +119,12 @@ class ChibiOS::CANIface : public AP_HAL::CANIface bool irq_init_; bool initialised_; bool had_activity_; - AP_HAL::EventHandle* event_handle_; -#if CH_CFG_USE_EVENTS == TRUE - static ChibiOS::EventSource evt_src_; -#endif + AP_HAL::BinarySemaphore *sem_handle; + const uint8_t self_index_; - bool computeTimings(uint32_t target_bitrate, Timings& out_timings); + bool computeTimings(uint32_t target_bitrate, Timings& out_timings) const; + bool computeFDTimings(uint32_t target_bitrate, Timings& out_timings) const; void setupMessageRam(void); @@ -224,10 +222,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) override; -#if CH_CFG_USE_EVENTS == TRUE // setup event handle for waiting on events - bool set_event_handle(AP_HAL::EventHandle* handle) override; -#endif + bool set_event_handle(AP_HAL::BinarySemaphore *handle) override; #if !defined(HAL_BOOTLOADER_BUILD) // fetch stats text and return the size of the same, diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h index 47809fa79b35a3..363c002d491ec8 100644 --- a/libraries/AP_HAL_ChibiOS/CANIface.h +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -46,7 +46,6 @@ # else #if HAL_NUM_CAN_IFACES #include "bxcan.hpp" -#include "EventSource.h" #ifndef HAL_CAN_RX_QUEUE_SIZE #define HAL_CAN_RX_QUEUE_SIZE 128 @@ -69,11 +68,11 @@ class ChibiOS::CANIface : public AP_HAL::CANIface struct CriticalSectionLocker { CriticalSectionLocker() { - chSysSuspend(); + chSysLock(); } ~CriticalSectionLocker() { - chSysEnable(); + chSysUnlock(); } }; @@ -109,10 +108,8 @@ class ChibiOS::CANIface : public AP_HAL::CANIface bool irq_init_:1; bool initialised_:1; bool had_activity_:1; -#if CH_CFG_USE_EVENTS == TRUE - AP_HAL::EventHandle* event_handle_; - static ChibiOS::EventSource evt_src_; -#endif + AP_HAL::BinarySemaphore *sem_handle; + const uint8_t self_index_; bool computeTimings(uint32_t target_bitrate, Timings& out_timings); @@ -210,10 +207,9 @@ class ChibiOS::CANIface : public AP_HAL::CANIface const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) override; -#if CH_CFG_USE_EVENTS == TRUE // setup event handle for waiting on events - bool set_event_handle(AP_HAL::EventHandle* handle) override; -#endif + bool set_event_handle(AP_HAL::BinarySemaphore *handle) override; + #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) // fetch stats text and return the size of the same, // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp index 97cd8acf2ad1be..f6704b6438d09d 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -522,12 +522,11 @@ void CANIface::handleTxInterrupt(const uint64_t utc_usec) handleTxMailboxInterrupt(2, txok, utc_usec); } -#if CH_CFG_USE_EVENTS == TRUE - if (event_handle_ != nullptr) { + if (sem_handle != nullptr) { PERF_STATS(stats.num_events); - evt_src_.signalI(1 << self_index_); + sem_handle->signal_ISR(); } -#endif + pollErrorFlagsFromISR(); } @@ -590,12 +589,11 @@ void CANIface::handleRxInterrupt(uint8_t fifo_index, uint64_t timestamp_us) had_activity_ = true; -#if CH_CFG_USE_EVENTS == TRUE - if (event_handle_ != nullptr) { + if (sem_handle != nullptr) { PERF_STATS(stats.num_events); - evt_src_.signalI(1 << self_index_); + sem_handle->signal_ISR(); } -#endif + pollErrorFlagsFromISR(); } @@ -702,17 +700,12 @@ uint32_t CANIface::getErrorCount() const #endif // #if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) -#if CH_CFG_USE_EVENTS == TRUE -ChibiOS::EventSource CANIface::evt_src_; -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) { - CriticalSectionLocker lock; - event_handle_ = handle; - event_handle_->set_source(&evt_src_); - return event_handle_->register_event(1 << self_index_); + sem_handle = handle; + return true; } -#endif // #if CH_CFG_USE_EVENTS == TRUE void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const { @@ -745,13 +738,13 @@ bool CANIface::select(bool &read, bool &write, return true; } -#if CH_CFG_USE_EVENTS == TRUE +#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_BOOTLOADER_BUILD) // we don't support blocking select in AP_Periph and bootloader while (time < blocking_deadline) { - if (event_handle_ == nullptr) { + if (sem_handle == nullptr) { break; } - event_handle_->wait(blocking_deadline - time); // Block until timeout expires or any iface updates + IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates checkAvailable(read, write, pending_tx); // Check what we got if ((read && in_read) || (write && in_write)) { return true; diff --git a/libraries/AP_HAL_ChibiOS/EventSource.cpp b/libraries/AP_HAL_ChibiOS/EventSource.cpp deleted file mode 100644 index f1821bca000b89..00000000000000 --- a/libraries/AP_HAL_ChibiOS/EventSource.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "EventSource.h" -#include - -using namespace ChibiOS; - -#if CH_CFG_USE_EVENTS == TRUE - -bool EventSource::wait(uint16_t duration_us, AP_HAL::EventHandle *evt_handle) -{ - chibios_rt::EventListener evt_listener; - eventmask_t evt_mask = evt_handle->get_evt_mask(); - msg_t ret = msg_t(); - ch_evt_src_.registerMask(&evt_listener, evt_mask); - if (duration_us == 0) { - ret = chEvtWaitAnyTimeout(evt_mask, TIME_IMMEDIATE); - } else { - const sysinterval_t wait_us = MIN(TIME_MAX_INTERVAL, MAX(CH_CFG_ST_TIMEDELTA, chTimeUS2I(duration_us))); - ret = chEvtWaitAnyTimeout(evt_mask, wait_us); - } - ch_evt_src_.unregister(&evt_listener); - return ret == MSG_OK; -} - -void EventSource::signal(uint32_t evt_mask) -{ - ch_evt_src_.broadcastFlags(evt_mask); -} - -__RAMFUNC__ void EventSource::signalI(uint32_t evt_mask) -{ - chSysLockFromISR(); - ch_evt_src_.broadcastFlagsI(evt_mask); - chSysUnlockFromISR(); -} -#endif //#if CH_CFG_USE_EVENTS == TRUE diff --git a/libraries/AP_HAL_ChibiOS/EventSource.h b/libraries/AP_HAL_ChibiOS/EventSource.h deleted file mode 100644 index a1dda78b2e3208..00000000000000 --- a/libraries/AP_HAL_ChibiOS/EventSource.h +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once -#include - -#include -#include -#include -#include -#include "AP_HAL_ChibiOS_Namespace.h" -#include - -#if CH_CFG_USE_EVENTS == TRUE -class ChibiOS::EventSource : public AP_HAL::EventSource { - // Single event source to be shared across multiple users - chibios_rt::EventSource ch_evt_src_; - -public: - // generate event from thread context - void signal(uint32_t evt_mask) override; - - // generate event from interrupt context - void signalI(uint32_t evt_mask) override; - - // Wait on an Event handle, method for internal use by EventHandle - bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override; -}; -#endif //#if CH_CFG_USE_EVENTS == TRUE diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp index 93d72a948c3281..f62bd73c27bfd2 100644 --- a/libraries/AP_HAL_ChibiOS/GPIO.cpp +++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp @@ -37,9 +37,8 @@ using namespace ChibiOS; extern AP_IOMCU iomcu; #endif - // GPIO pin table from hwdef.dat -static struct gpio_entry { +struct gpio_entry { uint8_t pin_num; bool enabled; uint8_t pwm_num; @@ -51,13 +50,22 @@ static struct gpio_entry { uint16_t isr_quota; uint8_t isr_disabled_ticks; AP_HAL::GPIO::INTERRUPT_TRIGGER_TYPE isr_mode; -} _gpio_tab[] = HAL_GPIO_PINS; +}; + +#ifdef HAL_GPIO_PINS +#define HAVE_GPIO_PINS 1 +static struct gpio_entry _gpio_tab[] = HAL_GPIO_PINS; +#else +#define HAVE_GPIO_PINS 0 +#endif + /* map a user pin number to a GPIO table entry */ static struct gpio_entry *gpio_by_pin_num(uint8_t pin_num, bool check_enabled=true) { +#if HAVE_GPIO_PINS for (uint8_t i=0; ipwm_num != 0) { g->enabled = SRV_Channels::is_GPIO((g->pwm_num-1)+chan_offset); } } +#endif // HAVE_GPIO_PINS #endif // HAL_BOOTLOADER_BUILD #ifdef HAL_PIN_ALT_CONFIG setup_alt_config(); @@ -139,6 +152,7 @@ void GPIO::setup_alt_config(void) if (alt_config == alt.alternate) { if (alt.periph_type == PERIPH_TYPE::GPIO) { // enable pin in GPIO table +#if HAVE_GPIO_PINS for (uint8_t j=0; jpal_line == alt.line) { @@ -146,6 +160,7 @@ void GPIO::setup_alt_config(void) break; } } +#endif // HAVE_GPIO_PINS continue; } const iomode_t mode = alt.mode & ~PAL_STM32_HIGH; @@ -523,7 +538,9 @@ bool GPIO::valid_pin(uint8_t pin) const // servo_ch uses zero-based indexing bool GPIO::pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const { +#if HAL_WITH_IO_MCU || HAVE_GPIO_PINS uint8_t fmu_chan_offset = 0; +#endif #if HAL_WITH_IO_MCU if (AP_BoardConfig::io_enabled()) { // check if this is one of the main pins @@ -537,6 +554,7 @@ bool GPIO::pin_to_servo_channel(uint8_t pin, uint8_t& servo_ch) const } #endif +#if HAVE_GPIO_PINS // search _gpio_tab for matching pin for (uint8_t i=0; isnprintf(buffer, buflen, "Pin %u disabled (ISR flood)", _gpio_tab[i].pin_num); return false; } } +#endif // HAVE_GPIO_PINS return true; } #endif // IOMCU_FW diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 46b352e5649f50..eb741e1ae7c520 100644 --- a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp +++ b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp @@ -45,27 +45,27 @@ #endif #ifndef HAL_NO_UARTDRIVER -static HAL_UARTA_DRIVER; -static HAL_UARTB_DRIVER; -static HAL_UARTC_DRIVER; -static HAL_UARTD_DRIVER; -static HAL_UARTE_DRIVER; -static HAL_UARTF_DRIVER; -static HAL_UARTG_DRIVER; -static HAL_UARTH_DRIVER; -static HAL_UARTI_DRIVER; -static HAL_UARTJ_DRIVER; +static HAL_SERIAL0_DRIVER; +static HAL_SERIAL1_DRIVER; +static HAL_SERIAL2_DRIVER; +static HAL_SERIAL3_DRIVER; +static HAL_SERIAL4_DRIVER; +static HAL_SERIAL5_DRIVER; +static HAL_SERIAL6_DRIVER; +static HAL_SERIAL7_DRIVER; +static HAL_SERIAL8_DRIVER; +static HAL_SERIAL9_DRIVER; #else -static Empty::UARTDriver uartADriver; -static Empty::UARTDriver uartBDriver; -static Empty::UARTDriver uartCDriver; -static Empty::UARTDriver uartDDriver; -static Empty::UARTDriver uartEDriver; -static Empty::UARTDriver uartFDriver; -static Empty::UARTDriver uartGDriver; -static Empty::UARTDriver uartHDriver; -static Empty::UARTDriver uartIDriver; -static Empty::UARTDriver uartJDriver; +static Empty::UARTDriver serial0Driver; +static Empty::UARTDriver serial1Driver; +static Empty::UARTDriver serial2Driver; +static Empty::UARTDriver serial3Driver; +static Empty::UARTDriver serial4Driver; +static Empty::UARTDriver serial5Driver; +static Empty::UARTDriver serial6Driver; +static Empty::UARTDriver serial7Driver; +static Empty::UARTDriver serial8Driver; +static Empty::UARTDriver serial9Driver; #endif #if HAL_USE_I2C == TRUE && defined(HAL_I2C_DEVICE_LIST) @@ -110,8 +110,6 @@ static AP_HAL::SIMState xsimstate; #if HAL_WITH_DSP static ChibiOS::DSP dspDriver; -#else -static Empty::DSP dspDriver; #endif #ifndef HAL_NO_FLASH_SUPPORT @@ -136,16 +134,16 @@ AP_IOMCU iomcu(uart_io); HAL_ChibiOS::HAL_ChibiOS() : AP_HAL::HAL( - &uartADriver, - &uartBDriver, - &uartCDriver, - &uartDDriver, - &uartEDriver, - &uartFDriver, - &uartGDriver, - &uartHDriver, - &uartIDriver, - &uartJDriver, + &serial0Driver, + &serial1Driver, + &serial2Driver, + &serial3Driver, + &serial4Driver, + &serial5Driver, + &serial6Driver, + &serial7Driver, + &serial8Driver, + &serial9Driver, &i2cDeviceManager, &spiDeviceManager, #if HAL_USE_WSPI == TRUE && defined(HAL_WSPI_DEVICE_LIST) @@ -155,7 +153,7 @@ HAL_ChibiOS::HAL_ChibiOS() : #endif &analogIn, &storageDriver, - &uartADriver, + &serial0Driver, &gpioDriver, &rcinDriver, &rcoutDriver, @@ -166,7 +164,9 @@ HAL_ChibiOS::HAL_ChibiOS() : #if AP_SIM_ENABLED &xsimstate, #endif +#if HAL_WITH_DSP &dspDriver, +#endif #if HAL_NUM_CAN_IFACES (AP_HAL::CANIface**)canDrivers #else @@ -227,7 +227,7 @@ static void main_loop() hal.serial(0)->begin(SERIAL0_BAUD); -#ifdef HAL_SPI_CHECK_CLOCK_FREQ +#if (HAL_USE_SPI == TRUE) && defined(HAL_SPI_CHECK_CLOCK_FREQ) // optional test of SPI clock frequencies ChibiOS::SPIDevice::test_clock_freq(); #endif @@ -268,10 +268,12 @@ static void main_loop() #ifdef IOMCU_FW stm32_watchdog_init(); #elif !defined(HAL_BOOTLOADER_BUILD) +#if !defined(HAL_EARLY_WATCHDOG_INIT) // setup watchdog to reset if main loop stops if (AP_BoardConfig::watchdog_enabled()) { stm32_watchdog_init(); } +#endif if (hal.util->was_watchdog_reset()) { INTERNAL_ERROR(AP_InternalError::error_t::watchdog_reset); @@ -314,6 +316,10 @@ static void main_loop() void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const { +#if defined(HAL_EARLY_WATCHDOG_INIT) && !defined(DISABLE_WATCHDOG) + stm32_watchdog_init(); + stm32_watchdog_pat(); +#endif /* * System initializations. * - ChibiOS HAL initialization, this also initializes the configured device drivers diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index e703d362991052..1817c3e6813eb9 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -16,6 +16,7 @@ * Bi-directional dshot based on Betaflight, code by Andy Piper and Siddharth Bharat Purohit * * There really is no dshot reference. For information try these resources: + * https://brushlesswhoop.com/dshot-and-bidirectional-dshot/ * https://blck.mn/2016/11/dshot-the-new-kid-on-the-block/ * https://www.swallenhardware.io/battlebots/2019/4/20/a-developers-guide-to-dshot-escs */ @@ -100,6 +101,10 @@ void RCOutput::init() if (AP_BoardConfig::io_enabled()) { // with IOMCU the local (FMU) channels start at 8 chan_offset = 8; + iomcu_enabled = true; + } + if (AP_BoardConfig::io_dshot()) { + iomcu_dshot = true; } #endif @@ -144,7 +149,7 @@ void RCOutput::init() } #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { iomcu.init(); } #endif @@ -336,8 +341,9 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread) continue; } + // dma handle will only be unlocked if the send was aborted if (group.dma_handle != nullptr && group.dma_handle->is_locked()) { - // calculate how long we have left + // if we have time left wait for the event const sysinterval_t wait_ticks = calc_ticks_remaining(group, time_out_us, led_thread ? LED_OUTPUT_PERIOD_US : _dshot_period_us); const eventmask_t mask = chEvtWaitOneTimeout(group.dshot_event_mask, wait_ticks); @@ -350,13 +356,11 @@ void RCOutput::dshot_collect_dma_locks(uint64_t time_out_us, bool led_thread) #ifdef HAL_WITH_BIDIR_DSHOT // if using input capture DMA then clean up if (group.bdshot.enabled) { - // the channel index only moves on with success - const uint8_t chan = mask ? group.bdshot.prev_telem_chan - : group.bdshot.curr_telem_chan; // only unlock if not shared - if (group.bdshot.ic_dma_handle[chan] != nullptr - && group.bdshot.ic_dma_handle[chan] != group.dma_handle) { - group.bdshot.ic_dma_handle[chan]->unlock(); + if (group.bdshot.curr_ic_dma_handle != nullptr + && group.bdshot.curr_ic_dma_handle != group.dma_handle) { + group.bdshot.curr_ic_dma_handle->unlock(); + group.bdshot.curr_ic_dma_handle = nullptr; } } #endif @@ -443,7 +447,7 @@ void RCOutput::set_freq_group(pwm_group &group) void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) { #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { // change frequency on IOMCU uint16_t io_chmask = chmask & 0xFF; if (io_chmask) { @@ -500,7 +504,7 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) void RCOutput::set_default_rate(uint16_t freq_hz) { #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { iomcu.set_default_rate(freq_hz); } #endif @@ -527,7 +531,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) _dshot_period_us = 1000UL; _dshot_rate = 0; #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_dshot()) { + if (iomcu_dshot) { iomcu.set_dshot_period(1000UL, 0); } #endif @@ -543,7 +547,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) #if HAL_WITH_IO_MCU // this is not strictly neccessary since the iomcu could run at a different rate, // but there is only one parameter to control this - if (AP_BoardConfig::io_dshot()) { + if (iomcu_dshot) { iomcu.set_dshot_period(1000UL, 0); } #endif @@ -567,7 +571,7 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz) } _dshot_period_us = 1000000UL / drate; #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_dshot()) { + if (iomcu_dshot) { iomcu.set_dshot_period(_dshot_period_us, _dshot_rate); } #endif @@ -593,6 +597,11 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type) DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_DEFAULT; break; } +#if HAL_WITH_IO_MCU + if (iomcu_dshot) { + iomcu.set_dshot_esc_type(dshot_esc_type); + } +#endif } #endif // #if HAL_DSHOT_ENABLED @@ -666,7 +675,7 @@ uint16_t RCOutput::get_freq(uint8_t chan) void RCOutput::enable_ch(uint8_t chan) { #if HAL_WITH_IO_MCU - if (chan < chan_offset && AP_BoardConfig::io_enabled()) { + if (chan < chan_offset && iomcu_enabled) { iomcu.enable_ch(chan); return; } @@ -682,7 +691,7 @@ void RCOutput::enable_ch(uint8_t chan) void RCOutput::disable_ch(uint8_t chan) { #if HAL_WITH_IO_MCU - if (chan < chan_offset && AP_BoardConfig::io_enabled()) { + if (chan < chan_offset && iomcu_enabled) { iomcu.disable_ch(chan); return; } @@ -713,7 +722,7 @@ void RCOutput::write(uint8_t chan, uint16_t period_us) #if HAL_WITH_IO_MCU // handle IO MCU channels - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { iomcu.write_channel(chan, period_us); } #endif @@ -973,10 +982,11 @@ bool RCOutput::setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_ #ifdef HAL_WITH_BIDIR_DSHOT // configure input capture DMA if required - if (is_bidir_dshot_enabled()) { + if (is_bidir_dshot_enabled(group)) { if (!bdshot_setup_group_ic_DMA(group)) { - group.dma_handle->unlock(); - return false; + _bdshot.mask &= ~group.ch_mask; // only use dshot on this group + _bdshot.disabled_mask |= group.ch_mask; + active_high = true; } } #endif @@ -1107,7 +1117,7 @@ void RCOutput::set_group_mode(pwm_group &group) case MODE_PWM_DSHOT150 ... MODE_PWM_DSHOT1200: { #if HAL_DSHOT_ENABLED const uint32_t rate = protocol_bitrate(group.current_mode); - bool active_high = is_bidir_dshot_enabled() ? false : true; + bool active_high = is_bidir_dshot_enabled(group) ? false : true; bool at_least_freq = false; // calculate min time between pulses const uint32_t pulse_send_time_us = 1000000UL * dshot_bit_length / rate; @@ -1123,7 +1133,7 @@ void RCOutput::set_group_mode(pwm_group &group) group.current_mode = MODE_PWM_NORMAL; break; } - if (is_bidir_dshot_enabled()) { + if (is_bidir_dshot_enabled(group)) { group.dshot_pulse_send_time_us = pulse_send_time_us; // to all intents and purposes the pulse time of send and receive are the same group.dshot_pulse_time_us = pulse_send_time_us + pulse_send_time_us + 30; @@ -1188,13 +1198,14 @@ void RCOutput::set_output_mode(uint32_t mask, const enum output_mode mode) } } #if HAL_WITH_IO_MCU + const uint16_t iomcu_mask = (mask & ((1U<= MODE_PWM_DSHOT150 && mode <= MODE_PWM_DSHOT600)) && - (mask & ((1U<is_shared()) { + /* Timer configured and started.*/ + group.pwm_drv->tim->CR1 = STM32_TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN; + group.pwm_drv->tim->DIER = group.pwm_drv->config->dier & ~STM32_TIM_DIER_IRQ_MASK; } #endif - chSysUnlock(); #if STM32_DMA_SUPPORTS_DMAMUX if (group.dma) { dmaSetRequestSource(group.dma, group.dma_up_channel); } #endif + chSysUnlock(); } } } @@ -1511,11 +1535,13 @@ void RCOutput::dma_deallocate(Shared_DMA *ctx) for (auto &group : pwm_group_list) { if (group.dma_handle == ctx && group.dma != nullptr) { chSysLock(); -#if defined(IOMCU_FW) +#if defined(STM32F1) // leaving the peripheral running on IOMCU plays havoc with the UART that is - // also sharing this channel - if (group.pwm_started) { - pwmStop(group.pwm_drv); + // also sharing this channel, we only turn it off rather than resetting so + // that we don't have to worry about line modes etc + if (group.pwm_started && group.dma_handle->is_shared()) { + group.pwm_drv->tim->CR1 = 0; + group.pwm_drv->tim->DIER = 0; } #endif dmaStreamFreeI(group.dma); @@ -1588,7 +1614,7 @@ void RCOutput::fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16 void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) { #if HAL_DSHOT_ENABLED - if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) { + if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) { // doing serial output or DMAR input, don't send DShot pulses return; } @@ -1601,7 +1627,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) // if we are sharing UP channels then it might have taken a long time to get here, // if there's not enough time to actually send a pulse then cancel #if AP_HAL_SHARED_DMA_ENABLED - if (time_out_us > 0 && AP_HAL::micros64() + group.dshot_pulse_time_us > time_out_us) { + if (AP_HAL::micros64() + group.dshot_pulse_time_us > time_out_us) { group.dma_handle->unlock(); return; } @@ -1681,7 +1707,7 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) } } - chEvtGetAndClearEvents(group.dshot_event_mask); + chEvtGetAndClearEvents(group.dshot_event_mask | DSHOT_CASCADE); // start sending the pulses out send_pulses_DMAR(group, DSHOT_BUFFER_LENGTH); #endif // HAL_DSHOT_ENABLED @@ -1700,7 +1726,7 @@ bool RCOutput::serial_led_send(pwm_group &group) } #if HAL_DSHOT_ENABLED - if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE) + if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state) || AP_HAL::micros64() - group.last_dmar_send_us < (group.dshot_pulse_time_us + 50)) { // doing serial output or DMAR input, don't send DShot pulses return false; @@ -1721,7 +1747,7 @@ bool RCOutput::serial_led_send(pwm_group &group) group.dshot_waiter = led_thread_ctx; - chEvtGetAndClearEvents(group.dshot_event_mask); + chEvtGetAndClearEvents(group.dshot_event_mask | DSHOT_CASCADE); // start sending the pulses out send_pulses_DMAR(group, group.dma_buffer_len); @@ -1766,10 +1792,18 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length) #endif dmaStreamSetMode(group.dma, STM32_DMA_CR_CHSEL(group.dma_up_channel) | - STM32_DMA_CR_DIR_M2P | STM32_DMA_CR_PSIZE_WORD | -#if defined(IOMCU_FW) + STM32_DMA_CR_DIR_M2P | +#if defined(STM32F1) +#ifdef HAL_WITH_BIDIR_DSHOT + STM32_DMA_CR_PSIZE_HWORD | + STM32_DMA_CR_MSIZE_HWORD | +#else + STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_BYTE | +#endif + #else + STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD | #endif STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) | @@ -1858,7 +1892,7 @@ void RCOutput::dma_cancel(pwm_group& group) } } chVTResetI(&group.dma_timeout); - chEvtGetAndClearEventsI(group.dshot_event_mask); + chEvtGetAndClearEventsI(group.dshot_event_mask | DSHOT_CASCADE); group.dshot_state = DshotState::IDLE; chSysUnlock(); @@ -2201,7 +2235,7 @@ void RCOutput::serial_end(void) AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void) { #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { safety_state = iomcu.get_safety_switch_state(); } #endif @@ -2217,7 +2251,7 @@ AP_HAL::Util::safety_state RCOutput::_safety_switch_state(void) bool RCOutput::force_safety_on(void) { #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { return iomcu.force_safety_on(); } #endif @@ -2231,7 +2265,7 @@ bool RCOutput::force_safety_on(void) void RCOutput::force_safety_off(void) { #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { iomcu.force_safety_off(); return; } @@ -2299,7 +2333,7 @@ void RCOutput::safety_update(void) void RCOutput::set_failsafe_pwm(uint32_t chmask, uint16_t period_us) { #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_enabled()) { + if (iomcu_enabled) { iomcu.set_failsafe_pwm(chmask, period_us); } #endif diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 1fbd983944e5c0..8fbfa6615b81f2 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -25,10 +25,17 @@ #if HAL_USE_PWM == TRUE -#if defined(IOMCU_FW) +#if defined(STM32F1) +#ifdef HAL_WITH_BIDIR_DSHOT +typedef uint16_t dmar_uint_t; // save memory to allow dshot on IOMCU +typedef int16_t dmar_int_t; +#else typedef uint8_t dmar_uint_t; // save memory to allow dshot on IOMCU +typedef int8_t dmar_int_t; +#endif #else typedef uint32_t dmar_uint_t; +typedef int32_t dmar_int_t; #endif #define RCOU_DSHOT_TIMING_DEBUG 0 @@ -59,6 +66,11 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput float get_erpm_error_rate(uint8_t chan) const override { return 100.0f * float(_bdshot.erpm_errors[chan]) / (1 + _bdshot.erpm_errors[chan] + _bdshot.erpm_clean_frames[chan]); } + /* + allow all erpm values to be read and for new updates to be detected - primarily for IOMCU + */ + bool new_erpm() override { return _bdshot.update_mask != 0; } + uint32_t read_erpm(uint16_t* erpm, uint8_t len) override; #endif void set_output_mode(uint32_t mask, const enum output_mode mode) override; enum output_mode get_output_mode(uint32_t& mask) override; @@ -152,14 +164,13 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput */ void set_telem_request_mask(uint32_t mask) override; -#ifdef HAL_WITH_BIDIR_DSHOT /* enable bi-directional telemetry request for a mask of channels. This is used with Dshot to get telemetry feedback The mask uses servo channel numbering */ void set_bidir_dshot_mask(uint32_t mask) override; - +#ifdef HAL_WITH_BIDIR_DSHOT void set_motor_poles(uint8_t poles) override { _bdshot.motor_poles = poles; } #endif @@ -277,7 +288,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput SEND_START = 1, SEND_COMPLETE = 2, RECV_START = 3, - RECV_COMPLETE = 4 + RECV_COMPLETE = 4, + RECV_FAILED = 5 }; struct PACKED SerialLed { @@ -296,8 +308,13 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput static const uint16_t DSHOT_BUFFER_LENGTH = dshot_bit_length * 4 * sizeof(dmar_uint_t); static const uint16_t MIN_GCR_BIT_LEN = 7; static const uint16_t MAX_GCR_BIT_LEN = 22; + static const uint16_t TELEM_IC_SAMPLE = 16; static const uint16_t GCR_TELEMETRY_BIT_LEN = MAX_GCR_BIT_LEN; - static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(uint32_t); + // input capture is expecting TELEM_IC_SAMPLE (16) ticks per transition (22) so the maximum + // value of the counter in CCR registers is 16*22 == 352, so must be 16-bit + static const uint16_t GCR_TELEMETRY_BUFFER_LEN = GCR_TELEMETRY_BIT_LEN*sizeof(dmar_uint_t); + static const uint16_t INVALID_ERPM = 0xffffU; + static const uint16_t ZERO_ERPM = 0x0fffU; struct pwm_group { // only advanced timers can do high clocks needed for more than 400Hz @@ -315,6 +332,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint8_t stream_id; uint8_t channel; } dma_ch[4]; +#ifdef HAL_TIM_UP_SHARED + bool shared_up_dma; // do we need to wait for TIMx_UP DMA to be finished after use +#endif #endif uint8_t alt_functions[4]; ioline_t pal_lines[4]; @@ -380,8 +400,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint8_t telem_tim_ch[4]; uint8_t curr_telem_chan; uint8_t prev_telem_chan; + Shared_DMA *curr_ic_dma_handle; // a shortcut to avoid logic errors involving the wrong lock uint16_t telempsc; - uint32_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN]; + dmar_uint_t dma_buffer_copy[GCR_TELEMETRY_BUFFER_LEN]; #if RCOU_DSHOT_TIMING_DEBUG uint16_t telem_rate[4]; uint16_t telem_err_rate[4]; @@ -499,6 +520,13 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput static pwm_group pwm_group_list[]; static const uint8_t NUM_GROUPS; +#if HAL_WITH_IO_MCU + // cached values of AP_BoardConfig::io_enabled() and AP_BoardConfig::io_dshot() + // in case the user changes them + bool iomcu_enabled; + bool iomcu_dshot; +#endif + // offset of first local channel uint8_t chan_offset; @@ -524,7 +552,9 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput // handling of bi-directional dshot struct { uint32_t mask; + uint32_t disabled_mask; // record of channels that were tried, but failed uint16_t erpm[max_channels]; + volatile uint32_t update_mask; #ifdef HAL_WITH_BIDIR_DSHOT uint16_t erpm_errors[max_channels]; uint16_t erpm_clean_frames[max_channels]; @@ -591,7 +621,11 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput volatile bool _initialised; - bool is_bidir_dshot_enabled() const { return _bdshot.mask != 0; } + bool is_bidir_dshot_enabled(const pwm_group& group) const { return (_bdshot.mask & group.ch_mask) != 0; } + + static bool is_dshot_send_allowed(DshotState state) { + return state == DshotState::IDLE || state == DshotState::RECV_COMPLETE || state == DshotState::RECV_FAILED; + } // are all the ESCs returning data bool group_escs_active(const pwm_group& group) const { @@ -644,6 +678,12 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput uint16_t create_dshot_packet(const uint16_t value, bool telem_request, bool bidir_telem); void fill_DMA_buffer_dshot(dmar_uint_t *buffer, uint8_t stride, uint16_t packet, uint16_t clockmul); + // event to allow dshot cascading +#if defined(HAL_TIM_UP_SHARED) + static const eventmask_t DSHOT_CASCADE = EVENT_MASK(16); +#else + static const eventmask_t DSHOT_CASCADE = 0; +#endif static const eventmask_t EVT_PWM_SEND = EVENT_MASK(11); static const eventmask_t EVT_PWM_SYNTHETIC_SEND = EVENT_MASK(13); @@ -672,7 +712,8 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput */ void bdshot_ic_dma_allocate(Shared_DMA *ctx); void bdshot_ic_dma_deallocate(Shared_DMA *ctx); - static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count); + static uint32_t bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count); + static uint32_t bdshot_decode_telemetry_packet_f1(dmar_uint_t* buffer, uint32_t count, bool reversed); bool bdshot_decode_telemetry_from_erpm(uint16_t erpm, uint8_t chan); bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan); static uint8_t bdshot_find_next_ic_channel(const pwm_group& group); @@ -681,8 +722,11 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput bool bdshot_setup_group_ic_DMA(pwm_group &group); void bdshot_prepare_for_next_pulse(pwm_group& group); static void bdshot_receive_pulses_DMAR(pwm_group* group); - static void bdshot_reset_pwm(pwm_group& group); + static void bdshot_receive_pulses_DMAR_f1(pwm_group* group); + static void bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel); + static void bdshot_reset_pwm_f1(pwm_group& group, uint8_t telem_channel); static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch); + static void bdshot_config_icu_dshot_f1(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch); static uint32_t bdshot_get_output_rate_hz(const enum output_mode mode); /* diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index 4a654c9610bb1d..d8fd73fc8f7529 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -21,39 +21,59 @@ #include "hwdef/common/stm32_util.h" #include #include +#include -#ifdef HAL_WITH_BIDIR_DSHOT +#if HAL_WITH_IO_MCU +#include +extern AP_IOMCU iomcu; +#endif + +#if defined(IOMCU_FW) +#undef INTERNAL_ERROR +#define INTERNAL_ERROR(x) do {} while (0) +#endif using namespace ChibiOS; extern const AP_HAL::HAL& hal; -#if RCOU_DSHOT_TIMING_DEBUG -#define DEBUG_CHANNEL 1 -#define TOGGLE_PIN_CH_DEBUG(pin, channel) do { if (channel == DEBUG_CHANNEL) palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0) -#else -#define TOGGLE_PIN_CH_DEBUG(pin, channel) do {} while (0) -#endif - -#define TELEM_IC_SAMPLE 16 - +#if HAL_USE_PWM /* * enable bi-directional telemetry request for a mask of channels. This is used * with DShot to get telemetry feedback */ void RCOutput::set_bidir_dshot_mask(uint32_t mask) { - _bdshot.mask = (mask >> chan_offset); +#if HAL_WITH_IO_MCU_BIDIR_DSHOT + const uint32_t iomcu_mask = ((1U<> chan_offset) & ~_bdshot.disabled_mask; + _bdshot.mask = local_mask; // we now need to reconfigure the DMA channels since they are affected by the value of the mask for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; - if (((group.ch_mask << chan_offset) & mask) == 0) { + if ((group.ch_mask & local_mask) == 0) { // this group is not affected continue; } set_group_mode(group); } +#endif } +#endif // HAL_USE_PWM + +#ifdef HAL_WITH_BIDIR_DSHOT + +#if RCOU_DSHOT_TIMING_DEBUG +#define DEBUG_CHANNEL 1 +#define TOGGLE_PIN_CH_DEBUG(pin, channel) do { if (channel == DEBUG_CHANNEL) palToggleLine(HAL_GPIO_LINE_GPIO ## pin); } while (0) +#else +#define TOGGLE_PIN_CH_DEBUG(pin, channel) do {} while (0) +#endif bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) { @@ -75,6 +95,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) // Return error return false; } + if (!group.bdshot.ic_dma_handle[i]) { // share up channel if required if (group.dma_ch[i].stream_id == group.dma_up_stream_id) { @@ -85,7 +106,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *)); } if (!group.bdshot.ic_dma_handle[i]) { - return false; + goto ic_dma_fail; } } } @@ -98,6 +119,11 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) if (group.chan[i] != CHAN_DISABLED && _bdshot.mask & group.ch_mask) { // bi-directional dshot requires less than MID2 speed and PUSHPULL in order to avoid noise on the line // when switching from output to input +#if defined(STM32F1) + // on F103 the line mode has to be managed manually + // PAL_MODE_STM32_ALTERNATE_PUSHPULL is 50Mhz, similar to the medieum speed on other MCUs + palSetLineMode(group.pal_lines[i], PAL_MODE_STM32_ALTERNATE_PUSHPULL); +#else palSetLineMode(group.pal_lines[i], PAL_MODE_ALTERNATE(group.alt_functions[i]) | PAL_STM32_OTYPE_PUSHPULL | PAL_STM32_PUPDR_PULLUP | #ifdef PAL_STM32_OSPEED_MID1 @@ -108,6 +134,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) #error "Cannot set bdshot line speed" #endif ); +#endif } if (!group.is_chan_enabled(i) || !(_bdshot.mask & (1 << group.chan[i]))) { @@ -127,11 +154,11 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) if (!group.dma_ch[curr_chan].have_dma) { // We can't find a DMA channel to use so // return error - return false; + goto ic_dma_fail; } if (group.bdshot.ic_dma_handle[i]) { INTERNAL_ERROR(AP_InternalError::error_t::dma_fail); - return false; + goto ic_dma_fail; } // share up channel if required if (group.dma_ch[curr_chan].stream_id == group.dma_up_stream_id) { @@ -143,7 +170,7 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) FUNCTOR_BIND_MEMBER(&RCOutput::bdshot_ic_dma_deallocate, void, Shared_DMA *)); } if (!group.bdshot.ic_dma_handle[i]) { - return false; + goto ic_dma_fail; } group.bdshot.telem_tim_ch[i] = curr_chan; group.dma_ch[i] = group.dma_ch[curr_chan]; @@ -159,6 +186,15 @@ bool RCOutput::bdshot_setup_group_ic_DMA(pwm_group &group) } return true; + +ic_dma_fail: + for (uint8_t i = 0; i < 4; i++) { + if (group.bdshot.ic_dma_handle[i] != group.dma_handle) { + delete group.bdshot.ic_dma_handle[i]; + } + group.bdshot.ic_dma_handle[i] = nullptr; + } + return false; } /* @@ -172,12 +208,12 @@ void RCOutput::bdshot_ic_dma_allocate(Shared_DMA *ctx) if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] == nullptr) { chSysLock(); group.bdshot.ic_dma[icuch] = dmaStreamAllocI(group.dma_ch[icuch].stream_id, 10, bdshot_dma_ic_irq_callback, &group); - chSysUnlock(); #if STM32_DMA_SUPPORTS_DMAMUX if (group.bdshot.ic_dma[icuch]) { dmaSetRequestSource(group.bdshot.ic_dma[icuch], group.dma_ch[icuch].channel); } #endif + chSysUnlock(); } } } @@ -191,12 +227,12 @@ void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx) for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { pwm_group &group = pwm_group_list[i]; for (uint8_t icuch = 0; icuch < 4; icuch++) { + chSysLock(); if (group.bdshot.ic_dma_handle[icuch] == ctx && group.bdshot.ic_dma[icuch] != nullptr) { - chSysLock(); dmaStreamFreeI(group.bdshot.ic_dma[icuch]); group.bdshot.ic_dma[icuch] = nullptr; - chSysUnlock(); } + chSysUnlock(); } } } @@ -204,7 +240,7 @@ void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx) // setup bdshot for sending and receiving the next pulse void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group) { - // assume that we won't be able to get the input capture lock + // assume that we won't be able to get the input capture lock group.bdshot.enabled = false; uint32_t active_channels = group.ch_mask & group.en_mask; @@ -214,8 +250,14 @@ void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group) // no locking required group.bdshot.enabled = true; } else { - osalDbgAssert(!group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->is_locked(), "DMA handle is already locked"); - group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]->lock(); + osalDbgAssert(!group.bdshot.curr_ic_dma_handle, "IC DMA handle has not been released"); + group.bdshot.curr_ic_dma_handle = group.bdshot.ic_dma_handle[group.bdshot.curr_telem_chan]; +#ifdef HAL_TIM_UP_SHARED + osalDbgAssert(group.shared_up_dma || !group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked"); +#else + osalDbgAssert(!group.bdshot.curr_ic_dma_handle->is_locked(), "IC DMA handle is already locked"); +#endif + group.bdshot.curr_ic_dma_handle->lock(); group.bdshot.enabled = true; } } @@ -236,31 +278,42 @@ void RCOutput::bdshot_prepare_for_next_pulse(pwm_group& group) _bdshot.erpm_errors[chan] = 0; _bdshot.erpm_last_stats_ms[chan] = now; } + } else if (group.dshot_state == DshotState::RECV_FAILED) { + _bdshot.erpm_errors[group.bdshot.curr_telem_chan]++; } if (group.bdshot.enabled) { if (group.pwm_started) { - bdshot_reset_pwm(group); - } else { + bdshot_reset_pwm(group, group.bdshot.prev_telem_chan); + } + else { pwmStart(group.pwm_drv, &group.pwm_cfg); + group.pwm_started = true; } - group.pwm_started = true; // we can be more precise for capture timer group.bdshot.telempsc = (uint16_t)(lrintf(((float)group.pwm_drv->clock / bdshot_get_output_rate_hz(group.current_mode) + 0.01f)/TELEM_IC_SAMPLE) - 1); } } -void RCOutput::bdshot_reset_pwm(pwm_group& group) +// reset pwm driver to output mode without resetting the clock or the peripheral +// the code here is the equivalent of pwmStart()/pwmStop() +void RCOutput::bdshot_reset_pwm(pwm_group& group, uint8_t telem_channel) { +#if defined(STM32F1) + bdshot_reset_pwm_f1(group, telem_channel); +#else + // on more capable MCUs we can do something very simple pwmStop(group.pwm_drv); pwmStart(group.pwm_drv, &group.pwm_cfg); +#endif } // see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 // called from the interrupt #pragma GCC push_options #pragma GCC optimize("O2") +#if !defined(STM32F1) void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) { // make sure the transaction finishes or times out, this function takes a little time to run so the most @@ -268,13 +321,12 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) // should be plenty chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 10U), bdshot_finish_dshot_gcr_transaction, group); - uint8_t curr_ch = group->bdshot.curr_telem_chan; group->pwm_drv->tim->CR1 = 0; - group->pwm_drv->tim->CCER = 0; // Configure Timer group->pwm_drv->tim->SR = 0; + group->pwm_drv->tim->CCER = 0; group->pwm_drv->tim->CCMR1 = 0; group->pwm_drv->tim->CCMR2 = 0; group->pwm_drv->tim->DIER = 0; @@ -286,6 +338,7 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) //TOGGLE_PIN_CH_DEBUG(54, curr_ch); group->pwm_drv->tim->ARR = 0xFFFF; // count forever group->pwm_drv->tim->CNT = 0; + uint8_t curr_ch = group->bdshot.curr_telem_chan; // Initialise ICU channels bdshot_config_icu_dshot(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]); @@ -308,7 +361,9 @@ void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group) #endif dmaStreamSetMode(ic_dma, STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel) | - STM32_DMA_CR_DIR_P2M | STM32_DMA_CR_PSIZE_WORD | STM32_DMA_CR_MSIZE_WORD | + STM32_DMA_CR_DIR_P2M | + STM32_DMA_CR_PSIZE_WORD | + STM32_DMA_CR_MSIZE_WORD | STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) | STM32_DMA_CR_TEIE | STM32_DMA_CR_TCIE); @@ -424,6 +479,7 @@ void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t break; } } +#endif // !defined(STM32F1) /* unlock DMA channel after a bi-directional dshot transaction completes @@ -441,15 +497,23 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t* // or the input channel buffer const stm32_dma_stream_t *dma = group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_telem_chan]; + osalDbgAssert(dma, "No DMA channel"); // record the transaction size before the stream is released dmaStreamDisable(dma); group->bdshot.dma_tx_size = MIN(uint16_t(GCR_TELEMETRY_BIT_LEN), GCR_TELEMETRY_BIT_LEN - dmaStreamGetTransactionSize(dma)); - stm32_cacheBufferInvalidate(group->dma_buffer, group->bdshot.dma_tx_size); - memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(uint32_t) * group->bdshot.dma_tx_size); - group->dshot_state = DshotState::RECV_COMPLETE; + stm32_cacheBufferInvalidate(group->dma_buffer, group->bdshot.dma_tx_size); + memcpy(group->bdshot.dma_buffer_copy, group->dma_buffer, sizeof(dmar_uint_t) * group->bdshot.dma_tx_size); +#ifdef HAL_TIM_UP_SHARED + // although it should be possible to start the next DMAR transaction concurrently with receiving + // telemetry, in practice it seems to interfere with the DMA engine + if (group->shared_up_dma && group->bdshot.enabled) { + // next dshot pulse can go out now + chEvtSignalI(group->dshot_waiter, DSHOT_CASCADE); + } +#endif // if using input capture DMA and sharing the UP and CH channels then clean up // by assigning the source back to UP #if STM32_DMA_SUPPORTS_DMAMUX @@ -458,9 +522,19 @@ __RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t* } #endif - // rotate to the next input channel group->bdshot.prev_telem_chan = group->bdshot.curr_telem_chan; + // rotate to the next input channel, we have to rotate even on failure otherwise + // we will not get data from active channels group->bdshot.curr_telem_chan = bdshot_find_next_ic_channel(*group); + + // dshot commands are issued without a response coming back, this allows + // us to handle the next packet correctly without it looking like a failure + if (group->bdshot.dma_tx_size > 0) { + group->dshot_state = DshotState::RECV_COMPLETE; + } else { + group->dshot_state = DshotState::RECV_FAILED; + } + // tell the waiting process we've done the DMA chEvtSignalI(group->dshot_waiter, group->dshot_event_mask); #ifdef HAL_GPIO_LINE_GPIO56 @@ -479,7 +553,12 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan) } // evaluate dshot telemetry +#if defined(STM32F1) + const bool reversed = (group.bdshot.telem_tim_ch[chan] & 1U) == 0; + group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet_f1(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size, reversed); +#else group.bdshot.erpm[chan] = bdshot_decode_telemetry_packet(group.bdshot.dma_buffer_copy, group.bdshot.dma_tx_size); +#endif group.dshot_state = DshotState::IDLE; @@ -496,7 +575,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan) TOGGLE_PIN_DEBUG(57); #endif } - +#if !defined(IOMCU_FW) uint64_t now = AP_HAL::micros64(); if (chan == DEBUG_CHANNEL && (now - group.bdshot.last_print) > 1000000) { hal.console->printf("TELEM: %d <%d Hz, %.1f%% err>", group.bdshot.erpm[chan], group.bdshot.telem_rate[chan], @@ -511,6 +590,7 @@ bool RCOutput::bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan) group.bdshot.telem_err_rate[chan] = 0; group.bdshot.last_print = now; } +#endif #endif return group.bdshot.erpm[chan] != 0xFFFF; } @@ -549,13 +629,19 @@ __RAMFUNC__ void RCOutput::dma_up_irq_callback(void *p, uint32_t flags) } dmaStreamDisable(group->dma); - if (group->in_serial_dma && irq.waiter) { + if (soft_serial_waiting()) { +#if HAL_SERIAL_ESC_COMM_ENABLED // tell the waiting process we've done the DMA chEvtSignalI(irq.waiter, serial_event_mask); +#endif } else if (!group->in_serial_dma && group->bdshot.enabled) { group->dshot_state = DshotState::SEND_COMPLETE; // sending is done, in 30us the ESC will send telemetry +#if defined(STM32F1) + bdshot_receive_pulses_DMAR_f1(group); +#else bdshot_receive_pulses_DMAR(group); +#endif } else { // non-bidir case, this prevents us ever having two dshot pulses too close together if (is_dshot_protocol(group->current_mode)) { @@ -604,20 +690,19 @@ uint32_t RCOutput::bdshot_get_output_rate_hz(const enum output_mode mode) } } -#define INVALID_ERPM 0xffffU - // decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket // see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol -uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count) +uint32_t RCOutput::bdshot_decode_telemetry_packet(dmar_uint_t* buffer, uint32_t count) { uint32_t value = 0; - uint32_t oldValue = buffer[0]; uint32_t bits = 0; uint32_t len; + dmar_uint_t oldValue = buffer[0]; + for (uint32_t i = 1; i <= count; i++) { if (i < count) { - int32_t diff = buffer[i] - oldValue; + dmar_int_t diff = buffer[i] - oldValue; if (bits >= 21U) { break; } @@ -631,6 +716,7 @@ uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t cou oldValue = buffer[i]; bits += len; } + if (bits != 21U) { return INVALID_ERPM; } @@ -665,8 +751,16 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c } // eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry) - uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU; - uint16_t value = (encodederpm & 0x000001ffU); + uint8_t expo = ((encodederpm & 0xfffffe00U) >> 9U) & 0xffU; // 3bits + uint16_t value = (encodederpm & 0x000001ffU); // 9bits +#if HAL_WITH_ESC_TELEM + uint8_t normalized_chan = chan; +#endif +#if HAL_WITH_IO_MCU + if (iomcu_dshot) { + normalized_chan = chan + chan_offset; + } +#endif if (!(value & 0x100U) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) { switch (expo) { @@ -675,7 +769,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c TelemetryData t { .temperature_cdeg = int16_t(value * 100) }; - update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); #endif return false; } @@ -685,7 +779,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c TelemetryData t { .voltage = 0.25f * value }; - update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE); + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE); #endif return false; } @@ -695,7 +789,7 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c TelemetryData t { .current = float(value) }; - update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT); + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT); #endif return false; } @@ -719,18 +813,28 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c erpm = (1000000U * 60U / 100U + erpm / 2U) / erpm; - if (encodederpm == 0x0fff) { // the special 0 encoding + if (encodederpm == ZERO_ERPM) { // the special 0 encoding erpm = 0; } // update the ESC telemetry data if (erpm < INVALID_ERPM) { _bdshot.erpm[chan] = erpm; + _bdshot.update_mask |= 1U< 0) { _dshot_cycle = (_dshot_cycle + 1) % _dshot_rate; @@ -98,6 +134,294 @@ void RCOutput::rcout_thread() { } } +#if defined(HAL_WITH_BIDIR_DSHOT) && defined(STM32F1) +// reset pwm driver to output mode without resetting the clock or the peripheral +// the code here is the equivalent of pwmStart()/pwmStop() +void RCOutput::bdshot_reset_pwm_f1(pwm_group& group, uint8_t telem_channel) +{ + osalSysLock(); + + stm32_tim_t* TIMx = group.pwm_drv->tim; + // pwmStop sets these + TIMx->CR1 = 0; /* Timer disabled. */ + TIMx->DIER = 0; /* All IRQs disabled. */ + TIMx->SR = 0; /* Clear eventual pending IRQs. */ + TIMx->CNT = 0; + TIMx->CCR[0] = 0; /* Comparator 1 disabled. */ + TIMx->CCR[1] = 0; /* Comparator 2 disabled. */ + TIMx->CCR[2] = 0; /* Comparator 3 disabled. */ + TIMx->CCR[3] = 0; /* Comparator 4 disabled. */ + // at the point this is called we will have done input capture on two CC channels + // we need to switch those channels back to output and the default settings + // all other channels will not have been modified + switch (group.bdshot.telem_tim_ch[telem_channel]) { + case 0: // CC1 + case 1: // CC2 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC2E | TIM_CCER_CC1E, 0); // disable CC so that it can be modified + MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC), + STM32_TIM_CCMR1_OC1M(6) | STM32_TIM_CCMR1_OC1PE); + MODIFY_REG(TIMx->CCMR1, (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC), + STM32_TIM_CCMR1_OC2M(6) | STM32_TIM_CCMR1_OC2PE); + MODIFY_REG(TIMx->CCER, (TIM_CCER_CC1P | TIM_CCER_CC2P), + (TIM_CCER_CC1P | TIM_CCER_CC2P | TIM_CCER_CC1E | TIM_CCER_CC2E)); + break; + case 2: // CC3 + case 3: // CC4 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3E | TIM_CCER_CC4E, 0); // disable CC so that it can be modified + MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC), + STM32_TIM_CCMR2_OC3M(6) | STM32_TIM_CCMR2_OC3PE); + MODIFY_REG(TIMx->CCMR2, (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC), + STM32_TIM_CCMR2_OC4M(6) | STM32_TIM_CCMR2_OC4PE); + MODIFY_REG(TIMx->CCER, (TIM_CCER_CC3P | TIM_CCER_CC4P), + (TIM_CCER_CC3P | TIM_CCER_CC4P | TIM_CCER_CC3E | TIM_CCER_CC4E)); + break; + default: + break; + } + // pwmStart sets these + uint32_t psc = (group.pwm_drv->clock / group.pwm_drv->config->frequency) - 1; + TIMx->PSC = psc; + TIMx->ARR = group.pwm_drv->period - 1; + TIMx->CR2 = group.pwm_drv->config->cr2; + TIMx->EGR = STM32_TIM_EGR_UG; /* Update event. */ + TIMx->SR = 0; /* Clear pending IRQs. */ + TIMx->DIER = group.pwm_drv->config->dier & /* DMA-related DIER settings. */ + ~STM32_TIM_DIER_IRQ_MASK; + if (group.pwm_drv->has_bdtr) { + TIMx->BDTR = group.pwm_drv->config->bdtr | STM32_TIM_BDTR_MOE; + } + + // we need to switch every output on the same input channel to avoid + // spurious line changes + for (uint8_t i = 0; i<4; i++) { + if (group.chan[i] == CHAN_DISABLED) { + continue; + } + if (group.bdshot.telem_tim_ch[telem_channel] == group.bdshot.telem_tim_ch[i]) { + palSetLineMode(group.pal_lines[i], PAL_MODE_STM32_ALTERNATE_PUSHPULL); + } + } + + /* Timer configured and started.*/ + TIMx->CR1 = STM32_TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_CEN; + + osalSysUnlock(); +} + +// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 +// called from the interrupt +void RCOutput::bdshot_receive_pulses_DMAR_f1(pwm_group* group) +{ + // make sure the transaction finishes or times out, this function takes a little time to run so the most + // accurate timing is from the beginning. the pulse time is slightly longer than we need so an extra 10U + // should be plenty + chVTSetI(&group->dma_timeout, chTimeUS2I(group->dshot_pulse_send_time_us + 30U + 10U), + bdshot_finish_dshot_gcr_transaction, group); + + group->pwm_drv->tim->CR1 = 0; + + // Configure Timer + group->pwm_drv->tim->SR = 0; + // do NOT set CCER to 0 here - this pulls the line low on F103 (at least) + // and since we are already doing bdshot the relevant options that are set for output + // also apply to input and bdshot_config_icu_dshot() will disable any channels that need + // disabling + group->pwm_drv->tim->DIER = 0; + group->pwm_drv->tim->CR2 = 0; + group->pwm_drv->tim->PSC = group->bdshot.telempsc; + + group->dshot_state = DshotState::RECV_START; + + //TOGGLE_PIN_CH_DEBUG(54, curr_ch); + group->pwm_drv->tim->ARR = 0xFFFF; // count forever + group->pwm_drv->tim->CNT = 0; + uint8_t curr_ch = group->bdshot.curr_telem_chan; + + // we need to switch every input on the same input channel to allow + // the ESCs to drive the lines + for (uint8_t i = 0; i<4; i++) { + if (group->chan[i] == CHAN_DISABLED) { + continue; + } + if (group->bdshot.telem_tim_ch[curr_ch] == group->bdshot.telem_tim_ch[i]) { + palSetLineMode(group->pal_lines[i], PAL_MODE_INPUT_PULLUP); + } + } + + // Initialise ICU channels + bdshot_config_icu_dshot_f1(group->pwm_drv->tim, curr_ch, group->bdshot.telem_tim_ch[curr_ch]); + + const stm32_dma_stream_t *ic_dma = + group->has_shared_ic_up_dma() ? group->dma : group->bdshot.ic_dma[curr_ch]; + + // Configure DMA + dmaStreamSetPeripheral(ic_dma, &(group->pwm_drv->tim->DMAR)); + dmaStreamSetMemory0(ic_dma, group->dma_buffer); + dmaStreamSetTransactionSize(ic_dma, GCR_TELEMETRY_BIT_LEN); + dmaStreamSetMode(ic_dma, + STM32_DMA_CR_CHSEL(group->dma_ch[curr_ch].channel) | + STM32_DMA_CR_DIR_P2M | + STM32_DMA_CR_PSIZE_HWORD | + STM32_DMA_CR_MSIZE_HWORD | + STM32_DMA_CR_MINC | STM32_DMA_CR_PL(3) | + STM32_DMA_CR_TEIE | STM32_DMA_CR_TCIE); + + // setup for transfers. 0x0D is the register + // address offset of the CCR registers in the timer peripheral + uint8_t telem_ch_pair = group->bdshot.telem_tim_ch[curr_ch] & ~1U; // round to the lowest of the channel pair + const uint8_t ccr_ofs = offsetof(stm32_tim_t, CCR)/4 + telem_ch_pair; + group->pwm_drv->tim->DCR = STM32_TIM_DCR_DBA(ccr_ofs) | STM32_TIM_DCR_DBL(1); // read two registers at a time + + // Start Timer + group->pwm_drv->tim->EGR |= STM32_TIM_EGR_UG; + group->pwm_drv->tim->SR = 0; + group->pwm_drv->tim->CR1 = TIM_CR1_ARPE | STM32_TIM_CR1_URS | STM32_TIM_CR1_UDIS | STM32_TIM_CR1_CEN; + dmaStreamEnable(ic_dma); +} + +void RCOutput::bdshot_config_icu_dshot_f1(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch) +{ + // F103 does not support both edges input capture so we need to set up two channels + // both pointing at the same input to capture the data. The triggered channel + // needs to handle the second edge - so rising or falling - so that we get an + // even number of half-words in the DMA buffer + switch(ccr_ch) { + case 0: + case 1: { + // Disable the IC1 and IC2: Reset the CCxE Bit + MODIFY_REG(TIMx->CCER, TIM_CCER_CC1E | TIM_CCER_CC2E, 0); + // Select the Input and set the filter and the prescaler value + if (chan == 0) { // TI1 + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC), + (TIM_CCMR1_CC1S_0 | TIM_CCMR1_IC1F_1));// 4 samples per output transition + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC), + (TIM_CCMR1_CC2S_1 | TIM_CCMR1_IC2F_1)); + } else { // TI2 + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC1S | TIM_CCMR1_IC1F | TIM_CCMR1_IC1PSC), + (TIM_CCMR1_CC1S_1 | TIM_CCMR1_IC1F_1)); + MODIFY_REG(TIMx->CCMR1, + (TIM_CCMR1_CC2S | TIM_CCMR1_IC2F | TIM_CCMR1_IC2PSC), + (TIM_CCMR1_CC2S_0 | TIM_CCMR1_IC2F_1)); + } + if (ccr_ch == 0) { + // Select the Polarity as falling on IC2 and rising on IC1 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC1P | TIM_CCER_CC2P, TIM_CCER_CC2P | TIM_CCER_CC1E | TIM_CCER_CC2E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE | TIM_DIER_CC2DE, TIM_DIER_CC1DE); + } else { + // Select the Polarity as falling on IC1 and rising on IC2 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC1P | TIM_CCER_CC2P, TIM_CCER_CC1P | TIM_CCER_CC1E | TIM_CCER_CC2E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC1DE | TIM_DIER_CC2DE, TIM_DIER_CC2DE); + } + break; + } + case 2: + case 3: { + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3E | TIM_CCER_CC4E, 0); + // Select the Input and set the filter and the prescaler value + if (chan == 2) { // TI3 + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC), + (TIM_CCMR2_CC3S_0 | TIM_CCMR2_IC3F_1)); + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC), + (TIM_CCMR2_CC4S_1 | TIM_CCMR2_IC4F_1)); + } else { // TI4 + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC3S | TIM_CCMR2_IC3F | TIM_CCMR2_IC3PSC), + (TIM_CCMR2_CC3S_1 | TIM_CCMR2_IC3F_1)); + MODIFY_REG(TIMx->CCMR2, + (TIM_CCMR2_CC4S | TIM_CCMR2_IC4F | TIM_CCMR2_IC4PSC), + (TIM_CCMR2_CC4S_0 | TIM_CCMR2_IC4F_1)); + } + if (ccr_ch == 2) { + // Select the Polarity as falling on IC4 and rising on IC3 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3P | TIM_CCER_CC4P, TIM_CCER_CC4P | TIM_CCER_CC3E | TIM_CCER_CC4E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE | TIM_DIER_CC4DE, TIM_DIER_CC3DE); + } else { + // Select the Polarity as falling on IC3 and rising on IC4 + MODIFY_REG(TIMx->CCER, TIM_CCER_CC3P | TIM_CCER_CC4P, TIM_CCER_CC3P | TIM_CCER_CC3E | TIM_CCER_CC4E); + MODIFY_REG(TIMx->DIER, TIM_DIER_CC3DE | TIM_DIER_CC4DE, TIM_DIER_CC4DE); + } + break; + + } + default: + break; + } +} + +// decode a telemetry packet from a GCR encoded stride buffer, take from betaflight decodeTelemetryPacket +// see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625 for a description of the protocol +uint32_t RCOutput::bdshot_decode_telemetry_packet_f1(dmar_uint_t* buffer, uint32_t count, bool reversed) +{ + if (!reversed) { + return bdshot_decode_telemetry_packet(buffer, count); + } + + uint32_t value = 0; + uint32_t bits = 0; + uint32_t len; + + // on F103 we are reading one edge with ICn and the other with ICn+1, the DMA architecture only + // allows to trigger on a single register dictated by the DMA input capture channel being used. + // even though we are reading multiple registers per transfer we always cannot trigger on one or other + // of the registers and if the one we trigger on is the one that is numerically first each register + // pair that we read will be swapped in time. in this case we trigger on ICn and then read CCRn and CCRn+1 + // giving us the new value of ICn and the old value of ICn+1. in order to avoid reading garbage on the + // first read we trigger ICn on the rising edge. this gives us all the data but with each pair of bytes + // transposed. we thus need to untranspose as we decode + dmar_uint_t oldValue = buffer[1]; + + for (int32_t i = 0; i <= count+1; ) { + if (i < count) { + dmar_int_t diff = buffer[i] - oldValue; + if (bits >= 21U) { + break; + } + len = (diff + TELEM_IC_SAMPLE/2U) / TELEM_IC_SAMPLE; + } else { + len = 21U - bits; + } + + value <<= len; + value |= 1U << (len - 1U); + oldValue = buffer[i]; + bits += len; + + i += (i%2 ? -1 : 3); + } + + + if (bits != 21U) { + return INVALID_ERPM; + } + + static const uint32_t decode[32] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15, + 0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 }; + + uint32_t decodedValue = decode[value & 0x1fU]; + decodedValue |= decode[(value >> 5U) & 0x1fU] << 4U; + decodedValue |= decode[(value >> 10U) & 0x1fU] << 8U; + decodedValue |= decode[(value >> 15U) & 0x1fU] << 12U; + + uint32_t csum = decodedValue; + csum = csum ^ (csum >> 8U); // xor bytes + csum = csum ^ (csum >> 4U); // xor nibbles + + if ((csum & 0xfU) != 0xfU) { + return INVALID_ERPM; + } + decodedValue >>= 4; + + return decodedValue; +} + +#endif // HAL_WITH_BIDIR_DSHOT && STM32F1 + #endif // HAL_USE_PWM #endif // IOMCU_FW && HAL_DSHOT_ENABLED diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp index db368a9cbfbb1a..dc6824256a1322 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp @@ -39,7 +39,7 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha return false; } - if (soft_serial_waiting() || (group.dshot_state != DshotState::IDLE && group.dshot_state != DshotState::RECV_COMPLETE)) { + if (soft_serial_waiting() || !is_dshot_send_allowed(group.dshot_state)) { // doing serial output or DMAR input, don't send DShot pulses return false; } @@ -57,18 +57,8 @@ bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t cha group.dshot_waiter = rcout_thread_ctx; bool bdshot_telem = false; #ifdef HAL_WITH_BIDIR_DSHOT - uint32_t active_channels = group.ch_mask & group.en_mask; - // no need to get the input capture lock - group.bdshot.enabled = false; - if ((_bdshot.mask & active_channels) == active_channels) { - bdshot_telem = true; - // it's not clear why this is required, but without it we get no output - if (group.pwm_started) { - pwmStop(group.pwm_drv); - } - pwmStart(group.pwm_drv, &group.pwm_cfg); - group.pwm_started = true; - } + bdshot_prepare_for_next_pulse(group); + bdshot_telem = group.bdshot.enabled; #endif memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH); @@ -110,7 +100,7 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman // not an FMU channel if (chan < chan_offset || chan == ALL_CHANNELS) { #if HAL_WITH_IO_MCU - if (AP_BoardConfig::io_dshot()) { + if (iomcu_dshot) { iomcu.send_dshot_command(command, chan, command_timeout_ms, repeat_count, priority); } #endif @@ -121,7 +111,11 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman DshotCommandPacket pkt; pkt.command = command; - pkt.chan = chan - chan_offset; + if (chan != ALL_CHANNELS) { + pkt.chan = chan - chan_offset; + } else { + pkt.chan = ALL_CHANNELS; + } if (command_timeout_ms == 0) { pkt.cycle = MAX(10, repeat_count); } else { @@ -157,7 +151,7 @@ void RCOutput::update_channel_masks() { } #if HAL_PWM_COUNT > 0 - for (uint8_t i=0; i #include "Semaphores.h" #include +#include #include "AP_HAL_ChibiOS.h" +#include #if CH_CFG_USE_MUTEXES == TRUE @@ -51,6 +53,9 @@ bool Semaphore::take(uint32_t timeout_ms) if (take_nonblocking()) { return true; } +#ifdef HAL_BOOTLOADER_BUILD + return false; +#else uint64_t start = AP_HAL::micros64(); do { hal.scheduler->delay_microseconds(200); @@ -59,6 +64,7 @@ bool Semaphore::take(uint32_t timeout_ms) } } while ((AP_HAL::micros64() - start) < timeout_ms*1000); return false; +#endif } bool Semaphore::take_nonblocking() @@ -78,4 +84,57 @@ void Semaphore::assert_owner(void) osalDbgAssert(check_owner(), "owner"); } +#if CH_CFG_USE_SEMAPHORES == TRUE +BinarySemaphore::BinarySemaphore(bool initial_state) : + AP_HAL::BinarySemaphore(initial_state) +{ + static_assert(sizeof(_lock) >= sizeof(semaphore_t), "invalid semaphore_t size"); + auto *sem = (binary_semaphore_t *)_lock; + /* + the initial_state in ChibiOS binary semaphores is 'taken', so we + need to negate it for the ArduPilot semantics + */ + chBSemObjectInit(sem, !initial_state); +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + auto *sem = (binary_semaphore_t *)_lock; + if (timeout_us == 0) { + return chBSemWaitTimeout(sem, TIME_IMMEDIATE) == MSG_OK; + } + // loop waiting for 60ms at a time. This ensures we can wait for + // any amount of time even on systems with a 16 bit timer + while (timeout_us > 0) { + const uint32_t us = MIN(timeout_us, 60000U); + if (chBSemWaitTimeout(sem, TIME_US2I(us)) == MSG_OK) { + return true; + } + timeout_us -= us; + } + return false; +} + +bool BinarySemaphore::wait_blocking(void) +{ + auto *sem = (binary_semaphore_t *)_lock; + return chBSemWait(sem) == MSG_OK; +} + +void BinarySemaphore::signal(void) +{ + auto *sem = (binary_semaphore_t *)_lock; + chBSemSignal(sem); +} + +void BinarySemaphore::signal_ISR(void) +{ + auto *sem = (binary_semaphore_t *)_lock; + chSysLockFromISR(); + chBSemSignalI(sem); + chSysUnlockFromISR(); +} + +#endif // CH_CFG_USE_SEMAPHORES == TRUE + #endif // CH_CFG_USE_MUTEXES diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.h b/libraries/AP_HAL_ChibiOS/Semaphores.h index 484570d0869da4..49f258366bcfff 100644 --- a/libraries/AP_HAL_ChibiOS/Semaphores.h +++ b/libraries/AP_HAL_ChibiOS/Semaphores.h @@ -37,3 +37,21 @@ class ChibiOS::Semaphore : public AP_HAL::Semaphore { // we declare the lock as a uint32_t array, and cast inside the cpp file uint32_t _lock[5]; }; + +/* + BinarySemaphore implementation + */ +class ChibiOS::BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + + CLASS_NO_COPY(BinarySemaphore); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + void signal_ISR(void) override; + +protected: + uint32_t _lock[5]; +}; diff --git a/libraries/AP_HAL_ChibiOS/Storage.cpp b/libraries/AP_HAL_ChibiOS/Storage.cpp index 56bc9d822d24f1..e4ee42cc4ddb42 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.cpp +++ b/libraries/AP_HAL_ChibiOS/Storage.cpp @@ -330,7 +330,11 @@ void Storage::_flash_load(void) #ifdef STORAGE_FLASH_PAGE _flash_page = STORAGE_FLASH_PAGE; +#if AP_FLASH_STORAGE_DOUBLE_PAGE + ::printf("Storage: Using flash pages %u to %u\n", _flash_page, _flash_page+3); +#else ::printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1); +#endif if (!_flash.init()) { AP_HAL::panic("Unable to init flash storage"); @@ -359,6 +363,9 @@ bool Storage::_flash_write(uint16_t line) bool Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length) { #ifdef STORAGE_FLASH_PAGE +#if AP_FLASH_STORAGE_DOUBLE_PAGE + sector *= 2; +#endif size_t base_address = hal.flash->getpageaddr(_flash_page+sector); for (uint8_t i=0; igetpageaddr(_flash_page+sector); const uint8_t *b = ((const uint8_t *)base_address)+offset; memcpy(data, b, length); @@ -405,6 +415,9 @@ bool Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, u bool Storage::_flash_erase_sector(uint8_t sector) { #ifdef STORAGE_FLASH_PAGE +#if AP_FLASH_STORAGE_DOUBLE_PAGE + sector *= 2; +#endif // erasing a page can take long enough that USB may not initialise properly if it happens // while the host is connecting. Only do a flash erase if we have been up for more than 4s for (uint8_t i=0; ierasepage(_flash_page+sector) && hal.flash->erasepage(_flash_page+sector+1)) { + return true; + } +#else if (hal.flash->erasepage(_flash_page+sector)) { return true; } +#endif hal.scheduler->delay(1); } return false; diff --git a/libraries/AP_HAL_ChibiOS/Storage.h b/libraries/AP_HAL_ChibiOS/Storage.h index 59266d19bb086c..80271fe3d80ed8 100644 --- a/libraries/AP_HAL_ChibiOS/Storage.h +++ b/libraries/AP_HAL_ChibiOS/Storage.h @@ -47,6 +47,13 @@ static_assert(CH_STORAGE_SIZE % CH_STORAGE_LINE_SIZE == 0, "Storage is not multiple of line size"); +/* + on boards with 8k sector sizes we double up to treat pairs of sectors as one + */ +#ifndef AP_FLASH_STORAGE_DOUBLE_PAGE +#define AP_FLASH_STORAGE_DOUBLE_PAGE 0 +#endif + class ChibiOS::Storage : public AP_HAL::Storage { public: void init() override {} @@ -86,7 +93,11 @@ class ChibiOS::Storage : public AP_HAL::Storage { #ifdef STORAGE_FLASH_PAGE AP_FlashStorage _flash{_buffer, +#if AP_FLASH_STORAGE_DOUBLE_PAGE + stm32_flash_getpagesize(STORAGE_FLASH_PAGE)*2, +#else stm32_flash_getpagesize(STORAGE_FLASH_PAGE), +#endif FUNCTOR_BIND_MEMBER(&Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t), FUNCTOR_BIND_MEMBER(&Storage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t), FUNCTOR_BIND_MEMBER(&Storage::_flash_erase_sector, bool, uint8_t), diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp index 911688f643e151..a04537fdbddafb 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp @@ -52,13 +52,13 @@ using namespace ChibiOS; extern ChibiOS::UARTDriver uart_io; #endif -const UARTDriver::SerialDef UARTDriver::_serial_tab[] = { HAL_UART_DEVICE_LIST }; +const UARTDriver::SerialDef UARTDriver::_serial_tab[] = { HAL_SERIAL_DEVICE_LIST }; // handle for UART handling thread thread_t* volatile UARTDriver::uart_rx_thread_ctx; // table to find UARTDrivers from serial number, used for event handling -UARTDriver *UARTDriver::uart_drivers[UART_MAX_DRIVERS]; +UARTDriver *UARTDriver::serial_drivers[UART_MAX_DRIVERS]; // event used to wake up waiting thread. This event number is for // caller threads @@ -104,8 +104,8 @@ serial_num(_serial_num), sdef(_serial_tab[_serial_num]), _baudrate(57600) { - osalDbgAssert(serial_num < UART_MAX_DRIVERS, "too many UART drivers"); - uart_drivers[serial_num] = this; + osalDbgAssert(serial_num < UART_MAX_DRIVERS, "too many SERIALn drivers"); + serial_drivers[serial_num] = this; } /* @@ -166,11 +166,11 @@ void UARTDriver::uart_rx_thread(void* arg) hal.scheduler->delay_microseconds(1000); for (uint8_t i=0; i_rx_initialised) { - uart_drivers[i]->_rx_timer_tick(); + if (serial_drivers[i]->_rx_initialised) { + serial_drivers[i]->_rx_timer_tick(); } } } @@ -430,6 +430,18 @@ void UARTDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) sercfg.cr2 = _cr2_options; sercfg.cr3 = _cr3_options; +#if defined(STM32H7) + /* + H7 defaults to 16x oversampling. To get the highest + possible baudrates we need to drop back to 8x + oversampling. The H7 UART clock is 100MHz. This allows + for up to 12.5MBps on H7 UARTs + */ + if (_baudrate > 100000000UL / 16U) { + sercfg.cr1 |= USART_CR1_OVER8; + } +#endif + #ifndef HAL_UART_NODMA if (rx_dma_enabled) { sercfg.cr1 |= USART_CR1_IDLEIE; diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h index 16cac8f8d0d2e1..1a045fa9e7e818 100644 --- a/libraries/AP_HAL_ChibiOS/UARTDriver.h +++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h @@ -25,7 +25,7 @@ #define RX_BOUNCE_BUFSIZE 64U #define TX_BOUNCE_BUFSIZE 64U -// enough for uartA to uartJ, plus IOMCU +// enough for serial0 to serial9, plus IOMCU #define UART_MAX_DRIVERS 11 class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { @@ -148,13 +148,13 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver { static thread_t* volatile uart_rx_thread_ctx; // table to find UARTDrivers from serial number, used for event handling - static UARTDriver *uart_drivers[UART_MAX_DRIVERS]; + static UARTDriver *serial_drivers[UART_MAX_DRIVERS]; // thread used for writing and reading thread_t* volatile uart_thread_ctx; char uart_thread_name[6]; - // index into uart_drivers table + // index into serial_drivers table uint8_t serial_num; uint32_t _baudrate; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 7d56981c5c4c39..6488aa2b043af5 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -37,7 +37,8 @@ #include #include #endif -#ifndef HAL_BOOTLOADER_BUILD +#include +#if HAL_LOGGING_ENABLED #include #endif @@ -76,6 +77,12 @@ void* Util::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) return malloc_dma(size); } else if (mem_type == AP_HAL::Util::MEM_FAST) { return malloc_fastmem(size); + } else if (mem_type == AP_HAL::Util::MEM_FILESYSTEM) { +#if defined(STM32H7) + return malloc_axi_sram(size); +#else + return malloc_dma(size); +#endif } else { return calloc(1, size); } @@ -220,7 +227,7 @@ void Util::toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t dur #endif // HAL_USE_PWM #if HAL_DSHOT_ALARM_ENABLED // don't play the motors while flying - if (!(_toneAlarm_types & AP_Notify::Notify_Buzz_DShot) || get_soft_armed() || hal.rcout->get_dshot_esc_type() != RCOutput::DSHOT_ESC_BLHELI) { + if (!(_toneAlarm_types & AP_Notify::Notify_Buzz_DShot) || get_soft_armed() || hal.rcout->get_dshot_esc_type() == RCOutput::DSHOT_ESC_NONE) { return; } @@ -261,6 +268,7 @@ uint64_t Util::get_hw_rtc() const #if AP_BOOTLOADER_FLASHING_ENABLED #if HAL_GCS_ENABLED +#include #define Debug(fmt, args ...) do { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } while (0) #endif // HAL_GCS_ENABLED @@ -755,7 +763,7 @@ bool Util::get_true_random_vals(uint8_t* data, size_t size, uint32_t timeout_us) */ void Util::log_stack_info(void) { -#if !defined(HAL_BOOTLOADER_BUILD) && HAL_LOGGING_ENABLED +#if HAL_LOGGING_ENABLED static thread_t *last_tp; static uint8_t thread_id; thread_t *tp = last_tp; diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index f0aecae053c953..5af64e1c7791bc 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -39,6 +39,11 @@ class ChibiOS::Util : public AP_HAL::Util { bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; } uint32_t available_memory() override; + // get path to custom defaults file for AP_Param + const char* get_custom_defaults_file() const override { + return "@ROMFS/defaults.parm"; + } + // Special Allocation Routines void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat index b9f1429dd39fab..f107450d24c8c9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/ARKV6X/hwdef.dat @@ -27,8 +27,11 @@ FLASH_SIZE_KB 2048 env OPTIMIZE -O2 -# order of UARTs (and USB) -SERIAL_ORDER OTG1 UART7 UART5 USART1 UART8 USART2 UART4 USART3 OTG2 +# order of UARTs (and USB) no IO MCU +SERIAL_ORDER OTG1 UART7 UART5 USART1 UART8 USART2 UART4 USART3 USART6 OTG2 + +# order of UARTs (and USB) with IO MCU +# SERIAL_ORDER OTG1 UART7 UART5 USART1 UART8 USART2 UART4 USART3 OTG2 # default to all pins low to avoid ESD issues DEFAULTGPIO OUTPUT LOW PULLDOWN diff --git a/libraries/AP_HAL_ChibiOS/hwdef/ARK_CANNODE/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/ARK_CANNODE/hwdef.dat index 8fa2061e0ab906..006fe14d9cf036 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/ARK_CANNODE/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/ARK_CANNODE/hwdef.dat @@ -125,3 +125,8 @@ define CAN_APP_NODE_NAME "org.ardupilot.ARK_CANNODE" define HAL_SUPPORT_RCOUT_SERIAL 1 define HAL_WITH_ESC_TELEM 1 define AP_SERIALLED_ENABLED 1 + +# Rangefinder +define HAL_PERIPH_ENABLE_RANGEFINDER +# disable rangefinder by default +define AP_PERIPH_RANGEFINDER_PORT_DEFAULT -1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DC2.Pilot peripherals.png b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DC2.Pilot peripherals.png new file mode 100644 index 00000000000000..e12b3c6c1edc7d Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DC2.Pilot peripherals.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DCS2.Pilot_BottomSide.png b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DCS2.Pilot_BottomSide.png new file mode 100644 index 00000000000000..694f1497f67f62 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DCS2.Pilot_BottomSide.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DCS2.Pilot_TopSide.png b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DCS2.Pilot_TopSide.png new file mode 100644 index 00000000000000..b8623b56a7233e Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/DCS2.Pilot_TopSide.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/README.md b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/README.md new file mode 100644 index 00000000000000..b747dd3f54be07 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/README.md @@ -0,0 +1,256 @@ +# Onboard FMU on Airvolute DCS2.Pilot board + +DroneCore 2.0 is a modular AI-driven open architecture autopilot designed for complex use cases that combines high computational processing power, redundant connectivity, small size, and low weight.The autopilot represents a one-stop-solution for developers integrating the functionality of carrier board, companion computer, and power distribution board into a single compact form factor. +This system usually uses a "CUBE" autopilot as its primary FMU, but can use an onboard STM32H743 as the FMU. This board definition and firmware on `the ArduPilot firmware server `__ is for this secondary FMU +For more informations on DCS2.Pilot board see: +https://docs.airvolute.com/dronecore-autopilot/dcs2 + +## Where To Buy +info@airvolute.com + +## Features + + - MCU: STM32H743 + - IMU: BMI088 + - Barometer: BMP390 + - 2 UARTS + - 2 CAN buses + - 4 PWM outputs + - PPM (RC input) + - external SPI and I2C + - SD card connector + - USB connection onboard with Jetson Host + - Ethernet + +## DCS2.Pilot peripherals diagram +DC2 Pilot peripherals + +## DCS2.Pilot onboard FMU related connectors pinout +### Top side +DCS2 Pilot_bottom + +#### PPM connector (RC input) +JST GH 1.25mm pitch, 3-Pin + +Matching connector JST GHR-03V-S. + +RC input is configured on the PPM_SBUS_PROT pin as part of the PPM connector. Pin is connected to UART3_RX and also to analog input on TIM3_CH1. This pin supports all unidirectional RC protocols, but for it to be enabled, it is necessary to set SERIAL3_PROTOCOL as RCIN. Also RC input is shared with primary FMU, so it is default disabled on this secondary FMU. + +5V supply is limited to 1A by internal current limiter. +
    ValueMeaning
    + + + + + + + + + + + + + + + + +
    Pin Signal
    1GND
    25V
    3PPM
    + +### Bottom side +DCS2 Pilot_top + +#### FMU SEC. connector +JST GH 1.25mm pitch, 12-Pin + +Matching connector JST GHR-12V-S. + +The DCS2 Onboard FMU supports up to 4 PWM outputs. These are directly attached to the STM32H743 and support all PWM protocols as well as DShot and bi-directional DShot. +The 4 PWM outputs are in 2 groups: +PWM 1,2 in group1 +PWM 3,4 in group2 +Channels within the same group need to use the same output rate. If any channel in a group uses DShot then all channels in the group need to use DShot. + +5V supply is limited to 1A by internal current limiter. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Pin Signal
    1GND
    2GND
    3GPIO/PWM output 4
    4GPIO/PWM output 3
    5GPIO/PWM output 2
    6GPIO/PWM output 1
    7Serial 1 RX
    8Serial 1 TX
    9Serial 2 RX
    10Serial 2 TX
    115V
    125V
    + + #### EXT. SENS. connector + BM23PF0.8-10DS-0.35V connector + + Matching connector BM23PF0.8-10DP-0.35V + + This connector allows connecting external IMU with I2C and SPI data buses. + + 5V supply is limited to 1.9A by internal current limiter. + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
    Pin Signal
    1SPI_MOSI
    2SPI_MISO
    3SPI_SCK
    4SPI_CS0
    5SPI_CS1
    6SPI_CS2
    7SPI_CS3
    8IMU_DRDY_EXT
    9I2C_SE_SDA
    10I2C_SE_SCL
    MP15V
    MP25V
    MP3GND
    MP4GND
    + + #### ETH EXP. connector + 505110-1692 connector + + Ethernet connector is routed to FMU through onboard switch. + + The onboard FMU is connected via the RMII bus with a speed of 100 Mbits. + + #### SD card connector + MEM2085-00-115-00-A connector + + Connector for standard microSD memory card. This card is primarily used to store flight data and logs. + +## Other connectors +### CAN 1, CAN 2 connectors +The board contains two CAN buses - CAN1 and CAN 2. The buses support speeds up to 1 Mbits and in FD mode up to 8 Mbits. + +These connectors are not part of DCS2.Pilot board, but they are routed on DCS2.Adapter_board. This board (DCS2.Adapter_board) is fully modular and can be modified according to the customer's requirements. For more informations see: https://docs.airvolute.com/dronecore-autopilot/dcs2/adapter-extension-boards/dcs2.-adapter-default-v1.0/connectors-and-pinouts + +JST GH 1.25mm pitch, 4-Pin + +Matching connector JST GHR-04V-S. + +5V supply is limited to 1.9A by internal current limiter. + + + + + + + + + + + + + + + + + + + + +
    Pin Signal
    15V
    2CAN_H
    3CAN_L
    4GND
    + +## UART Mapping + +- SERIAL0 -> USB (Default baud: 115200) +- SERIAL1 -> UART1 (FMU SEC) (Default baud: 57600, Default protocol: Mavlink2 (2)) +- SERIAL2 -> UART2 (FMU SEC) (Default baud: 57600, Default protocol: Mavlink2 (2)) +- SERIAL3 -> UART3 (RX pin only labeled as PPM on PPM connector) (Since this is a secondary FMU, default protocol is set to NONE instead of RCIN (23)) + +UARTs do not have RTS/CTS. UARTs 1 and 2 are routed to FMU_SEC. connector. + +## Loading Firmware + +Initial bootloader load is achievable only by SDW interface. Then it is possible to flash firmware thrugh onboard USB connection with Jetson host. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/defaults.parm new file mode 100644 index 00000000000000..6ffa986988d26c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/defaults.parm @@ -0,0 +1,4 @@ +SERIAL3_PROTOCOL -1 +NTF_BUZZ_TYPES 0 +NTF_BUZZ_VOLUME 0 +SYSID_THISMAV 2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/hwdef-bl.dat new file mode 100644 index 00000000000000..e11d30a2ba3901 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/hwdef-bl.dat @@ -0,0 +1,58 @@ +# hw definition file for processing by chibios_hwdef.py +# for H743 bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID for firmware load +APJ_BOARD_ID 5200 + +# the nucleo seems to have trouble with flashing the last sector? +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 + +# USB setup +USB_STRING_MANUFACTURER "Airvolute" + +# baudrate to run bootloader at on uarts +define BOOTLOADER_BAUDRATE 115200 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART1 + +# this is the pin that senses USB being connected. It is an input pin +# setup as OPENDRAIN +PA9 VBUS INPUT OPENDRAIN + +# UART1 (SE_CONNECTOR) +PB14 USART1_TX USART1 +PA10 USART1_RX USART1 + +PB1 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 1 + +define HAL_USE_SERIAL TRUE + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# Add CS pins to ensure they are high in bootloader +PD12 IMU_CS0 CS +PE13 IMU_CS1 CS +PD13 BARO_CS CS +PD15 EXT_0 CS +PD14 EXT_1 CS +PD11 EXT_2 CS +PD10 EXT_3 CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/hwdef.dat new file mode 100644 index 00000000000000..313d8e62453015 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Airvolute-DCS2/hwdef.dat @@ -0,0 +1,174 @@ +# hw definition file for processing by chibios_hwdef.py +#following exapmle: +#https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_HAL_ChibiOS/hwdef/mRoControlZeroH7/hwdef.dat#L105 +# for H743 bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID for firmware load +APJ_BOARD_ID 5200 + +# the nucleo seems to have trouble with flashing the last sector? +FLASH_SIZE_KB 2048 + +# This is the STM32 timer that ChibiOS will use for the low level +# driver. This must be a 32 bit timer. We currently only support +# timers 2, 3, 4, 5 and 21. See hal_st_lld.c in ChibiOS for details. + +# ChibiOS system timer +STM32_ST_USE_TIMER 5 + +#optimize for space +env OPTIMIZE -Os + +FLASH_RESERVE_START_KB 128 + +define HAL_STORAGE_SIZE 32768 + +# use last 2 pages for flash storage +# H743 has 16 pages of 128k each +STORAGE_FLASH_PAGE 14 + +# (---FUTURE REVISION---): PA0 AS VDD_SENSE_PIN +PA0 VDD_5V_SENS ADC1 SCALE(2) + +# order of I2C buses +I2C_ORDER I2C1 + +# I2C1 (SE_CONNECTOR) +PB8 I2C1_SCL I2C1 PULLUP +PB7 I2C1_SDA I2C1 PULLUP + +# USB setup +USB_STRING_MANUFACTURER "Airvolute" + +# This is the pin that senses USB being connected. It is an input pin +# setup as OPENDRAIN. +PA9 VBUS INPUT OPENDRAIN + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART1 USART2 USART3 + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# (---FUTURE REVISION---): PWM output for buzzer +PB15 TIM12_CH2 TIM12 GPIO(80) ALARM + +# (---FUTURE REVISION---): RC Input set for Interrupt not DMA +PA6 TIM3_CH1 TIM3 RCININT PULLDOWN LOW + +# UART SETUP +# USART1 (SE_CONNECTOR) +PB14 USART1_TX USART1 +PA10 USART1_RX USART1 + +# USART2 (SE_CONNECTOR) +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + +# USART3 (RC INPUT) +PD9 USART3_RX USART3 + +#LED RED +PB1 LED1 OUTPUT HIGH +#LED GREEN +PA3 FMU_LED_AMBER OUTPUT LOW GPIO(0) + +# microSD card +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# CS pins for SPI sensors. The labels for all CS pins need to +# match the SPI device table later in this file. +PD12 BMI088_ACC_CS CS +PE13 BMI088_GYRO_CS CS +PD13 BMP390_CS CS +PD15 EXT_0_CS CS +PD14 EXT_1_CS CS +PD11 EXT_2_CS CS +PD10 EXT_3_CS CS + +# CAN Busses +PD0 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 + +# CAN1 Silent Pin LOW Enable +PC7 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) + +PB5 CAN2_RX CAN2 +PB6 CAN2_TX CAN2 + +# CAN2 Silent Pin LOW Enable +PC6 GPIO_CAN2_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(71) + +# Ethernet +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +PB12 ETH_RMII_TXD0 ETH1 +PB13 ETH_RMII_TXD1 ETH1 +PB11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 + +define BOARD_PHY_RMII +define BOARD_PHY_ADDRESS 0x0000 +define STM32_MAC_PHY_LINK_TYPE MAC_LINK_100_FULLDUPLEX +define HAL_PERIPH_ENABLE_NETWORKING + +#PWM outputs +PB3 TIM2_CH2 TIM2 PWM(1) GPIO(50) BIDIR +PB10 TIM2_CH3 TIM2 PWM(2) GPIO(51) +PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) BIDIR +PE14 TIM1_CH4 TIM1 PWM(4) GPIO(53) + +# BMI088_DRDY +PE12 BMI088_DRDY INPUT + +# (---FUTURE REVISION---) EXT IMU DRDY +PD1 EXT_IMU_DRDY INPUT + +# SPI1 (ONBOARD SENSORS) +PA5 SPI1_SCK SPI1 +PB4 SPI1_MISO SPI1 +PD7 SPI1_MOSI SPI1 + +# SPI4 (EXTERNAL SENSORS) +PE2 SPI4_SCK SPI4 +PE5 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 + +# SPI devices +SPIDEV bmi088_g SPI1 DEVID1 BMI088_GYRO_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmi088_a SPI1 DEVID2 BMI088_ACC_CS MODE3 10*MHZ 10*MHZ +SPIDEV bmp390 SPI1 DEVID3 BMP390_CS MODE3 5*MHZ 5*MHZ +SPIDEV icm42688_ext SPI4 DEVID4 EXT_0_CS MODE3 5*MHZ 5*MHZ +SPIDEV lis3mdl SPI4 DEVID5 EXT_1_CS MODE3 5*MHZ 5*MHZ + +# Enable FAT filesystem support (needs a microSD defined via SDMMC). +define HAL_OS_FATFS_IO 1 + +# IMUs +IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_NONE +IMU Invensense SPI:icm42688_ext ROTATION_NONE + +# 1 baro +define HAL_BARO_ALLOW_INIT_NO_BARO 1 +BARO BMP388 SPI:bmp390 + +define ALLOW_ARM_NO_COMPASS + +define HAL_PROBE_EXTERNAL_I2C_COMPASSES diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/README.md b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/README.md new file mode 100644 index 00000000000000..08db2c7ab067af --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/README.md @@ -0,0 +1,87 @@ +# AnyLeaf Mercury H7 Flight Controller + +The Mercury H7 is a flight controller produced by [AnyLeaf](http://www.anyleaf.org/). + +## Features + Processor + STM32H743 32-bit processor + Sensors + ICM42688 Acc/Gyro with dedicated 32.768kHz crystal + DPS310 barometer + Power + 2S - 6S Lipo input voltage with voltage monitoring + 9V, 3A supply for powering video transmitters + 5V, 2A supply for powering servos and electronics + 3.3V, 500mA supply for powering electronics + Interfaces + 8x Bidirectional-DSHOT, or PWM-capable motor outputs + 1x CAN-FD port for external peripherals + 1x DJI-format Vtx connector + 4x UARTs/serial for external peripherals, 3 of which are available by default + 1x I2C bus for external peripherals + USB-C port + All UARTs support hardware inversion + Onboard ExpressLRS radio tranceiver for control and/or telemetry data. + Dimensions + Size: 37.5 x 37.5mm + Weight: 8g + +## Pinout + +![Anyleaf H7 pinout, bottom](anyleaf_h7_diagram_bottom.jpg) +![Anyleaf H7 connectors, top](anyleaf_h7_diagram_top.jpg) + +Pins and connector values are labeled on the flight controller PCB, with the following exceptions: +- The onboard ELRS tranceiver is connected to pins PA2 (FC Tx) and PA3 (FC Rx), on UART2. +- ESC telemetry is connected to UART3 Rx (PD9) +- OSD HDL (DJI hand controller interop) is connected to UART1 Rx (PB7) + +## UART Mapping + All UARTs are DMA capable + + - SERIAL0 -> USB + - SERIAL1 -> UART1 (External pads, and RX1 on DJI connector SBUS pin; defaulted to MAVLINK2) + - SERIAL2 -> UART2 (DJI Connector telemetry; defaulted to DisplayPort) + - SERIAL3 -> UART3 (ESC connector telemetry pin, or external pad; defaulted to ESC telemetry) + - SERIAL4 -> USART4 (External pads; GPS protocol by default) + - SERIAL5 -> UART7 (Onboard ELRS receiver only, RCIN protocol) + - SERIAL6 -> UART8 (USER, External pads) + +## Can FD port + +This flight controller includes a 4-pin DroneCAN standard CAN port. It's capable of 64-byte frames, +and up to 5Mbps data rates. It's useful for connecting GPS devices, compasses, power monitoring, sensors, motors, servos, and other CAN peripherals. + +## RC Input + +This flight controller includes a 2.4Ghz ExpressLRS transceiver, capable of receiving control input, and transmitting or receiving MavLink telemetry. To enable all ELRS features, either RC5 channel should be setup as an ARM switch (there are several RC5_OPTIONS that can do this) or by mapping the transmitter's Channel 5 to reflect ARM status from telemetry. See: https://youtu.be/YO2yA1fmZBs for an axample. + +SBUS on the DJI connector may be used if SERIAL5_PROTOCOL is changed to 0 and SERIAL1_PROTOCOL is changed to 23 for RC input. + +## OSD Support + +This flight controller has an MSP-DisplayPort output on a 6-pin DJI-compatible JST SH port re-configured. + +## Motor Output + +Motor 1-8 is capable of bidirectional DSHOT and PWM. + +All outputs in the motor groups below must be either PWM or DShot: +Motors 1-4 Group1 +Motors 5-6 Group2 +Motors 7-8 Group3 + +## Magnetometer + +This flight controller does not have a built-in magnetometer, but you can attach an external one using the CAN connector, or the I2C pads on the bottom. + +## Loading Firmware +Firmware for these boards can be found at https://firmware.ardupilot.org in sub-folders labeled “Anyleaf H7”. + +Initial firmware load can be done with DFU by plugging in USB with the +boot button pressed. Then you should load the "AnyleafH7_bl.hex" +firmware, using your favourite DFU loading tool. + +Subsequently, you can update firmware using Mission Planner or QGroundControl. + + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/anyleaf_h7_diagram_bottom.jpg b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/anyleaf_h7_diagram_bottom.jpg new file mode 100644 index 00000000000000..3b4276d4974bcf Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/anyleaf_h7_diagram_bottom.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/anyleaf_h7_diagram_top.jpg b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/anyleaf_h7_diagram_top.jpg new file mode 100644 index 00000000000000..4de4a27b10aea4 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/anyleaf_h7_diagram_top.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/common.inc b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/common.inc new file mode 100644 index 00000000000000..75ad7d1fce77ea --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/common.inc @@ -0,0 +1,20 @@ +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# board ID for firmware load +APJ_BOARD_ID 1146 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 16000000 + +FLASH_SIZE_KB 2048 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define AP_CAN_SCLAN_ENABLED 1 +define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/defaults.parm new file mode 100644 index 00000000000000..adcd6cca41638e --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/defaults.parm @@ -0,0 +1,23 @@ +# CAN +CAN_P1_DRIVER 1 # Enables the use of CAN + +# Onboard ELRS +SERIAL5_PROTOCOL 23 # RCIN +RC_OPTIONS 8192 # Set 420k baud for ELRS +# RC2_REVERSED 1 # Prevent inverse pitch controls +RSSI_TYPE 3 # RC provided RSSI +# ARMING_RUDDER 0 # Disable rudder arming + + +# Onboard OSD +OSD_TYPE 5 # MSP +SERIAL2_PROTOCOL 42 # DisplayPort +OSD_CELL_COUNT 0 # auto based on voltage at start + + +# ESC telemetry +SERIAL3_PROTOCOL 16 # ESC telemetry + + +# Remove default serial protocols; set to None. +SERIAL6_PROTOCOL -1 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/hwdef-bl.dat new file mode 100644 index 00000000000000..ce3391e05f04be --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/hwdef-bl.dat @@ -0,0 +1,16 @@ +include common.inc + + +# bootloader starts at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +# the H743 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 128 + +# order of UARTs (and USB). Allow bootloading on USB and telem1 +SERIAL_ORDER OTG1 + +# Add CS pins to ensure they are high in bootloader +PC4 IMU1_CS CS +PE11 FLASH_SPI_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/hwdef.dat new file mode 100644 index 00000000000000..91281228a2ca4f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/AnyleafH7/hwdef.dat @@ -0,0 +1,118 @@ +include common.inc + +env OPTIMIZE -Os + +# bootloader takes first sector +FLASH_RESERVE_START_KB 128 + +# use last 2 pages for flash storage +# H743 has 16 pages of 128k each +define HAL_STORAGE_SIZE 32768 +STORAGE_FLASH_PAGE 14 + +# ChibiOS system timer +STM32_ST_USE_TIMER 12 +define CH_CFG_ST_RESOLUTION 16 + +# SPI1 for IMU1 (ICM42688) +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PC4 IMU1_CS CS + +# SPI2 for flash (W25Q64JVZPIQ (64Mbit)) +PA9 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 +PE11 FLASH_CS CS + +# two I2C buses +I2C_ORDER I2C2 I2C1 + +# I2C1 +PB8 I2C1_SCL I2C1 PULLUP +PB9 I2C1_SDA I2C1 PULLUP + +# I2C2 +PB10 I2C2_SCL I2C2 PULLUP +PB11 I2C2_SDA I2C2 PULLUP + +# ADC +PA4 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PA0 BATT_CURRENT_SENS ADC1 SCALE(1) + +define HAL_BATT_MONITOR_DEFAULT 4 # Analog voltage and current +define HAL_BATT_VOLT_PIN 18 # ADC channel +define HAL_BATT_CURR_PIN 16 +define HAL_BATT_VOLT_SCALE 11.44 # Nominally a 11x divider. 11.44 is based on experimenting. +define HAL_BATT_CURR_SCALE 40.0 # This depends on the ESC's V/mA output ratio. + +# LED +PC13 LED0 OUTPUT LOW GPIO(90) # Amber; System status. +PC0 LED1 OUTPUT LOW GPIO(91) # Not connected; GPS Status. (Required, or the system status LED won't work.) +define HAL_GPIO_A_LED_PIN 90 +define HAL_GPIO_B_LED_PIN 91 +define HAL_GPIO_LED_ON 0 +define HAL_GPIO_LED_OFF 1 + +# order of UARTs (and USB) OTG2 may be required for SLCAN. +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART7 UART8 #OTG2 + +# USART1 +PB6 USART1_TX USART1 +PB7 USART1_RX USART1 + +# USART2 (MSP Displayport OSD) +PA2 USART2_TX USART2 +PA3 USART2_RX USART2 + +# USART3 +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 + +# UART4 +PC10 UART4_TX UART4 +PC11 UART4_RX UART4 + +# UART7 (ELRS receiver) +PB4 UART7_TX UART7 +PB3 UART7_RX UART7 + +PE1 UART8_TX UART8 +PE0 UART8_RX UART8 + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +# Motors +PC6 TIM3_CH1 TIM3 PWM(1) GPIO(50) BIDIR +PC7 TIM3_CH2 TIM3 PWM(2) GPIO(51) +PC8 TIM3_CH3 TIM3 PWM(3) GPIO(52) BIDIR +PC9 TIM3_CH4 TIM3 PWM(4) GPIO(53) +PE5 TIM15_CH1 TIM15 PWM(5) GPIO(54) BIDIR +PE6 TIM15_CH2 TIM15 PWM(6) GPIO(55) +PE13 TIM1_CH3 TIM1 PWM(7) GPIO(56) BIDIR +PE14 TIM1_CH4 TIM1 PWM(8) GPIO(57) + +# Beeper +PE9 TIM1_CH1 TIM1 GPIO(58) ALARM + +DMA_PRIORITY S* + +DMA_NOSHARE SPI1* TIM3* TIM15* TIM1* + +# SPI devices +SPIDEV icm42688 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 16*MHZ +SPIDEV dataflash SPI2 DEVID1 FLASH_CS MODE3 104*MHZ 104*MHZ + +# IMU +IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_270 +define HAL_DEFAULT_INS_FAST_SAMPLE 1 + +# DPS310 integrated on I2C2 bus, multiple possible choices for external barometer +BARO DPS310 I2C:0:0x77 + +# setup for OSD +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 5 # MSP Displayport diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat index 11006490e13e1e..5e27c96a75d303 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BETAFPV-F405/hwdef.dat @@ -142,3 +142,5 @@ define HAL_FRAME_TYPE_DEFAULT 12 # This is a whoop AIO board, not really suitable for anything other than copter AUTOBUILD_TARGETS Copter + +include ../include/minimize_fpv_osd.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BeastF7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BeastF7/hwdef.dat index 496e7563d11855..d1c3153292349b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BeastF7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BeastF7/hwdef.dat @@ -137,9 +137,6 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font0.bin define STM32_PWM_USE_ADVANCED TRUE # save some flash -include ../include/save_some_flash.inc - -# only include ublox GPS driver -include ../include/minimal_GPS.inc +include ../include/minimize_fpv_osd.inc define DEFAULT_NTF_LED_TYPES 257 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BeastF7v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BeastF7v2/hwdef.dat index 174747369a40ae..e1c82946b5ed06 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/BeastF7v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/BeastF7v2/hwdef.dat @@ -61,13 +61,6 @@ IMU BMI270 SPI:bmi270 ROTATION_ROLL_180_YAW_225 define HAL_BARO_ALLOW_INIT_NO_BARO 1 # save some flash -include ../include/save_some_flash.inc -define AP_GRIPPER_ENABLED 0 -define HAL_PARACHUTE_ENABLED 0 -define HAL_SPRAYER_ENABLED 0 -define AP_BATTERY_SMBUS_ENABLED 0 - -# only include ublox GPS driver -include ../include/minimal_GPS.inc +include ../include/minimize_fpv_osd.inc define DEFAULT_NTF_LED_TYPES 257 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat new file mode 100644 index 00000000000000..f330a1c45585c0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef-bl.dat @@ -0,0 +1,72 @@ +# hw definition file for processing by chibios_hwdef.py +# for the BotBloxSwitch hardware + +# default to all pins low to avoid ESD issues +DEFAULTGPIO OUTPUT LOW PULLDOWN + +# MCU class and specific type +MCU STM32H7xx STM32H723xx + +# crystal frequency +OSCILLATOR_HZ 0 + +# setup build for a peripheral bootloader +env AP_PERIPH 1 + +# board ID for firmware load +APJ_BOARD_ID AP_HW_BotBloxSwitch + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +FLASH_BOOTLOADER_LOAD_KB 128 + +# flash size +FLASH_SIZE_KB 1024 + +env OPTIMIZE -Os + +# USB +PA11 OTG_HS_DM OTG1 +PA12 OTG_HS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 +PD12 USART3_RTS USART3 +PD11 USART3_CTS USART3 + +# LEDs +PE5 LED_BOOTLOADER OUTPUT OPENDRAIN HIGH # blue +define HAL_LED_ON 0 + +define HAL_USE_EMPTY_STORAGE 1 +define HAL_STORAGE_SIZE 16384 + +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +#PB12 ETH_RMII_TXD0 ETH1 +PG13 ETH_RMII_TXD0 ETH1 +PB13 ETH_RMII_TXD1 ETH1 +#PB11 ETH_RMII_TX_EN ETH1 +PG11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 + +define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_RMII + +include ../include/network_bootloader.inc + +SERIAL_ORDER OTG1 USART3 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat new file mode 100644 index 00000000000000..063592224336ac --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/BotBloxSwitch/hwdef.dat @@ -0,0 +1,88 @@ +# hw definition file for processing by chibios_hwdef.py +# for the BotBloxSwitch hardware + +# default to all pins low to avoid ESD issues +DEFAULTGPIO OUTPUT LOW PULLDOWN + +# MCU class and specific type +MCU STM32H7xx STM32H723xx + +# crystal frequency +OSCILLATOR_HZ 0 + +# setup build for a peripheral bootloader +env AP_PERIPH 1 + +# board ID for firmware load +APJ_BOARD_ID AP_HW_BotBloxSwitch + +FLASH_RESERVE_START_KB 128 + +define HAL_STORAGE_SIZE 32768 + +# use last 2 pages for flash storage +# H723 has 8 pages of 128k each +STORAGE_FLASH_PAGE 6 + +# flash size +FLASH_SIZE_KB 1024 + +env OPTIMIZE -Os + +# pins for SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# USB +PA11 OTG_HS_DM OTG1 +PA12 OTG_HS_DP OTG1 +PA9 VBUS INPUT OPENDRAIN + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +PD8 USART3_TX USART3 +PD9 USART3_RX USART3 +PD12 USART3_RTS USART3 +PD11 USART3_CTS USART3 + +# LEDs +PE3 LED_RED OUTPUT OPENDRAIN HIGH +PE4 LED_GREEN OUTPUT OPENDRAIN HIGH +PE5 LED_BLUE OUTPUT OPENDRAIN HIGH +define HAL_LED_ON 0 + +# use blue LED +define HAL_GPIO_PIN_LED HAL_GPIO_PIN_LED_BLUE + +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +#PB12 ETH_RMII_TXD0 ETH1 +PG13 ETH_RMII_TXD0 ETH1 +PB13 ETH_RMII_TXD1 ETH1 +#PB11 ETH_RMII_TX_EN ETH1 +PG11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 + +define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_RMII + +include ../include/network_PPPGW.inc + +# allow load from USB +SERIAL_ORDER OTG1 USART3 + +# bootloader embedding / bootloader flashing not available +define AP_BOOTLOADER_FLASHING_ENABLED 0 + +# no ADC pins +define HAL_USE_ADC FALSE + +define STM32_ADC_USE_ADC1 FALSE +define STM32_ADC_USE_ADC2 FALSE +define STM32_ADC_USE_ADC3 FALSE + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat index c621a9f58bd269..807e790e303287 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CarbonixL496/hwdef.dat @@ -139,3 +139,5 @@ define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 BARO MS56XX I2C:0:0x76 COMPASS QMC5883P I2C:0:0x2C false ROTATION_YAW_180 + +define AP_COMPASS_QMC5883P_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack-periph/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack-periph/hwdef-bl.dat index effd49fee7a162..ba2212fa9cb623 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack-periph/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeBlack-periph/hwdef-bl.dat @@ -27,7 +27,7 @@ define HAL_LED_ON 0 # order of UARTs (and USB) SERIAL_ORDER OTG1 UART7 -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/Tools/Frame_params/Solo_Copter-4_GreenCube.param b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/defaults.parm similarity index 99% rename from Tools/Frame_params/Solo_Copter-4_GreenCube.param rename to libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/defaults.parm index 221f9f39c8d331..127840bf7f4503 100644 --- a/Tools/Frame_params/Solo_Copter-4_GreenCube.param +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/defaults.parm @@ -49,7 +49,7 @@ BRD_SAFETY_MASK,16368 BRD_SAFETYENABLE,0 CIRCLE_RADIUS,3000 COMPASS_ORIENT,38 -COMPASS_TYPEMASK,15392 +COMPASS_DISBLMSK,15392 COMPASS_USE3,0 EK2_ENABLE,0 EK3_ENABLE,1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat index 0266753fe17401..c02f667fd2ad56 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeGreen-solo/hwdef.dat @@ -6,10 +6,6 @@ include ../CubeBlack/hwdef.dat -# pull Solo's default parameters from /Tools/Frame_params -# these are parameters the Solo requires for proper operation that are different from the 4 standard defaults. -env DEFAULT_PARAMETERS 'Tools/Frame_params/Solo_Copter-4_GreenCube.param' - undef AP_NOTIFY_OREOLED_ENABLED define AP_NOTIFY_OREOLED_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/defaults.parm new file mode 100644 index 00000000000000..e189e7c80162da --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/defaults.parm @@ -0,0 +1 @@ +@include ../CubeOrange/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/hwdef.dat index 4f3c0cd9ba3c36..6666bb218600ea 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-bdshot/hwdef.dat @@ -10,5 +10,3 @@ PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) BIDIR PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53) # this will automatically be shared with TIM1_CH1 PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54) BIDIR PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55) BIDIR - -env DEFAULT_PARAMETERS libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef-bl.dat index 6d10b336b8f812..7a86058449d0db 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef-bl.dat @@ -27,7 +27,7 @@ define HAL_LED_ON 0 # order of UARTs (and USB) SERIAL_ORDER OTG1 UART7 -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.inc b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.inc index 5da67a0bdfd8b7..f5219200982ed2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef-bl.inc @@ -34,7 +34,7 @@ PD6 USART2_RX USART2 PD8 USART3_TX USART3 PD9 USART3_RX USART3 -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc index cd7ef40d20a07e..cb5769d8d514b3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc @@ -195,7 +195,7 @@ PE6 SPI4_MOSI SPI4 # means sensors off, then it is pulled HIGH in peripheral_power_enable() PE3 VDD_3V3_SENSORS_EN OUTPUT LOW -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus-bdshot/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus-bdshot/defaults.parm new file mode 100644 index 00000000000000..591b7847763ef2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus-bdshot/defaults.parm @@ -0,0 +1 @@ +@include ../CubeOrangePlus/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus-bdshot/hwdef.dat index 4962342835feeb..d2f45607b7eea2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus-bdshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus-bdshot/hwdef.dat @@ -10,5 +10,3 @@ PE11 TIM1_CH2 TIM1 PWM(3) GPIO(52) BIDIR PE9 TIM1_CH1 TIM1 PWM(4) GPIO(53) # this will automatically be shared with TIM1_CH1 PD13 TIM4_CH2 TIM4 PWM(5) GPIO(54) BIDIR PD14 TIM4_CH3 TIM4 PWM(6) GPIO(55) BIDIR - -env DEFAULT_PARAMETERS libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-CANMod/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-CANMod/hwdef.dat index 4994a739fb77af..965cd4462cba1e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-CANMod/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-CANMod/hwdef.dat @@ -85,11 +85,13 @@ define HAL_DISABLE_LOOP_DELAY ################################# define HAL_BARO_ALLOW_INIT_NO_BARO -define AP_RC_CHANNEL_ENABLED 1 define AP_INERTIALSENSOR_ENABLED 0 define AP_NETWORKING_MAX_INSTANCES 4 +define AP_SCRIPTING_ENABLED 1 +define AP_FILESYSTEM_ROMFS_ENABLED 1 + # listen for reboot command from uploader.py script # undefine to disable. Use -1 to allow on all ports, otherwise serial number index defined in SERIAL_ORDER starting at 0 define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/defaults.parm new file mode 100644 index 00000000000000..89ab4beebb035b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/defaults.parm @@ -0,0 +1,14 @@ +NET_ENABLED 1 +NET_OPTIONS 1 + +# enable hw flow control +UART1_RTSCTS 1 + +# swap TX and RX +UART1_OPTIONS 8 + +SCR_ENABLE 1 +SCR_VM_I_COUNT 1000000 + +WEB_ENABLE 1 +WEB_PORT 80 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef-bl.dat new file mode 100644 index 00000000000000..83e9d53d2fcf35 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef-bl.dat @@ -0,0 +1,20 @@ +include ../CubePilot-CANMod/hwdef-bl.dat + +# Ethernet +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +PB12 ETH_RMII_TXD0 ETH1 +PB13 ETH_RMII_TXD1 ETH1 +PB11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 + +define BOARD_PHY_ID MII_LAN8720_ID +define BOARD_PHY_RMII + +include ../include/network_bootloader.inc + +# allow USB bootloader too +SERIAL_ORDER OTG1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat new file mode 100644 index 00000000000000..70c50244fd8ef4 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef.dat @@ -0,0 +1,17 @@ +include ../CubePilot-CANMod/hwdef.dat + +# we need RTS/CTS for the PPP link +undef PD14 +undef PD13 +undef PE0 +undef PE1 + +# need to use UART8 to get RTS/CTS +PE1 UART8_TX UART8 +PE0 UART8_RX UART8 +PD13 UART8_RTS UART8 +PD14 UART8_CTS UART8 + +SERIAL_ORDER OTG1 UART8 + +include ../include/network_PPPGW.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/defaults.parm new file mode 100644 index 00000000000000..89ab4beebb035b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/defaults.parm @@ -0,0 +1,14 @@ +NET_ENABLED 1 +NET_OPTIONS 1 + +# enable hw flow control +UART1_RTSCTS 1 + +# swap TX and RX +UART1_OPTIONS 8 + +SCR_ENABLE 1 +SCR_VM_I_COUNT 1000000 + +WEB_ENABLE 1 +WEB_PORT 80 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef-bl.dat new file mode 100644 index 00000000000000..b34964db7c6f09 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef-bl.dat @@ -0,0 +1,40 @@ +include ../CubeRedPrimary/hwdef-bl.dat + +# setup build for a peripheral bootloader +env AP_PERIPH 1 + +# board ID for firmware load +APJ_BOARD_ID AP_HW_CUBERED_PERIPH + +# CAN config +PE10 GPIOCAN1_TERM OUTPUT HIGH +PG4 GPIOCAN2_TERM OUTPUT HIGH + +PB8 CAN1_RX CAN1 +PB9 CAN1_TX CAN1 + +PB5 CAN2_RX CAN2 +PB6 CAN2_TX CAN2 + +# Ethernet +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +PB12 ETH_RMII_TXD0 ETH1 +PB13 ETH_RMII_TXD1 ETH1 +PB11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 + +define BOARD_PHY_ID MII_LAN8720_ID +define BOARD_PHY_RMII + +# Refer to https://maclookup.app/vendors/cubepilot-pty-ltd +# Note, lower 3 bytes (ADDR3,4,5) will be replaced with the platform UUID +define AP_NETWORKING_BOOTLOADER_DEFAULT_MAC_ADDR "A8:B0:28:00:00:00" + +# allow load from USB too +SERIAL_ORDER OTG1 + +include ../include/network_bootloader.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef.dat new file mode 100644 index 00000000000000..b718f762d545fe --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary-PPPGW/hwdef.dat @@ -0,0 +1,43 @@ +include ../CubeRedPrimary/hwdef.dat + +undef ROMFS +undef HAL_HAVE_SAFETY_SWITCH +undef HAL_WITH_IO_MCU_BIDIR_DSHOT +undef COMPASS +undef BARO +undef DEFAULT_SERIAL7_PROTOCOL + +define AP_ADVANCEDFAILSAFE_ENABLED 0 + + +# board ID for firmware load +APJ_BOARD_ID AP_HW_CUBERED_PERIPH + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +define AP_CAN_SLCAN_ENABLED 0 + +define HAL_PERIPH_ENABLE_NETWORKING +define HAL_PERIPH_ENABLE_SERIAL_OPTIONS + +define AP_NETWORKING_BACKEND_PPP 1 + +define HAL_NO_MONITOR_THREAD +define HAL_DISABLE_LOOP_DELAY + +define HAL_USE_RTC FALSE +define DISABLE_SERIAL_ESC_COMM TRUE + +define HAL_NO_RCIN_THREAD + +# use amber LED +define HAL_GPIO_PIN_LED HAL_GPIO_PIN_FMU_LED_AMBER + +undef HAL_OS_FATFS_IO +undef SDMMC1 + +MAIN_STACK 0x2000 +PROCESS_STACK 0x6000 + +include ../include/network_PPPGW.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat index 76d2add5d96cb1..95690d23169b21 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedPrimary/hwdef.dat @@ -46,10 +46,6 @@ CANFD_SUPPORTED 8 STM32_ST_USE_TIMER 12 -# disable core m4 use to silence the asserts -# checking allocation of peripherals -define STM32_HAS_M4 0 - define CH_CFG_ST_RESOLUTION 16 # use last 2 pages for flash storage diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat index 82c5ad28103c86..19fe6a4e3cee64 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeRedSecondary/hwdef.dat @@ -152,8 +152,6 @@ define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_RCIN define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_Sbus1 define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_RCIN -define AP_SCRIPTING_ENABLED 0 - # only use pulse input for PPM, other protocols # are on serial define HAL_RCIN_PULSE_PPM_ONLY diff --git a/Tools/Frame_params/Solo_Copter-4.param b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/defaults.parm similarity index 99% rename from Tools/Frame_params/Solo_Copter-4.param rename to libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/defaults.parm index 0d9f09611087f9..0130fe56f90d4c 100644 --- a/Tools/Frame_params/Solo_Copter-4.param +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/defaults.parm @@ -50,7 +50,7 @@ BRD_SAFETYENABLE,0 BRD_TYPE,3 CIRCLE_RADIUS,3000 COMPASS_ORIENT,38 -COMPASS_TYPEMASK,15392 +COMPASS_DISBLMSK,15392 COMPASS_USE3,0 EK2_ENABLE,0 EK3_ENABLE,1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat index 8f15b713acb7da..899a75f1e9858e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeSolo/hwdef.dat @@ -12,10 +12,6 @@ env OPTIMIZE -O2 SPIDEV icm20948_ext SPI4 DEVID1 MPU_EXT_CS MODE3 4*MHZ 8*MHZ SPIDEV icm20602_ext SPI4 DEVID3 GYRO_EXT_CS MODE3 4*MHZ 8*MHZ -# pull Solo's default parameters from /Tools/Frame_params -# these are parameters the Solo requires for proper operation that are different from the copter defaults. -env DEFAULT_PARAMETERS 'Tools/Frame_params/Solo_Copter-4.param' - # three IMUs, but allow for different variants. First two IMUs are # isolated, 3rd isn't IMU Invensense SPI:mpu9250_ext ROTATION_PITCH_180 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat index c3773bc7b89b80..afc44a107c6050 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat @@ -86,20 +86,17 @@ USB_STRING_MANUFACTURER "Hex/ProfiCNC" # order of I2C buses I2C_ORDER I2C2 I2C1 -# now the UART order. These map to the hal.uartA to hal.uartF -# objects. If you use a shorter list then HAL_Empty::UARTDriver -# objects are substituted for later UARTs, or you can leave a gap by -# listing one or more of the uarts as EMPTY - -# the normal usage of this ordering is: +# now the SERIALn order. These map to the hal.serial(n) objects. If you use a +# shorter list then HAL_Empty::UARTDriver objects are substituted for later +# UARTs, or you can leave a gap by listing one or more of the uarts as EMPTY # 1) SERIAL0: console (primary mavlink, usually USB) -# 2) SERIAL3: primary GPS -# 3) SERIAL1: telem1 -# 4) SERIAL2: telem2 +# 2) SERIAL1: telem1 +# 3) SERIAL2: telem2 +# 4) SERIAL3: primary GPS # 5) SERIAL4: GPS2 # 6) SERIAL5: extra UART (usually RTOS debug console) -# order of UARTs (and USB) +# order of SERIALn (incl. USB) SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 UART7 OTG2 # if the board has an IOMCU connected via a UART then this defines the @@ -321,7 +318,7 @@ PE6 SPI4_MOSI SPI4 # timing affecting sensor stability. We pull it high by default PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters) +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/DrotekP3Pro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/DrotekP3Pro/hwdef.dat index 93d2626c16acff..7b20d22a687957 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/DrotekP3Pro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/DrotekP3Pro/hwdef.dat @@ -28,18 +28,17 @@ env OPTIMIZE -O2 # order of I2C buses I2C_ORDER I2C1 I2C2 -# now the UART order. These map to the hal.uartA to hal.uartF -# objects. If you use a shorter list then HAL_Empty::UARTDriver -# objects are substituted for later UARTs, or you can leave a gap by -# listing one or more of the uarts as EMPTY +# now the SERIALn order. These map to the hal.serial(n) objects. If you use a +# shorter list then HAL_Empty::UARTDriver objects are substituted for later +# UARTs, or you can leave a gap by listing one or more of the uarts as EMPTY # 1) SERIAL0: console (primary mavlink, usually USB) -# 2) SERIAL3: primary GPS -# 3) SERIAL1: telem1 -# 4) SERIAL2: telem2 +# 2) SERIAL1: telem1 +# 3) SERIAL2: telem2 +# 4) SERIAL3: primary GPS # 5) SERIAL4: GPS2 # 6) SERIAL5: extra UART (usually RTOS debug console) -# order of UARTs (and USB) +# order of SERIALn (incl. USB) SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 USART1 UART7 # UART for IOMCU diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/GOKUGN745AIO_Pinout.png b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/GOKUGN745AIO_v1.0_Pinout.png similarity index 100% rename from libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/GOKUGN745AIO_Pinout.png rename to libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/GOKUGN745AIO_v1.0_Pinout.png diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/GOKUGN745AIO_v1.2_Pinout.jpg b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/GOKUGN745AIO_v1.2_Pinout.jpg new file mode 100644 index 00000000000000..fc34078cc79532 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/GOKUGN745AIO_v1.2_Pinout.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/README.md b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/README.md index a491a15a10a64c..6c4817547a3bb3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/FlywooF745/README.md @@ -31,7 +31,13 @@ The Flywoo GOKU GN 745 AIO is a flight controller produced by [Flywoo](https://f ## Pinout -![GOKU GN 745 40A AIO](GOKUGN745AIO_Pinout.png "GOKU GN 745 40A AIO") +This board currently has two versions produced, with different pinouts. This is the original v1.0 board. + +![GOKU GN 745 40A AIO v1.0](GOKUGN745AIO_v1.0_Pinout.png "GOKU GN 745 40A AIO v1.0") + +The newer v1.2 board is shown below. Unfortunately, due to the reduced number of pads, some features, such as motors 5-8, are no longer available. + +![GOKU GN 745 40A AIO v1.2](GOKUGN745AIO_v1.2_Pinout.jpg "GOKU GN 745 40A AIO v1.2") ## UART Mapping diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/defaults.parm index 8099863e70f1c2..0a8a2328196559 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/defaults.parm @@ -1,4 +1,4 @@ -ALT_HOLD_RTL,5000 +RTL_ALTITUDE,50.00 ARMING_RUDDER,2 # GPS RX DMA conflicts with CRSF and I2C1 GPS_DRV_OPTIONS,4 \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/hwdef.dat index 086240a0f809e9..f3fee9ff6d7363 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/hwdef.dat @@ -157,14 +157,9 @@ define HAL_BATT_CURR_SCALE 26.7 #define BOARD_RSSI_ANA_PIN 8 #define HAL_DEFAULT_AIRSPEED_PIN 10 -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 define HAL_WITH_DSP FALSE define HAL_GYROFFT_ENABLED 0 -# --------------------- save flash ---------------------- -define AP_BATTMON_SMBUS_ENABLE 0 - include ../include/minimize_fpv_osd.inc include ../include/save_some_flash.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef-bl.dat index d94c02e9142b35..e3e6dca8be5ea1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4AP/hwdef-bl.dat @@ -50,8 +50,6 @@ define HAL_NO_MONITOR_THREAD PA13 JTMS-SWDIO SWD PA14 JTCK-SWCLK SWD -define AP_PERIPH_HAVE_LED - # order of UARTs SERIAL_ORDER define HAL_USE_UART FALSE diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/README.md b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/README.md new file mode 100644 index 00000000000000..42cdc2cc3eb7de --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/README.md @@ -0,0 +1,24 @@ +# Here4 GPS Module as Flight Controller + +Here4 Module is sold as a GPS module, but it can be used as a flight controller. + +## Features +- STM32H757 MCU +- ICM45686 IMU +- MS5611 Barometer +- RM3100 Compass +- 1x UART +- 2x CAN +- 8x PWM +- 1x RCIN +- 1x UBLOX NEO-F9P L1L5 GNSS + +## How to use +- To load the FC firmware on production Here4. User needs to first update Here4 through DroneCAN tool or Mission Planners DroneCAN config using FC firmware, which is .bin format. +- After first update, users can connect through the serial port. Check https://docs.cubepilot.org/user-guides/here-4/here-4-manual#pinout for breakout board pinouts. +- For enabling future updates to FC firmware, use flash bootloader through mission planner, and then user should be able to update firmware through Serial port. +- Please note that once the firmware is updated to FC, to rollback to GPS firmware user will need to update using APJ file available here https://github.com/CubePilot/GNSSPeriph-release/releases. + +## Pinout and Connectors + +https://docs.cubepilot.org/user-guides/here-4/here-4-manual#pinout diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/defaults.parm new file mode 100644 index 00000000000000..3cbd92be7659ee --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/defaults.parm @@ -0,0 +1,12 @@ +# setup for ProfiLED +SERVO9_FUNCTION 132 +SERVO10_FUNCTION 129 +NTF_LED_TYPES 512 +NTF_LED_BRIGHT 2 +NTF_LED_LEN 4 + +# Disable Logging by default +LOG_BACKEND_TYPE 0 + +# make node id to not conflict with ardupilot default +CAN_D1_UC_NODE 8 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/hwdef-bl.dat new file mode 100644 index 00000000000000..b83fa886e228f1 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/hwdef-bl.dat @@ -0,0 +1,65 @@ +# hw definition file for processing by chibios_hwdef.py +# for H757 bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H757xx + +define CORE_CM7 +define SMPS_PWR + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# crystal frequency +OSCILLATOR_HZ 24000000 + +# board ID for firmware load +APJ_BOARD_ID 1043 + +FLASH_SIZE_KB 2048 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# reserve space for flash storage in last 2 sectors +FLASH_RESERVE_END_KB 256 + +# the location where the bootloader will put the firmware +# the H757 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 256 + +# enable CAN support +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 +PD2 SLEEPCAN1 OUTPUT PUSHPULL SPEED_LOW LOW +PD3 TERMCAN1 OUTPUT LOW + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 +PB10 SLEEPCAN2 OUTPUT PUSHPULL SPEED_LOW LOW +PB11 TERMCAN2 OUTPUT LOW + +# board voltage +STM32_VDD 330U + +PB8 LED_SCK OUTPUT LOW +PB9 LED_DI OUTPUT HIGH + +define HAL_NO_MONITOR_THREAD + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define CAN_APP_NODE_NAME "com.cubepilot.here4fc" + +PB6 USART1_TX USART1 +PB7 USART1_RX USART1 + +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 + +# order of UARTs +SERIAL_ORDER USART1 UART8 + +# setup for blanking LEDs in bootloader +define AP_BOOTLOADER_CUSTOM_HERE4 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/hwdef.dat new file mode 100644 index 00000000000000..94d07648d6b5ee --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Here4FC/hwdef.dat @@ -0,0 +1,131 @@ +# hw definition file for processing by chibios_hwdef.py +# for H757 + +# MCU class and specific type +MCU STM32H7xx STM32H757xx + +define CORE_CM7 +define SMPS_PWR + +FLASH_SIZE_KB 2048 +APJ_BOARD_ID 1043 + + +# the location where the bootloader will put the firmware +# the H757 has 128k sectors +FLASH_BOOTLOADER_LOAD_KB 256 +FLASH_RESERVE_START_KB 256 + +# use last 2 pages for flash storage +# H757 has 16 pages of 128k each +STORAGE_FLASH_PAGE 14 +define HAL_STORAGE_SIZE 32768 + + +# crystal frequency +OSCILLATOR_HZ 24000000 + +CANFD_SUPPORTED 8 + +STDOUT_SERIAL SD6 +STDOUT_BAUDRATE 57600 + +PC6 USART6_TX USART6 + +PB6 USART1_TX USART1 GPIO(7) +PB7 USART1_RX USART1 GPIO(8) + +PD5 USART2_TX USART2 GPIO(9) +PD6 USART2_RX USART2 GPIO(10) + +PA11 UART4_RX UART4 +PA12 UART4_TX UART4 + +define GPIO_USART1_TX 7 +define GPIO_USART1_RX 8 +define GPIO_USART2_TX 9 +define GPIO_USART2_RX 10 + +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 + +SERIAL_ORDER USART1 UART4 UART8 USART2 + +define DRONEID_MODULE_PORT 2 +define DRONEID_MODULE_CHAN MAVLINK_COMM_0 + +define AP_UART_MONITOR_ENABLED 1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + + +# RCInput +PB15 TIM12_CH2 TIM12 RCININT PULLDOWN LOW + +# RCOutput +PE9 TIM1_CH1 TIM1 PWM(1) GPIO(50) BIDIR +PE11 TIM1_CH2 TIM1 PWM(2) GPIO(51) # shared with channel 1 +PE13 TIM1_CH3 TIM1 PWM(3) GPIO(52) BIDIR +PE14 TIM1_CH4 TIM1 PWM(4) GPIO(53) # shared with channel 3 +PC7 TIM3_CH2 TIM3 PWM(5) GPIO(54) BIDIR +PC8 TIM3_CH3 TIM3 PWM(6) GPIO(55) BIDIR +PC9 TIM3_CH4 TIM3 PWM(7) GPIO(56) BIDIR +PA3 TIM2_CH4 TIM2 PWM(8) GPIO(57) BIDIR + +# enable CAN support +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 +PD2 SLEEPCAN1 OUTPUT PUSHPULL SPEED_LOW LOW +PD3 TERMCAN1 OUTPUT HIGH GPIO(3) + +PE7 UART7_RX UART7 +PE8 UART7_TX UART7 + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 +PB10 SLEEPCAN2 OUTPUT PUSHPULL SPEED_LOW LOW +PB11 TERMCAN2 OUTPUT HIGH GPIO(4) + +define AP_OPENDRONEID_ENABLED 1 + +PB3 SPI3_SCK SPI3 +PB4 SPI3_MISO SPI3 +PB2 SPI3_MOSI SPI3 +PB1 MAG_CS CS + +SPIDEV rm3100 SPI3 DEVID1 MAG_CS MODE0 1*MHZ 1*MHZ +COMPASS RM3100 SPI:rm3100 false ROTATION_YAW_180 +define AP_RM3100_REVERSAL_MASK 4 + +# HOTSHOE pin repurposed for BATT Voltage Sens +PA1 BATT_VOLTAGE_SENS ADC1 SCALE(1) + +PA8 ICM_CS CS +PA9 BARO_CS CS +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +SPIDEV icm42688 SPI1 DEVID4 ICM_CS MODE3 2*MHZ 8*MHZ +SPIDEV ms5611 SPI1 DEVID3 BARO_CS MODE3 20*MHZ 20*MHZ + +BARO MS56XX SPI:ms5611 + +IMU Invensensev3 SPI:icm42688 ROTATION_YAW_270 + +define HAL_DEFAULT_INS_FAST_SAMPLE 5 + +# WS2812 LED +PB8 TIM4_CH3 TIM4 PWM(9) +PB9 TIM4_CH4 TIM4 PWM(10) + +PD11 GPS_TP1 OUTPUT LOW GPIO(5) +PD7 GPS_PPS INPUT PULLUP GPIO(6) +define CONFIGURE_PPS_PIN TRUE +define HAL_GPIO_PPS 6 + +PD12 GPS_SAFEBOOT_N INPUT FLOATING GPIO(11) +define GPIO_UBX_SAFEBOOT 11 +PD13 GPS_RESET_N INPUT FLOATING GPIO(12) +define GPIO_UBX_RESET 12 +define HAL_EARLY_WATCHDOG_INIT diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat index d1d9f2bae5a491..5f8cffae714cd1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat @@ -65,8 +65,6 @@ define HAL_DEVICE_THREAD_STACK 2048 define HAL_BARO_ALLOW_INIT_NO_BARO -define AP_PERIPH_HAVE_LED - define HAL_COMPASS_MAX_SENSORS 1 # GPS+MAG diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat index 85ee746142daea..d1e3894302df9c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat @@ -149,8 +149,6 @@ define HAL_OSD_TYPE_DEFAULT 1 # Font for OSD ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin -# -------reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 define HAL_GYROFFT_ENABLED 0 # --------------------- save flash ---------------------- diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat index 4c8b7c627e6643..ad7c9ee0344ded 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7/hwdef.dat @@ -164,6 +164,3 @@ define HAL_MSP_ENABLED 1 # need to probe external baros even 'though we're minimised: undef AP_BARO_PROBE_EXTERNAL_I2C_BUSES define AP_BARO_PROBE_EXTERNAL_I2C_BUSES 1 - -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7Mini/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7Mini/hwdef.dat index 561fc3011616a4..21f6b672626e0c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7Mini/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/KakuteF7Mini/hwdef.dat @@ -156,9 +156,6 @@ define STM32_PWM_USE_ADVANCED TRUE define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 - define HAL_MOUNT_ENABLED 0 # save some flash diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat index 78cd31eaaaf3d4..ad148e79bf6622 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat @@ -170,9 +170,4 @@ undef AP_BATTERY_SMBUS_ENABLED define AP_BATTERY_SMBUS_ENABLED 0 define HAL_PARACHUTE_ENABLED 0 -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 - - - define DEFAULT_NTF_LED_TYPES 487 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE/hwdef.dat index a69d9a94cf9ca6..ca85f8defc52cc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE/hwdef.dat @@ -170,8 +170,6 @@ define HAL_BATT_CURR_SCALE 66.7 define BOARD_RSSI_ANA_PIN 8 define HAL_DEFAULT_AIRSPEED_PIN 10 -# -------reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 define HAL_GYROFFT_ENABLED 0 # --------------------- save flash ---------------------- diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-Wing/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-Wing/hwdef.dat index 089fd07fdfebdc..9c942c302b9d98 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-Wing/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-Wing/hwdef.dat @@ -182,8 +182,6 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font0.bin define STM32_PWM_USE_ADVANCED TRUE -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 define HAL_GYROFFT_ENABLED 0 # save some flash diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat index effd88691850e2..650d1f960be605 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405/hwdef.dat @@ -160,9 +160,6 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font0.bin define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 - # save some flash include ../include/save_some_flash.inc include ../include/minimize_fpv_osd.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.dat new file mode 100644 index 00000000000000..5c8cb9539707f1 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef-bl.dat @@ -0,0 +1,49 @@ +# hw definition file for processing by chibios_pins.py +# for Matek H7A3 bootloader + +# MCU class and specific type +MCU STM32H7xx STM32H7A3xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_MatekH7A3 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +FLASH_SIZE_KB 2048 + +# bootloader starts at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +# the H7A3 has 8k sectors +FLASH_BOOTLOADER_LOAD_KB 32 + + +# order of UARTs (and USB). Allow bootloading on USB and telem1 +SERIAL_ORDER OTG1 UART7 + +# UART7 (telem1) +PE7 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA + +# PA10 IO-debug-console +PA11 OTG_HS_DM OTG1 +PA12 OTG_HS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# make sure Vsw is on during bootloader +PD10 PINIO1 OUTPUT LOW + +PE3 LED_BOOTLOADER OUTPUT LOW +define HAL_LED_ON 0 + +# Add CS pins to ensure they are high in bootloader +PC15 IMU1_CS CS +PB12 MAX7456_CS CS +PE11 IMU2_CS CS +PD4 EXT_CS1 CS +PE2 EXT_CS2 CS +PC13 IMU3_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat new file mode 100644 index 00000000000000..7892d581544716 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekH7A3/hwdef.dat @@ -0,0 +1,226 @@ +# hw definition file for processing by chibios_pins.py +# for Matek H7A3 + +# MCU class and specific type +MCU STM32H7xx STM32H7A3xx + +# board ID for firmware load +APJ_BOARD_ID AP_HW_MatekH7A3 + +# crystal frequency, setup to use external oscillator +OSCILLATOR_HZ 8000000 + +FLASH_SIZE_KB 2048 +env OPTIMIZE -Os + +# bootloader takes first 4 sectors +FLASH_RESERVE_START_KB 32 + +# ChibiOS system timer +STM32_ST_USE_TIMER 12 +define CH_CFG_ST_RESOLUTION 16 + +# USB +PA11 OTG_HS_DM OTG1 +PA12 OTG_HS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# SPI1 for IMU1 (MPU6000) +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PD7 SPI1_MOSI SPI1 +PC15 IMU1_CS CS + +# SPI2 for MAX7456 OSD +PB12 MAX7456_CS CS +PB13 SPI2_SCK SPI2 +PB14 SPI2_MISO SPI2 +PB15 SPI2_MOSI SPI2 + +# SPI3 - external +PB3 SPI3_SCK SPI3 +PB4 SPI3_MISO SPI3 +PB5 SPI3_MOSI SPI3 + +# external CS pins +PD4 EXT_CS1 CS +PE2 EXT_CS2 CS + +# SPI4 for IMU2 (ICM20602) +PE11 IMU2_CS CS +PE12 SPI4_SCK SPI4 +PE13 SPI4_MISO SPI4 +PE14 SPI4_MOSI SPI4 + +# add ICM42605 on SPI4 as a backup to replace ICM20602 +PC13 IMU3_CS CS + +# two I2C bus +I2C_ORDER I2C2 I2C1 + +# I2C1 +PB6 I2C1_SCL I2C1 +PB7 I2C1_SDA I2C1 + +# I2C2 +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +# ADC +PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC1 BATT_CURRENT_SENS ADC1 SCALE(1) +PA4 BATT2_VOLTAGE_SENS ADC1 SCALE(1) +PA7 BATT2_CURRENT_SENS ADC1 SCALE(1) + +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 10 +define HAL_BATT_CURR_PIN 11 +define HAL_BATT2_VOLT_PIN 18 +define HAL_BATT2_CURR_PIN 7 +define HAL_BATT_VOLT_SCALE 11.0 +define HAL_BATT_CURR_SCALE 40.0 +define HAL_BATT2_VOLT_SCALE 11.0 + +PC4 PRESSURE_SENS ADC1 SCALE(2) +define HAL_DEFAULT_AIRSPEED_PIN 4 + +PC5 RSSI_ADC ADC1 +#define BOARD_RSSI_ANA_PIN 8 + +# LED +# green LED1 marked as B/E +# blue LED0 marked as ACT +PE3 LED0 OUTPUT LOW GPIO(90) # blue +PE4 LED1 OUTPUT LOW GPIO(91) # green +define HAL_GPIO_A_LED_PIN 91 +define HAL_GPIO_B_LED_PIN 90 +define HAL_GPIO_LED_OFF 1 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 UART7 USART1 USART2 USART3 UART8 UART4 USART6 + +# USART1 (telem2) +PA10 USART1_RX USART1 +PA9 USART1_TX USART1 + +# USART2 (GPS1) +PD5 USART2_TX USART2 +PD6 USART2_RX USART2 + +# USART3 (GPS2) +PD9 USART3_RX USART3 +PD8 USART3_TX USART3 + +# UART4 (spare) +PB9 UART4_TX UART4 +PB8 UART4_RX UART4 + +# USART6 (RC input), SERIAL7 +PC7 TIM3_CH2 TIM3 RCININT PULLDOWN LOW +PC6 USART6_TX USART6 NODMA + +# as an alternative config setup the RX6 pin as a uart. This allows +# for bi-directional UART based receiver protocols such as FPort +# without any extra hardware +PC7 USART6_RX USART6 NODMA ALT(1) + +# UART7 (telem1) +PE7 UART7_RX UART7 +PE8 UART7_TX UART7 +PE10 UART7_CTS UART7 +PE9 UART7_RTS UART7 + +# UART8 (spare) +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 +PD3 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) + +# Motors +PB0 TIM8_CH2N TIM8 PWM(1) GPIO(50) +PB1 TIM8_CH3N TIM8 PWM(2) GPIO(51) +PA0 TIM5_CH1 TIM5 PWM(3) GPIO(52) +PA1 TIM5_CH2 TIM5 PWM(4) GPIO(53) +PA2 TIM5_CH3 TIM5 PWM(5) GPIO(54) +PA3 TIM5_CH4 TIM5 PWM(6) GPIO(55) +PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56) +PD13 TIM4_CH2 TIM4 PWM(8) GPIO(57) +PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58) +PD15 TIM4_CH4 TIM4 PWM(10) GPIO(59) +PE5 TIM15_CH1 TIM15 PWM(11) GPIO(60) +PE6 TIM15_CH2 TIM15 PWM(12) GPIO(61) +PA8 TIM1_CH1 TIM1 PWM(13) GPIO(62) # for WS2812 LED + +# Beeper +PA15 TIM2_CH1 TIM2 GPIO(32) ALARM + +# microSD support +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# GPIOs +PD10 PINIO1 OUTPUT GPIO(81) LOW +PD11 PINIO2 OUTPUT GPIO(82) LOW + +DMA_PRIORITY S* + +define HAL_STORAGE_SIZE 15360 + +# use last 4 pages for flash storage +# H7A3 has 256 pages of 8k each +STORAGE_FLASH_PAGE 252 + +# use double page size for flash storage to effectively +# give 16k pages +define AP_FLASH_STORAGE_DOUBLE_PAGE 1 + +# spi devices +SPIDEV icm42688 SPI1 DEVID1 IMU1_CS MODE3 2*MHZ 16*MHZ # Clock is 100Mhz so highest clock <= 24Mhz is 100Mhz/8 +SPIDEV mpu6000 SPI1 DEVID1 IMU1_CS MODE3 1*MHZ 4*MHZ +SPIDEV icm20602 SPI4 DEVID1 IMU2_CS MODE3 1*MHZ 4*MHZ +SPIDEV icm42605 SPI4 DEVID1 IMU3_CS MODE3 2*MHZ 16*MHZ +SPIDEV osd SPI2 DEVID4 MAX7456_CS MODE0 10*MHZ 10*MHZ + + +DMA_NOSHARE SPI1* SPI4* + +# SPI3 external connections +SPIDEV pixartflow SPI3 DEVID1 EXT_CS1 MODE3 2*MHZ 2*MHZ + +# no built-in compass, but probe the i2c bus for all possible +# external compass types +define ALLOW_ARM_NO_COMPASS +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_I2C_INTERNAL_MASK 0 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + +# two IMUs +# H743-V1, ICM20602, MPU6000 +# H743-V1.5/V2, ICM42605, MPU6000 +# H743-V3, ICM42688P, ICM42605 +IMU Invensensev3 SPI:icm42688 ROTATION_YAW_180 +IMU Invensensev3 SPI:icm42605 ROTATION_YAW_270 +IMU Invensense SPI:icm20602 ROTATION_ROLL_180_YAW_270 +IMU Invensense SPI:mpu6000 ROTATION_ROLL_180_YAW_270 +define HAL_DEFAULT_INS_FAST_SAMPLE 1 + +# DPS310 integrated on I2C2 bus, multiple possible choices for external barometer +BARO MS56XX I2C:0:0x77 +BARO DPS310 I2C:0:0x76 +BARO BMP280 I2C:0:0x76 + +define HAL_OS_FATFS_IO 1 + +# setup for OSD +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 1 +ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Serial/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Serial/hwdef-bl.dat new file mode 100644 index 00000000000000..b9005f84941a6a --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Serial/hwdef-bl.dat @@ -0,0 +1,2 @@ +include ../MatekL431/hwdef-bl.inc + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Serial/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Serial/hwdef.dat new file mode 100644 index 00000000000000..e9ace8923ddcb2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekL431-Serial/hwdef.dat @@ -0,0 +1,19 @@ +include ../MatekL431/hwdef.inc + +define HAL_PERIPH_ENABLE_SERIAL_OPTIONS + +# enable serial3 port with DMA +undef PB10 +undef PB11 +PB10 USART3_TX USART3 SPEED_HIGH +PB11 USART3_RX USART3 SPEED_HIGH + +# larger CAN pool for faster serial +undef HAL_CAN_POOL_SIZE +define HAL_CAN_POOL_SIZE 12000 + +define HAL_USE_ADC FALSE + +# make the UARTn numbers in parameters match the silkscreen +undef SERIAL_ORDER +SERIAL_ORDER EMPTY USART1 USART2 USART3 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef-bl.dat index f0486e5b9ca53b..c39441ecb0e56d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef-bl.dat @@ -50,7 +50,7 @@ PD9 USART3_RX USART3 PD11 USART3_CTS USART3 PD12 USART3_RTS USART3 -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters) +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef.dat index 8bec6df8eea62c..720f6b747c570a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-C3/hwdef.dat @@ -22,18 +22,17 @@ env OPTIMIZE -O2 # order of I2C buses I2C_ORDER I2C1 I2C2 -# now the UART order. These map to the hal.uartA to hal.uartF -# objects. If you use a shorter list then HAL_Empty::UARTDriver -# objects are substituted for later UARTs, or you can leave a gap by -# listing one or more of the uarts as EMPTY +# now the SERIALn order. These map to the hal.serial(n) objects. If you use a +# shorter list then HAL_Empty::UARTDriver objects are substituted for later +# UARTs, or you can leave a gap by listing one or more of the uarts as EMPTY # 1) SERIAL0: console (primary mavlink, usually USB) -# 2) SERIAL3: primary GPS -# 3) SERIAL1: telem1 -# 4) SERIAL2: telem2 +# 2) SERIAL1: telem1 +# 3) SERIAL2: telem2 +# 4) SERIAL3: primary GPS # 5) SERIAL4: GPS2 # 6) SERIAL5: extra UART (usually RTOS debug console) -# order of UARTs (and USB) +# order of SERIALn (incl. USB) SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 UART7 # UART for IOMCU diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V3/hwdef.dat index 4c95268d937f28..da84f273b9614c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V3/hwdef.dat @@ -24,18 +24,17 @@ USB_STRING_MANUFACTURER "ArduPilot" # order of I2C buses I2C_ORDER I2C3 I2C2 I2C1 -# now the UART order. These map to the hal.uartA to hal.uartF -# objects. If you use a shorter list then HAL_Empty::UARTDriver -# objects are substituted for later UARTs, or you can leave a gap by -# listing one or more of the uarts as EMPTY +# now the SERIALn order. These map to the hal.serial(n) objects. If you use a +# shorter list then HAL_Empty::UARTDriver objects are substituted for later +# UARTs, or you can leave a gap by listing one or more of the uarts as EMPTY # 1) SERIAL0: console (primary mavlink, usually USB) -# 2) SERIAL3: primary GPS -# 3) SERIAL1: telem1 -# 4) SERIAL2: telem2 +# 2) SERIAL1: telem1 +# 3) SERIAL2: telem2 +# 4) SERIAL3: primary GPS # 5) SERIAL4: GPS2 # 6) SERIAL5: extra UART (usually RTOS debug console) -# order of UARTs (and USB) +# order of SERIALn (incl. USB) SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 UART7 # UART for IOMCU diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V6/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V6/hwdef-bl.dat index a833003a0c44d0..7bd549b00aa7ee 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V6/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixPilot-V6/hwdef-bl.dat @@ -35,7 +35,7 @@ PD6 USART2_RX USART2 PD8 USART3_TX USART3 PD9 USART3_RX USART3 -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/PixSurveyA1-IND/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/PixSurveyA1-IND/hwdef.dat index 186fec11cecc93..00145bb34e56e7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/PixSurveyA1-IND/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/PixSurveyA1-IND/hwdef.dat @@ -23,18 +23,17 @@ env OPTIMIZE -O2 # order of I2C buses I2C_ORDER I2C3 I2C2 I2C1 -# now the UART order. These map to the hal.uartA to hal.uartF -# objects. If you use a shorter list then HAL_Empty::UARTDriver -# objects are substituted for later UARTs, or you can leave a gap by -# listing one or more of the uarts as EMPTY +# now the SERIALn order. These map to the hal.serial(n) objects. If you use a +# shorter list then HAL_Empty::UARTDriver objects are substituted for later +# UARTs, or you can leave a gap by listing one or more of the uarts as EMPTY # 1) SERIAL0: console (primary mavlink, usually USB) -# 2) SERIAL3: primary GPS -# 3) SERIAL1: telem1 -# 4) SERIAL2: telem2 +# 2) SERIAL1: telem1 +# 3) SERIAL2: telem2 +# 4) SERIAL3: primary GPS # 5) SERIAL4: GPS2 # 6) SERIAL5: extra UART (usually RTOS debug console) -# order of UARTs (and USB) +# order of SERIALn (incl. USB) SERIAL_ORDER OTG1 USART2 USART3 UART4 UART8 UART7 # UART for IOMCU diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C/hwdef.dat index 3103a5c72c75b1..4677c93b2ab37a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6C/hwdef.dat @@ -239,6 +239,9 @@ SPIDEV ramtron SPI2 DEVID1 FRAM_CS MODE3 8*MHZ 8*MHZ # 2 IMUs IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_90 IMU BMI055 SPI:bmi055_a SPI:bmi055_g ROTATION_PITCH_180 +# BMI055 is replaced by BMI088 on the lastest hardware revision +# They use the same CPU signal pins +IMU BMI088 SPI:bmi055_a SPI:bmi055_g ROTATION_PITCH_180 define HAL_DEFAULT_INS_FAST_SAMPLE 3 @@ -257,4 +260,4 @@ define HAL_OS_FATFS_IO 1 ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_f103_lowpolh.bin # enable support for dshot on iomcu ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin -define HAL_WITH_IO_MCU_DSHOT 1 +define HAL_WITH_IO_MCU_BIDIR_DSHOT 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/defaults.parm new file mode 100644 index 00000000000000..89ab4beebb035b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/defaults.parm @@ -0,0 +1,14 @@ +NET_ENABLED 1 +NET_OPTIONS 1 + +# enable hw flow control +UART1_RTSCTS 1 + +# swap TX and RX +UART1_OPTIONS 8 + +SCR_ENABLE 1 +SCR_VM_I_COUNT 1000000 + +WEB_ENABLE 1 +WEB_PORT 80 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/hwdef-bl.dat new file mode 100644 index 00000000000000..77e0825f9e4145 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/hwdef-bl.dat @@ -0,0 +1,35 @@ +include ../Pixhawk6X/hwdef-bl.dat + +undef UART7 +undef UART2 +undef UART3 + +# setup build for a peripheral bootloader +env AP_PERIPH 1 + +# board ID for firmware load +APJ_BOARD_ID AP_HW_PIXHAWK6X_PERIPH + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +PC1 ETH_MDC ETH1 +PA2 ETH_MDIO ETH1 +PC4 ETH_RMII_RXD0 ETH1 +PC5 ETH_RMII_RXD1 ETH1 +PG13 ETH_RMII_TXD0 ETH1 +PG12 ETH_RMII_TXD1 ETH1 +PB11 ETH_RMII_TX_EN ETH1 +PA7 ETH_RMII_CRS_DV ETH1 +PA1 ETH_RMII_REF_CLK ETH1 + +PG15 Ethernet_PWR_EN OUTPUT HIGH # disable power on ethernet + +define BOARD_PHY_ID MII_LAN8742A_ID +define BOARD_PHY_RMII + +include ../include/network_bootloader.inc + +# allow load from USB +SERIAL_ORDER OTG1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/hwdef.dat new file mode 100644 index 00000000000000..f2db5f603874f5 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X-PPPGW/hwdef.dat @@ -0,0 +1,45 @@ +include ../Pixhawk6X/hwdef.dat + +undef IOMCU_UART +undef USART6 +undef ROMFS +undef HAL_HAVE_SAFETY_SWITCH +undef HAL_WITH_IO_MCU_BIDIR_DSHOT +undef COMPASS +undef BARO + +define AP_ADVANCEDFAILSAFE_ENABLED 0 + + +# board ID for firmware load +APJ_BOARD_ID AP_HW_PIXHAWK6X_PERIPH + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +define AP_CAN_SLCAN_ENABLED 0 + +define HAL_PERIPH_ENABLE_NETWORKING +define HAL_PERIPH_ENABLE_SERIAL_OPTIONS + +define AP_NETWORKING_BACKEND_PPP 1 + +define HAL_NO_MONITOR_THREAD +define HAL_DISABLE_LOOP_DELAY + +define HAL_USE_RTC FALSE +define DISABLE_SERIAL_ESC_COMM TRUE + +define HAL_NO_RCIN_THREAD + +# use blue LED +define HAL_GPIO_PIN_LED HAL_GPIO_PIN_LED_BLUE + +undef HAL_OS_FATFS_IO + +undef SDMMC1 + +MAIN_STACK 0x2000 +PROCESS_STACK 0x6000 + +include ../include/network_PPPGW.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat index 1419ab5aa2e053..1cf5be3a9f7430 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat @@ -85,7 +85,6 @@ IOMCU_UART USART6 # uart6, RX only, RC input, if no IOMCU # PC7 USART6_RX USART6 -# ethernet (not implemented yet) PC1 ETH_MDC ETH1 PA2 ETH_MDIO ETH1 PC4 ETH_RMII_RXD0 ETH1 @@ -332,6 +331,11 @@ SPIDEV adis16470 SPI3 DEVID1 SP3_CS1 MODE3 1*MHZ 2*MHZ SPIDEV iim42652 SPI2 DEVID1 SP2_CS1 MODE3 2*MHZ 8*MHZ SPIDEV icm45686 SPI1 DEVID1 SP1_CS1 MODE3 2*MHZ 8*MHZ +# IMU devices for Holybro6X-45686 version +SPIDEV icm45686-3 SPI3 DEVID1 SP3_CS1 MODE3 2*MHZ 8*MHZ +SPIDEV icm45686-2 SPI2 DEVID1 SP2_CS1 MODE3 2*MHZ 8*MHZ +SPIDEV icm45686-1 SPI1 DEVID1 SP1_CS1 MODE3 2*MHZ 8*MHZ + # Holybro6X 3 IMUs IMU Invensensev3 SPI:icm42688 ROTATION_PITCH_180_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X) IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_180 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X) @@ -348,6 +352,11 @@ IMU ADIS1647x SPI:adis16470 ROTATION_ROLL_180 SP3_DRDY2 BOARD_MATCH(FMUV6_BOA IMU Invensensev3 SPI:iim42652 ROTATION_ROLL_180_YAW_270 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_REV6) IMU Invensensev3 SPI:icm45686 ROTATION_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_REV6) +# Holybro6X-45686 3 IMUs, the ones on SPI-2 and SPI-3 are isolated +IMU Invensensev3 SPI:icm45686-2 ROTATION_ROLL_180 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_45686) +IMU Invensensev3 SPI:icm45686-3 ROTATION_ROLL_180 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_45686) +IMU Invensensev3 SPI:icm45686-1 ROTATION_YAW_90 BOARD_MATCH(FMUV6_BOARD_HOLYBRO_6X_45686) + define HAL_DEFAULT_INS_FAST_SAMPLE 7 # enable RAMTROM parameter storage @@ -379,4 +388,4 @@ env BUILD_ABIN True # enable support for dshot on iomcu ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin -define HAL_WITH_IO_MCU_DSHOT 1 +define HAL_WITH_IO_MCU_BIDIR_DSHOT 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavIC/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavIC/hwdef-bl.dat new file mode 100644 index 00000000000000..ddbb1c3acbca7e --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavIC/hwdef-bl.dat @@ -0,0 +1,57 @@ +# HW definition file for Sierra-TrueNavIC + +# MCU class and specific type +MCU STM32L431 STM32L431xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID for firmware load +APJ_BOARD_ID 5302 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# Flash info +FLASH_RESERVE_START_KB 0 +FLASH_BOOTLOADER_LOAD_KB 36 +FLASH_SIZE_KB 256 + +# reserve some space for params +APP_START_OFFSET_KB 4 + +# a fault LED +PA1 LED_BOOTLOADER OUTPUT LOW # amber +define HAL_LED_ON 1 + +# enable CAN support +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 +PB1 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW +PB6 GPIO_CAN1_TERM OUTPUT PUSHPULL SPEED_LOW HIGH + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 + +# use a small bootloader timeout +define HAL_BOOTLOADER_TIMEOUT 1000 + +define HAL_USE_SERIAL FALSE +define HAL_NO_GPIO_IRQ +define HAL_USE_EMPTY_IO TRUE +define PORT_INT_REQUIRED_STACK 64 +define DMA_RESERVE_SIZE 0 + +MAIN_STACK 0x800 +PROCESS_STACK 0x800 + +define HAL_DISABLE_LOOP_DELAY + +# Add CS pins to ensure they are high in bootloader +PA8 BARO_CS CS + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define CAN_APP_NODE_NAME "in.sierraaerospace.TrueNavIC" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavIC/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavIC/hwdef.dat new file mode 100644 index 00000000000000..72f2c8418640f9 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavIC/hwdef.dat @@ -0,0 +1,120 @@ +# HW definition file for Sierra-TrueNavIC + +# MCU class and specific type +MCU STM32L431 STM32L431xx + +# bootloader starts firmware at 36k + 4k (STORAGE_FLASH) +FLASH_RESERVE_START_KB 40 +FLASH_SIZE_KB 256 + +# store parameters in pages 18 and 19 +STORAGE_FLASH_PAGE 18 +define HAL_STORAGE_SIZE 800 + +# ChibiOS system timer +STM32_ST_USE_TIMER 15 +define CH_CFG_ST_RESOLUTION 16 + +# board ID for firmware load +APJ_BOARD_ID 5302 + +# crystal frequency +OSCILLATOR_HZ 16000000 + +env AP_PERIPH 1 + +define HAL_NO_GPIO_IRQ +define SERIAL_BUFFERS_SIZE 512 +define PORT_INT_REQUIRED_STACK 64 + +define DMA_RESERVE_SIZE 0 + +# MAIN_STACK is stack for ISR handlers +MAIN_STACK 0x300 + +# PROCESS_STACK controls stack for main thread +PROCESS_STACK 0xA00 + +# save memory +define HAL_DISABLE_LOOP_DELAY +define HAL_GCS_ENABLED 0 +define HAL_NO_MONITOR_THREAD +define HAL_NO_LOGGING +define HAL_USE_ADC FALSE +define HAL_NO_RCIN_THREAD + +# we setup a small defaults.parm +define AP_PARAM_MAX_EMBEDDED_PARAM 256 + +# debugger support +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# --------------------- SPI1 RM3100+DPS310 ----------------------- +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PA8 BARO_CS CS + +# Baro probe +SPIDEV dps310 SPI1 DEVID3 BARO_CS MODE3 5*MHZ 5*MHZ +BARO DPS310 SPI:dps310 + +# QMC5883L on I2C2 +I2C_ORDER I2C2 + +PB13 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +define HAL_I2C_CLEAR_ON_TIMEOUT 0 +define HAL_I2C_INTERNAL_MASK 1 +COMPASS QMC5883L I2C:0:0xd false ROTATION_YAW_180 + +# ---------------------- CAN bus ------------------------- +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 +PB1 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW +PB6 GPIO_CAN1_TERM OUTPUT PUSHPULL SPEED_LOW LOW + +define HAL_CAN_POOL_SIZE 8000 + +# ---------------------- UARTs --------------------------- +SERIAL_ORDER EMPTY USART1 + +# USART1 for GPS +PA9 USART1_TX USART1 SPEED_HIGH +PA10 USART1_RX USART1 SPEED_HIGH + +# keep ROMFS uncompressed as we don't have enough RAM +# to uncompress the bootloader at runtime +env ROMFS_UNCOMPRESSED True + +# enable GPS and compass +define HAL_PERIPH_ENABLE_GPS +define GPS_MAX_RATE_MS 200 +define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO + +# disable dual GPS and GPS blending to save flash space +define GPS_MAX_RECEIVERS 1 +define GPS_MAX_INSTANCES 1 +define HAL_COMPASS_MAX_SENSORS 1 +define BARO_MAX_INSTANCES 1 +define HAL_PERIPH_GPS_PORT_DEFAULT 1 + +# GPS PPS +PA15 GPS_PPS_IN INPUT + +# PWM, WS2812 LED +PB10 TIM2_CH3 TIM2 PWM(1) + +define CAN_APP_NODE_NAME "in.sierraaerospace.TrueNavIC" + +# Enable GPS LDO +PC13 VDD_3V3_SENSORS_EN OUTPUT HIGH + +# do direct neopixel LED output to enable the 'rainbow' effect on startup +define HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY 0 +define HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY 8 +define DEFAULT_NTF_LED_TYPES 455 +# PA1 LED OUTPUT LOW GPIO(1) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef-bl.dat new file mode 100644 index 00000000000000..a36f9fc9b917a0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef-bl.dat @@ -0,0 +1,57 @@ +# hw definition file for processing by chibios_pins.py + +# MCU class and specific type +MCU STM32G4xx STM32G491xx + +FLASH_RESERVE_START_KB 0 +FLASH_BOOTLOADER_LOAD_KB 32 + +# reserve some space for params +APP_START_OFFSET_KB 4 + +FLASH_SIZE_KB 256 + +# board ID for firmware load +APJ_BOARD_ID 5301 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# crystal frequency +OSCILLATOR_HZ 16000000 + +define CH_CFG_ST_FREQUENCY 1000000 + +# blue LED +PA4 LED_BOOTLOADER OUTPUT HIGH +define HAL_LED_ON 1 + +# SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +define HAL_NO_GPIO_IRQ +define HAL_USE_EMPTY_IO TRUE +define HAL_NO_TIMER_THREAD +define HAL_NO_RCIN_THREAD +define DMA_RESERVE_SIZE 0 +define HAL_DISABLE_LOOP_DELAY +define HAL_USE_SERIAL FALSE + +# enable CAN support +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 +PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW +PA2 GPIO_CAN1_TERM OUTPUT PUSHPULL SPEED_LOW LOW + +# make bl baudrate match debug baudrate for easier debugging +define BOOTLOADER_BAUDRATE 57600 + +# use a smaller bootloader timeout +define HAL_BOOTLOADER_TIMEOUT 2500 + +# Add CS pins to ensure they are high in bootloader +PA9 RM3100_CS CS +PA10 DPS310_CS CS + +define CAN_APP_NODE_NAME "in.sierraaerospace.TrueNavPro-G4" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef.dat new file mode 100644 index 00000000000000..4aa860c48f92b5 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Sierra-TrueNavPro-G4/hwdef.dat @@ -0,0 +1,113 @@ +# hw definition file for processing by chibios_pins.py + +# MCU class and specific type +MCU STM32G4xx STM32G491xx + +FLASH_RESERVE_START_KB 36 + +STORAGE_FLASH_PAGE 16 +define HAL_STORAGE_SIZE 800 + +# board ID for firmware load +APJ_BOARD_ID 5301 + +# setup build for a peripheral firmware +env AP_PERIPH 1 + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# ChibiOS system timer +STM32_ST_USE_TIMER 15 +define CH_CFG_ST_RESOLUTION 16 + +define CH_CFG_ST_FREQUENCY 1000000 + +FLASH_SIZE_KB 256 + +# order of UARTs +SERIAL_ORDER EMPTY USART2 + +# USART2, GPS +PB3 USART2_TX USART2 SPEED HIGH +PB4 USART2_RX USART2 SPEED HIGH + +# USART1, debug, disabled to save flash +# PB6 USART1_TX USART1 +# PB7 USART1_RX USART1 + +# SWD debugging +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# one SPI bus +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 + +# SPI CS +PA9 RM3100_CS CS +PA10 DPS310_CS CS + +# GPS PPS +PA15 GPS_PPS_IN INPUT + +# Baro probe +SPIDEV dps310 SPI1 DEVID3 DPS310_CS MODE3 5*MHZ 5*MHZ +BARO DPS310 SPI:dps310 + +# Mag probe +SPIDEV rm3100 SPI1 DEVID1 RM3100_CS MODE0 1*MHZ 1*MHZ +COMPASS RM3100 SPI:rm3100 false ROTATION_YAW_180 +define AP_RM3100_REVERSAL_MASK 7 + +define HAL_USE_ADC FALSE +define STM32_ADC_USE_ADC1 FALSE +define HAL_DISABLE_ADC_DRIVER TRUE +define HAL_NO_GPIO_IRQ + +# avoid RCIN thread to save memory +define HAL_NO_RCIN_THREAD + +define HAL_USE_RTC FALSE +define DMA_RESERVE_SIZE 0 +define HAL_DISABLE_LOOP_DELAY +define HAL_NO_MONITOR_THREAD + +define HAL_DEVICE_THREAD_STACK 768 + +# enable CAN support +PA11 CAN1_RX CAN1 +PA12 CAN1_TX CAN1 +PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW +PA2 GPIO_CAN1_TERM OUTPUT PUSHPULL SPEED_LOW LOW + +# disable dual GPS and GPS blending to save flash space +define GPS_MAX_RECEIVERS 1 +define GPS_MAX_INSTANCES 1 +define HAL_COMPASS_MAX_SENSORS 1 + +# GPS+MAG+BARO+LEDs +define HAL_PERIPH_ENABLE_GPS +define HAL_PERIPH_ENABLE_MAG +define HAL_PERIPH_ENABLE_BARO +define HAL_PERIPH_NEOPIXEL_CHAN_WITHOUT_NOTIFY 0 +define HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY 8 +define DEFAULT_NTF_LED_TYPES 455 + +# single baro +define BARO_MAX_INSTANCES 1 + +# GPS on 1st port +define HAL_PERIPH_GPS_PORT_DEFAULT 1 + +# keep ROMFS uncompressed as we don't have enough RAM +# to uncompress the bootloader at runtime +env ROMFS_UNCOMPRESSED True + +# PWM, WS2812 LED +PA3 TIM2_CH4 TIM2 PWM(1) + +DMA_NOSHARE USART2* + +define CAN_APP_NODE_NAME "in.sierraaerospace.TrueNavPro-G4" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat index 1ddcd7c9eb8aa9..f6842833c8d386 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat @@ -169,18 +169,8 @@ PC13 PINIO1 OUTPUT GPIO(81) LOW define STM32_PWM_USE_ADVANCED TRUE -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 define HAL_WITH_DSP FALSE -# save some flash - -# disable SMBUS and fuel battery monitors to save flash -define AP_BATTMON_SMBUS_ENABLE 0 -define AP_BATTMON_FUEL_ENABLE 0 -define AP_BATTMON_SYNTHETIC_CURRENT_ENABLED 0 -define HAL_BATTMON_INA2XX_ENABLED 0 - # disable parachute and sprayer to save flash define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat index e5a1777c032cca..69124a38f631f3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SuccexF4/hwdef.dat @@ -128,9 +128,6 @@ ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 -# reduce max size of embedded params for apj_tool.py -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 - # minimal drivers to reduce flash usage include ../include/minimize_fpv_osd.inc include ../include/no_bootloader_DFU.inc diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm index 74d6255c6339bd..1207d066588786 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/Swan-K1/defaults.parm @@ -38,16 +38,16 @@ ARMING_RUDDER 2 ARSPD_TUBE_ORDR 0 ARSPD_USE 1 -ALT_HOLD_RTL 3000 -ARSPD_FBW_MAX 25 -ARSPD_FBW_MIN 8 +RTL_ALTITUDE 30.00 +AIRSPEED_MAX 25 +AIRSPEED_MIN 8 ARSPD_RATIO 1.9 -LIM_ROLL_CD 4000 +ROLL_LIMIT_DEG 40.00 # the HWing can become unstable at high speeds. Limit pitch angle # to prevent overspeed -LIM_PITCH_MAX 4500 -LIM_PITCH_MIN -1500 +PTCH_LIM_MAX_DEG 45.00 +PTCH_LIM_MIN_DEG -15.00 # setup left 3-pos switch for QHOVER, QLOITER and FBWA FLTMODE1 18 @@ -70,7 +70,7 @@ INS_HNTCH_REF 0.4560 KFF_RDDRMIX 0 # reasonable attitude limits -LIM_PITCH_MAX 4000 +PTCH_LIM_MAX_DEG 40.00 LOG_FILE_DSRMROT 1 # lower logging rate a bit to make logs faster to fetch @@ -133,7 +133,7 @@ RC_OPTIONS 32 TECS_LAND_ARSPD 8 # 16 m/s target airspeed in auto modes -TRIM_ARSPD_CM 1600 +AIRSPEED_CRUISE 16.00 TRIM_THROTTLE 29 # 2nd lane tends to be better diff --git a/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat index f5e2451e2d4538..6cb3544f3ccedb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/VRUBrain-v51/hwdef.dat @@ -136,8 +136,6 @@ FLASH_RESERVE_START_KB 64 # enable RAMTROM parameter storage #define HAL_WITH_RAMTRON 1 -define AP_PARAM_MAX_EMBEDDED_PARAM 1024 - # enable FAT filesystem define HAL_OS_FATFS_IO 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/README.md b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/README.md new file mode 100644 index 00000000000000..da550d2dc7e84d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/README.md @@ -0,0 +1,289 @@ +# A6Ultra Flight Controller + +The A6Ultra flight controller is manufactured and sold by [YJUAV](http://www.yjuav.net). + +## Features + + - STM32H743 microcontroller + - Onboard Flash: 2048Mbits + - Three IMUs: ICM42688,ICM42688,IIM42652 + - Internal ITS8310 I2C magnetometer + - Internal two DPS310 SPI barometer + - Internal RGB LED + - MicroSD card slot port + - 2 Analog power ports + - 5 UARTs and 1 USB ports + - 3 I2C and 2 CAN ports + - 13 PWM output ports + - Safety switch port + - External SPI port + - Buzzer port + - RC IN port + +## Pinout +![YJUAV_A6Ultra Board](YJUAV_A6Ultra-pinout.jpg "YJUAV_A6Ultra") + + +## Connectors + +**POWER1 ADC** + +| Pin | Signal | Volt | +| :--: | :-------------: | :---: | +| 1 | VCC_IN | +5V | +| 2 | VCC_IN | +5V | +| 3 | BAT_CURRENT_ADC | +3.3V | +| 4 | BAT_VOLTAGE_ADC | +3.3V | +| 5 | GND | GND | +| 6 | GND | GND | + +**POWER2 ADC** + +| Pin | Signal | Volt | +| :--: | :-------------: | :---: | +| 1 | VCC_IN | +5V | +| 2 | VCC_IN | +5V | +| 3 | BAT2_CURRENT_ADC | +3.3V | +| 4 | BAT2_VOLTAGE_ADC | +3.3V | +| 5 | GND | GND | +| 6 | GND | GND | + +**TELEM1** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | USART2_TX | +3.3V | +| 3 | USART2_RX | +3.3V | +| 4 | CTS | +3.3V | +| 5 | RTS | +3.3V | +| 6 | GND | GND | + +**TELEM2** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | USART6_TX | +3.3V | +| 3 | USART6_RX | +3.3V | +| 4 | CTS | +3.3V | +| 5 | RTS | +3.3V | +| 6 | GND | GND | + +**ADC** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | ADC_3V3 | +3.3V | +| 3 | ADC_6V6 | +6.6V | +| 4 | GND | GND | + +**SPI** + +| Pin | Signal | Volt | +| :--: | :------: | :---: | +| 1 | VCC | +5V | +| 2 | SPI_SCK | +3.3V | +| 3 | SPI_MISO | +3.3V | +| 4 | SPI_MOSI | +3.3V | +| 5 | SPI_CS | +3.3V | +| 6 | GND | GND | + +**I2C** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | I2C4_SCL | +3.3V | +| 3 | I2C4_SDA | +3.3V | +| 4 | GND | GND | + +**CAN1** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | VCC | +5V | +| 2 | CAN1_P | +3.3V | +| 3 | CAN1_N | +3.3V | +| 4 | GND | GND | + +**CAN2** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | VCC | +5V | +| 2 | CAN2_P | +3.3V | +| 3 | CAN2_N | +3.3V | +| 4 | GND | GND | + +**GPS1** + +| Pin | Signal | Volt | +| :--: | :-----: | :---: | +| 1 | VCC | +5V | +| 2 | USART3_TX | +3.3V | +| 3 | USART3_RX | +3.3V | +| 4 | I2C2_SCL | +3.3V | +| 5 | I2C2_SDA | +3.3V | +| 6 | GND | GND | + +**GPS2&SAFETY** + +| Pin | Signal | Volt | +| :--: | :-----------: | :---: | +| 1 | VCC | +5V | +| 2 | USART1_TX | +3.3V | +| 3 | USART1_RX | +3.3V | +| 4 | I2C3_SCL | +3.3V | +| 5 | I2C3_SDA | +3.3V | +| 6 | SAFETY_SW | +3.3V | +| 7 | SAFETY_SW_LED | +3.3V | +| 8 | 3V3_OUT | +3.3V | +| 9 | BUZZER | +3.3V | +| 10 | GND | GND | + +**DEBUG** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | VCC | +5V | +| 2 | UART7_TX | +3.3V | +| 3 | UART7_RX | +3.3V | +| 4 | SWDIO | +3.3V | +| 5 | SWCLK | +3.3V | +| 6 | GND | GND | + +**SAFETY** + +| Pin | Signal | Volt | +| :--: | :-----------: | :---: | +| 1 | 3V3_OUT | +3.3V | +| 2 | SAFETY_SW | +3.3V | +| 3 | SAFETY_SW_LED | +3.3V | +| 4 | UART8_TX(SBUS_OUT) | +3.3V | +| 5 | RSSI | +3.3V | +| 6 | GND | GND | + +**USB EX** + +| Pin | Signal | Volt | +| :--: | :----: | :---: | +| 1 | VCC_IN | +5V | +| 2 | DM | +3.3V | +| 3 | DP | +3.3V | +| 4 | GND | GND | + +## UART Mapping + + - SERIAL0 -> USB (OTG1) + - SERIAL1 -> USART2 (Telem1) + - SERIAL2 -> USART6 (Telem2) + - SERIAL3 -> USART3 (GPS1) + - SERIAL4 -> USART1 (GPS2) + - SERIAL5 -> UART8 (USER) TX only, normally used for SBUS_OUT with protocol change + - SERIAL6 -> UART7 (USER/Debug), NODMA + - SERIAL7 -> USB2 (OTG2) + +## RC Input + +The RCIN pin is mapped to a timer input instead of the UART, and can be used for all ArduPilot supported receiver protocols, except CRSF/ELRS and SRXL2 which require a true UART connection. However, FPort, when connected in this manner, can provide RC without telemetry. + +To allow CRSF and embedded telemetry available in Fport, CRSF, and SRXL2 receivers, a full UART must be used. For example, UART1 can have its protocol changed from the default GPS protocol for GPS2 to RX input protocol: + +With this option, SERIAL4_PROTOCOL must be set to “23”, and: + +PPM is not supported. + +SBUS/DSM/SRXL connects to the RX1 pin. + +FPort requires connection to TX1 and RX1. See FPort Receivers. + +CRSF also requires a TX1 connection, in addition to RX1, and automatically provides telemetry. + +SRXL2 requires a connection to TX1 and automatically provides telemetry. Set SERIAL4_OPTIONS to “4”. + +Any UART can be used for RC system connections in ArduPilot also, and is compatible with all protocols except PPM. See Radio Control Systems for details. + +## PWM Output + +The A6Ultra supports up to 13 PWM outputs,support all PWM protocols as well as DShot. All 13 PWM outputs have GND on the bottom row, 5V on the middle row and Signal on the top row. + +The 13 PWM outputs are in 4 groups: + + - PWM 1, 2, 3 and 4 in group1 + - PWM 5, 6, 7 and 8 in group2 + - PWM 9, 10, 11 in group3 + - PWM 12, 13 in group4 + +Channels 1-8 support bi-directional Dshot. +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. + +## GPIOs + +All 13 PWM channels can be used for GPIO functions (relays, buttons, RPM etc). + +The pin numbers for these PWM channels in ArduPilot are shown below: + +| PWM Channels | Pin | PWM Channels | Pin | +| ------------ | ---- | ------------ | ---- | +| PWM1 | 50 | PWM8 | 57 | +| PWM2 | 51 | PWM9 | 58 | +| PWM3 | 52 | PWM10 | 59 | +| PWM4 | 53 | PWM11 | 60 | +| PWM5 | 54 | PWM12 | 61 | +| PWM6 | 55 | PWM13 | 62 | +| PWM7 | 56 | | | + +## Analog inputs + +The A6Ultra flight controller has 7 Analog inputs + + - ADC Pin4 -> Battery Current + - ADC Pin2 -> Battery Voltage + - ADC Pin16 -> Battery2 Current + - ADC Pin12 -> Battery2 Voltage + - ADC Pin8 -> ADC 3V3 Sense + - ADC Pin10 -> ADC 6V6 Sense + - ADC Pin11 -> RSSI voltage monitoring + + ## Battery Monitor Configuration + +The board has voltage and current sensor inputs on the POWER1_ADC and POWER2_ADC connector. + +The correct battery setting parameters are: + +Enable Battery1 monitor: + +BATT_MONITOR 4 + +BATT_VOLT_PIN 2 + +BATT_CURR_PIN 4 + +BATT_VOLT_MULT 21.0 (may need adjustment if supplied monitor is not used) + +BATT_AMP_PERVLT 34.6 (may need adjustment if supplied monitor is not used) + +Enable Battery2 monitor: +BATT2_MONITOR 4 + +BATT2_VOLT_PIN 12 + +BATT2_CURR_PIN 16 + +BATT2_VOLT_MULT 21.0 (may need adjustment if supplied monitor is not used) + +BATT2_AMP_PERVLT 34.6 (may need adjustment if supplied monitor is not used) + +## Build the FC + +./waf configure --board YJUAV_A6Ultra +./waf copter + +The compiled firmware is located in folder **"build/YJUAV_A6Ultra/bin/arducopter.apj"**. + + +## Loading Firmware + +The A6Ultra flight controller comes pre-installed with an ArduPilot compatible bootloader, allowing the loading of *.apj firmware files with any ArduPilot compatible ground station. \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/YJUAV_A6Ultra-pinout.jpg b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/YJUAV_A6Ultra-pinout.jpg new file mode 100644 index 00000000000000..44e0a5d828935f Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/YJUAV_A6Ultra-pinout.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/defaults.parm new file mode 100644 index 00000000000000..45e17a29a301bd --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/defaults.parm @@ -0,0 +1,4 @@ +# setup the heater temperature to 45 degree +BRD_HEAT_TARG 45 + +CAN_P1_DRIVER 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/hwdef-bl.dat new file mode 100644 index 00000000000000..a376a5c9e54662 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/hwdef-bl.dat @@ -0,0 +1,52 @@ +# hw definition file for processing by chibios_hwdef.py +# for YJUAV_A6Ultra board + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID for firmware load +APJ_BOARD_ID 1144 + +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 + +# order of UARTs (and USB). Allow bootloading on USB and Debug +SERIAL_ORDER OTG1 UART7 + +# UART7 DEBUG +PE7 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA +define BOOTLOADER_DEBUG SD7 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +PE15 LED_RED OUTPUT OPENDRAIN HIGH # red +PD10 LED_BOOTLOADER OUTPUT OPENDRAIN HIGH # green +PG0 LED_ACTIVITY OUTPUT OPENDRAIN HIGH # blue +define HAL_LED_ON 0 + +# Add CS pins to ensure they are high in bootloader +PE4 IMU1_CS CS +PF3 IMU2_CS CS +PF4 IMU3_CS CS +PE10 FRAM_CS CS +PE9 BAROMETER1_CS CS +PE3 BAROMETER2_CS CS +PC15 RESERVE_CS CS + +# Extra SPI CS +PE5 EXT_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/hwdef.dat new file mode 100644 index 00000000000000..6d2d24f2a606f3 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/YJUAV_A6Ultra/hwdef.dat @@ -0,0 +1,256 @@ +# hw definition file for processing by chibios_hwdef.py for YJUAV_A6Ultra + +# MCU class and specific type +MCU STM32H7xx STM32H743xx + +# crystal frequency +OSCILLATOR_HZ 16000000 + +# board ID for firmware load +APJ_BOARD_ID 1144 + +env OPTIMIZE -O2 + +# ChibiOS system timer +STM32_ST_USE_TIMER 5 + +FLASH_SIZE_KB 2048 + +# bootloader takes first sector +FLASH_RESERVE_START_KB 128 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART2 USART6 USART3 USART1 UART8 UART7 OTG2 + +# now we define the pins that USB is connected on +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# USART2 TELEM1 +PD6 USART2_RX USART2 +PD5 USART2_TX USART2 +PD3 USART2_CTS USART2 +PD4 USART2_RTS USART2 + +# USART6 TELEM2 +PC7 USART6_RX USART6 +PC6 USART6_TX USART6 +PG13 USART6_CTS USART6 +PG8 USART6_RTS USART6 + +# USART3 GPS1 +PD9 USART3_RX USART3 +PD8 USART3_TX USART3 + +# USART1 GPS2 +PB7 USART1_RX USART1 +PA9 USART1_TX USART1 + +# SBUS, DSM port +PE0 UART8_RX UART8 +PE1 UART8_TX UART8 + +# UART7 DEBUG +PE7 UART7_RX UART7 NODMA +PE8 UART7_TX UART7 NODMA + +# these are the pins for SWD debugging with a STlinkv2 or black-magic probe +PA13 JTMS-SWDIO SWD +PA14 JTCK-SWCLK SWD + +# SPI1 -IMU1 +PB3 SPI1_SCK SPI1 +PB4 SPI1_MISO SPI1 +PD7 SPI1_MOSI SPI1 + +# SPI6 -IMU2 +PA5 SPI6_SCK SPI6 +PA6 SPI6_MISO SPI6 +PA7 SPI6_MOSI SPI6 + +# SPI5 -IMU3 +PF7 SPI5_SCK SPI5 +PF8 SPI5_MISO SPI5 +PF9 SPI5_MOSI SPI5 + +# SPI4 -Extra SPI +PE12 SPI4_SCK SPI4 +PE13 SPI4_MISO SPI4 +PE6 SPI4_MOSI SPI4 + +# sensors cs +PE4 IMU1_CS CS +PF3 IMU2_CS CS +PF4 IMU3_CS CS +PE10 FRAM_CS CS +PE9 BAROMETER1_CS CS +PE3 BAROMETER2_CS CS +PC15 RESERVE_CS CS + +# Extra SPI CS +PE5 EXT_CS CS + +# I2C buses +PB8 I2C1_SCL I2C1 +PB9 I2C1_SDA I2C1 + +PB10 I2C2_SCL I2C2 +PB11 I2C2_SDA I2C2 + +PH7 I2C3_SCL I2C3 +PH8 I2C3_SDA I2C3 + +PF14 I2C4_SCL I2C4 +PF15 I2C4_SDA I2C4 + +# order of I2C buses +I2C_ORDER I2C1 I2C2 I2C3 I2C4 + +NODMA I2C* +define STM32_I2C_USE_DMA FALSE +define HAL_I2C_INTERNAL_MASK 1 + +# PWM channels +PA15 TIM2_CH1 TIM2 PWM(1) GPIO(50) BIDIR +PA1 TIM2_CH2 TIM2 PWM(2) GPIO(51) +PA2 TIM2_CH3 TIM2 PWM(3) GPIO(52) BIDIR +PA3 TIM2_CH4 TIM2 PWM(4) GPIO(53) +PA8 TIM1_CH1 TIM1 PWM(5) GPIO(54) BIDIR +PE11 TIM1_CH2 TIM1 PWM(6) GPIO(55) +PA10 TIM1_CH3 TIM1 PWM(7) GPIO(56) BIDIR +PE14 TIM1_CH4 TIM1 PWM(8) GPIO(57) +PB5 TIM3_CH2 TIM3 PWM(9) GPIO(58) +PB0 TIM3_CH3 TIM3 PWM(10) GPIO(59) +PB1 TIM3_CH4 TIM3 PWM(11) GPIO(60) +PH6 TIM12_CH1 TIM12 PWM(12) GPIO(61) NODMA +PH9 TIM12_CH2 TIM12 PWM(13) GPIO(62) NODMA + +# PWM output for buzzer +PD14 TIM4_CH3 TIM4 GPIO(77) ALARM + +# RC input +PI7 TIM8_CH3 TIM8 RCININT PULLUP LOW + +# Analog in +PC4 BATT_CURRENT_SENS ADC1 SCALE(1) +PF11 BATT_VOLTAGE_SENS ADC1 SCALE(1) + +PA0 BATT2_CURRENT_SENS ADC1 SCALE(1) +PC2 BATT2_VOLTAGE_SENS ADC1 SCALE(1) + +# Analog sys 5v +PF12 VDD_5V_SENS ADC1 SCALE(2) + +# ADC3.3/ADC6.6 +PC5 SPARE1_ADC1 ADC1 SCALE(1) +PC0 SPARE2_ADC1 ADC1 SCALE(2) + +PC1 RSSI_IN ADC1 SCALE(1) + +PC3 FMU_SERVORAIL_VCC_SENS ADC1 SCALE(3) + +# CAN bus +PD0 CAN1_RX CAN1 +PD1 CAN1_TX CAN1 + +PB12 CAN2_RX CAN2 +PB13 CAN2_TX CAN2 + +# GPIOs +PC13 HEATER_EN OUTPUT LOW GPIO(80) +define HAL_HEATER_GPIO_PIN 80 +define HAL_HAVE_IMU_HEATER 1 + +# define default battery setup +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 2 +define HAL_BATT_CURR_PIN 4 +define HAL_BATT_VOLT_SCALE 21 +define HAL_BATT_CURR_SCALE 34.6 + +# batt2 default disable +define HAL_BATT2_VOLT_PIN 12 +define HAL_BATT2_CURR_PIN 16 +define HAL_BATT2_VOLT_SCALE 21 +define HAL_BATT2_CURR_SCALE 34.6 + +#analog rssi pin (also could be used as analog airspeed input) +# PC1 +define BOARD_RSSI_ANA_PIN 11 + +# enable pins +PC14 VDD_3V3_SENSORS_EN OUTPUT LOW + +# red LED marked as B/E +PE15 LED_R1 OUTPUT OPENDRAIN HIGH GPIO(0) +PD10 LED_G1 OUTPUT OPENDRAIN LOW GPIO(1) +PG0 LED_B1 OUTPUT OPENDRAIN HIGH GPIO(2) + +define HAL_GPIO_A_LED_PIN 0 +define HAL_GPIO_B_LED_PIN 1 +define HAL_GPIO_C_LED_PIN 2 + +define HAL_GPIO_LED_ON 0 +define HAL_GPIO_LED_OFF 1 + +# use pixracer style 3-LED indicators +define HAL_HAVE_PIXRACER_LED + +# allow to have have a dedicated safety switch pin +define HAL_HAVE_SAFETY_SWITCH 1 +PB15 LED_SAFETY OUTPUT +PA4 SAFETY_IN INPUT PULLDOWN + +# SPI devices +SPIDEV imu1 SPI6 DEVID1 IMU1_CS MODE3 2*MHZ 8*MHZ +SPIDEV imu2 SPI6 DEVID2 IMU2_CS MODE3 2*MHZ 8*MHZ +SPIDEV imu3 SPI1 DEVID1 IMU3_CS MODE3 2*MHZ 8*MHZ +SPIDEV dps310_1 SPI1 DEVID2 BAROMETER1_CS MODE3 5*MHZ 5*MHZ +SPIDEV dps310_2 SPI5 DEVID1 BAROMETER2_CS MODE3 5*MHZ 5*MHZ +SPIDEV ramtron SPI5 DEVID2 FRAM_CS MODE3 8*MHZ 8*MHZ + +# IMU1 +IMU Invensense SPI:imu1 ROTATION_NONE +IMU Invensensev3 SPI:imu1 ROTATION_NONE + +# IMU2 +IMU Invensense SPI:imu2 ROTATION_NONE +IMU Invensensev3 SPI:imu2 ROTATION_NONE + +# IMU3 +IMU Invensense SPI:imu3 ROTATION_NONE +IMU Invensensev3 SPI:imu3 ROTATION_NONE + +define HAL_DEFAULT_INS_FAST_SAMPLE 3 + +# baro +BARO DPS310 SPI:dps310_1 +BARO DPS310 SPI:dps310_2 + +# compass +define HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE +define HAL_PROBE_EXTERNAL_I2C_COMPASSES + +COMPASS IST8310 I2C:ALL_EXTERNAL:0x0E true ROTATION_ROLL_180_YAW_90 +COMPASS IST8310 I2C:0:0x0E false ROTATION_YAW_180 + +# microSD support +PC8 SDMMC1_D0 SDMMC1 +PC9 SDMMC1_D1 SDMMC1 +PC10 SDMMC1_D2 SDMMC1 +PC11 SDMMC1_D3 SDMMC1 +PC12 SDMMC1_CK SDMMC1 +PD2 SDMMC1_CMD SDMMC1 + +# enable FAT filesystem support (needs a microSD defined via SDMMC) +define HAL_OS_FATFS_IO 1 + +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + +# enable RAMTROM parameter storage +define HAL_STORAGE_SIZE 32768 +define HAL_WITH_RAMTRON 1 + +DMA_PRIORITY SPI1* SPI6* TIM*UP* +DMA_NOSHARE SPI1* SPI6* TIM*UP* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c index 590855bd59ed46..f1f735ab8f3532 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/bouncebuffer.c @@ -21,23 +21,6 @@ #include #include "bouncebuffer.h" -#if defined(STM32H7) -// always use a bouncebuffer on H7, to ensure alignment and padding -#define IS_DMA_SAFE(addr) false -#elif defined(STM32F732xx) -// always use bounce buffer on F732 -#define IS_DMA_SAFE(addr) false -#elif defined(STM32F7) -// on F76x we only consider first half of DTCM memory as DMA safe, 2nd half is used as fast memory for EKF -// on F74x we only have 64k of DTCM -#define IS_DMA_SAFE(addr) ((((uint32_t)(addr)) & ((0xFFFFFFFF & ~(64*1024U-1)) | 1U)) == 0x20000000) -#elif defined(STM32F1) -#define IS_DMA_SAFE(addr) true -#else -// this checks an address is in main memory and 16 bit aligned -#define IS_DMA_SAFE(addr) ((((uint32_t)(addr)) & 0xF0000001) == 0x20000000) -#endif - // Enable when trying to check if you are not just listening yourself #define ENABLE_ECHO_SAFE 0 @@ -64,7 +47,7 @@ void bouncebuffer_init(struct bouncebuffer_t **bouncebuffer, uint32_t prealloc_b */ bool bouncebuffer_setup_read(struct bouncebuffer_t *bouncebuffer, uint8_t **buf, uint32_t size) { - if (!bouncebuffer || IS_DMA_SAFE(*buf)) { + if (!bouncebuffer || mem_is_dma_safe(*buf, size, bouncebuffer->on_axi_sram)) { // nothing needs to be done return true; } @@ -113,7 +96,7 @@ void bouncebuffer_finish_read(struct bouncebuffer_t *bouncebuffer, const uint8_t */ bool bouncebuffer_setup_write(struct bouncebuffer_t *bouncebuffer, const uint8_t **buf, uint32_t size) { - if (!bouncebuffer || IS_DMA_SAFE(*buf)) { + if (!bouncebuffer || mem_is_dma_safe(*buf, size, bouncebuffer->on_axi_sram)) { // nothing needs to be done return true; } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h index f673dcc4b93f63..d71f13ea412b79 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h @@ -708,8 +708,10 @@ extern "C" { * @brief Threads descriptor structure extension. * @details User fields added to the end of the @p thread_t structure. */ +#ifndef CH_CFG_THREAD_EXTRA_FIELDS #define CH_CFG_THREAD_EXTRA_FIELDS \ /* Add threads custom fields here.*/ +#endif /** * @brief Threads initialization hook. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk index 821fcf0981229a..893b358f549685 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk @@ -68,10 +68,6 @@ ifeq ($(USE_FATFS),yes) include $(CHIBIOS)/os/various/fatfs_bindings/fatfs.mk endif -ifeq ($(USE_LWIP),yes) -include $(CHIBIOS)/os/various/lwip_bindings/lwip.mk -endif - # # Build global options ############################################################################## @@ -149,10 +145,6 @@ CSRC += $(CHIBIOS)/os/various/scsi_bindings/lib_scsi.c \ $(CHIBIOS)/os/hal/src/hal_usb_msd.c endif -ifeq ($(USE_LWIP),yes) -CSRC += $(CHIBIOS)/os/various/evtimer.c -endif - # $(TESTSRC) \ # test.c ifneq ($(CRASHCATCHER),) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c index 1e7fae3d282e99..3f81cc9ce1ef99 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/flash.c @@ -122,9 +122,14 @@ static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(32), KB(32), KB(32 #define STM32_FLASH_NPAGES 1 #define STM32_FLASH_NBANKS 1 #define STM32_FLASH_FIXED_PAGE_SIZE 128 +#elif defined(STM32H7A3xx) +#define STM32_FLASH_NPAGES (BOARD_FLASH_SIZE / 8) +#define STM32_FLASH_NBANKS (BOARD_FLASH_SIZE/1024) +#define STM32_FLASH_FIXED_PAGE_SIZE 8 #elif defined(STM32H7) #define STM32_FLASH_NPAGES (BOARD_FLASH_SIZE / 128) #define STM32_FLASH_FIXED_PAGE_SIZE 128 +#define STM32_FLASH_NBANKS (BOARD_FLASH_SIZE/1024) #elif defined(STM32F100_MCUCONF) || defined(STM32F103_MCUCONF) #define STM32_FLASH_NPAGES BOARD_FLASH_SIZE #define STM32_FLASH_FIXED_PAGE_SIZE 1 @@ -147,6 +152,11 @@ static const uint32_t flash_memmap[STM32_FLASH_NPAGES] = { KB(32), KB(32), KB(32 #error "Unsupported processor for flash.c" #endif +// for now all multi-bank MCUs have 1MByte banks +#ifdef STM32_FLASH_FIXED_PAGE_SIZE +#define STM32_FLASH_FIXED_PAGE_PER_BANK (1024 / STM32_FLASH_FIXED_PAGE_SIZE) +#endif + #ifndef STM32_FLASH_NBANKS #define STM32_FLASH_NBANKS 2 #endif @@ -465,16 +475,18 @@ bool stm32_flash_erasepage(uint32_t page) stm32_flash_clear_errors(); #if defined(STM32H7) - if (page < 8) { + if (page < STM32_FLASH_FIXED_PAGE_PER_BANK) { // first bank FLASH->SR1 = ~0; stm32_flash_wait_idle(); - uint32_t snb = page << 8; - // use 32 bit operations - FLASH->CR1 = FLASH_CR_PSIZE_1 | snb | FLASH_CR_SER; +#ifdef FLASH_CR_PSIZE_1 + FLASH->CR1 = FLASH_CR_PSIZE_1 | (page<CR1 = (page<CR1 |= FLASH_CR_START; while (FLASH->SR1 & FLASH_SR_QW) ; } @@ -485,10 +497,12 @@ bool stm32_flash_erasepage(uint32_t page) stm32_flash_wait_idle(); - uint32_t snb = (page-8) << 8; - // use 32 bit operations - FLASH->CR2 = FLASH_CR_PSIZE_1 | snb | FLASH_CR_SER; +#ifdef FLASH_CR_PSIZE_1 + FLASH->CR2 = FLASH_CR_PSIZE_1 | ((page-STM32_FLASH_FIXED_PAGE_PER_BANK)<CR2 = ((page-STM32_FLASH_FIXED_PAGE_PER_BANK)<CR2 |= FLASH_CR_START; while (FLASH->SR2 & FLASH_SR_QW) ; } @@ -568,7 +582,7 @@ static bool stm32h7_flash_write32(uint32_t addr, const void *buf) volatile uint32_t *CCR = &FLASH->CCR1; volatile uint32_t *SR = &FLASH->SR1; #if STM32_FLASH_NBANKS > 1 - if (addr - STM32_FLASH_BASE >= 8 * STM32_FLASH_FIXED_PAGE_SIZE * 1024) { + if (addr - STM32_FLASH_BASE >= STM32_FLASH_FIXED_PAGE_PER_BANK * STM32_FLASH_FIXED_PAGE_SIZE * 1024) { CR = &FLASH->CR2; CCR = &FLASH->CCR2; SR = &FLASH->SR2; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c index 6c481475dab5de..f1e510ccebd07e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.c @@ -25,6 +25,8 @@ #pragma GCC optimize("O2") +#include "../../../AP_Math/div1000.h" + /* we have 4 possible configurations of boards, made up of boards that have the following properties: @@ -37,7 +39,7 @@ boot in microseconds, and which wraps at 0xFFFFFFFF. On top of this base function we build get_systime_us32() which has - the same property, but which also maintains timer_base_us64 to allow + the same property, but which allows for a startup offset for micros64() */ @@ -68,48 +70,88 @@ static uint32_t system_time_u32_us(void) #error "unsupported timer resolution" #endif -// offset for micros64() -static uint64_t timer_base_us64; - static uint32_t get_systime_us32(void) { - static uint32_t last_us32; uint32_t now = system_time_u32_us(); #ifdef AP_BOARD_START_TIME now += AP_BOARD_START_TIME; #endif - if (now < last_us32) { - const uint64_t dt_us = 0x100000000ULL; - timer_base_us64 += dt_us; - } - last_us32 = now; return now; } /* - for the exposed functions we use chSysGetStatusAndLockX() to prevent - an interrupt changing the globals while allowing this call from any - context + for the exposed functions we use chVTGetTimeStampI which handles + wrap and directly gives a uint64_t (aka systimestamp_t) */ -uint64_t hrt_micros64() +static uint64_t hrt_micros64I(void) { - syssts_t sts = chSysGetStatusAndLockX(); - uint32_t now = get_systime_us32(); - uint64_t ret = timer_base_us64 + now; - chSysRestoreStatusX(sts); + uint64_t ret = chVTGetTimeStampI(); +#if CH_CFG_ST_FREQUENCY != 1000000U + ret *= 1000000U/CH_CFG_ST_FREQUENCY; +#endif +#ifdef AP_BOARD_START_TIME + ret += AP_BOARD_START_TIME; +#endif return ret; } +static inline bool is_locked(void) { + return !port_irq_enabled(port_get_irq_status()); +} + +uint64_t hrt_micros64() +{ + if (is_locked()) { + return hrt_micros64I(); + } else if (port_is_isr_context()) { + uint64_t ret; + chSysLockFromISR(); + ret = hrt_micros64I(); + chSysUnlockFromISR(); + return ret; + } else { + uint64_t ret; + chSysLock(); + ret = hrt_micros64I(); + chSysUnlock(); + return ret; + } +} + uint32_t hrt_micros32() { - syssts_t sts = chSysGetStatusAndLockX(); - uint32_t ret = get_systime_us32(); - chSysRestoreStatusX(sts); - return ret; +#if CH_CFG_ST_RESOLUTION == 16 + // boards with 16 bit timers need to call get_systime_us32() in a + // lock state because on those boards we have local static + // variables that need protection + if (is_locked()) { + return get_systime_us32(); + } else if (port_is_isr_context()) { + uint32_t ret; + chSysLockFromISR(); + ret = get_systime_us32(); + chSysUnlockFromISR(); + return ret; + } else { + uint32_t ret; + chSysLock(); + ret = get_systime_us32(); + chSysUnlock(); + return ret; + } +#else + return get_systime_us32(); +#endif } +uint64_t hrt_millis64() +{ + return uint64_div1000(hrt_micros64()); +} + uint32_t hrt_millis32() { - return (uint32_t)(hrt_micros64() / 1000U); + return (uint32_t)(hrt_millis64()); } + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h index 9ecfd9a373c812..e8ca91ef724423 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/hrt.h @@ -4,10 +4,16 @@ #ifdef __cplusplus extern "C" { #endif + void hrt_init(void); uint64_t hrt_micros64(void); +uint64_t hrt_micros64_from_ISR(void); // from an ISR uint32_t hrt_micros32(void); uint32_t hrt_millis32(void); +uint32_t hrt_millis32I(void); // from locked context +uint32_t hrt_millis32_from_ISR(void); // from an ISR +uint64_t hrt_millis64(void); + #ifdef __cplusplus } #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h b/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h deleted file mode 100644 index 7002236e2eda90..00000000000000 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h +++ /dev/null @@ -1,2155 +0,0 @@ -/** - * @file - * - * lwIP Options Configuration - */ - -/* - * Copyright (c) 2001-2004 Swedish Institute of Computer Science. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. The name of the author may not be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED - * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT - * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT - * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING - * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY - * OF SUCH DAMAGE. - * - * This file is part of the lwIP TCP/IP stack. - * - * Author: Adam Dunkels - * - */ -#ifndef __LWIPOPT_H__ -#define __LWIPOPT_H__ - -#include "stdio.h" -#include "stm32_util.h" - -#define LWIP_ERRNO_STDINCLUDE - -/* - ----------------------------------------------- - ---------- Platform specific locking ---------- - ----------------------------------------------- -*/ - -#define LWIP_PLATFORM_DIAG(x) do {__wrap_printf x; } while(0) -// #define LWIP_DEBUG -#define U16_F "u" -#define X8_F "x" -#define X16_F "x" -#define LWIP_STATS_DISPLAY 1 -#define ETHARP_STATS 1 -#define LWIP_IGMP 1 -#define MEMP_NUM_NETCONN 10 // up to 10 sockets -#define MEMP_NUM_TCP_PCB 10 -#define MEM_LIBC_MALLOC 1 -#define MEMP_MEM_MALLOC 1 -#define SO_REUSE 1 -#define SO_REUSE_RXTOALL 1 -#define DHCP_DEBUG LWIP_DBG_ON - -#ifndef LWIP_IPV6 - // This uses an additional 18KB Flash - #define LWIP_IPV6 0 -#endif - -/** - * SYS_LIGHTWEIGHT_PROT==1: if you want inter-task protection for certain - * critical regions during buffer allocation, deallocation and memory - * allocation and deallocation. - */ -#ifndef SYS_LIGHTWEIGHT_PROT -#define SYS_LIGHTWEIGHT_PROT 0 -#endif - -/** - * NO_SYS==1: Provides VERY minimal functionality. Otherwise, - * use lwIP facilities. - */ -#ifndef NO_SYS -#define NO_SYS 0 -#endif - -/** - * NO_SYS_NO_TIMERS==1: Drop support for sys_timeout when NO_SYS==1 - * Mainly for compatibility to old versions. - */ -#ifndef NO_SYS_NO_TIMERS -#define NO_SYS_NO_TIMERS 0 -#endif - -/** - * MEMCPY: override this if you have a faster implementation at hand than the - * one included in your C library - */ -#ifndef MEMCPY -#define MEMCPY(dst,src,len) memcpy(dst,src,len) -#endif - -/** - * SMEMCPY: override this with care! Some compilers (e.g. gcc) can inline a - * call to memcpy() if the length is known at compile time and is small. - */ -#ifndef SMEMCPY -#define SMEMCPY(dst,src,len) memcpy(dst,src,len) -#endif - -/* - ------------------------------------ - ---------- Memory options ---------- - ------------------------------------ -*/ -/** - * MEM_LIBC_MALLOC==1: Use malloc/free/realloc provided by your C-library - * instead of the lwip internal allocator. Can save code size if you - * already use it. - */ -#ifndef MEM_LIBC_MALLOC -#define MEM_LIBC_MALLOC 0 -#endif - -/** -* MEMP_MEM_MALLOC==1: Use mem_malloc/mem_free instead of the lwip pool allocator. -* Especially useful with MEM_LIBC_MALLOC but handle with care regarding execution -* speed and usage from interrupts! -*/ -#ifndef MEMP_MEM_MALLOC -#define MEMP_MEM_MALLOC 0 -#endif - -/** - * MEM_ALIGNMENT: should be set to the alignment of the CPU - * 4 byte alignment -> #define MEM_ALIGNMENT 4 - * 2 byte alignment -> #define MEM_ALIGNMENT 2 - */ -#ifndef MEM_ALIGNMENT -#define MEM_ALIGNMENT 4 -#endif - -/** - * MEM_SIZE: the size of the heap memory. If the application will send - * a lot of data that needs to be copied, this should be set high. - */ -#ifndef MEM_SIZE -#define MEM_SIZE 16000 -#endif - -/** - * MEMP_SEPARATE_POOLS: if defined to 1, each pool is placed in its own array. - * This can be used to individually change the location of each pool. - * Default is one big array for all pools - */ -#ifndef MEMP_SEPARATE_POOLS -#define MEMP_SEPARATE_POOLS 0 -#endif - -/** - * MEMP_OVERFLOW_CHECK: memp overflow protection reserves a configurable - * amount of bytes before and after each memp element in every pool and fills - * it with a prominent default value. - * MEMP_OVERFLOW_CHECK == 0 no checking - * MEMP_OVERFLOW_CHECK == 1 checks each element when it is freed - * MEMP_OVERFLOW_CHECK >= 2 checks each element in every pool every time - * memp_malloc() or memp_free() is called (useful but slow!) - */ -#ifndef MEMP_OVERFLOW_CHECK -#define MEMP_OVERFLOW_CHECK 0 -#endif - -/** - * MEMP_SANITY_CHECK==1: run a sanity check after each memp_free() to make - * sure that there are no cycles in the linked lists. - */ -#ifndef MEMP_SANITY_CHECK -#define MEMP_SANITY_CHECK 0 -#endif - -/** - * MEM_USE_POOLS==1: Use an alternative to malloc() by allocating from a set - * of memory pools of various sizes. When mem_malloc is called, an element of - * the smallest pool that can provide the length needed is returned. - * To use this, MEMP_USE_CUSTOM_POOLS also has to be enabled. - */ -#ifndef MEM_USE_POOLS -#define MEM_USE_POOLS 0 -#endif - -/** - * MEM_USE_POOLS_TRY_BIGGER_POOL==1: if one malloc-pool is empty, try the next - * bigger pool - WARNING: THIS MIGHT WASTE MEMORY but it can make a system more - * reliable. */ -#ifndef MEM_USE_POOLS_TRY_BIGGER_POOL -#define MEM_USE_POOLS_TRY_BIGGER_POOL 0 -#endif - -/** - * MEMP_USE_CUSTOM_POOLS==1: whether to include a user file lwippools.h - * that defines additional pools beyond the "standard" ones required - * by lwIP. If you set this to 1, you must have lwippools.h in your - * inlude path somewhere. - */ -#ifndef MEMP_USE_CUSTOM_POOLS -#define MEMP_USE_CUSTOM_POOLS 0 -#endif - -/** - * Set this to 1 if you want to free PBUF_RAM pbufs (or call mem_free()) from - * interrupt context (or another context that doesn't allow waiting for a - * semaphore). - * If set to 1, mem_malloc will be protected by a semaphore and SYS_ARCH_PROTECT, - * while mem_free will only use SYS_ARCH_PROTECT. mem_malloc SYS_ARCH_UNPROTECTs - * with each loop so that mem_free can run. - * - * ATTENTION: As you can see from the above description, this leads to dis-/ - * enabling interrupts often, which can be slow! Also, on low memory, mem_malloc - * can need longer. - * - * If you don't want that, at least for NO_SYS=0, you can still use the following - * functions to enqueue a deallocation call which then runs in the tcpip_thread - * context: - * - pbuf_free_callback(p); - * - mem_free_callback(m); - */ -#ifndef LWIP_ALLOW_MEM_FREE_FROM_OTHER_CONTEXT -#define LWIP_ALLOW_MEM_FREE_FROM_OTHER_CONTEXT 0 -#endif - -/* - ------------------------------------------------ - ---------- Internal Memory Pool Sizes ---------- - ------------------------------------------------ -*/ -/** - * MEMP_NUM_PBUF: the number of memp struct pbufs (used for PBUF_ROM and PBUF_REF). - * If the application sends a lot of data out of ROM (or other static memory), - * this should be set high. - */ -#ifndef MEMP_NUM_PBUF -#define MEMP_NUM_PBUF 16 -#endif - -/** - * MEMP_NUM_RAW_PCB: Number of raw connection PCBs - * (requires the LWIP_RAW option) - */ -#ifndef MEMP_NUM_RAW_PCB -#define MEMP_NUM_RAW_PCB 4 -#endif - -/** - * MEMP_NUM_UDP_PCB: the number of UDP protocol control blocks. One - * per active UDP "connection". - * (requires the LWIP_UDP option) - */ -#ifndef MEMP_NUM_UDP_PCB -#define MEMP_NUM_UDP_PCB 4 -#endif - -/** - * MEMP_NUM_TCP_PCB: the number of simulatenously active TCP connections. - * (requires the LWIP_TCP option) - */ -#ifndef MEMP_NUM_TCP_PCB -#define MEMP_NUM_TCP_PCB 5 -#endif - -/** - * MEMP_NUM_TCP_PCB_LISTEN: the number of listening TCP connections. - * (requires the LWIP_TCP option) - */ -#ifndef MEMP_NUM_TCP_PCB_LISTEN -#define MEMP_NUM_TCP_PCB_LISTEN 8 -#endif - -/** - * MEMP_NUM_TCP_SEG: the number of simultaneously queued TCP segments. - * (requires the LWIP_TCP option) - */ -#ifndef MEMP_NUM_TCP_SEG -#define MEMP_NUM_TCP_SEG 16 -#endif - -/** - * MEMP_NUM_REASSDATA: the number of IP packets simultaneously queued for - * reassembly (whole packets, not fragments!) - */ -#ifndef MEMP_NUM_REASSDATA -#define MEMP_NUM_REASSDATA 5 -#endif - -/** - * MEMP_NUM_FRAG_PBUF: the number of IP fragments simultaneously sent - * (fragments, not whole packets!). - * This is only used with IP_FRAG_USES_STATIC_BUF==0 and - * LWIP_NETIF_TX_SINGLE_PBUF==0 and only has to be > 1 with DMA-enabled MACs - * where the packet is not yet sent when netif->output returns. - */ -#ifndef MEMP_NUM_FRAG_PBUF -#define MEMP_NUM_FRAG_PBUF 15 -#endif - -/** - * MEMP_NUM_ARP_QUEUE: the number of simulateously queued outgoing - * packets (pbufs) that are waiting for an ARP request (to resolve - * their destination address) to finish. - * (requires the ARP_QUEUEING option) - */ -#ifndef MEMP_NUM_ARP_QUEUE -#define MEMP_NUM_ARP_QUEUE 30 -#endif - -/** - * MEMP_NUM_IGMP_GROUP: The number of multicast groups whose network interfaces - * can be members et the same time (one per netif - allsystems group -, plus one - * per netif membership). - * (requires the LWIP_IGMP option) - */ -#ifndef MEMP_NUM_IGMP_GROUP -#define MEMP_NUM_IGMP_GROUP 8 -#endif - -/** - * MEMP_NUM_SYS_TIMEOUT: the number of simulateously active timeouts. - * (requires NO_SYS==0) - * The default number of timeouts is calculated here for all enabled modules. - * The formula expects settings to be either '0' or '1'. - */ -#ifndef MEMP_NUM_SYS_TIMEOUT -#define MEMP_NUM_SYS_TIMEOUT (LWIP_TCP + IP_REASSEMBLY + LWIP_ARP + (2*LWIP_DHCP) + LWIP_AUTOIP + LWIP_IGMP + LWIP_DNS + PPP_SUPPORT + (LWIP_IPV6 * (1 + LWIP_IPV6_REASS + LWIP_IPV6_MLD))) + 2 -#endif - -/** - * MEMP_NUM_NETBUF: the number of struct netbufs. - * (only needed if you use the sequential API, like api_lib.c) - */ -#ifndef MEMP_NUM_NETBUF -#define MEMP_NUM_NETBUF 2 -#endif - -/** - * MEMP_NUM_NETCONN: the number of struct netconns. - * (only needed if you use the sequential API, like api_lib.c) - */ -#ifndef MEMP_NUM_NETCONN -#define MEMP_NUM_NETCONN 4 -#endif - -/** - * MEMP_NUM_TCPIP_MSG_API: the number of struct tcpip_msg, which are used - * for callback/timeout API communication. - * (only needed if you use tcpip.c) - */ -#ifndef MEMP_NUM_TCPIP_MSG_API -#define MEMP_NUM_TCPIP_MSG_API 8 -#endif - -/** - * MEMP_NUM_TCPIP_MSG_INPKT: the number of struct tcpip_msg, which are used - * for incoming packets. - * (only needed if you use tcpip.c) - */ -#ifndef MEMP_NUM_TCPIP_MSG_INPKT -#define MEMP_NUM_TCPIP_MSG_INPKT 8 -#endif - -/** - * MEMP_NUM_SNMP_NODE: the number of leafs in the SNMP tree. - */ -#ifndef MEMP_NUM_SNMP_NODE -#define MEMP_NUM_SNMP_NODE 50 -#endif - -/** - * MEMP_NUM_SNMP_ROOTNODE: the number of branches in the SNMP tree. - * Every branch has one leaf (MEMP_NUM_SNMP_NODE) at least! - */ -#ifndef MEMP_NUM_SNMP_ROOTNODE -#define MEMP_NUM_SNMP_ROOTNODE 30 -#endif - -/** - * MEMP_NUM_SNMP_VARBIND: the number of concurrent requests (does not have to - * be changed normally) - 2 of these are used per request (1 for input, - * 1 for output) - */ -#ifndef MEMP_NUM_SNMP_VARBIND -#define MEMP_NUM_SNMP_VARBIND 2 -#endif - -/** - * MEMP_NUM_SNMP_VALUE: the number of OID or values concurrently used - * (does not have to be changed normally) - 3 of these are used per request - * (1 for the value read and 2 for OIDs - input and output) - */ -#ifndef MEMP_NUM_SNMP_VALUE -#define MEMP_NUM_SNMP_VALUE 3 -#endif - -/** - * MEMP_NUM_NETDB: the number of concurrently running lwip_addrinfo() calls - * (before freeing the corresponding memory using lwip_freeaddrinfo()). - */ -#ifndef MEMP_NUM_NETDB -#define MEMP_NUM_NETDB 1 -#endif - -/** - * MEMP_NUM_LOCALHOSTLIST: the number of host entries in the local host list - * if DNS_LOCAL_HOSTLIST_IS_DYNAMIC==1. - */ -#ifndef MEMP_NUM_LOCALHOSTLIST -#define MEMP_NUM_LOCALHOSTLIST 1 -#endif - -/** - * MEMP_NUM_PPPOE_INTERFACES: the number of concurrently active PPPoE - * interfaces (only used with PPPOE_SUPPORT==1) - */ -#ifndef MEMP_NUM_PPPOE_INTERFACES -#define MEMP_NUM_PPPOE_INTERFACES 1 -#endif - -/** - * PBUF_POOL_SIZE: the number of buffers in the pbuf pool. - */ -#ifndef PBUF_POOL_SIZE -#define PBUF_POOL_SIZE 16 -#endif - -/* - --------------------------------- - ---------- ARP options ---------- - --------------------------------- -*/ -/** - * LWIP_ARP==1: Enable ARP functionality. - */ -#ifndef LWIP_ARP -#define LWIP_ARP 1 -#endif - -/** - * ARP_TABLE_SIZE: Number of active MAC-IP address pairs cached. - */ -#ifndef ARP_TABLE_SIZE -#define ARP_TABLE_SIZE 10 -#endif - -/** - * ARP_QUEUEING==1: Multiple outgoing packets are queued during hardware address - * resolution. By default, only the most recent packet is queued per IP address. - * This is sufficient for most protocols and mainly reduces TCP connection - * startup time. Set this to 1 if you know your application sends more than one - * packet in a row to an IP address that is not in the ARP cache. - */ -#ifndef ARP_QUEUEING -#define ARP_QUEUEING 0 -#endif - -/** - * ETHARP_TRUST_IP_MAC==1: Incoming IP packets cause the ARP table to be - * updated with the source MAC and IP addresses supplied in the packet. - * You may want to disable this if you do not trust LAN peers to have the - * correct addresses, or as a limited approach to attempt to handle - * spoofing. If disabled, lwIP will need to make a new ARP request if - * the peer is not already in the ARP table, adding a little latency. - * The peer *is* in the ARP table if it requested our address before. - * Also notice that this slows down input processing of every IP packet! - */ -#ifndef ETHARP_TRUST_IP_MAC -#define ETHARP_TRUST_IP_MAC 0 -#endif - -/** - * ETHARP_SUPPORT_VLAN==1: support receiving ethernet packets with VLAN header. - * Additionally, you can define ETHARP_VLAN_CHECK to an u16_t VLAN ID to check. - * If ETHARP_VLAN_CHECK is defined, only VLAN-traffic for this VLAN is accepted. - * If ETHARP_VLAN_CHECK is not defined, all traffic is accepted. - * Alternatively, define a function/define ETHARP_VLAN_CHECK_FN(eth_hdr, vlan) - * that returns 1 to accept a packet or 0 to drop a packet. - */ -#ifndef ETHARP_SUPPORT_VLAN -#define ETHARP_SUPPORT_VLAN 0 -#endif - -/** LWIP_ETHERNET==1: enable ethernet support for PPPoE even though ARP - * might be disabled - */ -#ifndef LWIP_ETHERNET -#define LWIP_ETHERNET (LWIP_ARP || PPPOE_SUPPORT) -#endif - -/** ETH_PAD_SIZE: number of bytes added before the ethernet header to ensure - * alignment of payload after that header. Since the header is 14 bytes long, - * without this padding e.g. addresses in the IP header will not be aligned - * on a 32-bit boundary, so setting this to 2 can speed up 32-bit-platforms. - */ -#ifndef ETH_PAD_SIZE -#define ETH_PAD_SIZE 0 -#endif - -/** ETHARP_SUPPORT_STATIC_ENTRIES==1: enable code to support static ARP table - * entries (using etharp_add_static_entry/etharp_remove_static_entry). - */ -#ifndef ETHARP_SUPPORT_STATIC_ENTRIES -#define ETHARP_SUPPORT_STATIC_ENTRIES 0 -#endif - - -/* - -------------------------------- - ---------- IP options ---------- - -------------------------------- -*/ -/** - * IP_FORWARD==1: Enables the ability to forward IP packets across network - * interfaces. If you are going to run lwIP on a device with only one network - * interface, define this to 0. - */ -#ifndef IP_FORWARD -#define IP_FORWARD 0 -#endif - -/** - * IP_OPTIONS_ALLOWED: Defines the behavior for IP options. - * IP_OPTIONS_ALLOWED==0: All packets with IP options are dropped. - * IP_OPTIONS_ALLOWED==1: IP options are allowed (but not parsed). - */ -#ifndef IP_OPTIONS_ALLOWED -#define IP_OPTIONS_ALLOWED 1 -#endif - -/** - * IP_REASSEMBLY==1: Reassemble incoming fragmented IP packets. Note that - * this option does not affect outgoing packet sizes, which can be controlled - * via IP_FRAG. - */ -#ifndef IP_REASSEMBLY -#define IP_REASSEMBLY 1 -#endif - -/** - * IP_FRAG==1: Fragment outgoing IP packets if their size exceeds MTU. Note - * that this option does not affect incoming packet sizes, which can be - * controlled via IP_REASSEMBLY. - */ -#ifndef IP_FRAG -#define IP_FRAG 1 -#endif - -/** - * IP_REASS_MAXAGE: Maximum time (in multiples of IP_TMR_INTERVAL - so seconds, normally) - * a fragmented IP packet waits for all fragments to arrive. If not all fragments arrived - * in this time, the whole packet is discarded. - */ -#ifndef IP_REASS_MAXAGE -#define IP_REASS_MAXAGE 3 -#endif - -/** - * IP_REASS_MAX_PBUFS: Total maximum amount of pbufs waiting to be reassembled. - * Since the received pbufs are enqueued, be sure to configure - * PBUF_POOL_SIZE > IP_REASS_MAX_PBUFS so that the stack is still able to receive - * packets even if the maximum amount of fragments is enqueued for reassembly! - */ -#ifndef IP_REASS_MAX_PBUFS -#define IP_REASS_MAX_PBUFS 10 -#endif - -/** - * IP_FRAG_USES_STATIC_BUF==1: Use a static MTU-sized buffer for IP - * fragmentation. Otherwise pbufs are allocated and reference the original - * packet data to be fragmented (or with LWIP_NETIF_TX_SINGLE_PBUF==1, - * new PBUF_RAM pbufs are used for fragments). - * ATTENTION: IP_FRAG_USES_STATIC_BUF==1 may not be used for DMA-enabled MACs! - */ -#ifndef IP_FRAG_USES_STATIC_BUF -#define IP_FRAG_USES_STATIC_BUF 0 -#endif - -/** - * IP_FRAG_MAX_MTU: Assumed max MTU on any interface for IP frag buffer - * (requires IP_FRAG_USES_STATIC_BUF==1) - */ -#if IP_FRAG_USES_STATIC_BUF && !defined(IP_FRAG_MAX_MTU) -#define IP_FRAG_MAX_MTU 1500 -#endif - -/** - * IP_DEFAULT_TTL: Default value for Time-To-Live used by transport layers. - */ -#ifndef IP_DEFAULT_TTL -#define IP_DEFAULT_TTL 255 -#endif - -/** - * IP_SOF_BROADCAST=1: Use the SOF_BROADCAST field to enable broadcast - * filter per pcb on udp and raw send operations. To enable broadcast filter - * on recv operations, you also have to set IP_SOF_BROADCAST_RECV=1. - */ -#ifndef IP_SOF_BROADCAST -#define IP_SOF_BROADCAST 1 -#endif - -/** - * IP_SOF_BROADCAST_RECV (requires IP_SOF_BROADCAST=1) enable the broadcast - * filter on recv operations. - */ -#ifndef IP_SOF_BROADCAST_RECV -#define IP_SOF_BROADCAST_RECV 1 -#endif - -/** - * IP_FORWARD_ALLOW_TX_ON_RX_NETIF==1: allow ip_forward() to send packets back - * out on the netif where it was received. This should only be used for - * wireless networks. - * ATTENTION: When this is 1, make sure your netif driver correctly marks incoming - * link-layer-broadcast/multicast packets as such using the corresponding pbuf flags! - */ -#ifndef IP_FORWARD_ALLOW_TX_ON_RX_NETIF -#define IP_FORWARD_ALLOW_TX_ON_RX_NETIF 0 -#endif - -/** - * LWIP_RANDOMIZE_INITIAL_LOCAL_PORTS==1: randomize the local port for the first - * local TCP/UDP pcb (default==0). This can prevent creating predictable port - * numbers after booting a device. - */ -#ifndef LWIP_RANDOMIZE_INITIAL_LOCAL_PORTS -#define LWIP_RANDOMIZE_INITIAL_LOCAL_PORTS 0 -#endif - -/* - ---------------------------------- - ---------- ICMP options ---------- - ---------------------------------- -*/ -/** - * LWIP_ICMP==1: Enable ICMP module inside the IP stack. - * Be careful, disable that make your product non-compliant to RFC1122 - */ -#ifndef LWIP_ICMP -#define LWIP_ICMP 1 -#endif - -/** - * ICMP_TTL: Default value for Time-To-Live used by ICMP packets. - */ -#ifndef ICMP_TTL -#define ICMP_TTL (IP_DEFAULT_TTL) -#endif - -/** - * LWIP_BROADCAST_PING==1: respond to broadcast pings (default is unicast only) - */ -#ifndef LWIP_BROADCAST_PING -#define LWIP_BROADCAST_PING 0 -#endif - -/** - * LWIP_MULTICAST_PING==1: respond to multicast pings (default is unicast only) - */ -#ifndef LWIP_MULTICAST_PING -#define LWIP_MULTICAST_PING 0 -#endif - -/* - --------------------------------- - ---------- RAW options ---------- - --------------------------------- -*/ -/** - * LWIP_RAW==1: Enable application layer to hook into the IP layer itself. - */ -#ifndef LWIP_RAW -#define LWIP_RAW 1 -#endif - -/** - * LWIP_RAW==1: Enable application layer to hook into the IP layer itself. - */ -#ifndef RAW_TTL -#define RAW_TTL (IP_DEFAULT_TTL) -#endif - -/* - ---------------------------------- - ---------- DHCP options ---------- - ---------------------------------- -*/ -/** - * LWIP_DHCP==1: Enable DHCP module. - */ -#ifndef LWIP_DHCP -// Flash use: 7960 Bytes (9100 on IPv6) -#define LWIP_DHCP 1 -#endif - -/** - * DHCP_DOES_ARP_CHECK==1: Do an ARP check on the offered address. - */ -#ifndef DHCP_DOES_ARP_CHECK -#define DHCP_DOES_ARP_CHECK ((LWIP_DHCP) && (LWIP_ARP)) -#endif - -/* - ------------------------------------ - ---------- AUTOIP options ---------- - ------------------------------------ -*/ -/** - * LWIP_AUTOIP==1: Enable AUTOIP module. - */ -#ifndef LWIP_AUTOIP -#define LWIP_AUTOIP 0 -#endif - -/** - * LWIP_DHCP_AUTOIP_COOP==1: Allow DHCP and AUTOIP to be both enabled on - * the same interface at the same time. - */ -#ifndef LWIP_DHCP_AUTOIP_COOP -#define LWIP_DHCP_AUTOIP_COOP 0 -#endif - -/** - * LWIP_DHCP_AUTOIP_COOP_TRIES: Set to the number of DHCP DISCOVER probes - * that should be sent before falling back on AUTOIP. This can be set - * as low as 1 to get an AutoIP address very quickly, but you should - * be prepared to handle a changing IP address when DHCP overrides - * AutoIP. - */ -#ifndef LWIP_DHCP_AUTOIP_COOP_TRIES -#define LWIP_DHCP_AUTOIP_COOP_TRIES 9 -#endif - -/* - ---------------------------------- - ---------- SNMP options ---------- - ---------------------------------- -*/ -/** - * LWIP_SNMP==1: Turn on SNMP module. UDP must be available for SNMP - * transport. - */ -#ifndef LWIP_SNMP -#define LWIP_SNMP 0 -#endif - -/** - * SNMP_CONCURRENT_REQUESTS: Number of concurrent requests the module will - * allow. At least one request buffer is required. - * Does not have to be changed unless external MIBs answer request asynchronously - */ -#ifndef SNMP_CONCURRENT_REQUESTS -#define SNMP_CONCURRENT_REQUESTS 1 -#endif - -/** - * SNMP_TRAP_DESTINATIONS: Number of trap destinations. At least one trap - * destination is required - */ -#ifndef SNMP_TRAP_DESTINATIONS -#define SNMP_TRAP_DESTINATIONS 1 -#endif - -/** - * SNMP_PRIVATE_MIB: - * When using a private MIB, you have to create a file 'private_mib.h' that contains - * a 'struct mib_array_node mib_private' which contains your MIB. - */ -#ifndef SNMP_PRIVATE_MIB -#define SNMP_PRIVATE_MIB 0 -#endif - -/** - * Only allow SNMP write actions that are 'safe' (e.g. disabeling netifs is not - * a safe action and disabled when SNMP_SAFE_REQUESTS = 1). - * Unsafe requests are disabled by default! - */ -#ifndef SNMP_SAFE_REQUESTS -#define SNMP_SAFE_REQUESTS 1 -#endif - -/** - * The maximum length of strings used. This affects the size of - * MEMP_SNMP_VALUE elements. - */ -#ifndef SNMP_MAX_OCTET_STRING_LEN -#define SNMP_MAX_OCTET_STRING_LEN 127 -#endif - -/** - * The maximum depth of the SNMP tree. - * With private MIBs enabled, this depends on your MIB! - * This affects the size of MEMP_SNMP_VALUE elements. - */ -#ifndef SNMP_MAX_TREE_DEPTH -#define SNMP_MAX_TREE_DEPTH 15 -#endif - -/** - * The size of the MEMP_SNMP_VALUE elements, normally calculated from - * SNMP_MAX_OCTET_STRING_LEN and SNMP_MAX_TREE_DEPTH. - */ -#ifndef SNMP_MAX_VALUE_SIZE -#define SNMP_MAX_VALUE_SIZE LWIP_MAX((SNMP_MAX_OCTET_STRING_LEN)+1, sizeof(s32_t)*(SNMP_MAX_TREE_DEPTH)) -#endif - -/* - ---------------------------------- - ---------- IGMP options ---------- - ---------------------------------- -*/ -/** - * LWIP_IGMP==1: Turn on IGMP module. - */ -#ifndef LWIP_IGMP -#define LWIP_IGMP 0 -#endif - -/* - ---------------------------------- - ---------- DNS options ----------- - ---------------------------------- -*/ -/** - * LWIP_DNS==1: Turn on DNS module. UDP must be available for DNS - * transport. - */ -#ifndef LWIP_DNS -#define LWIP_DNS 1 -#endif - -/** DNS maximum number of entries to maintain locally. */ -#ifndef DNS_TABLE_SIZE -#define DNS_TABLE_SIZE 4 -#endif - -/** DNS maximum host name length supported in the name table. */ -#ifndef DNS_MAX_NAME_LENGTH -#define DNS_MAX_NAME_LENGTH 256 -#endif - -/** The maximum of DNS servers */ -#ifndef DNS_MAX_SERVERS -#define DNS_MAX_SERVERS 2 -#endif - -/** DNS do a name checking between the query and the response. */ -#ifndef DNS_DOES_NAME_CHECK -#define DNS_DOES_NAME_CHECK 1 -#endif - -/** DNS message max. size. Default value is RFC compliant. */ -#ifndef DNS_MSG_SIZE -#define DNS_MSG_SIZE 512 -#endif - -/** DNS_LOCAL_HOSTLIST: Implements a local host-to-address list. If enabled, - * you have to define - * #define DNS_LOCAL_HOSTLIST_INIT {{"host1", 0x123}, {"host2", 0x234}} - * (an array of structs name/address, where address is an u32_t in network - * byte order). - * - * Instead, you can also use an external function: - * #define DNS_LOOKUP_LOCAL_EXTERN(x) extern u32_t my_lookup_function(const char *name) - * that returns the IP address or INADDR_NONE if not found. - */ -#ifndef DNS_LOCAL_HOSTLIST -#define DNS_LOCAL_HOSTLIST 0 -#endif /* DNS_LOCAL_HOSTLIST */ - -/** If this is turned on, the local host-list can be dynamically changed - * at runtime. */ -#ifndef DNS_LOCAL_HOSTLIST_IS_DYNAMIC -#define DNS_LOCAL_HOSTLIST_IS_DYNAMIC 0 -#endif /* DNS_LOCAL_HOSTLIST_IS_DYNAMIC */ - -/* - --------------------------------- - ---------- UDP options ---------- - --------------------------------- -*/ -/** - * LWIP_UDP==1: Turn on UDP. - */ -#ifndef LWIP_UDP -#define LWIP_UDP 1 -#endif - -/** - * LWIP_UDPLITE==1: Turn on UDP-Lite. (Requires LWIP_UDP) - */ -#ifndef LWIP_UDPLITE -#define LWIP_UDPLITE 0 -#endif - -/** - * UDP_TTL: Default Time-To-Live value. - */ -#ifndef UDP_TTL -#define UDP_TTL (IP_DEFAULT_TTL) -#endif - -/** - * LWIP_NETBUF_RECVINFO==1: append destination addr and port to every netbuf. - */ -#ifndef LWIP_NETBUF_RECVINFO -#define LWIP_NETBUF_RECVINFO 0 -#endif - -/* - --------------------------------- - ---------- TCP options ---------- - --------------------------------- -*/ -/** - * LWIP_TCP==1: Turn on TCP. - */ -#ifndef LWIP_TCP -#define LWIP_TCP 1 -#endif - -/** - * TCP_TTL: Default Time-To-Live value. - */ -#ifndef TCP_TTL -#define TCP_TTL (IP_DEFAULT_TTL) -#endif - -/** - * TCP_WND: The size of a TCP window. This must be at least - * (2 * TCP_MSS) for things to work well - */ -#ifndef TCP_WND -#define TCP_WND (4 * TCP_MSS) -#endif - -/** - * TCP_MAXRTX: Maximum number of retransmissions of data segments. - */ -#ifndef TCP_MAXRTX -#define TCP_MAXRTX 12 -#endif - -/** - * TCP_SYNMAXRTX: Maximum number of retransmissions of SYN segments. - */ -#ifndef TCP_SYNMAXRTX -#define TCP_SYNMAXRTX 6 -#endif - -/** - * TCP_QUEUE_OOSEQ==1: TCP will queue segments that arrive out of order. - * Define to 0 if your device is low on memory. - */ -#ifndef TCP_QUEUE_OOSEQ -#define TCP_QUEUE_OOSEQ (LWIP_TCP) -#endif - -/** - * TCP_MSS: TCP Maximum segment size. (default is 536, a conservative default, - * you might want to increase this.) - * For the receive side, this MSS is advertised to the remote side - * when opening a connection. For the transmit size, this MSS sets - * an upper limit on the MSS advertised by the remote host. - */ -#ifndef TCP_MSS -#define TCP_MSS 1480 -#endif - -/** - * TCP_CALCULATE_EFF_SEND_MSS: "The maximum size of a segment that TCP really - * sends, the 'effective send MSS,' MUST be the smaller of the send MSS (which - * reflects the available reassembly buffer size at the remote host) and the - * largest size permitted by the IP layer" (RFC 1122) - * Setting this to 1 enables code that checks TCP_MSS against the MTU of the - * netif used for a connection and limits the MSS if it would be too big otherwise. - */ -#ifndef TCP_CALCULATE_EFF_SEND_MSS -#define TCP_CALCULATE_EFF_SEND_MSS 1 -#endif - - -/** - * TCP_SND_BUF: TCP sender buffer space (bytes). - * To achieve good performance, this should be at least 2 * TCP_MSS. - */ -#ifndef TCP_SND_BUF -#define TCP_SND_BUF (2 * TCP_MSS) -#endif - -/** - * TCP_SND_QUEUELEN: TCP sender buffer space (pbufs). This must be at least - * as much as (2 * TCP_SND_BUF/TCP_MSS) for things to work. - */ -#ifndef TCP_SND_QUEUELEN -#define TCP_SND_QUEUELEN ((4 * (TCP_SND_BUF) + (TCP_MSS - 1))/(TCP_MSS)) -#endif - -/** - * TCP_SNDLOWAT: TCP writable space (bytes). This must be less than - * TCP_SND_BUF. It is the amount of space which must be available in the - * TCP snd_buf for select to return writable (combined with TCP_SNDQUEUELOWAT). - */ -#ifndef TCP_SNDLOWAT -#define TCP_SNDLOWAT LWIP_MIN(LWIP_MAX(((TCP_SND_BUF)/2), (2 * TCP_MSS) + 1), (TCP_SND_BUF) - 1) -#endif - -/** - * TCP_SNDQUEUELOWAT: TCP writable bufs (pbuf count). This must be less - * than TCP_SND_QUEUELEN. If the number of pbufs queued on a pcb drops below - * this number, select returns writable (combined with TCP_SNDLOWAT). - */ -#ifndef TCP_SNDQUEUELOWAT -#define TCP_SNDQUEUELOWAT LWIP_MAX(((TCP_SND_QUEUELEN)/2), 5) -#endif - -/** - * TCP_OOSEQ_MAX_BYTES: The maximum number of bytes queued on ooseq per pcb. - * Default is 0 (no limit). Only valid for TCP_QUEUE_OOSEQ==0. - */ -#ifndef TCP_OOSEQ_MAX_BYTES -#define TCP_OOSEQ_MAX_BYTES 0 -#endif - -/** - * TCP_OOSEQ_MAX_PBUFS: The maximum number of pbufs queued on ooseq per pcb. - * Default is 0 (no limit). Only valid for TCP_QUEUE_OOSEQ==0. - */ -#ifndef TCP_OOSEQ_MAX_PBUFS -#define TCP_OOSEQ_MAX_PBUFS 0 -#endif - -/** - * TCP_LISTEN_BACKLOG: Enable the backlog option for tcp listen pcb. - */ -#ifndef TCP_LISTEN_BACKLOG -#define TCP_LISTEN_BACKLOG 0 -#endif - -/** - * The maximum allowed backlog for TCP listen netconns. - * This backlog is used unless another is explicitly specified. - * 0xff is the maximum (u8_t). - */ -#ifndef TCP_DEFAULT_LISTEN_BACKLOG -#define TCP_DEFAULT_LISTEN_BACKLOG 0xff -#endif - -/** - * TCP_OVERSIZE: The maximum number of bytes that tcp_write may - * allocate ahead of time in an attempt to create shorter pbuf chains - * for transmission. The meaningful range is 0 to TCP_MSS. Some - * suggested values are: - * - * 0: Disable oversized allocation. Each tcp_write() allocates a new - pbuf (old behaviour). - * 1: Allocate size-aligned pbufs with minimal excess. Use this if your - * scatter-gather DMA requires aligned fragments. - * 128: Limit the pbuf/memory overhead to 20%. - * TCP_MSS: Try to create unfragmented TCP packets. - * TCP_MSS/4: Try to create 4 fragments or less per TCP packet. - */ -#ifndef TCP_OVERSIZE -#define TCP_OVERSIZE TCP_MSS -#endif - -/** - * LWIP_TCP_TIMESTAMPS==1: support the TCP timestamp option. - */ -#ifndef LWIP_TCP_TIMESTAMPS -#define LWIP_TCP_TIMESTAMPS 0 -#endif - -/** - * TCP_WND_UPDATE_THRESHOLD: difference in window to trigger an - * explicit window update - */ -#ifndef TCP_WND_UPDATE_THRESHOLD -#define TCP_WND_UPDATE_THRESHOLD (TCP_WND / 4) -#endif - -/** - * LWIP_EVENT_API and LWIP_CALLBACK_API: Only one of these should be set to 1. - * LWIP_EVENT_API==1: The user defines lwip_tcp_event() to receive all - * events (accept, sent, etc) that happen in the system. - * LWIP_CALLBACK_API==1: The PCB callback function is called directly - * for the event. This is the default. - */ -#if !defined(LWIP_EVENT_API) && !defined(LWIP_CALLBACK_API) -#define LWIP_EVENT_API 0 -#define LWIP_CALLBACK_API 1 -#endif - - -/* - ---------------------------------- - ---------- Pbuf options ---------- - ---------------------------------- -*/ -/** - * PBUF_LINK_HLEN: the number of bytes that should be allocated for a - * link level header. The default is 14, the standard value for - * Ethernet. - */ -#ifndef PBUF_LINK_HLEN -#define PBUF_LINK_HLEN (14 + ETH_PAD_SIZE) -#endif - -/** - * PBUF_POOL_BUFSIZE: the size of each pbuf in the pbuf pool. The default is - * designed to accomodate single full size TCP frame in one pbuf, including - * TCP_MSS, IP header, and link header. - */ -#ifndef PBUF_POOL_BUFSIZE -#define PBUF_POOL_BUFSIZE LWIP_MEM_ALIGN_SIZE(TCP_MSS+40+PBUF_LINK_HLEN) -#endif - -/* - ------------------------------------------------ - ---------- Network Interfaces options ---------- - ------------------------------------------------ -*/ -/** - * LWIP_NETIF_HOSTNAME==1: use DHCP_OPTION_HOSTNAME with netif's hostname - * field. - */ -#ifndef LWIP_NETIF_HOSTNAME -#define LWIP_NETIF_HOSTNAME 0 -#endif - -/** - * LWIP_NETIF_API==1: Support netif api (in netifapi.c) - */ -#ifndef LWIP_NETIF_API -#define LWIP_NETIF_API 0 -#endif - -/** - * LWIP_NETIF_STATUS_CALLBACK==1: Support a callback function whenever an interface - * changes its up/down status (i.e., due to DHCP IP acquistion) - */ -#ifndef LWIP_NETIF_STATUS_CALLBACK -#define LWIP_NETIF_STATUS_CALLBACK 0 -#endif - -/** - * LWIP_NETIF_LINK_CALLBACK==1: Support a callback function from an interface - * whenever the link changes (i.e., link down) - */ -#ifndef LWIP_NETIF_LINK_CALLBACK -#define LWIP_NETIF_LINK_CALLBACK 0 -#endif - -/** - * LWIP_NETIF_REMOVE_CALLBACK==1: Support a callback function that is called - * when a netif has been removed - */ -#ifndef LWIP_NETIF_REMOVE_CALLBACK -#define LWIP_NETIF_REMOVE_CALLBACK 0 -#endif - -/** - * LWIP_NETIF_HWADDRHINT==1: Cache link-layer-address hints (e.g. table - * indices) in struct netif. TCP and UDP can make use of this to prevent - * scanning the ARP table for every sent packet. While this is faster for big - * ARP tables or many concurrent connections, it might be counterproductive - * if you have a tiny ARP table or if there never are concurrent connections. - */ -#ifndef LWIP_NETIF_HWADDRHINT -#define LWIP_NETIF_HWADDRHINT 0 -#endif - -/** - * LWIP_NETIF_LOOPBACK==1: Support sending packets with a destination IP - * address equal to the netif IP address, looping them back up the stack. - */ -#ifndef LWIP_NETIF_LOOPBACK -#define LWIP_NETIF_LOOPBACK 0 -#endif - -/** - * LWIP_LOOPBACK_MAX_PBUFS: Maximum number of pbufs on queue for loopback - * sending for each netif (0 = disabled) - */ -#ifndef LWIP_LOOPBACK_MAX_PBUFS -#define LWIP_LOOPBACK_MAX_PBUFS 0 -#endif - -/** - * LWIP_NETIF_LOOPBACK_MULTITHREADING: Indicates whether threading is enabled in - * the system, as netifs must change how they behave depending on this setting - * for the LWIP_NETIF_LOOPBACK option to work. - * Setting this is needed to avoid reentering non-reentrant functions like - * tcp_input(). - * LWIP_NETIF_LOOPBACK_MULTITHREADING==1: Indicates that the user is using a - * multithreaded environment like tcpip.c. In this case, netif->input() - * is called directly. - * LWIP_NETIF_LOOPBACK_MULTITHREADING==0: Indicates a polling (or NO_SYS) setup. - * The packets are put on a list and netif_poll() must be called in - * the main application loop. - */ -#ifndef LWIP_NETIF_LOOPBACK_MULTITHREADING -#define LWIP_NETIF_LOOPBACK_MULTITHREADING (!NO_SYS) -#endif - -/** - * LWIP_NETIF_TX_SINGLE_PBUF: if this is set to 1, lwIP tries to put all data - * to be sent into one single pbuf. This is for compatibility with DMA-enabled - * MACs that do not support scatter-gather. - * Beware that this might involve CPU-memcpy before transmitting that would not - * be needed without this flag! Use this only if you need to! - * - * @todo: TCP and IP-frag do not work with this, yet: - */ -#ifndef LWIP_NETIF_TX_SINGLE_PBUF -#define LWIP_NETIF_TX_SINGLE_PBUF 0 -#endif /* LWIP_NETIF_TX_SINGLE_PBUF */ - -/* - ------------------------------------ - ---------- LOOPIF options ---------- - ------------------------------------ -*/ -/** - * LWIP_HAVE_LOOPIF==1: Support loop interface (127.0.0.1) and loopif.c - */ -#ifndef LWIP_HAVE_LOOPIF -#define LWIP_HAVE_LOOPIF 0 -#endif - -/* - ------------------------------------ - ---------- SLIPIF options ---------- - ------------------------------------ -*/ -/** - * LWIP_HAVE_SLIPIF==1: Support slip interface and slipif.c - */ -#ifndef LWIP_HAVE_SLIPIF -#define LWIP_HAVE_SLIPIF 0 -#endif - -/* - ------------------------------------ - ---------- Thread options ---------- - ------------------------------------ -*/ -/** - * TCPIP_THREAD_NAME: The name assigned to the main tcpip thread. - */ -#ifndef TCPIP_THREAD_NAME -#define TCPIP_THREAD_NAME "tcpip_thread" -#endif - -/** - * TCPIP_THREAD_STACKSIZE: The stack size used by the main tcpip thread. - * The stack size value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef TCPIP_THREAD_STACKSIZE -#define TCPIP_THREAD_STACKSIZE 1024 -#endif - -/** - * TCPIP_THREAD_PRIO: The priority assigned to the main tcpip thread. - * The priority value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef TCPIP_THREAD_PRIO -#define TCPIP_THREAD_PRIO (LOWPRIO + 1) -#endif - -/** - * TCPIP_MBOX_SIZE: The mailbox size for the tcpip thread messages - * The queue size value itself is platform-dependent, but is passed to - * sys_mbox_new() when tcpip_init is called. - */ -#ifndef TCPIP_MBOX_SIZE -#define TCPIP_MBOX_SIZE MEMP_NUM_PBUF -#endif - -/** - * SLIPIF_THREAD_NAME: The name assigned to the slipif_loop thread. - */ -#ifndef SLIPIF_THREAD_NAME -#define SLIPIF_THREAD_NAME "slipif_loop" -#endif - -/** - * SLIP_THREAD_STACKSIZE: The stack size used by the slipif_loop thread. - * The stack size value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef SLIPIF_THREAD_STACKSIZE -#define SLIPIF_THREAD_STACKSIZE 1024 -#endif - -/** - * SLIPIF_THREAD_PRIO: The priority assigned to the slipif_loop thread. - * The priority value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef SLIPIF_THREAD_PRIO -#define SLIPIF_THREAD_PRIO (LOWPRIO + 1) -#endif - -/** - * PPP_THREAD_NAME: The name assigned to the pppInputThread. - */ -#ifndef PPP_THREAD_NAME -#define PPP_THREAD_NAME "pppInputThread" -#endif - -/** - * PPP_THREAD_STACKSIZE: The stack size used by the pppInputThread. - * The stack size value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef PPP_THREAD_STACKSIZE -#define PPP_THREAD_STACKSIZE 1024 -#endif - -/** - * PPP_THREAD_PRIO: The priority assigned to the pppInputThread. - * The priority value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef PPP_THREAD_PRIO -#define PPP_THREAD_PRIO (LOWPRIO + 1) -#endif - -/** - * DEFAULT_THREAD_NAME: The name assigned to any other lwIP thread. - */ -#ifndef DEFAULT_THREAD_NAME -#define DEFAULT_THREAD_NAME "lwIP" -#endif - -/** - * DEFAULT_THREAD_STACKSIZE: The stack size used by any other lwIP thread. - * The stack size value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef DEFAULT_THREAD_STACKSIZE -#define DEFAULT_THREAD_STACKSIZE 1024 -#endif - -/** - * DEFAULT_THREAD_PRIO: The priority assigned to any other lwIP thread. - * The priority value itself is platform-dependent, but is passed to - * sys_thread_new() when the thread is created. - */ -#ifndef DEFAULT_THREAD_PRIO -#define DEFAULT_THREAD_PRIO (LOWPRIO + 1) -#endif - -/** - * DEFAULT_RAW_RECVMBOX_SIZE: The mailbox size for the incoming packets on a - * NETCONN_RAW. The queue size value itself is platform-dependent, but is passed - * to sys_mbox_new() when the recvmbox is created. - */ -#ifndef DEFAULT_RAW_RECVMBOX_SIZE -#define DEFAULT_RAW_RECVMBOX_SIZE 4 -#endif - -/** - * DEFAULT_UDP_RECVMBOX_SIZE: The mailbox size for the incoming packets on a - * NETCONN_UDP. The queue size value itself is platform-dependent, but is passed - * to sys_mbox_new() when the recvmbox is created. - */ -#ifndef DEFAULT_UDP_RECVMBOX_SIZE -#define DEFAULT_UDP_RECVMBOX_SIZE 4 -#endif - -/** - * DEFAULT_TCP_RECVMBOX_SIZE: The mailbox size for the incoming packets on a - * NETCONN_TCP. The queue size value itself is platform-dependent, but is passed - * to sys_mbox_new() when the recvmbox is created. - */ -#ifndef DEFAULT_TCP_RECVMBOX_SIZE -#define DEFAULT_TCP_RECVMBOX_SIZE 40 -#endif - -/** - * DEFAULT_ACCEPTMBOX_SIZE: The mailbox size for the incoming connections. - * The queue size value itself is platform-dependent, but is passed to - * sys_mbox_new() when the acceptmbox is created. - */ -#ifndef DEFAULT_ACCEPTMBOX_SIZE -#define DEFAULT_ACCEPTMBOX_SIZE 4 -#endif - -/* - ---------------------------------------------- - ---------- Sequential layer options ---------- - ---------------------------------------------- -*/ -/** - * LWIP_TCPIP_CORE_LOCKING: (EXPERIMENTAL!) - * Don't use it if you're not an active lwIP project member - */ -#ifndef LWIP_TCPIP_CORE_LOCKING -#define LWIP_TCPIP_CORE_LOCKING 0 -#endif - -/** - * LWIP_TCPIP_CORE_LOCKING_INPUT: (EXPERIMENTAL!) - * Don't use it if you're not an active lwIP project member - */ -#ifndef LWIP_TCPIP_CORE_LOCKING_INPUT -#define LWIP_TCPIP_CORE_LOCKING_INPUT 0 -#endif - -/** - * LWIP_NETCONN==1: Enable Netconn API (require to use api_lib.c) - */ -#ifndef LWIP_NETCONN -#define LWIP_NETCONN 1 -#endif - -/** LWIP_TCPIP_TIMEOUT==1: Enable tcpip_timeout/tcpip_untimeout tod create - * timers running in tcpip_thread from another thread. - */ -#ifndef LWIP_TCPIP_TIMEOUT -#define LWIP_TCPIP_TIMEOUT 1 -#endif - -/* - ------------------------------------ - ---------- Socket options ---------- - ------------------------------------ -*/ -/** - * LWIP_SOCKET==1: Enable Socket API (require to use sockets.c) - */ -#ifndef LWIP_SOCKET -#define LWIP_SOCKET 1 -#endif - -/** - * LWIP_COMPAT_SOCKETS==1: Enable BSD-style sockets functions names. - * (only used if you use sockets.c) - */ -#ifndef LWIP_COMPAT_SOCKETS -#define LWIP_COMPAT_SOCKETS 2 -#endif - -/** - * LWIP_POSIX_SOCKETS_IO_NAMES==1: Enable POSIX-style sockets functions names. - * Disable this option if you use a POSIX operating system that uses the same - * names (read, write & close). (only used if you use sockets.c) - */ -#ifndef LWIP_POSIX_SOCKETS_IO_NAMES -#define LWIP_POSIX_SOCKETS_IO_NAMES 1 -#endif - -/** - * LWIP_TCP_KEEPALIVE==1: Enable TCP_KEEPIDLE, TCP_KEEPINTVL and TCP_KEEPCNT - * options processing. Note that TCP_KEEPIDLE and TCP_KEEPINTVL have to be set - * in seconds. (does not require sockets.c, and will affect tcp.c) - */ -#ifndef LWIP_TCP_KEEPALIVE -#define LWIP_TCP_KEEPALIVE 0 -#endif - -/** - * LWIP_SO_SNDTIMEO==1: Enable send timeout for sockets/netconns and - * SO_SNDTIMEO processing. - */ -#ifndef LWIP_SO_SNDTIMEO -#define LWIP_SO_SNDTIMEO 0 -#endif - -/** - * LWIP_SO_RCVTIMEO==1: Enable receive timeout for sockets/netconns and - * SO_RCVTIMEO processing. - */ -#ifndef LWIP_SO_RCVTIMEO -#define LWIP_SO_RCVTIMEO 0 -#endif - -/** - * LWIP_SO_RCVBUF==1: Enable SO_RCVBUF processing. - */ -#ifndef LWIP_SO_RCVBUF -#define LWIP_SO_RCVBUF 0 -#endif - -/** - * If LWIP_SO_RCVBUF is used, this is the default value for recv_bufsize. - */ -#ifndef RECV_BUFSIZE_DEFAULT -#define RECV_BUFSIZE_DEFAULT INT_MAX -#endif - -/** - * SO_REUSE==1: Enable SO_REUSEADDR option. - */ -#ifndef SO_REUSE -#define SO_REUSE 0 -#endif - -/** - * SO_REUSE_RXTOALL==1: Pass a copy of incoming broadcast/multicast packets - * to all local matches if SO_REUSEADDR is turned on. - * WARNING: Adds a memcpy for every packet if passing to more than one pcb! - */ -#ifndef SO_REUSE_RXTOALL -#define SO_REUSE_RXTOALL 0 -#endif - -/* - ---------------------------------------- - ---------- Statistics options ---------- - ---------------------------------------- -*/ -/** - * LWIP_STATS==1: Enable statistics collection in lwip_stats. - */ -#ifndef LWIP_STATS -#define LWIP_STATS 1 -#endif - -#if LWIP_STATS - -/** - * LWIP_STATS_DISPLAY==1: Compile in the statistics output functions. - */ -#ifndef LWIP_STATS_DISPLAY -#define LWIP_STATS_DISPLAY 0 -#endif - -/** - * LINK_STATS==1: Enable link stats. - */ -#ifndef LINK_STATS -#define LINK_STATS 1 -#endif - -/** - * ETHARP_STATS==1: Enable etharp stats. - */ -#ifndef ETHARP_STATS -#define ETHARP_STATS (LWIP_ARP) -#endif - -/** - * IP_STATS==1: Enable IP stats. - */ -#ifndef IP_STATS -#define IP_STATS 1 -#endif - -/** - * IPFRAG_STATS==1: Enable IP fragmentation stats. Default is - * on if using either frag or reass. - */ -#ifndef IPFRAG_STATS -#define IPFRAG_STATS (IP_REASSEMBLY || IP_FRAG) -#endif - -/** - * ICMP_STATS==1: Enable ICMP stats. - */ -#ifndef ICMP_STATS -#define ICMP_STATS 1 -#endif - -/** - * IGMP_STATS==1: Enable IGMP stats. - */ -#ifndef IGMP_STATS -#define IGMP_STATS (LWIP_IGMP) -#endif - -/** - * UDP_STATS==1: Enable UDP stats. Default is on if - * UDP enabled, otherwise off. - */ -#ifndef UDP_STATS -#define UDP_STATS (LWIP_UDP) -#endif - -/** - * TCP_STATS==1: Enable TCP stats. Default is on if TCP - * enabled, otherwise off. - */ -#ifndef TCP_STATS -#define TCP_STATS (LWIP_TCP) -#endif - -/** - * MEM_STATS==1: Enable mem.c stats. - */ -#ifndef MEM_STATS -#define MEM_STATS ((MEM_LIBC_MALLOC == 0) && (MEM_USE_POOLS == 0)) -#endif - -/** - * MEMP_STATS==1: Enable memp.c pool stats. - */ -#ifndef MEMP_STATS -#define MEMP_STATS (MEMP_MEM_MALLOC == 0) -#endif - -/** - * SYS_STATS==1: Enable system stats (sem and mbox counts, etc). - */ -#ifndef SYS_STATS -#define SYS_STATS (NO_SYS == 0) -#endif - -#else - -#define LINK_STATS 0 -#define IP_STATS 0 -#define IPFRAG_STATS 0 -#define ICMP_STATS 0 -#define IGMP_STATS 0 -#define UDP_STATS 0 -#define TCP_STATS 0 -#define MEM_STATS 0 -#define MEMP_STATS 0 -#define SYS_STATS 0 -#define LWIP_STATS_DISPLAY 0 - -#endif /* LWIP_STATS */ - -/* - --------------------------------- - ---------- PPP options ---------- - --------------------------------- -*/ -/** - * PPP_SUPPORT==1: Enable PPP. - */ -#ifndef PPP_SUPPORT -#define PPP_SUPPORT 0 -#endif - -/** - * PPPOE_SUPPORT==1: Enable PPP Over Ethernet - */ -#ifndef PPPOE_SUPPORT -#define PPPOE_SUPPORT 0 -#endif - -/** - * PPPOS_SUPPORT==1: Enable PPP Over Serial - */ -#ifndef PPPOS_SUPPORT -#define PPPOS_SUPPORT PPP_SUPPORT -#endif - -#if PPP_SUPPORT - -/** - * NUM_PPP: Max PPP sessions. - */ -#ifndef NUM_PPP -#define NUM_PPP 1 -#endif - -/** - * PAP_SUPPORT==1: Support PAP. - */ -#ifndef PAP_SUPPORT -#define PAP_SUPPORT 0 -#endif - -/** - * CHAP_SUPPORT==1: Support CHAP. - */ -#ifndef CHAP_SUPPORT -#define CHAP_SUPPORT 0 -#endif - -/** - * MSCHAP_SUPPORT==1: Support MSCHAP. CURRENTLY NOT SUPPORTED! DO NOT SET! - */ -#ifndef MSCHAP_SUPPORT -#define MSCHAP_SUPPORT 0 -#endif - -/** - * CBCP_SUPPORT==1: Support CBCP. CURRENTLY NOT SUPPORTED! DO NOT SET! - */ -#ifndef CBCP_SUPPORT -#define CBCP_SUPPORT 0 -#endif - -/** - * CCP_SUPPORT==1: Support CCP. CURRENTLY NOT SUPPORTED! DO NOT SET! - */ -#ifndef CCP_SUPPORT -#define CCP_SUPPORT 0 -#endif - -/** - * VJ_SUPPORT==1: Support VJ header compression. - */ -#ifndef VJ_SUPPORT -#define VJ_SUPPORT 0 -#endif - -/** - * MD5_SUPPORT==1: Support MD5 (see also CHAP). - */ -#ifndef MD5_SUPPORT -#define MD5_SUPPORT 0 -#endif - -/* - * Timeouts - */ -#ifndef FSM_DEFTIMEOUT -#define FSM_DEFTIMEOUT 6 /* Timeout time in seconds */ -#endif - -#ifndef FSM_DEFMAXTERMREQS -#define FSM_DEFMAXTERMREQS 2 /* Maximum Terminate-Request transmissions */ -#endif - -#ifndef FSM_DEFMAXCONFREQS -#define FSM_DEFMAXCONFREQS 10 /* Maximum Configure-Request transmissions */ -#endif - -#ifndef FSM_DEFMAXNAKLOOPS -#define FSM_DEFMAXNAKLOOPS 5 /* Maximum number of nak loops */ -#endif - -#ifndef UPAP_DEFTIMEOUT -#define UPAP_DEFTIMEOUT 6 /* Timeout (seconds) for retransmitting req */ -#endif - -#ifndef UPAP_DEFREQTIME -#define UPAP_DEFREQTIME 30 /* Time to wait for auth-req from peer */ -#endif - -#ifndef CHAP_DEFTIMEOUT -#define CHAP_DEFTIMEOUT 6 /* Timeout time in seconds */ -#endif - -#ifndef CHAP_DEFTRANSMITS -#define CHAP_DEFTRANSMITS 10 /* max # times to send challenge */ -#endif - -/* Interval in seconds between keepalive echo requests, 0 to disable. */ -#ifndef LCP_ECHOINTERVAL -#define LCP_ECHOINTERVAL 0 -#endif - -/* Number of unanswered echo requests before failure. */ -#ifndef LCP_MAXECHOFAILS -#define LCP_MAXECHOFAILS 3 -#endif - -/* Max Xmit idle time (in jiffies) before resend flag char. */ -#ifndef PPP_MAXIDLEFLAG -#define PPP_MAXIDLEFLAG 100 -#endif - -/* - * Packet sizes - * - * Note - lcp shouldn't be allowed to negotiate stuff outside these - * limits. See lcp.h in the pppd directory. - * (XXX - these constants should simply be shared by lcp.c instead - * of living in lcp.h) - */ -#define PPP_MTU 1500 /* Default MTU (size of Info field) */ -#ifndef PPP_MAXMTU -/* #define PPP_MAXMTU 65535 - (PPP_HDRLEN + PPP_FCSLEN) */ -#define PPP_MAXMTU 1500 /* Largest MTU we allow */ -#endif -#define PPP_MINMTU 64 -#define PPP_MRU 1500 /* default MRU = max length of info field */ -#define PPP_MAXMRU 1500 /* Largest MRU we allow */ -#ifndef PPP_DEFMRU -#define PPP_DEFMRU 296 /* Try for this */ -#endif -#define PPP_MINMRU 128 /* No MRUs below this */ - -#ifndef MAXNAMELEN -#define MAXNAMELEN 256 /* max length of hostname or name for auth */ -#endif -#ifndef MAXSECRETLEN -#define MAXSECRETLEN 256 /* max length of password or secret */ -#endif - -#endif /* PPP_SUPPORT */ - -/* - -------------------------------------- - ---------- Checksum options ---------- - -------------------------------------- -*/ -/** - * CHECKSUM_GEN_IP==1: Generate checksums in software for outgoing IP packets. - */ -#ifndef CHECKSUM_GEN_IP -#define CHECKSUM_GEN_IP 1 -#endif - -/** - * CHECKSUM_GEN_UDP==1: Generate checksums in software for outgoing UDP packets. - */ -#ifndef CHECKSUM_GEN_UDP -#define CHECKSUM_GEN_UDP 1 -#endif - -/** - * CHECKSUM_GEN_TCP==1: Generate checksums in software for outgoing TCP packets. - */ -#ifndef CHECKSUM_GEN_TCP -#define CHECKSUM_GEN_TCP 1 -#endif - -/** - * CHECKSUM_GEN_ICMP==1: Generate checksums in software for outgoing ICMP packets. - */ -#ifndef CHECKSUM_GEN_ICMP -#define CHECKSUM_GEN_ICMP 1 -#endif - -/** - * CHECKSUM_CHECK_IP==1: Check checksums in software for incoming IP packets. - */ -#ifndef CHECKSUM_CHECK_IP -#define CHECKSUM_CHECK_IP 1 -#endif - -/** - * CHECKSUM_CHECK_UDP==1: Check checksums in software for incoming UDP packets. - */ -#ifndef CHECKSUM_CHECK_UDP -#define CHECKSUM_CHECK_UDP 1 -#endif - -/** - * CHECKSUM_CHECK_TCP==1: Check checksums in software for incoming TCP packets. - */ -#ifndef CHECKSUM_CHECK_TCP -#define CHECKSUM_CHECK_TCP 1 -#endif - -/** - * LWIP_CHECKSUM_ON_COPY==1: Calculate checksum when copying data from - * application buffers to pbufs. - */ -#ifndef LWIP_CHECKSUM_ON_COPY -#define LWIP_CHECKSUM_ON_COPY 0 -#endif - -/* - --------------------------------------- - ---------- Hook options --------------- - --------------------------------------- -*/ - -/* Hooks are undefined by default, define them to a function if you need them. */ - -/** - * LWIP_HOOK_IP4_INPUT(pbuf, input_netif): - * - called from ip_input() (IPv4) - * - pbuf: received struct pbuf passed to ip_input() - * - input_netif: struct netif on which the packet has been received - * Return values: - * - 0: Hook has not consumed the packet, packet is processed as normal - * - != 0: Hook has consumed the packet. - * If the hook consumed the packet, 'pbuf' is in the responsibility of the hook - * (i.e. free it when done). - */ - -/** - * LWIP_HOOK_IP4_ROUTE(dest): - * - called from ip_route() (IPv4) - * - dest: destination IPv4 address - * Returns the destination netif or NULL if no destination netif is found. In - * that case, ip_route() continues as normal. - */ - -/* - --------------------------------------- - ---------- Debugging options ---------- - --------------------------------------- -*/ -/** - * LWIP_DBG_MIN_LEVEL: After masking, the value of the debug is - * compared against this value. If it is smaller, then debugging - * messages are written. - */ -#ifndef LWIP_DBG_MIN_LEVEL -#define LWIP_DBG_MIN_LEVEL LWIP_DBG_LEVEL_ALL -#endif - -/** - * LWIP_DBG_TYPES_ON: A mask that can be used to globally enable/disable - * debug messages of certain types. - */ -#ifndef LWIP_DBG_TYPES_ON -#define LWIP_DBG_TYPES_ON LWIP_DBG_ON -#endif - -/** - * ETHARP_DEBUG: Enable debugging in etharp.c. - */ -#ifndef ETHARP_DEBUG -#define ETHARP_DEBUG LWIP_DBG_ON -#endif - -/** - * NETIF_DEBUG: Enable debugging in netif.c. - */ -#ifndef NETIF_DEBUG -#define NETIF_DEBUG LWIP_DBG_OFF -#endif - -/** - * PBUF_DEBUG: Enable debugging in pbuf.c. - */ -#ifndef PBUF_DEBUG -#define PBUF_DEBUG LWIP_DBG_OFF -#endif - -/** - * API_LIB_DEBUG: Enable debugging in api_lib.c. - */ -#ifndef API_LIB_DEBUG -#define API_LIB_DEBUG LWIP_DBG_OFF -#endif - -/** - * API_MSG_DEBUG: Enable debugging in api_msg.c. - */ -#ifndef API_MSG_DEBUG -#define API_MSG_DEBUG LWIP_DBG_OFF -#endif - -/** - * SOCKETS_DEBUG: Enable debugging in sockets.c. - */ -#ifndef SOCKETS_DEBUG -#define SOCKETS_DEBUG LWIP_DBG_OFF -#endif - -/** - * ICMP_DEBUG: Enable debugging in icmp.c. - */ -#ifndef ICMP_DEBUG -#define ICMP_DEBUG LWIP_DBG_OFF -#endif - -/** - * IGMP_DEBUG: Enable debugging in igmp.c. - */ -#ifndef IGMP_DEBUG -#define IGMP_DEBUG LWIP_DBG_OFF -#endif - -/** - * INET_DEBUG: Enable debugging in inet.c. - */ -#ifndef INET_DEBUG -#define INET_DEBUG LWIP_DBG_OFF -#endif - -/** - * IP_DEBUG: Enable debugging for IP. - */ -#ifndef IP_DEBUG -#define IP_DEBUG LWIP_DBG_OFF -#endif - -/** - * IP_REASS_DEBUG: Enable debugging in ip_frag.c for both frag & reass. - */ -#ifndef IP_REASS_DEBUG -#define IP_REASS_DEBUG LWIP_DBG_OFF -#endif - -/** - * RAW_DEBUG: Enable debugging in raw.c. - */ -#ifndef RAW_DEBUG -#define RAW_DEBUG LWIP_DBG_ON -#endif - -/** - * MEM_DEBUG: Enable debugging in mem.c. - */ -#ifndef MEM_DEBUG -#define MEM_DEBUG LWIP_DBG_ON -#endif - -/** - * MEMP_DEBUG: Enable debugging in memp.c. - */ -#ifndef MEMP_DEBUG -#define MEMP_DEBUG LWIP_DBG_OFF -#endif - -/** - * SYS_DEBUG: Enable debugging in sys.c. - */ -#ifndef SYS_DEBUG -#define SYS_DEBUG LWIP_DBG_ON -#endif - -/** - * TIMERS_DEBUG: Enable debugging in timers.c. - */ -#ifndef TIMERS_DEBUG -#define TIMERS_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_DEBUG: Enable debugging for TCP. - */ -#ifndef TCP_DEBUG -#define TCP_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_INPUT_DEBUG: Enable debugging in tcp_in.c for incoming debug. - */ -#ifndef TCP_INPUT_DEBUG -#define TCP_INPUT_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_FR_DEBUG: Enable debugging in tcp_in.c for fast retransmit. - */ -#ifndef TCP_FR_DEBUG -#define TCP_FR_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_RTO_DEBUG: Enable debugging in TCP for retransmit - * timeout. - */ -#ifndef TCP_RTO_DEBUG -#define TCP_RTO_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_CWND_DEBUG: Enable debugging for TCP congestion window. - */ -#ifndef TCP_CWND_DEBUG -#define TCP_CWND_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_WND_DEBUG: Enable debugging in tcp_in.c for window updating. - */ -#ifndef TCP_WND_DEBUG -#define TCP_WND_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_OUTPUT_DEBUG: Enable debugging in tcp_out.c output functions. - */ -#ifndef TCP_OUTPUT_DEBUG -#define TCP_OUTPUT_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_RST_DEBUG: Enable debugging for TCP with the RST message. - */ -#ifndef TCP_RST_DEBUG -#define TCP_RST_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCP_QLEN_DEBUG: Enable debugging for TCP queue lengths. - */ -#ifndef TCP_QLEN_DEBUG -#define TCP_QLEN_DEBUG LWIP_DBG_OFF -#endif - -/** - * UDP_DEBUG: Enable debugging in UDP. - */ -#ifndef UDP_DEBUG -#define UDP_DEBUG LWIP_DBG_OFF -#endif - -/** - * TCPIP_DEBUG: Enable debugging in tcpip.c. - */ -#ifndef TCPIP_DEBUG -#define TCPIP_DEBUG LWIP_DBG_ON -#endif - -/** - * PPP_DEBUG: Enable debugging for PPP. - */ -#ifndef PPP_DEBUG -#define PPP_DEBUG LWIP_DBG_OFF -#endif - -/** - * SLIP_DEBUG: Enable debugging in slipif.c. - */ -#ifndef SLIP_DEBUG -#define SLIP_DEBUG LWIP_DBG_OFF -#endif - -/** - * DHCP_DEBUG: Enable debugging in dhcp.c. - */ -#ifndef DHCP_DEBUG -#define DHCP_DEBUG LWIP_DBG_OFF -#endif - -/** - * AUTOIP_DEBUG: Enable debugging in autoip.c. - */ -#ifndef AUTOIP_DEBUG -#define AUTOIP_DEBUG LWIP_DBG_OFF -#endif - -/** - * SNMP_MSG_DEBUG: Enable debugging for SNMP messages. - */ -#ifndef SNMP_MSG_DEBUG -#define SNMP_MSG_DEBUG LWIP_DBG_OFF -#endif - -/** - * SNMP_MIB_DEBUG: Enable debugging for SNMP MIBs. - */ -#ifndef SNMP_MIB_DEBUG -#define SNMP_MIB_DEBUG LWIP_DBG_OFF -#endif - -/** - * DNS_DEBUG: Enable debugging for DNS. - */ -#ifndef DNS_DEBUG -#define DNS_DEBUG LWIP_DBG_OFF -#endif - -#define LWIP_RAND chibios_rand_generate - -#endif /* __LWIPOPT_H__ */ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index d9b30d5a69379b..bf39aa6d448072 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -616,3 +616,48 @@ void ff_memfree(void* mblock) free(mblock); } #endif // USE_POSIX + +/* + return true if a memory region is safe for a DMA operation + */ +bool mem_is_dma_safe(const void *addr, uint32_t size, bool filesystem_op) +{ + (void)filesystem_op; +#if defined(STM32F1) + // F1 is always OK + (void)addr; + (void)size; + return true; +#else + uint32_t flags = MEM_REGION_FLAG_DMA_OK; +#if defined(STM32H7) + if (!filesystem_op) { + // use bouncebuffer for all non FS ops on H7 + return false; + } + if (((uint32_t)addr) & 0x1F) { + return false; + } + if (filesystem_op) { + flags = MEM_REGION_FLAG_AXI_BUS; + } +#elif defined(STM32F4) + if (((uint32_t)addr) & 0x01) { + return false; + } +#else + if (((uint32_t)addr) & 0x07) { + return false; + } +#endif + for (uint8_t i=0; i= (uint32_t)memory_regions[i].address && + ((uint32_t)addr + size) <= ((uint32_t)memory_regions[i].address + memory_regions[i].size)) { + return true; + } + } + } + return false; +#endif // STM32F1 +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h index 75915cced112d0..cd032473d5fd65 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/mcuconf.h @@ -53,8 +53,10 @@ #include "stm32f3_mcuconf.h" #elif defined(STM32F4) || defined(STM32F7) #include "stm32f47_mcuconf.h" -#elif defined(STM32H730xx) +#elif defined(STM32H730xx) || defined(STM32H723xx) #include "stm32h7_type2_mcuconf.h" +#elif defined(STM32H7A3xx) +#include "stm32h7_A3_mcuconf.h" #elif defined(STM32H7) #include "stm32h7_mcuconf.h" #elif defined(STM32G4) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 0a56342d256fdb..828ceed09ca69a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h @@ -43,6 +43,7 @@ void *malloc_axi_sram(size_t size); void *malloc_fastmem(size_t size); void *malloc_eth_safe(size_t size); thread_t *thread_create_alloc(size_t size, const char *name, tprio_t prio, tfunc_t pf, void *arg); +bool mem_is_dma_safe(const void *addr, uint32_t size, bool filesystem_op); struct memory_region { void *address; @@ -185,10 +186,10 @@ void stm32_flash_protect_flash(bool bootloader, bool protect); void stm32_flash_unprotect_flash(void); // allow stack view code to show free ISR stack -extern uint32_t __main_stack_base__; -extern uint32_t __main_stack_end__; -extern uint32_t __main_thread_stack_base__; -extern uint32_t __main_thread_stack_end__; +extern stkalign_t __main_stack_base__; +extern stkalign_t __main_stack_end__; +extern stkalign_t __main_thread_stack_base__; +extern stkalign_t __main_thread_stack_end__; #ifdef __cplusplus } diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h index 7cd46d1ac225ca..3c692db1d5b84b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32g4_mcuconf.h @@ -25,6 +25,12 @@ #define STM32_LSEDRV (3U << 3U) #endif +/* + we need to use HAL_LLD_USE_CLOCK_MANAGEMENT in order to allow a + different clock tree in the main firmware from the bootloader. + */ +#define HAL_LLD_USE_CLOCK_MANAGEMENT 1 + /* * STM32G4xx drivers configuration. * The following settings override the default settings present in @@ -89,10 +95,10 @@ #define STM32_LSI_ENABLED FALSE #define STM32_LSE_ENABLED FALSE #define STM32_SW STM32_SW_PLLRCLK -#define STM32_PLLN_VALUE 42 +#define STM32_PLLN_VALUE 40 #define STM32_PLLPDIV_VALUE 0 #define STM32_PLLP_VALUE 7 -#define STM32_PLLQ_VALUE 8 +#define STM32_PLLQ_VALUE 4 #define STM32_PLLR_VALUE 2 #define STM32_HPRE STM32_HPRE_DIV1 #define STM32_PPRE1 STM32_PPRE1_DIV1 @@ -117,11 +123,7 @@ #define STM32_LPTIM1SEL STM32_LPTIM1SEL_PCLK1 #define STM32_SAI1SEL STM32_SAI1SEL_SYSCLK #define STM32_I2S23SEL STM32_I2S23SEL_SYSCLK -#if STM32_HSECLK == 0U #define STM32_FDCANSEL STM32_FDCANSEL_PLLQCLK -#else -#define STM32_FDCANSEL STM32_FDCANSEL_HSE -#endif #define STM32_CLK48SEL STM32_CLK48SEL_HSI48 #define STM32_ADC12SEL STM32_ADC12SEL_PLLPCLK #define STM32_ADC345SEL STM32_ADC345SEL_PLLPCLK diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_A3_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_A3_mcuconf.h new file mode 100644 index 00000000000000..6472f71a67b813 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_A3_mcuconf.h @@ -0,0 +1,584 @@ +/* + ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +/* + this header is modelled on the one for the Nucleo-144 H743 board from ChibiOS + */ +#pragma once + +// this is a newer H7 varient +#define STM32_ENFORCE_H7_REV_XY + +// MPU region for ethernet +#define STM32_NOCACHE_MPU_REGION_ETH MPU_REGION_2 + +#ifndef STM32_LSECLK +#define STM32_LSECLK 32768U +#endif +#ifndef STM32_LSEDRV +#define STM32_LSEDRV (3U << 3U) +#endif + +/* + * STM32H7xx drivers configuration. + * The following settings override the default settings present in + * the various device driver implementation headers. + * Note that the settings for each driver only have effect if the whole + * driver is enabled in halconf.h. + * + * IRQ priorities: + * 15...0 Lowest...Highest. + * + * DMA priorities: + * 0...3 Lowest...Highest. + */ + +#define STM32H7xx_MCUCONF +#define STM32H7A3_MCUCONF + +/* + * General settings. + */ +#ifndef STM32_NO_INIT +#define STM32_NO_INIT FALSE +#endif +#define STM32_TARGET_CORE 1 + +/* + * Memory attributes settings. + */ +// Disable ChibiOS memory protection which is fixed to SRAM1-3 +#define STM32_NOCACHE_ENABLE FALSE + +// enable memory protection on SRAM4, used for bdshot +#define STM32_NOCACHE_MPU_REGION_1 MPU_REGION_5 +#define STM32_NOCACHE_MPU_REGION_1_BASE 0x38000000U +#define STM32_NOCACHE_MPU_REGION_1_SIZE MPU_RASR_SIZE_16K + +// enable memory protection on part of AXI used for SDMMC +#define STM32_NOCACHE_MPU_REGION_2 MPU_REGION_4 +#define STM32_NOCACHE_MPU_REGION_2_BASE 0x24044000 +#define STM32_NOCACHE_MPU_REGION_2_SIZE MPU_RASR_SIZE_16K + +/* + * PWR system settings. + * Reading STM32 Reference Manual is required, settings in PWR_CR3 are + * very critical. + * Register constants are taken from the ST header. + */ +#define STM32_PWR_CR1 (PWR_CR1_SVOS_1 | PWR_CR1_SVOS_0) +#define STM32_PWR_CR2 (PWR_CR2_BREN) +#ifdef SMPS_PWR +#define STM32_PWR_CR3 (PWR_CR3_SMPSEN | PWR_CR3_USB33DEN) +#else +#define STM32_PWR_CR3 (PWR_CR3_LDOEN | PWR_CR3_USB33DEN) +#endif +#define STM32_PWR_CPUCR 0 + +/* + * Clock tree static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_LSI_ENABLED FALSE +#define STM32_CSI_ENABLED FALSE +#define STM32_HSI48_ENABLED TRUE +#define STM32_LSE_ENABLED FALSE +#define STM32_HSIDIV STM32_HSIDIV_DIV1 + +/* + * Setup clocks for the STM32H7A3 + */ +#define STM32_VOS STM32_VOS_SCALE0 +/* + setup PLLs based on HSE clock + */ +#if STM32_HSECLK == 0U +// no crystal, this gives 280MHz system clock +#define STM32_HSE_ENABLED FALSE +#define STM32_HSI_ENABLED TRUE // HSI is 64MHzs +#define STM32_PLL1_DIVM_VALUE 32 +#define STM32_PLL2_DIVM_VALUE 32 +#define STM32_PLL3_DIVM_VALUE 32 +#define STM32_PLLSRC STM32_PLLSRC_HSI_CK +#define STM32_MCO1SEL STM32_MCO1SEL_HSI_CK +#define STM32_CKPERSEL STM32_CKPERSEL_HSI_CK + +#elif STM32_HSECLK == 8000000U +#define STM32_HSE_ENABLED TRUE +#define STM32_HSI_ENABLED FALSE +#define STM32_PLL1_DIVM_VALUE 4 +#define STM32_PLL2_DIVM_VALUE 4 +#define STM32_PLL3_DIVM_VALUE 4 +#define STM32_PLLSRC STM32_PLLSRC_HSE_CK +#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK + +#elif STM32_HSECLK == 16000000U +#define STM32_HSE_ENABLED TRUE +#define STM32_HSI_ENABLED FALSE +#define STM32_PLL1_DIVM_VALUE 8 +#define STM32_PLL2_DIVM_VALUE 8 +#define STM32_PLL3_DIVM_VALUE 8 +#define STM32_PLLSRC STM32_PLLSRC_HSE_CK +#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK + +#else +#error "Unsupported HSE clock" +#endif + +#if (STM32_HSECLK == 0U) || (STM32_HSECLK == 8000000U) || (STM32_HSECLK == 16000000U) +// common clock tree for multiples of 8MHz crystals +#define STM32_PLL1_DIVN_VALUE 280 +#define STM32_PLL1_DIVP_VALUE 2 +#define STM32_PLL1_DIVQ_VALUE 2 +#define STM32_PLL1_DIVR_VALUE 2 + +#define STM32_PLL2_DIVN_VALUE 160 +#define STM32_PLL2_DIVP_VALUE 2 +#define STM32_PLL2_DIVQ_VALUE 4 // 80MHz for FDCAN +#define STM32_PLL2_DIVR_VALUE 2 + +#define STM32_PLL3_DIVN_VALUE 100 +#define STM32_PLL3_DIVP_VALUE 2 +#define STM32_PLL3_DIVQ_VALUE 2 +#define STM32_PLL3_DIVR_VALUE 4 +#endif // clock selection + +/* + * PLLs static settings. + * Reading STM32 Reference Manual is required. + */ +#define STM32_PLLCFGR_MASK ~0 + +#define STM32_PLL1_ENABLED TRUE +#define STM32_PLL1_P_ENABLED TRUE +#define STM32_PLL1_Q_ENABLED TRUE +#define STM32_PLL1_R_ENABLED TRUE +#define STM32_PLL1_FRACN_VALUE 0 + +#define STM32_PLL2_ENABLED TRUE +#define STM32_PLL2_P_ENABLED TRUE +#define STM32_PLL2_Q_ENABLED TRUE +#define STM32_PLL2_R_ENABLED TRUE +#define STM32_PLL2_FRACN_VALUE 0 + +#define STM32_PLL3_ENABLED TRUE +#define STM32_PLL3_P_ENABLED TRUE +#define STM32_PLL3_Q_ENABLED TRUE +#define STM32_PLL3_R_ENABLED TRUE +#define STM32_PLL3_FRACN_VALUE 0 + +/* + * Core clocks dynamic settings (can be changed at runtime). + * Reading STM32 Reference Manual is required. + */ +#define STM32_SW STM32_SW_PLL1_P_CK +#define STM32_RTCSEL STM32_RTCSEL_LSI_CK +#define STM32_D1CPRE STM32_D1CPRE_DIV1 +#define STM32_D1HPRE STM32_D1HPRE_DIV2 +#define STM32_D1PPRE3 STM32_D1PPRE3_DIV2 +#define STM32_D2PPRE1 STM32_D2PPRE1_DIV2 +#define STM32_D2PPRE2 STM32_D2PPRE2_DIV2 +#define STM32_D3PPRE4 STM32_D3PPRE4_DIV2 + +/* + * Peripherals clocks static settings. + * Reading STM32 Reference Manual is required. + */ +#ifndef STM32_MCO1SEL +#define STM32_MCO1SEL STM32_MCO1SEL_HSE_CK +#endif +#ifndef STM32_MCO1PRE_VALUE +#define STM32_MCO1PRE_VALUE 1 +#endif +#ifndef STM32_MCO2SEL +#define STM32_MCO2SEL STM32_MCO2SEL_SYS_CK +#endif +#ifndef STM32_MCO2PRE_VALUE +#define STM32_MCO2PRE_VALUE 1 +#endif +#define STM32_TIMPRE_ENABLE TRUE +#define STM32_HRTIMSEL 0 +#define STM32_STOPKERWUCK 0 +#define STM32_STOPWUCK 0 +#define STM32_RTCPRE_VALUE 2 +#define STM32_SDMMCSEL STM32_SDMMCSEL_PLL2_R_CK +#define STM32_OCTOSPISEL STM32_OCTOSPISEL_HCLK +#define STM32_FMCSEL STM32_OCTOSPISEL_HCLK + +#define STM32_SWPSEL STM32_SWPSEL_PCLK1 +#define STM32_FDCANSEL STM32_FDCANSEL_PLL2_Q_CK +#define STM32_DFSDM1SEL STM32_DFSDM1SEL_PCLK2 +#define STM32_SPDIFSEL STM32_SPDIFSEL_PLL1_Q_CK +#define STM32_SPI45SEL STM32_SPI45SEL_PCLK2 +#define STM32_SPI123SEL STM32_SPI123SEL_PLL3_P_CK +#define STM32_SAI1SEL STM32_SAI1SEL_PLL1_Q_CK +#define STM32_LPTIM1SEL STM32_LPTIM1SEL_PCLK1 +#define STM32_LPTIM3SEL STM32_LPTIM3SEL_PLL3_P_CK +#define STM32_CECSEL STM32_CECSEL_DISABLE +#define STM32_USBSEL STM32_USBSEL_HSI48_CK +#define STM32_I2C123SEL STM32_I2C123SEL_PLL3_R_CK +#define STM32_RNGSEL STM32_RNGSEL_HSI48_CK +#define STM32_USART16SEL STM32_USART16SEL_PLL3_Q_CK +#define STM32_USART234578SEL STM32_USART234578SEL_PLL3_Q_CK +#define STM32_SPI6SEL STM32_SPI6SEL_PLL3_Q_CK +#define STM32_SAI4BSEL STM32_SAI4BSEL_PLL1_Q_CK +#define STM32_SAI4ASEL STM32_SAI4ASEL_PLL1_Q_CK +#define STM32_ADCSEL STM32_ADCSEL_PLL3_R_CK +#define STM32_LPTIM345SEL STM32_LPTIM345SEL_PCLK4 +#define STM32_LPTIM2SEL STM32_LPTIM2SEL_PCLK4 +#define STM32_I2C4SEL STM32_I2C4SEL_PLL3_R_CK +#define STM32_LPUART1SEL STM32_LPUART1SEL_PCLK4 // really PCLK3 + + +/* + * IRQ system settings. + */ +#define STM32_IRQ_EXTI0_PRIORITY 6 +#define STM32_IRQ_EXTI1_PRIORITY 6 +#define STM32_IRQ_EXTI2_PRIORITY 6 +#define STM32_IRQ_EXTI3_PRIORITY 6 +#define STM32_IRQ_EXTI4_PRIORITY 6 +#define STM32_IRQ_EXTI5_9_PRIORITY 6 +#define STM32_IRQ_EXTI10_15_PRIORITY 6 +#define STM32_IRQ_EXTI16_PRIORITY 6 +#define STM32_IRQ_EXTI17_PRIORITY 15 +#define STM32_IRQ_EXTI18_PRIORITY 6 +#define STM32_IRQ_EXTI19_PRIORITY 6 +#define STM32_IRQ_EXTI20_21_PRIORITY 6 + +#define STM32_IRQ_FDCAN1_PRIORITY 10 +#define STM32_IRQ_FDCAN2_PRIORITY 10 + +#define STM32_IRQ_MDMA_PRIORITY 9 +#define STM32_IRQ_OCTOSPI1_PRIORITY 10 +#define STM32_IRQ_OCTOSPI2_PRIORITY 10 + +#define STM32_IRQ_SDMMC1_PRIORITY 9 +#define STM32_IRQ_SDMMC2_PRIORITY 9 + +#define STM32_IRQ_TIM1_UP_PRIORITY 7 +#define STM32_IRQ_TIM1_CC_PRIORITY 7 +#define STM32_IRQ_TIM2_PRIORITY 7 +#define STM32_IRQ_TIM3_PRIORITY 7 +#define STM32_IRQ_TIM4_PRIORITY 7 +#define STM32_IRQ_TIM5_PRIORITY 7 +#define STM32_IRQ_TIM6_PRIORITY 7 +#define STM32_IRQ_TIM7_PRIORITY 7 +#define STM32_IRQ_TIM8_BRK_TIM12_PRIORITY 7 +#define STM32_IRQ_TIM8_UP_TIM13_PRIORITY 7 +#define STM32_IRQ_TIM8_TRGCO_TIM14_PRIORITY 7 +#define STM32_IRQ_TIM8_CC_PRIORITY 7 +#define STM32_IRQ_TIM15_PRIORITY 7 +#define STM32_IRQ_TIM16_PRIORITY 7 +#define STM32_IRQ_TIM17_PRIORITY 7 + +#define STM32_IRQ_USART1_PRIORITY 12 +#define STM32_IRQ_USART2_PRIORITY 12 +#define STM32_IRQ_USART3_PRIORITY 12 +#define STM32_IRQ_UART4_PRIORITY 12 +#define STM32_IRQ_UART5_PRIORITY 12 +#define STM32_IRQ_USART6_PRIORITY 12 +#define STM32_IRQ_UART7_PRIORITY 12 +#define STM32_IRQ_UART8_PRIORITY 12 +#define STM32_IRQ_UART9_PRIORITY 12 +#define STM32_IRQ_USART10_PRIORITY 12 + +/* + * ADC driver system settings. + */ +#ifndef STM32_ADC_DUAL_MODE +#define STM32_ADC_DUAL_MODE FALSE +#endif +#ifndef STM32_ADC_SAMPLES_SIZE +#define STM32_ADC_SAMPLES_SIZE 16 +#endif +#define STM32_ADC_COMPACT_SAMPLES FALSE +#define STM32_ADC_USE_ADC12 TRUE +#define STM32_ADC_ADC12_DMA_PRIORITY 2 +#define STM32_ADC_ADC3_DMA_PRIORITY 2 +#define STM32_ADC_ADC12_IRQ_PRIORITY 5 +#define STM32_ADC_ADC3_IRQ_PRIORITY 5 +#define STM32_ADC_ADC12_CLOCK_MODE ADC_CCR_CKMODE_ADCCK +#define STM32_ADC_ADC3_CLOCK_MODE ADC_CCR_CKMODE_ADCCK + +// we call it ADC1 in hwdef.dat, but driver uses ADC12 for DMA stream +#ifdef STM32_ADC_ADC1_DMA_STREAM +#define STM32_ADC_ADC12_DMA_STREAM STM32_ADC_ADC1_DMA_STREAM +#elif defined(STM32_ADC_ADC2_DMA_STREAM) +#define STM32_ADC_ADC12_DMA_STREAM STM32_ADC_ADC2_DMA_STREAM +#endif + +/* + * CAN driver system settings. + */ +#define STM32_CAN_USE_FDCAN1 FALSE +#define STM32_CAN_USE_FDCAN2 FALSE + +/* + * DAC driver system settings. + */ +#define STM32_DAC_DUAL_MODE FALSE +#define STM32_DAC_USE_DAC1_CH1 FALSE +#define STM32_DAC_USE_DAC1_CH2 FALSE +#define STM32_DAC_DAC1_CH1_IRQ_PRIORITY 10 +#define STM32_DAC_DAC1_CH2_IRQ_PRIORITY 10 +#define STM32_DAC_DAC1_CH1_DMA_PRIORITY 2 +#define STM32_DAC_DAC1_CH2_DMA_PRIORITY 2 + +/* + * GPT driver system settings. + */ +#define STM32_GPT_USE_TIM1 FALSE +#define STM32_GPT_USE_TIM2 FALSE +#define STM32_GPT_USE_TIM3 FALSE +#define STM32_GPT_USE_TIM4 FALSE +#define STM32_GPT_USE_TIM5 FALSE +#define STM32_GPT_USE_TIM6 FALSE +#define STM32_GPT_USE_TIM7 FALSE +#define STM32_GPT_USE_TIM8 FALSE +#define STM32_GPT_USE_TIM12 FALSE +#define STM32_GPT_USE_TIM13 FALSE +#define STM32_GPT_USE_TIM14 FALSE +#define STM32_GPT_USE_TIM15 FALSE +#define STM32_GPT_USE_TIM16 FALSE +#define STM32_GPT_USE_TIM17 FALSE + +/* + * I2C driver system settings. + */ +#define STM32_I2C_BUSY_TIMEOUT 50 +#define STM32_I2C_I2C1_IRQ_PRIORITY 5 +#define STM32_I2C_I2C2_IRQ_PRIORITY 5 +#define STM32_I2C_I2C3_IRQ_PRIORITY 5 +#define STM32_I2C_I2C4_IRQ_PRIORITY 5 +#define STM32_I2C_I2C1_DMA_PRIORITY 3 +#define STM32_I2C_I2C2_DMA_PRIORITY 3 +#define STM32_I2C_I2C3_DMA_PRIORITY 3 +#define STM32_I2C_I2C4_DMA_PRIORITY 3 +#define STM32_I2C_DMA_ERROR_HOOK(i2cp) STM32_DMA_ERROR_HOOK(i2cp) + +/* + * ICU driver system settings. + */ +#define STM32_ICU_USE_TIM1 FALSE +#define STM32_ICU_USE_TIM2 FALSE +#define STM32_ICU_USE_TIM3 FALSE +#define STM32_ICU_USE_TIM4 FALSE +#define STM32_ICU_USE_TIM5 FALSE +#define STM32_ICU_USE_TIM8 FALSE +#define STM32_ICU_USE_TIM9 FALSE +#define STM32_ICU_TIM1_IRQ_PRIORITY 7 +#define STM32_ICU_TIM2_IRQ_PRIORITY 7 +#define STM32_ICU_TIM3_IRQ_PRIORITY 7 +#define STM32_ICU_TIM4_IRQ_PRIORITY 7 +#define STM32_ICU_TIM5_IRQ_PRIORITY 7 +#define STM32_ICU_TIM8_IRQ_PRIORITY 7 +#define STM32_ICU_TIM9_IRQ_PRIORITY 7 + +/* + * MAC driver system settings. + */ +#define STM32_MAC_TRANSMIT_BUFFERS 2 +#define STM32_MAC_RECEIVE_BUFFERS 4 +#define STM32_MAC_BUFFERS_SIZE 1522 +#define STM32_MAC_PHY_TIMEOUT 100 +#define STM32_MAC_ETH1_CHANGE_PHY_STATE TRUE +#define STM32_MAC_ETH1_IRQ_PRIORITY 13 +#define STM32_MAC_IP_CHECKSUM_OFFLOAD 0 + +/* + * PWM driver system settings. + */ + +/* + * RTC driver system settings. + */ +#define STM32_RTC_PRESA_VALUE 32 +#define STM32_RTC_PRESS_VALUE 1024 +#define STM32_RTC_CR_INIT 0 +#define STM32_RTC_TAMPCR_INIT 0 + +/* + * SDC driver system settings. + */ +#define STM32_SDC_SDMMC_UNALIGNED_SUPPORT TRUE +#define STM32_SDC_SDMMC_WRITE_TIMEOUT 1000 +#define STM32_SDC_SDMMC_READ_TIMEOUT 1000 +#define STM32_SDC_SDMMC_CLOCK_DELAY 10 +#define STM32_SDC_SDMMC1_DMA_PRIORITY 3 +#define STM32_SDC_SDMMC1_IRQ_PRIORITY 9 +#define STM32_SDC_SDMMC_PWRSAV TRUE + +/* + * SERIAL driver system settings. + */ +#define STM32_SERIAL_USART1_PRIORITY 12 +#define STM32_SERIAL_USART2_PRIORITY 12 +#define STM32_SERIAL_USART3_PRIORITY 12 +#define STM32_SERIAL_UART4_PRIORITY 12 +#define STM32_SERIAL_UART5_PRIORITY 12 +#define STM32_SERIAL_USART6_PRIORITY 12 +#define STM32_SERIAL_UART7_PRIORITY 12 +#define STM32_SERIAL_UART8_PRIORITY 12 + +/* + * SIO driver system settings. + */ +#define STM32_SIO_USE_USART1 FALSE +#define STM32_SIO_USE_USART2 FALSE +#define STM32_SIO_USE_USART3 FALSE +#define STM32_SIO_USE_UART4 FALSE +#define STM32_SIO_USE_UART5 FALSE +#define STM32_SIO_USE_USART6 FALSE +#define STM32_SIO_USE_UART7 FALSE +#define STM32_SIO_USE_UART8 FALSE +#define STM32_SIO_USE_LPUART1 FALSE + +/* + * SPI driver system settings. + */ +#ifndef STM32_SPI_USE_SPI1 +#define STM32_SPI_USE_SPI1 FALSE +#endif +#ifndef STM32_SPI_USE_SPI2 +#define STM32_SPI_USE_SPI2 FALSE +#endif +#ifndef STM32_SPI_USE_SPI3 +#define STM32_SPI_USE_SPI3 FALSE +#endif +#ifndef STM32_SPI_USE_SPI4 +#define STM32_SPI_USE_SPI4 FALSE +#endif +#ifndef STM32_SPI_USE_SPI5 +#define STM32_SPI_USE_SPI5 FALSE +#endif +#ifndef STM32_SPI_USE_SPI6 +#define STM32_SPI_USE_SPI6 FALSE +#endif +#define STM32_SPI_SPI1_DMA_PRIORITY 1 +#define STM32_SPI_SPI2_DMA_PRIORITY 1 +#define STM32_SPI_SPI3_DMA_PRIORITY 1 +#define STM32_SPI_SPI4_DMA_PRIORITY 1 +#define STM32_SPI_SPI5_DMA_PRIORITY 1 +#define STM32_SPI_SPI6_DMA_PRIORITY 1 +#define STM32_SPI_SPI1_IRQ_PRIORITY 10 +#define STM32_SPI_SPI2_IRQ_PRIORITY 10 +#define STM32_SPI_SPI3_IRQ_PRIORITY 10 +#define STM32_SPI_SPI4_IRQ_PRIORITY 10 +#define STM32_SPI_SPI5_IRQ_PRIORITY 10 +#define STM32_SPI_SPI6_IRQ_PRIORITY 10 +#define STM32_SPI_DMA_ERROR_HOOK(spip) STM32_DMA_ERROR_HOOK(spip) + +/* + * ST driver system settings. + */ +#define STM32_ST_IRQ_PRIORITY 8 +#ifndef STM32_ST_USE_TIMER +#define STM32_ST_USE_TIMER 5 +#endif + +/* + * TRNG driver system settings. + */ +#define STM32_TRNG_USE_RNG1 FALSE + +/* + * UART driver system settings. + */ +#define STM32_UART_USART1_IRQ_PRIORITY 12 +#define STM32_UART_USART2_IRQ_PRIORITY 12 +#define STM32_UART_USART3_IRQ_PRIORITY 12 +#define STM32_UART_UART4_IRQ_PRIORITY 12 +#define STM32_UART_UART5_IRQ_PRIORITY 12 +#define STM32_UART_USART6_IRQ_PRIORITY 12 +#define STM32_UART_USART1_DMA_PRIORITY 0 +#define STM32_UART_USART2_DMA_PRIORITY 0 +#define STM32_UART_USART3_DMA_PRIORITY 0 +#define STM32_UART_UART4_DMA_PRIORITY 0 +#define STM32_UART_UART5_DMA_PRIORITY 0 +#define STM32_UART_USART6_DMA_PRIORITY 0 +#define STM32_UART_UART7_DMA_PRIORITY 0 +#define STM32_UART_UART8_DMA_PRIORITY 0 +#define STM32_UART_DMA_ERROR_HOOK(uartp) STM32_DMA_ERROR_HOOK(uartp) + +#define STM32_IRQ_LPUART1_PRIORITY 12 + +/* + * USB driver system settings. + */ +#define STM32_USB_USE_OTG2 TRUE +#define STM32_USB_USE_OTG1 FALSE +#define STM32_USB_OTG1_IRQ_PRIORITY 14 +#define STM32_USB_OTG2_IRQ_PRIORITY 14 +#define STM32_USB_OTG1_RX_FIFO_SIZE 512 +#define STM32_USB_OTG2_RX_FIFO_SIZE 1024 +#define STM32_USB_HOST_WAKEUP_DURATION 2 +#define STM32_OTG2_IS_OTG1 TRUE + +/* + * WDG driver system settings. + */ +#define STM32_WDG_USE_IWDG FALSE + +#define STM32_EXTI_ENHANCED + +// limit ISR count per byte +#define STM32_I2C_ISR_LIMIT 6 + +// limit SDMMC clock to 12.5MHz by default. This increases +// reliability +#ifndef STM32_SDC_MAX_CLOCK +#define STM32_SDC_MAX_CLOCK 12500000 +#endif + +#ifndef STM32_WSPI_USE_OCTOSPI1 +#define STM32_WSPI_USE_OCTOSPI1 FALSE +#endif + +#if HAL_XIP_ENABLED +#define STM32_OSPI1_NO_RESET TRUE +#define STM32_OSPI2_NO_RESET TRUE +#endif + +#if STM32_WSPI_USE_OCTOSPI1 +#define STM32_WSPI_OCTOSPI1_MDMA_CHANNEL STM32_MDMA_CHANNEL_ID_ANY +#define STM32_WSPI_OCTOSPI1_MDMA_PRIORITY 1 +#define STM32_WSPI_OCTOSPI1_PRESCALER_VALUE (STM32_OCTOSPICLK / HAL_OSPI1_CLK) +#endif + +/* + we use a fixed allocation of BDMA streams. We previously dynamically + allocated these, but bugs in the chip make that unreliable. This is + a tested set of allocations that is known to work on boards that are + using all 3 of ADC3, I2C4 and SPI6. They are the only peripherals + that can use BDMA, so fixed allocation is possible as we have 8 + streams and a maximum of 6 needed. + + The layout is chosen to: + + - avoid stream 0, as this doesn't work on ADC3 or SPI6_RX for no known reason + - leave a gap between the peripheral types, as we have previously found that we sometimes + lost SPI6 BDMA completion interrupts if SPI6 and I2c4 are neighbours + */ +#define STM32_I2C_I2C4_RX_BDMA_STREAM 1 +#define STM32_I2C_I2C4_TX_BDMA_STREAM 2 +#define STM32_SPI_SPI6_RX_BDMA_STREAM 4 +#define STM32_SPI_SPI6_TX_BDMA_STREAM 5 +#define STM32_ADC_ADC3_BDMA_STREAM 7 + +// disable DMA on I2C by default on H7 +#ifndef STM32_I2C_USE_DMA +#define STM32_I2C_USE_DMA FALSE +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index aeae4a3f7b56ac..8268c4853f412d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -53,6 +53,7 @@ #define STM32H755_MCUCONF #define STM32H747_MCUCONF #define STM32H757_MCUCONF +#define STM32H723_MCUCONF /* * General settings. @@ -101,7 +102,7 @@ #define STM32_HSIDIV STM32_HSIDIV_DIV1 /* - * Clock setup for all other H7 variants including H743, H753, H750 and H757 + * Clock setup for all other H7 variants including H743, H723, H753, H750 and H757 */ #define STM32_VOS STM32_VOS_SCALE1 /* @@ -306,7 +307,9 @@ #ifndef STM32_QSPISEL #define STM32_QSPISEL STM32_QSPISEL_PLL2_R_CK #endif +#ifdef STM32_QSPISEL_HCLK #define STM32_FMCSEL STM32_QSPISEL_HCLK +#endif #define STM32_SWPSEL STM32_SWPSEL_PCLK1 #define STM32_FDCANSEL STM32_FDCANSEL_PLL1_Q_CK @@ -357,6 +360,9 @@ #define STM32_IRQ_QUADSPI1_PRIORITY 10 #define STM32_IRQ_QUADSPI2_PRIORITY 10 +#define STM32_IRQ_OCTOSPI1_PRIORITY 10 +#define STM32_IRQ_OCTOSPI2_PRIORITY 10 + #define STM32_IRQ_SDMMC1_PRIORITY 9 #define STM32_IRQ_SDMMC2_PRIORITY 9 @@ -633,10 +639,9 @@ // limit ISR count per byte #define STM32_I2C_ISR_LIMIT 6 -// limit SDMMC clock to 12.5MHz by default. This increases -// reliability +// limit SDMMC clock to 50MHz by default #ifndef STM32_SDC_MAX_CLOCK -#define STM32_SDC_MAX_CLOCK 12500000 +#define STM32_SDC_MAX_CLOCK 50000000 #endif #ifndef STM32_WSPI_USE_QUADSPI1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_type2_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_type2_mcuconf.h index e4e220f2d0751a..8c02c8378466cf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_type2_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_type2_mcuconf.h @@ -23,6 +23,9 @@ #define STM32_ENFORCE_H7_REV_XY #endif +// MPU region for ethernet +#define STM32_NOCACHE_MPU_REGION_ETH MPU_REGION_2 + #ifndef STM32_LSECLK #define STM32_LSECLK 32768U #endif @@ -109,7 +112,7 @@ #if STM32_HSECLK == 0U // no crystal, this gives 400MHz system clock #define STM32_HSE_ENABLED FALSE -#define STM32_HSI_ENABLED TRUE +#define STM32_HSI_ENABLED TRUE // HSI is 64MHz #define STM32_PLL1_DIVM_VALUE 32 #define STM32_PLL2_DIVM_VALUE 32 #define STM32_PLL3_DIVM_VALUE 32 @@ -122,39 +125,26 @@ #define STM32_HSE_ENABLED TRUE #define STM32_HSI_ENABLED FALSE #define STM32_PLL1_DIVM_VALUE 4 -#define STM32_PLL2_DIVM_VALUE 8 -#define STM32_PLL3_DIVM_VALUE 8 +#define STM32_PLL2_DIVM_VALUE 4 +#define STM32_PLL3_DIVM_VALUE 4 +#define STM32_PLLSRC STM32_PLLSRC_HSE_CK +#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK #elif STM32_HSECLK == 16000000U // this gives 520MHz system clock #define STM32_HSE_ENABLED TRUE #define STM32_HSI_ENABLED FALSE #define STM32_PLL1_DIVM_VALUE 8 -#define STM32_PLL2_DIVM_VALUE 16 -#define STM32_PLL3_DIVM_VALUE 16 +#define STM32_PLL2_DIVM_VALUE 8 +#define STM32_PLL3_DIVM_VALUE 8 +#define STM32_PLLSRC STM32_PLLSRC_HSE_CK +#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK #else #error "Unsupported HSE clock" #endif -#if STM32_HSECLK == 0U -// no crystal -#define STM32_PLL1_DIVN_VALUE 260 -#define STM32_PLL1_DIVP_VALUE 1 -#define STM32_PLL1_DIVQ_VALUE 6 -#define STM32_PLL1_DIVR_VALUE 4 - -#define STM32_PLL2_DIVN_VALUE 200 -#define STM32_PLL2_DIVP_VALUE 3 -#define STM32_PLL2_DIVQ_VALUE 3 -#define STM32_PLL2_DIVR_VALUE 2 - -#define STM32_PLL3_DIVN_VALUE 96 -#define STM32_PLL3_DIVP_VALUE 1 -#define STM32_PLL3_DIVQ_VALUE 2 -#define STM32_PLL3_DIVR_VALUE 4 - -#elif (STM32_HSECLK == 8000000U) || (STM32_HSECLK == 16000000U) +#if (STM32_HSECLK == 0U) || (STM32_HSECLK == 8000000U) || (STM32_HSECLK == 16000000U) // common clock tree for multiples of 8MHz crystals #ifdef HAL_CUSTOM_MCU_CLOCKRATE #if HAL_CUSTOM_MCU_CLOCKRATE == 520000000 @@ -178,7 +168,7 @@ #define STM32_PLL2_DIVN_VALUE 400 #define STM32_PLL2_DIVP_VALUE 3 -#define STM32_PLL2_DIVQ_VALUE 3 +#define STM32_PLL2_DIVQ_VALUE 10 // 80MHz for FDCAN #define STM32_PLL2_DIVR_VALUE 2 #define STM32_PLL3_DIVN_VALUE 192 @@ -190,7 +180,6 @@ * PLLs static settings. * Reading STM32 Reference Manual is required. */ -#define STM32_PLLSRC STM32_PLLSRC_HSE_CK #define STM32_PLLCFGR_MASK ~0 #define STM32_PLL1_ENABLED TRUE @@ -245,13 +234,12 @@ #define STM32_STOPKERWUCK 0 #define STM32_STOPWUCK 0 #define STM32_RTCPRE_VALUE 2 -#define STM32_CKPERSEL STM32_CKPERSEL_HSE_CK #define STM32_SDMMCSEL STM32_SDMMCSEL_PLL1_Q_CK #define STM32_OCTOSPISEL STM32_OCTOSPISEL_HCLK #define STM32_FMCSEL STM32_OCTOSPISEL_HCLK #define STM32_SWPSEL STM32_SWPSEL_PCLK1 -#define STM32_FDCANSEL STM32_FDCANSEL_PLL1_Q_CK +#define STM32_FDCANSEL STM32_FDCANSEL_PLL2_Q_CK #define STM32_DFSDM1SEL STM32_DFSDM1SEL_PCLK2 #define STM32_SPDIFSEL STM32_SPDIFSEL_PLL1_Q_CK #define STM32_SPI45SEL STM32_SPI45SEL_PCLK2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3-bdshot/hwdef.dat index 5af9e12d7a18b2..781cc6e59ff853 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3-bdshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3-bdshot/hwdef.dat @@ -356,7 +356,7 @@ PE1 UART8_TX UART8 # timing affecting sensor stability. We pull it high by default. PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat index 002fbad6143b3c..bd90c7351d11cf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat @@ -50,7 +50,7 @@ PD6 USART2_RX USART2 PD3 USART2_CTS USART2 PD4 USART2_RTS USART2 -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters) +# UART7 maps to SERIAL5. #PE7 UART7_RX UART7 #PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat index 96f0b2f3117a01..67e3ebf22edd11 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef.dat @@ -356,7 +356,7 @@ PE6 SPI4_MOSI SPI4 # timing affecting sensor stability. We pull it high by default. PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc index c7fc0f6e61700d..5b3ea0e7c2e9ee 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_common.inc @@ -122,6 +122,7 @@ define AP_MAVLINK_MSG_RELAY_STATUS_ENABLED 0 #fewer airspeed sensors define AP_AIRSPEED_BACKEND_DEFAULT_ENABLED 0 define AP_AIRSPEED_MS4525_ENABLED 1 +define AP_AIRSPEED_ASP5033_ENABLED 1 define AP_AIRSPEED_ANALOG_ENABLED 1 define AP_AIRSPEED_MS5525_ENABLED 1 define AP_AIRSPEED_SDP3X_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/network_PPPGW.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/network_PPPGW.inc new file mode 100644 index 00000000000000..c49bfa387cc845 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/network_PPPGW.inc @@ -0,0 +1,24 @@ +undef AP_RC_CHANNEL_ENABLED +define AP_RC_CHANNEL_ENABLED 0 + +define HAL_PERIPH_ENABLE_RTC + +define HAL_PERIPH_ENABLE_SERIAL_OPTIONS +define AP_NETWORKING_BACKEND_PPP 1 + +define AP_PERIPH_NET_PPP_PORT_DEFAULT 1 +define AP_PERIPH_NET_PPP_BAUD_DEFAULT 12500000 + +// add scripting for web interface +define AP_SCRIPTING_ENABLED 1 + +// ROMFS filesystem only +define AP_FILESYSTEM_ROMFS_ENABLED 1 + +// allow scripts to add parameters +define AP_PARAM_DYNAMIC_ENABLED 1 + +ROMFS_DIRECTORY Tools/AP_Periph/Web + +# we don't want to use external flash with network bootloader +EXT_FLASH_SIZE_MB 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/network_bootloader.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/network_bootloader.inc new file mode 100644 index 00000000000000..0703f3397a7f61 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/network_bootloader.inc @@ -0,0 +1,24 @@ +define CH_CFG_USE_EVENTS 1 +undef CH_CFG_USE_SEMAPHORES +define CH_CFG_USE_SEMAPHORES 1 +undef CH_CFG_USE_MUTEXES +define CH_CFG_USE_MUTEXES 1 +undef CH_CFG_USE_HEAP +define CH_CFG_USE_MEMCORE 1 +define CH_CFG_USE_HEAP 1 +define CH_CFG_USE_REGISTRY 1 +define CH_CFG_USE_DYNAMIC 1 +define CH_CFG_USE_WAITEXIT 1 + +define AP_NETWORKING_ENABLED 1 + +ROMFS_WILDCARD Tools/AP_Bootloader/Web/*.html +env ROMFS_UNCOMPRESSED True + +# we don't want to use external flash with network bootloader +EXT_FLASH_SIZE_MB 0 + +# don't combine sdcard bl and network bl +undef AP_BOOTLOADER_FLASH_FROM_SD_ENABLED + +define CH_CFG_THREAD_EXTRA_FIELDS struct ch_thread *delete_next; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-dshot/hwdef.inc b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-dshot/hwdef.inc index 5f6404aa15dc71..b51e0b156d951a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-dshot/hwdef.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-dshot/hwdef.inc @@ -16,7 +16,7 @@ undef AP_HAL_SHARED_DMA_ENABLED define STM32_TIM_TIM2_UP_DMA_STREAM STM32_DMA_STREAM_ID(1, 2) define STM32_TIM_TIM2_UP_DMA_CHAN 1 -# TIM4_UP (PWM 3/4) cannot be used with sharing as channels used by high speed USART2 RX/TX +# TIM4_UP (PWM 3/4) cannot be used without sharing as channels used by high speed USART2 RX/TX define AP_HAL_SHARED_DMA_ENABLED 1 define STM32_TIM_TIM4_UP_DMA_STREAM STM32_DMA_STREAM_ID(1, 7) define STM32_TIM_TIM4_UP_DMA_CHAN 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat index 10f3b2a2f3f65c..10b71b35a9a0ad 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/iomcu-f103-dshot/hwdef.dat @@ -12,3 +12,39 @@ DMA_NOMAP 1 # only four timers on F103xB so use TIM1 for system timer STM32_ST_USE_TIMER 1 + +define HAL_WITH_ESC_TELEM 1 + +undef PA0 PA1 PB8 PB9 + +# the order is important here as it determines the order that timers are used to sending dshot +# TIM4 needs to go first so that TIM4_UP can be freed up to be used by input capture for TIM2 +PB8 TIM4_CH3 TIM4 PWM(3) GPIO(103) BIDIR UP_SHARED +PB9 TIM4_CH4 TIM4 PWM(4) GPIO(104) +PA0 TIM2_CH1 TIM2 PWM(1) GPIO(101) +PA1 TIM2_CH2 TIM2 PWM(2) GPIO(102) BIDIR # DMA channel 7, shared with TIM4_UP and USART2_TX + +# currently no support for having mixed outputs on the same timer +# PB1 TIM3_CH4 TIM3 PWM(8) GPIO(108) BIDIR # DMA channel 3, shared with TIM3_UP + +# TIM2_UP - (1,2) +# TIM4_UP - (1,7) +# TIM3_UP - (1,3) + +# TIM2_CH2 (PWM 1/2) +define STM32_TIM_TIM2_CH2_DMA_STREAM STM32_DMA_STREAM_ID(1, 7) +define STM32_TIM_TIM2_CH2_DMA_CHAN 1 + +# TIM4_CH3 (PWM 3/4) +define STM32_TIM_TIM4_CH3_DMA_STREAM STM32_DMA_STREAM_ID(1, 5) +define STM32_TIM_TIM4_CH3_DMA_CHAN 1 + +# TIM3_CH4 (PWM 7-8) +define STM32_TIM_TIM3_CH4_DMA_STREAM STM32_DMA_STREAM_ID(1, 3) +define STM32_TIM_TIM3_CH4_DMA_CHAN 1 + +undef SHARED_DMA_MASK +define SHARED_DMA_MASK (1U<>= 4 + + csum = ~csum + csum &= 0xF + packet = (packet << 4) | csum + + return packet + +def rll_encode(value): + old_bit = 0 + rll_value = 0 + + for i in range(1, 21): + if value & 1: + new_bit = (1 ^ old_bit) + else: + new_bit = old_bit + value >>= 1 + rll_value |= new_bit << i + old_bit = new_bit + return rll_value + +def gcr_encode(value): + expo = 0 + + if value: + while value & 1 == 0: + value >>= 1 + expo = expo + 1 + + value = (value & 0x1FF) | (expo << 9) + value = dshot_encode(value) + + nibble_map = [0x19, 0x1b, 0x12, 0x13, 0x1d, 0x15, 0x16, 0x17, 0x1a, 0x09, 0x0a, 0x0b, 0x1e, 0x0d, 0x0e, 0x0f ] + + new_value = 0 + for i in range(4): + new_value |= (nibble_map[value & 0xF] << ((3-i) * 5)) + value >> 4 + + return rll_encode(new_value) + +def print_signal(value): + old_bit = 0 + print("_ ", sep='', end='') + for i in range(21): + bit = (value>>(20-i)) & 1 + if bit != old_bit: + print(' ', sep='', end='') + if bit: + print('_', sep='', end='') + else: + print(' ', sep='', end='') + old_bit = bit + print(" _", sep='', end='') + print('') + + print(" |", sep='', end='') + for i in range(21): + bit = (value>>(20-i)) & 1 + if bit != old_bit: + print('|', sep='', end='') + print(' ', sep='', end='') + old_bit = bit + print("| ", sep='', end='') + print('') + + print(" |", sep='', end='') + for i in range(21): + bit = (value>>(20-i)) & 1 + if bit != old_bit: + print('|', sep='', end='') + if bit == 0: + print("_", sep='', end='') + else: + print(' ', sep='', end='') + old_bit = bit + print("| ", sep='', end='') + print('') + +if args.value.startswith("0b"): + value = int(args.value[2:], 2) +elif args.value.startswith("0x"): + value = int(args.value[2:], 16) +else: + value = int(args.value) + +encoded_value = gcr_encode(value) +print("0b{:020b}".format(encoded_value)) +print_signal(encoded_value) + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index 6b1275bf00a351..f100b0dae2c807 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -25,12 +25,17 @@ class ChibiOSHWDef(object): f1_vtypes = ['CRL', 'CRH', 'ODR'] af_labels = ['USART', 'UART', 'SPI', 'I2C', 'SDIO', 'SDMMC', 'OTG', 'JT', 'TIM', 'CAN', 'QUADSPI', 'OCTOSPI', 'ETH', 'MCO'] - def __init__(self, bootloader=False, signed_fw=False, outdir=None, hwdef=[], default_params_filepath=None): + def __init__(self, quiet=False, bootloader=False, signed_fw=False, outdir=None, hwdef=[], default_params_filepath=None): self.outdir = outdir self.hwdef = hwdef self.bootloader = bootloader self.signed_fw = signed_fw self.default_params_filepath = default_params_filepath + self.quiet = quiet + + # if true then parameters will be appended in special apj-tool + # section at end of binary: + self.force_apj_default_parameters = False self.default_gpio = ['INPUT', 'FLOATING'] @@ -111,9 +116,6 @@ def __init__(self, bootloader=False, signed_fw=False, outdir=None, hwdef=[], def self.dma_exclude_pattern = [] - # map from uart names to SERIALn numbers - self.uart_serial_num = {} - self.mcu_type = None self.dual_USB_enabled = False @@ -123,6 +125,9 @@ def __init__(self, bootloader=False, signed_fw=False, outdir=None, hwdef=[], def # integer defines self.intdefines = {} + # list of shared up timers + self.shared_up = [] + def is_int(self, str): '''check if a string is an integer''' try: @@ -684,11 +689,13 @@ def get_mcu_config(self, name, required=False): return lib.mcu[name] def get_ram_reserve_start(self): - '''get amount of memory to reserve for bootloader comms''' + '''get amount of memory to reserve for bootloader comms and the address if non-zero''' ram_reserve_start = self.get_config('RAM_RESERVE_START', default=0, type=int) if ram_reserve_start == 0 and self.is_periph_fw(): ram_reserve_start = 256 - return ram_reserve_start + ram_map_bootloader = self.get_ram_map(use_bootloader=True) + ram0_start_address = ram_map_bootloader[0][0] + return ram_reserve_start, ram0_start_address def make_line(self, label): '''return a line for a label''' @@ -735,7 +742,7 @@ def enable_can(self, f): f.write('#define CAN1_BASE CAN_BASE\n') self.env_vars['HAL_NUM_CAN_IFACES'] = str(len(base_list)) - if self.mcu_series.startswith("STM32H7") and not args.bootloader: + if self.mcu_series.startswith("STM32H7") and not self.is_bootloader_fw(): # set maximum supported canfd bit rate in MBits/sec canfd_supported = int(self.get_config('CANFD_SUPPORTED', 0, default=4, required=False)) f.write('#define HAL_CANFD_SUPPORTED %d\n' % canfd_supported) @@ -752,31 +759,24 @@ def has_sdcard_spi(self): return True return False - def get_ram_map(self): + def get_ram_map(self, use_bootloader=False): '''get RAM_MAP. May be different for bootloader''' - if 'APP_RAM_START' not in self.env_vars: - self.env_vars['APP_RAM_START'] = None - if args.bootloader: + if self.is_bootloader_fw() or use_bootloader: ram_map = self.get_mcu_config('RAM_MAP_BOOTLOADER', False) if ram_map is not None: - app_ram_map = self.get_mcu_config('RAM_MAP', False) - if app_ram_map is not None and app_ram_map[0][0] != ram_map[0][0]: - # we need to find the location of app_ram_map[0] in ram_map - for i in range(len(ram_map)): - if app_ram_map[0][0] == ram_map[i][0] and self.env_vars['APP_RAM_START'] is None: - self.env_vars['APP_RAM_START'] = i return ram_map elif self.env_vars['EXT_FLASH_SIZE_MB'] and not self.env_vars['INT_FLASH_PRIMARY']: ram_map = self.get_mcu_config('RAM_MAP_EXTERNAL_FLASH', False) if ram_map is not None: return ram_map elif int(self.env_vars.get('USE_ALT_RAM_MAP', 0)) == 1: - print("Using ALT_RAM_MAP") + self.progress("Using ALT_RAM_MAP") return self.get_mcu_config('ALT_RAM_MAP', True) return self.get_mcu_config('RAM_MAP', True) def get_flash_pages_sizes(self): mcu_series = self.mcu_series + mcu_type = self.mcu_type if mcu_series.startswith('STM32F4') or mcu_series.startswith('CKS32F4'): if self.get_config('FLASH_SIZE_KB', type=int) == 512: return [16, 16, 16, 16, 64, 128, 128, 128] @@ -803,6 +803,8 @@ def get_flash_pages_sizes(self): 256, 256, 256, 256] else: raise Exception("Unsupported flash size %u" % self.get_config('FLASH_SIZE_KB', type=int)) + elif mcu_type.startswith('STM32H7A'): + return [8] * (self.get_config('FLASH_SIZE_KB', type=int)//8) elif mcu_series.startswith('STM32H7'): return [128] * (self.get_config('FLASH_SIZE_KB', type=int)//128) elif mcu_series.startswith('STM32F100') or mcu_series.startswith('STM32F103'): @@ -854,7 +856,7 @@ def get_storage_flash_page(self): storage_flash_page = self.get_config('STORAGE_FLASH_PAGE', default=None, type=int, required=False) if storage_flash_page is not None: return storage_flash_page - if args.bootloader and args.hwdef[0].find("-bl") != -1: + if self.is_bootloader_fw() and args.hwdef[0].find("-bl") != -1: hwdefdat = args.hwdef[0].replace("-bl", "") if os.path.exists(hwdefdat): ret = None @@ -874,6 +876,8 @@ def validate_flash_storage_size(self): storage_flash_page = self.get_storage_flash_page() pages = self.get_flash_pages_sizes() page_size = pages[storage_flash_page] * 1024 + if self.intdefines.get('AP_FLASH_STORAGE_DOUBLE_PAGE', 0) == 1: + page_size *= 2 storage_size = self.intdefines.get('HAL_STORAGE_SIZE', None) if storage_size is None: self.error('Need HAL_STORAGE_SIZE define') @@ -903,6 +907,14 @@ def get_numeric_board_id(self): raise ValueError("Unable to map (%s) to a board ID using %s" % (some_id, board_types_filepath)) + def enable_networking(self, f): + f.write(''' +#ifndef AP_NETWORKING_ENABLED +#define AP_NETWORKING_ENABLED 1 +#endif +#define CH_CFG_USE_MAILBOXES 1 +''') + def write_mcu_config(self, f): '''write MCU config defines''' f.write('#define CHIBIOS_BOARD_NAME "%s"\n' % os.path.basename(os.path.dirname(args.hwdef[0]))) @@ -965,17 +977,12 @@ def write_mcu_config(self, f): f.write('#define STM32_USB_USE_OTG2 TRUE\n') if 'ETH1' in self.bytype: - self.build_flags.append('USE_LWIP=yes') + self.enable_networking(f) f.write(''' -#ifndef AP_NETWORKING_ENABLED -// Configure for Ethernet support -#define AP_NETWORKING_ENABLED 1 -#define CH_CFG_USE_MAILBOXES TRUE #define HAL_USE_MAC TRUE #define MAC_USE_EVENTS TRUE #define STM32_ETH_BUFFERS_EXTERN -#endif ''') @@ -1025,10 +1032,9 @@ def write_mcu_config(self, f): if 'HAL_USE_CAN' in d: using_chibios_can = True f.write('#define %s\n' % d[7:]) - # extract numerical defines for processing by other parts of the script - result = re.match(r'define\s*([A-Z_]+)\s*([0-9]+)', d) - if result: - self.intdefines[result.group(1)] = int(result.group(2)) + + if self.intdefines.get('AP_NETWORKING_ENABLED', 0) == 1: + self.enable_networking(f) if self.intdefines.get('HAL_USE_USB_MSD', 0) == 1: self.build_flags.append('USE_USB_MSD=yes') @@ -1044,7 +1050,7 @@ def write_mcu_config(self, f): f.write('\n// location of loaded firmware\n') f.write('#define FLASH_LOAD_ADDRESS 0x%08x\n' % (0x08000000 + flash_reserve_start*1024)) # can be no persistent parameters if no space allocated for them - if not args.bootloader and flash_reserve_start == 0: + if not self.is_bootloader_fw() and flash_reserve_start == 0: f.write('#define HAL_ENABLE_SAVE_PERSISTENT_PARAMS 0\n') f.write('#define EXT_FLASH_SIZE_MB %u\n' % self.get_config('EXT_FLASH_SIZE_MB', default=0, type=int)) @@ -1053,7 +1059,7 @@ def write_mcu_config(self, f): self.env_vars['EXT_FLASH_SIZE_MB'] = self.get_config('EXT_FLASH_SIZE_MB', default=0, type=int) self.env_vars['INT_FLASH_PRIMARY'] = self.get_config('INT_FLASH_PRIMARY', default=False, type=bool) - if self.env_vars['EXT_FLASH_SIZE_MB'] and not args.bootloader and not self.env_vars['INT_FLASH_PRIMARY']: + if self.env_vars['EXT_FLASH_SIZE_MB'] and not self.is_bootloader_fw() and not self.env_vars['INT_FLASH_PRIMARY']: f.write('#define CRT0_AREAS_NUMBER 4\n') f.write('#define __FASTRAMFUNC__ __attribute__ ((__section__(".fastramfunc")))\n') f.write('#define __RAMFUNC__ __attribute__ ((__section__(".ramfunc")))\n') @@ -1071,7 +1077,7 @@ def write_mcu_config(self, f): storage_flash_page = self.get_storage_flash_page() flash_reserve_end = self.get_config('FLASH_RESERVE_END_KB', default=0, type=int) if storage_flash_page is not None: - if not args.bootloader: + if not self.is_bootloader_fw(): f.write('#define STORAGE_FLASH_PAGE %u\n' % storage_flash_page) self.validate_flash_storage_size() elif self.get_config('FLASH_RESERVE_END_KB', type=int, required=False) is None: @@ -1082,14 +1088,14 @@ def write_mcu_config(self, f): if offset > bl_offset: flash_reserve_end = flash_size - offset - crashdump_enabled = bool(self.intdefines.get('AP_CRASHDUMP_ENABLED', (flash_size >= 2048 and not args.bootloader))) + crashdump_enabled = bool(self.intdefines.get('AP_CRASHDUMP_ENABLED', (flash_size >= 2048 and not self.is_bootloader_fw()))) # noqa # lets pick a flash sector for Crash log f.write('#ifndef AP_CRASHDUMP_ENABLED\n') f.write('#define AP_CRASHDUMP_ENABLED %u\n' % crashdump_enabled) f.write('#endif\n') self.env_vars['ENABLE_CRASHDUMP'] = crashdump_enabled - if args.bootloader: + if self.is_bootloader_fw(): if self.env_vars['EXT_FLASH_SIZE_MB'] and not self.env_vars['INT_FLASH_PRIMARY']: f.write('\n// location of loaded firmware in external flash\n') f.write('#define APP_START_ADDRESS 0x%08x\n' % (0x90000000 + self.get_config( @@ -1100,7 +1106,13 @@ def write_mcu_config(self, f): f.write('#define APP_START_OFFSET_KB %u\n' % self.get_config('APP_START_OFFSET_KB', default=0, type=int)) f.write('\n') + ram_reserve_start,ram0_start_address = self.get_ram_reserve_start() + f.write('#define HAL_RAM0_START 0x%08x\n' % ram0_start_address) + if ram_reserve_start > 0: + f.write('#define HAL_RAM_RESERVE_START 0x%08x\n' % ram_reserve_start) + ram_map = self.get_ram_map() + f.write('// memory regions\n') regions = [] cc_regions = [] @@ -1108,8 +1120,7 @@ def write_mcu_config(self, f): for (address, size, flags) in ram_map: size *= 1024 cc_regions.append('{0x%08x, 0x%08x, CRASH_CATCHER_BYTE }' % (address, address + size)) - if self.env_vars['APP_RAM_START'] is not None and address == ram_map[self.env_vars['APP_RAM_START']][0]: - ram_reserve_start = self.get_ram_reserve_start() + if address == ram0_start_address: address += ram_reserve_start size -= ram_reserve_start regions.append('{(void*)0x%08x, 0x%08x, 0x%02x }' % (address, size, flags)) @@ -1118,14 +1129,6 @@ def write_mcu_config(self, f): f.write('#define HAL_CC_MEMORY_REGIONS %s\n' % ', '.join(cc_regions)) f.write('#define HAL_MEMORY_TOTAL_KB %u\n' % (total_memory/1024)) - if self.env_vars['APP_RAM_START'] is not None: - f.write('#define HAL_RAM0_START 0x%08x\n' % ram_map[self.env_vars['APP_RAM_START']][0]) - else: - f.write('#define HAL_RAM0_START 0x%08x\n' % ram_map[0][0]) - ram_reserve_start = self.get_ram_reserve_start() - if ram_reserve_start > 0: - f.write('#define HAL_RAM_RESERVE_START 0x%08x\n' % ram_reserve_start) - f.write('\n// CPU serial number (12 bytes)\n') udid_start = self.get_mcu_config('UDID_START') if udid_start is None: @@ -1155,7 +1158,7 @@ def write_mcu_config(self, f): cortex = self.get_mcu_config('CORTEX') self.env_vars['CPU_FLAGS'] = self.get_mcu_config('CPU_FLAGS').split() build_info['MCU'] = cortex - print("MCU Flags: %s %s" % (cortex, self.env_vars['CPU_FLAGS'])) + self.progress("MCU Flags: %s %s" % (cortex, self.env_vars['CPU_FLAGS'])) elif self.mcu_series.startswith("STM32F1"): cortex = "cortex-m3" self.env_vars['CPU_FLAGS'] = ["-mcpu=%s" % cortex] @@ -1188,13 +1191,13 @@ def write_mcu_config(self, f): self.env_vars['CORTEX'] = cortex - if not args.bootloader: + if not self.is_bootloader_fw(): if cortex == 'cortex-m4': self.env_vars['CPU_FLAGS'].append('-DARM_MATH_CM4') elif cortex == 'cortex-m7': self.env_vars['CPU_FLAGS'].append('-DARM_MATH_CM7') - if not self.mcu_series.startswith("STM32F1") and not args.bootloader: + if not self.mcu_series.startswith("STM32F1") and not self.is_bootloader_fw(): self.env_vars['CPU_FLAGS'].append('-u_printf_float') build_info['ENV_UDEFS'] = "-DCHPRINTF_USE_FLOAT=1" @@ -1203,7 +1206,7 @@ def write_mcu_config(self, f): self.build_flags.append('%s=%s' % (v, build_info[v])) # setup for bootloader build - if args.bootloader: + if self.is_bootloader_fw(): if self.get_config('FULL_CHIBIOS_BOOTLOADER', required=False, default=False): # we got enough space to fit everything so we enable almost everything f.write(''' @@ -1254,7 +1257,9 @@ def write_mcu_config(self, f): #ifndef CH_CFG_USE_MUTEXES #define CH_CFG_USE_MUTEXES FALSE #endif +#ifndef CH_CFG_USE_EVENTS #define CH_CFG_USE_EVENTS FALSE +#endif #define CH_CFG_USE_EVENTS_TIMEOUT FALSE #define CH_CFG_OPTIMIZE_SPEED FALSE #define HAL_USE_EMPTY_STORAGE 1 @@ -1283,10 +1288,10 @@ def write_mcu_config(self, f): if self.env_vars.get('ROMFS_UNCOMPRESSED', False): f.write('#define HAL_ROMFS_UNCOMPRESSED\n') - if not args.bootloader: + if not self.is_bootloader_fw(): f.write('''#define STM32_DMA_REQUIRED TRUE\n\n''') - if args.bootloader: + if self.is_bootloader_fw(): # do not enable flash protection in bootloader, even if hwdef # requests it: f.write(''' @@ -1364,7 +1369,7 @@ def write_ldscript(self, fname): ext_flash_size = self.get_config('EXT_FLASH_SIZE_MB', default=0, type=int) int_flash_primary = self.get_config('INT_FLASH_PRIMARY', default=False, type=int) - if not args.bootloader: + if not self.is_bootloader_fw(): flash_length = flash_size - (flash_reserve_start + flash_reserve_end) ext_flash_length = ext_flash_size * 1024 - (ext_flash_reserve_start + ext_flash_reserve_end) else: @@ -1373,20 +1378,18 @@ def write_ldscript(self, fname): self.env_vars['FLASH_TOTAL'] = flash_length * 1024 - print("Generating ldscript.ld") + self.progress("Generating ldscript.ld") f = open(fname, 'w') ram0_start = ram_map[0][0] ram0_len = ram_map[0][1] * 1024 if ext_flash_size > 32: self.error("We only support 24bit addressing over external flash") - if self.env_vars['APP_RAM_START'] is None: - # default to start of ram for shared ram - # possibly reserve some memory for app/bootloader comms - ram_reserve_start = self.get_ram_reserve_start() + ram_reserve_start,ram0_start_address = self.get_ram_reserve_start() + if ram_reserve_start > 0 and ram0_start_address == ram0_start: ram0_start += ram_reserve_start ram0_len -= ram_reserve_start - if ext_flash_length == 0 or args.bootloader: + if ext_flash_length == 0 or self.is_bootloader_fw(): self.env_vars['HAS_EXTERNAL_FLASH_SECTIONS'] = 0 f.write('''/* generated ldscript.ld */ MEMORY @@ -1429,7 +1432,7 @@ def write_ldscript(self, fname): def copy_common_linkerscript(self, outdir): dirpath = os.path.dirname(os.path.realpath(__file__)) - if args.bootloader: + if self.is_bootloader_fw(): linker = 'common.ld' else: linker = self.get_mcu_config('LINKER_CONFIG') @@ -1467,10 +1470,10 @@ def write_USB_config(self, f): f.write('#define HAL_USB_STRING_MANUFACTURER %s\n' % self.get_config("USB_STRING_MANUFACTURER", default="\"ArduPilot\"")) default_product = "%BOARD%" - if args.bootloader: + if self.is_bootloader_fw(): default_product += "-BL" product_string = self.get_config("USB_STRING_PRODUCT", default="\"%s\"" % default_product) - if args.bootloader and args.signed_fw: + if self.is_bootloader_fw() and args.signed_fw: product_string = product_string.replace("-BL", "-Secure-BL-v10") f.write('#define HAL_USB_STRING_PRODUCT %s\n' % product_string) @@ -1547,7 +1550,7 @@ def write_WSPI_table(self, f): devlist = [] for dev in self.wspidev: if len(dev) != 6: - print("Badly formed WSPIDEV line %s" % dev) + self.progress("Badly formed WSPIDEV line %s" % dev) name = '"' + dev[0] + '"' bus = dev[1] mode = dev[2] @@ -1580,7 +1583,7 @@ def write_WSPI_config(self, f): # only the bootloader must run the hal lld (and QSPI clock) otherwise it is not possible to # bootstrap into external flash for t in list(self.bytype.keys()) + list(self.alttype.keys()): - if (t.startswith('QUADSPI') or t.startswith('OCTOSPI')) and not args.bootloader: + if (t.startswith('QUADSPI') or t.startswith('OCTOSPI')) and not self.is_bootloader_fw(): f.write('#define HAL_XIP_ENABLED TRUE\n') if len(self.wspidev) == 0: @@ -1588,7 +1591,7 @@ def write_WSPI_config(self, f): return for t in list(self.bytype.keys()) + list(self.alttype.keys()): - print(t) + self.progress(t) if t.startswith('QUADSPI') or t.startswith('OCTOSPI'): self.wspi_list.append(t) @@ -1814,75 +1817,51 @@ def get_extra_bylabel(self, label, name, default=None): return default return p.extra_value(name, type=str, default=default) - def get_UART_ORDER(self): - '''get UART_ORDER from SERIAL_ORDER option''' - if self.get_config('UART_ORDER', required=False, aslist=True) is not None: - self.error('Please convert UART_ORDER to SERIAL_ORDER') - serial_order = self.get_config('SERIAL_ORDER', required=False, aslist=True) - if serial_order is None: - return None - if args.bootloader: - # in bootloader SERIAL_ORDER is treated the same as UART_ORDER - return serial_order - map = [0, 3, 1, 2, 4, 5, 6, 7, 8, 9, 10, 11, 12] - while len(serial_order) < 4: - serial_order += ['EMPTY'] - uart_order = [] - for i in range(len(serial_order)): - uart_order.append(serial_order[map[i]]) - self.uart_serial_num[serial_order[i]] = i - return uart_order - def write_UART_config(self, f): '''write UART config defines''' - uart_list = self.get_UART_ORDER() - if uart_list is None: + serial_list = self.get_config('SERIAL_ORDER', required=False, aslist=True) + if serial_list is None: return + while len(serial_list) < 3: # enough ports for CrashCatcher UART discovery + serial_list += ['EMPTY'] f.write('\n// UART configuration\n') # write out which serial ports we actually have - idx = 0 nports = 0 - serial_order = self.get_config('SERIAL_ORDER', required=False, aslist=True) - for serial in serial_order: + for idx, serial in enumerate(serial_list): if serial == 'EMPTY': f.write('#define HAL_HAVE_SERIAL%u 0\n' % idx) else: f.write('#define HAL_HAVE_SERIAL%u 1\n' % idx) nports = nports + 1 - idx += 1 f.write('#define HAL_NUM_SERIAL_PORTS %u\n' % nports) # write out driver declarations for HAL_ChibOS_Class.cpp - devnames = "ABCDEFGHIJ" sdev = 0 - idx = 0 - for dev in uart_list: + for idx, dev in enumerate(serial_list): if dev == 'EMPTY': - f.write('#define HAL_UART%s_DRIVER Empty::UARTDriver uart%sDriver\n' % - (devnames[idx], devnames[idx])) + f.write('#define HAL_SERIAL%s_DRIVER Empty::UARTDriver serial%sDriver\n' % + (idx, idx)) sdev += 1 else: f.write( - '#define HAL_UART%s_DRIVER ChibiOS::UARTDriver uart%sDriver(%u)\n' - % (devnames[idx], devnames[idx], sdev)) + '#define HAL_SERIAL%s_DRIVER ChibiOS::UARTDriver serial%sDriver(%u)\n' + % (idx, idx, sdev)) sdev += 1 - idx += 1 - for idx in range(len(uart_list), len(devnames)): - f.write('#define HAL_UART%s_DRIVER Empty::UARTDriver uart%sDriver\n' % - (devnames[idx], devnames[idx])) + for idx in range(len(serial_list), 10): + f.write('#define HAL_SERIAL%s_DRIVER Empty::UARTDriver serial%sDriver\n' % + (idx, idx)) if 'IOMCU_UART' in self.config: if 'io_firmware.bin' not in self.romfs: self.error("Need io_firmware.bin in ROMFS for IOMCU") f.write('#define HAL_WITH_IO_MCU 1\n') - idx = len(uart_list) - f.write('#define HAL_UART_IOMCU_IDX %u\n' % idx) + f.write('#define HAL_UART_IOMCU_IDX %u\n' % len(serial_list)) f.write( '#define HAL_UART_IO_DRIVER ChibiOS::UARTDriver uart_io(HAL_UART_IOMCU_IDX)\n' ) - uart_list.append(self.config['IOMCU_UART'][0]) + serial_list.append(self.config['IOMCU_UART'][0]) f.write('#define HAL_HAVE_SERVO_VOLTAGE 1\n') # make the assumption that IO gurantees servo monitoring # all IOMCU capable boards have SBUS out f.write('#define AP_FEATURE_SBUS_OUT 1\n') @@ -1897,17 +1876,17 @@ def write_UART_config(self, f): crash_uart = None # write config for CrashCatcher UART - if not uart_list[0].startswith('OTG') and not uart_list[0].startswith('EMPTY'): - crash_uart = uart_list[0] - elif not uart_list[2].startswith('OTG') and not uart_list[2].startswith('EMPTY'): - crash_uart = uart_list[2] + if not serial_list[0].startswith('OTG') and not serial_list[0].startswith('EMPTY'): + crash_uart = serial_list[0] + elif not serial_list[2].startswith('OTG') and not serial_list[2].startswith('EMPTY'): + crash_uart = serial_list[2] if crash_uart is not None and self.get_config('FLASH_SIZE_KB', type=int) >= 2048: f.write('#define HAL_CRASH_SERIAL_PORT %s\n' % crash_uart) f.write('#define IRQ_DISABLE_HAL_CRASH_SERIAL_PORT() nvicDisableVector(STM32_%s_NUMBER)\n' % crash_uart) f.write('#define RCC_RESET_HAL_CRASH_SERIAL_PORT() rccReset%s(); rccEnable%s(true)\n' % (crash_uart, crash_uart)) f.write('#define HAL_CRASH_SERIAL_PORT_CLOCK STM32_%sCLK\n' % crash_uart) - for dev in uart_list: + for num, dev in enumerate(serial_list): if dev.startswith('UART'): n = int(dev[4:]) elif dev.startswith('USART'): @@ -1918,7 +1897,7 @@ def write_UART_config(self, f): devlist.append('{}') continue else: - self.error("Invalid element %s in UART_ORDER" % dev) + self.error("Invalid element %s in SERIAL_ORDER" % dev) devlist.append('HAL_%s_CONFIG' % dev) tx_line = self.make_line(dev + '_TX') rx_line = self.make_line(dev + '_RX') @@ -1926,12 +1905,12 @@ def write_UART_config(self, f): cts_line = self.make_line(dev + '_CTS') if rts_line != "0": have_rts_cts = True - f.write('#define HAL_HAVE_RTSCTS_SERIAL%u\n' % self.uart_serial_num[dev]) + f.write('#define HAL_HAVE_RTSCTS_SERIAL%u\n' % num) if dev.startswith('OTG2'): f.write( '#define HAL_%s_CONFIG {(BaseSequentialStream*) &SDU2, 2, true, false, 0, 0, false, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2}\n' % dev) # noqa - OTG2_index = uart_list.index(dev) + OTG2_index = serial_list.index(dev) self.dual_USB_enabled = True elif dev.startswith('OTG'): f.write( @@ -1968,44 +1947,44 @@ def write_UART_config(self, f): #endif ''' % (OTG2_index, OTG2_index)) - f.write('#define HAL_UART_DEVICE_LIST %s\n\n' % ','.join(devlist)) - if not need_uart_driver and not args.bootloader: + f.write('#define HAL_SERIAL_DEVICE_LIST %s\n\n' % ','.join(devlist)) + if not need_uart_driver and not self.is_bootloader_fw(): f.write(''' #ifndef HAL_USE_SERIAL #define HAL_USE_SERIAL HAL_USE_SERIAL_USB #endif ''') - num_uarts = len(devlist) + num_ports = len(devlist) if 'IOMCU_UART' in self.config: - num_uarts -= 1 - if num_uarts > 10: - self.error("Exceeded max num UARTs of 10 (%u)" % num_uarts) - f.write('#define HAL_UART_NUM_SERIAL_PORTS %u\n' % num_uarts) + num_ports -= 1 + if num_ports > 10: + self.error("Exceeded max num SERIALs of 10 (%u)" % num_ports) + f.write('#define HAL_UART_NUM_SERIAL_PORTS %u\n' % num_ports) def write_UART_config_bootloader(self, f): '''write UART config defines''' - uart_list = self.get_UART_ORDER() - if uart_list is None: + serial_list = self.get_config('SERIAL_ORDER', required=False, aslist=True) + if serial_list is None: return f.write('\n// UART configuration\n') devlist = [] - have_uart = False + have_serial = False OTG2_index = None - for u in uart_list: - if u.startswith('OTG2'): + for s in serial_list: + if s.startswith('OTG2'): devlist.append('(BaseChannel *)&SDU2') - OTG2_index = uart_list.index(u) - elif u.startswith('OTG'): + OTG2_index = serial_list.index(s) + elif s.startswith('OTG'): devlist.append('(BaseChannel *)&SDU1') else: - unum = int(u[-1]) - devlist.append('(BaseChannel *)&SD%u' % unum) - have_uart = True + snum = int(s[-1]) + devlist.append('(BaseChannel *)&SD%u' % snum) + have_serial = True if len(devlist) > 0: f.write('#define BOOTLOADER_DEV_LIST %s\n' % ','.join(devlist)) if OTG2_index is not None: f.write('#define HAL_OTG2_UART_INDEX %d\n' % OTG2_index) - if not have_uart: + if not have_serial: f.write(''' #ifndef HAL_USE_SERIAL #define HAL_USE_SERIAL FALSE @@ -2015,7 +1994,7 @@ def write_UART_config_bootloader(self, f): def write_I2C_config(self, f): '''write I2C config defines''' if not self.have_type_prefix('I2C'): - print("No I2C peripherals") + self.progress("No I2C peripherals") f.write(''' #ifndef HAL_USE_I2C #define HAL_USE_I2C FALSE @@ -2067,6 +2046,7 @@ def write_PWM_config(self, f, ordered_timers): rc_in_int = None alarm = None bidir = None + up_shared = None pwm_out = [] # start with the ordered list from the dma resolver pwm_timers = ordered_timers @@ -2084,12 +2064,14 @@ def write_PWM_config(self, f, ordered_timers): pwm_out.append(p) if p.has_extra('BIDIR'): bidir = p + if p.has_extra('UP_SHARED'): + up_shared = p if p.type not in pwm_timers: pwm_timers.append(p.type) f.write('#define HAL_PWM_COUNT %u\n' % len(pwm_out)) if not pwm_out and not alarm: - print("No PWM output defined") + self.progress("No PWM output defined") f.write(''' #ifndef HAL_USE_PWM #define HAL_USE_PWM FALSE @@ -2170,6 +2152,10 @@ def write_PWM_config(self, f, ordered_timers): f.write('// PWM timer config\n') if bidir is not None: f.write('#define HAL_WITH_BIDIR_DSHOT\n') + if up_shared is not None: + f.write('#define HAL_TIM_UP_SHARED\n') + for t in self.shared_up: + f.write('#define HAL_%s_SHARED true\n' % t) for t in pwm_timers: n = int(t[3:]) f.write('#define STM32_PWM_USE_TIM%u TRUE\n' % n) @@ -2225,13 +2211,19 @@ def write_PWM_config(self, f, ordered_timers): # define HAL_IC%u_CH%u_DMA_CONFIG false, 0, 0 #endif ''' % (n, i, n, i, n, i, n, i, n, i, n, i) - hal_icu_cfg += '}, \\' + if up_shared is not None: + hal_icu_cfg += '}, HAL_TIM%u_UP_SHARED, \\' % n + else: + hal_icu_cfg += '}, \\' f.write('''#if defined(STM32_TIM_TIM%u_UP_DMA_STREAM) && defined(STM32_TIM_TIM%u_UP_DMA_CHAN) # define HAL_PWM%u_DMA_CONFIG true, STM32_TIM_TIM%u_UP_DMA_STREAM, STM32_TIM_TIM%u_UP_DMA_CHAN #else # define HAL_PWM%u_DMA_CONFIG false, 0, 0 #endif\n%s''' % (n, n, n, n, n, n, hal_icu_def)) + f.write('''#if !defined(HAL_TIM%u_UP_SHARED) +#define HAL_TIM%u_UP_SHARED false +#endif\n''' % (n, n)) f.write('''#define HAL_PWM_GROUP%u { %s, \\ {%u, %u, %u, %u}, \\ /* Group Initial Config */ \\ @@ -2396,12 +2388,13 @@ def write_GPIO_config(self, f): gpios = sorted(gpios) for (gpio, pwm, port, pin, p, enabled) in gpios: f.write('#define HAL_GPIO_LINE_GPIO%u PAL_LINE(GPIO%s,%uU)\n' % (gpio, port, pin)) - f.write('#define HAL_GPIO_PINS { \\\n') - for (gpio, pwm, port, pin, p, enabled) in gpios: - f.write('{ %3u, %s, %2u, PAL_LINE(GPIO%s,%uU)}, /* %s */ \\\n' % - (gpio, enabled, pwm, port, pin, p)) - # and write #defines for use by config code - f.write('}\n\n') + if len(gpios) > 0: + f.write('#define HAL_GPIO_PINS { \\\n') + for (gpio, pwm, port, pin, p, enabled) in gpios: + f.write('{ %3u, %s, %2u, PAL_LINE(GPIO%s,%uU)}, /* %s */ \\\n' % + (gpio, enabled, pwm, port, pin, p)) + # and write #defines for use by config code + f.write('}\n\n') f.write('// full pin define list\n') last_label = None for label in sorted(list(set(self.bylabel.keys()))): @@ -2441,8 +2434,10 @@ def embed_bootloader(self, f): bp = self.bootloader_path() if not os.path.exists(bp): - self.error("Bootloader (%s) does not exist and AP_BOOTLOADER_FLASHING_ENABLED" % - (bp,)) + self.error('''Bootloader (%s) does not exist and AP_BOOTLOADER_FLASHING_ENABLED +Please run: Tools/scripts/build_bootloaders.py %s +''' % + (bp, os.path.basename(os.path.dirname(args.hwdef[0])))) bp = os.path.realpath(bp) @@ -2526,9 +2521,12 @@ def write_all_lines(self, hwdat): if not self.is_periph_fw(): self.romfs["hwdef.dat"] = hwdat + def write_define(self, f, name, value): + f.write(f"#define {name} {value}\n") + def write_hwdef_header(self, outfilename): '''write hwdef header file''' - print("Writing hwdef setup in %s" % outfilename) + self.progress("Writing hwdef setup in %s" % outfilename) tmpfile = outfilename + ".tmp" f = open(tmpfile, 'w') @@ -2588,6 +2586,11 @@ def write_hwdef_header(self, outfilename): self.write_peripheral_enable(f) + if os.path.exists(self.processed_defaults_filepath()): + self.write_define(f, 'AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED', 1) + else: + self.write_define(f, 'AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED', 0) + if self.mcu_series.startswith("STM32H7"): # add in ADC3 on H7 to get MCU temperature and reference voltage self.periph_list.append('ADC3') @@ -2601,10 +2604,11 @@ def write_hwdef_header(self, outfilename): self.mcu_type, dma_exclude=self.get_dma_exclude(self.periph_list), dma_priority=self.get_config('DMA_PRIORITY', default='TIM* SPI*', spaces=True), - dma_noshare=self.dma_noshare + dma_noshare=self.dma_noshare, + quiet=self.quiet, ) - if not args.bootloader: + if not self.is_bootloader_fw(): self.write_PWM_config(f, ordered_timers) self.write_I2C_config(f) self.write_UART_config(f) @@ -2721,7 +2725,7 @@ def write_hwdef_header(self, outfilename): # see if we ended up with the same file, on an unnecessary reconfigure try: if filecmp.cmp(outfilename, tmpfile): - print("No change in hwdef.h") + self.progress("No change in hwdef.h") os.unlink(tmpfile) return except Exception: @@ -2735,6 +2739,7 @@ def write_hwdef_header(self, outfilename): def build_peripheral_list(self): '''build a list of peripherals for DMA resolver to work on''' peripherals = [] + self.shared_up = [] done = set() prefixes = ['SPI', 'USART', 'UART', 'I2C'] periph_pins = self.allpins[:] @@ -2785,6 +2790,8 @@ def build_peripheral_list(self): (_, _, compl) = self.parse_timer(ch_label) if ch_label not in peripherals and p.has_extra('BIDIR') and not compl: peripherals.append(ch_label) + if label not in self.shared_up and p.has_extra('UP_SHARED') and not compl: + self.shared_up.append(label) done.add(type) return peripherals @@ -2822,21 +2829,21 @@ def write_processed_defaults_file(self, filepath): defaults_abspath = None if os.path.exists(defaults_path): defaults_abspath = os.path.abspath(self.default_params_filepath) - print("Default parameters path from command line: %s" % self.default_params_filepath) + self.progress("Default parameters path from command line: %s" % self.default_params_filepath) elif os.path.exists(defaults_filename): defaults_abspath = os.path.abspath(defaults_filename) - print("Default parameters path from hwdef: %s" % defaults_filename) + self.progress("Default parameters path from hwdef: %s" % defaults_filename) if defaults_abspath is None: - print("No default parameter file found") - return + self.progress("No default parameter file found") + return False content = self.get_processed_defaults_file(defaults_abspath) with open(filepath, "w") as processed_defaults_fh: processed_defaults_fh.write(content) - self.env_vars['DEFAULT_PARAMETERS'] = filepath + return True def write_env_py(self, filename): '''write out env.py for environment variables to control the build process''' @@ -2856,11 +2863,14 @@ def romfs_wildcard(self, pattern): if fnmatch.fnmatch(f, pattern): self.romfs[f] = os.path.join(pattern_dir, f) - def romfs_add_dir(self, subdirs): + def romfs_add_dir(self, subdirs, relative_to_base=False): '''add a filesystem directory to ROMFS''' for dirname in subdirs: - romfs_dir = os.path.join(os.path.dirname(args.hwdef[0]), dirname) - if not args.bootloader and os.path.exists(romfs_dir): + if relative_to_base: + romfs_dir = os.path.join(os.path.dirname(__file__), '..', '..', '..', '..', dirname) + else: + romfs_dir = os.path.join(os.path.dirname(args.hwdef[0]), dirname) + if not self.is_bootloader_fw() and os.path.exists(romfs_dir): for root, d, files in os.walk(romfs_dir): for f in files: if fnmatch.fnmatch(f, '*~'): @@ -2868,6 +2878,8 @@ def romfs_add_dir(self, subdirs): continue fullpath = os.path.join(root, f) relpath = os.path.normpath(os.path.join(dirname, os.path.relpath(root, romfs_dir), f)) + if relative_to_base: + relpath = relpath[len(dirname)+1:] self.romfs[relpath] = fullpath def valid_type(self, ptype, label): @@ -2998,9 +3010,11 @@ def process_line(self, line): self.romfs_add(a[1], a[2]) elif a[0] == 'ROMFS_WILDCARD': self.romfs_wildcard(a[1]) + elif a[0] == 'ROMFS_DIRECTORY': + self.romfs_add_dir([a[1]], relative_to_base=True) elif a[0] == 'undef': for u in a[1:]: - print("Removing %s" % u) + self.progress("Removing %s" % u) self.config.pop(u, '') self.bytype.pop(u, '') self.bylabel.pop(u, '') @@ -3032,16 +3046,24 @@ def process_line(self, line): if u == 'AIRSPEED': self.airspeed_list = [] elif a[0] == 'env': - print("Adding environment %s" % ' '.join(a[1:])) + self.progress("Adding environment %s" % ' '.join(a[1:])) if len(a[1:]) < 2: self.error("Bad env line for %s" % a[0]) name = a[1] value = ' '.join(a[2:]) if name == 'AP_PERIPH' and value != "1": raise ValueError("AP_PERIPH may only have value 1") - if name == 'APP_RAM_START': - value = int(value, 0) self.env_vars[name] = value + elif a[0] == 'define': + # extract numerical defines for processing by other parts of the script + result = re.match(r'define\s*([A-Z_]+)\s*([0-9]+)', line) + if result: + self.intdefines[result.group(1)] = int(result.group(2)) + + def progress(self, message): + if self.quiet: + return + print(message) def process_file(self, filename): '''process a hwdef.dat file''' @@ -3061,7 +3083,7 @@ def process_file(self, filename): dir = os.path.dirname(filename) include_file = os.path.normpath( os.path.join(dir, include_file)) - print("Including %s" % include_file) + self.progress("Including %s" % include_file) self.process_file(include_file) else: self.process_line(line) @@ -3075,7 +3097,7 @@ def add_apperiph_defaults(self, f): self.add_firmware_defaults_from_file(f, "defaults_periph.h", "AP_Periph") def is_bootloader_fw(self): - return args.bootloader + return self.bootloader def add_bootloader_defaults(self, f): '''add default defines for peripherals''' @@ -3085,7 +3107,7 @@ def add_bootloader_defaults(self, f): self.add_firmware_defaults_from_file(f, "defaults_bootloader.h", "bootloader") def add_firmware_defaults_from_file(self, f, filename, description): - print("Setting up as %s" % description) + self.progress("Setting up as %s" % description) dirpath = os.path.dirname(os.path.realpath(__file__)) filepath = os.path.join(dirpath, filename) @@ -3132,6 +3154,31 @@ def add_normal_firmware_defaults(self, f): self.add_firmware_defaults_from_file(f, "defaults_normal.h", "normal") + def processed_defaults_filepath(self): + return os.path.join(self.outdir, "processed_defaults.parm") + + def write_default_parameters(self): + '''handle default parameters''' + + if self.is_bootloader_fw(): + return + + if self.is_io_fw(): + return + + filepath = self.processed_defaults_filepath() + if not self.write_processed_defaults_file(filepath): + return + + if self.get_config('FORCE_APJ_DEFAULT_PARAMETERS', default=False): + # set env variable so that post-processing in waf uses + # apj-tool to append parameters to image: + if os.path.exists(filepath): + self.env_vars['DEFAULT_PARAMETERS'] = filepath + return + + self.romfs_add('defaults.parm', filepath) + def run(self): # process input file @@ -3142,11 +3189,14 @@ def run(self): self.error("Missing MCU type in config") self.mcu_type = self.get_config('MCU', 1) - print("Setup for MCU %s" % self.mcu_type) + self.progress("Setup for MCU %s" % self.mcu_type) # build a list for peripherals for DMA resolver self.periph_list = self.build_peripheral_list() + # write out a default parameters file, decide how to use it: + self.write_default_parameters() + # write out hw.dat for ROMFS self.write_all_lines(os.path.join(self.outdir, "hw.dat")) @@ -3165,9 +3215,6 @@ def run(self): # exist in the same directory as the ldscript.ld file we generate. self.copy_common_linkerscript(self.outdir) - if not args.bootloader: - self.write_processed_defaults_file(os.path.join(self.outdir, "processed_defaults.parm")) - self.write_env_py(os.path.join(self.outdir, "env.py")) @@ -3184,6 +3231,8 @@ def run(self): 'hwdef', type=str, nargs='+', default=None, help='hardware definition file') parser.add_argument( '--params', type=str, default=None, help='user default params path') + parser.add_argument( + '--quiet', action='store_true', default=False, help='quiet running') args = parser.parse_args() @@ -3192,6 +3241,7 @@ def run(self): bootloader=args.bootloader, signed_fw=args.signed_fw, hwdef=args.hwdef, - default_params_filepath=args.params + default_params_filepath=args.params, + quiet=args.quiet, ) c.run() diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/convert_uart_order.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/convert_uart_order.py deleted file mode 100755 index d502d91f6113be..00000000000000 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/convert_uart_order.py +++ /dev/null @@ -1,37 +0,0 @@ -#!/usr/bin/env python -''' -convert UART_ORDER in a hwdef.dat into a SERIAL_ORDER -''' - -import sys, shlex - -def convert_file(fname): - lines = open(fname, 'r').readlines() - for i in range(len(lines)): - if lines[i].startswith('SERIAL_ORDER'): - print("Already has SERIAL_ORDER: %s" % fname) - return - - for i in range(len(lines)): - line = lines[i] - if not line.startswith('UART_ORDER'): - continue - a = shlex.split(line, posix=False) - if a[0] != 'UART_ORDER': - continue - uart_order = a[1:] - if not fname.endswith('-bl.dat'): - while len(uart_order) < 4: - uart_order += ['EMPTY'] - a += ['EMPTY'] - map = [ 0, 2, 3, 1, 4, 5, 6, 7, 8, 9, 10, 11, 12 ] - for j in range(len(uart_order)): - a[j+1] = uart_order[map[j]] - a[0] = 'SERIAL_ORDER' - print("%s new order " % fname, a) - lines[i] = ' '.join(a) + '\n' - open(fname, 'w').write(''.join(lines)) - -files=sys.argv[1:] -for fname in files: - convert_file(fname) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h index c002446c74b436..ce6514e51f2df0 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_bootloader.h @@ -35,3 +35,30 @@ #ifndef AP_WATCHDOG_SAVE_FAULT_ENABLED #define AP_WATCHDOG_SAVE_FAULT_ENABLED 0 #endif + +// less LWIP functionality in the bootloader +#define LWIP_DHCP 0 +#define LWIP_UDP 0 +#define LWIP_PPP 0 +#define LWIP_IGMP 0 +#define LWIP_ALTCP 0 +#define IP_FORWARD 0 +#define LWIP_SINGLE_NETIF 1 +#define SO_REUSE 0 +#define LWIP_SOCKET_POLL 0 +#define LINK_STATS 0 +#define ICMP_STATS 0 +#define IPFRAG_STATS 0 +#define TCP_STATS 0 +#define ARP_PROXYARP_SUPPORT 0 +#define LWIP_HAVE_LOOPIF 0 +#define LWIP_NETIF_LOOPBACK 0 + +/* + we need DMA on H7 to allow for ECC error checking + Note that ChibiOS uses #ifdef for STM32_DMA_REQUIRED not #if + */ +#if !defined(STM32_DMA_REQUIRED) && defined(STM32H7) +#define STM32_DMA_REQUIRED 1 +#endif + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h index 7cd350f397347f..8c441c3397cedc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h @@ -35,9 +35,25 @@ // no crossfire telemetry from iomcu! #define HAL_CRSF_TELEM_ENABLED 0 +// no ghost telemtry from IOMCU: +#define AP_GHST_TELEM_ENABLED 0 + // allow the IOMCU to have its allowed protocols to be set: #define AP_RCPROTOCOL_ENABLE_SET_RC_PROTOCOLS 1 #ifndef AP_INTERNALERROR_ENABLED #define AP_INTERNALERROR_ENABLED 0 #endif + +#ifndef HAL_TIM2_UP_SHARED +#define HAL_TIM2_UP_SHARED 0 +#endif +#ifndef HAL_TIM3_UP_SHARED +#define HAL_TIM3_UP_SHARED 0 +#endif +#ifndef HAL_TIM4_UP_SHARED +#define HAL_TIM4_UP_SHARED 0 +#endif +#ifndef HAL_TIM_UP_SHARED +#define HAL_TIM_UP_SHARED (HAL_TIM2_UP_SHARED || HAL_TIM3_UP_SHARED || HAL_TIM4_UP_SHARED) +#endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h index 3a0a8c372330d5..c2f75d230642fc 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -130,6 +130,9 @@ #ifndef COMPASS_CAL_ENABLED #define COMPASS_CAL_ENABLED 0 #endif +#ifndef AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED +#define AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED 0 +#endif #ifndef COMPASS_MOT_ENABLED #define COMPASS_MOT_ENABLED 0 #endif @@ -147,6 +150,7 @@ #endif #define HAL_CRSF_TELEM_ENABLED 0 +#define AP_GHST_TELEM_ENABLED 0 #ifndef AP_SERVORELAYEVENTS_ENABLED #define AP_SERVORELAYEVENTS_ENABLED 0 @@ -335,7 +339,7 @@ #endif #ifndef AP_UART_MONITOR_ENABLED -#define AP_UART_MONITOR_ENABLED defined(HAL_PERIPH_ENABLE_GPS) && (GPS_MOVING_BASELINE || BOARD_FLASH_SIZE>=256) +#define AP_UART_MONITOR_ENABLED defined(HAL_PERIPH_ENABLE_SERIAL_OPTIONS) || (defined(HAL_PERIPH_ENABLE_GPS) && (GPS_MOVING_BASELINE || BOARD_FLASH_SIZE>=256)) #endif #ifndef HAL_BOARD_LOG_DIRECTORY diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py index 4f78382deb3c56..0988076775b198 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py @@ -67,9 +67,9 @@ def can_share(periph, noshare_list): return False -# list of peripherals on H7 that are on DMAMUX2 and BDMA +# list of peripherals that are on DMAMUX2 and BDMA have_DMAMUX = False -DMAMUX2_peripherals = [ 'I2C4', 'SPI6', 'ADC3' ] +DMAMUX2_peripherals = [] def dmamux_channel(key): '''return DMAMUX channel for H7''' @@ -293,11 +293,15 @@ def forbidden_list(p, peripheral_list): def write_dma_header(f, peripheral_list, mcu_type, dma_exclude=[], - dma_priority='', dma_noshare=[]): + dma_priority='', dma_noshare=[], quiet=False): '''write out a DMA resolver header file''' global dma_map, have_DMAMUX, has_bdshot timer_ch_periph = [] + if mcu_type.startswith('STM32H7'): + global DMAMUX2_peripherals + DMAMUX2_peripherals = [ 'I2C4', 'SPI6', 'ADC3' ] + has_bdshot = False # form a list of DMA priorities @@ -331,7 +335,8 @@ def write_dma_header(f, peripheral_list, mcu_type, dma_exclude=[], dma_map = generate_DMAMUX_map(peripheral_list, noshare_list, dma_exclude, stream_ofs) - print("Writing DMA map") + if not quiet: + print("Writing DMA map") unassigned = [] curr_dict = {} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat index acad0918627d0f..602edd8721cb7f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-f412-rev1/hwdef.dat @@ -110,6 +110,10 @@ IMU Invensense SPI:icm20789 ROTATION_NONE define HAL_GPIO_RADIO_IRQ 100 define HAL_RCINPUT_WITH_AP_RADIO 1 +define AP_RADIO_BACKEND_DEFAULT_ENABLED 0 +define AP_RADIO_CC2500_ENABLED 1 +define AP_RADIO_CYRF6936_ENABLED 1 + STORAGE_FLASH_PAGE 1 define HAL_STORAGE_SIZE 15360 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat index 67af5681fe33db..5a3282bd63e638 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-journey/hwdef.dat @@ -100,6 +100,11 @@ IMU Invensense I2C:1:0x68 ROTATION_NONE define HAL_GPIO_RADIO_IRQ 100 define HAL_RCINPUT_WITH_AP_RADIO 1 +define AP_RADIO_BACKEND_DEFAULT_ENABLED 0 +define AP_RADIO_CC2500_ENABLED 1 +define AP_RADIO_CYRF6936_ENABLED 1 +define AP_RADIO_BK2425_ENABLED 1 + STORAGE_FLASH_PAGE 1 define HAL_STORAGE_SIZE 15360 diff --git a/Tools/Frame_params/SkyViper-2450GPS/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/defaults.parm similarity index 100% rename from Tools/Frame_params/SkyViper-2450GPS/defaults.parm rename to libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/defaults.parm diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat index 831c079648e98b..88a6bf4711a66b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat @@ -32,6 +32,9 @@ SERIAL_ORDER OTG1 USART2 EMPTY UART4 # enable AP_Radio support define HAL_RCINPUT_WITH_AP_RADIO 1 +define AP_RADIO_BACKEND_DEFAULT_ENABLED 0 +define AP_RADIO_CC2500_ENABLED 1 +define AP_RADIO_CYRF6936_ENABLED 1 define HAL_GPIO_RADIO_RESET 1 // PB0 GPIO from FMU3 @@ -61,8 +64,6 @@ PA1 UART4_RX UART4 NODMA # use flash storage STORAGE_FLASH_PAGE 22 -env DEFAULT_PARAMETERS 'Tools/Frame_params/SkyViper-2450GPS/defaults.parm' - # the web UI uses an abin file for firmware uploads env BUILD_ABIN True diff --git a/libraries/AP_HAL_ChibiOS/hwdef/thepeach-k1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/thepeach-k1/hwdef.dat index 827c5ff9592282..d5c1c44015b91e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/thepeach-k1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/thepeach-k1/hwdef.dat @@ -209,7 +209,7 @@ PE1 UART8_TX UART8 # timing affecting sensor stability. We pull it high by default. PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/thepeach-r1/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/thepeach-r1/hwdef.dat index 4122e0fa579919..4b128cb02b6565 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/thepeach-r1/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/thepeach-r1/hwdef.dat @@ -209,7 +209,7 @@ PE1 UART8_TX UART8 # timing affecting sensor stability. We pull it high by default. PE3 VDD_3V3_SENSORS_EN OUTPUT HIGH -# UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters). +# UART7 maps to SERIAL5. PE7 UART7_RX UART7 PE8 UART7_TX UART7 diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index c8cc8dcf7eba8f..922440826b06dd 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -73,7 +73,11 @@ bool sdcard_init() if (sdcd.bouncebuffer == nullptr) { // allocate 4k bouncebuffer for microSD to match size in // AP_Logger +#if defined(STM32H7) bouncebuffer_init(&sdcd.bouncebuffer, 4096, true); +#else + bouncebuffer_init(&sdcd.bouncebuffer, 4096, false); +#endif } if (sdcard_running) { diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp index 2f6c8c7be8822f..68b86d4b27f64d 100644 --- a/libraries/AP_HAL_ChibiOS/shared_dma.cpp +++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp @@ -63,6 +63,11 @@ bool Shared_DMA::is_shared(uint8_t stream_id) return (stream_id < SHARED_DMA_MAX_STREAM_ID) && ((1U< +// we rely on systimestamp_t for 64 bit timestamps +static_assert(sizeof(uint64_t) == sizeof(systimestamp_t), "unexpected systimestamp_t size"); + +#define STR(x) #x +#define XSTR(x) STR(x) + #if CH_CFG_ST_RESOLUTION == 16 static_assert(sizeof(systime_t) == 2, "expected 16 bit systime_t"); #elif CH_CFG_ST_RESOLUTION == 32 @@ -39,9 +45,9 @@ static_assert(sizeof(systime_t) == sizeof(sysinterval_t), "expected systime_t sa #if defined(HAL_EXPECTED_SYSCLOCK) #ifdef STM32_SYS_CK -static_assert(HAL_EXPECTED_SYSCLOCK == STM32_SYS_CK, "unexpected STM32_SYS_CK value"); +static_assert(HAL_EXPECTED_SYSCLOCK == STM32_SYS_CK, "unexpected STM32_SYS_CK value got " XSTR(STM32_HCLK) " expected " XSTR(HAL_EXPECTED_SYSCLOCK)); #elif defined(STM32_HCLK) -static_assert(HAL_EXPECTED_SYSCLOCK == STM32_HCLK, "unexpected STM32_HCLK value"); +static_assert(HAL_EXPECTED_SYSCLOCK == STM32_HCLK, "unexpected STM32_HCLK value got " XSTR(STM32_HCLK) " expected " XSTR(HAL_EXPECTED_SYSCLOCK)); #else #error "unknown system clock" #endif @@ -385,7 +391,7 @@ __FASTRAMFUNC__ uint64_t micros64() __FASTRAMFUNC__ uint64_t millis64() { - return hrt_micros64() / 1000U; + return hrt_millis64(); } diff --git a/libraries/AP_HAL_ESP32/AnalogIn.cpp b/libraries/AP_HAL_ESP32/AnalogIn.cpp index 0cb88f3e17384c..6ae04bc489591e 100644 --- a/libraries/AP_HAL_ESP32/AnalogIn.cpp +++ b/libraries/AP_HAL_ESP32/AnalogIn.cpp @@ -70,7 +70,7 @@ const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ESP32_ADC_PINS; #define DEFAULT_VREF 1100 //Use adc2_vref_to_gpio() to obtain a better estimate #define NO_OF_SAMPLES 256 //Multisampling -static const adc_atten_t atten = ADC_ATTEN_DB_11; +static const adc_atten_t atten = ADC_ATTEN_DB_12; //ardupin is the ardupilot assigned number, starting from 1-8(max) // 'pin' and _pin is a macro like 'ADC1_GPIO35_CHANNEL' from board config .h diff --git a/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp index 9a175a362f7a1d..b00af1bc543c09 100644 --- a/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp +++ b/libraries/AP_HAL_ESP32/HAL_ESP32_Class.cpp @@ -28,29 +28,33 @@ #include "Storage.h" #include "AnalogIn.h" #include "Util.h" +#if AP_SIM_ENABLED +#include +#endif -static Empty::UARTDriver uartADriver; static ESP32::UARTDriver cons(0); -static ESP32::UARTDriver uartBDriver(1); #ifdef HAL_ESP32_WIFI #if HAL_ESP32_WIFI == 1 -static ESP32::WiFiDriver uartCDriver; //tcp, client should connect to 192.168.4.1 port 5760 +static ESP32::WiFiDriver serial1Driver; //tcp, client should connect to 192.168.4.1 port 5760 #elif HAL_ESP32_WIFI == 2 -static ESP32::WiFiUdpDriver uartCDriver; //udp +static ESP32::WiFiUdpDriver serial1Driver; //udp #endif #else -static Empty::UARTDriver uartCDriver; +static Empty::UARTDriver serial1Driver; #endif -static ESP32::UARTDriver uartDDriver(2); -static Empty::UARTDriver uartEDriver; -static Empty::UARTDriver uartFDriver; -static Empty::UARTDriver uartGDriver; -static Empty::UARTDriver uartHDriver; -static Empty::UARTDriver uartIDriver; -static Empty::UARTDriver uartJDriver; +static ESP32::UARTDriver serial2Driver(2); +static ESP32::UARTDriver serial3Driver(1); +static Empty::UARTDriver serial4Driver; +static Empty::UARTDriver serial5Driver; +static Empty::UARTDriver serial6Driver; +static Empty::UARTDriver serial7Driver; +static Empty::UARTDriver serial8Driver; +static Empty::UARTDriver serial9Driver; +#if HAL_WITH_DSP static Empty::DSP dspDriver; +#endif static ESP32::I2CDeviceManager i2cDeviceManager; static ESP32::SPIDeviceManager spiDeviceManager; @@ -59,29 +63,41 @@ static ESP32::AnalogIn analogIn; #else static Empty::AnalogIn analogIn; #endif +#ifdef HAL_USE_EMPTY_STORAGE +static Empty::Storage storageDriver; +#else static ESP32::Storage storageDriver; +#endif static Empty::GPIO gpioDriver; +#if AP_SIM_ENABLED +static Empty::RCOutput rcoutDriver; +#else static ESP32::RCOutput rcoutDriver; +#endif static ESP32::RCInput rcinDriver; static ESP32::Scheduler schedulerInstance; static ESP32::Util utilInstance; static Empty::OpticalFlow opticalFlowDriver; static Empty::Flash flashDriver; +#if AP_SIM_ENABLED +static AP_HAL::SIMState xsimstate; +#endif + extern const AP_HAL::HAL& hal; HAL_ESP32::HAL_ESP32() : AP_HAL::HAL( &cons, //Console/mavlink - &uartBDriver, //GPS 1 - &uartCDriver, //Telem 1 - &uartDDriver, //Telem 2 - &uartEDriver, //GPS 2 - &uartFDriver, //Extra 1 - &uartGDriver, //Extra 2 - &uartHDriver, //Extra 3 - &uartIDriver, //Extra 4 - &uartJDriver, //Extra 5 + &serial1Driver, //Telem 1 + &serial2Driver, //Telem 2 + &serial3Driver, //GPS 1 + &serial4Driver, //GPS 2 + &serial5Driver, //Extra 1 + &serial6Driver, //Extra 2 + &serial7Driver, //Extra 3 + &serial8Driver, //Extra 4 + &serial9Driver, //Extra 5 &i2cDeviceManager, &spiDeviceManager, nullptr, @@ -95,7 +111,12 @@ HAL_ESP32::HAL_ESP32() : &utilInstance, &opticalFlowDriver, &flashDriver, +#if AP_SIM_ENABLED + &xsimstate, +#endif +#if HAL_WITH_DSP &dspDriver, +#endif nullptr ) {} diff --git a/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h b/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h index d59f080c0c7249..fbd28b5dc6259e 100644 --- a/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h +++ b/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h @@ -15,6 +15,7 @@ class RCInput; class Util; class Semaphore; class Semaphore_Recursive; +class BinarySemaphore; class GPIO; class DigitalSource; class Storage; diff --git a/libraries/AP_HAL_ESP32/RCInput.cpp b/libraries/AP_HAL_ESP32/RCInput.cpp index 2923872dbd2a88..704877962d4e08 100644 --- a/libraries/AP_HAL_ESP32/RCInput.cpp +++ b/libraries/AP_HAL_ESP32/RCInput.cpp @@ -89,10 +89,10 @@ uint8_t RCInput::read(uint16_t* periods, uint8_t len) void RCInput::_timer_tick(void) { +#if AP_RCPROTOCOL_ENABLED if (!_init) { return; } - AP_RCProtocol &rcprot = AP::RC(); #ifdef HAL_ESP32_RCIN @@ -126,4 +126,5 @@ void RCInput::_timer_tick(void) #endif #endif +#endif // AP_RCPROTOCOL_ENABLED } diff --git a/libraries/AP_HAL_ESP32/README.md b/libraries/AP_HAL_ESP32/README.md index de2fbe7e70d9ac..e891fa9571edad 100644 --- a/libraries/AP_HAL_ESP32/README.md +++ b/libraries/AP_HAL_ESP32/README.md @@ -96,8 +96,10 @@ ESPBAUD=921600 ./waf plane --upload You can use your default build system (make or ninja) to build other esp-idf target. For example : -- ninja flash -- ninja monitor + source modules/esp_idf/export.sh + cd /home/buzz2/ardupilot/build/esp32buzz/esp-idf_build + ninja flash + ninja monitor If you want to specify the port, specify before any command: ``` diff --git a/libraries/AP_HAL_ESP32/Scheduler.cpp b/libraries/AP_HAL_ESP32/Scheduler.cpp index f4d26dde3ae0ed..976b7245fdd484 100644 --- a/libraries/AP_HAL_ESP32/Scheduler.cpp +++ b/libraries/AP_HAL_ESP32/Scheduler.cpp @@ -38,6 +38,8 @@ using namespace ESP32; extern const AP_HAL::HAL& hal; bool Scheduler::_initialized = true; +TaskHandle_t idle_0 = NULL; +TaskHandle_t idle_1 = NULL; Scheduler::Scheduler() { @@ -46,7 +48,7 @@ Scheduler::Scheduler() void disableCore0WDT() { - TaskHandle_t idle_0 = xTaskGetIdleTaskHandleForCPU(0); + idle_0 = xTaskGetIdleTaskHandleForCPU(0); if (idle_0 == NULL || esp_task_wdt_delete(idle_0) != ESP_OK) { //print("Failed to remove Core 0 IDLE task from WDT"); } @@ -54,11 +56,23 @@ void disableCore0WDT() void disableCore1WDT() { - TaskHandle_t idle_1 = xTaskGetIdleTaskHandleForCPU(1); + idle_1 = xTaskGetIdleTaskHandleForCPU(1); if (idle_1 == NULL || esp_task_wdt_delete(idle_1) != ESP_OK) { //print("Failed to remove Core 1 IDLE task from WDT"); } } +void enableCore0WDT() +{ + if (idle_0 != NULL && esp_task_wdt_add(idle_0) != ESP_OK) { + //print("Failed to add Core 0 IDLE task to WDT"); + } +} +void enableCore1WDT() +{ + if (idle_1 != NULL && esp_task_wdt_add(idle_1) != ESP_OK) { + //print("Failed to add Core 1 IDLE task to WDT"); + } +} void Scheduler::init() { @@ -67,57 +81,69 @@ void Scheduler::init() printf("%s:%d \n", __PRETTY_FUNCTION__, __LINE__); #endif + // disable wd while booting, as things like mounting the sd-card in the io thread can take a while, especially if there isn't hardware attached. + disableCore0WDT(); //FASTCPU + disableCore1WDT(); //SLOWCPU + + hal.console->printf("%s:%d running with CONFIG_FREERTOS_HZ=%d\n", __PRETTY_FUNCTION__, __LINE__,CONFIG_FREERTOS_HZ); + // keep main tasks that need speed on CPU 0 + // pin potentially slow stuff to CPU 1, as we have disabled the WDT on that core. + #define FASTCPU 0 + #define SLOWCPU 1 + // pin main thread to Core 0, and we'll also pin other heavy-tasks to core 1, like wifi-related. - if (xTaskCreatePinnedToCore(_main_thread, "APM_MAIN", Scheduler::MAIN_SS, this, Scheduler::MAIN_PRIO, &_main_task_handle,1) != pdPASS) { + if (xTaskCreatePinnedToCore(_main_thread, "APM_MAIN", Scheduler::MAIN_SS, this, Scheduler::MAIN_PRIO, &_main_task_handle,FASTCPU) != pdPASS) { //if (xTaskCreate(_main_thread, "APM_MAIN", Scheduler::MAIN_SS, this, Scheduler::MAIN_PRIO, &_main_task_handle) != pdPASS) { - hal.console->printf("FAILED to create task _main_thread\n"); + hal.console->printf("FAILED to create task _main_thread on FASTCPU\n"); } else { - hal.console->printf("OK created task _main_thread\n"); + hal.console->printf("OK created task _main_thread on FASTCPU\n"); } - if (xTaskCreate(_timer_thread, "APM_TIMER", TIMER_SS, this, TIMER_PRIO, &_timer_task_handle) != pdPASS) { - hal.console->printf("FAILED to create task _timer_thread\n"); + if (xTaskCreatePinnedToCore(_timer_thread, "APM_TIMER", TIMER_SS, this, TIMER_PRIO, &_timer_task_handle,FASTCPU) != pdPASS) { + hal.console->printf("FAILED to create task _timer_thread on FASTCPU\n"); } else { - hal.console->printf("OK created task _timer_thread\n"); + hal.console->printf("OK created task _timer_thread on FASTCPU\n"); } - if (xTaskCreatePinnedToCore(_rcout_thread, "APM_RCOUT", RCOUT_SS, this, RCOUT_PRIO, &_rcout_task_handle,0) != pdPASS) { - hal.console->printf("FAILED to create task _rcout_thread\n"); + if (xTaskCreatePinnedToCore(_rcout_thread, "APM_RCOUT", RCOUT_SS, this, RCOUT_PRIO, &_rcout_task_handle,SLOWCPU) != pdPASS) { + hal.console->printf("FAILED to create task _rcout_thread on SLOWCPU\n"); } else { - hal.console->printf("OK created task _rcout_thread\n"); + hal.console->printf("OK created task _rcout_thread on SLOWCPU\n"); } - if (xTaskCreatePinnedToCore(_rcin_thread, "APM_RCIN", RCIN_SS, this, RCIN_PRIO, &_rcin_task_handle,0) != pdPASS) { - hal.console->printf("FAILED to create task _rcin_thread\n"); + if (xTaskCreatePinnedToCore(_rcin_thread, "APM_RCIN", RCIN_SS, this, RCIN_PRIO, &_rcin_task_handle,SLOWCPU) != pdPASS) { + hal.console->printf("FAILED to create task _rcin_thread on SLOWCPU\n"); } else { - hal.console->printf("OK created task _rcin_thread\n"); + hal.console->printf("OK created task _rcin_thread on SLOWCPU\n"); } - // pin this thread to Core 1 - if (xTaskCreatePinnedToCore(_uart_thread, "APM_UART", UART_SS, this, UART_PRIO, &_uart_task_handle,0) != pdPASS) { - hal.console->printf("FAILED to create task _uart_thread\n"); + // pin this thread to Core 1 as it keeps all teh uart/s feed data, and we need that quick. + if (xTaskCreatePinnedToCore(_uart_thread, "APM_UART", UART_SS, this, UART_PRIO, &_uart_task_handle,FASTCPU) != pdPASS) { + hal.console->printf("FAILED to create task _uart_thread on FASTCPU\n"); } else { - hal.console->printf("OK created task _uart_thread\n"); + hal.console->printf("OK created task _uart_thread on FASTCPU\n"); } - if (xTaskCreate(_io_thread, "SchedulerIO:APM_IO", IO_SS, this, IO_PRIO, &_io_task_handle) != pdPASS) { - hal.console->printf("FAILED to create task _io_thread\n"); + // we put thos on the SLOW core as it mounts the sd card, and that often isn't conencted. + if (xTaskCreatePinnedToCore(_io_thread, "SchedulerIO:APM_IO", IO_SS, this, IO_PRIO, &_io_task_handle,SLOWCPU) != pdPASS) { + hal.console->printf("FAILED to create task _io_thread on SLOWCPU\n"); } else { - hal.console->printf("OK created task _io_thread\n"); + hal.console->printf("OK created task _io_thread on SLOWCPU\n"); } - if (xTaskCreate(_storage_thread, "APM_STORAGE", STORAGE_SS, this, STORAGE_PRIO, &_storage_task_handle) != pdPASS) { //no actual flash writes without this, storage kinda appears to work, but does an erase on every boot and params don't persist over reset etc. + if (xTaskCreatePinnedToCore(_storage_thread, "APM_STORAGE", STORAGE_SS, this, STORAGE_PRIO, &_storage_task_handle,SLOWCPU) != pdPASS) { //no actual flash writes without this, storage kinda appears to work, but does an erase on every boot and params don't persist over reset etc. hal.console->printf("FAILED to create task _storage_thread\n"); } else { hal.console->printf("OK created task _storage_thread\n"); } - // xTaskCreate(_print_profile, "APM_PROFILE", IO_SS, this, IO_PRIO, nullptr); + // xTaskCreatePinnedToCore(_print_profile, "APM_PROFILE", IO_SS, this, IO_PRIO, nullptr,SLOWCPU); - //disableCore0WDT(); - //disableCore1WDT(); + hal.console->printf("OK Sched Init, enabling WD\n"); + enableCore0WDT(); //FASTCPU + //enableCore1WDT(); //we don't enable WD on SLOWCPU right now. } @@ -164,8 +190,9 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_ { PRIORITY_RCIN, RCIN_PRIO}, { PRIORITY_IO, IO_PRIO}, { PRIORITY_UART, UART_PRIO}, + { PRIORITY_NET, WIFI_PRIO1}, { PRIORITY_STORAGE, STORAGE_PRIO}, - { PRIORITY_SCRIPTING, IO_PRIO}, + { PRIORITY_SCRIPTING, UART_PRIO}, }; for (uint8_t i=0; i_initialized) { sched->delay_microseconds(10000); } #ifdef SCHEDDEBUG @@ -474,7 +502,7 @@ void Scheduler::_uart_thread(void *arg) printf("%s:%d start \n", __PRETTY_FUNCTION__, __LINE__); #endif Scheduler *sched = (Scheduler *)arg; - while (!_initialized) { + while (!sched->_initialized) { sched->delay_microseconds(2000); } #ifdef SCHEDDEBUG diff --git a/libraries/AP_HAL_ESP32/SdCard.cpp b/libraries/AP_HAL_ESP32/SdCard.cpp index f652e865fd5fc9..0f91711e1a9a90 100644 --- a/libraries/AP_HAL_ESP32/SdCard.cpp +++ b/libraries/AP_HAL_ESP32/SdCard.cpp @@ -32,6 +32,8 @@ #include #include "SPIDevice.h" +#include "soc/rtc_wdt.h" + #ifdef HAL_ESP32_SDCARD #if CONFIG_IDF_TARGET_ESP32S2 ||CONFIG_IDF_TARGET_ESP32C3 @@ -165,7 +167,6 @@ void mount_sdcard_mmc() sdmmc_card_t* card; esp_err_t ret = esp_vfs_fat_sdmmc_mount("/SDCARD", &host, &slot_config, &mount_config, &card); - if (ret == ESP_OK) { mkdir("/SDCARD/APM", 0777); mkdir("/SDCARD/APM/LOGS", 0777); diff --git a/libraries/AP_HAL_ESP32/Semaphores.cpp b/libraries/AP_HAL_ESP32/Semaphores.cpp index a397a53b183b88..d76ca820aeae18 100644 --- a/libraries/AP_HAL_ESP32/Semaphores.cpp +++ b/libraries/AP_HAL_ESP32/Semaphores.cpp @@ -73,3 +73,46 @@ bool Semaphore::check_owner() { return xSemaphoreGetMutexHolder((QueueHandle_t)handle) == xTaskGetCurrentTaskHandle(); } + + +/* + BinarySemaphore implementation + */ +BinarySemaphore::BinarySemaphore(bool initial_state) +{ + _sem = xSemaphoreCreateBinary(); + if (initial_state) { + xSemaphoreGive(_sem); + } +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + TickType_t ticks = pdMS_TO_TICKS(timeout_us / 1000U); + return xSemaphoreTake(_sem, ticks) == pdTRUE; +} + +bool BinarySemaphore::wait_blocking() +{ + return xSemaphoreTake(_sem, portMAX_DELAY) == pdTRUE; +} + +void BinarySemaphore::signal() +{ + xSemaphoreGive(_sem); +} + +void BinarySemaphore::signal_ISR() +{ + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(_sem, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +BinarySemaphore::~BinarySemaphore(void) +{ + if (_sem != nullptr) { + vSemaphoreDelete(_sem); + } + _sem = nullptr; +} diff --git a/libraries/AP_HAL_ESP32/Semaphores.h b/libraries/AP_HAL_ESP32/Semaphores.h index 844fd1b271d7b7..0a950f7a106208 100644 --- a/libraries/AP_HAL_ESP32/Semaphores.h +++ b/libraries/AP_HAL_ESP32/Semaphores.h @@ -20,6 +20,8 @@ #include #include #include "HAL_ESP32_Namespace.h" +#include +#include class ESP32::Semaphore : public AP_HAL::Semaphore { @@ -34,3 +36,20 @@ class ESP32::Semaphore : public AP_HAL::Semaphore protected: void* handle; }; + +class ESP32::BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + ~BinarySemaphore(void); + + CLASS_NO_COPY(BinarySemaphore); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + void signal_ISR(void) override; + +protected: + SemaphoreHandle_t _sem; +}; + diff --git a/libraries/AP_HAL_ESP32/Util.cpp b/libraries/AP_HAL_ESP32/Util.cpp index fa0e6060b62b20..d232ff890765e5 100644 --- a/libraries/AP_HAL_ESP32/Util.cpp +++ b/libraries/AP_HAL_ESP32/Util.cpp @@ -33,6 +33,7 @@ #include "esp_log.h" #include "esp_system.h" #include "esp_heap_caps.h" +#include extern const AP_HAL::HAL& hal; @@ -268,28 +269,17 @@ bool Util::was_watchdog_reset() const || reason == ESP_RST_WDT; } -#if CH_DBG_ENABLE_STACK_CHECK == TRUE /* display stack usage as text buffer for @SYS/threads.txt */ -size_t Util::thread_info(char *buf, size_t bufsize) +void Util::thread_info(ExpandingString &str) { - thread_t *tp; - size_t total = 0; - // a header to allow for machine parsers to determine format - int n = snprintf(buf, bufsize, "ThreadsV1\n"); - if (n <= 0) { - return 0; - } + str.printf("ThreadsV1\n"); // char buffer[1024]; // vTaskGetRunTimeStats(buffer); // snprintf(buf, bufsize,"\n\n%s\n", buffer); - - // total = .. - - return total; } -#endif // CH_DBG_ENABLE_STACK_CHECK == TRUE + diff --git a/libraries/AP_HAL_ESP32/Util.h b/libraries/AP_HAL_ESP32/Util.h index 15cf1a72a25d0b..3360e1ad5a1974 100644 --- a/libraries/AP_HAL_ESP32/Util.h +++ b/libraries/AP_HAL_ESP32/Util.h @@ -62,10 +62,8 @@ class ESP32::Util : public AP_HAL::Util // return true if the reason for the reboot was a watchdog reset bool was_watchdog_reset() const override; -#if CH_DBG_ENABLE_STACK_CHECK == TRUE // request information on running threads - size_t thread_info(char *buf, size_t bufsize) override; -#endif + void thread_info(ExpandingString &str) override; private: #ifdef HAL_PWM_ALARM diff --git a/libraries/AP_HAL_ESP32/WiFiDriver.cpp b/libraries/AP_HAL_ESP32/WiFiDriver.cpp index b11c6b978c266e..bc57391f009311 100644 --- a/libraries/AP_HAL_ESP32/WiFiDriver.cpp +++ b/libraries/AP_HAL_ESP32/WiFiDriver.cpp @@ -24,6 +24,7 @@ #include "esp_system.h" #include "esp_wifi.h" #include "esp_event.h" +#include "esp_log.h" #include "nvs_flash.h" #include "lwip/err.h" @@ -50,10 +51,15 @@ void WiFiDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) if (_state == NOT_INITIALIZED) { initialize_wifi(); - if (xTaskCreatePinnedToCore(_wifi_thread, "APM_WIFI1", Scheduler::WIFI_SS1, this, Scheduler::WIFI_PRIO1, &_wifi_task_handle,0) != pdPASS) { - hal.console->printf("FAILED to create task _wifi_thread\n"); + // keep main tasks that need speed on CPU 0 + // pin potentially slow stuff to CPU 1, as we have disabled the WDT on that core. + #define FASTCPU 0 + #define SLOWCPU 1 + + if (xTaskCreatePinnedToCore(_wifi_thread, "APM_WIFI1", Scheduler::WIFI_SS1, this, Scheduler::WIFI_PRIO1, &_wifi_task_handle,FASTCPU) != pdPASS) { + hal.console->printf("FAILED to create task _wifi_thread on FASTCPU\n"); } else { - hal.console->printf("OK created task _wifi_thread\n"); + hal.console->printf("OK created task _wifi_thread on FASTCPU\n"); } _readbuf.set_size(RX_BUF_SIZE); @@ -203,31 +209,158 @@ bool WiFiDriver::write_data() return true; } +#if WIFI_STATION + +#define WIFI_CONNECTED_BIT BIT0 +#define WIFI_FAIL_BIT BIT1 + +#ifndef ESP_STATION_MAXIMUM_RETRY +#define ESP_STATION_MAXIMUM_RETRY 10 +#endif + +static const char *TAG = "wifi station"; +static int s_retry_num = 0; +static EventGroupHandle_t s_wifi_event_group; + +static void _sta_event_handler(void* arg, esp_event_base_t event_base, + int32_t event_id, void* event_data) +{ + if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) { + esp_wifi_connect(); + } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) { + if (s_retry_num < ESP_STATION_MAXIMUM_RETRY) { + esp_wifi_connect(); + s_retry_num++; + ESP_LOGI(TAG, "retry to connect to the AP"); + } else { + xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT); + } + ESP_LOGI(TAG,"connect to the AP fail"); + } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) { + s_retry_num = 0; + xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); + } +} +#endif + void WiFiDriver::initialize_wifi() { - tcpip_adapter_init(); - nvs_flash_init(); - esp_event_loop_init(nullptr, nullptr); - wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); - esp_wifi_init(&cfg); - esp_wifi_set_storage(WIFI_STORAGE_FLASH); +#ifndef WIFI_PWD + #default WIFI_PWD "ardupilot1" +#endif + //Initialize NVS + esp_err_t ret = nvs_flash_init(); + if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { + ESP_ERROR_CHECK(nvs_flash_erase()); + ret = nvs_flash_init(); + } + ESP_ERROR_CHECK(ret); + + ESP_ERROR_CHECK(esp_netif_init()); + ESP_ERROR_CHECK(esp_event_loop_create_default()); + wifi_config_t wifi_config; - memset(&wifi_config, 0, sizeof(wifi_config)); -#ifdef WIFI_SSID - strcpy((char *)wifi_config.ap.ssid, WIFI_SSID); -#else - strcpy((char *)wifi_config.ap.ssid, "ardupilot"); + bzero(&wifi_config, sizeof(wifi_config)); + +/* + Acting as an Access Point (softAP) +*/ +#if !WIFI_STATION +#ifndef WIFI_SSID + #define WIFI_SSID "ardupilot" +#endif +#ifndef WIFI_CHANNEL + #define WIFI_CHANNEL 1 #endif -#ifdef WIFI_PWD + + esp_netif_create_default_wifi_ap(); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + + + strcpy((char *)wifi_config.ap.ssid, WIFI_SSID); strcpy((char *)wifi_config.ap.password, WIFI_PWD); + wifi_config.ap.ssid_len = strlen(WIFI_SSID), + wifi_config.ap.max_connection = WIFI_MAX_CONNECTION, + wifi_config.ap.authmode = WIFI_AUTH_WPA2_PSK; + wifi_config.ap.channel = WIFI_CHANNEL; + + if (strlen(WIFI_PWD) == 0) { + wifi_config.ap.authmode = WIFI_AUTH_OPEN; + } + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config)); + ESP_ERROR_CHECK(esp_wifi_start()); + + hal.console->printf("WiFi softAP init finished. SSID: %s password: %s channel: %d\n", + wifi_config.ap.ssid, wifi_config.ap.password, wifi_config.ap.channel); + +/* + Acting as a Station (WiFi Client) +*/ #else - strcpy((char *)wifi_config.ap.password, "ardupilot1"); +#ifndef WIFI_SSID_STATION + #define WIFI_SSID_STATION "ardupilot" +#endif +#ifndef WIFI_HOSTNAME + #define WIFI_HOSTNAME "ArduPilotESP32" +#endif + s_wifi_event_group = xEventGroupCreate(); + esp_netif_t *netif = esp_netif_create_default_wifi_sta(); + esp_netif_set_hostname(netif, WIFI_HOSTNAME); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + + esp_event_handler_instance_t instance_any_id; + esp_event_handler_instance_t instance_got_ip; + ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, + ESP_EVENT_ANY_ID, + &_sta_event_handler, + NULL, + &instance_any_id)); + ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, + IP_EVENT_STA_GOT_IP, + &_sta_event_handler, + NULL, + &instance_got_ip)); + + strcpy((char *)wifi_config.sta.ssid, WIFI_SSID_STATION); + strcpy((char *)wifi_config.sta.password, WIFI_PWD); + wifi_config.sta.threshold.authmode = WIFI_AUTH_OPEN; + wifi_config.sta.sae_pwe_h2e = WPA3_SAE_PWE_BOTH; + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA) ); + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config) ); + ESP_ERROR_CHECK(esp_wifi_start() ); + + hal.console->printf("WiFi Station init finished. Connecting:\n"); + + /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum + * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */ + EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group, + WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, + pdFALSE, pdFALSE, portMAX_DELAY); + + /* xEventGroupWaitBits() returns the bits before the call returned, hence we can test which event actually + * happened. */ + if (bits & WIFI_CONNECTED_BIT) { + ESP_LOGI(TAG, "connected to ap SSID: %s password: %s", + wifi_config.sta.ssid, wifi_config.sta.password); + } else if (bits & WIFI_FAIL_BIT) { + ESP_LOGI(TAG, "Failed to connect to SSID: %s, password: %s", + wifi_config.sta.ssid, wifi_config.sta.password); + } else { + ESP_LOGE(TAG, "UNEXPECTED EVENT"); + } + + /* The event will not be processed after unregister */ + ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip)); + ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id)); + vEventGroupDelete(s_wifi_event_group); #endif - wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK; - wifi_config.ap.max_connection = WIFI_MAX_CONNECTION; - esp_wifi_set_mode(WIFI_MODE_AP); - esp_wifi_set_config(WIFI_IF_AP, &wifi_config); - esp_wifi_start(); } size_t WiFiDriver::_write(const uint8_t *buffer, size_t size) diff --git a/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp index 6e08cabfb2a4d0..7352254d85fe81 100644 --- a/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp +++ b/libraries/AP_HAL_ESP32/WiFiUdpDriver.cpp @@ -32,6 +32,8 @@ #include "lwip/sys.h" #include "lwip/netdb.h" +#include "soc/rtc_wdt.h" + using namespace ESP32; extern const AP_HAL::HAL& hal; @@ -52,10 +54,15 @@ void WiFiUdpDriver::_begin(uint32_t b, uint16_t rxS, uint16_t txS) return; } - if (xTaskCreatePinnedToCore(_wifi_thread2, "APM_WIFI2", Scheduler::WIFI_SS2, this, Scheduler::WIFI_PRIO2, &_wifi_task_handle,0) != pdPASS) { - hal.console->printf("FAILED to create task _wifi_thread2\n"); + // keep main tasks that need speed on CPU 0 + // pin potentially slow stuff to CPU 1, as we have disabled the WDT on that core. + #define FASTCPU 0 + #define SLOWCPU 1 + + if (xTaskCreatePinnedToCore(_wifi_thread2, "APM_WIFI2", Scheduler::WIFI_SS2, this, Scheduler::WIFI_PRIO2, &_wifi_task_handle,FASTCPU) != pdPASS) { + hal.console->printf("FAILED to create task _wifi_thread2 on FASTCPU\n"); } else { - hal.console->printf("OK created task _wifi_thread2\n"); + hal.console->printf("OK created task _wifi_thread2 on FASTCPU\n"); } _readbuf.set_size(RX_BUF_SIZE); @@ -171,33 +178,158 @@ bool WiFiUdpDriver::write_data() return true; } +#if WIFI_STATION + +#define WIFI_CONNECTED_BIT BIT0 +#define WIFI_FAIL_BIT BIT1 + +#ifndef ESP_STATION_MAXIMUM_RETRY +#define ESP_STATION_MAXIMUM_RETRY 10 +#endif + +static const char *TAG = "wifi station"; +static int s_retry_num = 0; +static EventGroupHandle_t s_wifi_event_group; + +static void _sta_event_handler(void* arg, esp_event_base_t event_base, + int32_t event_id, void* event_data) +{ + if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) { + esp_wifi_connect(); + } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) { + if (s_retry_num < ESP_STATION_MAXIMUM_RETRY) { + esp_wifi_connect(); + s_retry_num++; + ESP_LOGI(TAG, "retry to connect to the AP"); + } else { + xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT); + } + ESP_LOGI(TAG,"connect to the AP fail"); + } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) { + s_retry_num = 0; + xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); + } +} +#endif + void WiFiUdpDriver::initialize_wifi() { - esp_event_loop_init(nullptr, nullptr); +#ifndef WIFI_PWD + #default WIFI_PWD "ardupilot1" +#endif + //Initialize NVS + esp_err_t ret = nvs_flash_init(); + if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) { + ESP_ERROR_CHECK(nvs_flash_erase()); + ret = nvs_flash_init(); + } + ESP_ERROR_CHECK(ret); - tcpip_adapter_init(); - nvs_flash_init(); + ESP_ERROR_CHECK(esp_netif_init()); + ESP_ERROR_CHECK(esp_event_loop_create_default()); - wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); - esp_wifi_init(&cfg); - esp_wifi_set_storage(WIFI_STORAGE_FLASH); wifi_config_t wifi_config; - memset(&wifi_config, 0, sizeof(wifi_config)); -#ifdef WIFI_SSID - strcpy((char *)wifi_config.ap.ssid, WIFI_SSID); -#else - strcpy((char *)wifi_config.ap.ssid, "ardupilot"); + bzero(&wifi_config, sizeof(wifi_config)); + +/* + Acting as an Access Point (softAP) +*/ +#if !WIFI_STATION +#ifndef WIFI_SSID + #define WIFI_SSID "ardupilot" #endif -#ifdef WIFI_PWD +#ifndef WIFI_CHANNEL + #define WIFI_CHANNEL 1 +#endif + + esp_netif_create_default_wifi_ap(); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + + + strcpy((char *)wifi_config.ap.ssid, WIFI_SSID); strcpy((char *)wifi_config.ap.password, WIFI_PWD); + wifi_config.ap.ssid_len = strlen(WIFI_SSID), + wifi_config.ap.max_connection = WIFI_MAX_CONNECTION, + wifi_config.ap.authmode = WIFI_AUTH_WPA2_PSK; + wifi_config.ap.channel = WIFI_CHANNEL; + + if (strlen(WIFI_PWD) == 0) { + wifi_config.ap.authmode = WIFI_AUTH_OPEN; + } + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config)); + ESP_ERROR_CHECK(esp_wifi_start()); + + hal.console->printf("WiFi softAP init finished. SSID: %s password: %s channel: %d\n", + wifi_config.ap.ssid, wifi_config.ap.password, wifi_config.ap.channel); + +/* + Acting as a Station (WiFi Client) +*/ #else - strcpy((char *)wifi_config.ap.password, "ardupilot1"); +#ifndef WIFI_SSID_STATION + #define WIFI_SSID_STATION "ardupilot" +#endif +#ifndef WIFI_HOSTNAME + #define WIFI_HOSTNAME "ArduPilotESP32" +#endif + s_wifi_event_group = xEventGroupCreate(); + esp_netif_t *netif = esp_netif_create_default_wifi_sta(); + esp_netif_set_hostname(netif, WIFI_HOSTNAME); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + + esp_event_handler_instance_t instance_any_id; + esp_event_handler_instance_t instance_got_ip; + ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, + ESP_EVENT_ANY_ID, + &_sta_event_handler, + NULL, + &instance_any_id)); + ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, + IP_EVENT_STA_GOT_IP, + &_sta_event_handler, + NULL, + &instance_got_ip)); + + strcpy((char *)wifi_config.sta.ssid, WIFI_SSID_STATION); + strcpy((char *)wifi_config.sta.password, WIFI_PWD); + wifi_config.sta.threshold.authmode = WIFI_AUTH_OPEN; + wifi_config.sta.sae_pwe_h2e = WPA3_SAE_PWE_BOTH; + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA) ); + ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config) ); + ESP_ERROR_CHECK(esp_wifi_start() ); + + hal.console->printf("WiFi Station init finished. Connecting:\n"); + + /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum + * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */ + EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group, + WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, + pdFALSE, pdFALSE, portMAX_DELAY); + + /* xEventGroupWaitBits() returns the bits before the call returned, hence we can test which event actually + * happened. */ + if (bits & WIFI_CONNECTED_BIT) { + ESP_LOGI(TAG, "connected to ap SSID: %s password: %s", + wifi_config.sta.ssid, wifi_config.sta.password); + } else if (bits & WIFI_FAIL_BIT) { + ESP_LOGI(TAG, "Failed to connect to SSID: %s, password: %s", + wifi_config.sta.ssid, wifi_config.sta.password); + } else { + ESP_LOGE(TAG, "UNEXPECTED EVENT"); + } + + /* The event will not be processed after unregister */ + ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip)); + ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id)); + vEventGroupDelete(s_wifi_event_group); #endif - wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK; - wifi_config.ap.max_connection = 4; - esp_wifi_set_mode(WIFI_MODE_AP); - esp_wifi_set_config(WIFI_IF_AP, &wifi_config); - esp_wifi_start(); } size_t WiFiUdpDriver::_write(const uint8_t *buffer, size_t size) @@ -221,7 +353,6 @@ void WiFiUdpDriver::_wifi_thread2(void *arg) fd_set rfds; FD_ZERO(&rfds); FD_SET(self->accept_socket, &rfds); - int s = select(self->accept_socket + 1, &rfds, NULL, NULL, &tv); if (s > 0 && FD_ISSET(self->accept_socket, &rfds)) { self->read_all(); diff --git a/libraries/AP_HAL_ESP32/WiFiUdpDriver.h b/libraries/AP_HAL_ESP32/WiFiUdpDriver.h index 9b11464e35aba5..e59b4d1b1104b0 100644 --- a/libraries/AP_HAL_ESP32/WiFiUdpDriver.h +++ b/libraries/AP_HAL_ESP32/WiFiUdpDriver.h @@ -22,6 +22,9 @@ #include "lwip/sockets.h" #include "esp_event.h" +#ifndef WIFI_MAX_CONNECTION +#define WIFI_MAX_CONNECTION 5 +#endif class ESP32::WiFiUdpDriver : public AP_HAL::UARTDriver { diff --git a/libraries/AP_HAL_ESP32/boards/esp32empty.h b/libraries/AP_HAL_ESP32/boards/esp32empty.h index 33f5d7c219ca99..d95e30f9b2af95 100644 --- a/libraries/AP_HAL_ESP32/boards/esp32empty.h +++ b/libraries/AP_HAL_ESP32/boards/esp32empty.h @@ -148,3 +148,4 @@ #define HAL_LOGGING_BACKENDS_DEFAULT 1 +#define AP_RCPROTOCOL_ENABLED 0 diff --git a/libraries/AP_HAL_ESP32/boards/esp32s3empty.h b/libraries/AP_HAL_ESP32/boards/esp32s3empty.h new file mode 100644 index 00000000000000..37c40f9a6419a6 --- /dev/null +++ b/libraries/AP_HAL_ESP32/boards/esp32s3empty.h @@ -0,0 +1,95 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_ESP32_S3EMPTY +// make sensor selection clearer + +//- these are missing from esp-idf......will not be needed later +#define RTC_WDT_STG_SEL_OFF 0 +#define RTC_WDT_STG_SEL_INT 1 +#define RTC_WDT_STG_SEL_RESET_CPU 2 +#define RTC_WDT_STG_SEL_RESET_SYSTEM 3 +#define RTC_WDT_STG_SEL_RESET_RTC 4 + +#define HAL_ESP32_BOARD_NAME "esp32s3empty" + +#define HAL_ESP32_RMT_RX_PIN_NUMBER GPIO_NUM_14 + +// no sensors +#define HAL_INS_DEFAULT HAL_INS_NONE + +#define HAL_BARO_ALLOW_INIT_NO_BARO 1 + +#define AP_COMPASS_ENABLE_DEFAULT 0 +#define ALLOW_ARM_NO_COMPASS +#define AP_AIRSPEED_ENABLED 0 +#define AP_AIRSPEED_ANALOG_ENABLED 0 +#define AP_AIRSPEED_BACKEND_DEFAULT_ENABLED 0 + +// no ADC +#define HAL_DISABLE_ADC_DRIVER 1 +#define HAL_USE_ADC 0 + +// 2 use udp, 1 use tcp... for udp,client needs to connect as UDPCL in missionplanner etc to 192.168.4.1 port 14550 +#define HAL_ESP32_WIFI 2 + +// see boards.py +#ifndef ENABLE_HEAP +#define ENABLE_HEAP 1 +#endif + +#define WIFI_SSID "ardupilot123" +#define WIFI_PWD "ardupilot123" + +//RCOUT which pins are used? + +#define HAL_ESP32_RCOUT { GPIO_NUM_11,GPIO_NUM_10, GPIO_NUM_9, GPIO_NUM_8, GPIO_NUM_7, GPIO_NUM_6 } + +// SPI BUS setup, including gpio, dma, etc +// note... we use 'vspi' for the bmp280 and mpu9250 +#define HAL_ESP32_SPI_BUSES {} + +// SPI per-device setup, including speeds, etc. +#define HAL_ESP32_SPI_DEVICES {} + +//I2C bus list +#define HAL_ESP32_I2C_BUSES {.port=I2C_NUM_0, .sda=GPIO_NUM_13, .scl=GPIO_NUM_14, .speed=400*KHZ, .internal=true, .soft=true} + +// rcin on what pin? +#define HAL_ESP32_RCIN GPIO_NUM_14 + + +//HARDWARE UARTS +#define HAL_ESP32_UART_DEVICES \ + {.port=UART_NUM_0, .rx=GPIO_NUM_44, .tx=GPIO_NUM_43 },{.port=UART_NUM_1, .rx=GPIO_NUM_17, .tx=GPIO_NUM_18 } + +#define HAL_LOGGING_FILESYSTEM_ENABLED 0 +#define HAL_LOGGING_DATAFLASH_ENABLED 0 +#define HAL_LOGGING_MAVLINK_ENABLED 0 + +#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" +#define HAL_BOARD_STORAGE_DIRECTORY "/SDCARD/APM/STORAGE" +#define HAL_BOARD_LOG_DIRECTORY "/SDCARD/APM/LOGS" +#define HAL_BOARD_TERRAIN_DIRECTORY "/SDCARD/APM/TERRAIN" + +#define HAL_LOGGING_BACKENDS_DEFAULT 1 + +#define AP_RCPROTOCOL_ENABLED 0 + +#define AP_FILESYSTEM_ESP32_ENABLED 0 +#define AP_SCRIPTING_ENABLED 0 +#define HAL_USE_EMPTY_STORAGE 1 + diff --git a/libraries/AP_HAL_ESP32/hwdef/esp32s3empty/hwdef.dat b/libraries/AP_HAL_ESP32/hwdef/esp32s3empty/hwdef.dat new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/libraries/AP_HAL_ESP32/system.cpp b/libraries/AP_HAL_ESP32/system.cpp index 4ee607c5617b72..021b921dc1e60d 100644 --- a/libraries/AP_HAL_ESP32/system.cpp +++ b/libraries/AP_HAL_ESP32/system.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "SdCard.h" #include @@ -51,7 +52,7 @@ uint64_t micros64() uint64_t millis64() { - return micros64()/1000; + return uint64_div1000(micros64()); } } // namespace AP_HAL 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 07f1a4df7e8e77..5c03820e3f827e 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt +++ b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/CMakeLists.txt @@ -34,25 +34,31 @@ if(NOT DEFINED ARDUPILOT_CMD) set(ARDUPILOT_CMD "none") endif() -IF(${ARDUPILOT_CMD} STREQUAL "plane") +message("ARDUPILOT_CMD=${ARDUPILOT_CMD}") +message("WAF_BUILD_TARGET=${WAF_BUILD_TARGET}") + +string(REGEX MATCH "^(examples|tool)/" IS_EXAMPLE "${WAF_BUILD_TARGET}") + +if (IS_EXAMPLE) + string(REPLACE "/" ";" A ${WAF_BUILD_TARGET}) + list(GET A 0 EXAMPLE_BASE) + list(GET A 1 EXAMPLE_NAME) + message("Building ${EXAMPLE_BASE} ${EXAMPLE_NAME}") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/${EXAMPLE_BASE}/lib${EXAMPLE_NAME}.a") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libap.a") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "plane") message("Building for plane") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libarduplane.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduPlane_libs.a") -ENDIF() - -IF(${ARDUPILOT_CMD} STREQUAL "copter") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "copter") message("Building for copter") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libarducopter.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduCopter_libs.a") -ENDIF() - -IF(${ARDUPILOT_CMD} STREQUAL "rover") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "rover") message("Building for rover") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libardurover.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libRover_libs.a") -ENDIF() - -IF(${ARDUPILOT_CMD} STREQUAL "sub") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "sub") message("Building for submarine") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libardusub.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduSub_libs.a") diff --git a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig index 397f554e1c4135..e1a5a3b0da288b 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig +++ b/libraries/AP_HAL_ESP32/targets/esp32/esp-idf/sdkconfig @@ -370,6 +370,7 @@ CONFIG_ESP_SLEEP_RTC_BUS_ISO_WORKAROUND=y # CONFIG_ESP_SLEEP_GPIO_RESET_WORKAROUND is not set CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND=y # CONFIG_ESP_SLEEP_MSPI_NEED_ALL_IO_PU is not set +CONFIG_ESP_SLEEP_GPIO_ENABLE_INTERNAL_RESISTORS=y # end of Sleep Config # @@ -403,6 +404,10 @@ CONFIG_ESP_PHY_CALIBRATION_AND_DATA_STORAGE=y CONFIG_ESP_PHY_MAX_WIFI_TX_POWER=20 CONFIG_ESP_PHY_MAX_TX_POWER=20 CONFIG_ESP_PHY_REDUCE_TX_POWER=y +CONFIG_ESP_PHY_RF_CAL_PARTIAL=y +# CONFIG_ESP_PHY_RF_CAL_NONE is not set +# CONFIG_ESP_PHY_RF_CAL_FULL is not set +CONFIG_ESP_PHY_CALIBRATION_MODE=0 # end of PHY # @@ -487,6 +492,10 @@ CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=16 CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1 CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=16 +CONFIG_ESP_WIFI_STATIC_RX_MGMT_BUFFER=y +# CONFIG_ESP_WIFI_DYNAMIC_RX_MGMT_BUFFER is not set +CONFIG_ESP_WIFI_DYNAMIC_RX_MGMT_BUF=0 +CONFIG_ESP_WIFI_RX_MGMT_BUF_NUM_DEF=5 # CONFIG_ESP32_WIFI_CSI_ENABLED is not set # CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED is not set # CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED is not set @@ -495,7 +504,7 @@ CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_0=y # CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_1 is not set CONFIG_ESP32_WIFI_SOFTAP_BEACON_MAX_LEN=752 CONFIG_ESP32_WIFI_MGMT_SBUF_NUM=32 -# CONFIG_ESP32_WIFI_IRAM_OPT is not set +CONFIG_ESP32_WIFI_IRAM_OPT=y CONFIG_ESP32_WIFI_RX_IRAM_OPT=y CONFIG_ESP32_WIFI_ENABLE_WPA3_SAE=y # CONFIG_ESP_WIFI_SLP_IRAM_OPT is not set @@ -517,6 +526,7 @@ CONFIG_ESP_COREDUMP_DATA_FORMAT_ELF=y CONFIG_ESP_COREDUMP_CHECKSUM_CRC32=y # CONFIG_ESP_COREDUMP_CHECKSUM_SHA256 is not set CONFIG_ESP_COREDUMP_ENABLE=y +CONFIG_ESP_COREDUMP_LOGS=y CONFIG_ESP_COREDUMP_MAX_TASKS_NUM=64 CONFIG_ESP_COREDUMP_UART_DELAY=0 CONFIG_ESP_COREDUMP_STACK_SIZE=0 @@ -652,6 +662,7 @@ CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y # CONFIG_LWIP_LOCAL_HOSTNAME="espressif" # CONFIG_LWIP_NETIF_API is not set +CONFIG_LWIP_TCPIP_TASK_PRIO=18 # CONFIG_LWIP_TCPIP_CORE_LOCKING is not set # CONFIG_LWIP_CHECK_THREAD_SAFETY is not set CONFIG_LWIP_DNS_SUPPORT_MDNS_QUERIES=y @@ -717,6 +728,8 @@ CONFIG_LWIP_TCP_SND_BUF_DEFAULT=5744 CONFIG_LWIP_TCP_WND_DEFAULT=5744 CONFIG_LWIP_TCP_RECVMBOX_SIZE=6 CONFIG_LWIP_TCP_QUEUE_OOSEQ=y +CONFIG_LWIP_TCP_OOSEQ_TIMEOUT=6 +CONFIG_LWIP_TCP_OOSEQ_MAX_PBUFS=4 # CONFIG_LWIP_TCP_SACK_OUT is not set # CONFIG_LWIP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES is not set CONFIG_LWIP_TCP_OVERSIZE_MSS=y 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 7cf8efb64a3e54..4e0fafb3eac05f 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/CMakeLists.txt @@ -34,35 +34,35 @@ if(NOT DEFINED ARDUPILOT_CMD) set(ARDUPILOT_CMD "none") endif() -IF(${ARDUPILOT_CMD} STREQUAL "plane") +message("WAF_BUILD_TARGET=${WAF_BUILD_TARGET}") + +string(REGEX MATCH "^(examples|tool)/" IS_EXAMPLE "${WAF_BUILD_TARGET}") + +if (IS_EXAMPLE) + string(REPLACE "/" ";" A ${WAF_BUILD_TARGET}) + list(GET A 0 EXAMPLE_BASE) + list(GET A 1 EXAMPLE_NAME) + message("Building ${EXAMPLE_BASE} ${EXAMPLE_NAME}") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/${EXAMPLE_BASE}/lib${EXAMPLE_NAME}.a") + target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libap.a") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "plane") message("Building for plane") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libarduplane.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduPlane_libs.a") -ENDIF() - -IF(${ARDUPILOT_CMD} STREQUAL "copter") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "copter") message("Building for copter") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libarducopter.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduCopter_libs.a") -ENDIF() - -IF(${ARDUPILOT_CMD} STREQUAL "rover") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "rover") message("Building for rover") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libardurover.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libRover_libs.a") -ENDIF() - -IF(${ARDUPILOT_CMD} STREQUAL "sub") +ELSEIF(${ARDUPILOT_CMD} STREQUAL "sub") message("Building for submarine") target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libardusub.a") target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libArduSub_libs.a") ENDIF() -IF(${ARDUPILOT_CMD} STREQUAL "antennatracker") - message("Building AntennaTracker") - target_link_libraries(${elf_file} "${ARDUPILOT_BIN}/libantennatracker.a") - target_link_libraries(${elf_file} "${ARDUPILOT_LIB}/libAntennaTracker_libs.a") -ENDIF() add_custom_target(showinc ALL COMMAND echo -e "$" diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig index ecac3313a903da..3d841f1251b122 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/esp-idf/sdkconfig @@ -28,9 +28,9 @@ CONFIG_APP_BUILD_USE_FLASH_SECTIONS=y # # Application manager # -CONFIG_APP_COMPILE_TIME_DATE=y -# CONFIG_APP_EXCLUDE_PROJECT_VER_VAR is not set -# CONFIG_APP_EXCLUDE_PROJECT_NAME_VAR is not set +# CONFIG_APP_COMPILE_TIME_DATE is not set +CONFIG_APP_EXCLUDE_PROJECT_VER_VAR=y +CONFIG_APP_EXCLUDE_PROJECT_NAME_VAR=y # CONFIG_APP_PROJECT_VER_FROM_CONFIG is not set CONFIG_APP_RETRIEVE_LEN_ELF_SHA=16 # end of Application manager @@ -70,8 +70,6 @@ CONFIG_BOOTLOADER_FLASH_XMC_SUPPORT=y # # Security features # -CONFIG_SECURE_BOOT_SUPPORTS_RSA=y -CONFIG_SECURE_TARGET_HAS_SECURE_ROM_DL_MODE=y # CONFIG_SECURE_SIGNED_APPS_NO_SECURE_BOOT is not set # CONFIG_SECURE_BOOT is not set # CONFIG_SECURE_FLASH_ENC_ENABLED is not set @@ -98,11 +96,11 @@ CONFIG_ESPTOOLPY_FLASHMODE_DIO=y # CONFIG_ESPTOOLPY_FLASHMODE_DOUT is not set CONFIG_ESPTOOLPY_FLASH_SAMPLE_MODE_STR=y CONFIG_ESPTOOLPY_FLASHMODE="dio" -# CONFIG_ESPTOOLPY_FLASHFREQ_120M is not set -# CONFIG_ESPTOOLPY_FLASHFREQ_80M is not set -CONFIG_ESPTOOLPY_FLASHFREQ_40M=y +CONFIG_ESPTOOLPY_FLASHFREQ_80M=y +# CONFIG_ESPTOOLPY_FLASHFREQ_40M is not set +# CONFIG_ESPTOOLPY_FLASHFREQ_26M is not set # CONFIG_ESPTOOLPY_FLASHFREQ_20M is not set -CONFIG_ESPTOOLPY_FLASHFREQ="40m" +CONFIG_ESPTOOLPY_FLASHFREQ="80m" # CONFIG_ESPTOOLPY_FLASHSIZE_1MB is not set # CONFIG_ESPTOOLPY_FLASHSIZE_2MB is not set # CONFIG_ESPTOOLPY_FLASHSIZE_4MB is not set @@ -147,9 +145,9 @@ CONFIG_PARTITION_TABLE_MD5=y # # Compiler options # -CONFIG_COMPILER_OPTIMIZATION_DEFAULT=y +# CONFIG_COMPILER_OPTIMIZATION_DEFAULT is not set # CONFIG_COMPILER_OPTIMIZATION_SIZE is not set -# CONFIG_COMPILER_OPTIMIZATION_PERF is not set +CONFIG_COMPILER_OPTIMIZATION_PERF=y # CONFIG_COMPILER_OPTIMIZATION_NONE is not set CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_ENABLE=y # CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_SILENT is not set @@ -189,7 +187,6 @@ CONFIG_APPTRACE_LOCK_ENABLE=y # # CONFIG_ADC_FORCE_XPD_FSM is not set CONFIG_ADC_DISABLE_DAC=y -# CONFIG_ADC_CONTINUOUS_FORCE_USE_ADC2_ON_C3_S3 is not set # end of ADC configuration # @@ -201,7 +198,7 @@ CONFIG_ADC_DISABLE_DAC=y # # SPI configuration # -# CONFIG_SPI_MASTER_IN_IRAM is not set +CONFIG_SPI_MASTER_IN_IRAM=y CONFIG_SPI_MASTER_ISR_IN_IRAM=y # CONFIG_SPI_SLAVE_IN_IRAM is not set CONFIG_SPI_SLAVE_ISR_IN_IRAM=y @@ -211,7 +208,6 @@ CONFIG_SPI_SLAVE_ISR_IN_IRAM=y # TWAI configuration # # CONFIG_TWAI_ISR_IN_IRAM is not set -# CONFIG_TWAI_ERRATA_FIX_LISTEN_ONLY_DOM is not set # end of TWAI configuration # @@ -239,18 +235,10 @@ CONFIG_EFUSE_MAX_BLK_LEN=256 # # ESP32S3-Specific # -CONFIG_ESP32S3_REV_MIN_0=y -# CONFIG_ESP32S3_REV_MIN_1 is not set -# CONFIG_ESP32S3_REV_MIN_2 is not set -CONFIG_ESP32S3_REV_MIN_FULL=0 -CONFIG_ESP_REV_MIN_FULL=0 -CONFIG_ESP32S3_REV_MAX_FULL_STR_OPT=y -CONFIG_ESP32S3_REV_MAX_FULL=99 -CONFIG_ESP_REV_MAX_FULL=99 # CONFIG_ESP32S3_DEFAULT_CPU_FREQ_80 is not set -CONFIG_ESP32S3_DEFAULT_CPU_FREQ_160=y -# CONFIG_ESP32S3_DEFAULT_CPU_FREQ_240 is not set -CONFIG_ESP32S3_DEFAULT_CPU_FREQ_MHZ=160 +#CONFIG_ESP32S3_DEFAULT_CPU_FREQ_160 is not set +CONFIG_ESP32S3_DEFAULT_CPU_FREQ_240=y +CONFIG_ESP32S3_DEFAULT_CPU_FREQ_MHZ=240 # # Cache config @@ -312,6 +300,9 @@ CONFIG_ESP32S3_DEEP_SLEEP_WAKEUP_DELAY=2000 # # ADC-Calibration # +CONFIG_ADC_CAL_EFUSE_TP_ENABLE=y +CONFIG_ADC_CAL_EFUSE_VREF_ENABLE=y +CONFIG_ADC_CAL_LUT_ENABLE=y # end of ADC-Calibration # @@ -355,15 +346,15 @@ CONFIG_ESP_MAC_ADDR_UNIVERSE_WIFI_STA=y CONFIG_ESP_MAC_ADDR_UNIVERSE_WIFI_AP=y CONFIG_ESP_MAC_ADDR_UNIVERSE_BT=y CONFIG_ESP_MAC_ADDR_UNIVERSE_ETH=y -# CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES_TWO is not set -CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES_FOUR=y -CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES=4 +CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES_TWO=y +#CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES_FOUR is not set +CONFIG_ESP32S3_UNIVERSAL_MAC_ADDRESSES=2 # end of MAC Config # # Sleep Config # -CONFIG_ESP_SLEEP_POWER_DOWN_FLASH=y +# CONFIG_ESP_SLEEP_POWER_DOWN_FLASH is not set CONFIG_ESP_SLEEP_RTC_BUS_ISO_WORKAROUND=y # CONFIG_ESP_SLEEP_GPIO_RESET_WORKAROUND is not set # CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND is not set @@ -411,13 +402,6 @@ CONFIG_PM_POWER_DOWN_CPU_IN_LIGHT_SLEEP=y CONFIG_PM_POWER_DOWN_TAGMEM_IN_LIGHT_SLEEP=y # end of Power Management -# -# ESP Ringbuf -# -# CONFIG_RINGBUF_PLACE_FUNCTIONS_INTO_FLASH is not set -# CONFIG_RINGBUF_PLACE_ISR_FUNCTIONS_INTO_FLASH is not set -# end of ESP Ringbuf - # # ESP System Settings # @@ -454,7 +438,7 @@ CONFIG_ESP_CONSOLE_MULTIPLE_UART=y CONFIG_ESP_CONSOLE_UART_NUM=0 CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200 CONFIG_ESP_INT_WDT=y -CONFIG_ESP_INT_WDT_TIMEOUT_MS=300 +CONFIG_ESP_INT_WDT_TIMEOUT_MS=1000 CONFIG_ESP_INT_WDT_CHECK_CPU1=y CONFIG_ESP_TASK_WDT=y # CONFIG_ESP_TASK_WDT_PANIC is not set @@ -507,16 +491,24 @@ CONFIG_ESP32_WIFI_ENABLE_WPA3_SAE=y # CONFIG_ESP_WIFI_GCMP_SUPPORT is not set # CONFIG_ESP_WIFI_GMAC_SUPPORT is not set CONFIG_ESP_WIFI_SOFTAP_SUPPORT=y -# CONFIG_ESP_WIFI_SLP_BEACON_LOST_OPT is not set -CONFIG_ESP_WIFI_ESPNOW_MAX_ENCRYPT_NUM=7 # end of Wi-Fi # # Core dump # # CONFIG_ESP_COREDUMP_ENABLE_TO_FLASH is not set -# CONFIG_ESP_COREDUMP_ENABLE_TO_UART is not set -CONFIG_ESP_COREDUMP_ENABLE_TO_NONE=y +CONFIG_ESP_COREDUMP_ENABLE_TO_UART=y +# CONFIG_ESP_COREDUMP_ENABLE_TO_NONE is not set +# CONFIG_ESP_COREDUMP_DATA_FORMAT_BIN is not set +CONFIG_ESP_COREDUMP_DATA_FORMAT_ELF=y +CONFIG_ESP_COREDUMP_CHECKSUM_CRC32=y +# CONFIG_ESP_COREDUMP_CHECKSUM_SHA256 is not set +CONFIG_ESP_COREDUMP_ENABLE=y +CONFIG_ESP_COREDUMP_MAX_TASKS_NUM=64 +CONFIG_ESP_COREDUMP_UART_DELAY=0 +CONFIG_ESP_COREDUMP_DECODE_INFO=y +# CONFIG_ESP_COREDUMP_DECODE_DISABLE is not set +CONFIG_ESP_COREDUMP_DECODE="info" # end of Core dump # @@ -563,7 +555,7 @@ CONFIG_FREERTOS_TICK_SUPPORT_SYSTIMER=y CONFIG_FREERTOS_CORETIMER_SYSTIMER_LVL1=y # CONFIG_FREERTOS_CORETIMER_SYSTIMER_LVL3 is not set CONFIG_FREERTOS_SYSTICK_USES_SYSTIMER=y -CONFIG_FREERTOS_HZ=100 +CONFIG_FREERTOS_HZ=500 CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y # CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE is not set # CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL is not set @@ -575,7 +567,7 @@ CONFIG_FREERTOS_ASSERT_FAIL_ABORT=y # CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE is not set # CONFIG_FREERTOS_ASSERT_DISABLE is not set CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1536 -CONFIG_FREERTOS_ISR_STACKSIZE=1536 +CONFIG_FREERTOS_ISR_STACKSIZE=2100 # CONFIG_FREERTOS_LEGACY_HOOKS is not set CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16 CONFIG_FREERTOS_SUPPORT_STATIC_ALLOCATION=y @@ -645,7 +637,6 @@ CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y CONFIG_LWIP_LOCAL_HOSTNAME="espressif" # CONFIG_LWIP_NETIF_API is not set # CONFIG_LWIP_TCPIP_CORE_LOCKING is not set -# CONFIG_LWIP_CHECK_THREAD_SAFETY is not set CONFIG_LWIP_DNS_SUPPORT_MDNS_QUERIES=y # CONFIG_LWIP_L2_TO_L3_COPY is not set # CONFIG_LWIP_IRAM_OPTIMIZATION is not set @@ -666,15 +657,12 @@ CONFIG_LWIP_IP6_FRAG=y # CONFIG_LWIP_ETHARP_TRUST_IP_MAC is not set CONFIG_LWIP_ESP_GRATUITOUS_ARP=y CONFIG_LWIP_GARP_TMR_INTERVAL=60 -CONFIG_LWIP_ESP_MLDV6_REPORT=y -CONFIG_LWIP_MLDV6_TMR_INTERVAL=40 CONFIG_LWIP_TCPIP_RECVMBOX_SIZE=32 CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y # CONFIG_LWIP_DHCP_DISABLE_CLIENT_ID is not set CONFIG_LWIP_DHCP_DISABLE_VENDOR_CLASS_ID=y # CONFIG_LWIP_DHCP_RESTORE_LAST_IP is not set CONFIG_LWIP_DHCP_OPTIONS_LEN=68 -CONFIG_LWIP_DHCP_COARSE_TIMER_SECS=1 # # DHCP server @@ -704,7 +692,6 @@ CONFIG_LWIP_TCP_SYNMAXRTX=12 CONFIG_LWIP_TCP_MSS=1440 CONFIG_LWIP_TCP_TMR_INTERVAL=250 CONFIG_LWIP_TCP_MSL=60000 -CONFIG_LWIP_TCP_FIN_WAIT_TIMEOUT=20000 CONFIG_LWIP_TCP_SND_BUF_DEFAULT=5744 CONFIG_LWIP_TCP_WND_DEFAULT=5744 CONFIG_LWIP_TCP_RECVMBOX_SIZE=6 @@ -815,7 +802,6 @@ CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_FULL=y # CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_CMN is not set # CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_NONE is not set # CONFIG_MBEDTLS_CUSTOM_CERTIFICATE_BUNDLE is not set -CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_MAX_CERTS=200 # end of Certificate Bundle # CONFIG_MBEDTLS_ECP_RESTARTABLE is not set @@ -932,7 +918,6 @@ CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y # # NVS # -# CONFIG_NVS_ASSERT_ERROR_CHECK is not set # end of NVS # @@ -1003,6 +988,7 @@ CONFIG_VFS_SUPPORT_TERMIOS=y # Host File System I/O (Semihosting) # CONFIG_VFS_SEMIHOSTFS_MAX_MOUNT_POINTS=1 +CONFIG_VFS_SEMIHOSTFS_HOST_PATH_MAX_LEN=128 # end of Host File System I/O (Semihosting) # end of Virtual file system @@ -1046,9 +1032,9 @@ CONFIG_LOG_BOOTLOADER_LEVEL_ERROR=y CONFIG_LOG_BOOTLOADER_LEVEL=1 # CONFIG_APP_ROLLBACK_ENABLE is not set # CONFIG_FLASH_ENCRYPTION_ENABLED is not set -# CONFIG_FLASHMODE_QIO is not set +CONFIG_FLASHMODE_QIO=y # CONFIG_FLASHMODE_QOUT is not set -CONFIG_FLASHMODE_DIO=y +# CONFIG_FLASHMODE_DIO is not set # CONFIG_FLASHMODE_DOUT is not set # CONFIG_MONITOR_BAUD_9600B is not set # CONFIG_MONITOR_BAUD_57600B is not set @@ -1059,7 +1045,7 @@ CONFIG_MONITOR_BAUD_115200B=y # CONFIG_MONITOR_BAUD_OTHER is not set CONFIG_MONITOR_BAUD_OTHER_VAL=115200 CONFIG_MONITOR_BAUD=115200 -CONFIG_COMPILER_OPTIMIZATION_LEVEL_DEBUG=y +# CONFIG_COMPILER_OPTIMIZATION_LEVEL_DEBUG is not set # CONFIG_COMPILER_OPTIMIZATION_LEVEL_RELEASE is not set CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y # CONFIG_OPTIMIZATION_ASSERTIONS_SILENT is not set @@ -1102,7 +1088,7 @@ CONFIG_CONSOLE_UART=y CONFIG_CONSOLE_UART_NUM=0 CONFIG_CONSOLE_UART_BAUDRATE=115200 CONFIG_INT_WDT=y -CONFIG_INT_WDT_TIMEOUT_MS=300 +CONFIG_INT_WDT_TIMEOUT_MS=1000 CONFIG_INT_WDT_CHECK_CPU1=y CONFIG_TASK_WDT=y # CONFIG_TASK_WDT_PANIC is not set @@ -1112,8 +1098,18 @@ CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU1=y # CONFIG_ESP32_DEBUG_STUBS_ENABLE is not set CONFIG_TIMER_TASK_STACK_SIZE=3584 # CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH is not set -# CONFIG_ESP32_ENABLE_COREDUMP_TO_UART is not set -CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE=y +CONFIG_ESP32_ENABLE_COREDUMP_TO_UART=y +# CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE is not set +# CONFIG_ESP32_COREDUMP_DATA_FORMAT_BIN is not set +CONFIG_ESP32_COREDUMP_DATA_FORMAT_ELF=y +CONFIG_ESP32_COREDUMP_CHECKSUM_CRC32=y +# CONFIG_ESP32_COREDUMP_CHECKSUM_SHA256 is not set +CONFIG_ESP32_ENABLE_COREDUMP=y +CONFIG_ESP32_CORE_DUMP_MAX_TASKS_NUM=64 +CONFIG_ESP32_CORE_DUMP_UART_DELAY=0 +CONFIG_ESP32_CORE_DUMP_DECODE_INFO=y +# CONFIG_ESP32_CORE_DUMP_DECODE_DISABLE is not set +CONFIG_ESP32_CORE_DUMP_DECODE="info" # CONFIG_ENABLE_STATIC_TASK_CLEAN_UP_HOOK is not set CONFIG_TIMER_TASK_PRIORITY=1 CONFIG_TIMER_TASK_STACK_DEPTH=2048 @@ -1156,4 +1152,5 @@ CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ABORTS=y CONFIG_SUPPRESS_SELECT_DEBUG_OUTPUT=y CONFIG_SUPPORT_TERMIOS=y CONFIG_SEMIHOSTFS_MAX_MOUNT_POINTS=1 +CONFIG_SEMIHOSTFS_HOST_PATH_MAX_LEN=128 # End of deprecated options diff --git a/libraries/AP_HAL_ESP32/targets/esp32s3/partitions.csv b/libraries/AP_HAL_ESP32/targets/esp32s3/partitions.csv index 70f23f3af89566..8a292ba165f517 100644 --- a/libraries/AP_HAL_ESP32/targets/esp32s3/partitions.csv +++ b/libraries/AP_HAL_ESP32/targets/esp32s3/partitions.csv @@ -2,4 +2,4 @@ nvs, data, nvs, , 0x6000 phy_init, data, phy, , 0x1000 factory, app, factory, , 3M -storage, 0x45, 0x0, , 128K +storage, 0x45, 0x0, , 256K diff --git a/libraries/AP_HAL_Empty/HAL_Empty_Class.cpp b/libraries/AP_HAL_Empty/HAL_Empty_Class.cpp index 2045e9c2893ee5..9d03bcabf78953 100644 --- a/libraries/AP_HAL_Empty/HAL_Empty_Class.cpp +++ b/libraries/AP_HAL_Empty/HAL_Empty_Class.cpp @@ -9,9 +9,10 @@ using namespace Empty; -static UARTDriver uartADriver; -static UARTDriver uartBDriver; -static UARTDriver uartCDriver; +static UARTDriver serial0Driver; +static UARTDriver serial1Driver; +static UARTDriver serial2Driver; +static UARTDriver serial3Driver; static SPIDeviceManager spiDeviceManager; static AnalogIn analogIn; static Storage storageDriver; @@ -25,20 +26,20 @@ static Flash flashDriver; HAL_Empty::HAL_Empty() : AP_HAL::HAL( - &uartADriver, - &uartBDriver, - &uartCDriver, - nullptr, /* no uartD */ - nullptr, /* no uartE */ - nullptr, /* no uartF */ - nullptr, /* no uartG */ - nullptr, /* no uartH */ - nullptr, /* no uartI */ - nullptr, /* no uartJ */ + &serial0Driver, + &serial1Driver, + &serial2Driver, + &serial3Driver, + nullptr, /* no SERIAL4 */ + nullptr, /* no SERIAL5 */ + nullptr, /* no SERIAL6 */ + nullptr, /* no SERIAL7 */ + nullptr, /* no SERIAL8 */ + nullptr, /* no SERIAL9 */ &spiDeviceManager, &analogIn, &storageDriver, - &uartADriver, + &serial0Driver, &gpioDriver, &rcinDriver, &rcoutDriver, diff --git a/libraries/AP_HAL_Linux/CANSocketIface.cpp b/libraries/AP_HAL_Linux/CANSocketIface.cpp index 03f3be88da8c59..05a02280ce9cec 100644 --- a/libraries/AP_HAL_Linux/CANSocketIface.cpp +++ b/libraries/AP_HAL_Linux/CANSocketIface.cpp @@ -51,8 +51,6 @@ using namespace Linux; #define Debug(fmt, args...) #endif -CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES]; - static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) { can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } }; @@ -193,6 +191,9 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u out_flags = rx.flags; } (void)_rx_queue.pop(); + if (sem_handle != nullptr) { + sem_handle->signal(); + } return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags); } @@ -507,8 +508,9 @@ bool CANIface::select(bool &read_select, bool &write_select, stats.num_tx_poll_req++; } } - if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) { - _evt_handle->wait(blocking_deadline - AP_HAL::micros64()); + const uint64_t now_us = AP_HAL::micros64(); + if (sem_handle != nullptr && blocking_deadline > now_us) { + IGNORE_RETURN(sem_handle->wait(blocking_deadline - now_us)); } } @@ -528,67 +530,13 @@ bool CANIface::select(bool &read_select, bool &write_select, return true; } -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) { - _evt_handle = handle; - evt_can_socket[_self_index]._ifaces[_self_index] = this; - _evt_handle->set_source(&evt_can_socket[_self_index]); - return true; -} - - -bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) { - if (evt_handle == nullptr) { - return false; - } - pollfd pollfds[HAL_NUM_CAN_IFACES] {}; - uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {}; - unsigned long int num_pollfds = 0; - - // Poll FD set setup - for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) { - if (_ifaces[i] == nullptr) { - continue; - } - if (_ifaces[i]->_down) { - continue; - } - pollfds[num_pollfds] = _ifaces[i]->_pollfd; - pollfd_iface_map[num_pollfds] = i; - num_pollfds++; - _ifaces[i]->stats.num_poll_waits++; - } - - if (num_pollfds == 0) { - return true; - } - - // Timeout conversion - auto ts = timespec(); - ts.tv_sec = 0; - ts.tv_nsec = duration_us * 1000UL; - - // Blocking here - const int res = ppoll(pollfds, num_pollfds, &ts, nullptr); - - if (res < 0) { - return false; - } - - // Handling poll output - for (unsigned i = 0; i < num_pollfds; i++) { - if (_ifaces[pollfd_iface_map[i]] == nullptr) { - continue; - } - _ifaces[pollfd_iface_map[i]]->_updateDownStatusFromPollResult(pollfds[i]); - - const bool poll_read = pollfds[i].revents & POLLIN; - const bool poll_write = pollfds[i].revents & POLLOUT; - _ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write); - } + sem_handle = handle; return true; } + void CANIface::get_stats(ExpandingString &str) { str.printf("tx_requests: %u\n" diff --git a/libraries/AP_HAL_Linux/CANSocketIface.h b/libraries/AP_HAL_Linux/CANSocketIface.h index a7e85f45753c6f..1e4d01a7b4666f 100644 --- a/libraries/AP_HAL_Linux/CANSocketIface.h +++ b/libraries/AP_HAL_Linux/CANSocketIface.h @@ -109,22 +109,12 @@ class CANIface: public AP_HAL::CANIface { uint64_t blocking_deadline) override; // setup event handle for waiting on events - bool set_event_handle(AP_HAL::EventHandle* handle) override; + bool set_event_handle(AP_HAL::BinarySemaphore *handle) override; // fetch stats text and return the size of the same, // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt void get_stats(ExpandingString &str) override; - class CANSocketEventSource : public AP_HAL::EventSource { - friend class CANIface; - CANIface *_ifaces[HAL_NUM_CAN_IFACES]; - - public: - // we just poll fd, no signaling is done - void signal(uint32_t evt_mask) override { return; } - bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override; - }; - private: void _pollWrite(); @@ -164,8 +154,7 @@ class CANIface: public AP_HAL::CANIface { const unsigned _max_frames_in_socket_tx_queue; unsigned _frames_in_socket_tx_queue; uint32_t _tx_frame_counter; - AP_HAL::EventHandle *_evt_handle; - static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES]; + AP_HAL::BinarySemaphore *sem_handle; pollfd _pollfd; std::map _errors; diff --git a/libraries/AP_HAL_Linux/ConsoleDevice.h b/libraries/AP_HAL_Linux/ConsoleDevice.h index 7782434a3c37b3..940c6fc2740785 100644 --- a/libraries/AP_HAL_Linux/ConsoleDevice.h +++ b/libraries/AP_HAL_Linux/ConsoleDevice.h @@ -1,7 +1,7 @@ #pragma once #include "SerialDevice.h" -#include +#include class ConsoleDevice: public SerialDevice { public: diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 6618e83bc3240b..4b2cd90e28fa68 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -64,16 +64,17 @@ static UtilRPI utilInstance; static Util utilInstance; #endif -// 9 serial ports on Linux -static UARTDriver uartADriver(true); -static UARTDriver uartCDriver(false); -static UARTDriver uartDDriver(false); -static UARTDriver uartEDriver(false); -static UARTDriver uartFDriver(false); -static UARTDriver uartGDriver(false); -static UARTDriver uartHDriver(false); -static UARTDriver uartIDriver(false); -static UARTDriver uartJDriver(false); +// 10 serial ports on Linux +static UARTDriver serial0Driver(true); +static UARTDriver serial1Driver(false); +static UARTDriver serial2Driver(false); +// serial3Driver declared below depending on board type +static UARTDriver serial4Driver(false); +static UARTDriver serial5Driver(false); +static UARTDriver serial6Driver(false); +static UARTDriver serial7Driver(false); +static UARTDriver serial8Driver(false); +static UARTDriver serial9Driver(false); static I2CDeviceManager i2c_mgr_instance; static SPIDeviceManager spi_mgr_instance; @@ -81,11 +82,24 @@ static SPIDeviceManager spi_mgr_instance; #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH -static SPIUARTDriver uartBDriver; +static SPIUARTDriver serial3Driver; #else -static UARTDriver uartBDriver(false); +static UARTDriver serial3Driver(false); #endif +static UARTDriver* serialDrivers[] = { + &serial0Driver, + &serial1Driver, + &serial2Driver, + &serial3Driver, + &serial4Driver, + &serial5Driver, + &serial6Driver, + &serial7Driver, + &serial8Driver, + &serial9Driver, +}; + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ @@ -237,7 +251,9 @@ static OpticalFlow_Onboard opticalFlow; static Empty::OpticalFlow opticalFlow; #endif +#if HAL_WITH_DSP static Empty::DSP dspDriver; +#endif static Empty::Flash flashDriver; static Empty::WSPIDeviceManager wspi_mgr_instance; @@ -247,22 +263,22 @@ static CANIface* canDrivers[HAL_NUM_CAN_IFACES]; HAL_Linux::HAL_Linux() : AP_HAL::HAL( - &uartADriver, - &uartBDriver, - &uartCDriver, - &uartDDriver, - &uartEDriver, - &uartFDriver, - &uartGDriver, - &uartHDriver, - &uartIDriver, - &uartJDriver, + &serial0Driver, + &serial1Driver, + &serial2Driver, + &serial3Driver, + &serial4Driver, + &serial5Driver, + &serial6Driver, + &serial7Driver, + &serial8Driver, + &serial9Driver, &i2c_mgr_instance, &spi_mgr_instance, &wspi_mgr_instance, &analogIn, &storageDriver, - &uartADriver, + &serial0Driver, &gpioDriver, &rcinDriver, &rcoutDriver, @@ -270,7 +286,9 @@ HAL_Linux::HAL_Linux() : &utilInstance, &opticalFlow, &flashDriver, +#if HAL_WITH_DSP &dspDriver, +#endif #if HAL_NUM_CAN_IFACES (AP_HAL::CANIface**)canDrivers #else @@ -281,16 +299,16 @@ HAL_Linux::HAL_Linux() : void _usage(void) { - printf("Usage: --serial0 serial0Path --serial1 serial2Path \n"); + printf("Usage: --serial0 serial0Path --serial1 serial1Path \n"); printf("Examples:\n"); printf("\tserial (0 through 9 available):\n"); printf("\t --serial0 /dev/ttyO4\n"); printf("\t --serial3 /dev/ttyS1\n"); - printf("\tlegacy UART options still work, their mappings are:\n"); + printf("\tlegacy UART options are deprecated, their mappings are:\n"); printf("\t -A/--uartA is SERIAL0\n"); - printf("\t -B/--uartB is SERIAL3\n"); - printf("\t -C/--uartC is SERIAL1\n"); + printf("\t -C/--uartC is SERIAL1\n"); // ordering captures the historical use of uartB as SERIAL3 printf("\t -D/--uartD is SERIAL2\n"); + printf("\t -B/--uartB is SERIAL3\n"); printf("\t -E/--uartE is SERIAL4\n"); printf("\t -F/--uartF is SERIAL5\n"); printf("\t -G/--uartG is SERIAL6\n"); @@ -330,28 +348,41 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const const char *module_path = AP_MODULE_DEFAULT_DIRECTORY; #endif + enum long_options { + CMDLINE_SERIAL0 = 1, // must be in 0-9 order and numbered consecutively + CMDLINE_SERIAL1, + CMDLINE_SERIAL2, + CMDLINE_SERIAL3, + CMDLINE_SERIAL4, + CMDLINE_SERIAL5, + CMDLINE_SERIAL6, + CMDLINE_SERIAL7, + CMDLINE_SERIAL8, + CMDLINE_SERIAL9, + }; + int opt; const struct GetOptLong::option options[] = { {"uartA", true, 0, 'A'}, - {"serial0", true, 0, 'A'}, {"uartB", true, 0, 'B'}, - {"serial3", true, 0, 'B'}, {"uartC", true, 0, 'C'}, - {"serial1", true, 0, 'C'}, {"uartD", true, 0, 'D'}, - {"serial2", true, 0, 'D'}, {"uartE", true, 0, 'E'}, - {"serial4", true, 0, 'E'}, {"uartF", true, 0, 'F'}, - {"serial5", true, 0, 'F'}, {"uartG", true, 0, 'G'}, - {"serial6", true, 0, 'G'}, {"uartH", true, 0, 'H'}, - {"serial7", true, 0, 'H'}, {"uartI", true, 0, 'I'}, - {"serial8", true, 0, 'I'}, {"uartJ", true, 0, 'J'}, - {"serial9", true, 0, 'J'}, + {"serial0", true, 0, CMDLINE_SERIAL0}, + {"serial1", true, 0, CMDLINE_SERIAL1}, + {"serial2", true, 0, CMDLINE_SERIAL2}, + {"serial3", true, 0, CMDLINE_SERIAL3}, + {"serial4", true, 0, CMDLINE_SERIAL4}, + {"serial5", true, 0, CMDLINE_SERIAL5}, + {"serial6", true, 0, CMDLINE_SERIAL6}, + {"serial7", true, 0, CMDLINE_SERIAL7}, + {"serial8", true, 0, CMDLINE_SERIAL8}, + {"serial9", true, 0, CMDLINE_SERIAL9}, {"log-directory", true, 0, 'l'}, {"terrain-directory", true, 0, 't'}, {"storage-directory", true, 0, 's'}, @@ -371,34 +402,36 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const while ((opt = gopt.getoption()) != -1) { switch (opt) { case 'A': - uartADriver.set_device_path(gopt.optarg); - break; case 'B': - uartBDriver.set_device_path(gopt.optarg); - break; case 'C': - uartCDriver.set_device_path(gopt.optarg); - break; case 'D': - uartDDriver.set_device_path(gopt.optarg); - break; case 'E': - uartEDriver.set_device_path(gopt.optarg); - break; case 'F': - uartFDriver.set_device_path(gopt.optarg); - break; case 'G': - uartGDriver.set_device_path(gopt.optarg); - break; case 'H': - uartHDriver.set_device_path(gopt.optarg); - break; case 'I': - uartIDriver.set_device_path(gopt.optarg); + case 'J': { + int uart_idx = opt - 'A'; + // ordering captures the historical use of uartB as SERIAL3 + static const uint8_t mapping[] = { 0, 3, 1, 2, 4, 5, 6, 7, 8, 9 }; + int serial_idx = mapping[uart_idx]; + printf("WARNING: deprecated option --uart%c/-%c will be removed in " + "a future release. Use --serial%d instead.\n", + (char)opt, (char)opt, serial_idx); + serialDrivers[serial_idx]->set_device_path(gopt.optarg); break; - case 'J': - uartJDriver.set_device_path(gopt.optarg); + } + case CMDLINE_SERIAL0: + case CMDLINE_SERIAL1: + case CMDLINE_SERIAL2: + case CMDLINE_SERIAL3: + case CMDLINE_SERIAL4: + case CMDLINE_SERIAL5: + case CMDLINE_SERIAL6: + case CMDLINE_SERIAL7: + case CMDLINE_SERIAL8: + case CMDLINE_SERIAL9: + serialDrivers[opt - CMDLINE_SERIAL0]->set_device_path(gopt.optarg); break; case 'l': utilInstance.set_custom_log_directory(gopt.optarg); diff --git a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp index 981afc77bbaa8a..9f597ace534133 100644 --- a/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp +++ b/libraries/AP_HAL_Linux/OpticalFlow_Onboard.cpp @@ -220,7 +220,7 @@ void OpticalFlow_Onboard::push_gyro(float gyro_x, float gyro_y, float dt) _integrated_gyro.x += (gyro_x - _gyro_bias.x) * dt; _integrated_gyro.y += (gyro_y - _gyro_bias.y) * dt; sample.gyro = _integrated_gyro; - sample.time_us = 1.0e6 * (ts.tv_sec + (ts.tv_nsec*1.0e-9)); + sample.time_us = ts.tv_sec*1000000ULL + ts.tv_nsec/1000ULL; _gyro_ring_buffer->push(sample); } diff --git a/libraries/AP_HAL_Linux/RCInput_SoloLink.h b/libraries/AP_HAL_Linux/RCInput_SoloLink.h index 29538a30abc603..e9638df79fa11a 100644 --- a/libraries/AP_HAL_Linux/RCInput_SoloLink.h +++ b/libraries/AP_HAL_Linux/RCInput_SoloLink.h @@ -18,11 +18,15 @@ #include -#include +#include #include #include "RCInput.h" +#ifndef AP_SOCKET_NATIVE_ENABLED +#error "need native" +#endif + namespace Linux { class RCInput_SoloLink : public RCInput @@ -48,7 +52,7 @@ class RCInput_SoloLink : public RCInput bool _check_hdr(ssize_t len); - SocketAPM _socket{true}; + SocketAPM_native _socket{true}; uint64_t _last_usec = 0; uint16_t _last_seq = 0; union packet _packet; diff --git a/libraries/AP_HAL_Linux/RCInput_UDP.h b/libraries/AP_HAL_Linux/RCInput_UDP.h index 2154a26a65af92..9f87af9b5ce76f 100644 --- a/libraries/AP_HAL_Linux/RCInput_UDP.h +++ b/libraries/AP_HAL_Linux/RCInput_UDP.h @@ -1,7 +1,7 @@ #pragma once #include "RCInput.h" -#include +#include #include "RCInput_UDP_Protocol.h" #define RCINPUT_UDP_DEF_PORT 777 @@ -15,7 +15,7 @@ class RCInput_UDP : public RCInput void init() override; void _timer_tick(void) override; private: - SocketAPM _socket{true}; + SocketAPM_native _socket{true}; uint16_t _port; struct rc_udp_packet_v3 _buf; uint64_t _last_buf_ts; diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index d196ef9796b1ea..7c2cce5d2bfb3c 100644 --- a/libraries/AP_HAL_Linux/Scheduler.cpp +++ b/libraries/AP_HAL_Linux/Scheduler.cpp @@ -26,6 +26,7 @@ extern const AP_HAL::HAL& hal; #define APM_LINUX_MAX_PRIORITY 20 #define APM_LINUX_TIMER_PRIORITY 15 #define APM_LINUX_UART_PRIORITY 14 +#define APM_LINUX_NET_PRIORITY 14 #define APM_LINUX_RCIN_PRIORITY 13 #define APM_LINUX_MAIN_PRIORITY 12 #define APM_LINUX_IO_PRIORITY 10 @@ -385,6 +386,7 @@ uint8_t Scheduler::calculate_thread_priority(priority_base base, int8_t priority { PRIORITY_UART, APM_LINUX_UART_PRIORITY}, { PRIORITY_STORAGE, APM_LINUX_IO_PRIORITY}, { PRIORITY_SCRIPTING, APM_LINUX_SCRIPTING_PRIORITY}, + { PRIORITY_NET, APM_LINUX_NET_PRIORITY}, }; for (uint8_t i=0; i= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) { + return false; + } + } + pending = false; + return true; +} + +bool BinarySemaphore::wait_blocking(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + if (pthread_cond_wait(&cond, &mtx._lock) != 0) { + return false; + } + } + pending = false; + return true; +} + +void BinarySemaphore::signal(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + pending = true; + pthread_cond_signal(&cond); + } +} diff --git a/libraries/AP_HAL_Linux/Semaphores.h b/libraries/AP_HAL_Linux/Semaphores.h index 90655871ec1aa5..e10af8116927b0 100644 --- a/libraries/AP_HAL_Linux/Semaphores.h +++ b/libraries/AP_HAL_Linux/Semaphores.h @@ -10,6 +10,7 @@ namespace Linux { class Semaphore : public AP_HAL::Semaphore { public: + friend class BinarySemaphore; Semaphore(); bool give() override; bool take(uint32_t timeout_ms) override; @@ -18,4 +19,19 @@ class Semaphore : public AP_HAL::Semaphore { pthread_mutex_t _lock; }; + +class BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + +protected: + Semaphore mtx; + pthread_cond_t cond; + bool pending; +}; + } diff --git a/libraries/AP_HAL_Linux/TCPServerDevice.h b/libraries/AP_HAL_Linux/TCPServerDevice.h index 757e336df2e4a3..2dd5d401a7dfc9 100644 --- a/libraries/AP_HAL_Linux/TCPServerDevice.h +++ b/libraries/AP_HAL_Linux/TCPServerDevice.h @@ -1,7 +1,11 @@ #pragma once +#include #include "SerialDevice.h" -#include + +#ifndef AP_SOCKET_NATIVE_ENABLED +#error "need native" +#endif class TCPServerDevice: public SerialDevice { public: @@ -16,8 +20,8 @@ class TCPServerDevice: public SerialDevice { virtual ssize_t read(uint8_t *buf, uint16_t n) override; private: - SocketAPM listener{false}; - SocketAPM *sock = nullptr; + SocketAPM_native listener{false}; + SocketAPM_native *sock = nullptr; const char *_ip; uint16_t _port; bool _wait; diff --git a/libraries/AP_HAL_Linux/UARTDevice.h b/libraries/AP_HAL_Linux/UARTDevice.h index b6dcae6ed6cd39..582089d47028cc 100644 --- a/libraries/AP_HAL_Linux/UARTDevice.h +++ b/libraries/AP_HAL_Linux/UARTDevice.h @@ -1,7 +1,7 @@ #pragma once #include "SerialDevice.h" -#include +#include class UARTDevice: public SerialDevice { public: diff --git a/libraries/AP_HAL_Linux/UDPDevice.h b/libraries/AP_HAL_Linux/UDPDevice.h index a51e2ddc5b5b1b..9f8e31336f3fee 100644 --- a/libraries/AP_HAL_Linux/UDPDevice.h +++ b/libraries/AP_HAL_Linux/UDPDevice.h @@ -1,7 +1,7 @@ #pragma once +#include #include "SerialDevice.h" -#include class UDPDevice: public SerialDevice { public: @@ -15,7 +15,7 @@ class UDPDevice: public SerialDevice { virtual ssize_t write(const uint8_t *buf, uint16_t n) override; virtual ssize_t read(uint8_t *buf, uint16_t n) override; private: - SocketAPM socket{true}; + SocketAPM_native socket{true}; const char *_ip; uint16_t _port; bool _bcast; diff --git a/libraries/AP_HAL_Linux/system.cpp b/libraries/AP_HAL_Linux/system.cpp index 0e0bb8e4f73ab2..2bc1641ecf66e6 100644 --- a/libraries/AP_HAL_Linux/system.cpp +++ b/libraries/AP_HAL_Linux/system.cpp @@ -7,18 +7,26 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; namespace AP_HAL { static struct { - struct timespec start_time; + uint64_t start_time_ns; } state; +static uint64_t ts_to_nsec(struct timespec &ts) +{ + return ts.tv_sec*1000000000ULL + ts.tv_nsec; +} + void init() { - clock_gettime(CLOCK_MONOTONIC, &state.start_time); + struct timespec ts {}; + clock_gettime(CLOCK_MONOTONIC, &ts); + state.start_time_ns = ts_to_nsec(ts); } void WEAK panic(const char *errormsg, ...) @@ -59,24 +67,12 @@ uint64_t micros64() struct timespec ts; clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - - (state.start_time.tv_sec + - (state.start_time.tv_nsec*1.0e-9))); + return uint64_div1000(ts_to_nsec(ts) - state.start_time_ns); } uint64_t millis64() { - const Linux::Scheduler* scheduler = Linux::Scheduler::from(hal.scheduler); - uint64_t stopped_usec = scheduler->stopped_clock_usec(); - if (stopped_usec) { - return stopped_usec / 1000; - } - - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - return 1.0e3*((ts.tv_sec + (ts.tv_nsec*1.0e-9)) - - (state.start_time.tv_sec + - (state.start_time.tv_nsec*1.0e-9))); + return uint64_div1000(micros64()); } } // namespace AP_HAL diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h index 5a360cbf591140..f5924ebd8de17f 100644 --- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h +++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h @@ -15,6 +15,7 @@ class ADCSource; class RCInput; class Util; class Semaphore; +class BinarySemaphore; class GPIO; class DigitalSource; class DSP; diff --git a/libraries/AP_HAL_SITL/CANSocketIface.cpp b/libraries/AP_HAL_SITL/CANSocketIface.cpp index abe9330acf1959..af8fc068759256 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.cpp +++ b/libraries/AP_HAL_SITL/CANSocketIface.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "Scheduler.h" #include #include @@ -51,8 +52,6 @@ using namespace HALSITL; #define Debug(fmt, args...) #endif -CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES]; - uint8_t CANIface::_num_interfaces; bool CANIface::is_initialized() const @@ -232,6 +231,9 @@ bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) transport = nullptr; return false; } + if (sem_handle != nullptr) { + transport->set_event_handle(sem_handle); + } return true; } @@ -255,8 +257,9 @@ bool CANIface::select(bool &read_select, bool &write_select, _pollfd.fd = transport->get_read_fd(); _pollfd.events |= POLLIN; } - if (_evt_handle != nullptr && blocking_deadline > AP_HAL::micros64()) { - _evt_handle->wait(blocking_deadline - AP_HAL::micros64()); + const uint64_t now_us = AP_HAL::micros64(); + if (sem_handle != nullptr && blocking_deadline > now_us) { + IGNORE_RETURN(sem_handle->wait(blocking_deadline - now_us)); } // Writing the output masks @@ -266,70 +269,16 @@ bool CANIface::select(bool &read_select, bool &write_select, return true; } -bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) -{ - _evt_handle = handle; - evt_can_socket[_self_index]._ifaces[_self_index] = this; - _evt_handle->set_source(&evt_can_socket[_self_index]); - return true; -} - - -bool CANIface::CANSocketEventSource::wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) { - if (evt_handle == nullptr) { - return false; - } - pollfd pollfds[HAL_NUM_CAN_IFACES] {}; - uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {}; - unsigned long int num_pollfds = 0; - - // Poll FD set setup - for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) { - if (_ifaces[i] == nullptr) { - continue; - } - pollfds[num_pollfds] = _ifaces[i]->_pollfd; - pollfd_iface_map[num_pollfds] = i; - num_pollfds++; - } - - if (num_pollfds == 0) { - return true; - } - - const uint32_t start_us = AP_HAL::micros(); - do { - uint16_t wait_us = MIN(100, duration_us); - - // check FD for input - const int res = poll(pollfds, num_pollfds, wait_us/1000U); - - if (res < 0) { - return false; - } - if (res > 0) { - break; - } - - // ensure simulator runs - hal.scheduler->delay_microseconds(wait_us); - } while (AP_HAL::micros() - start_us < duration_us); - - - // Handling poll output - for (unsigned i = 0; i < num_pollfds; i++) { - if (_ifaces[pollfd_iface_map[i]] == nullptr) { - continue; - } - - const bool poll_read = pollfds[i].revents & POLLIN; - const bool poll_write = pollfds[i].revents & POLLOUT; - _ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write); + sem_handle = handle; + if (transport != nullptr) { + transport->set_event_handle(handle); } return true; } + void CANIface::get_stats(ExpandingString &str) { str.printf("tx_requests: %u\n" diff --git a/libraries/AP_HAL_SITL/CANSocketIface.h b/libraries/AP_HAL_SITL/CANSocketIface.h index c6f012a38d19da..5876217ecc4c4b 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.h +++ b/libraries/AP_HAL_SITL/CANSocketIface.h @@ -88,7 +88,7 @@ class CANIface: public AP_HAL::CANIface { uint64_t blocking_deadline) override; // setup event handle for waiting on events - bool set_event_handle(AP_HAL::EventHandle* handle) override; + bool set_event_handle(AP_HAL::BinarySemaphore *handle) override; // fetch stats text and return the size of the same, // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt @@ -101,16 +101,6 @@ class CANIface: public AP_HAL::CANIface { return &stats; } - class CANSocketEventSource : public AP_HAL::EventSource { - friend class CANIface; - CANIface *_ifaces[HAL_NUM_CAN_IFACES]; - - public: - // we just poll fd, no signaling is done - void signal(uint32_t evt_mask) override { return; } - bool wait(uint16_t duration_us, AP_HAL::EventHandle* evt_handle) override; - }; - private: void _pollWrite(); @@ -134,8 +124,7 @@ class CANIface: public AP_HAL::CANIface { unsigned _frames_in_socket_tx_queue; uint32_t _tx_frame_counter; - AP_HAL::EventHandle *_evt_handle; - static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES]; + AP_HAL::BinarySemaphore *sem_handle; pollfd _pollfd; std::priority_queue _tx_queue; diff --git a/libraries/AP_HAL_SITL/CAN_Multicast.cpp b/libraries/AP_HAL_SITL/CAN_Multicast.cpp index fe843473241a37..8b7b7652167095 100644 --- a/libraries/AP_HAL_SITL/CAN_Multicast.cpp +++ b/libraries/AP_HAL_SITL/CAN_Multicast.cpp @@ -84,6 +84,10 @@ bool CAN_Multicast::receive(AP_HAL::CANFrame &frame) // run constructor to initialise new(&frame) AP_HAL::CANFrame(pkt.message_id, pkt.data, ret-10, (pkt.flags & MCAST_FLAG_CANFD) != 0); + if (sem_handle != nullptr) { + sem_handle->signal(); + } + return true; } diff --git a/libraries/AP_HAL_SITL/CAN_Multicast.h b/libraries/AP_HAL_SITL/CAN_Multicast.h index d3725458e3d4fc..4bff83d4c7a765 100644 --- a/libraries/AP_HAL_SITL/CAN_Multicast.h +++ b/libraries/AP_HAL_SITL/CAN_Multicast.h @@ -9,6 +9,7 @@ class CAN_Multicast : public CAN_Transport { public: + bool init(uint8_t instance) override; bool send(const AP_HAL::CANFrame &frame) override; bool receive(AP_HAL::CANFrame &frame) override; @@ -17,7 +18,7 @@ class CAN_Multicast : public CAN_Transport { } private: - SocketAPM sock{true}; + SocketAPM_native sock{true}; }; #endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_SITL/CAN_SocketCAN.cpp b/libraries/AP_HAL_SITL/CAN_SocketCAN.cpp index 09df128ba374be..44c3447d8c8b9d 100644 --- a/libraries/AP_HAL_SITL/CAN_SocketCAN.cpp +++ b/libraries/AP_HAL_SITL/CAN_SocketCAN.cpp @@ -85,6 +85,10 @@ bool CAN_SocketCAN::receive(AP_HAL::CANFrame &frame) // run constructor to initialise new(&frame) AP_HAL::CANFrame(receive_frame.can_id, receive_frame.data, receive_frame.can_dlc, false); + + if (sem_handle != nullptr) { + sem_handle->signal(); + } return true; } diff --git a/libraries/AP_HAL_SITL/CAN_Transport.h b/libraries/AP_HAL_SITL/CAN_Transport.h index 52307c4c95799f..76346109e30e6b 100644 --- a/libraries/AP_HAL_SITL/CAN_Transport.h +++ b/libraries/AP_HAL_SITL/CAN_Transport.h @@ -16,6 +16,13 @@ class CAN_Transport { virtual bool send(const AP_HAL::CANFrame &frame) = 0; virtual bool receive(AP_HAL::CANFrame &frame) = 0; virtual int get_read_fd(void) const = 0; + + void set_event_handle(AP_HAL::BinarySemaphore *handle) { + sem_handle = handle; + } + +protected: + AP_HAL::BinarySemaphore *sem_handle; }; #endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_SITL/DSP.cpp b/libraries/AP_HAL_SITL/DSP.cpp index c67223bfe7ec4b..2bb4d8131ad34a 100644 --- a/libraries/AP_HAL_SITL/DSP.cpp +++ b/libraries/AP_HAL_SITL/DSP.cpp @@ -17,6 +17,8 @@ #include +#if HAL_WITH_DSP + #include "AP_HAL_SITL.h" #include #include @@ -209,3 +211,5 @@ void DSP::calculate_fft(complexf *samples, uint16_t fftlen) istep <<= 1; } } + +#endif diff --git a/libraries/AP_HAL_SITL/DSP.h b/libraries/AP_HAL_SITL/DSP.h index 9813fe4bfe5fb2..c8acab8ed416d5 100644 --- a/libraries/AP_HAL_SITL/DSP.h +++ b/libraries/AP_HAL_SITL/DSP.h @@ -17,6 +17,9 @@ #pragma once #include + +#if HAL_WITH_DSP + #include "AP_HAL_SITL.h" #include @@ -55,3 +58,5 @@ class HALSITL::DSP : public AP_HAL::DSP { void vector_add_float(const float* vin1, const float* vin2, float* vout, uint16_t len) const override; void calculate_fft(complexf* f, uint16_t length); }; + +#endif diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp index bf8bc073510971..ab4a67dd83fe26 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp @@ -6,6 +6,9 @@ #include #include #include +#include +#include +#include #include "AP_HAL_SITL.h" #include "AP_HAL_SITL_Namespace.h" @@ -46,23 +49,25 @@ static Empty::RCInput sitlRCInput; static RCOutput sitlRCOutput(&sitlState); static GPIO sitlGPIO(&sitlState); static AnalogIn sitlAnalogIn(&sitlState); +#if HAL_WITH_DSP static DSP dspDriver; +#endif // use the Empty HAL for hardware we don't emulate static Empty::OpticalFlow emptyOpticalFlow; static Empty::Flash emptyFlash; -static UARTDriver sitlUart0Driver(0, &sitlState); -static UARTDriver sitlUart1Driver(1, &sitlState); -static UARTDriver sitlUart2Driver(2, &sitlState); -static UARTDriver sitlUart3Driver(3, &sitlState); -static UARTDriver sitlUart4Driver(4, &sitlState); -static UARTDriver sitlUart5Driver(5, &sitlState); -static UARTDriver sitlUart6Driver(6, &sitlState); -static UARTDriver sitlUart7Driver(7, &sitlState); -static UARTDriver sitlUart8Driver(8, &sitlState); -static UARTDriver sitlUart9Driver(9, &sitlState); +static UARTDriver sitlSerial0Driver(0, &sitlState); +static UARTDriver sitlSerial1Driver(1, &sitlState); +static UARTDriver sitlSerial2Driver(2, &sitlState); +static UARTDriver sitlSerial3Driver(3, &sitlState); +static UARTDriver sitlSerial4Driver(4, &sitlState); +static UARTDriver sitlSerial5Driver(5, &sitlState); +static UARTDriver sitlSerial6Driver(6, &sitlState); +static UARTDriver sitlSerial7Driver(7, &sitlState); +static UARTDriver sitlSerial8Driver(8, &sitlState); +static UARTDriver sitlSerial9Driver(9, &sitlState); static I2CDeviceManager i2c_mgr_instance; @@ -81,22 +86,22 @@ static Empty::WSPIDeviceManager wspi_mgr_instance; HAL_SITL::HAL_SITL() : AP_HAL::HAL( - &sitlUart0Driver, /* uartA */ - &sitlUart1Driver, /* uartB */ - &sitlUart2Driver, /* uartC */ - &sitlUart3Driver, /* uartD */ - &sitlUart4Driver, /* uartE */ - &sitlUart5Driver, /* uartF */ - &sitlUart6Driver, /* uartG */ - &sitlUart7Driver, /* uartH */ - &sitlUart8Driver, /* uartI */ - &sitlUart9Driver, /* uartJ */ + &sitlSerial0Driver, + &sitlSerial1Driver, + &sitlSerial2Driver, + &sitlSerial3Driver, + &sitlSerial4Driver, + &sitlSerial5Driver, + &sitlSerial6Driver, + &sitlSerial7Driver, + &sitlSerial8Driver, + &sitlSerial9Driver, &i2c_mgr_instance, &spi_mgr_instance, /* spi */ &wspi_mgr_instance, &sitlAnalogIn, /* analogin */ &sitlStorage, /* storage */ - &sitlUart0Driver, /* console */ + &sitlSerial0Driver, /* console */ &sitlGPIO, /* gpio */ &sitlRCInput, /* rcinput */ &sitlRCOutput, /* rcoutput */ @@ -104,7 +109,9 @@ HAL_SITL::HAL_SITL() : &utilInstance, /* util */ &emptyOpticalFlow, /* onboard optical flow */ &emptyFlash, /* flash driver */ +#if HAL_WITH_DSP &dspDriver, /* dsp driver */ +#endif #if HAL_NUM_CAN_IFACES (AP_HAL::CANIface**)canDrivers #else diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index 27f1b063c86a42..a65e56310e5b8e 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -83,11 +83,9 @@ void SITL_State::init(int argc, char * const argv[]) { case CMDLINE_SERIAL6: case CMDLINE_SERIAL7: case CMDLINE_SERIAL8: - case CMDLINE_SERIAL9: { - static const uint8_t mapping[] = { 0, 2, 3, 1, 4, 5, 6, 7, 8, 9 }; - _uart_path[mapping[opt - CMDLINE_SERIAL0]] = gopt.optarg; + case CMDLINE_SERIAL9: + _serial_path[opt - CMDLINE_SERIAL0] = gopt.optarg; break; - } case CMDLINE_DEFAULTS: defaults_path = strdup(gopt.optarg); break; diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.h b/libraries/AP_HAL_SITL/SITL_Periph_State.h index b7e21d1491cc94..61453d4a0fae54 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.h +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.h @@ -22,7 +22,7 @@ #include #include #include -#include +#include class SimMCast : public SITL::Aircraft { public: @@ -30,8 +30,8 @@ class SimMCast : public SITL::Aircraft { void update(const struct sitl_input &input) override; private: - SocketAPM sock{true}; - SocketAPM servo_sock{true}; + SocketAPM_native sock{true}; + SocketAPM_native servo_sock{true}; // offset between multicast timestamp and local timestamp uint64_t base_time_us; @@ -61,11 +61,11 @@ class HALSITL::SITL_State : public SITL_State_Common { } // paths for UART devices - const char *_uart_path[9] { + const char *_serial_path[9] { "none:0", - "GPS1", "none:1", "sim:adsb", + "GPS1", "udpclient:127.0.0.1:15550", // for CAN UART test "none:5", "none:6", diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 361b837cf1dbef..5bc6b410dd4d29 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -14,11 +14,16 @@ #include #include #include +#include #include +#include +#include +#include +#include #include #include -#include +#include extern const AP_HAL::HAL& hal; @@ -108,28 +113,25 @@ void SITL_State::_sitl_setup() /* setup a SITL FDM listening UDP port */ -void SITL_State::_setup_fdm(void) +bool SITL_State::_setup_fdm(void) { + if (_rc_in_started) { + return true; + } if (!_sitl_rc_in.reuseaddress()) { - fprintf(stderr, "SITL: socket reuseaddress failed on RC in port: %d - %s\n", _rcin_port, strerror(errno)); - fprintf(stderr, "Aborting launch...\n"); - exit(1); + return false; } if (!_sitl_rc_in.bind("0.0.0.0", _rcin_port)) { - fprintf(stderr, "SITL: socket bind failed on RC in port : %d - %s\n", _rcin_port, strerror(errno)); - fprintf(stderr, "Aborting launch...\n"); - exit(1); + return false; } if (!_sitl_rc_in.set_blocking(false)) { - fprintf(stderr, "SITL: socket set_blocking(false) failed on RC in port: %d - %s\n", _rcin_port, strerror(errno)); - fprintf(stderr, "Aborting launch...\n"); - exit(1); + return false; } if (!_sitl_rc_in.set_cloexec()) { - fprintf(stderr, "SITL: socket set_cloexec() failed on RC in port: %d - %s\n", _rcin_port, strerror(errno)); - fprintf(stderr, "Aborting launch...\n"); - exit(1); + return false; } + _rc_in_started = true; + return true; } @@ -244,6 +246,9 @@ bool SITL_State::_read_rc_sitl_input() uint16_t pwm[16]; } pwm_pkt; + if (!_setup_fdm()) { + return false; + } const ssize_t size = _sitl_rc_in.recv(&pwm_pkt, sizeof(pwm_pkt), 0); // if we are simulating no pulses RC failure, do not update pwm_input @@ -327,12 +332,13 @@ void SITL_State::_output_to_flightgear(void) */ void SITL_State::_fdm_input_local(void) { + if (_sitl == nullptr) { + return; + } struct sitl_input input; // check for direct RC input - if (_sitl != nullptr) { - _check_rc_input(); - } + _check_rc_input(); // construct servos structure for FDM _simulator_servos(input); @@ -350,19 +356,17 @@ void SITL_State::_fdm_input_local(void) sitl_model->update_model(input); // get FDM output from the model - if (_sitl) { - sitl_model->fill_fdm(_sitl->state); + sitl_model->fill_fdm(_sitl->state); #if HAL_NUM_CAN_IFACES - if (CANIface::num_interfaces() > 0) { - multicast_state_send(); - } + if (CANIface::num_interfaces() > 0) { + multicast_state_send(); + } #endif - if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) { - for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) { - pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000; - } + if (_sitl->rc_fail == SITL::SIM::SITL_RCFail_None) { + for (uint8_t i=0; i< _sitl->state.rcin_chan_count; i++) { + pwm_input[i] = 1000 + _sitl->state.rcin[i]*1000; } } @@ -373,16 +377,12 @@ void SITL_State::_fdm_input_local(void) sim_update(); - if (_sitl && _use_fg_view) { + if (_use_fg_view) { _output_to_flightgear(); } // update simulation time - if (_sitl) { - hal.scheduler->stop_clock(_sitl->state.timestamp_us); - } else { - hal.scheduler->stop_clock(AP_HAL::micros64()+100); - } + hal.scheduler->stop_clock(_sitl->state.timestamp_us); set_height_agl(); @@ -395,6 +395,9 @@ void SITL_State::_fdm_input_local(void) */ void SITL_State::_simulator_servos(struct sitl_input &input) { + if (_sitl == nullptr) { + return; + } static uint32_t last_update_usec; /* this maps the registers used for PWM outputs. The RC @@ -595,7 +598,7 @@ void SITL_State::set_height_agl(void) AP_Terrain *_terrain = AP_Terrain::get_singleton(); if (_terrain != nullptr && _terrain->height_amsl(location, terrain_height_amsl, false)) { - _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl; + _sitl->state.height_agl = _sitl->state.altitude - terrain_height_amsl; return; } } @@ -603,7 +606,7 @@ void SITL_State::set_height_agl(void) if (_sitl != nullptr) { // fall back to flat earth model - _sitl->height_agl = _sitl->state.altitude - home_alt; + _sitl->state.height_agl = _sitl->state.altitude - home_alt; } } @@ -660,7 +663,7 @@ void SITL_State::multicast_state_open(void) } sockaddr.sin_addr.s_addr = htonl(INADDR_ANY); - sockaddr.sin_port = htons(SITL_SERVO_PORT); + sockaddr.sin_port = htons(SITL_SERVO_PORT + _instance); ret = bind(servo_in_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); if (ret == -1) { diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 8ca5afd4526af9..4aa73d4a11fd6e 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -29,11 +29,11 @@ class HALSITL::SITL_State : public SITL_State_Common { } // paths for UART devices - const char *_uart_path[9] { + const char *_serial_path[9] { "tcp:0:wait", - "GPS1", "tcp:2", "tcp:3", + "GPS1", "GPS2", "tcp:5", "tcp:6", @@ -59,7 +59,7 @@ class HALSITL::SITL_State : public SITL_State_Common { void _set_param_default(const char *parm); void _usage(void); void _sitl_setup(); - void _setup_fdm(void); + bool _setup_fdm(void); void _setup_timer(void); void _setup_adc(void); @@ -85,7 +85,8 @@ class HALSITL::SITL_State : public SITL_State_Common { Scheduler *_scheduler; - SocketAPM _sitl_rc_in{true}; + SocketAPM_native _sitl_rc_in{true}; + bool _rc_in_started; uint16_t _rcin_port; uint16_t _fg_view_port; uint16_t _irlock_port; diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 718d80537e182e..b4f96e4cf44dd4 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include extern const AP_HAL::HAL& hal; @@ -222,6 +222,11 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const sitl_model->set_adsb(adsb); return sagetech_mxs; #endif +#if AP_SIM_LOWEHEISER_ENABLED + } else if (streq(name, "loweheiser")) { + sitl_model->set_loweheiser(&_sitl->loweheiser_sim); + return &_sitl->loweheiser_sim; +#endif #if !defined(HAL_BUILD_AP_PERIPH) } else if (streq(name, "richenpower")) { sitl_model->set_richenpower(&_sitl->richenpower_sim); @@ -276,6 +281,14 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const } microstrain7 = new SITL::MicroStrain7(); return microstrain7; + + } else if (streq(name, "ILabs")) { + if (inertiallabs != nullptr) { + AP_HAL::panic("Only one InertialLabs INS at a time"); + } + inertiallabs = new SITL::InertialLabs(); + return inertiallabs; + #if HAL_SIM_AIS_ENABLED } else if (streq(name, "AIS")) { if (ais != nullptr) { @@ -445,6 +458,9 @@ void SITL_State_Common::sim_update(void) if (microstrain7 != nullptr) { microstrain7->update(); } + if (inertiallabs != nullptr) { + inertiallabs->update(); + } #if HAL_SIM_AIS_ENABLED if (ais != nullptr) { diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index 3a2d722fc5ebde..82c281e305e57c 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.h +++ b/libraries/AP_HAL_SITL/SITL_State_common.h @@ -8,6 +8,7 @@ #define SITL_MCAST_PORT 20721 #define SITL_SERVO_PORT 20722 +#include #include #include #include @@ -34,6 +35,7 @@ #include #include #include +#include #include #include @@ -47,6 +49,7 @@ #include #include +#include #include #include "AP_HAL_SITL.h" @@ -55,10 +58,6 @@ #include "RCInput.h" #include -#include -#include -#include -#include #include #include @@ -67,7 +66,6 @@ #include #include #include -#include class HAL_SITL; @@ -203,6 +201,9 @@ class HALSITL::SITL_State_Common { // simulated MicroStrain system SITL::MicroStrain7 *microstrain7; + // simulated InertialLabs INS + SITL::InertialLabs *inertiallabs; + #if HAL_SIM_JSON_MASTER_ENABLED // Ride along instances via JSON SITL backend SITL::JSON_Master ride_along; @@ -220,7 +221,7 @@ class HALSITL::SITL_State_Common { SITL::EFI_Hirth *efi_hirth; // output socket for flightgear viewing - SocketAPM fg_socket{true}; + SocketAPM_native fg_socket{true}; const char *defaults_path = HAL_PARAM_DEFAULTS_PATH; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index a413c3840c799c..96ec46d879d993 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -88,20 +88,20 @@ void SITL_State::_usage(void) "\t--model|-M MODEL set simulation model\n" "\t--config string set additional simulation config string\n" "\t--fg|-F ADDRESS set Flight Gear view address, defaults to 127.0.0.1\n" - "\t--disable-fgview disable Flight Gear view\n" + "\t--enable-fgview enable Flight Gear view\n" "\t--gimbal enable simulated MAVLink gimbal\n" "\t--autotest-dir DIR set directory for additional files\n" "\t--defaults path set path to defaults file\n" - "\t--uartA device set device string for UARTA\n" - "\t--uartB device set device string for UARTB\n" - "\t--uartC device set device string for UARTC\n" - "\t--uartD device set device string for UARTD\n" - "\t--uartE device set device string for UARTE\n" - "\t--uartF device set device string for UARTF\n" - "\t--uartG device set device string for UARTG\n" - "\t--uartH device set device string for UARTH\n" - "\t--uartI device set device string for UARTI\n" - "\t--uartJ device set device string for UARTJ\n" + "\t--uartA device (deprecated) set device string for SERIAL0\n" + "\t--uartC device (deprecated) set device string for SERIAL1\n" // ordering captures the historical use of uartB as SERIAL3 + "\t--uartD device (deprecated) set device string for SERIAL2\n" + "\t--uartB device (deprecated) set device string for SERIAL3\n" + "\t--uartE device (deprecated) set device string for SERIAL4\n" + "\t--uartF device (deprecated) set device string for SERIAL5\n" + "\t--uartG device (deprecated) set device string for SERIAL6\n" + "\t--uartH device (deprecated) set device string for SERIAL7\n" + "\t--uartI device (deprecated) set device string for SERIAL8\n" + "\t--uartJ device (deprecated) set device string for SERIAL9\n" "\t--serial0 device set device string for SERIAL0\n" "\t--serial1 device set device string for SERIAL1\n" "\t--serial2 device set device string for SERIAL2\n" @@ -213,7 +213,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) const char *home_str = nullptr; const char *model_str = nullptr; const char *vehicle_str = SKETCH; - _use_fg_view = true; + _use_fg_view = false; char *autotest_dir = nullptr; _fg_address = "127.0.0.1"; const char* config = ""; @@ -246,7 +246,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) CMDLINE_FGVIEW, CMDLINE_AUTOTESTDIR, CMDLINE_DEFAULTS, - CMDLINE_UARTA, + CMDLINE_UARTA, // must be in A-J order and numbered consecutively CMDLINE_UARTB, CMDLINE_UARTC, CMDLINE_UARTD, @@ -256,7 +256,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) CMDLINE_UARTH, CMDLINE_UARTI, CMDLINE_UARTJ, - CMDLINE_SERIAL0, + CMDLINE_SERIAL0, // must be in 0-9 order and numbered consecutively CMDLINE_SERIAL1, CMDLINE_SERIAL2, CMDLINE_SERIAL3, @@ -302,7 +302,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) {"config", true, 0, 'c'}, {"fg", true, 0, 'F'}, {"gimbal", false, 0, CMDLINE_GIMBAL}, - {"disable-fgview", false, 0, CMDLINE_FGVIEW}, + {"enable-fgview", false, 0, CMDLINE_FGVIEW}, {"autotest-dir", true, 0, CMDLINE_AUTOTESTDIR}, {"defaults", true, 0, CMDLINE_DEFAULTS}, {"uartA", true, 0, CMDLINE_UARTA}, @@ -441,7 +441,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) enable_gimbal = true; break; case CMDLINE_FGVIEW: - _use_fg_view = false; + _use_fg_view = true; break; case CMDLINE_AUTOTESTDIR: autotest_dir = strdup(gopt.optarg); @@ -458,9 +458,18 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) case CMDLINE_UARTG: case CMDLINE_UARTH: case CMDLINE_UARTI: - case CMDLINE_UARTJ: - _uart_path[opt - CMDLINE_UARTA] = gopt.optarg; + case CMDLINE_UARTJ: { + int uart_idx = opt - CMDLINE_UARTA; + // ordering captures the historical use of uartB as SERIAL3 + static const uint8_t mapping[] = { 0, 3, 1, 2, 4, 5, 6, 7, 8, 9 }; + int serial_idx = mapping[uart_idx]; + char uart_letter = (char)(uart_idx)+'A'; + printf("WARNING: deprecated option --uart%c will be removed in a " + "future release. Use --serial%d instead.\n", + uart_letter, serial_idx); + _serial_path[serial_idx] = gopt.optarg; break; + } case CMDLINE_SERIAL0: case CMDLINE_SERIAL1: case CMDLINE_SERIAL2: @@ -470,11 +479,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) case CMDLINE_SERIAL6: case CMDLINE_SERIAL7: case CMDLINE_SERIAL8: - case CMDLINE_SERIAL9: { - static const uint8_t mapping[] = { 0, 2, 3, 1, 4, 5, 6, 7, 8, 9 }; - _uart_path[mapping[opt - CMDLINE_SERIAL0]] = gopt.optarg; + case CMDLINE_SERIAL9: + _serial_path[opt - CMDLINE_SERIAL0] = gopt.optarg; break; - } case CMDLINE_RTSCTS: _use_rtscts = true; break; diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 2531722d710142..c925a0ca5b4189 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -14,6 +14,7 @@ #endif #include #ifdef UBSAN_ENABLED +#include #include #endif @@ -61,6 +62,7 @@ void __ubsan_get_current_report_data(const char **OutIssueKind, const char **OutFilename, unsigned *OutLine, unsigned *OutCol, char **OutMemoryAddr); +void __ubsan_on_report(); void __ubsan_on_report() { static int fd = -1; @@ -377,6 +379,11 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_ if (pthread_create(&thread, &a->attr, thread_create_trampoline, a) != 0) { goto failed; } + +#if !defined(__APPLE__) + pthread_setname_np(thread, name); +#endif + a->next = threads; threads = a; return true; diff --git a/libraries/AP_HAL_SITL/Semaphores.cpp b/libraries/AP_HAL_SITL/Semaphores.cpp index 216c1d98ee39df..0067fd9c148f18 100644 --- a/libraries/AP_HAL_SITL/Semaphores.cpp +++ b/libraries/AP_HAL_SITL/Semaphores.cpp @@ -76,4 +76,77 @@ bool Semaphore::take_nonblocking() return false; } + +/* + binary semaphore using pthread condition variables + */ + +BinarySemaphore::BinarySemaphore(bool initial_state) : + AP_HAL::BinarySemaphore(initial_state) +{ + pthread_cond_init(&cond, NULL); + pending = initial_state; +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + if (hal.scheduler->in_main_thread() || + Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) { + /* + when in the main thread we need to do a busy wait to ensure + the clock advances + */ + uint64_t end_us = AP_HAL::micros64() + timeout_us; + struct timespec ts {}; + do { + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) == 0) { + pending = false; + return true; + } + hal.scheduler->delay_microseconds(10); + } while (AP_HAL::micros64() < end_us); + return false; + } + + struct timespec ts; + if (clock_gettime(CLOCK_REALTIME, &ts) != 0) { + return false; + } + ts.tv_sec += timeout_us/1000000UL; + ts.tv_nsec += (timeout_us % 1000000U) * 1000UL; + if (ts.tv_nsec >= 1000000000L) { + ts.tv_sec++; + ts.tv_nsec -= 1000000000L; + } + if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) { + return false; + } + } + pending = false; + return true; +} + +bool BinarySemaphore::wait_blocking(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + if (pthread_cond_wait(&cond, &mtx._lock) != 0) { + return false; + } + } + pending = false; + return true; +} + +void BinarySemaphore::signal(void) +{ + WITH_SEMAPHORE(mtx); + if (!pending) { + pending = true; + pthread_cond_signal(&cond); + } +} + #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_SITL/Semaphores.h b/libraries/AP_HAL_SITL/Semaphores.h index 63aa57f2f5b6f3..c88d76854f3d8d 100644 --- a/libraries/AP_HAL_SITL/Semaphores.h +++ b/libraries/AP_HAL_SITL/Semaphores.h @@ -9,6 +9,7 @@ class HALSITL::Semaphore : public AP_HAL::Semaphore { public: + friend class HALSITL::BinarySemaphore; Semaphore(); bool give() override; bool take(uint32_t timeout_ms) override; @@ -24,3 +25,20 @@ class HALSITL::Semaphore : public AP_HAL::Semaphore { // semaphore once we're done with it uint8_t take_count; }; + + +class HALSITL::BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + + CLASS_NO_COPY(BinarySemaphore); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + +private: + HALSITL::Semaphore mtx; + pthread_cond_t cond; + bool pending; +}; diff --git a/libraries/AP_HAL_SITL/UARTDriver.cpp b/libraries/AP_HAL_SITL/UARTDriver.cpp index 9c90928630dea3..9b7516af6938bb 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.cpp +++ b/libraries/AP_HAL_SITL/UARTDriver.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include "UARTDriver.h" #include "SITL_State.h" @@ -56,11 +57,17 @@ bool UARTDriver::_console; void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) { - if (_portNumber >= ARRAY_SIZE(_sitlState->_uart_path)) { - AP_HAL::panic("port number out of range; you may need to extend _sitlState->_uart_path"); + if (baud == 0 && rxSpace == 0 && txSpace == 0) { + // this is a claim of the uart for the current thread, which + // is currently not implemented in SITL + return; + } + + if (_portNumber >= ARRAY_SIZE(_sitlState->_serial_path)) { + AP_HAL::panic("port number out of range; you may need to extend _sitlState->_serial_path"); } - const char *path = _sitlState->_uart_path[_portNumber]; + const char *path = _sitlState->_serial_path[_portNumber]; if (baud != 0) { _uart_baudrate = baud; @@ -95,11 +102,11 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) char *args1 = strtok_r(nullptr, ":", &saveptr); char *args2 = strtok_r(nullptr, ":", &saveptr); #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) - if (_portNumber == 2 && AP::sitl()->adsb_plane_count >= 0) { + if (_portNumber == 1 && AP::sitl()->adsb_plane_count >= 0) { // this is ordinarily port 5762. The ADSB simulation assumed // this port, so if enabled we assume we'll be doing ADSB... // add sanity check here that we're doing mavlink on this port? - ::printf("SIM-ADSB connection on port %u\n", _portNumber); + ::printf("SIM-ADSB connection on SERIAL%u\n", _portNumber); _connected = true; _sim_serial_device = _sitlState->create_serial_sim("adsb", nullptr); } else @@ -122,7 +129,7 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) _uart_start_connection(); } else if (strcmp(devtype, "sim") == 0) { if (!_connected) { - ::printf("SIM connection %s:%s on port %u\n", args1, args2, _portNumber); + ::printf("SIM connection %s:%s on SERIAL%u\n", args1, args2, _portNumber); _connected = true; _sim_serial_device = _sitlState->create_serial_sim(args1, args2); } @@ -155,6 +162,16 @@ void UARTDriver::_begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) AP_HAL::panic("Failed to open (%s): %m", args1); } _connected = true; + } else if (strcmp(devtype, "outfile") == 0) { + if (_connected) { + AP::FS().close(_fd); + } + ::printf("FILE output connection %s\n", args1); + _fd = AP::FS().open(args1, O_WRONLY|O_CREAT|O_TRUNC, 0644); + if (_fd == -1) { + AP_HAL::panic("Failed to open (%s): %m", args1); + } + _connected = true; } else if (strcmp(devtype, "logic_async_csv") == 0) { if (_connected) { AP::FS().close(_fd); @@ -284,6 +301,7 @@ void UARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection) { int one=1; int ret; + struct sockaddr_in _listen_sockaddr {}; if (_connected) { return; @@ -334,7 +352,7 @@ void UARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection) exit(1); } - fprintf(stderr, "bind port %u for %u\n", + fprintf(stderr, "bind port %u for SERIAL%u\n", (unsigned)ntohs(_listen_sockaddr.sin_port), (unsigned)_portNumber); @@ -352,7 +370,7 @@ void UARTDriver::_tcp_start_connection(uint16_t port, bool wait_for_connection) exit(1); } - fprintf(stderr, "Serial port %u on TCP port %u\n", _portNumber, + fprintf(stderr, "SERIAL%u on TCP port %u\n", _portNumber, (unsigned)ntohs(_listen_sockaddr.sin_port)); fflush(stdout); } @@ -630,7 +648,7 @@ void UARTDriver::_check_connection(void) setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); fcntl(_fd, F_SETFD, FD_CLOEXEC); - fprintf(stdout, "New connection on serial port %u\n", _portNumber); + fprintf(stdout, "New connection on SERIAL%u\n", _portNumber); } } } @@ -781,6 +799,7 @@ uint16_t UARTDriver::read_from_async_csv(uint8_t *buffer, uint16_t space) void UARTDriver::handle_writing_from_writebuffer_to_device() { + WITH_SEMAPHORE(write_mtx); if (!_connected) { _check_reconnect(); return; @@ -917,7 +936,7 @@ void UARTDriver::handle_reading_from_device_to_readbuffer() close(_fd); _fd = -1; _connected = false; - fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); + fprintf(stdout, "Closed connection on SERIAL%u\n", _portNumber); fflush(stdout); #if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD) if (_portNumber == 0) { @@ -990,7 +1009,7 @@ ssize_t UARTDriver::get_system_outqueue_length() const uint32_t UARTDriver::bw_in_bytes_per_second() const { // if connected, assume at least a 10/100Mbps connection - const uint32_t bitrate = (_connected && _tcp_client_addr != nullptr) ? 10E6 : _uart_baudrate; + const uint32_t bitrate = _connected ? 10E6 : _uart_baudrate; return bitrate/10; // convert bits to bytes minus overhead }; #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_HAL_SITL/UARTDriver.h b/libraries/AP_HAL_SITL/UARTDriver.h index d47378a8054dae..70d1fa332f4b27 100644 --- a/libraries/AP_HAL_SITL/UARTDriver.h +++ b/libraries/AP_HAL_SITL/UARTDriver.h @@ -6,7 +6,7 @@ #include #include #include "AP_HAL_SITL_Namespace.h" -#include +#include #include #include @@ -76,7 +76,6 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver { bool _connected = false; // true if a client has connected bool _use_send_recv = false; int _listen_fd; // socket we are listening on - struct sockaddr_in _listen_sockaddr; int _serial_port; static bool _console; ByteBuffer _readbuffer{16384}; @@ -89,9 +88,6 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver { const char *_uart_path; uint32_t _uart_baudrate; - // IPv4 address of target for uartC - const char *_tcp_client_addr; - void _tcp_start_connection(uint16_t port, bool wait_for_connection); void _uart_start_connection(void); void _check_reconnect(); @@ -113,6 +109,8 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver { uint32_t last_read_tick_us; uint32_t last_write_tick_us; + HAL_Semaphore write_mtx; + SITL::SerialDevice *_sim_serial_device; struct { diff --git a/libraries/AP_HAL_SITL/Util.cpp b/libraries/AP_HAL_SITL/Util.cpp index 5404bf35ff6951..958f2809564f0b 100644 --- a/libraries/AP_HAL_SITL/Util.cpp +++ b/libraries/AP_HAL_SITL/Util.cpp @@ -2,6 +2,9 @@ #include #include #include "RCOutput.h" +#include +#include +#include extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_HAL_SITL/system.cpp b/libraries/AP_HAL_SITL/system.cpp index 138ee2a7c2bef6..81c5b4941b0c07 100644 --- a/libraries/AP_HAL_SITL/system.cpp +++ b/libraries/AP_HAL_SITL/system.cpp @@ -9,6 +9,7 @@ #include #include "Scheduler.h" +#include extern const AP_HAL::HAL& hal; @@ -17,12 +18,19 @@ using HALSITL::Scheduler; namespace AP_HAL { static struct { - struct timeval start_time; + uint64_t start_time_ns; } state; +static uint64_t ts_to_nsec(struct timespec &ts) +{ + return ts.tv_sec*1000000000ULL + ts.tv_nsec; +} + void init() { - gettimeofday(&state.start_time, nullptr); + struct timespec ts {}; + clock_gettime(CLOCK_MONOTONIC, &ts); + state.start_time_ns = ts_to_nsec(ts); } #if defined(__CYGWIN__) || defined(__CYGWIN64__) || defined(CYGWIN_BUILD) @@ -169,28 +177,14 @@ uint64_t micros64() return stopped_usec; } - struct timeval tp; - gettimeofday(&tp, nullptr); - uint64_t ret = 1.0e6 * ((tp.tv_sec + (tp.tv_usec * 1.0e-6)) - - (state.start_time.tv_sec + - (state.start_time.tv_usec * 1.0e-6))); - return ret; + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return uint64_div1000(ts_to_nsec(ts) - state.start_time_ns); } uint64_t millis64() { - const HALSITL::Scheduler* scheduler = HALSITL::Scheduler::from(hal.scheduler); - uint64_t stopped_usec = scheduler->stopped_clock_usec(); - if (stopped_usec) { - return stopped_usec / 1000; - } - - struct timeval tp; - gettimeofday(&tp, nullptr); - uint64_t ret = 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - - (state.start_time.tv_sec + - (state.start_time.tv_usec*1.0e-6))); - return ret; + return uint64_div1000(micros64()); } } // namespace AP_HAL diff --git a/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp b/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp index 98bff2d86ecbaa..14a2550343614b 100644 --- a/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp +++ b/libraries/AP_Hott_Telem/AP_Hott_Telem.cpp @@ -109,6 +109,7 @@ void AP_Hott_Telem::send_EAM(void) uint8_t stop_byte = 0x7D; //#44 stop } msg {}; +#if AP_BATTERY_ENABLED const AP_BattMonitor &battery = AP::battery(); if (battery.num_instances() > 0) { msg.batt1_voltage = uint16_t(battery.voltage(0) * 10); @@ -125,6 +126,7 @@ void AP_Hott_Telem::send_EAM(void) if (battery.consumed_mah(used_mah)) { msg.batt_used = used_mah * 0.1; } +#endif // AP_BATTERY_ENABLED const AP_Baro &baro = AP::baro(); msg.temp1 = uint8_t(baro.get_temperature(0) + 20.5); diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index d7afd09e92fd8f..b5202ea7220cb5 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -165,14 +165,8 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = { AP_GROUPINFO("REDLINE_RPM", 17, AP_ICEngine, redline_rpm, 0), #endif -#if AP_RELAY_ENABLED - // @Param: IGNITION_RLY - // @DisplayName: Ignition relay channel - // @Description: This is a a relay channel to use for ignition control - // @User: Standard - // @Values: 0:None,1:Relay1,2:Relay2,3:Relay3,4:Relay4,5:Relay5,6:Relay6 - AP_GROUPINFO("IGNITION_RLY", 18, AP_ICEngine, ignition_relay, 0), -#endif + // 18 was IGNITION_RLY + AP_GROUPEND }; @@ -608,15 +602,14 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle) void AP_ICEngine::set_ignition(bool on) { SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, on? pwm_ignition_on : pwm_ignition_off); + #if AP_RELAY_ENABLED - // optionally use a relay as well - if (ignition_relay > 0) { - auto *relay = AP::relay(); - if (relay != nullptr) { - relay->set(ignition_relay-1, on); - } + AP_Relay *relay = AP::relay(); + if (relay != nullptr) { + relay->set(AP_Relay_Params::FUNCTION::IGNITION, on); } #endif // AP_RELAY_ENABLED + } /* @@ -629,6 +622,13 @@ void AP_ICEngine::set_starter(bool on) #if AP_ICENGINE_TCA9554_STARTER_ENABLED tca9554_starter.set_starter(on); #endif + +#if AP_RELAY_ENABLED + AP_Relay *relay = AP::relay(); + if (relay != nullptr) { + relay->set(AP_Relay_Params::FUNCTION::ICE_STARTER, on); + } +#endif // AP_RELAY_ENABLED } @@ -638,6 +638,19 @@ bool AP_ICEngine::allow_throttle_while_disarmed() const hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; } +#if AP_RELAY_ENABLED +bool AP_ICEngine::get_legacy_ignition_relay_index(int8_t &num) +{ + // PARAMETER_CONVERSION - Added: Dec-2023 + if (!enable || !AP_Param::get_param_by_index(this, 18, AP_PARAM_INT8, &num)) { + return false; + } + // convert to zero indexed + num -= 1; + return true; +} +#endif + // singleton instance. Should only ever be set in the constructor. AP_ICEngine *AP_ICEngine::_singleton; namespace AP { diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h index 919e763b28201f..351302e7638f0f 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.h +++ b/libraries/AP_ICEngine/AP_ICEngine.h @@ -66,6 +66,11 @@ class AP_ICEngine { // do we have throttle while disarmed enabled? bool allow_throttle_while_disarmed(void) const; +#if AP_RELAY_ENABLED + // Needed for param conversion from relay numbers to functions + bool get_legacy_ignition_relay_index(int8_t &num); +#endif + static AP_ICEngine *get_singleton() { return _singleton; } private: @@ -136,11 +141,6 @@ class AP_ICEngine { AP_Float idle_slew; #endif -#if AP_RELAY_ENABLED - // relay number for ignition - AP_Int8 ignition_relay; -#endif - // height when we enter ICE_START_HEIGHT_DELAY float initial_height; diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 9045193bb2d3b1..bfefb6c253cb78 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -78,12 +78,16 @@ void AP_IOMCU::init(void) uart.begin(1500*1000, 128, 128); uart.set_unbuffered_writes(true); +#if IOMCU_DEBUG_ENABLE + crc_is_ok = true; +#else AP_BoardConfig *boardconfig = AP_BoardConfig::get_singleton(); if ((!boardconfig || boardconfig->io_enabled() == 1) && !hal.util->was_watchdog_reset()) { check_crc(); } else { crc_is_ok = true; } +#endif if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU", 1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) { @@ -113,14 +117,21 @@ void AP_IOMCU::thread_main(void) uart.begin(1500*1000, 128, 128); uart.set_unbuffered_writes(true); +#if HAL_WITH_IO_MCU_BIDIR_DSHOT + AP_BLHeli* blh = AP_BLHeli::get_singleton(); + uint16_t erpm_period_ms = 10; // default 100Hz + if (blh && blh->get_telemetry_rate() > 0) { + erpm_period_ms = constrain_int16(1000 / blh->get_telemetry_rate(), 1, 1000); + } +#endif trigger_event(IOEVENT_INIT); while (!do_shutdown) { // check if we have lost contact with the IOMCU const uint32_t now_ms = AP_HAL::millis(); - if (last_reg_read_ms != 0 && now_ms - last_reg_read_ms > 1000U) { + if (last_reg_access_ms != 0 && now_ms - last_reg_access_ms > 1000) { INTERNAL_ERROR(AP_InternalError::error_t::iomcu_reset); - last_reg_read_ms = 0; + last_reg_access_ms = 0; } eventmask_t mask = chEvtWaitAnyTimeout(~0, chTimeMS2I(10)); @@ -307,7 +318,23 @@ void AP_IOMCU::thread_main(void) read_servo(); last_servo_read_ms = AP_HAL::millis(); } +#if HAL_WITH_IO_MCU_BIDIR_DSHOT + if (AP_BoardConfig::io_dshot() && now - last_erpm_read_ms > erpm_period_ms) { + // read erpm at configured rate. A more efficient scheme might be to + // send erpm info back with the response from a PWM send, but that would + // require a reworking of the registers model + read_erpm(); + last_erpm_read_ms = AP_HAL::millis(); + } + if (AP_BoardConfig::io_dshot() && now - last_telem_read_ms > 100) { + // read dshot telemetry at 10Hz + // needs to be at least 4Hz since each ESC updates at ~1Hz and we + // are reading 4 at a time + read_telem(); + last_telem_read_ms = AP_HAL::millis(); + } +#endif if (now - last_safety_option_check_ms > 1000) { update_safety_options(); last_safety_option_check_ms = now; @@ -371,6 +398,64 @@ void AP_IOMCU::read_rc_input() } } +#if HAL_WITH_IO_MCU_BIDIR_DSHOT +/* + read dshot erpm + */ +void AP_IOMCU::read_erpm() +{ + uint16_t *r = (uint16_t *)&dshot_erpm; + if (!read_registers(PAGE_RAW_DSHOT_ERPM, 0, sizeof(dshot_erpm)/2, r)) { + return; + } + uint8_t motor_poles = 14; + AP_BLHeli* blh = AP_BLHeli::get_singleton(); + if (blh) { + motor_poles = blh->get_motor_poles(); + } + for (uint8_t i = 0; i < IOMCU_MAX_TELEM_CHANNELS/4; i++) { + for (uint8_t j = 0; j < 4; j++) { + const uint8_t esc_id = (i * 4 + j); + if (dshot_erpm.update_mask & 1U< 4 + case 1: + page = PAGE_RAW_DSHOT_TELEM_5_8; + break; +#endif + default: + break; + } + + if (!read_registers(page, 0, sizeof(page_dshot_telem)/2, r)) { + return; + } + for (uint i = 0; i<4; i++) { + TelemetryData t { + .temperature_cdeg = int16_t(telem->temperature_cdeg[i]), + .voltage = float(telem->voltage_cvolts[i]) * 0.01, + .current = float(telem->current_camps[i]) * 0.01 + }; + update_telem_data(esc_group * 4 + i, t, telem->types[i]); + } + esc_group = (esc_group + 1) % (IOMCU_MAX_TELEM_CHANNELS / 4); +} +#endif + /* read status registers */ @@ -419,6 +504,7 @@ void AP_IOMCU::write_log() uint32_t now = AP_HAL::millis(); if (now - last_log_ms >= 1000U) { last_log_ms = now; +#if HAL_LOGGING_ENABLED if (AP_Logger::get_singleton()) { // @LoggerMessage: IOMC // @Description: IOMCU diagnostic information @@ -440,6 +526,7 @@ void AP_IOMCU::write_log() reg_status.num_errors, num_delayed); } +#endif // HAL_LOGGING_ENABLED #if IOMCU_DEBUG_ENABLE static uint32_t last_io_print; if (now - last_io_print >= 5000) { @@ -463,6 +550,7 @@ void AP_IOMCU::write_log() } } + /* read servo output values */ @@ -597,7 +685,7 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1 total_errors += protocol_fail_count; protocol_fail_count = 0; protocol_count++; - last_reg_read_ms = AP_HAL::millis(); + last_reg_access_ms = AP_HAL::millis(); return true; } @@ -606,7 +694,10 @@ bool AP_IOMCU::read_registers(uint8_t page, uint8_t offset, uint8_t count, uint1 */ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, const uint16_t *regs) { - while (count > PKT_MAX_REGS) { + // The use of offset is very, very evil - it can either be a command within the page + // or a genuine offset, offsets within PAGE_SETUP are assumed to be commands, otherwise to be an + // actual offset + while (page != PAGE_SETUP && count > PKT_MAX_REGS) { if (!write_registers(page, offset, PKT_MAX_REGS, regs)) { return false; } @@ -673,6 +764,9 @@ bool AP_IOMCU::write_registers(uint8_t page, uint8_t offset, uint8_t count, cons total_errors += protocol_fail_count; protocol_fail_count = 0; protocol_count++; + + last_reg_access_ms = AP_HAL::millis(); + return true; } @@ -792,7 +886,7 @@ bool AP_IOMCU::enable_sbus_out(uint16_t rate_hz) bool AP_IOMCU::check_rcinput(uint32_t &last_frame_us, uint8_t &num_channels, uint16_t *channels, uint8_t max_chan) { if (last_frame_us != uint32_t(rc_last_input_ms * 1000U)) { - num_channels = MIN(MIN(rc_input.count, IOMCU_MAX_CHANNELS), max_chan); + num_channels = MIN(MIN(rc_input.count, IOMCU_MAX_RC_CHANNELS), max_chan); memcpy(channels, rc_input.pwm, num_channels*2); last_frame_us = uint32_t(rc_last_input_ms * 1000U); return true; @@ -841,6 +935,13 @@ void AP_IOMCU::set_dshot_period(uint16_t period_us, uint8_t drate) trigger_event(IOEVENT_SET_DSHOT_PERIOD); } +// set the dshot esc_type +void AP_IOMCU::set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType dshot_esc_type) +{ + mode_out.esc_type = uint16_t(dshot_esc_type); + trigger_event(IOEVENT_SET_OUTPUT_MODE); +} + // set output mode void AP_IOMCU::set_telem_request_mask(uint32_t mask) { @@ -873,6 +974,13 @@ void AP_IOMCU::set_output_mode(uint16_t mask, uint16_t mode) trigger_event(IOEVENT_SET_OUTPUT_MODE); } +// set output mode +void AP_IOMCU::set_bidir_dshot_mask(uint16_t mask) +{ + mode_out.bdmask = mask; + trigger_event(IOEVENT_SET_OUTPUT_MODE); +} + AP_HAL::RCOutput::output_mode AP_IOMCU::get_output_mode(uint8_t& mask) const { mask = reg_status.rcout_mask; @@ -980,7 +1088,7 @@ bool AP_IOMCU::check_crc(void) write_registers(PAGE_SETUP, PAGE_REG_SETUP_REBOOT_BL, 1, &magic); // avoid internal error on fw upload delay - last_reg_read_ms = 0; + last_reg_access_ms = 0; if (!upload_fw()) { AP_ROMFS::free(fw); diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 794844a950ab9b..253b1b9890b457 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -13,11 +13,16 @@ #include "iofirmware/ioprotocol.h" #include #include +#include typedef uint32_t eventmask_t; typedef struct ch_thread thread_t; -class AP_IOMCU { +class AP_IOMCU +#ifdef HAL_WITH_ESC_TELEM + : public AP_ESC_Telem_Backend +#endif +{ public: AP_IOMCU(AP_HAL::UARTDriver &uart); @@ -102,6 +107,9 @@ class AP_IOMCU { // set output mode void set_output_mode(uint16_t mask, uint16_t mode); + // set bi-directional mask + void set_bidir_dshot_mask(uint16_t mask); + // get output mode AP_HAL::RCOutput::output_mode get_output_mode(uint8_t& mask) const; @@ -118,6 +126,9 @@ class AP_IOMCU { // set telem request mask void set_telem_request_mask(uint32_t mask); + // set the dshot esc_type + void set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType dshot_esc_type); + // send a dshot command void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms, uint16_t repeat_count, bool priority); #endif @@ -191,7 +202,9 @@ class AP_IOMCU { uint32_t last_rc_read_ms; uint32_t last_servo_read_ms; uint32_t last_safety_option_check_ms; - uint32_t last_reg_read_ms; + uint32_t last_reg_access_ms; + uint32_t last_erpm_read_ms; + uint32_t last_telem_read_ms; // last value of safety options uint16_t last_safety_options = 0xFFFF; @@ -204,6 +217,8 @@ class AP_IOMCU { void send_servo_out(void); void read_rc_input(void); + void read_erpm(void); + void read_telem(void); void read_servo(void); void read_status(void); void discard_input(void); @@ -256,15 +271,18 @@ class AP_IOMCU { uint16_t rate; } dshot_rate; +#if HAL_WITH_IO_MCU_BIDIR_DSHOT + // bi-directional dshot erpm values + struct page_dshot_erpm dshot_erpm; + struct page_dshot_telem dshot_telem[IOMCU_MAX_TELEM_CHANNELS/4]; + uint8_t esc_group; +#endif // queue of dshot commands that need sending ObjectBuffer dshot_command_queue{8}; struct page_GPIO GPIO; // output mode values - struct { - uint16_t mask; - uint16_t mode; - } mode_out; + struct page_mode_out mode_out; // IMU heater duty cycle uint8_t heater_duty_cycle; diff --git a/libraries/AP_IOMCU/iofirmware/analog.cpp b/libraries/AP_IOMCU/iofirmware/analog.cpp index e2c6f820810274..1da6d7f4e461fb 100644 --- a/libraries/AP_IOMCU/iofirmware/analog.cpp +++ b/libraries/AP_IOMCU/iofirmware/analog.cpp @@ -13,9 +13,9 @@ along with this program. If not, see . */ /* - analog capture for IOMCU. This uses direct register access to avoid - using up a DMA channel and to minimise latency. We capture a single - sample at a time + analog capture for IOMCU. This uses direct register access to avoid + using up a DMA channel and to minimise latency. We capture a single + sample at a time */ #include "ch.h" @@ -25,64 +25,135 @@ #if HAL_USE_ADC != TRUE #error "HAL_USE_ADC must be set" #endif +// we build this file with optimisation to lower the interrupt +// latency. +#pragma GCC optimize("O2") + +extern "C" { + extern void Vector88(); +} + +#define STM32_ADC1_NUMBER 18 +#define STM32_ADC1_HANDLER Vector88 + +const uint32_t VSERVO_CHANNEL = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4); +const uint32_t VRSSI_CHANNEL = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN5); + +static uint16_t vrssi_val = 0xFFFF; +static uint16_t vservo_val = 0xFFFF; +static bool sample_vrssi_enable = true; +static bool sampling_vservo = true; /* initialise ADC capture */ void adc_init(void) { - adc_lld_init(); rccEnableADC1(true); + ADC1->CR1 = 0; + ADC1->CR2 = ADC_CR2_ADON; + + /* Reset calibration just to be safe.*/ + ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_RSTCAL; + while ((ADC1->CR2 & ADC_CR2_RSTCAL) != 0) + ; + + /* Calibration.*/ + ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_CAL; + while ((ADC1->CR2 & ADC_CR2_CAL) != 0) + ; /* set channels 4 and 5 for 28.5us sample time */ ADC1->SMPR2 = ADC_SMPR2_SMP_AN4(ADC_SAMPLE_28P5) | ADC_SMPR2_SMP_AN5(ADC_SAMPLE_28P5); - /* capture a single sample at a time */ + /* capture one sample at a time */ ADC1->SQR1 = 0; ADC1->SQR2 = 0; + + ADC1->CR1 |= ADC_CR1_EOCIE; + + nvicEnableVector(STM32_ADC1_NUMBER, STM32_ADC_ADC1_IRQ_PRIORITY); } /* - capture one sample on a channel + capture VSERVO in mV */ -static uint16_t adc_sample_channel(uint32_t channel) +void adc_enable_vrssi(void) { - // clear EOC - ADC1->SR = 0; + sample_vrssi_enable = true; +} - /* capture one sample */ - ADC1->SQR3 = channel; - ADC1->CR2 |= ADC_CR2_ADON; +/* + don't capture VRSSI + */ +void adc_disable_vrssi(void) +{ + sample_vrssi_enable = false; +} - /* wait for the conversion to complete */ - uint32_t counter = 16; +/* + capture one sample on a channel + */ +void adc_sample_channels() +{ + chSysLock(); - while (!(ADC1->SR & ADC_SR_EOC)) { - if (--counter == 0) { - // ensure EOC is clear - ADC1->SR = 0; - return 0xffff; - } + if (ADC1->SR & ADC_SR_STRT) { + return; // still waiting for sample } - // return sample (this also clears EOC flag) - return ADC1->DR; + /* capture another sample */ + ADC1->CR2 |= ADC_CR2_ADON; + + chSysUnlock(); } /* capture VSERVO in mV */ -uint16_t adc_sample_vservo(void) +uint16_t adc_vservo(void) { - const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4); - return adc_sample_channel(channel); + return vservo_val; } /* capture VRSSI in mV */ -uint16_t adc_sample_vrssi(void) +uint16_t adc_vrssi(void) { - const uint32_t channel = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN5); - return adc_sample_channel(channel); + return vrssi_val; +} + +static void adc_read_sample() +{ + if (ADC1->SR & ADC_SR_EOC) { + + ADC1->SR &= ~(ADC_SR_EOC | ADC_SR_STRT); + + if (sampling_vservo) { + vservo_val = ADC1->DR; + if (sample_vrssi_enable) { + /* capture another sample */ + ADC1->SQR3 = VRSSI_CHANNEL; + ADC1->CR2 |= ADC_CR2_ADON; + sampling_vservo = false; + } + } else { + vrssi_val = ADC1->DR; + ADC1->SQR3 = VSERVO_CHANNEL; + sampling_vservo = true; + } + } +} + +OSAL_IRQ_HANDLER(STM32_ADC1_HANDLER) { + OSAL_IRQ_PROLOGUE(); + + chSysLockFromISR(); + + adc_read_sample(); + + chSysUnlockFromISR(); + + OSAL_IRQ_EPILOGUE(); } diff --git a/libraries/AP_IOMCU/iofirmware/analog.h b/libraries/AP_IOMCU/iofirmware/analog.h index 4871cdb32f7fc5..0e0622b3860cd7 100644 --- a/libraries/AP_IOMCU/iofirmware/analog.h +++ b/libraries/AP_IOMCU/iofirmware/analog.h @@ -9,9 +9,18 @@ void adc_init(void); /* capture VSERVO */ -uint16_t adc_sample_vservo(void); +uint16_t adc_vservo(void); /* capture VRSSI */ -uint16_t adc_sample_vrssi(void); +uint16_t adc_vrssi(void); + +/* start another update */ +void adc_sample_channels(void); + +/* capture VRSSI */ +void adc_enable_vrssi(void); + +/* don't capture VRSSI */ +void adc_disable_vrssi(void); diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 3050a5b8e07cb4..6e97fa4cf2714f 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -51,6 +51,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); */ #define IOMCU_ENABLE_RESET_TEST 0 +//#define IOMCU_LOOP_TIMING_DEBUG // enable timing GPIO pings #ifdef IOMCU_LOOP_TIMING_DEBUG #undef TOGGLE_PIN_DEBUG @@ -103,9 +104,6 @@ static void dma_rx_end_cb(hal_uart_driver *uart) chSysLockFromISR(); uart->usart->CR3 &= ~USART_CR3_DMAR; - (void)uart->usart->SR; // sequence to clear IDLE status - (void)uart->usart->DR; - (void)uart->usart->DR; dmaStreamDisable(uart->dmarx); iomcu.process_io_packet(); @@ -135,10 +133,15 @@ static void dma_tx_end_cb(hal_uart_driver *uart) (void)uart->usart->DR; (void)uart->usart->DR; +#ifdef HAL_GPIO_LINE_GPIO108 TOGGLE_PIN_DEBUG(108); TOGGLE_PIN_DEBUG(108); - +#endif +#if AP_HAL_SHARED_DMA_ENABLED + chSysLockFromISR(); chEvtSignalI(iomcu.thread_ctx, IOEVENT_TX_END); + chSysUnlockFromISR(); +#endif } /* replacement for ChibiOS uart_lld_serve_interrupt() */ @@ -152,41 +155,44 @@ static void idle_rx_handler(hal_uart_driver *uart) USART_SR_NE | /* noise error - we have lost a byte due to noise */ USART_SR_FE | USART_SR_PE)) { /* framing error - start/stop bit lost or line break */ + + (void)uart->usart->DR; /* SR reset step 2 - clear ORE | FE.*/ + /* send a line break - this will abort transmission/reception on the other end */ chSysLockFromISR(); - uart->usart->SR = ~USART_SR_LBD; uart->usart->CR1 = cr1 | USART_CR1_SBK; + iomcu.reg_status.num_errors++; iomcu.reg_status.err_uart++; + /* disable RX DMA */ uart->usart->CR3 &= ~USART_CR3_DMAR; - (void)uart->usart->SR; // clears ORE | FE - (void)uart->usart->DR; - (void)uart->usart->DR; setup_rx_dma(uart); chSysUnlockFromISR(); - return; } if ((sr & USART_SR_TC) && (cr1 & USART_CR1_TCIE)) { - chSysLockFromISR(); - /* TC interrupt cleared and disabled.*/ uart->usart->SR &= ~USART_SR_TC; uart->usart->CR1 = cr1 & ~USART_CR1_TCIE; - +#ifdef HAL_GPIO_LINE_GPIO105 + TOGGLE_PIN_DEBUG(105); + TOGGLE_PIN_DEBUG(105); +#endif /* End of transmission, a callback is generated.*/ - _uart_tx2_isr_code(uart); - - chSysUnlockFromISR(); + dma_tx_end_cb(uart); } - if (sr & USART_SR_IDLE) { + if ((sr & USART_SR_IDLE) && (cr1 & USART_CR1_IDLEIE)) { + (void)uart->usart->DR; /* SR reset step 2 - clear IDLE.*/ + /* the DMA size is the maximum packet size, but smaller packets are perfectly possible leading to an IDLE ISR. The data still must be processed. */ + + /* End of receive, a callback is generated.*/ dma_rx_end_cb(uart); } } @@ -246,9 +252,9 @@ static UARTConfig uart_cfg = { dma_tx_end_cb, dma_rx_end_cb, nullptr, - nullptr, - idle_rx_handler, - nullptr, + nullptr, // error + idle_rx_handler, // global irq + nullptr, // idle 1500000, //1.5MBit USART_CR1_IDLEIE, 0, @@ -335,8 +341,8 @@ void AP_IOMCU_FW::init() #if CH_DBG_ENABLE_STACK_CHECK == TRUE static void stackCheck(uint16_t& mstack, uint16_t& pstack) { - extern uint32_t __main_stack_base__[]; - extern uint32_t __main_stack_end__[]; + extern stkalign_t __main_stack_base__[]; + extern stkalign_t __main_stack_end__[]; uint32_t stklimit = (uint32_t)__main_stack_end__; uint32_t stkbase = (uint32_t)__main_stack_base__; uint32_t *crawl = (uint32_t *)stkbase; @@ -348,8 +354,8 @@ static void stackCheck(uint16_t& mstack, uint16_t& pstack) { chDbgAssert(free > 0, "mstack exhausted"); mstack = (uint16_t)free; - extern uint32_t __main_thread_stack_base__[]; - extern uint32_t __main_thread_stack_end__[]; + extern stkalign_t __main_thread_stack_base__[]; + extern stkalign_t __main_thread_stack_end__[]; stklimit = (uint32_t)__main_thread_stack_end__; stkbase = (uint32_t)__main_thread_stack_base__; crawl = (uint32_t *)stkbase; @@ -409,15 +415,10 @@ static void stackCheck(uint16_t& mstack, uint16_t& pstack) { */ void AP_IOMCU_FW::update() { -#if CH_CFG_ST_FREQUENCY == 1000000 eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END | IOEVENT_TX_BEGIN, TIME_US2I(1000)); -#else - // we are not running any other threads, so we can use an - // immediate timeout here for lowest latency - eventmask_t mask = chEvtWaitAnyTimeout(IOEVENT_PWM | IOEVENT_TX_END, TIME_IMMEDIATE); -#endif - +#ifdef HAL_GPIO_LINE_GPIO107 TOGGLE_PIN_DEBUG(107); +#endif iomcu.reg_status.total_ticks++; if (mask) { @@ -465,7 +466,7 @@ void AP_IOMCU_FW::update() now - sbus_last_ms >= sbus_interval_ms) { // output a new SBUS frame sbus_last_ms = now; - sbus_out_write(reg_servo.pwm, IOMCU_MAX_CHANNELS); + sbus_out_write(reg_servo.pwm, IOMCU_MAX_RC_CHANNELS); } // handle FMU failsafe if (now - fmu_data_received_time > 200) { @@ -482,12 +483,20 @@ void AP_IOMCU_FW::update() // turn amber off AMBER_SET(0); } + // update status page at 20Hz if (now - last_status_ms > 50) { last_status_ms = now; page_status_update(); } - +#ifdef HAL_WITH_BIDIR_DSHOT + // EDT updates are semt at ~1Hz per ESC, but we want to make sure + // that we don't delay updates unduly so sample at 5Hz + if (now - last_telem_ms > 200) { + last_telem_ms = now; + telem_update(); + } +#endif // run fast loop functions at 1Khz if (now_us - last_fast_loop_us >= 1000) { @@ -495,6 +504,9 @@ void AP_IOMCU_FW::update() heater_update(); rcin_update(); rcin_serial_update(); +#ifdef HAL_WITH_BIDIR_DSHOT + erpm_update(); +#endif } // run remaining functions at 100Hz @@ -521,7 +533,9 @@ void AP_IOMCU_FW::update() tx_dma_handle->unlock(); } #endif +#ifdef HAL_GPIO_LINE_GPIO107 TOGGLE_PIN_DEBUG(107); +#endif } void AP_IOMCU_FW::pwm_out_update() @@ -567,7 +581,7 @@ void AP_IOMCU_FW::rcin_update() const auto &rc = AP::RC(); rc_input.count = hal.rcin->num_channels(); rc_input.flags_rc_ok = true; - hal.rcin->read(rc_input.pwm, IOMCU_MAX_CHANNELS); + hal.rcin->read(rc_input.pwm, IOMCU_MAX_RC_CHANNELS); rc_last_input_ms = last_ms; rc_input.rc_protocol = (uint16_t)rc.protocol_detected(); rc_input.rssi = rc.get_RSSI(); @@ -590,7 +604,7 @@ void AP_IOMCU_FW::rcin_update() if (mixing.enabled && mixing.rc_chan_override > 0 && rc_input.flags_rc_ok && - mixing.rc_chan_override <= IOMCU_MAX_CHANNELS) { + mixing.rc_chan_override <= IOMCU_MAX_RC_CHANNELS) { override_active = (rc_input.pwm[mixing.rc_chan_override-1] >= 1750); } else { override_active = false; @@ -603,10 +617,59 @@ void AP_IOMCU_FW::rcin_update() } } +#ifdef HAL_WITH_BIDIR_DSHOT +void AP_IOMCU_FW::erpm_update() +{ + uint32_t now_us = AP_HAL::micros(); + + if (hal.rcout->new_erpm()) { + dshot_erpm.update_mask |= hal.rcout->read_erpm(dshot_erpm.erpm, IOMCU_MAX_TELEM_CHANNELS); + last_erpm_us = now_us; + } else if (now_us - last_erpm_us > ESC_RPM_DATA_TIMEOUT_US) { + dshot_erpm.update_mask = 0; + } +} + +void AP_IOMCU_FW::telem_update() +{ + uint32_t now_ms = AP_HAL::millis(); + + for (uint8_t i = 0; i < IOMCU_MAX_TELEM_CHANNELS/4; i++) { + for (uint8_t j = 0; j < 4; j++) { + const uint8_t esc_id = (i * 4 + j); + if (esc_id >= IOMCU_MAX_TELEM_CHANNELS) { + continue; + } + dshot_telem[i].error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0)); +#if HAL_WITH_ESC_TELEM + const volatile AP_ESC_Telem_Backend::TelemetryData& telem = esc_telem.get_telem_data(esc_id); + // if data is stale then set to zero to avoid phantom data appearing in mavlink + if (now_ms - telem.last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS) { + dshot_telem[i].voltage_cvolts[j] = 0; + dshot_telem[i].current_camps[j] = 0; + dshot_telem[i].temperature_cdeg[j] = 0; + continue; + } + dshot_telem[i].voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100)); + dshot_telem[i].current_camps[j] = uint16_t(roundf(telem.current * 100)); + dshot_telem[i].temperature_cdeg[j] = telem.temperature_cdeg; + dshot_telem[i].types[j] = telem.types; +#endif + } + } +} +#endif + void AP_IOMCU_FW::process_io_packet() { iomcu.reg_status.total_pkts++; + if (rx_io_packet.code == CODE_NOOP) { + iomcu.reg_status.num_errors++; + iomcu.reg_status.err_bad_opcode++; + return; + } + uint8_t rx_crc = rx_io_packet.crc; uint8_t calc_crc; rx_io_packet.crc = 0; @@ -661,9 +724,18 @@ void AP_IOMCU_FW::process_io_packet() default: { iomcu.reg_status.num_errors++; iomcu.reg_status.err_bad_opcode++; + rx_io_packet.code = CODE_NOOP; + rx_io_packet.count = 0; + return; } break; } + + // prevent a spurious DMA callback from doing anything bad + rx_io_packet.code = CODE_NOOP; + rx_io_packet.count = 0; + + return; } /* @@ -671,13 +743,15 @@ void AP_IOMCU_FW::process_io_packet() */ void AP_IOMCU_FW::page_status_update(void) { + adc_sample_channels(); + if ((reg_setup.features & P_SETUP_FEATURES_SBUS1_OUT) == 0) { // we can only get VRSSI when sbus is disabled - reg_status.vrssi = adc_sample_vrssi(); + reg_status.vrssi = adc_vrssi(); } else { reg_status.vrssi = 0; } - reg_status.vservo = adc_sample_vservo(); + reg_status.vservo = adc_vservo(); } bool AP_IOMCU_FW::handle_code_read() @@ -699,6 +773,19 @@ bool AP_IOMCU_FW::handle_code_read() case PAGE_RAW_RCIN: COPY_PAGE(rc_input); break; +#ifdef HAL_WITH_BIDIR_DSHOT + case PAGE_RAW_DSHOT_ERPM: + COPY_PAGE(dshot_erpm); + break; + case PAGE_RAW_DSHOT_TELEM_1_4: + COPY_PAGE(dshot_telem[0]); + break; +#if IOMCU_MAX_TELEM_CHANNELS > 4 + case PAGE_RAW_DSHOT_TELEM_5_8: + COPY_PAGE(dshot_telem[1]); + break; +#endif +#endif case PAGE_STATUS: COPY_PAGE(reg_status); break; @@ -725,6 +812,16 @@ bool AP_IOMCU_FW::handle_code_read() memcpy(tx_io_packet.regs, values, sizeof(uint16_t)*tx_io_packet.count); tx_io_packet.crc = 0; tx_io_packet.crc = crc_crc8((const uint8_t *)&tx_io_packet, tx_io_packet.get_size()); + +#ifdef HAL_WITH_BIDIR_DSHOT + switch (rx_io_packet.page) { + case PAGE_RAW_DSHOT_ERPM: + memset(&dshot_erpm, 0, sizeof(dshot_erpm)); + break; + default: + break; + } +#endif return true; } @@ -798,8 +895,10 @@ bool AP_IOMCU_FW::handle_code_write() // or disable SBUS out AFIO->MAPR = AFIO_MAPR_SWJ_CFG_NOJNTRST; + adc_disable_vrssi(); palClearLine(HAL_GPIO_PIN_SBUS_OUT_EN); } else { + adc_enable_vrssi(); palSetLine(HAL_GPIO_PIN_SBUS_OUT_EN); } if (reg_setup.features & P_SETUP_FEATURES_HEATER) { @@ -810,6 +909,8 @@ bool AP_IOMCU_FW::handle_code_write() case PAGE_REG_SETUP_OUTPUT_MODE: mode_out.mask = rx_io_packet.regs[0]; mode_out.mode = rx_io_packet.regs[1]; + mode_out.bdmask = rx_io_packet.regs[2]; + mode_out.esc_type = rx_io_packet.regs[3]; break; case PAGE_REG_SETUP_HEATER_DUTY_CYCLE: @@ -860,18 +961,17 @@ bool AP_IOMCU_FW::handle_code_write() // no input when override is active break; } - /* copy channel data */ - uint16_t i = 0, offset = rx_io_packet.offset, num_values = rx_io_packet.count; - if (offset + num_values > sizeof(reg_direct_pwm.pwm)/2) { + if (rx_io_packet.count > sizeof(reg_direct_pwm.pwm)/2) { return false; } - while ((offset < IOMCU_MAX_CHANNELS) && (num_values > 0)) { + /* copy channel data */ + uint16_t i = 0, num_values = rx_io_packet.count; + while ((i < IOMCU_MAX_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ if (rx_io_packet.regs[i] != PWM_IGNORE_THIS_CHANNEL) { - reg_direct_pwm.pwm[offset] = rx_io_packet.regs[i]; + reg_direct_pwm.pwm[i] = rx_io_packet.regs[i]; } - offset++; num_values--; i++; } @@ -880,7 +980,7 @@ bool AP_IOMCU_FW::handle_code_write() break; } - case PAGE_MIXING: { + case PAGE_MIXING: { // multi-packet message uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; if (offset + num_values > sizeof(mixing)/2) { return false; @@ -890,11 +990,10 @@ bool AP_IOMCU_FW::handle_code_write() } case PAGE_FAILSAFE_PWM: { - uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; - if (offset + num_values > sizeof(reg_failsafe_pwm.pwm)/2) { + if (rx_io_packet.count != sizeof(reg_failsafe_pwm.pwm)/2) { return false; } - memcpy((®_failsafe_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2); + memcpy((®_failsafe_pwm.pwm[0]), &rx_io_packet.regs[0], rx_io_packet.count*2); break; } @@ -906,11 +1005,10 @@ bool AP_IOMCU_FW::handle_code_write() break; case PAGE_DSHOT: { - uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; - if (offset + num_values > sizeof(dshot)/2) { + if (rx_io_packet.count != sizeof(dshot)/2) { return false; } - memcpy(((uint16_t *)&dshot)+offset, &rx_io_packet.regs[0], num_values*2); + memcpy(((uint16_t *)&dshot)+rx_io_packet.offset, &rx_io_packet.regs[0], rx_io_packet.count*2); if(dshot.telem_mask) { hal.rcout->set_telem_request_mask(dshot.telem_mask); } @@ -1059,10 +1157,16 @@ void AP_IOMCU_FW::rcout_config_update(void) } } last_channel_mask = reg_setup.channel_mask; + // channel enablement will affect the reported output mode + uint32_t output_mask = 0; + reg_status.rcout_mode = hal.rcout->get_output_mode(output_mask); + reg_status.rcout_mask = uint8_t(0xFF & output_mask); } // see if there is anything to do, we only support setting the mode for a particular channel once - if ((last_output_mode_mask & ~mode_out.mask) == mode_out.mask) { + if ((last_output_mode_mask & mode_out.mask) == mode_out.mask + && (last_output_bdmask & mode_out.bdmask) == mode_out.bdmask + && last_output_esc_type == mode_out.esc_type) { return; } @@ -1071,11 +1175,17 @@ void AP_IOMCU_FW::rcout_config_update(void) case AP_HAL::RCOutput::MODE_PWM_DSHOT300: #if defined(STM32F103xB) || defined(STM32F103x8) case AP_HAL::RCOutput::MODE_PWM_DSHOT600: +#endif +#ifdef HAL_WITH_BIDIR_DSHOT + hal.rcout->set_bidir_dshot_mask(mode_out.bdmask); + hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type)); #endif hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode); // enabling dshot changes the memory allocation reg_status.freemem = hal.util->available_memory(); last_output_mode_mask |= mode_out.mask; + last_output_bdmask |= mode_out.bdmask; + last_output_esc_type = mode_out.esc_type; break; case AP_HAL::RCOutput::MODE_PWM_ONESHOT: case AP_HAL::RCOutput::MODE_PWM_ONESHOT125: diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index 62c8b12565b6e8..824a2238e4220c 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "hal.h" #include "ch.h" @@ -29,6 +30,8 @@ class AP_IOMCU_FW { void pwm_out_update(); void heater_update(); void rcin_update(); + void erpm_update(); + void telem_update(); bool handle_code_write(); bool handle_code_read(); @@ -112,12 +115,11 @@ class AP_IOMCU_FW { } rate; // output mode values - struct { - uint16_t mask; - uint16_t mode; - } mode_out; + struct page_mode_out mode_out; uint16_t last_output_mode_mask; + uint16_t last_output_bdmask; + uint16_t last_output_esc_type; // MIXER values struct page_mixing mixing; @@ -135,6 +137,15 @@ class AP_IOMCU_FW { void tx_dma_deallocate(ChibiOS::Shared_DMA *ctx); ChibiOS::Shared_DMA* tx_dma_handle; +#endif +#ifdef HAL_WITH_BIDIR_DSHOT + struct page_dshot_erpm dshot_erpm; + uint32_t last_erpm_us; + struct page_dshot_telem dshot_telem[IOMCU_MAX_TELEM_CHANNELS/4]; + uint32_t last_telem_ms; +#if HAL_WITH_ESC_TELEM + AP_ESC_Telem esc_telem; +#endif #endif // true when override channel active diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 21b0a4263a07c9..9e13fafc48478f 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -9,7 +9,9 @@ // 22 is enough for the rc_input page in one transfer #define PKT_MAX_REGS 22 -#define IOMCU_MAX_CHANNELS 16 +#define IOMCU_MAX_RC_CHANNELS 16 +#define IOMCU_MAX_CHANNELS 8 +#define IOMCU_MAX_TELEM_CHANNELS 4 //#define IOMCU_DEBUG @@ -35,6 +37,7 @@ enum iocode { // read types CODE_READ = 0, CODE_WRITE = 1, + CODE_NOOP = 2, // reply codes CODE_SUCCESS = 0, @@ -58,6 +61,11 @@ enum iopage { PAGE_MIXING = 200, PAGE_GPIO = 201, PAGE_DSHOT = 202, + PAGE_RAW_DSHOT_ERPM = 203, + PAGE_RAW_DSHOT_TELEM_1_4 = 204, + PAGE_RAW_DSHOT_TELEM_5_8 = 205, + PAGE_RAW_DSHOT_TELEM_9_12 = 206, + PAGE_RAW_DSHOT_TELEM_13_16 = 207, }; // setup page registers @@ -131,6 +139,7 @@ struct page_reg_status { uint8_t err_write; uint8_t err_uart; uint8_t err_lock; + uint8_t spare; }; struct page_rc_input { @@ -138,7 +147,7 @@ struct page_rc_input { uint8_t flags_failsafe:1; uint8_t flags_rc_ok:1; uint8_t rc_protocol; - uint16_t pwm[IOMCU_MAX_CHANNELS]; + uint16_t pwm[IOMCU_MAX_RC_CHANNELS]; int16_t rssi; }; @@ -184,7 +193,14 @@ struct __attribute__((packed, aligned(2))) page_GPIO { uint8_t output_mask; }; -struct __attribute__((packed, aligned(2))) page_dshot { +struct page_mode_out { + uint16_t mask; + uint16_t mode; + uint16_t bdmask; + uint16_t esc_type; +}; + +struct page_dshot { uint16_t telem_mask; uint8_t command; uint8_t chan; @@ -192,3 +208,17 @@ struct __attribute__((packed, aligned(2))) page_dshot { uint8_t repeat_count; uint8_t priority; }; + +struct page_dshot_erpm { + uint16_t erpm[IOMCU_MAX_TELEM_CHANNELS]; + uint32_t update_mask; +}; + +// separate telemetry packet because (a) it's too big otherwise and (b) slower update rate +struct page_dshot_telem { + uint16_t error_rate[4]; // as a centi-percentage + uint16_t voltage_cvolts[4]; + uint16_t current_camps[4]; + uint16_t temperature_cdeg[4]; + uint16_t types[4]; +}; diff --git a/libraries/AP_IOMCU/iofirmware/wscript b/libraries/AP_IOMCU/iofirmware/wscript index 7e051d72cbaefd..445b95c0e1ee0e 100644 --- a/libraries/AP_IOMCU/iofirmware/wscript +++ b/libraries/AP_IOMCU/iofirmware/wscript @@ -12,6 +12,7 @@ def build(bld): 'AP_Math', 'AP_RCProtocol', 'AP_BoardConfig', + 'AP_ESC_Telem', 'AP_SBusOut' ], exclude_src=[ diff --git a/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h b/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h index b20e9f62418495..21ee5ff5653326 100644 --- a/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h +++ b/libraries/AP_IRLock/AP_IRLock_SITL_Gazebo.h @@ -6,7 +6,7 @@ */ #pragma once -#include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include "IRLock.h" @@ -24,6 +24,6 @@ class AP_IRLock_SITL_Gazebo : public IRLock private: uint32_t _last_timestamp; - SocketAPM sock; + SocketAPM_native sock; }; #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index e8bfb60eaed66d..37504d8897abf3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -1340,7 +1340,7 @@ bool AP_InertialSensor::_calculate_trim(const Vector3f &accel_sample, Vector3f & if (view != nullptr) { // Use pitch to guess which axis the user is trying to trim // 5 deg buffer to favor normal AHRS and avoid floating point funny business - if (fabsf(view->pitch) < (fabsf(AP::ahrs().pitch)+radians(5)) ) { + if (fabsf(view->pitch) < (fabsf(AP::ahrs().get_pitch())+radians(5)) ) { // user is trying to calibrate view rotation = view->get_rotation(); if (!is_zero(view->get_pitch_trim())) { @@ -1460,7 +1460,7 @@ bool AP_InertialSensor::get_accel_health_all(void) const return (get_accel_count() > 0); } -#if HAL_GCS_ENABLED +#if HAL_GCS_ENABLED && AP_AHRS_ENABLED /* calculate the trim_roll and trim_pitch. This is used for redoing the trim without needing a full accel cal @@ -1521,6 +1521,7 @@ MAV_RESULT AP_InertialSensor::calibrate_trim() // reset ahrs's trim to suggested values from calibration routine ahrs.set_trim(trim_rad); + last_accel_cal_ms = AP_HAL::millis(); _trimming_accel = false; return MAV_RESULT_ACCEPTED; @@ -1530,7 +1531,7 @@ MAV_RESULT AP_InertialSensor::calibrate_trim() _trimming_accel = false; return MAV_RESULT_FAILED; } -#endif // HAL_GCS_ENABLED +#endif // HAL_GCS_ENABLED && AP_AHRS_ENABLED /* check if the accelerometers are calibrated in 3D and that current number of accels matched number when calibrated @@ -2408,7 +2409,9 @@ bool AP_InertialSensor::calibrate_gyros() if (!gyro_calibrated_ok_all()) { return false; } +#if AP_AHRS_ENABLED AP::ahrs().reset_gyro_drift(); +#endif return true; } @@ -2550,8 +2553,10 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal() #endif } +#if AP_AHRS_ENABLED // force trim to zero AP::ahrs().set_trim(Vector3f(0, 0, 0)); +#endif } else { DEV_PRINTF("\nFAILED\n"); // restore old values @@ -2572,8 +2577,10 @@ MAV_RESULT AP_InertialSensor::simple_accel_cal() update(); } +#if AP_AHRS_ENABLED // and reset state estimators AP::ahrs().reset(); +#endif // stop flashing leds AP_Notify::flags.initialising = false; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 9ceb54c12d210a..85c9ed8f34e359 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -1,6 +1,7 @@ #define AP_INLINE_VECTOR_OPS #include +#include #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" #include @@ -218,7 +219,7 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const continue; } bool inactive = notch.is_inactive(); -#ifndef HAL_BUILD_AP_PERIPH +#if AP_AHRS_ENABLED // by default we only run the expensive notch filters on the // currently active IMU we reset the inactive notch filters so // that if we switch IMUs we're not left with old data @@ -442,8 +443,14 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa return; } +#if AP_AHRS_ENABLED + const bool log_because_primary_gyro = _imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == AP::ahrs().get_primary_gyro_index()); +#else + const bool log_because_primary_gyro = false; +#endif + if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::ALL_GYROS) || - (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRIMARY_GYRO_ONLY) && (instance == AP::ahrs().get_primary_gyro_index())) || + log_because_primary_gyro || should_log_imu_raw()) { if (_imu.raw_logging_option_set(AP_InertialSensor::RAW_LOGGING_OPTION::PRE_AND_POST_FILTER)) { @@ -810,6 +817,7 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */ } } +#if HAL_LOGGING_ENABLED bool AP_InertialSensor_Backend::should_log_imu_raw() const { if (_imu._log_raw_bit == (uint32_t)-1) { @@ -825,6 +833,7 @@ bool AP_InertialSensor_Backend::should_log_imu_raw() const } return true; } +#endif // HAL_LOGGING_ENABLED // log an unexpected change in a register for an IMU void AP_InertialSensor_Backend::log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg ®) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 880571ee4f51e1..e51d9362b80b50 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -312,7 +312,7 @@ class AP_InertialSensor_Backend */ void notify_accel_fifo_reset(uint8_t instance) __RAMFUNC__; void notify_gyro_fifo_reset(uint8_t instance) __RAMFUNC__; - + // log an unexpected change in a register for an IMU void log_register_change(uint32_t bus_id, const AP_HAL::Device::checkreg ®) __RAMFUNC__; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index e59019d7ded194..2318766a6720a6 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -1,3 +1,7 @@ +#include + +#if HAL_LOGGING_ENABLED + #include "AP_InertialSensor.h" #include "AP_InertialSensor_Backend.h" @@ -188,3 +192,5 @@ void AP_InertialSensor::write_notch_log_messages() const } } } + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp index b64a06b9358f59..1d4ff5f19887bc 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_NONE.cpp @@ -7,7 +7,7 @@ #if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 -float rand_float(void) +static float sim_rand_float(void) { return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6; } @@ -56,7 +56,7 @@ void AP_InertialSensor_NONE::accumulate() // calculate a noisy noise component static float calculate_noise(float noise, float noise_variation) { - return noise * (1.0f + noise_variation * rand_float()); + return noise * (1.0f + noise_variation * sim_rand_float()); } /* @@ -81,9 +81,9 @@ void AP_InertialSensor_NONE::generate_accel() // this smears the individual motor peaks somewhat emulating physical motors //float freq_variation = 0.12f; // add in sensor noise - xAccel += accel_noise * rand_float(); - yAccel += accel_noise * rand_float(); - zAccel += accel_noise * rand_float(); + xAccel += accel_noise * sim_rand_float(); + yAccel += accel_noise * sim_rand_float(); + zAccel += accel_noise * sim_rand_float(); bool motors_on = 1; @@ -99,9 +99,9 @@ void AP_InertialSensor_NONE::generate_accel() if (vibe_freq.is_zero()) { // no rpm noise, so add in background noise if any - xAccel += accel_noise * rand_float(); - yAccel += accel_noise * rand_float(); - zAccel += accel_noise * rand_float(); + xAccel += accel_noise * sim_rand_float(); + yAccel += accel_noise * sim_rand_float(); + zAccel += accel_noise * sim_rand_float(); } if (!vibe_freq.is_zero() && motors_on) { @@ -189,9 +189,9 @@ void AP_InertialSensor_NONE::generate_gyro() // this smears the individual motor peaks somewhat emulating physical motors float freq_variation = 0.12f; // add in sensor noise - p += gyro_noise * rand_float(); - q += gyro_noise * rand_float(); - r += gyro_noise * rand_float(); + p += gyro_noise * sim_rand_float(); + q += gyro_noise * sim_rand_float(); + r += gyro_noise * sim_rand_float(); bool motors_on = 1; // on a real 180mm copter gyro noise varies between 0.2-0.4 rad/s for throttle 0.2-0.8 @@ -206,9 +206,9 @@ void AP_InertialSensor_NONE::generate_gyro() if ( vibe_freq.is_zero() ) { // no rpm noise, so add in background noise if any - p += gyro_noise * rand_float(); - q += gyro_noise * rand_float(); - r += gyro_noise * rand_float(); + p += gyro_noise * sim_rand_float(); + q += gyro_noise * sim_rand_float(); + r += gyro_noise * sim_rand_float(); } if (!vibe_freq.is_zero() && motors_on) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index 49a691b6061eb7..c6b994413a8bb9 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -2,7 +2,7 @@ #include "AP_InertialSensor_SITL.h" #include #include -#include +#include #if AP_SIM_INS_ENABLED @@ -409,8 +409,8 @@ void AP_InertialSensor_SITL::read_gyro_from_file() { if (gyro_fd == -1) { char namebuf[32]; - snprintf(namebuf, 32, "/tmp/gyro%d.dat", gyro_instance); - gyro_fd = open(namebuf, O_RDONLY|O_CLOEXEC); + snprintf(namebuf, sizeof(namebuf), "/tmp/gyro%d.dat", gyro_instance); + gyro_fd = AP::FS().open(namebuf, O_RDONLY); if (gyro_fd == -1) { AP_HAL::panic("gyro data file %s not found", namebuf); } @@ -426,10 +426,12 @@ void AP_InertialSensor_SITL::read_gyro_from_file() if (ret <= 0) { if (sitl->gyro_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF) { +#if HAL_LOGGING_ENABLED //stop logging if (AP_Logger::get_singleton()) { AP::logger().StopLogging(); } +#endif exit(0); } lseek(gyro_fd, 0, SEEK_SET); @@ -460,7 +462,7 @@ void AP_InertialSensor_SITL::write_gyro_to_file(const Vector3f& gyro) { if (gyro_fd == -1) { char namebuf[32]; - snprintf(namebuf, 32, "/tmp/gyro%d.dat", gyro_instance); + snprintf(namebuf, sizeof(namebuf), "/tmp/gyro%d.dat", gyro_instance); gyro_fd = open(namebuf, O_WRONLY|O_TRUNC|O_CREAT, S_IRWXU|S_IRGRP|S_IROTH); } @@ -475,7 +477,7 @@ void AP_InertialSensor_SITL::read_accel_from_file() { if (accel_fd == -1) { char namebuf[32]; - snprintf(namebuf, 32, "/tmp/accel%d.dat", accel_instance); + snprintf(namebuf, sizeof(namebuf), "/tmp/accel%d.dat", accel_instance); accel_fd = open(namebuf, O_RDONLY|O_CLOEXEC); if (accel_fd == -1) { AP_HAL::panic("accel data file %s not found", namebuf); @@ -492,10 +494,12 @@ void AP_InertialSensor_SITL::read_accel_from_file() if (ret <= 0) { if (sitl->accel_file_rw == SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF) { +#if HAL_LOGGING_ENABLED //stop logging if (AP_Logger::get_singleton()) { AP::logger().StopLogging(); } +#endif exit(0); } lseek(accel_fd, 0, SEEK_SET); @@ -530,7 +534,7 @@ void AP_InertialSensor_SITL::write_accel_to_file(const Vector3f& accel) if (accel_fd == -1) { char namebuf[32]; - snprintf(namebuf, 32, "/tmp/accel%d.dat", accel_instance); + snprintf(namebuf, sizeof(namebuf), "/tmp/accel%d.dat", accel_instance); accel_fd = open(namebuf, O_WRONLY|O_TRUNC|O_CREAT, S_IRWXU|S_IRGRP|S_IROTH); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp index 486d14965a116f..ef5062cb93d257 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp @@ -26,6 +26,7 @@ #include #include #include +#include "AP_InertialSensor.h" // this scale factor ensures params are easy to work with in GUI parameter editors #define SCALE_FACTOR 1.0e6 diff --git a/libraries/AP_L1_Control/AP_L1_Control.cpp b/libraries/AP_L1_Control/AP_L1_Control.cpp index 816637af457c6f..2750c232298d0d 100644 --- a/libraries/AP_L1_Control/AP_L1_Control.cpp +++ b/libraries/AP_L1_Control/AP_L1_Control.cpp @@ -56,9 +56,9 @@ const AP_Param::GroupInfo AP_L1_Control::var_info[] = { float AP_L1_Control::get_yaw() const { if (_reverse) { - return wrap_PI(M_PI + _ahrs.yaw); + return wrap_PI(M_PI + _ahrs.get_yaw()); } - return _ahrs.yaw; + return _ahrs.get_yaw(); } /* @@ -88,7 +88,7 @@ int32_t AP_L1_Control::nav_roll_cd(void) const Made changes to avoid zero division as proposed by Andrew Tridgell: https://github.com/ArduPilot/ardupilot/pull/24331#discussion_r1267798397 */ float pitchLimL1 = radians(60); // Suggestion: constraint may be modified to pitch limits if their absolute values are less than 90 degree and more than 60 degrees. - float pitchL1 = constrain_float(_ahrs.pitch,-pitchLimL1,pitchLimL1); + float pitchL1 = constrain_float(_ahrs.get_pitch(),-pitchLimL1,pitchLimL1); ret = degrees(atanf(_latAccDem * (1.0f/(GRAVITY_MSS * cosf(pitchL1))))) * 100.0f; ret = constrain_float(ret, -9000, 9000); return ret; @@ -396,7 +396,7 @@ void AP_L1_Control::update_loiter(const Location ¢er_WP, float radius, int8_ A_air_unit = A_air.normalized(); } else { if (_groundspeed_vector.length() < 0.1f) { - A_air_unit = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)); + A_air_unit = Vector2f(cosf(_ahrs.get_yaw()), sinf(_ahrs.get_yaw())); } else { A_air_unit = _groundspeed_vector.normalized(); } @@ -504,7 +504,7 @@ void AP_L1_Control::update_level_flight(void) { // copy to _target_bearing_cd and _nav_bearing _target_bearing_cd = _ahrs.yaw_sensor; - _nav_bearing = _ahrs.yaw; + _nav_bearing = _ahrs.get_yaw(); _bearing_error = 0; _crosstrack_error = 0; diff --git a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp index e6ad80edbf21d8..5d3bdc2f41558a 100644 --- a/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp +++ b/libraries/AP_LTM_Telem/AP_LTM_Telem.cpp @@ -70,6 +70,7 @@ void AP_LTM_Telem::send_Gframe(void) int32_t lon = 0; // longtitude uint8_t gndspeed = 0; // gps ground speed (m/s) int32_t alt = 0; +#if AP_AHRS_ENABLED { AP_AHRS &ahrs = AP::ahrs(); WITH_SEMAPHORE(ahrs.get_semaphore()); @@ -83,6 +84,7 @@ void AP_LTM_Telem::send_Gframe(void) gndspeed = (uint8_t) roundf(gps.ground_speed()); } } +#endif uint8_t lt_buff[LTM_GFRAME_SIZE]; // protocol: START(2 bytes)FRAMEID(1byte)LAT(cm,4 bytes)LON(cm,4bytes)SPEED(m/s,1bytes)ALT(cm,4bytes)SATS(6bits)FIX(2bits)CRC(xor,1byte) @@ -114,6 +116,7 @@ void AP_LTM_Telem::send_Gframe(void) // Sensors frame void AP_LTM_Telem::send_Sframe(void) { +#if AP_BATTERY_ENABLED const AP_BattMonitor &battery = AP::battery(); const uint16_t volt = (uint16_t) roundf(battery.voltage() * 1000.0f); // battery voltage (expects value in mV) float current; @@ -122,6 +125,10 @@ void AP_LTM_Telem::send_Sframe(void) } // note: max. current value we can send is 65.536 A const uint16_t amp = (uint16_t) roundf(current * 100.0f); // current sensor (expects value in hundredth of A) +#else + const uint16_t volt = 0; + const uint16_t amp = 0; +#endif // airspeed in m/s if available and enabled - even if not used - otherwise send 0 uint8_t airspeed = 0; // airspeed sensor (m/s) @@ -168,6 +175,7 @@ void AP_LTM_Telem::send_Aframe(void) int16_t pitch; int16_t roll; int16_t heading; +#if AP_AHRS_ENABLED { AP_AHRS &ahrs = AP::ahrs(); WITH_SEMAPHORE(ahrs.get_semaphore()); @@ -175,6 +183,11 @@ void AP_LTM_Telem::send_Aframe(void) roll = roundf(ahrs.roll_sensor * 0.01); // attitude roll in degrees heading = roundf(ahrs.yaw_sensor * 0.01); // heading in degrees } +#else + pitch = 0; + roll = 0; + heading = 0; +#endif uint8_t lt_buff[LTM_AFRAME_SIZE]; // A Frame: $T(2 bytes)A(1byte)PITCH(2 bytes)ROLL(2bytes)HEADING(2bytes)CRC(xor,1byte) diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index e3947d40f680da..51ab12a63bf77e 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -43,18 +43,18 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { // @User: Advanced AP_GROUPINFO("ABORT_DEG", 2, AP_Landing, slope_recalc_steep_threshold_to_abort, 0), - // @Param: PITCH_CD + // @Param: PITCH_DEG // @DisplayName: Landing Pitch // @Description: Used in autoland to give the minimum pitch in the final stage of landing (after the flare). This parameter can be used to ensure that the final landing attitude is appropriate for the type of undercarriage on the aircraft. Note that it is a minimum pitch only - the landing code will control pitch above this value to try to achieve the configured landing sink rate. - // @Units: cdeg - // @Range: -2000 2000 + // @Units: deg + // @Range: -20 20 // @Increment: 10 // @User: Advanced - AP_GROUPINFO("PITCH_CD", 3, AP_Landing, pitch_cd, 0), + AP_GROUPINFO("PITCH_DEG", 3, AP_Landing, pitch_deg, 0), // @Param: FLARE_ALT // @DisplayName: Landing flare altitude - // @Description: Altitude in autoland at which to lock heading and flare to the LAND_PITCH_CD pitch. Note that this option is secondary to LAND_FLARE_SEC. For a good landing it preferable that the flare is triggered by LAND_FLARE_SEC. + // @Description: Altitude in autoland at which to lock heading and flare to the LAND_PITCH_DEG pitch. Note that this option is secondary to LAND_FLARE_SEC. For a good landing it preferable that the flare is triggered by LAND_FLARE_SEC. // @Units: m // @Range: 0 30 // @Increment: 0.1 @@ -141,7 +141,7 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { // @Param: OPTIONS // @DisplayName: Landing options bitmask // @Description: Bitmask of options to use with landing. - // @Bitmask: 0: honor min throttle during landing flare,1: Increase Target landing airspeed constraint From Trim Airspeed to ARSPD_FBW_MAX + // @Bitmask: 0: honor min throttle during landing flare,1: Increase Target landing airspeed constraint From Trim Airspeed to AIRSPEED_MAX // @User: Advanced AP_GROUPINFO("OPTIONS", 16, AP_Landing, _options, 0), @@ -156,7 +156,7 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { // @Param: WIND_COMP // @DisplayName: Headwind Compensation when Landing - // @Description: This param controls how much headwind compensation is used when landing. Headwind speed component multiplied by this parameter is added to TECS_LAND_ARSPD command. Set to Zero to disable. Note: The target landing airspeed command is still limited to ARSPD_FBW_MAX. + // @Description: This param controls how much headwind compensation is used when landing. Headwind speed component multiplied by this parameter is added to TECS_LAND_ARSPD command. Set to Zero to disable. Note: The target landing airspeed command is still limited to AIRSPEED_MAX. // @Range: 0 100 // @Units: % // @Increment: 1 @@ -253,7 +253,7 @@ bool AP_Landing::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, default: // returning TRUE while executing verify_land() will increment the // mission index which in many cases will trigger an RTL for end-of-mission - gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE"); success = true; break; } @@ -486,14 +486,14 @@ bool AP_Landing::restart_landing_sequence() mission.set_current_cmd(current_index+1)) { // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it - gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", (signed)cmd.content.location.alt/100); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", (signed)cmd.content.location.alt/100); success = true; } else if (do_land_start_index != 0 && mission.set_current_cmd(do_land_start_index)) { // look for a DO_LAND_START and use that index - gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index); success = true; } else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE && @@ -501,10 +501,10 @@ bool AP_Landing::restart_landing_sequence() { // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope - gcs().send_text(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index); success = true; } else { - gcs().send_text(MAV_SEVERITY_WARNING, "Unable to restart landing sequence"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to restart landing sequence"); success = false; } @@ -548,33 +548,6 @@ bool AP_Landing::get_target_altitude_location(Location &location) } } -/* - * Determine how aligned heading_deg is with the wind. Return result - * is 1.0 when perfectly aligned heading into wind, -1 when perfectly - * aligned with-wind, and zero when perfect cross-wind. There is no - * distinction between a left or right cross-wind. Wind speed is ignored - */ -float AP_Landing::wind_alignment(const float heading_deg) -{ - const Vector3f wind = ahrs.wind_estimate(); - const float wind_heading_rad = atan2f(-wind.y, -wind.x); - return cosf(wind_heading_rad - radians(heading_deg)); -} - -/* - * returns head-wind in m/s, 0 for tail-wind. - */ -float AP_Landing::head_wind(void) -{ - const float alignment = wind_alignment(ahrs.yaw_sensor*0.01f); - - if (alignment <= 0) { - return 0; - } - - return alignment * ahrs.wind_estimate().length(); -} - /* * returns target airspeed in cm/s depending on flight stage */ @@ -582,7 +555,7 @@ int32_t AP_Landing::get_target_airspeed_cm(void) { if (!flags.in_progress) { // not landing, use regular cruise airspeed - return aparm.airspeed_cruise_cm; + return aparm.airspeed_cruise*100; } switch (type) { @@ -652,6 +625,7 @@ bool AP_Landing::is_complete(void) const void AP_Landing::Log(void) const { +#if HAL_LOGGING_ENABLED switch (type) { case TYPE_STANDARD_GLIDE_SLOPE: type_slope_log(); @@ -664,6 +638,7 @@ void AP_Landing::Log(void) const default: break; } +#endif } /* @@ -734,3 +709,12 @@ bool AP_Landing::terminate(void) { return false; } } + +/* + run parameter conversions + */ +void AP_Landing::convert_parameters(void) +{ + // added January 2024 + pitch_deg.convert_centi_parameter(AP_PARAM_INT16); +} diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index b9cfc80e888a9e..80349010bc9da5 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -61,9 +61,11 @@ class AP_Landing { // we support upto 32 boolean bits for users wanting to change landing behaviour. enum OptionsMask { ON_LANDING_FLARE_USE_THR_MIN = (1<<0), // If set then set trottle to thr_min instead of zero on final flare - ON_LANDING_USE_ARSPD_MAX = (1<<1), // If set then allow landing throttle constraint to be increased from trim airspeed to max airspeed (ARSPD_FBW_MAX) + ON_LANDING_USE_ARSPD_MAX = (1<<1), // If set then allow landing throttle constraint to be increased from trim airspeed to max airspeed (AIRSPEED_MAX) }; + void convert_parameters(void); + void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude); bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed); @@ -91,14 +93,12 @@ class AP_Landing { // helper functions bool restart_landing_sequence(void); - float wind_alignment(const float heading_deg); - float head_wind(void); int32_t get_target_airspeed_cm(void); // accessor functions for the params and states static const struct AP_Param::GroupInfo var_info[]; - int16_t get_pitch_cd(void) const { return pitch_cd; } + int16_t get_pitch_cd(void) const { return pitch_deg*100; } float get_flare_sec(void) const { return flare_sec; } int8_t get_disarm_delay(void) const { return disarm_delay; } int8_t get_then_servos_neutral(void) const { return then_servos_neutral; } @@ -153,7 +153,7 @@ class AP_Landing { AP_Landing_Deepstall deepstall; #endif - AP_Int16 pitch_cd; + AP_Float pitch_deg; AP_Float flare_alt; AP_Float flare_sec; AP_Float pre_flare_airspeed; diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index 550fa92ecc6c3d..96b395a2e9ff80 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -345,7 +345,7 @@ bool AP_Landing_Deepstall::override_servos(void) if (elevator == nullptr) { // deepstalls are impossible without these channels, abort the process - gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Deepstall: Unable to find the elevator channels"); request_go_around(); return false; } @@ -429,7 +429,7 @@ int32_t AP_Landing_Deepstall::get_target_airspeed_cm(void) const stage == DEEPSTALL_STAGE_LAND) { return landing.pre_flare_airspeed * 100; } else { - return landing.aparm.airspeed_cruise_cm; + return landing.aparm.airspeed_cruise*100; } } @@ -456,6 +456,7 @@ const AP_PIDInfo& AP_Landing_Deepstall::get_pid_info(void) const return ds_PID.get_pid_info(); } +#if HAL_LOGGING_ENABLED void AP_Landing_Deepstall::Log(void) const { const AP_PIDInfo& pid_info = ds_PID.get_pid_info(); struct log_DSTL pkt = { @@ -479,6 +480,7 @@ void AP_Landing_Deepstall::Log(void) const { }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif // termination handling, expected to set the servo outputs bool AP_Landing_Deepstall::terminate(void) { @@ -533,17 +535,17 @@ void AP_Landing_Deepstall::build_approach_path(bool use_current_heading) #ifdef DEBUG_PRINTS // TODO: Send this information via a MAVLink packet - gcs().send_text(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Arc: %3.8f %3.8f", (double)(arc.lat / 1e7),(double)( arc.lng / 1e7)); - gcs().send_text(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loiter en: %3.8f %3.8f", (double)(arc_entry.lat / 1e7), (double)(arc_entry.lng / 1e7)); - gcs().send_text(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Loiter ex: %3.8f %3.8f", (double)(arc_exit.lat / 1e7), (double)(arc_exit.lng / 1e7)); - gcs().send_text(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Extended: %3.8f %3.8f", (double)(extended_approach.lat / 1e7), (double)(extended_approach.lng / 1e7)); - gcs().send_text(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m, + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Extended by: %f (%f)", (double)approach_extension_m, (double)expected_travel_distance); - gcs().send_text(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Target Heading: %3.1f", (double)target_heading_deg); #endif // DEBUG_PRINTS } @@ -590,7 +592,7 @@ float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const f if(print) { // allow printing the travel distances on the final entry as its used for tuning - gcs().send_text(MAV_SEVERITY_INFO, "Deepstall: Entry: %0.1f (m) Travel: %0.1f (m)", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Deepstall: Entry: %0.1f (m) Travel: %0.1f (m)", (double)stall_distance, (double)predicted_travel_distance); } @@ -620,7 +622,7 @@ float AP_Landing_Deepstall::update_steering() // panic if no position source is available // continue the stall but target just holding the wings held level as deepstall should be a minimal // energy configuration on the aircraft, and if a position isn't available aborting would be worse - gcs().send_text(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Deepstall: Invalid data from AHRS. Holding level"); hold_level = true; } @@ -644,7 +646,7 @@ float AP_Landing_Deepstall::update_steering() L1_xtrack_i = constrain_float(L1_xtrack_i, -0.5f, 0.5f); nu1 += L1_xtrack_i; } - desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.yaw) / time_constant; + desired_change = wrap_PI(radians(target_heading_deg) + nu1 - landing.ahrs.get_yaw()) / time_constant; } float yaw_rate = landing.ahrs.get_gyro().z; @@ -652,7 +654,7 @@ float AP_Landing_Deepstall::update_steering() float error = wrap_PI(constrain_float(desired_change, -yaw_rate_limit_rps, yaw_rate_limit_rps) - yaw_rate); #ifdef DEBUG_PRINTS - gcs().send_text(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "x: %f e: %f r: %f d: %f", (double)crosstrack_error, (double)error, (double)degrees(yaw_rate), diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index 9499dc80f47d70..4ebecb3b84e9e3 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -38,7 +38,7 @@ void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, cons type_slope_flags.post_stats = false; type_slope_stage = SlopeStage::NORMAL; - gcs().send_text(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude); } void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed) @@ -106,9 +106,9 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n if (type_slope_stage != SlopeStage::FINAL) { type_slope_flags.post_stats = true; if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); } else { - gcs().send_text(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", (double)height, (double)sink_rate, (double)gps.ground_speed(), (double)current_loc.get_distance(next_WP_loc)); @@ -122,7 +122,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n AP_LandingGear *LG_inst = AP_LandingGear::get_singleton(); if (LG_inst != nullptr && !LG_inst->check_before_land()) { type_slope_request_go_around(); - gcs().send_text(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Landing gear was not deployed"); } #endif } @@ -132,8 +132,8 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n // been set for landing. We don't do this till ground // speed drops below 3.0 m/s as otherwise we will change // target speeds too early. - aparm.airspeed_cruise_cm.load(); - aparm.min_gndspeed_cm.load(); + aparm.airspeed_cruise.load(); + aparm.min_groundspeed.load(); aparm.throttle_cruise.load(); } } else if (type_slope_stage == SlopeStage::APPROACH && pre_flare_airspeed > 0) { @@ -158,7 +158,7 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm if (type_slope_flags.post_stats && !is_armed) { type_slope_flags.post_stats = false; - gcs().send_text(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)current_loc.get_distance(next_WP_loc)); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)current_loc.get_distance(next_WP_loc)); } // check if we should auto-disarm after a confirmed landing @@ -226,7 +226,7 @@ void AP_Landing::type_slope_adjust_landing_slope_for_rangefinder_bump(AP_FixedWi // is projected slope too steep? if (new_slope_deg - initial_slope_deg > slope_recalc_steep_threshold_to_abort) { - gcs().send_text(MAV_SEVERITY_INFO, "Landing slope too steep, aborting (%.0fm %.1fdeg)", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing slope too steep, aborting (%.0fm %.1fdeg)", (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg)); alt_offset = rangefinder_state.correction; flags.commanded_go_around = true; @@ -319,7 +319,7 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo bool is_first_calc = is_zero(slope); slope = (sink_height - aim_height) / (total_distance - flare_distance); if (is_first_calc) { - gcs().send_text(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope))); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope))); } // calculate point along that slope 500m ahead @@ -345,15 +345,15 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) // pre-flare airspeeds. Also increase for head-winds const float land_airspeed = tecs_Controller->get_land_airspeed(); - int32_t target_airspeed_cm = aparm.airspeed_cruise_cm; + int32_t target_airspeed_cm = aparm.airspeed_cruise*100; if (land_airspeed >= 0) { target_airspeed_cm = land_airspeed * 100; } else { - target_airspeed_cm = 0.5 * (aparm.airspeed_cruise_cm * 0.01 + aparm.airspeed_min); + target_airspeed_cm = 100 * 0.5 * (aparm.airspeed_cruise + aparm.airspeed_min); } switch (type_slope_stage) { case SlopeStage::NORMAL: - target_airspeed_cm = aparm.airspeed_cruise_cm; + target_airspeed_cm = aparm.airspeed_cruise*100; break; case SlopeStage::APPROACH: break; @@ -368,9 +368,9 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) // when landing, add half of head-wind. const float head_wind_comp = constrain_float(wind_comp, 0.0f, 100.0f)*0.01; - const int32_t head_wind_compensation_cm = head_wind() * head_wind_comp * 100; + const int32_t head_wind_compensation_cm = ahrs.head_wind() * head_wind_comp * 100; - const uint32_t max_airspeed_cm = AP_Landing::allow_max_airspeed_on_land() ? aparm.airspeed_max*100 : aparm.airspeed_cruise_cm; + const uint32_t max_airspeed_cm = AP_Landing::allow_max_airspeed_on_land() ? aparm.airspeed_max*100 : aparm.airspeed_cruise*100; return constrain_int32(target_airspeed_cm + head_wind_compensation_cm, target_airspeed_cm, max_airspeed_cm); @@ -407,6 +407,7 @@ bool AP_Landing::type_slope_is_complete(void) const return (type_slope_stage == SlopeStage::FINAL); } +#if HAL_LOGGING_ENABLED void AP_Landing::type_slope_log(void) const { // @LoggerMessage: LAND @@ -429,6 +430,7 @@ void AP_Landing::type_slope_log(void) const (double)alt_offset, (double)height_flare_log); } +#endif bool AP_Landing::type_slope_is_throttle_suppressed(void) const { diff --git a/libraries/AP_Landing/LogStructure.h b/libraries/AP_Landing/LogStructure.h index aabeb98fc13fab..98b62f42a72c7e 100644 --- a/libraries/AP_Landing/LogStructure.h +++ b/libraries/AP_Landing/LogStructure.h @@ -45,7 +45,7 @@ struct PACKED log_DSTL { #define LOG_STRUCTURE_FROM_LANDING \ { LOG_DSTL_MSG, sizeof(log_DSTL), \ - "DSTL", "QBfLLeccfeffff", "TimeUS,Stg,THdg,Lat,Lng,Alt,XT,Travel,L1I,Loiter,Des,P,I,D", "s??DUm--------", "F??000--------" , true }, + "DSTL", "QBfLLeccfeffff", "TimeUS,Stg,THdg,Lat,Lng,Alt,XT,Travel,L1I,Loiter,Des,P,I,D", "s-hDUm--------", "F-0000--------" , true }, #else #define LOG_STRUCTURE_FROM_LANDING #endif diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index b7a7bc3ebb45bd..7ac89719947864 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.cpp +++ b/libraries/AP_LandingGear/AP_LandingGear.cpp @@ -2,7 +2,6 @@ #if AP_LANDINGGEAR_ENABLED -#include #include #include #include @@ -175,7 +174,7 @@ void AP_LandingGear::deploy() // set deployed flag _deployed = true; _have_changed = true; - AP::logger().Write_Event(LogEvent::LANDING_GEAR_DEPLOYED); + LOGGER_WRITE_EVENT(LogEvent::LANDING_GEAR_DEPLOYED); } /// retract - retract landing gear @@ -191,7 +190,7 @@ void AP_LandingGear::retract() // reset deployed flag _deployed = false; _have_changed = true; - AP::logger().Write_Event(LogEvent::LANDING_GEAR_RETRACTED); + LOGGER_WRITE_EVENT(LogEvent::LANDING_GEAR_RETRACTED); // send message only if output has been configured if (SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control)) { @@ -304,6 +303,7 @@ void AP_LandingGear::update(float height_above_ground_m) _last_height_above_ground = alt_m; } +#if HAL_LOGGING_ENABLED // log weight on wheels state void AP_LandingGear::log_wow_state(LG_WOW_State state) { @@ -311,6 +311,7 @@ void AP_LandingGear::log_wow_state(LG_WOW_State state) AP_HAL::micros64(), (int8_t)gear_state_current, (int8_t)state); } +#endif bool AP_LandingGear::check_before_land(void) { diff --git a/libraries/AP_LandingGear/AP_LandingGear.h b/libraries/AP_LandingGear/AP_LandingGear.h index 6cafc77d5ad89d..cfa0eebaae7faf 100644 --- a/libraries/AP_LandingGear/AP_LandingGear.h +++ b/libraries/AP_LandingGear/AP_LandingGear.h @@ -8,6 +8,7 @@ #include #include +#include /// @class AP_LandingGear /// @brief Class managing the control of landing gear @@ -122,8 +123,12 @@ class AP_LandingGear { /// deploy - deploy the landing gear void deploy(); +#if HAL_LOGGING_ENABLED // log weight on wheels state void log_wow_state(LG_WOW_State state); +#else + void log_wow_state(LG_WOW_State state) {} +#endif static AP_LandingGear *_singleton; }; diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 6373b9d2d64c6d..fb55e52f4ddeb5 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -289,7 +289,7 @@ static uint8_t count_commas(const char *string) /// return a unit name given its ID const char* AP_Logger::unit_name(const uint8_t unit_id) { - for (uint8_t i=0; i #include #include @@ -609,3 +611,13 @@ class AP_Logger namespace AP { AP_Logger &logger(); }; + +#define LOGGER_WRITE_ERROR(subsys, err) AP::logger().Write_Error(subsys, err) +#define LOGGER_WRITE_EVENT(evt) AP::logger().Write_Event(evt) + +#else + +#define LOGGER_WRITE_ERROR(subsys, err) +#define LOGGER_WRITE_EVENT(evt) + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp index 3403d5f059363b..08486b60e9b19e 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp @@ -17,13 +17,15 @@ along with this program. If not, see . */ +#include +#include + +#if HAL_LOGGING_ENABLED && HAL_GCS_ENABLED #include #include #include // for LOG_ENTRY -#if HAL_GCS_ENABLED - extern const AP_HAL::HAL& hal; /** @@ -327,4 +329,4 @@ bool AP_Logger::handle_log_send_data() return true; } -#endif +#endif // HAL_LOGGING_ENABLED && HAL_GCS_ENABLED diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 0666943e94b87e..2febb3577fba0d 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -248,6 +248,7 @@ void AP_Logger::Write_RCOUT(void) } +#if AP_RSSI_ENABLED // Write an RSSI packet void AP_Logger::Write_RSSI() { @@ -264,6 +265,7 @@ void AP_Logger::Write_RSSI() }; WriteBlock(&pkt, sizeof(pkt)); } +#endif void AP_Logger::Write_Command(const mavlink_command_int_t &packet, uint8_t source_system, @@ -476,6 +478,8 @@ void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info) enum class log_PID_Flags : uint8_t { LIMIT = 1U<<0, // true if the output is saturated, I term anti windup is active PD_SUM_LIMIT = 1U<<1, // true if the PD sum limit is active + RESET = 1U<<2, // true if the controller was reset + I_TERM_SET = 1U<<3, // true if the I term has been set externally including reseting to 0 }; uint8_t flags = 0; @@ -485,6 +489,12 @@ void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info) if (info.PD_limit) { flags |= (uint8_t)log_PID_Flags::PD_SUM_LIMIT; } + if (info.reset) { + flags |= (uint8_t)log_PID_Flags::RESET; + } + if (info.I_term_set) { + flags |= (uint8_t)log_PID_Flags::I_TERM_SET; + } const struct log_PID pkt{ LOG_PACKET_HEADER_INIT(msg_type), diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 1259c6417cd75f..8845c9ba00fc2f 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -817,7 +817,8 @@ struct PACKED log_VER { // @Field: TimeUS: Time since system startup // @Field: LandingGear: Current landing gear state // @FieldValueEnum: LandingGear: AP_LandingGear::LG_LandingGear_State -// @Field: WeightOnWheels: True if there is weight on wheels +// @Field: WeightOnWheels: Weight on wheels state +// @FieldValueEnum: WeightOnWheels: AP_LandingGear::LG_WOW_State // @LoggerMessage: MAG // @Description: Information received from compasses @@ -891,8 +892,8 @@ struct PACKED log_VER { // @Field: Qual: Estimated sensor data quality // @Field: flowX: Sensor flow rate, X-axis // @Field: flowY: Sensor flow rate,Y-axis -// @Field: bodyX: derived velocity, X-axis -// @Field: bodyY: derived velocity, Y-axis +// @Field: bodyX: derived rotational velocity, X-axis +// @Field: bodyY: derived rotational velocity, Y-axis // @LoggerMessage: PARM // @Description: parameter value @@ -901,8 +902,20 @@ struct PACKED log_VER { // @Field: Value: parameter value // @Field: Default: default parameter value for this board and config -// @LoggerMessage: PIDR,PIDP,PIDY,PIDA,PIDS,PIDN,PIDE -// @Description: Proportional/Integral/Derivative gain values for Roll/Pitch/Yaw/Altitude/Steering +// @LoggerMessage: PIDR +// @Description: Proportional/Integral/Derivative gain values for Roll rate +// @LoggerMessage: PIDP +// @Description: Proportional/Integral/Derivative gain values for Pitch rate +// @LoggerMessage: PIDY +// @Description: Proportional/Integral/Derivative gain values for Yaw rate +// @LoggerMessage: PIDA +// @Description: Proportional/Integral/Derivative gain values for vertical acceleration +// @LoggerMessage: PIDS +// @Description: Proportional/Integral/Derivative gain values for ground steering yaw rate +// @LoggerMessage: PIDN +// @Description: Proportional/Integral/Derivative gain values for North/South velocity +// @LoggerMessage: PIDE +// @Description: Proportional/Integral/Derivative gain values for East/West velocity // @Field: TimeUS: Time since system startup // @Field: Tar: desired value // @Field: Act: achieved value @@ -941,7 +954,9 @@ struct PACKED log_VER { // @Field: Vcc: Flight board voltage // @Field: VServo: Servo rail voltage // @Field: Flags: System power flags +// @FieldBitmaskEnum: Flags: AP_HAL::AnalogIn::PowerStatusFlag // @Field: AccFlags: Accumulated System power flags; all flags which have ever been set +// @FieldBitmaskEnum: AccFlags: AP_HAL::AnalogIn::PowerStatusFlag // @Field: Safety: Hardware Safety Switch status // @LoggerMessage: MCU @@ -1224,7 +1239,7 @@ LOG_STRUCTURE_FROM_GPS \ { LOG_RCOUT3_MSG, sizeof(log_RCOUT), \ "RCO3", "QHHHHHHHHHHHHHH", "TimeUS,C19,C20,C21,C22,C23,C24,C25,C26,C27,C28,C29,C30,C31,C32", "sYYYYYYYYYYYYYY", "F--------------", true }, \ { LOG_RSSI_MSG, sizeof(log_RSSI), \ - "RSSI", "Qff", "TimeUS,RXRSSI,RXLQ", "s--", "F--", true }, \ + "RSSI", "Qff", "TimeUS,RXRSSI,RXLQ", "s-%", "F--", true }, \ LOG_STRUCTURE_FROM_BARO \ LOG_STRUCTURE_FROM_PRECLAND \ { LOG_POWR_MSG, sizeof(log_POWR), \ @@ -1257,7 +1272,7 @@ LOG_STRUCTURE_FROM_MOUNT \ "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \ LOG_STRUCTURE_FROM_AVOIDANCE \ { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \ - "SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????", true }, \ + "SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU----", "FBBB0GG0000", true }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","QBLLHffHHf","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs", "s-DU-mm--m", "F-GG-00--0", true }, \ LOG_STRUCTURE_FROM_ESC_TELEM \ diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index ee01834cb19d6f..e2e804d0d21692 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -66,7 +66,9 @@ void AP_MSP_Telem_Backend::setup_wfq_scheduler(void) set_scheduler_entry(ATTITUDE, 200, 200); // 5Hz attitude set_scheduler_entry(ALTITUDE, 250, 250); // 4Hz altitude(cm) and velocity(cm/s) set_scheduler_entry(ANALOG, 250, 250); // 4Hz rssi + batt +#if AP_BATTERY_ENABLED set_scheduler_entry(BATTERY_STATE, 500, 500); // 2Hz battery +#endif #if HAL_WITH_ESC_TELEM set_scheduler_entry(ESC_SENSOR_DATA, 500, 500); // 2Hz ESC telemetry #endif @@ -114,7 +116,9 @@ bool AP_MSP_Telem_Backend::is_packet_ready(uint8_t idx, bool queue_empty) case ATTITUDE: // Attitude case ALTITUDE: // Altitude and Vario case ANALOG: // Rssi, Battery, mAh, Current +#if AP_BATTERY_ENABLED case BATTERY_STATE: // voltage, capacity, current, mAh +#endif #if HAL_WITH_ESC_TELEM case ESC_SENSOR_DATA: // esc temp + rpm #endif @@ -155,10 +159,12 @@ void AP_MSP_Telem_Backend::process_packet(uint8_t idx) _msp_port.c_state = MSP_IDLE; } +#if AP_BATTERY_ENABLED uint8_t AP_MSP_Telem_Backend::calc_cell_count(const float battery_voltage) { return floorf((battery_voltage / CELLFULL) + 1); } +#endif float AP_MSP_Telem_Backend::get_vspeed_ms(void) const { @@ -215,6 +221,7 @@ void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state) } } +#if AP_BATTERY_ENABLED void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state) { memset(&battery_state, 0, sizeof(battery_state)); @@ -241,6 +248,7 @@ void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state) battery_state.batt_cellcount = cc; } } +#endif // AP_BATTERY_ENABLED void AP_MSP_Telem_Backend::update_airspeed(airspeed_state_t &airspeed_state) { @@ -334,7 +342,9 @@ void AP_MSP_Telem_Backend::enable_warnings() return; } BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_FAIL_SAFE); +#if AP_BATTERY_ENABLED BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_BATTERY_CRITICAL); +#endif } void AP_MSP_Telem_Backend::process_incoming_data() @@ -466,8 +476,10 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_command(uint16_t cmd_msp, return msp_process_out_altitude(dst); case MSP_ANALOG: return msp_process_out_analog(dst); +#if AP_BATTERY_ENABLED case MSP_BATTERY_STATE: return msp_process_out_battery_state(dst); +#endif case MSP_UID: return msp_process_out_uid(dst); #if HAL_WITH_ESC_TELEM @@ -905,6 +917,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_altitude(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst) { +#if AP_BATTERY_ENABLED battery_state_t battery_state; update_battery_state(battery_state); @@ -922,10 +935,27 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst) current_ca : (int16_t)constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF), // current A to cA (0.01 steps, range is -320A to 320A) voltage_cv : (uint16_t)constrain_int32(battery_state.batt_voltage_v * 100,0,0xFFFF) // battery voltage in 0.01V steps }; +#else + float rssi; + const struct PACKED { + uint8_t voltage_dv; + uint16_t mah; + uint16_t rssi; + int16_t current_ca; + uint16_t voltage_cv; + } analog { + voltage_dv : 0, + mah : 0, + rssi : uint16_t(get_rssi(rssi) ? constrain_float(rssi,0,1) * 1023 : 0), // rssi 0-1 to 0-1023) + current_ca : 0, + voltage_cv : 0 + }; +#endif sbuf_write_data(dst, &analog, sizeof(analog)); return MSP_RESULT_ACK; } +#if AP_BATTERY_ENABLED MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst) { const AP_MSP *msp = AP::msp(); @@ -956,6 +986,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst sbuf_write_data(dst, &battery, sizeof(battery)); return MSP_RESULT_ACK; } +#endif MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *dst) { @@ -1104,7 +1135,9 @@ void AP_MSP_Telem_Backend::hide_osd_items(void) if (msp == nullptr) { return; } +#if AP_BATTERY_ENABLED const AP_Notify ¬ify = AP::notify(); +#endif // clear all and only set the flashing ones BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SATS); BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIR); @@ -1146,11 +1179,13 @@ void AP_MSP_Telem_Backend::hide_osd_items(void) if (msp->_msp_status.flight_mode_focus) { BIT_SET(osd_hidden_items_bitmask, OSD_CRAFT_NAME); } +#if AP_BATTERY_ENABLED // flash battery on failsafe if (notify.flags.failsafe_battery) { BIT_SET(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE); BIT_SET(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE); } +#endif // flash rtc if no time available #if AP_RTC_ENABLED uint64_t time_usec; @@ -1261,6 +1296,7 @@ bool AP_MSP_Telem_Backend::displaying_stats_screen() const bool AP_MSP_Telem_Backend::get_rssi(float &rssi) const { +#if AP_RSSI_ENABLED AP_RSSI* ap_rssi = AP::rssi(); if (ap_rssi == nullptr) { return false; @@ -1270,5 +1306,9 @@ bool AP_MSP_Telem_Backend::get_rssi(float &rssi) const } rssi = ap_rssi->read_receiver_rssi(); // range is [0-1] return true; +#else + return false; +#endif } + #endif //HAL_MSP_ENABLED diff --git a/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp index fcb3108c71609b..bafa921bd0263c 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp @@ -224,6 +224,7 @@ bool AP_MSP_Telem_DJI::get_rssi(float &rssi) const if (!displaying_stats_screen()) { return true; } +#if AP_RSSI_ENABLED AP_RSSI* ap_rssi = AP::rssi(); if (ap_rssi == nullptr) { return false; @@ -231,6 +232,9 @@ bool AP_MSP_Telem_DJI::get_rssi(float &rssi) const if (!ap_rssi->enabled()) { return false; } +#else + return false; +#endif AP_OSD *osd = AP::osd(); if (osd == nullptr) { return false; diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp index 074b3e06036e7e..fa907ec87c48ce 100644 --- a/libraries/AP_Math/SCurve.cpp +++ b/libraries/AP_Math/SCurve.cpp @@ -883,6 +883,7 @@ void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, fl // @Field: t4_out: segment duration // @Field: t6_out: segment duration +#if HAL_LOGGING_ENABLED static bool logged_scve; // only log once if (!logged_scve) { logged_scve = true; @@ -906,6 +907,8 @@ void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, fl (double)t6_out ); } +#endif // HAL_LOGGING_ENABLED + #endif // APM_BUILD_COPTER_OR_HELI Jm_out = 0.0f; diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 985a02c290baa4..de81543b485ccd 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -599,13 +599,19 @@ uint8_t parity(uint8_t byte) return p; } -// sums the bytes in the supplied buffer, returns that sum mod 256 -// (i.e. shoved into a uint8_t) -uint8_t crc_sum_of_bytes(uint8_t *data, uint16_t count) +// sums the bytes in the supplied buffer, returns that sum mod 0xFFFF +uint16_t crc_sum_of_bytes_16(const uint8_t *data, uint16_t count) { - uint8_t ret = 0; + uint16_t ret = 0; for (uint32_t i=0; i>= 3U; + uint64_t a_lo = (uint32_t)x; + uint64_t a_hi = x >> 32; + const uint64_t b_lo = 0xe353f7cfU; + const uint64_t b_hi = 0x20c49ba5U; + + uint64_t a_x_b_hi = a_hi * b_hi; + uint64_t a_x_b_mid = a_hi * b_lo; + uint64_t b_x_a_mid = b_hi * a_lo; + uint32_t a_x_b_lo = (a_lo * b_lo)>>32; + + // 64-bit product + two 32-bit values + uint64_t middle = a_x_b_mid + a_x_b_lo + (uint32_t)b_x_a_mid; + + // 64-bit product + two 32-bit values + uint64_t r = a_x_b_hi + (middle >> 32) + (b_x_a_mid >> 32); + return r >> 4U; +} diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index 0043a38c5b1b14..afd8a164c829f3 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/libraries/AP_Math/tests/test_math.cpp @@ -7,6 +7,7 @@ #include #include +#include const AP_HAL::HAL& hal = AP_HAL::get_HAL(); @@ -692,6 +693,17 @@ TEST(CRCTest, parity) EXPECT_EQ(parity(0b11111111), 0); } +TEST(MathTest, div1000) +{ + for (uint32_t i=0; i<1000000; i++) { + uint64_t v; + EXPECT_EQ(hal.util->get_random_vals((uint8_t*)&v, sizeof(v)), true); + uint64_t v1 = v / 1000ULL; + uint64_t v2 = uint64_div1000(v); + EXPECT_EQ(v1, v2); + } +} + AP_GTEST_PANIC() AP_GTEST_MAIN() diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 08f6337cef4d90..d4d3a1baa63cb2 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -281,6 +281,12 @@ class Vector3 return Vector3{x,y,z}; } + // convert from right-front-up to front-right-down + // or ENU to NED + Vector3 rfu_to_frd() const { + return Vector3{y,x,-z}; + } + // given a position p1 and a velocity v1 produce a vector // perpendicular to v1 maximising distance from p1. If p1 is the // zero vector the return from the function will always be the diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 3487d3a974fcaa..2fd101c8fab0bf 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -1,15 +1,20 @@ /// @file AP_Mission.cpp /// @brief Handles the MAVLINK command mission stack. Reads and writes mission to storage. +#include "AP_Mission_config.h" +#include +#include +#include + +#if AP_MISSION_ENABLED + #include "AP_Mission.h" #include -#include #include #include -#include -#include #include #include +#include const AP_Param::GroupInfo AP_Mission::var_info[] = { @@ -405,8 +410,10 @@ bool AP_Mission::start_command(const Mission_Command& cmd) } switch (cmd.id) { +#if AP_RC_CHANNEL_ENABLED case MAV_CMD_DO_AUX_FUNCTION: return start_command_do_aux_function(cmd); +#endif #if AP_GRIPPER_ENABLED case MAV_CMD_DO_GRIPPER: return start_command_do_gripper(cmd); @@ -721,6 +728,7 @@ struct PACKED Packed_Location_Option_Flags { uint8_t origin_alt : 1; // this altitude is above ekf origin uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location uint8_t type_specific_bit_0 : 1; // each mission item type can use this for storing 1 bit of extra data + uint8_t type_specific_bit_1 : 1; // each mission item type can use this for storing 1 bit of extra data }; struct PACKED PackedLocation { @@ -747,12 +755,12 @@ union PackedContent { }; -assert_storage_size assert_storage_size_PackedContent; - /// load_cmd_from_storage - load command from storage /// true is return if successful bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const { + ASSERT_STORAGE_SIZE(PackedContent, 12); + WITH_SEMAPHORE(_rsem); // special handling for command #0 which is home @@ -808,7 +816,10 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con cmd.content.location.lng = packed_content.location.lng; if (packed_content.location.flags.type_specific_bit_0) { - cmd.type_specific_bits = 1U << 0; + cmd.type_specific_bits |= 1U << 0; + } + if (packed_content.location.flags.type_specific_bit_1) { + cmd.type_specific_bits |= 1U << 1; } } else { // all other options in Content are assumed to be packed: @@ -825,6 +836,8 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con return true; } +#endif // AP_MISSION_ENABLED + bool AP_Mission::stored_in_location(uint16_t id) { switch (id) { @@ -851,6 +864,8 @@ bool AP_Mission::stored_in_location(uint16_t id) } } +#if AP_MISSION_ENABLED + /// write_cmd_to_storage - write a command to storage /// index is used to calculate the storage location /// true is returned if successful @@ -871,7 +886,8 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd packed.location.flags.terrain_alt = cmd.content.location.terrain_alt; packed.location.flags.origin_alt = cmd.content.location.origin_alt; packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack; - packed.location.flags.type_specific_bit_0 = cmd.type_specific_bits & (1U<<0); + packed.location.flags.type_specific_bit_0 = (cmd.type_specific_bits & (1U<<0)) >> 0; + packed.location.flags.type_specific_bit_1 = (cmd.type_specific_bits & (1U<<1)) >> 1; packed.location.alt = cmd.content.location.alt; packed.location.lat = cmd.content.location.lat; packed.location.lng = cmd.content.location.lng; @@ -923,6 +939,8 @@ void AP_Mission::write_home_to_storage() write_cmd_to_storage(0,home_cmd); } +#endif // AP_MISSION_ENABLED + MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_int_t& packet) { uint8_t nan_mask; @@ -970,6 +988,8 @@ MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_in // return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t& packet, AP_Mission::Mission_Command& cmd) { + cmd = {}; + // command's position in mission list and mavlink id cmd.index = packet.seq; cmd.id = packet.command; @@ -1016,18 +1036,25 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_NAV_LOITER_TURNS: { // MAV ID: 18 - // number of turns is stored in the lowest bits. radii below - // 255m are stored in the top 8 bits as an 8-bit integer. + // number of turns is stored in the lowest bits. Number of + // turns 0 < N < 1 are stored multiplied by 256 and a bit set + // in storage so that on retrieval they are divided by 256. + // Radii below 255m are stored in the top 8 bits as an 8-bit integer. // Radii above 255m are stored divided by 10 and a bit set in // storage so that on retrieval they are multiplied by 10 - cmd.p1 = MIN(255, packet.param1); // store number of times to circle in low p1 + float param1_stored = packet.param1; + if (param1_stored > 0 && param1_stored < 1) { + param1_stored *= 256.0; + cmd.type_specific_bits |= (1U << 1); + } + cmd.p1 = MIN(255, param1_stored); // store number of times to circle in low p1 uint8_t radius_m; const float abs_radius = fabsf(packet.param3); if (abs_radius <= 255) { radius_m = abs_radius; } else { radius_m = MIN(255, abs_radius * 0.1); - cmd.type_specific_bits = 1U << 0; + cmd.type_specific_bits |= (1U << 0); } cmd.p1 |= (radius_m<<8); // store radius in high byte of p1 cmd.content.location.loiter_ccw = (packet.param3 < 0); @@ -1483,6 +1510,8 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const ma return MAV_MISSION_ACCEPTED; } +#if AP_MISSION_ENABLED + // mission_cmd_to_mavlink_int - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS // return true on success, false on failure // NOTE: callers to this method current fill parts of "packet" in before calling this method, so do NOT attempt to zero the entire packet in here @@ -1535,6 +1564,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c if (cmd.type_specific_bits & (1U<<0)) { packet.param3 *= 10; } + if (cmd.type_specific_bits & (1U<<1)) { + packet.param1 /= 256.0; + } packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; @@ -2714,6 +2746,8 @@ bool AP_Mission::contains_item(MAV_CMD command) const return false; } +#endif // AP_MISSION_ENABLED + /* return true if the mission item has a location */ @@ -2723,6 +2757,8 @@ bool AP_Mission::cmd_has_location(const uint16_t command) return stored_in_location(command); } +#if AP_MISSION_ENABLED + /* return true if the mission has a terrain relative item. ~2200us for 530 items on H7 */ @@ -2898,3 +2934,5 @@ AP_Mission *mission() } } + +#endif // AP_MISSION_ENABLED diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index b50fa4d9822b0b..c7204b315bacb6 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -415,6 +415,19 @@ class AP_Mission // comparison operator (relies on all bytes in the structure even if they may not be used) bool operator ==(const Mission_Command &b) const { return (memcmp(this, &b, sizeof(Mission_Command)) == 0); } bool operator !=(const Mission_Command &b) const { return !operator==(b); } + + /* + return the number of turns for a LOITER_TURNS command + this has special handling for loiter turns from cmd.p1 and type_specific_bits + */ + float get_loiter_turns(void) const { + float turns = LOWBYTE(p1); + if (type_specific_bits & (1U<<1)) { + // special storage handling allows for fractional turns + turns *= (1.0/256.0); + } + return turns; + } }; diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 83716ec71849cd..21d6731b733b07 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -1,3 +1,7 @@ +#include "AP_Mission_config.h" + +#if AP_MISSION_ENABLED + #include "AP_Mission.h" #include @@ -10,6 +14,7 @@ #include #include +#if AP_RC_CHANNEL_ENABLED bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd) { const RC_Channel::AUX_FUNC function = (RC_Channel::AUX_FUNC)cmd.content.auxfunction.function; @@ -28,6 +33,7 @@ bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command rc().run_aux_function(function, pos, RC_Channel::AuxFuncTriggerSource::MISSION); return true; } +#endif // AP_RC_CHANNEL_ENABLED #if AP_GRIPPER_ENABLED bool AP_Mission::start_command_do_gripper(const AP_Mission::Mission_Command& cmd) @@ -330,3 +336,5 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss // if we got this far then message is not handled return false; } + +#endif // AP_MISSION_ENABLED diff --git a/libraries/AP_Module/AP_Module.cpp b/libraries/AP_Module/AP_Module.cpp index 6d740ecce59a83..3bad1d59ef852c 100644 --- a/libraries/AP_Module/AP_Module.cpp +++ b/libraries/AP_Module/AP_Module.cpp @@ -165,9 +165,9 @@ void AP_Module::call_hook_AHRS_update(const AP_AHRS &ahrs) state.quat[2] = q[2]; state.quat[3] = q[3]; - state.eulers[0] = ahrs.roll; - state.eulers[1] = ahrs.pitch; - state.eulers[2] = ahrs.yaw; + state.eulers[0] = ahrs.get_roll(); + state.eulers[1] = ahrs.get_pitch(); + state.eulers[2] = ahrs.get_yaw(); Location loc; if (ahrs.get_origin(loc)) { diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp index 951ecabbcbb7bd..239544f9685bd6 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.cpp +++ b/libraries/AP_Motors/AP_Motors6DOF.cpp @@ -359,6 +359,7 @@ void AP_Motors6DOF::output_armed_stabilizing() } } +#if AP_BATTERY_ENABLED const AP_BattMonitor &battery = AP::battery(); // Current limiting @@ -384,6 +385,7 @@ void AP_Motors6DOF::output_armed_stabilizing() batt_current_ratio = predicted_current_ratio; } _output_limited += (_dt / (_dt + _batt_current_time_constant)) * (1 - batt_current_ratio); +#endif _output_limited = constrain_float(_output_limited, 0.0f, 1.0f); diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 52176330bac8b3..fc93264ed5d7f9 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -190,7 +190,7 @@ void AP_MotorsHeli::output_min() // move swash to mid move_actuators(0.0f,0.0f,0.5f,0.0f); - update_motor_control(ROTOR_CONTROL_STOP); + update_motor_control(AP_MotorsHeli_RSC::RotorControlState::STOP); // override limits flags set_limit_flag_pitch_roll_yaw(true); @@ -590,3 +590,26 @@ void AP_MotorsHeli::set_desired_rotor_speed(float desired_speed) { _main_rotor.set_desired_speed(desired_speed); } + +// Converts AP_Motors::SpoolState from _spool_state variable to AP_MotorsHeli_RSC::RotorControlState +AP_MotorsHeli_RSC::RotorControlState AP_MotorsHeli::get_rotor_control_state() const +{ + switch (_spool_state) { + case SpoolState::SHUT_DOWN: + // sends minimum values out to the motors + return AP_MotorsHeli_RSC::RotorControlState::STOP; + case SpoolState::GROUND_IDLE: + // sends idle output to motors when armed. rotor could be static or turning (autorotation) + return AP_MotorsHeli_RSC::RotorControlState::IDLE; + case SpoolState::SPOOLING_UP: + case SpoolState::THROTTLE_UNLIMITED: + // set motor output based on thrust requests + return AP_MotorsHeli_RSC::RotorControlState::ACTIVE; + case SpoolState::SPOOLING_DOWN: + // sends idle output to motors and wait for rotor to stop + return AP_MotorsHeli_RSC::RotorControlState::IDLE; + } + + // Should be unreachable, but needed to keep the compiler happy + return AP_MotorsHeli_RSC::RotorControlState::STOP; +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 7cf311a84ca50d..f2a8ce6100400b 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -189,7 +189,10 @@ class AP_MotorsHeli : public AP_Motors { AP_MotorsHeli_RSC _main_rotor; // main rotor // update_motor_controls - sends commands to motor controllers - virtual void update_motor_control(RotorControlState state) = 0; + virtual void update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) = 0; + + // Converts AP_Motors::SpoolState from _spool_state variable to AP_MotorsHeli_RSC::RotorControlState + AP_MotorsHeli_RSC::RotorControlState get_rotor_control_state() const; // run spool logic void output_logic(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 06abef19c2f76b..ddbcedc896e44f 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -360,12 +360,12 @@ void AP_MotorsHeli_Dual::mix_intermeshing(float pitch_input, float roll_input, f } // update_motor_controls - sends commands to motor controllers -void AP_MotorsHeli_Dual::update_motor_control(RotorControlState state) +void AP_MotorsHeli_Dual::update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) { // Send state update to motors _main_rotor.output(state); - if (state == ROTOR_CONTROL_STOP) { + if (state == AP_MotorsHeli_RSC::RotorControlState::STOP) { // set engine run enable aux output to not run position to kill engine when disarmed SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MIN); } else { @@ -525,25 +525,8 @@ void AP_MotorsHeli_Dual::output_to_motors() _swashplate1.output(); _swashplate2.output(); - switch (_spool_state) { - case SpoolState::SHUT_DOWN: - // sends minimum values out to the motors - update_motor_control(ROTOR_CONTROL_STOP); - break; - case SpoolState::GROUND_IDLE: - // sends idle output to motors when armed. rotor could be static or turning (autorotation) - update_motor_control(ROTOR_CONTROL_IDLE); - break; - case SpoolState::SPOOLING_UP: - case SpoolState::THROTTLE_UNLIMITED: - // set motor output based on thrust requests - update_motor_control(ROTOR_CONTROL_ACTIVE); - break; - case SpoolState::SPOOLING_DOWN: - // sends idle output to motors and wait for rotor to stop - update_motor_control(ROTOR_CONTROL_IDLE); - break; - } + update_motor_control(get_rotor_control_state()); + } // servo_test - move servos through full range of movement diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 22b2d2502fff4e..46672faddb3004 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -65,7 +65,7 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli { void init_outputs () override; // update_motor_controls - sends commands to motor controllers - void update_motor_control(RotorControlState state) override; + void update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) override; // get_swashplate - calculate movement of each swashplate based on configuration float get_swashplate(int8_t swash_num, int8_t swash_axis, float pitch_input, float roll_input, float yaw_input, float coll_input); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp index 2b0fd53083c23b..515ec063e0f2c0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.cpp @@ -145,12 +145,12 @@ void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors() } // update_motor_controls - sends commands to motor controllers -void AP_MotorsHeli_Quad::update_motor_control(RotorControlState state) +void AP_MotorsHeli_Quad::update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) { // Send state update to motors _main_rotor.output(state); - if (state == ROTOR_CONTROL_STOP) { + if (state == AP_MotorsHeli_RSC::RotorControlState::STOP) { // set engine run enable aux output to not run position to kill engine when disarmed SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MIN); } else { @@ -273,25 +273,8 @@ void AP_MotorsHeli_Quad::output_to_motors() rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE); } - switch (_spool_state) { - case SpoolState::SHUT_DOWN: - // sends minimum values out to the motors - update_motor_control(ROTOR_CONTROL_STOP); - break; - case SpoolState::GROUND_IDLE: - // sends idle output to motors when armed. rotor could be static or turning (autorotation) - update_motor_control(ROTOR_CONTROL_IDLE); - break; - case SpoolState::SPOOLING_UP: - case SpoolState::THROTTLE_UNLIMITED: - // set motor output based on thrust requests - update_motor_control(ROTOR_CONTROL_ACTIVE); - break; - case SpoolState::SPOOLING_DOWN: - // sends idle output to motors and wait for rotor to stop - update_motor_control(ROTOR_CONTROL_IDLE); - break; - } + update_motor_control(get_rotor_control_state()); + } // servo_test - move servos through full range of movement diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.h b/libraries/AP_Motors/AP_MotorsHeli_Quad.h index 3644255b1cb4b7..a67cb409116eb5 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.h @@ -49,7 +49,7 @@ class AP_MotorsHeli_Quad : public AP_MotorsHeli { void init_outputs () override; // update_motor_controls - sends commands to motor controllers - void update_motor_control(RotorControlState state) override; + void update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) override; // calculate_roll_pitch_collective_factors - setup rate factors void calculate_roll_pitch_collective_factors (); diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 57a3046fc82198..a54c583d6a0da7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -275,7 +275,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) } switch (state) { - case ROTOR_CONTROL_STOP: + case RotorControlState::STOP: // set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp() update_rotor_ramp(0.0f, dt); @@ -296,7 +296,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) _idle_throttle = get_idle_output(); break; - case ROTOR_CONTROL_IDLE: + case RotorControlState::IDLE: // set rotor ramp to decrease speed to zero update_rotor_ramp(0.0f, dt); @@ -313,12 +313,12 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) _idle_throttle = constrain_float( get_arot_idle_output(), 0.0f, 0.4f); } if (!_autorotating) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Autorotation"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Autorotation"); _autorotating =true; } } else { if (_autorotating) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "Autorotation Stopped"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Autorotation Stopped"); _autorotating =false; } // set rotor control speed to idle speed parameter, this happens instantly and ignores ramping @@ -326,7 +326,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) _idle_throttle += 0.001f; if (_control_output >= 1.0f) { _idle_throttle = get_idle_output(); - gcs().send_text(MAV_SEVERITY_INFO, "Turbine startup"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Turbine startup"); _starting = false; } } else { @@ -348,7 +348,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state) _control_output = _idle_throttle; break; - case ROTOR_CONTROL_ACTIVE: + case RotorControlState::ACTIVE: // set main rotor ramp to increase to full speed update_rotor_ramp(1.0f, dt); @@ -408,7 +408,7 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt) if (_rotor_ramp_output < rotor_ramp_input) { if (_use_bailout_ramp) { if (!_bailing_out) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "bailing_out"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "bailing_out"); _bailing_out = true; if (_control_mode == ROTOR_CONTROL_MODE_AUTOTHROTTLE) {_gov_bailing_out = true;} } @@ -563,9 +563,9 @@ void AP_MotorsHeli_RSC::autothrottle_run() governor_reset(); _governor_fault = true; if (_rotor_rpm >= (_governor_rpm + _governor_range)) { - gcs().send_text(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed"); } else { - gcs().send_text(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Underspeed"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Underspeed"); } } } else { @@ -577,7 +577,7 @@ void AP_MotorsHeli_RSC::autothrottle_run() if (_rotor_rpm > (_governor_rpm + _governor_range) && !_gov_bailing_out) { _governor_fault = true; governor_reset(); - gcs().send_text(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Governor Fault: Rotor Overspeed"); _governor_output = 0.0f; // when performing power recovery from autorotation, this waits for user to load rotor in order to @@ -594,7 +594,7 @@ void AP_MotorsHeli_RSC::autothrottle_run() _governor_engage = true; _autothrottle = true; _gov_bailing_out = false; - gcs().send_text(MAV_SEVERITY_NOTICE, "Governor Engaged"); + GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Governor Engaged"); } } else { // temporary use of throttle curve and ramp timer to accelerate rotor to governor min torque rise speed diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 5d06b37b43a2ea..276728ca4d1dc3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -30,13 +30,6 @@ // RSC governor defaults #define AP_MOTORS_HELI_RSC_GOVERNOR_RANGE_DEFAULT 100 -// rotor controller states -enum RotorControlState { - ROTOR_CONTROL_STOP = 0, - ROTOR_CONTROL_IDLE, - ROTOR_CONTROL_ACTIVE -}; - // rotor control modes enum RotorControlMode { ROTOR_CONTROL_MODE_DISABLED = 0, @@ -60,6 +53,13 @@ class AP_MotorsHeli_RSC { AP_Param::setup_object_defaults(this, var_info); }; + // rotor controller states + enum class RotorControlState { + STOP = 0, + IDLE, + ACTIVE + }; + // init_servo - servo initialization on start-up void init_servo(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index fe13951f4d1dac..1c73e10f5082ac 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -28,16 +28,16 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Param: TAIL_TYPE // @DisplayName: Tail Type - // @Description: Tail type selection. Simpler yaw controller used if external gyro is selected. Direct Drive Variable Pitch is used for tails that have a motor that is governed at constant speed by an ESC. Tail pitch is still accomplished with a servo. Direct Drive Fixed Pitch (DDFP) CW is used for helicopters with a rotor that spins clockwise when viewed from above. Direct Drive Fixed Pitch (DDFP) CCW is used for helicopters with a rotor that spins counter clockwise when viewed from above. In both DDFP cases, no servo is used for the tail and the tail motor esc is controlled by the yaw axis. - // @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch CW,4:DirectDrive FixedPitch CCW,5:DDVP with external governor + // @Description: Tail type selection. Servo Only uses tail rotor pitch to provide yaw control (including stabilization) via an output assigned to Motor4. Servo with External Gyro uses an external gyro to control tail rotor pitch via a servo. Yaw control without stabilization is passed to the external gyro via the output assigned to Motor4. Direct Drive Variable Pitch(DDVP) is used for tails that have a motor whose ESC is connected to an output with function HeliTailRSC. Tail pitch is still accomplished with a servo on an output assigned to Motor4 function. Direct Drive Fixed Pitch (DDFP) CW is used for helicopters with a rotor that spins clockwise when viewed from above with a motor whose ESC is controlled by an output whose function is Motor4. Direct Drive Fixed Pitch (DDFP) CCW is used for helicopters with a rotor that spins counter clockwise when viewed from above with a motor whose ESC is controlled by an output whose function is Motor4. In both DDFP cases, no servo is used for the tail and the tail motor esc on Motor4 output is used to control the yaw axis using motor speed. + // @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch CW,4:DirectDrive FixedPitch CCW // @User: Standard - AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO), + AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, float(TAIL_TYPE::SERVO)), // Indice 5 was used by SWASH_TYPE and should not be used // @Param: GYR_GAIN // @DisplayName: External Gyro Gain - // @Description: PWM in microseconds sent to external gyro on ch7 when tail type is Servo w/ ExtGyro + // @Description: PWM in microseconds sent to external gyro on an servo/output whose function is Motor7 when tail type is Servo w/ ExtGyro // @Range: 0 1000 // @Units: PWM // @Increment: 1 @@ -57,7 +57,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Param: TAIL_SPEED // @DisplayName: DDVP Tail ESC speed - // @Description: Direct drive, variable pitch tail ESC speed in percent output to the tail motor esc (HeliTailRSC Servo) when motor interlock enabled (throttle hold off). + // @Description: Direct drive, variable pitch tail ESC speed in percent output to the tail motor esc (HeliTailRSC Servo) when motor interlock enabled (throttle hold off) and speed fully ramped up after spoolup. // @Range: 0 100 // @Units: % // @Increment: 1 @@ -66,7 +66,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Param: GYR_GAIN_ACRO // @DisplayName: ACRO External Gyro Gain - // @Description: PWM in microseconds sent to external gyro on ch7 when tail type is Servo w/ ExtGyro. A value of zero means to use H_GYR_GAIN + // @Description: PWM in microseconds sent to external gyro on an servo/output whose function is Motor7 when tail type is Servo w/ ExtGyro in mode ACRO instead of H_GYR_GAIN. A value of zero means to use H_GYR_GAIN // @Range: 0 1000 // @Units: PWM // @Increment: 1 @@ -210,25 +210,33 @@ void AP_MotorsHeli_Single::init_outputs() // initialize main rotor servo _main_rotor.init_servo(); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { - _tail_rotor.init_servo(); - } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // external gyro output - add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO); - } - } + switch (get_tail_type()) { + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW: + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW: + // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation. + SRV_Channels::set_range(SRV_Channel::k_motor4, 1.0f); + break; - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // External Gyro uses PWM output thus servo endpoints are forced - SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000); - } + case TAIL_TYPE::DIRECTDRIVE_VARPITCH: + case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV: + _tail_rotor.init_servo(); + break; + + case TAIL_TYPE::SERVO_EXTGYRO: + // external gyro output + add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO); + + + // External Gyro uses PWM output thus servo endpoints are forced + SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000); + FALLTHROUGH; - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation. - SRV_Channels::set_range(SRV_Channel::k_motor4, 1.0f); - } else if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { - // yaw servo is an angle from -4500 to 4500 - SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE); + case TAIL_TYPE::SERVO: + default: + // yaw servo is an angle from -4500 to 4500 + SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE); + break; + } } set_initialised_ok(_frame_class == MOTOR_FRAME_HELI); @@ -266,13 +274,13 @@ void AP_MotorsHeli_Single::calculate_armed_scalars() _main_rotor.set_autorotation_flag(_heliflags.in_autorotation); // set bailout ramp time _main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_autorotation_flag(_heliflags.in_autorotation); _tail_rotor.use_bailout_ramp_time(_heliflags.enable_bailout); } } else { _main_rotor.set_autorotation_flag(false); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_autorotation_flag(false); } } @@ -310,7 +318,7 @@ void AP_MotorsHeli_Single::calculate_scalars() calculate_armed_scalars(); // send setpoints to DDVP rotor controller and trigger recalculation of scalars - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) { + if (use_tail_RSC()) { _tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SETPOINT); _tail_rotor.set_ramp_time(_main_rotor._ramp_time.get()); _tail_rotor.set_runup_time(_main_rotor._runup_time.get()); @@ -339,13 +347,13 @@ uint32_t AP_MotorsHeli_Single::get_motor_mask() } // update_motor_controls - sends commands to motor controllers -void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) +void AP_MotorsHeli_Single::update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) { // Send state update to motors _tail_rotor.output(state); _main_rotor.output(state); - if (state == ROTOR_CONTROL_STOP){ + if (state == AP_MotorsHeli_RSC::RotorControlState::STOP){ // set engine run enable aux output to not run position to kill engine when disarmed SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MIN); } else { @@ -370,8 +378,6 @@ void AP_MotorsHeli_Single::update_motor_control(RotorControlState state) // void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) { - float yaw_offset = 0.0f; - // initialize limits flag limit.throttle_lower = false; limit.throttle_upper = false; @@ -422,26 +428,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float // updates takeoff collective flag based on 50% hover collective update_takeoff_collective_flag(collective_out); - // if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors - if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) { - // rudder feed forward based on collective - // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque - // also not required if we are using external gyro - if ((get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // sanity check collective_yaw_scale - _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); - // This feedforward compensation follows the hover performance theory that hover power required - // is a function of gross weight to the 3/2 power - yaw_offset = _collective_yaw_scale * powf(fabsf(collective_out - _collective_zero_thrust_pct),1.5f); - - // Add yaw trim for DDFP tails - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - yaw_offset += _yaw_trim.get(); - } - } - } else { - yaw_offset = 0.0f; - } + // Get yaw offset required to cancel out steady state main rotor torque + const float yaw_offset = get_yaw_offset(collective_out); // feed power estimate into main rotor controller // ToDo: include tail rotor power? @@ -475,72 +463,92 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out) _servo4_out = yaw_out; } -void AP_MotorsHeli_Single::output_to_motors() +// Get yaw offset required to cancel out steady state main rotor torque +float AP_MotorsHeli_Single::get_yaw_offset(float collective) { - if (!initialised_ok()) { - return; + if ((get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO) || (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED)) { + // Not in direct control of tail with external gyro or manual servo mode + return 0.0; } - // Write swashplate outputs - _swashplate.output(); - - if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE); - } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { - // output gain to exernal gyro - if (_acro_tail && _ext_gyro_gain_acro > 0) { - rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro); - } else { - rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std); - } + if (_heliflags.in_autorotation || (get_control_output() <= _main_rotor.get_idle_output())) { + // Motor is stopped or at idle, and thus not creating torque + return 0.0; } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - _servo4_out = -_servo4_out; + // sanity check collective_yaw_scale + _collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE)); + + // This feedforward compensation follows the hover performance theory that hover power required + // is a function of gross weight to the 3/2 power + float yaw_offset = _collective_yaw_scale * powf(fabsf(collective - _collective_zero_thrust_pct),1.5f); + + // Add yaw trim for DDFP tails + if (have_DDFP_tail()) { + yaw_offset += _yaw_trim.get(); } - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // calc filtered battery voltage and lift_max - thr_lin.update_lift_max_from_batt_voltage(); + return yaw_offset; +} + +void AP_MotorsHeli_Single::output_to_motors() +{ + if (!initialised_ok()) { + return; } - // Warning: This spool state logic is what prevents DDFP tails from actuating when doing H_SV_MAN and H_SV_TEST tests - switch (_spool_state) { - case SpoolState::SHUT_DOWN: - // sends minimum values out to the motors - update_motor_control(ROTOR_CONTROL_STOP); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Set DDFP to servo min - output_to_ddfp_tail(0.0); - } - break; - case SpoolState::GROUND_IDLE: - // sends idle output to motors when armed. rotor could be static or turning (autorotation) - update_motor_control(ROTOR_CONTROL_IDLE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Set DDFP to servo min - output_to_ddfp_tail(0.0); - } - break; - case SpoolState::SPOOLING_UP: - case SpoolState::THROTTLE_UNLIMITED: - // set motor output based on thrust requests - update_motor_control(ROTOR_CONTROL_ACTIVE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) { - // Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation - output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out)); + // Write swashplate outputs + _swashplate.output(); + + // Output main rotor + update_motor_control(get_rotor_control_state()); + + // Output tail rotor + switch (get_tail_type()) { + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW: + // Invert output for CCW tail + _servo4_out *= -1.0; + FALLTHROUGH; + + case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW: { + // calc filtered battery voltage and lift_max + thr_lin.update_lift_max_from_batt_voltage(); + + // Only throttle up if in active spool state + switch (_spool_state) { + case AP_Motors::SpoolState::SHUT_DOWN: + case AP_Motors::SpoolState::GROUND_IDLE: + case AP_Motors::SpoolState::SPOOLING_DOWN: + // Set DDFP to servo min + output_to_ddfp_tail(0.0); + break; + + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: + // Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation + output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out)); + break; } break; - case SpoolState::SPOOLING_DOWN: - // sends idle output to motors and wait for rotor to stop - update_motor_control(ROTOR_CONTROL_IDLE); - if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){ - // Set DDFP to servo min - output_to_ddfp_tail(0.0); + } + + case TAIL_TYPE::SERVO_EXTGYRO: + // output gain to external gyro + if (_acro_tail && _ext_gyro_gain_acro > 0) { + rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro); + } else { + rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std); } + FALLTHROUGH; + + case TAIL_TYPE::SERVO: + case TAIL_TYPE::DIRECTDRIVE_VARPITCH: + case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV: + default: + rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE); break; } + } // handle output limit flags and send throttle to servos lib @@ -655,9 +663,7 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void) // Previous DDFP configs used servo trim for setting the yaw trim, which no longer works with thrust linearisation. Convert servo trim // to H_YAW_TRIM. Default thrust linearisation gives linear thrust to throttle relationship to preserve previous setup behaviours so // we can assume linear relationship in the conversion. - if ((_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || - _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) && - !_yaw_trim.configured()) { + if (have_DDFP_tail() && !_yaw_trim.configured()) { SRV_Channel *c = SRV_Channels::get_channel_for(SRV_Channel::k_motor4); if (c != nullptr) { @@ -673,3 +679,19 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void) _yaw_trim.save(); } } + +// Helper to return true for direct drive fixed pitch tail, either CW or CCW +bool AP_MotorsHeli_Single::have_DDFP_tail() const +{ + const TAIL_TYPE type = get_tail_type(); + return (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW) || + (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW); +} + +// Helper to return true if the tail RSC should be used +bool AP_MotorsHeli_Single::use_tail_RSC() const +{ + const TAIL_TYPE type = get_tail_type(); + return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) || + (type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 595ce98a04f90e..61e6588a5ab9fb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -14,15 +14,6 @@ #define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7 #define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7 -// tail types -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO 0 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO 1 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH 2 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW 3 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW 4 -#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV 5 - - // direct-drive variable pitch defaults #define AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT 50 @@ -72,8 +63,8 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { // has_flybar - returns true if we have a mechical flybar bool has_flybar() const override { return _flybar_mode; } - // supports_yaw_passthrought - returns true if we support yaw passthrough - bool supports_yaw_passthrough() const override { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; } + // supports_yaw_passthrough - returns true if we support yaw passthrough + bool supports_yaw_passthrough() const override { return get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO; } void set_acro_tail(bool set) override { _acro_tail = set; } @@ -95,7 +86,7 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { void init_outputs() override; // update_motor_controls - sends commands to motor controllers - void update_motor_control(RotorControlState state) override; + void update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) override; // heli_move_actuators - moves swash plate and tail rotor void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override; @@ -103,12 +94,33 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { // move_yaw - moves the yaw servo void move_yaw(float yaw_out); + // Get yaw offset required to cancel out steady state main rotor torque + float get_yaw_offset(float collective); + // handle output limit flags and send throttle to servos lib void output_to_ddfp_tail(float throttle); // servo_test - move servos through full range of movement void servo_test() override; + // Tail types + enum class TAIL_TYPE { + SERVO = 0, + SERVO_EXTGYRO = 1, + DIRECTDRIVE_VARPITCH = 2, + DIRECTDRIVE_FIXEDPITCH_CW = 3, + DIRECTDRIVE_FIXEDPITCH_CCW = 4, + DIRECTDRIVE_VARPIT_EXT_GOV = 5 + }; + + TAIL_TYPE get_tail_type() const { return TAIL_TYPE(_tail_type.get()); } + + // Helper to return true for direct drive fixed pitch tail, either CW or CCW + bool have_DDFP_tail() const; + + // Helper to return true if the tail RSC should be used + bool use_tail_RSC() const; + // external objects we depend upon AP_MotorsHeli_RSC _tail_rotor; // tail rotor AP_MotorsHeli_Swash _swashplate; // swashplate diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 5359ad16e70409..e68cf85c8208ab 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -345,6 +345,7 @@ void AP_MotorsMulticopter::update_throttle_filter() // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max float AP_MotorsMulticopter::get_current_limit_max_throttle() { +#if AP_BATTERY_ENABLED AP_BattMonitor &battery = AP::battery(); const uint8_t batt_idx = thr_lin.get_battery_index(); @@ -376,8 +377,12 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() // limit max throttle return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit); +#else + return 1.0; +#endif } +#if HAL_LOGGING_ENABLED // 10hz logging of voltage scaling and max trust void AP_MotorsMulticopter::Log_Write() { @@ -393,6 +398,7 @@ void AP_MotorsMulticopter::Log_Write() }; AP::logger().WriteBlock(&pkt_mot, sizeof(pkt_mot)); } +#endif // convert actuator output (0~1) range to pwm range int16_t AP_MotorsMulticopter::output_to_pwm(float actuator) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index f6e07e204a74c4..3f97e5105a3bda 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -89,8 +89,10 @@ class AP_MotorsMulticopter : public AP_Motors { // convert values to PWM min and max if not configured void convert_pwm_min_max_param(int16_t radio_min, int16_t radio_max); +#if HAL_LOGGING_ENABLED // 10hz logging of voltage scaling and max trust void Log_Write() override; +#endif // Run arming checks bool arming_checks(size_t buflen, char *buffer) const override; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index b8b39fbfb55665..92c62bf0540773 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -5,6 +5,7 @@ #include // filter library #include #include +#include // offsets for motors in motor_out and _motor_filtered arrays #define AP_MOTORS_MOT_1 0U @@ -275,8 +276,10 @@ class AP_Motors { void set_frame_string(const char * str); #endif +#if HAL_LOGGING_ENABLED // write log, to be called at 10hz virtual void Log_Write() {}; +#endif enum MotorOptions : uint8_t { BATT_RAW_VOLTAGE = (1 << 0U) diff --git a/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp b/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp index 92d4f800c9b676..ff2114864a4556 100644 --- a/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp +++ b/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp @@ -151,6 +151,7 @@ float Thrust_Linearization::remove_thrust_curve_and_volt_scaling(float throttle) // update_lift_max from battery voltage - used for voltage compensation void Thrust_Linearization::update_lift_max_from_batt_voltage() { +#if AP_BATTERY_ENABLED // sanity check battery_voltage_min is not too small // if disabled or misconfigured exit immediately float _batt_voltage = motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE) ? AP::battery().voltage(batt_idx) : AP::battery().voltage_resting_estimate(batt_idx); @@ -177,6 +178,7 @@ void Thrust_Linearization::update_lift_max_from_batt_voltage() // calculate lift max float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0); lift_max = batt_voltage_filt.get() * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt.get() * batt_voltage_filt.get(); +#endif } // return gain scheduling gain based on voltage and air density diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index d65c940cd86f89..491bb0fc60bff6 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -38,6 +38,7 @@ SRV_Channels srvs; AP_BattMonitor _battmonitor{0, nullptr, nullptr}; AP_Motors *motors; +AP_MotorsHeli *motors_heli; AP_MotorsMatrix *motors_matrix; bool thrust_boost = false; @@ -130,6 +131,30 @@ void setup() AP_Int8 *dual_mode = (AP_Int8*)motors + AP_MotorsHeli_Dual::var_info[1].offset; dual_mode->set(value); + } else if (strcmp(cmd,"tail_type") == 0) { + if (frame_class != AP_Motors::MOTOR_FRAME_HELI) { + ::printf("tail_type only supported by single heli frame type (%i), got %i\n", AP_Motors::MOTOR_FRAME_HELI, frame_class); + exit(1); + } + + // Union allows pointers to be aligned despite different sizes + // avoids "increases required alignment of target type" error when casting from char* to AP_Int16* + union { + char *char_ptr; + AP_Int16 *int16; + } tail_type; + + // look away now, more dodgy param access. + tail_type.char_ptr = (char*)motors + AP_MotorsHeli_Single::var_info[1].offset; + tail_type.int16->set(value); + + // Re-init motors to switch to the new tail type + // Have to do this twice to make sure the tail type sticks + motors->set_initialised_ok(false); + motors->init(frame_class, AP_Motors::MOTOR_FRAME_TYPE_X); + motors->set_initialised_ok(false); + motors->init(frame_class, AP_Motors::MOTOR_FRAME_TYPE_X); + } else if (strcmp(cmd,"frame_class") == 0) { // We must have the frame_class argument 2nd as resulting class is used to determine if // we have access to certain functions in the multicopter motors child class @@ -151,19 +176,27 @@ void setup() break; case AP_Motors::MOTOR_FRAME_HELI: - motors = new AP_MotorsHeli_Single(400); - // Mot 1-3 swashplate, mot 4 tail rotor pitch, mot 5 for 4th servo in H4-90 swash - num_outputs = 5; + motors_heli = new AP_MotorsHeli_Single(400); + motors = motors_heli; + // Mot 1-3: Swash plate 1 to 3 + // Mot 4: Tail rotor + // Mot 5: 4th servo in H4-90 swash + // Mot 6: Unused + // Mot 7: Tail rotor RSC / external governor output + // Mot 8: Main rotor RSC + num_outputs = 8; break; case AP_Motors::MOTOR_FRAME_HELI_DUAL: - motors = new AP_MotorsHeli_Dual(400); + motors_heli = new AP_MotorsHeli_Dual(400); + motors = motors_heli; // Mot 1-3 swashplate 1, mot 4-6 swashplate 2, mot 7 and 8 for 4th servos on H4-90 swash plates front and back, respectively num_outputs = 8; break; case AP_Motors::MOTOR_FRAME_HELI_QUAD: - motors = new AP_MotorsHeli_Quad(400); + motors_heli = new AP_MotorsHeli_Quad(400); + motors = motors_heli; num_outputs = 4; // Only 4 collective servos break; @@ -186,6 +219,29 @@ void setup() exit(1); } + } else if (strcmp(cmd,"COL2YAW") == 0) { + if (frame_class != AP_Motors::MOTOR_FRAME_HELI) { + ::printf("COL2YAW only supported by single heli frame type (%i), got %i\n", AP_Motors::MOTOR_FRAME_HELI, frame_class); + exit(1); + } + + // Union allows pointers to be aligned despite different sizes + // avoids "increases required alignment of target type" error when casting from char* to AP_Int16* + union { + char *char_ptr; + AP_Float *ap_float; + } collective_yaw_scale; + + collective_yaw_scale.char_ptr = (char*)motors + AP_MotorsHeli_Single::var_info[7].offset; + collective_yaw_scale.ap_float->set(value); + + } else if (strcmp(cmd,"autorotation") == 0) { + if (motors_heli == nullptr) { + ::printf("autorotation only supported by heli frame types, got %i\n", frame_class); + exit(1); + } + motors_heli->set_in_autorotation(!is_zero(value)); + } else { ::printf("Expected \"frame_class\", \"yaw_headroom\" or \"throttle_avg_max\"\n"); exit(1); diff --git a/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py b/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py index 118a5de193fcb0..7136c65fef806d 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py +++ b/libraries/AP_Motors/examples/AP_Motors_test/run_heli_comparison.py @@ -100,8 +100,15 @@ def get_fields(self): 1: 'Transverse', 2: 'Intermeshing_or_Coaxial'} +tail_type_lookup = {0: 'Servo_only', + 1: 'Servo_with_ExtGyro', + 2: 'DirectDrive_VarPitch', + 3: 'DirectDrive_FixedPitch_CW', + 4: 'DirectDrive_FixedPitch_CCW', + 5: 'DDVP_with_external_governor'} + # Run sweep over range of types -def run_sweep(frame_class, swash_type, dual_mode, dir_name): +def run_sweep(frame_class, swash_type, secondary_iter, secondary_lookup, secondary_name, dir_name): # configure and build the test os.system('./waf configure --board linux') @@ -110,28 +117,28 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): # Run sweep for fc in frame_class: for swash in swash_type: - for mode in dual_mode: + for sec in secondary_iter: if swash is not None: swash_cmd = 'swash=%d' % swash - if mode is not None: - name = 'frame class = %s (%i), swash = %s (%i), dual mode = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash, dual_mode_lookup[mode], mode) - filename = '%s_%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash], dual_mode_lookup[mode]) - mode_cmd = 'dual_mode=%d' % mode + if sec is not None: + name = 'frame class = %s (%i), swash = %s (%i), %s = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash, secondary_name.replace('_', ' '), secondary_lookup[sec], sec) + filename = '%s_%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash], secondary_lookup[sec]) + sec_cmd = '%s=%d' % (secondary_name, sec) else: name = 'frame class = %s (%i), swash = %s (%i)' % (frame_class_lookup[fc], fc, swash_type_lookup[swash], swash) filename = '%s_%s_motor_test.csv' % (frame_class_lookup[fc], swash_type_lookup[swash]) - mode_cmd = '' + sec_cmd = '' else: name = 'frame class = %s (%i)' % (frame_class_lookup[fc], fc) filename = '%s_motor_test.csv' % (frame_class_lookup[fc]) swash_cmd = '' - mode_cmd = '' + sec_cmd = '' print('Running motors test for %s' % name) - os.system('./build/linux/examples/AP_Motors_test s frame_class=%d %s %s > %s/%s' % (fc, swash_cmd, mode_cmd, dir_name, filename)) + os.system('./build/linux/examples/AP_Motors_test s frame_class=%d %s %s > %s/%s' % (fc, swash_cmd, sec_cmd, dir_name, filename)) print('%s complete\n' % name) @@ -149,6 +156,7 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): parser.add_argument("-c", "--compare", action='store_true', help='Compare only, do not re-run tests') parser.add_argument("-p", "--plot", action='store_true', help='Plot comparison results') parser.add_argument("-m", "--dual_mode", type=int, dest='dual_mode', help='Set DUAL_MODE, 0:Longitudinal, 1:Transverse, 2:Intermeshing/Coaxial, default:run all') + parser.add_argument("-t", "--tail_type", type=int, dest='tail_type', help='Set TAIL_TYPE, 0:Servo Only, 1:Servo with ExtGyro, 2:DirectDrive VarPitch, 3:DirectDrive FixedPitch CW, 4:DirectDrive FixedPitch CCW, 5:DDVP with external governor, default:run all') args = parser.parse_args() if 13 in args.frame_class: @@ -171,6 +179,30 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): else: args.dual_mode = [None] + if (args.frame_class != [6]) and (args.tail_type is not None): + print('Only Frame %s (%i) supports tail_type' % (frame_class_lookup[6], 6)) + quit() + + if args.frame_class == [6]: + if args.tail_type is None: + args.tail_type = (0, 1, 2, 3, 4, 5) + else: + args.tail_type = [None] + + # Secondary iterator, tail type for single heli and dual mode for dual heli + secondary_iter = [None] + secondary_lookup = None + secondary_name = None + if args.dual_mode != [None]: + secondary_iter = args.dual_mode + secondary_lookup = dual_mode_lookup + secondary_name = 'dual_mode' + + elif args.tail_type != [None]: + secondary_iter = args.tail_type + secondary_lookup = tail_type_lookup + secondary_name = 'tail_type' + dir_name = 'motors_comparison' if not args.compare: @@ -185,7 +217,7 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): print('\nRunning motor tests with current changes\n') # run the test - run_sweep(args.frame_class, args.swash_type, args.dual_mode, new_name) + run_sweep(args.frame_class, args.swash_type, secondary_iter, secondary_lookup, secondary_name, new_name) if args.head: # rewind head and repeat test @@ -237,14 +269,14 @@ def run_sweep(frame_class, swash_type, dual_mode, dir_name): # Print comparison for fc in args.frame_class: for sw in args.swash_type: - for dm in args.dual_mode: + for sec in secondary_iter: frame = frame_class_lookup[fc] if sw is not None: swash = swash_type_lookup[sw] - if dm is not None: - mode = dual_mode_lookup[dm] - name = frame + ' ' + swash + ' ' + mode - filename = '%s_%s_%s_motor_test.csv' % (frame, swash, mode) + if sec is not None: + sec_str = secondary_lookup[sec] + name = frame + ' ' + swash + ' ' + sec_str + filename = '%s_%s_%s_motor_test.csv' % (frame, swash, sec_str) else: name = frame + ' ' + swash diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 9c79d79205d50f..b6e7543ec01ce8 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -18,6 +18,7 @@ #include #include #include +#include const AP_Param::GroupInfo AP_Mount::var_info[] = { @@ -711,6 +712,7 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_ backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); } +#if HAL_LOGGING_ENABLED // write mount log packet for all backends void AP_Mount::write_log() { @@ -730,6 +732,7 @@ void AP_Mount::write_log(uint8_t instance, uint64_t timestamp_us) } backend->write_log(timestamp_us); } +#endif // point at system ID sysid void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 8545d75b90bf23..28536134adaee3 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -358,6 +358,9 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const ma return MAV_RESULT_FAILED; } + // backup the current values so we can detect a change + mavlink_control_id_t prev_control_id = mavlink_control_id; + // convert negative packet1 and packet2 values int16_t new_sysid = packet.param1; switch (new_sysid) { @@ -382,6 +385,11 @@ MAV_RESULT AP_Mount_Backend::handle_command_do_gimbal_manager_configure(const ma break; } + // send gimbal_manager_status if control has changed + if (prev_control_id != mavlink_control_id) { + gcs().send_message(MSG_GIMBAL_MANAGER_STATUS); + } + return MAV_RESULT_ACCEPTED; } @@ -401,11 +409,12 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli return true; } +#if HAL_LOGGING_ENABLED // write mount log packet void AP_Mount_Backend::write_log(uint64_t timestamp_us) { // return immediately if no yaw estimate - float ahrs_yaw = AP::ahrs().yaw; + float ahrs_yaw = AP::ahrs().get_yaw(); if (isnan(ahrs_yaw)) { return; } @@ -448,6 +457,7 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) }; AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); } +#endif #if AP_MOUNT_POI_TO_LATLONALT_ENABLED // get poi information. Returns true on success and fills in gimbal attitude, location and poi location @@ -570,6 +580,47 @@ void AP_Mount_Backend::calculate_poi() } #endif +// change to RC_TARGETING mode if rc inputs have changed by more than the dead zone +// should be called on every update +void AP_Mount_Backend::set_rctargeting_on_rcinput_change() +{ + // exit immediately if no RC input + if (!rc().has_valid_input()) { + return; + } + + const RC_Channel *roll_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_ROLL : RC_Channel::AUX_FUNC::MOUNT2_ROLL); + const RC_Channel *pitch_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_PITCH : RC_Channel::AUX_FUNC::MOUNT2_PITCH); + const RC_Channel *yaw_ch = rc().find_channel_for_option(_instance == 0 ? RC_Channel::AUX_FUNC::MOUNT1_YAW : RC_Channel::AUX_FUNC::MOUNT2_YAW); + + // get rc input + const int16_t roll_in = (roll_ch == nullptr) ? 0 : roll_ch->get_radio_in(); + const int16_t pitch_in = (pitch_ch == nullptr) ? 0 : pitch_ch->get_radio_in(); + const int16_t yaw_in = (yaw_ch == nullptr) ? 0 : yaw_ch->get_radio_in(); + + // if not in RC_TARGETING or RETRACT modes then check for RC change + if (get_mode() != MAV_MOUNT_MODE_RC_TARGETING && get_mode() != MAV_MOUNT_MODE_RETRACT) { + // get dead zones + const int16_t roll_dz = (roll_ch == nullptr) ? 10 : MAX(roll_ch->get_dead_zone(), 10); + const int16_t pitch_dz = (pitch_ch == nullptr) ? 10 : MAX(pitch_ch->get_dead_zone(), 10); + const int16_t yaw_dz = (yaw_ch == nullptr) ? 10 : MAX(yaw_ch->get_dead_zone(), 10); + + // check if RC input has changed by more than the dead zone + if ((abs(last_rc_input.roll_in - roll_in) > roll_dz) || + (abs(last_rc_input.pitch_in - pitch_in) > pitch_dz) || + (abs(last_rc_input.yaw_in - yaw_in) > yaw_dz)) { + set_mode(MAV_MOUNT_MODE_RC_TARGETING); + } + } + + // if in RC_TARGETING or RETRACT mode then store last RC input + if (get_mode() == MAV_MOUNT_MODE_RC_TARGETING || get_mode() == MAV_MOUNT_MODE_RETRACT) { + last_rc_input.roll_in = roll_in; + last_rc_input.pitch_in = pitch_in; + last_rc_input.yaw_in = yaw_in; + } +} + // get pilot input (in the range -1 to +1) received through RC void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const { @@ -685,7 +736,7 @@ float AP_Mount_Backend::MountTarget::get_bf_yaw() const { if (yaw_is_ef) { // convert to body-frame - return wrap_PI(yaw - AP::ahrs().yaw); + return wrap_PI(yaw - AP::ahrs().get_yaw()); } // target is already body-frame @@ -701,7 +752,7 @@ float AP_Mount_Backend::MountTarget::get_ef_yaw() const } // convert to earth-frame - return wrap_PI(yaw + AP::ahrs().yaw); + return wrap_PI(yaw + AP::ahrs().get_yaw()); } // sets roll, pitch, yaw and yaw_is_ef diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index b88ddf42bf760c..f75ea29da5289f 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -240,6 +240,10 @@ class AP_Mount_Backend 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; @@ -308,11 +312,22 @@ class AP_Mount_Backend uint32_t _last_warning_ms; // system time of last warning sent to GCS + // structure holding the last RC inputs + struct { + int16_t roll_in; + int16_t pitch_in; + int16_t yaw_in; + } last_rc_input; + // structure holding mavlink sysid and compid of controller of this gimbal // see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS - struct { + struct mavlink_control_id_t { uint8_t sysid; uint8_t compid; + + // equality operators + bool operator==(const mavlink_control_id_t &rhs) const { return (sysid == rhs.sysid && compid == rhs.compid); } + bool operator!=(const mavlink_control_id_t &rhs) const { return !(*this == rhs); } } mavlink_control_id; }; diff --git a/libraries/AP_Mount/AP_Mount_Gremsy.cpp b/libraries/AP_Mount/AP_Mount_Gremsy.cpp index b9bf372a680646..6efe6b7ec4b39e 100644 --- a/libraries/AP_Mount/AP_Mount_Gremsy.cpp +++ b/libraries/AP_Mount/AP_Mount_Gremsy.cpp @@ -20,6 +20,9 @@ void AP_Mount_Gremsy::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch (get_mode()) { diff --git a/libraries/AP_Mount/AP_Mount_SToRM32.cpp b/libraries/AP_Mount/AP_Mount_SToRM32.cpp index 1c1e5da3eadc1e..de3c8035c989a9 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32.cpp @@ -18,6 +18,9 @@ void AP_Mount_SToRM32::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // flag to trigger sending target angles to gimbal bool resend_now = false; diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 518b698ef69e76..928a40a297496b 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -29,6 +29,9 @@ void AP_Mount_SToRM32_serial::update() read_incoming(); // read the incoming messages from the gimbal + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // flag to trigger sending target angles to gimbal bool resend_now = false; diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index 9eb738503919bc..c2b93304e67ee5 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -15,6 +15,9 @@ extern const AP_HAL::HAL& hal; // update mount position - should be called periodically void AP_Mount_Scripting::update() { + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch (get_mode()) { // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? diff --git a/libraries/AP_Mount/AP_Mount_Servo.cpp b/libraries/AP_Mount/AP_Mount_Servo.cpp index 196b79edff0a89..2f926c7d848a38 100644 --- a/libraries/AP_Mount/AP_Mount_Servo.cpp +++ b/libraries/AP_Mount/AP_Mount_Servo.cpp @@ -27,6 +27,9 @@ void AP_Mount_Servo::init() // update mount position - should be called periodically void AP_Mount_Servo::update() { + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + auto mount_mode = get_mode(); switch (mount_mode) { // move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage @@ -157,7 +160,7 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad) } // retrieve lean angles from ahrs - Vector2f ahrs_angle_rad = {ahrs.roll, ahrs.pitch}; + Vector2f ahrs_angle_rad = {ahrs.get_roll(), ahrs.get_pitch()}; // rotate ahrs roll and pitch angles to gimbal yaw if (has_pan_control()) { @@ -171,7 +174,7 @@ void AP_Mount_Servo::update_angle_outputs(const MountTarget& angle_rad) // lead filter const Vector3f &gyro = ahrs.get_gyro(); - if (!is_zero(_params.roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) { + if (!is_zero(_params.roll_stb_lead) && fabsf(ahrs.get_pitch()) < M_PI/3.0f) { // Compute rate of change of euler roll angle float roll_rate = gyro.x + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll()); _angle_bf_output_rad.x -= roll_rate * _params.roll_stb_lead; diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 0cdc907f9dee8d..2925e5b70e41fa 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -103,6 +103,9 @@ void AP_Mount_Siyi::update() // run zoom control update_zoom_control(); + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // Get the target angles or rates first depending on the current mount mode switch (get_mode()) { case MAV_MOUNT_MODE_RETRACT: { @@ -479,6 +482,8 @@ void AP_Mount_Siyi::process_packet() sev = MAV_SEVERITY_WARNING; break; } + (void)msg; // in case GCS_SEND_TEXT not available + (void)sev; // in case GCS_SEND_TEXT not available GCS_SEND_TEXT(sev, "Siyi: recording %s", msg); } @@ -704,7 +709,7 @@ void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_ const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100); // convert yaw angle to body-frame - float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().yaw) : yaw_rad; + float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw()) : yaw_rad; // enforce body-frame yaw angle limits. If beyond limits always use body-frame control const float yaw_bf_min = radians(_params.yaw_angle_min); @@ -1122,9 +1127,9 @@ void AP_Mount_Siyi::send_attitude(void) const uint32_t now_ms = AP_HAL::millis(); attitude.time_boot_ms = now_ms; - attitude.roll = ahrs.roll; - attitude.pitch = ahrs.pitch; - attitude.yaw = ahrs.yaw; + attitude.roll = ahrs.get_roll(); + attitude.pitch = ahrs.get_pitch(); + attitude.yaw = ahrs.get_yaw(); attitude.rollspeed = gyro.x; attitude.pitchspeed = gyro.y; attitude.yawspeed = gyro.z; diff --git a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp index 907b09a0d4574c..b188f945a2f23f 100644 --- a/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp +++ b/libraries/AP_Mount/AP_Mount_SoloGimbal.cpp @@ -31,6 +31,9 @@ void AP_Mount_SoloGimbal::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch(get_mode()) { // move mount to a "retracted" position. we do not implement a separate servo based retract mechanism @@ -122,19 +125,26 @@ void AP_Mount_SoloGimbal::handle_gimbal_report(mavlink_channel_t chan, const mav _gimbal.update_target(Vector3f{mnt_target.angle_rad.roll, mnt_target.angle_rad.pitch, mnt_target.angle_rad.get_bf_yaw()}); _gimbal.receive_feedback(chan,msg); +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr) { return; } - - if(!_params_saved && logger->logging_started()) { +#endif + if(!_params_saved +#if HAL_LOGGING_ENABLED + && logger->logging_started() +#endif + ) { _gimbal.fetch_params(); //last parameter save might not be stored in logger so retry _params_saved = true; } +#if HAL_LOGGING_ENABLED if (_gimbal.get_log_dt() > 1.0f/25.0f) { _gimbal.write_logs(); } +#endif } void AP_Mount_SoloGimbal::handle_param_value(const mavlink_message_t &msg) diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 07608d689bfff1..03dc19504261e1 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -76,6 +76,9 @@ void AP_Mount_Viewpro::update() // send vehicle attitude and position send_m_ahrs(); + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // if tracking is active we do not send new targets to the gimbal if (_last_tracking_status == TrackingStatus::SEARCHING || _last_tracking_status == TrackingStatus::TRACKING) { return; @@ -541,7 +544,7 @@ bool AP_Mount_Viewpro::send_target_angles(float pitch_rad, float yaw_rad, bool y } // convert yaw angle to body-frame - float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().yaw) : yaw_rad; + float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw()) : yaw_rad; // enforce body-frame yaw angle limits. If beyond limits always use body-frame control const float yaw_bf_min = radians(_params.yaw_angle_min); diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index 9f210993d4350d..239d416455abcd 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -110,6 +110,9 @@ void AP_Mount_Xacti::update() return; } + // change to RC_TARGETING mode if RC input has changed + set_rctargeting_on_rcinput_change(); + // update based on mount mode switch (get_mode()) { // move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode? @@ -376,7 +379,7 @@ void AP_Mount_Xacti::send_target_rates(float pitch_rads, float yaw_rads, bool ya void AP_Mount_Xacti::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef) { // convert yaw to body frame - const float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().yaw) : yaw_rad; + const float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().get_yaw()) : yaw_rad; // send angle target to gimbal send_gimbal_control(2, degrees(pitch_rad) * 100, degrees(yaw_bf_rad) * 100); diff --git a/libraries/AP_Mount/SoloGimbal.cpp b/libraries/AP_Mount/SoloGimbal.cpp index 01b8aec8297b5e..4503c02cec3f5c 100644 --- a/libraries/AP_Mount/SoloGimbal.cpp +++ b/libraries/AP_Mount/SoloGimbal.cpp @@ -400,6 +400,7 @@ void SoloGimbal::update_target(const Vector3f &newTarget) _att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x,radians(-30),radians(30)); } +#if HAL_LOGGING_ENABLED void SoloGimbal::write_logs() { AP_Logger &logger = AP::logger(); @@ -481,6 +482,7 @@ void SoloGimbal::write_logs() _log_del_ang.zero(); _log_del_vel.zero(); } +#endif bool SoloGimbal::joints_near_limits() const { diff --git a/libraries/AP_Mount/SoloGimbal_Parameters.cpp b/libraries/AP_Mount/SoloGimbal_Parameters.cpp index 8245086ba32a72..60bf156acb3ba2 100644 --- a/libraries/AP_Mount/SoloGimbal_Parameters.cpp +++ b/libraries/AP_Mount/SoloGimbal_Parameters.cpp @@ -187,10 +187,12 @@ void SoloGimbal_Parameters::handle_param_value(const mavlink_message_t &msg) mavlink_param_value_t packet; mavlink_msg_param_value_decode(&msg, &packet); +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { logger->Write_Parameter(packet.param_id, packet.param_value); } +#endif for(uint8_t i=0; itm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6); + char tstring[10]; + hal.util->snprintf(tstring, sizeof(tstring), "%02u%02u%05.2f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6); Location loc; const auto &gps = AP::gps(); @@ -131,7 +131,7 @@ void AP_NMEA_Output::update() char lat_string[13]; double deg = fabs(loc.lat * 1.0e-7f); double min_dec = ((fabs(loc.lat) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f; - snprintf(lat_string, + hal.util->snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c", (unsigned) deg, @@ -142,7 +142,7 @@ void AP_NMEA_Output::update() char lng_string[14]; deg = fabs(loc.lng * 1.0e-7f); min_dec = ((fabs(loc.lng) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f; - snprintf(lng_string, + hal.util->snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c", (unsigned) deg, @@ -208,7 +208,7 @@ void AP_NMEA_Output::update() lng_string, fix_quality, gps.num_sats(), - gps.get_hdop(), + gps.get_hdop()*0.01, loc.alt * 0.01f); space_required += gga_length; @@ -219,7 +219,7 @@ void AP_NMEA_Output::update() if ((_message_enable_bitmask.get() & static_cast(Enabled_Messages::GPRMC)) != 0) { // format date string char dstring[7]; - snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); + hal.util->snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); // get speed #if AP_AHRS_ENABLED diff --git a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp index 0f80af9f69e37f..2805bb81fbf0cc 100644 --- a/libraries/AP_NavEKF/AP_NavEKF_Source.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF_Source.cpp @@ -153,12 +153,14 @@ void AP_NavEKF_Source::setPosVelYawSourceSet(uint8_t source_set_idx) // sanity check source idx if (source_set_idx < AP_NAKEKF_SOURCE_SET_MAX) { active_source_set = source_set_idx; +#if HAL_LOGGING_ENABLED static const LogEvent evt[AP_NAKEKF_SOURCE_SET_MAX] { LogEvent::EK3_SOURCES_SET_TO_PRIMARY, LogEvent::EK3_SOURCES_SET_TO_SECONDARY, LogEvent::EK3_SOURCES_SET_TO_TERTIARY, }; AP::logger().Write_Event(evt[active_source_set]); +#endif } } diff --git a/libraries/AP_NavEKF/EKFGSF_Logging.cpp b/libraries/AP_NavEKF/EKFGSF_Logging.cpp index 64673085ce9f91..68e57afa7b7bd7 100644 --- a/libraries/AP_NavEKF/EKFGSF_Logging.cpp +++ b/libraries/AP_NavEKF/EKFGSF_Logging.cpp @@ -2,6 +2,8 @@ #include +#if HAL_LOGGING_ENABLED + #pragma GCC diagnostic ignored "-Wnarrowing" void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, uint8_t core_index) @@ -48,3 +50,5 @@ void EKFGSF_yaw::Log_Write(uint64_t time_us, LogMessages id0, LogMessages id1, u }; AP::logger().WriteBlock(&ky1, sizeof(ky1)); } + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp index 5f5d25f048ae6c..73874b775f95cf 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp @@ -1,3 +1,7 @@ +#include + +#if HAL_LOGGING_ENABLED + #include "AP_NavEKF2.h" #include "AP_NavEKF2_core.h" @@ -331,3 +335,5 @@ void NavEKF2_core::Log_Write_GSF(uint64_t time_us) const } yawEstimator->Log_Write(time_us, LOG_NKY0_MSG, LOG_NKY1_MSG, DAL_CORE(core_index)); } + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_NavEKF2/LogStructure.h b/libraries/AP_NavEKF2/LogStructure.h index f24cba6bfd8973..753d9ceb0f4b33 100644 --- a/libraries/AP_NavEKF2/LogStructure.h +++ b/libraries/AP_NavEKF2/LogStructure.h @@ -300,7 +300,7 @@ struct PACKED log_NKT { "NKF4","QBcccccfffHBIHb","TimeUS,C,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI", "s#------mm-----", "F-------??-----" , true }, \ { LOG_NKF5_MSG, sizeof(log_NKF5), \ "NKF5","QBBhhhcccCCfff","TimeUS,C,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos", "s#----m???mrnm", "F-----BBBBB000" , true }, \ - { LOG_NKQ_MSG, sizeof(log_NKQ), "NKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \ + { LOG_NKQ_MSG, sizeof(log_NKQ), "NKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#----", "F-0000" , true }, \ { LOG_NKT_MSG, sizeof(log_NKT), \ "NKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, #endif diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 100c47a253c87d..1f60ae65f932c6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -1,3 +1,7 @@ +#include + +#if HAL_LOGGING_ENABLED + #include "AP_NavEKF3.h" #include "AP_NavEKF3_core.h" @@ -440,3 +444,5 @@ void NavEKF3_core::Log_Write_GSF(uint64_t time_us) } yawEstimator->Log_Write(time_us, LOG_XKY0_MSG, LOG_XKY1_MSG, DAL_CORE(core_index)); } + +#endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index e409061698bf2c..9ebe777515a84e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -1394,6 +1394,7 @@ void NavEKF3_core::updateMovementCheck(void) if (logStatusChange || imuSampleTime_ms - lastMoveCheckLogTime_ms > 200) { lastMoveCheckLogTime_ms = imuSampleTime_ms; +#if HAL_LOGGING_ENABLED const struct log_XKFM pkt{ LOG_PACKET_HEADER_INIT(LOG_XKFM_MSG), time_us : dal.micros64(), @@ -1405,6 +1406,7 @@ void NavEKF3_core::updateMovementCheck(void) accel_diff_ratio : float(accel_diff_ratio), }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); +#endif } } diff --git a/libraries/AP_NavEKF3/LogStructure.h b/libraries/AP_NavEKF3/LogStructure.h index f3ef63d1bfb7a4..8e124462fcf0db 100644 --- a/libraries/AP_NavEKF3/LogStructure.h +++ b/libraries/AP_NavEKF3/LogStructure.h @@ -440,7 +440,7 @@ struct PACKED log_XKV { "XKFM", "QBBffff", "TimeUS,C,OGNM,GLR,ALR,GDR,ADR", "s#-----", "F------", true }, \ { LOG_XKFS_MSG, sizeof(log_XKFS), \ "XKFS","QBBBBBB","TimeUS,C,MI,BI,GI,AI,SS", "s#-----", "F------" , true }, \ - { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#????", "F-????" , true }, \ + { LOG_XKQ_MSG, sizeof(log_XKQ), "XKQ", "QBffff", "TimeUS,C,Q1,Q2,Q3,Q4", "s#----", "F-0000" , true }, \ { LOG_XKT_MSG, sizeof(log_XKT), \ "XKT", "QBIffffffff", "TimeUS,C,Cnt,IMUMin,IMUMax,EKFMin,EKFMax,AngMin,AngMax,VMin,VMax", "s#sssssssss", "F-000000000", true }, \ { LOG_XKTV_MSG, sizeof(log_XKTV), \ diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index df2f207e9c5abf..3506e472e3c125 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -8,15 +8,23 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; #if AP_NETWORKING_BACKEND_CHIBIOS #include "AP_Networking_ChibiOS.h" #include -#include -#else -#include +#endif + +#include +#include + + +#include + +#if AP_NETWORKING_BACKEND_PPP +#include "AP_Networking_PPP.h" #endif #if AP_NETWORKING_BACKEND_SITL @@ -78,6 +86,20 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { AP_SUBGROUPINFO(param.test_ipaddr, "TEST_IP", 8, AP_Networking, AP_Networking_IPV4), #endif + // @Param: OPTIONS + // @DisplayName: Networking options + // @Description: Networking options + // @Bitmask: 0:EnablePPP Ethernet gateway + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("OPTIONS", 9, AP_Networking, param.options, 0), + +#if AP_NETWORKING_PPP_GATEWAY_ENABLED + // @Group: REMPPP_IP + // @Path: AP_Networking_address.cpp + AP_SUBGROUPINFO(param.remote_ppp_ip, "REMPPP_IP", 10, AP_Networking, AP_Networking_IPV4), +#endif + AP_GROUPEND }; @@ -121,11 +143,32 @@ void AP_Networking::init() } #endif +#if AP_NETWORKING_PPP_GATEWAY_ENABLED + if (option_is_set(OPTION::PPP_ETHERNET_GATEWAY)) { + /* + when we are a PPP/Ethernet gateway we bring up the ethernet first + */ + backend = new AP_Networking_ChibiOS(*this); + backend_PPP = new AP_Networking_PPP(*this); + } +#endif + + +#if AP_NETWORKING_BACKEND_PPP + if (backend == nullptr && AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_PPP, 0)) { + backend = new AP_Networking_PPP(*this); + } +#endif + #if AP_NETWORKING_BACKEND_CHIBIOS - backend = new AP_Networking_ChibiOS(*this); + if (backend == nullptr) { + backend = new AP_Networking_ChibiOS(*this); + } #endif #if AP_NETWORKING_BACKEND_SITL - backend = new AP_Networking_SITL(*this); + if (backend == nullptr) { + backend = new AP_Networking_SITL(*this); + } #endif if (backend == nullptr) { @@ -140,6 +183,13 @@ void AP_Networking::init() return; } +#if AP_NETWORKING_PPP_GATEWAY_ENABLED + if (backend_PPP != nullptr && !backend_PPP->init()) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: backend_PPP init failed"); + backend_PPP = nullptr; + } +#endif + announce_address_changes(); GCS_SEND_TEXT(MAV_SEVERITY_INFO,"NET: Initialized"); @@ -164,9 +214,12 @@ void AP_Networking::announce_address_changes() return; } - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: IP %s", get_ip_active_str()); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Mask %s", get_netmask_active_str()); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Gateway %s", get_gateway_active_str()); +#if AP_HAVE_GCS_SEND_TEXT + char ipstr[16]; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: IP %s", SocketAPM::inet_addr_to_str(get_ip_active(), ipstr, sizeof(ipstr))); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Mask %s", SocketAPM::inet_addr_to_str(get_netmask_active(), ipstr, sizeof(ipstr))); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "NET: Gateway %s", SocketAPM::inet_addr_to_str(get_gateway_active(), ipstr, sizeof(ipstr))); +#endif announce_ms = as.last_change_ms; } @@ -204,21 +257,6 @@ uint8_t AP_Networking::convert_netmask_ip_to_bitcount(const uint32_t netmask_ip) return netmask_bitcount; } -uint32_t AP_Networking::convert_str_to_ip(const char* ip_str) -{ - uint32_t ip = 0; - inet_pton(AF_INET, ip_str, &ip); - return ntohl(ip); -} - -const char* AP_Networking::convert_ip_to_str(uint32_t ip) -{ - ip = htonl(ip); - static char _str_buffer[20]; - inet_ntop(AF_INET, &ip, _str_buffer, sizeof(_str_buffer)); - return _str_buffer; -} - /* convert a string to an ethernet MAC address */ @@ -282,6 +320,93 @@ void AP_Networking::startup_wait(void) const #endif } +/* + send the rest of a file to a socket + */ +bool AP_Networking::sendfile(SocketAPM *sock, int fd) +{ + WITH_SEMAPHORE(sem); + if (sendfile_buf == nullptr) { + uint32_t bufsize = AP_NETWORKING_SENDFILE_BUFSIZE; + do { + sendfile_buf = (uint8_t *)hal.util->malloc_type(bufsize, AP_HAL::Util::MEM_FILESYSTEM); + if (sendfile_buf != nullptr) { + sendfile_bufsize = bufsize; + break; + } + bufsize /= 2; + } while (bufsize >= 4096); + if (sendfile_buf == nullptr) { + return false; + } + } + if (!sendfile_thread_started) { + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::sendfile_check, void), + "sendfile", + 2048, AP_HAL::Scheduler::PRIORITY_UART, 0)) { + return false; + } + sendfile_thread_started = true; + } + for (auto &s : sendfiles) { + if (s.sock == nullptr) { + s.sock = sock->duplicate(); + if (s.sock == nullptr) { + return false; + } + s.fd = fd; + return true; + } + } + return false; +} + +void AP_Networking::SendFile::close(void) +{ + AP::FS().close(fd); + delete sock; + sock = nullptr; +} + +#include +/* + check for sendfile updates + */ +void AP_Networking::sendfile_check(void) +{ + while (true) { + hal.scheduler->delay(1); + WITH_SEMAPHORE(sem); + bool none_active = true; + for (auto &s : sendfiles) { + if (s.sock == nullptr) { + continue; + } + none_active = false; + if (!s.sock->pollout(0)) { + continue; + } + const auto nread = AP::FS().read(s.fd, sendfile_buf, sendfile_bufsize); + if (nread <= 0) { + s.close(); + continue; + } + const auto nsent = s.sock->send(sendfile_buf, nread); + if (nsent <= 0) { + s.close(); + continue; + } + if (nsent < nread) { + AP::FS().lseek(s.fd, nsent - nread, SEEK_CUR); + } + } + if (none_active) { + free(sendfile_buf); + sendfile_buf = nullptr; + } + } +} + AP_Networking *AP_Networking::singleton; namespace AP @@ -292,4 +417,45 @@ AP_Networking &network() } } +/* + debug printfs from LWIP + */ +int ap_networking_printf(const char *fmt, ...) +{ + WITH_SEMAPHORE(AP::network().get_semaphore()); +#ifdef AP_NETWORKING_LWIP_DEBUG_FILE + static int fd = -1; + if (fd == -1) { + fd = AP::FS().open(AP_NETWORKING_LWIP_DEBUG_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644); + if (fd == -1) { + return -1; + } + } + va_list ap; + va_start(ap, fmt); + vdprintf(fd, fmt, ap); + va_end(ap); +#else + va_list ap; + va_start(ap, fmt); + hal.console->vprintf(fmt, ap); + va_end(ap); +#endif + return 0; +} + +// address to string using a static return buffer +const char *AP_Networking::address_to_str(uint32_t addr) +{ + static char buf[16]; // 16 for aaa.bbb.ccc.ddd + return SocketAPM::inet_addr_to_str(addr, buf, sizeof(buf)); +} + +#ifdef LWIP_PLATFORM_ASSERT +void ap_networking_platform_assert(const char *msg, int line, const char *file) +{ + AP_HAL::panic("LWIP: %s: %s:%u", msg, file, line); +} +#endif + #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 1359b3abe55723..128e6c2c29398e 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -26,6 +26,7 @@ class AP_Networking public: friend class AP_Networking_Backend; friend class AP_Networking_ChibiOS; + friend class AP_Networking_PPP; friend class AP_Vehicle; friend class Networking_Periph; @@ -45,6 +46,11 @@ class AP_Networking return singleton; } + HAL_Semaphore &get_semaphore(void) + { + return sem; + } + // Networking interface is enabled and initialized bool is_healthy() const { @@ -86,15 +92,6 @@ class AP_Networking #endif } - /* - returns a null terminated string of the active IP address. Example: "192.168.12.13" - Note that the returned - */ - const char *get_ip_active_str() const - { - return convert_ip_to_str(get_ip_active()); - } - // sets the user-parameter static IP address from a 32bit value void set_ip_param(const uint32_t ip) { @@ -117,29 +114,6 @@ class AP_Networking #endif } - // returns a null terminated string of the active Netmask address. Example: "192.168.12.13" - const char *get_netmask_active_str() - { - return convert_ip_to_str(get_netmask_active()); - } - - const char *get_netmask_param_str() - { - return convert_ip_to_str(get_netmask_param()); - } - - void set_netmask_param_str(const char* nm_str) - { - set_netmask_param(convert_str_to_ip((char*)nm_str)); - } - - void set_netmask_param(const uint32_t nm) - { -#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED - param.netmask.set(convert_netmask_ip_to_bitcount(nm)); -#endif - } - uint32_t get_gateway_active() const; uint32_t get_gateway_param() const @@ -152,21 +126,6 @@ class AP_Networking #endif } - const char *get_gateway_active_str() - { - return convert_ip_to_str(get_gateway_active()); - } - - const char *get_gateway_param_str() - { - return convert_ip_to_str(get_gateway_param()); - } - - void set_gateway_param_str(const char* gw_str) - { - set_gateway_param(convert_str_to_ip((char*)gw_str)); - } - void set_gateway_param(const uint32_t gw) { #if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED @@ -177,19 +136,30 @@ class AP_Networking // wait in a thread for network startup void startup_wait(void) const; - // helper functions to convert between 32bit IP addresses and null terminated strings and back - static uint32_t convert_str_to_ip(const char* ip_str); - static const char* convert_ip_to_str(uint32_t ip); - // convert string to ethernet mac address static bool convert_str_to_macaddr(const char *mac_str, uint8_t addr[6]); + // address to string using a static return buffer for scripting + static const char *address_to_str(uint32_t addr); + // helper functions to convert between 32bit Netmask and counting consecutive bits and back static uint32_t convert_netmask_bitcount_to_ip(const uint32_t netmask_bitcount); static uint8_t convert_netmask_ip_to_bitcount(const uint32_t netmask_ip); + /* + send contents of a file to a socket then close both socket and file + */ + bool sendfile(SocketAPM *sock, int fd); + static const struct AP_Param::GroupInfo var_info[]; + enum class OPTION { + PPP_ETHERNET_GATEWAY=(1U<<0), + }; + bool option_is_set(OPTION option) const { + return (param.options.get() & int32_t(option)) != 0; + } + private: static AP_Networking *singleton; @@ -213,10 +183,18 @@ class AP_Networking AP_Int32 tests; AP_Networking_IPV4 test_ipaddr{AP_NETWORKING_TEST_IP}; #endif + +#if AP_NETWORKING_PPP_GATEWAY_ENABLED + AP_Networking_IPV4 remote_ppp_ip{AP_NETWORKING_REMOTE_PPP_IP}; +#endif } param; AP_Networking_Backend *backend; +#if AP_NETWORKING_PPP_GATEWAY_ENABLED + AP_Networking_Backend *backend_PPP; +#endif + HAL_Semaphore sem; enum class NetworkPortType { @@ -227,6 +205,7 @@ class AP_Networking TCP_SERVER = 4, }; +#if AP_NETWORKING_REGISTER_PORT_ENABLED // class for NET_Pn_* parameters class Port : public AP_SerialManager::RegisteredPort { public: @@ -292,6 +271,7 @@ class AP_Networking HAL_Semaphore sem; }; +#endif // AP_NETWORKING_REGISTER_PORT_ENABLED private: uint32_t announce_ms; @@ -308,8 +288,22 @@ class AP_Networking void test_TCP_discard(void); #endif // AP_NETWORKING_TESTS_ENABLED +#if AP_NETWORKING_REGISTER_PORT_ENABLED // ports for registration with serial manager Port ports[AP_NETWORKING_NUM_PORTS]; +#endif + + // support for sendfile() + struct SendFile { + SocketAPM *sock; + int fd; + void close(void); + } sendfiles[AP_NETWORKING_NUM_SENDFILES]; + + uint8_t *sendfile_buf; + uint32_t sendfile_bufsize; + void sendfile_check(void); + bool sendfile_thread_started; void ports_init(void); }; @@ -319,4 +313,8 @@ namespace AP AP_Networking &network(); }; +extern "C" { +int ap_networking_printf(const char *fmt, ...); +} + #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_Backend.h b/libraries/AP_Networking/AP_Networking_Backend.h index 9233442c44da67..7fe0d21322802d 100644 --- a/libraries/AP_Networking/AP_Networking_Backend.h +++ b/libraries/AP_Networking/AP_Networking_Backend.h @@ -19,7 +19,7 @@ class AP_Networking_Backend CLASS_NO_COPY(AP_Networking_Backend); virtual bool init() = 0; - virtual void update() = 0; + virtual void update() {}; protected: AP_Networking &frontend; diff --git a/libraries/AP_Networking/AP_Networking_ChibiOS.cpp b/libraries/AP_Networking/AP_Networking_ChibiOS.cpp index ea5e4f42a6b0e1..889a8526be7523 100644 --- a/libraries/AP_Networking/AP_Networking_ChibiOS.cpp +++ b/libraries/AP_Networking/AP_Networking_ChibiOS.cpp @@ -6,9 +6,17 @@ #include "AP_Networking_ChibiOS.h" #include -#include #include #include +#include +#include +#if LWIP_DHCP +#include +#endif +#include +#include +#include "../../modules/ChibiOS/os/various/evtimer.h" +#include extern const AP_HAL::HAL& hal; @@ -18,23 +26,30 @@ extern const AP_HAL::HAL& hal; /* these are referenced as globals inside lwip - */ +*/ stm32_eth_rx_descriptor_t *__eth_rd; stm32_eth_tx_descriptor_t *__eth_td; uint32_t *__eth_rb[STM32_MAC_RECEIVE_BUFFERS]; uint32_t *__eth_tb[STM32_MAC_TRANSMIT_BUFFERS]; +#define LWIP_SEND_TIMEOUT_MS 50 +#define LWIP_NETIF_MTU 1500 +#define LWIP_LINK_POLL_INTERVAL TIME_S2I(5) + +#define PERIODIC_TIMER_ID 1 +#define FRAME_RECEIVED_ID 2 + /* allocate buffers for LWIP - */ +*/ bool AP_Networking_ChibiOS::allocate_buffers() { - #define AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE ((((STM32_MAC_BUFFERS_SIZE - 1) | 3) + 1) / 4) // typically == 381 +#define AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE ((((STM32_MAC_BUFFERS_SIZE - 1) | 3) + 1) / 4) // typically == 381 // check total size of buffers const uint32_t total_size = sizeof(stm32_eth_rx_descriptor_t)*STM32_MAC_RECEIVE_BUFFERS + - sizeof(stm32_eth_tx_descriptor_t)*STM32_MAC_TRANSMIT_BUFFERS + - sizeof(uint32_t)*STM32_MAC_RECEIVE_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE + - sizeof(uint32_t)*STM32_MAC_TRANSMIT_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE; // typically == 9240 + sizeof(stm32_eth_tx_descriptor_t)*STM32_MAC_TRANSMIT_BUFFERS + + sizeof(uint32_t)*STM32_MAC_RECEIVE_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE + + sizeof(uint32_t)*STM32_MAC_TRANSMIT_BUFFERS*AP_NETWORKING_EXTERN_MAC_BUFFER_SIZE; // typically == 9240 // ensure that we allocate 32-bit aligned memory, and mark it non-cacheable uint32_t size = 1; @@ -48,11 +63,14 @@ bool AP_Networking_ChibiOS::allocate_buffers() if (mem == nullptr) { return false; } + +#ifndef HAL_BOOTLOADER_BUILD // ensure our memory is aligned // ref. Cortex-M7 peripherals PM0253, section 4.6.4 MPU region base address register if (((uint32_t)mem) % size) { AP_HAL::panic("Bad alignment of ETH memory"); } +#endif // for total_size == 9240, size should be 16384 and (rasr-1) should be 13 (MPU_RASR_SIZE_16K) const uint32_t rasr_size = MPU_RASR_SIZE(rasr-1); @@ -84,7 +102,7 @@ bool AP_Networking_ChibiOS::allocate_buffers() /* initialise ChibiOS network backend using LWIP - */ +*/ bool AP_Networking_ChibiOS::init() { #ifdef HAL_GPIO_ETH_ENABLE @@ -102,21 +120,6 @@ bool AP_Networking_ChibiOS::init() return false; } - lwip_options = new lwipthread_opts; - - if (frontend.get_dhcp_enabled()) { - lwip_options->addrMode = NET_ADDRESS_DHCP; - } else { - lwip_options->addrMode = NET_ADDRESS_STATIC; - lwip_options->address = htonl(frontend.get_ip_param()); - lwip_options->netmask = htonl(frontend.get_netmask_param()); - lwip_options->gateway = htonl(frontend.get_gateway_param()); - } - frontend.param.macaddr.get_address(macaddr); - lwip_options->macaddress = macaddr; - - lwipInit(lwip_options); - #if LWIP_IGMP if (ETH != nullptr) { // enbale "permit multicast" so we can receive multicast packets @@ -124,17 +127,252 @@ bool AP_Networking_ChibiOS::init() } #endif + thisif = new netif; + if (thisif == nullptr) { + return false; + } + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking_ChibiOS::thread, void), + "network", + 2048, AP_HAL::Scheduler::PRIORITY_NET, 0)) { + return false; + } + return true; } +void AP_Networking_ChibiOS::link_up_cb(void *p) +{ +#if LWIP_DHCP + auto *driver = (AP_Networking_ChibiOS *)p; + if (driver->frontend.get_dhcp_enabled()) { + dhcp_start(driver->thisif); + } +#endif +} + +void AP_Networking_ChibiOS::link_down_cb(void *p) +{ +#if LWIP_DHCP + auto *driver = (AP_Networking_ChibiOS *)p; + if (driver->frontend.get_dhcp_enabled()) { + dhcp_stop(driver->thisif); + } +#endif +} + /* - update called at 10Hz + * This function does the actual transmission of the packet. The packet is + * contained in the pbuf that is passed to the function. This pbuf + * might be chained. + * + * @param netif the lwip network interface structure for this ethernetif + * @param p the MAC packet to send (e.g. IP packet including MAC addresses and type) + * @return ERR_OK if the packet could be sent + * an err_t value if the packet couldn't be sent + * + * @note Returning ERR_MEM here if a DMA queue of your MAC is full can lead to + * strange results. You might consider waiting for space in the DMA queue + * to become available since the stack doesn't retry to send a packet + * dropped because of memory failure (except for the TCP timers). */ +int8_t AP_Networking_ChibiOS::low_level_output(struct netif *netif, struct pbuf *p) +{ + struct pbuf *q; + MACTransmitDescriptor td; + (void)netif; + + if (macWaitTransmitDescriptor(ÐD1, &td, TIME_MS2I(LWIP_SEND_TIMEOUT_MS)) != MSG_OK) { + return ERR_TIMEOUT; + } + +#if ETH_PAD_SIZE + pbuf_header(p, -ETH_PAD_SIZE); /* drop the padding word */ +#endif + + /* Iterates through the pbuf chain. */ + for(q = p; q != NULL; q = q->next) { + macWriteTransmitDescriptor(&td, (uint8_t *)q->payload, (size_t)q->len); + } + macReleaseTransmitDescriptorX(&td); + +#if ETH_PAD_SIZE + pbuf_header(p, ETH_PAD_SIZE); /* reclaim the padding word */ +#endif + + return ERR_OK; +} + +/* + * Receives a frame. + * Allocates a pbuf and transfers the bytes of the incoming + * packet from the interface into the pbuf. + * + * @param netif the lwip network interface structure for this ethernetif + * @return a pbuf filled with the received packet (including MAC header) + * NULL on memory error + */ +bool AP_Networking_ChibiOS::low_level_input(struct netif *netif, struct pbuf **pbuf) +{ + MACReceiveDescriptor rd; + struct pbuf *q; + u16_t len; + + (void)netif; + + if (macWaitReceiveDescriptor(ÐD1, &rd, TIME_IMMEDIATE) != MSG_OK) { + return false; + } + + len = (u16_t)rd.size; + +#if ETH_PAD_SIZE + len += ETH_PAD_SIZE; /* allow room for Ethernet padding */ +#endif + + /* We allocate a pbuf chain of pbufs from the pool. */ + *pbuf = pbuf_alloc(PBUF_RAW, len, PBUF_POOL); + + if (*pbuf != nullptr) { +#if ETH_PAD_SIZE + pbuf_header(*pbuf, -ETH_PAD_SIZE); /* drop the padding word */ +#endif + + /* Iterates through the pbuf chain. */ + for(q = *pbuf; q != NULL; q = q->next) { + macReadReceiveDescriptor(&rd, (uint8_t *)q->payload, (size_t)q->len); + } + macReleaseReceiveDescriptorX(&rd); + +#if ETH_PAD_SIZE + pbuf_header(*pbuf, ETH_PAD_SIZE); /* reclaim the padding word */ +#endif + } else { + macReleaseReceiveDescriptorX(&rd); // Drop packet + } + + return true; +} + +int8_t AP_Networking_ChibiOS::ethernetif_init(struct netif *netif) +{ + netif->state = NULL; + netif->name[0] = 'm'; + netif->name[1] = 's'; + netif->output = etharp_output; + netif->linkoutput = low_level_output; + + /* set MAC hardware address length */ + netif->hwaddr_len = ETHARP_HWADDR_LEN; + + /* maximum transfer unit */ + netif->mtu = LWIP_NETIF_MTU; + + /* device capabilities */ + netif->flags = NETIF_FLAG_BROADCAST | NETIF_FLAG_ETHARP; + +#if LWIP_IGMP + // also enable multicast + netif->flags |= NETIF_FLAG_IGMP; +#endif + + return ERR_OK; +} + +/* + networking thread +*/ +void AP_Networking_ChibiOS::thread() +{ + while (!hal.scheduler->is_system_initialized()) { + hal.scheduler->delay_microseconds(1000); + } + + /* start tcpip thread */ + tcpip_init(NULL, NULL); + + frontend.param.macaddr.get_address(thisif->hwaddr); + + struct { + ip4_addr_t ip, gateway, netmask; + } addr {}; + + if (!frontend.get_dhcp_enabled()) { + addr.ip.addr = htonl(frontend.get_ip_param()); + addr.gateway.addr = htonl(frontend.get_gateway_param()); + addr.netmask.addr = htonl(frontend.get_netmask_param()); + } + + const MACConfig mac_config = {thisif->hwaddr}; + macStart(ÐD1, &mac_config); + + /* Add interface. */ + auto result = netifapi_netif_add(thisif, &addr.ip, &addr.netmask, &addr.gateway, NULL, ethernetif_init, tcpip_input); + if (result != ERR_OK) { + AP_HAL::panic("Failed to initialise netif"); + } + + netifapi_netif_set_default(thisif); + netifapi_netif_set_up(thisif); + + /* Setup event sources.*/ + event_timer_t evt; + event_listener_t el0, el1; + + evtObjectInit(&evt, LWIP_LINK_POLL_INTERVAL); + evtStart(&evt); + chEvtRegisterMask(&evt.et_es, &el0, PERIODIC_TIMER_ID); + chEvtRegisterMaskWithFlags(macGetEventSource(ÐD1), &el1, + FRAME_RECEIVED_ID, MAC_FLAGS_RX); + chEvtAddEvents(PERIODIC_TIMER_ID | FRAME_RECEIVED_ID); + + while (true) { + eventmask_t mask = chEvtWaitAny(ALL_EVENTS); + if (mask & PERIODIC_TIMER_ID) { + bool current_link_status = macPollLinkStatus(ÐD1); + if (current_link_status != netif_is_link_up(thisif)) { + if (current_link_status) { + tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_up, thisif, 0); + tcpip_callback_with_block(link_up_cb, this, 0); + } + else { + tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_down, thisif, 0); + tcpip_callback_with_block(link_down_cb, this, 0); + } + } + } + + if (mask & FRAME_RECEIVED_ID) { + struct pbuf *p; + while (low_level_input(thisif, &p)) { + if (p != NULL) { + struct eth_hdr *ethhdr = (struct eth_hdr *)p->payload; + switch (htons(ethhdr->type)) { + /* IP or ARP packet? */ + case ETHTYPE_IP: + case ETHTYPE_ARP: + /* full packet send to tcpip_thread to process */ + if (thisif->input(p, thisif) == ERR_OK) { + break; + } + /* Falls through */ + default: + pbuf_free(p); + } + } + } + } + } +} + +/* + update called at 10Hz +*/ void AP_Networking_ChibiOS::update() { - const uint32_t ip = ntohl(lwipGetIp()); - const uint32_t nm = ntohl(lwipGetNetmask()); - const uint32_t gw = ntohl(lwipGetGateway()); + const uint32_t ip = ntohl(thisif->ip_addr.addr); + const uint32_t nm = ntohl(thisif->netmask.addr); + const uint32_t gw = ntohl(thisif->gw.addr); if (ip != activeSettings.ip || nm != activeSettings.nm || diff --git a/libraries/AP_Networking/AP_Networking_ChibiOS.h b/libraries/AP_Networking/AP_Networking_ChibiOS.h index 71ebfb57a62504..7ca302f615662c 100644 --- a/libraries/AP_Networking/AP_Networking_ChibiOS.h +++ b/libraries/AP_Networking/AP_Networking_ChibiOS.h @@ -8,6 +8,7 @@ class AP_Networking_ChibiOS : public AP_Networking_Backend { public: + friend class BL_Network; using AP_Networking_Backend::AP_Networking_Backend; /* Do not allow copies */ @@ -17,11 +18,18 @@ class AP_Networking_ChibiOS : public AP_Networking_Backend void update() override; private: - bool allocate_buffers(void); + static bool allocate_buffers(void); + void thread(void); + static void link_up_cb(void*); + static void link_down_cb(void*); + static int8_t ethernetif_init(struct netif *netif); + static int8_t low_level_output(struct netif *netif, struct pbuf *p); + static bool low_level_input(struct netif *netif, struct pbuf **pbuf); -private: struct lwipthread_opts *lwip_options; uint8_t macaddr[6]; + + struct netif *thisif; }; #endif // AP_NETWORKING_BACKEND_CHIBIOS diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index 5d32334510dc55..5b66134957849e 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -1,34 +1,57 @@ #include +#include + +#if defined(AP_NETWORKING_BACKEND_PPP) && !defined(AP_NETWORKING_ENABLED) +// allow --enable-ppp to enable networking +#define AP_NETWORKING_ENABLED AP_NETWORKING_BACKEND_PPP +#endif + #ifndef AP_NETWORKING_ENABLED +#if !defined(__APPLE__) && defined(__clang__) +// clang fails on linux #define AP_NETWORKING_ENABLED 0 +#else +#define AP_NETWORKING_ENABLED ((CONFIG_HAL_BOARD == HAL_BOARD_LINUX) || (CONFIG_HAL_BOARD == HAL_BOARD_SITL)) +#endif #endif #ifndef AP_NETWORKING_BACKEND_DEFAULT_ENABLED #define AP_NETWORKING_BACKEND_DEFAULT_ENABLED AP_NETWORKING_ENABLED #endif -#ifndef AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED -// AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED should only be true if we have the ability to -// change the IP address. If not then the IP, GW, NetMask, MAC and DHCP params are hidden. -// This does not mean that the system/OS does not have the ability to set the IP, just that -// we have no control from this scope. For example, Linux systems (including SITL) have -// their own DHCP client running but we have no control over it. -#define AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS) -#endif - // --------------------------- // Backends // --------------------------- #ifndef AP_NETWORKING_BACKEND_CHIBIOS -#define AP_NETWORKING_BACKEND_CHIBIOS (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS)) +#ifndef HAL_USE_MAC +#define HAL_USE_MAC 0 +#endif +#define AP_NETWORKING_BACKEND_CHIBIOS (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS) && HAL_USE_MAC) +#endif + +#ifndef AP_NETWORKING_BACKEND_PPP +#define AP_NETWORKING_BACKEND_PPP 0 #endif #ifndef AP_NETWORKING_BACKEND_SITL #define AP_NETWORKING_BACKEND_SITL (AP_NETWORKING_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL)) #endif -#define AP_NETWORKING_SOCKETS_ENABLED (HAL_OS_SOCKETS || AP_NETWORKING_BACKEND_CHIBIOS) +#ifndef AP_NETWORKING_SOCKETS_ENABLED +#define AP_NETWORKING_SOCKETS_ENABLED AP_NETWORKING_ENABLED +#endif + +#ifndef AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED +// AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED should only be true if we have the ability to +// change the IP address. If not then the IP, GW, NetMask, MAC and DHCP params are hidden. +// This does not mean that the system/OS does not have the ability to set the IP, just that +// we have no control from this scope. For example, Linux systems (including SITL) have +// their own DHCP client running but we have no control over it. +#define AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED AP_NETWORKING_BACKEND_CHIBIOS +#endif + +#define AP_NETWORKING_NEED_LWIP (AP_NETWORKING_BACKEND_CHIBIOS || AP_NETWORKING_BACKEND_PPP) // --------------------------- // IP Features @@ -89,3 +112,28 @@ #ifndef AP_NETWORKING_NUM_PORTS #define AP_NETWORKING_NUM_PORTS 4 #endif + +#ifndef AP_NETWORKING_NUM_SENDFILES +#define AP_NETWORKING_NUM_SENDFILES 20 +#endif + +#ifndef AP_NETWORKING_SENDFILE_BUFSIZE +#define AP_NETWORKING_SENDFILE_BUFSIZE (64*512) +#endif + +#ifndef AP_NETWORKING_PPP_GATEWAY_ENABLED +#define AP_NETWORKING_PPP_GATEWAY_ENABLED (AP_NETWORKING_BACKEND_CHIBIOS && AP_NETWORKING_BACKEND_PPP) +#endif + +/* + the IP address given to the remote end of the PPP link when running + as a PPP<->ethernet gateway. If this is on the same subnet as the + ethernet interface IP then proxyarp will be used + */ +#ifndef AP_NETWORKING_REMOTE_PPP_IP +#define AP_NETWORKING_REMOTE_PPP_IP "0.0.0.0" +#endif + +#ifndef AP_NETWORKING_REGISTER_PORT_ENABLED +#define AP_NETWORKING_REGISTER_PORT_ENABLED AP_NETWORKING_ENABLED && AP_SERIALMANAGER_REGISTER_ENABLED +#endif diff --git a/libraries/AP_Networking/AP_Networking_PPP.cpp b/libraries/AP_Networking/AP_Networking_PPP.cpp new file mode 100644 index 00000000000000..18f16f0bdbb56e --- /dev/null +++ b/libraries/AP_Networking/AP_Networking_PPP.cpp @@ -0,0 +1,231 @@ + +#include "AP_Networking_Config.h" + +#if AP_NETWORKING_BACKEND_PPP + +#include "AP_Networking_PPP.h" +#include + +#include +#include +#include +#include +#include +#include +#include + + +extern const AP_HAL::HAL& hal; + +#if LWIP_TCPIP_CORE_LOCKING +#define LWIP_TCPIP_LOCK() sys_lock_tcpip_core() +#define LWIP_TCPIP_UNLOCK() sys_unlock_tcpip_core() +#else +#define LWIP_TCPIP_LOCK() +#define LWIP_TCPIP_UNLOCK() +#endif + +/* + output some data to the uart + */ +uint32_t AP_Networking_PPP::ppp_output_cb(ppp_pcb *pcb, const void *data, uint32_t len, void *ctx) +{ + auto &driver = *(AP_Networking_PPP *)ctx; + LWIP_UNUSED_ARG(pcb); + uint32_t remaining = len; + const uint8_t *ptr = (const uint8_t *)data; + while (remaining > 0) { + const auto n = driver.uart->write(ptr, remaining); + if (n > 0) { + remaining -= n; + ptr += n; + } else { + hal.scheduler->delay_microseconds(100); + } + } + return len; +} + +/* + callback on link status change + */ +void AP_Networking_PPP::ppp_status_callback(struct ppp_pcb_s *pcb, int code, void *ctx) +{ + auto &driver = *(AP_Networking_PPP *)ctx; + struct netif *pppif = ppp_netif(pcb); + + switch (code) { + case PPPERR_NONE: + // got new addresses for the link +#if AP_NETWORKING_PPP_GATEWAY_ENABLED + if (driver.frontend.option_is_set(AP_Networking::OPTION::PPP_ETHERNET_GATEWAY)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: got addresses"); + } else +#endif + { + driver.activeSettings.ip = ntohl(netif_ip4_addr(pppif)->addr); + driver.activeSettings.gw = ntohl(netif_ip4_gw(pppif)->addr); + driver.activeSettings.nm = ntohl(netif_ip4_netmask(pppif)->addr); + driver.activeSettings.last_change_ms = AP_HAL::millis(); + } + break; + + case PPPERR_OPEN: + case PPPERR_CONNECT: + case PPPERR_PEERDEAD: + case PPPERR_IDLETIMEOUT: + case PPPERR_CONNECTTIME: + driver.need_restart = true; + break; + + default: + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: error %d", code); + break; + } +} + + +/* + initialise PPP network backend using LWIP + */ +bool AP_Networking_PPP::init() +{ + auto &sm = AP::serialmanager(); + uart = sm.find_serial(AP_SerialManager::SerialProtocol_PPP, 0); + if (uart == nullptr) { + return false; + } + + pppif = new netif; + if (pppif == nullptr) { + return false; + } + + const bool ethernet_gateway = frontend.option_is_set(AP_Networking::OPTION::PPP_ETHERNET_GATEWAY); + if (!ethernet_gateway) { + // initialise TCP/IP thread + LWIP_TCPIP_LOCK(); + tcpip_init(NULL, NULL); + LWIP_TCPIP_UNLOCK(); + } + + hal.scheduler->delay(100); + + // create ppp connection + LWIP_TCPIP_LOCK(); + + ppp = pppos_create(pppif, ppp_output_cb, ppp_status_callback, this); + if (ppp == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: failed to create link"); + return false; + } + LWIP_TCPIP_UNLOCK(); + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: started"); + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking_PPP::ppp_loop, void), + "ppp", + 2048, AP_HAL::Scheduler::PRIORITY_NET, 0); + + return true; +} + +/* + main loop for PPP + */ +void AP_Networking_PPP::ppp_loop(void) +{ + while (!hal.scheduler->is_system_initialized()) { + hal.scheduler->delay_microseconds(1000); + } + const bool ppp_gateway = frontend.option_is_set(AP_Networking::OPTION::PPP_ETHERNET_GATEWAY); + if (ppp_gateway) { + // wait for the ethernet interface to be up + AP::network().startup_wait(); + } + + // ensure this thread owns the uart + uart->begin(AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_PPP, 0)); + uart->set_unbuffered_writes(true); + + while (true) { + uint8_t buf[1024]; + + // connect and set as default route + LWIP_TCPIP_LOCK(); + +#if AP_NETWORKING_PPP_GATEWAY_ENABLED + if (ppp_gateway) { + /* + when bridging setup the ppp interface with the same IP + as the ethernet interface, and set the remote IP address + as the local address + 1 + */ + ip4_addr_t our_ip, his_ip; + const uint32_t ip = frontend.get_ip_active(); + uint32_t rem_ip = frontend.param.remote_ppp_ip.get_uint32(); + if (rem_ip == 0) { + // use ethernet IP +1 by default + rem_ip = ip+1; + } + our_ip.addr = htonl(ip); + his_ip.addr = htonl(rem_ip); + ppp_set_ipcp_ouraddr(ppp, &our_ip); + ppp_set_ipcp_hisaddr(ppp, &his_ip); + if (netif_list != nullptr) { + const uint32_t nmask = frontend.get_netmask_param(); + if ((ip & nmask) == (rem_ip & nmask)) { + // remote PPP IP is on the same subnet as the + // local ethernet IP, so enable proxyarp to avoid + // users having to setup routes in all devices + netif_set_proxyarp(netif_list, &his_ip); + } + } + } + + // connect to the remote end + ppp_connect(ppp, 0); + + if (ppp_gateway) { + extern struct netif *netif_list; + /* + when we are setup as a PPP gateway we want the pppif to be + first in the list so routing works if it is on the same + subnet + */ + if (netif_list != nullptr && + netif_list->next != nullptr && + netif_list->next->next == pppif) { + netif_list->next->next = nullptr; + pppif->next = netif_list; + netif_list = pppif; + } + } else { + netif_set_default(pppif); + } +#else + // normal PPP link, connect to the remote end and set as the + // default route + ppp_connect(ppp, 0); + netif_set_default(pppif); +#endif // AP_NETWORKING_PPP_GATEWAY_ENABLED + + LWIP_TCPIP_UNLOCK(); + + need_restart = false; + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PPP: connected"); + + while (!need_restart) { + auto n = uart->read(buf, sizeof(buf)); + if (n > 0) { + LWIP_TCPIP_LOCK(); + pppos_input(ppp, buf, n); + LWIP_TCPIP_UNLOCK(); + } else { + hal.scheduler->delay_microseconds(200); + } + } + } +} + +#endif // AP_NETWORKING_BACKEND_PPP diff --git a/libraries/AP_Networking/AP_Networking_PPP.h b/libraries/AP_Networking/AP_Networking_PPP.h new file mode 100644 index 00000000000000..7e4a16d60a5a27 --- /dev/null +++ b/libraries/AP_Networking/AP_Networking_PPP.h @@ -0,0 +1,30 @@ +#pragma once + +#include "AP_Networking_Config.h" + +#ifdef AP_NETWORKING_BACKEND_PPP +#include "AP_Networking_Backend.h" + +class AP_Networking_PPP : public AP_Networking_Backend +{ +public: + using AP_Networking_Backend::AP_Networking_Backend; + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Networking_PPP); + + bool init() override; + +private: + void ppp_loop(void); + + AP_HAL::UARTDriver *uart; + struct netif *pppif; + struct ppp_pcb_s *ppp; + bool need_restart; + + static void ppp_status_callback(struct ppp_pcb_s *pcb, int code, void *ctx); + static uint32_t ppp_output_cb(struct ppp_pcb_s *pcb, const void *data, uint32_t len, void *ctx); +}; + +#endif // AP_NETWORKING_BACKEND_PPP diff --git a/libraries/AP_Networking/AP_Networking_SITL.h b/libraries/AP_Networking/AP_Networking_SITL.h index 915e43f5f73413..d4e9a8c65be536 100644 --- a/libraries/AP_Networking/AP_Networking_SITL.h +++ b/libraries/AP_Networking/AP_Networking_SITL.h @@ -16,7 +16,6 @@ class AP_Networking_SITL : public AP_Networking_Backend bool init() override { return true; } - void update() override {} }; #endif // AP_NETWORKING_BACKEND_SITL diff --git a/libraries/AP_Networking/AP_Networking_address.cpp b/libraries/AP_Networking/AP_Networking_address.cpp index 28900e387b2673..93997479f198b0 100644 --- a/libraries/AP_Networking/AP_Networking_address.cpp +++ b/libraries/AP_Networking/AP_Networking_address.cpp @@ -8,6 +8,7 @@ #include #include "AP_Networking.h" +#include const AP_Param::GroupInfo AP_Networking_IPV4::var_info[] = { // @Param: 0 @@ -48,7 +49,7 @@ const AP_Param::GroupInfo AP_Networking_IPV4::var_info[] = { AP_Networking_IPV4::AP_Networking_IPV4(const char *default_addr) { AP_Param::setup_object_defaults(this, var_info); - set_default_uint32(AP_Networking::convert_str_to_ip(default_addr)); + set_default_uint32(SocketAPM::inet_str_to_addr(default_addr)); } uint32_t AP_Networking_IPV4::get_uint32(void) const @@ -71,9 +72,8 @@ void AP_Networking_IPV4::set_default_uint32(uint32_t v) const char* AP_Networking_IPV4::get_str() { - const auto ip = ntohl(get_uint32()); - inet_ntop(AF_INET, &ip, strbuf, sizeof(strbuf)); - return strbuf; + const auto ip = get_uint32(); + return SocketAPM::inet_addr_to_str(ip, strbuf, sizeof(strbuf)); } #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp index 4c0e384ad6ea0b..07c35ab070800d 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -4,7 +4,7 @@ #include "AP_Networking_Config.h" -#if AP_NETWORKING_ENABLED +#if AP_NETWORKING_REGISTER_PORT_ENABLED #include "AP_Networking.h" #include @@ -38,14 +38,16 @@ const AP_Param::GroupInfo AP_Networking::Port::var_info[] = { AP_GROUPINFO_FLAGS("TYPE", 1, AP_Networking::Port, type, 0, AP_PARAM_FLAG_ENABLE), // @Param: PROTOCOL - // @DisplayName: protocol - // @Description: protocol + // @DisplayName: Protocol + // @Description: Networked serial port protocol // @User: Advanced + // @RebootRequired: True // @CopyFieldsFrom: SERIAL1_PROTOCOL AP_GROUPINFO("PROTOCOL", 2, AP_Networking::Port, state.protocol, 0), // @Group: IP // @Path: AP_Networking_address.cpp + // @RebootRequired : True AP_SUBGROUPINFO(ip, "IP", 3, AP_Networking::Port, AP_Networking_IPV4), // @Param: PORT @@ -317,10 +319,14 @@ bool AP_Networking::Port::send_receive(void) { bool active = false; - WITH_SEMAPHORE(sem); + uint32_t space; + // handle incoming packets - const auto space = readbuffer->space(); + { + WITH_SEMAPHORE(sem); + space = readbuffer->space(); + } if (space > 0) { const uint32_t n = MIN(300U, space); uint8_t buf[n]; @@ -332,6 +338,7 @@ bool AP_Networking::Port::send_receive(void) return false; } if (ret > 0) { + WITH_SEMAPHORE(sem); readbuffer->write(buf, ret); active = true; have_received = true; @@ -340,22 +347,33 @@ bool AP_Networking::Port::send_receive(void) if (connected) { // handle outgoing packets - uint32_t available = writebuffer->available(); - available = MIN(300U, available); + uint32_t available; + + { + WITH_SEMAPHORE(sem); + available = writebuffer->available(); + available = MIN(300U, available); #if HAL_GCS_ENABLED - if (packetise) { - available = mavlink_packetise(*writebuffer, available); - } + if (packetise) { + available = mavlink_packetise(*writebuffer, available); + } #endif - if (available > 0) { - uint8_t buf[available]; - auto n = writebuffer->peekbytes(buf, available); - if (n > 0) { - const auto ret = sock->send(buf, n); - if (ret > 0) { - writebuffer->advance(ret); - active = true; - } + if (available == 0) { + return active; + } + } + uint8_t buf[available]; + uint32_t n; + { + WITH_SEMAPHORE(sem); + n = writebuffer->peekbytes(buf, available); + } + if (n > 0) { + const auto ret = sock->send(buf, n); + if (ret > 0) { + WITH_SEMAPHORE(sem); + writebuffer->advance(ret); + active = true; } } } else { @@ -458,4 +476,6 @@ enum AP_HAL::UARTDriver::flow_control AP_Networking::Port::get_flow_control(void return AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE; } -#endif // AP_NETWORKING_ENABLED +#endif // AP_NETWORKING_REGISTER_PORT_ENABLED + + diff --git a/libraries/AP_Networking/AP_Networking_tests.cpp b/libraries/AP_Networking/AP_Networking_tests.cpp index d4c33f980630c6..839f17f74647ad 100644 --- a/libraries/AP_Networking/AP_Networking_tests.cpp +++ b/libraries/AP_Networking/AP_Networking_tests.cpp @@ -113,10 +113,10 @@ void AP_Networking::test_TCP_discard(void) return; } // connect to the discard service, which is port 9 - if (!sock->connect(dest, 9)) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP_discard: connect failed"); - return; + while (!sock->connect(dest, 9)) { + hal.scheduler->delay(10); } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_discard: connected"); const uint32_t bufsize = 1024; uint8_t *buf = (uint8_t*)malloc(bufsize); for (uint32_t i=0; idelay(1); + continue; + } total_sent += sock->send(buf, bufsize); const uint32_t now = AP_HAL::millis(); if (now - last_report_ms >= 1000) { diff --git a/libraries/AP_Networking/config/lwipopts.h b/libraries/AP_Networking/config/lwipopts.h new file mode 100644 index 00000000000000..c1342901c60585 --- /dev/null +++ b/libraries/AP_Networking/config/lwipopts.h @@ -0,0 +1,402 @@ +/* + * Copyright (c) 2001-2003 Swedish Institute of Computer Science. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. The name of the author may not be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT + * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT + * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + * + * This file is part of the lwIP TCP/IP stack. + * + * Author: Adam Dunkels + * + */ +#pragma once + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include "hwdef.h" +#endif + +#ifdef __cplusplus +extern "C" +{ +#endif + +/* macOS specific */ +#if defined(__APPLE__) +/* lwip/contrib/ports/unix/port/netif/sio.c */ +#include + +/* lwip/src/apps/http/makefsdata/makefsdata.c */ +#define GETCWD(path, len) getcwd(path, len) +#define GETCWD_SUCCEEDED(ret) (ret != NULL) +#define CHDIR(path) chdir(path) +#define CHDIR_SUCCEEDED(ret) (ret == 0) + +/* lwip/src/netif/ppp/fsm.c */ +/* lwip/src/netif/ppp/pppos.c */ +/* lwip/src/netif/ppp/vj.c */ +#pragma GCC diagnostic ignored "-Wimplicit-fallthrough" +#endif + +#ifdef LWIP_OPTTEST_FILE +#include "lwipopts_test.h" +#else /* LWIP_OPTTEST_FILE */ + +#define LWIP_IPV4 1 +#define LWIP_IPV6 0 + +#define MEM_LIBC_MALLOC 1 +#define MEMP_MEM_MALLOC 1 +#define LWIP_NETCONN_SEM_PER_THREAD 0 + + +#define NO_SYS 0 +#define LWIP_SOCKET (NO_SYS==0) +#define LWIP_NETCONN (NO_SYS==0) +#define LWIP_NETIF_API (NO_SYS==0) + +#ifndef LWIP_IGMP +#define LWIP_IGMP LWIP_IPV4 +#endif +#define LWIP_ICMP LWIP_IPV4 + +#define LWIP_SNMP LWIP_UDP +#ifdef LWIP_HAVE_MBEDTLS +#define LWIP_SNMP_V3 (LWIP_SNMP) +#endif + +#define LWIP_DNS 0 +#define LWIP_MDNS_RESPONDER 0 + +#define LWIP_NUM_NETIF_CLIENT_DATA (LWIP_MDNS_RESPONDER) + +#ifndef LWIP_HAVE_LOOPIF +#define LWIP_HAVE_LOOPIF 1 +#endif +#ifndef LWIP_NETIF_LOOPBACK +#define LWIP_NETIF_LOOPBACK 1 +#endif +#define LWIP_LOOPBACK_MAX_PBUFS 10 + +#define TCP_LISTEN_BACKLOG 1 + +#define LWIP_COMPAT_SOCKETS 0 +#define LWIP_SO_RCVTIMEO 1 +#define LWIP_SO_RCVBUF 1 + +#define LWIP_TCPIP_CORE_LOCKING 1 + +#define LWIP_NETIF_LINK_CALLBACK 1 +#define LWIP_NETIF_STATUS_CALLBACK 1 +#define LWIP_NETIF_EXT_STATUS_CALLBACK 1 + +#define USE_PPP 1 + +#define LWIP_TIMEVAL_PRIVATE 0 +#define LWIP_FD_SET_PRIVATE 0 + +#define TCP_WND 12000 +#define TCP_SND_BUF 12000 +#define DEFAULT_ACCEPTMBOX_SIZE 20 + + +// #define LWIP_DEBUG +#ifdef LWIP_DEBUG +#define LWIP_DBG_MIN_LEVEL 0 +#define PPP_DEBUG LWIP_DBG_ON +#define MEM_DEBUG LWIP_DBG_ON +#define MEMP_DEBUG LWIP_DBG_ON +#define PBUF_DEBUG LWIP_DBG_OFF +#define API_LIB_DEBUG LWIP_DBG_OFF +#define API_MSG_DEBUG LWIP_DBG_OFF +#define TCPIP_DEBUG LWIP_DBG_ON +#define NETIF_DEBUG LWIP_DBG_ON +#define SOCKETS_DEBUG LWIP_DBG_OFF +#define DNS_DEBUG LWIP_DBG_OFF +#define AUTOIP_DEBUG LWIP_DBG_OFF +#define DHCP_DEBUG LWIP_DBG_OFF +#define IP_DEBUG LWIP_DBG_OFF +#define IP_REASS_DEBUG LWIP_DBG_OFF +#define ICMP_DEBUG LWIP_DBG_OFF +#define IGMP_DEBUG LWIP_DBG_OFF +#define UDP_DEBUG LWIP_DBG_OFF +#define TCP_DEBUG LWIP_DBG_ON +#define TCP_INPUT_DEBUG LWIP_DBG_ON +#define TCP_OUTPUT_DEBUG LWIP_DBG_ON +#define TCP_RTO_DEBUG LWIP_DBG_OFF +#define TCP_CWND_DEBUG LWIP_DBG_OFF +#define TCP_WND_DEBUG LWIP_DBG_OFF +#define TCP_FR_DEBUG LWIP_DBG_OFF +#define TCP_QLEN_DEBUG LWIP_DBG_OFF +#define TCP_RST_DEBUG LWIP_DBG_OFF +#endif + +#define LWIP_DBG_TYPES_ON (LWIP_DBG_ON|LWIP_DBG_TRACE|LWIP_DBG_STATE|LWIP_DBG_FRESH|LWIP_DBG_HALT) + + +/* ---------- Memory options ---------- */ +/* MEM_ALIGNMENT: should be set to the alignment of the CPU for which + lwIP is compiled. 4 byte alignment -> define MEM_ALIGNMENT to 4, 2 + byte alignment -> define MEM_ALIGNMENT to 2. */ +/* MSVC port: intel processors don't need 4-byte alignment, + but are faster that way! */ +#define MEM_ALIGNMENT 4U + +/* MEM_SIZE: the size of the heap memory. If the application will send +a lot of data that needs to be copied, this should be set high. */ +#define MEM_SIZE 10240 + +/* MEMP_NUM_PBUF: the number of memp struct pbufs. If the application + sends a lot of data out of ROM (or other static memory), this + should be set high. */ +#define MEMP_NUM_PBUF 64 +/* MEMP_NUM_RAW_PCB: the number of UDP protocol control blocks. One + per active RAW "connection". */ +#define MEMP_NUM_RAW_PCB 3 +/* MEMP_NUM_UDP_PCB: the number of UDP protocol control blocks. One + per active UDP "connection". */ +#define MEMP_NUM_UDP_PCB 8 +/* MEMP_NUM_TCP_PCB: the number of simultaneously active TCP + connections. */ +#define MEMP_NUM_TCP_PCB 5 +/* MEMP_NUM_TCP_PCB_LISTEN: the number of listening TCP + connections. */ +#define MEMP_NUM_TCP_PCB_LISTEN 8 +/* MEMP_NUM_TCP_SEG: the number of simultaneously queued TCP + segments. */ +#define MEMP_NUM_TCP_SEG 16 +/* MEMP_NUM_SYS_TIMEOUT: the number of simultaneously active + timeouts. */ +#define MEMP_NUM_SYS_TIMEOUT 17 + +/* The following four are used only with the sequential API and can be + set to 0 if the application only will use the raw API. */ +/* MEMP_NUM_NETBUF: the number of struct netbufs. */ +#define MEMP_NUM_NETBUF 100 +/* MEMP_NUM_NETCONN: the number of struct netconns. */ +#define MEMP_NUM_NETCONN 64 +/* MEMP_NUM_TCPIP_MSG_*: the number of struct tcpip_msg, which is used + for sequential API communication and incoming packets. Used in + src/api/tcpip.c. */ +#define MEMP_NUM_TCPIP_MSG_API 16 +#define MEMP_NUM_TCPIP_MSG_INPKT 16 + + +/* ---------- Pbuf options ---------- */ +/* PBUF_POOL_SIZE: the number of buffers in the pbuf pool. */ +#define PBUF_POOL_SIZE 120 + +/* PBUF_POOL_BUFSIZE: the size of each pbuf in the pbuf pool. */ +#define PBUF_POOL_BUFSIZE 256 + +/** SYS_LIGHTWEIGHT_PROT + * define SYS_LIGHTWEIGHT_PROT in lwipopts.h if you want inter-task protection + * for certain critical regions during buffer allocation, deallocation and memory + * allocation and deallocation. + */ +#define SYS_LIGHTWEIGHT_PROT (NO_SYS==0) + + +/* ---------- TCP options ---------- */ +#define LWIP_TCP 1 +#define TCP_TTL 255 + +#ifndef LWIP_ALTCP +#define LWIP_ALTCP (LWIP_TCP) +#endif +#ifdef LWIP_HAVE_MBEDTLS +#define LWIP_ALTCP_TLS (LWIP_TCP) +#define LWIP_ALTCP_TLS_MBEDTLS (LWIP_TCP) +#endif + + +/* Controls if TCP should queue segments that arrive out of + order. Define to 0 if your device is low on memory. */ +#define TCP_QUEUE_OOSEQ 1 + +/* TCP Maximum segment size. */ +#define TCP_MSS 1024 + +/* TCP sender buffer space (bytes). */ +#ifndef TCP_SND_BUF +#define TCP_SND_BUF 2048 +#endif + +/* TCP sender buffer space (pbufs). This must be at least = 2 * + TCP_SND_BUF/TCP_MSS for things to work. */ +#define TCP_SND_QUEUELEN (4 * TCP_SND_BUF/TCP_MSS) + +/* TCP writable space (bytes). This must be less than or equal + to TCP_SND_BUF. It is the amount of space which must be + available in the tcp snd_buf for select to return writable */ +#define TCP_SNDLOWAT (TCP_SND_BUF/2) + +/* TCP receive window. */ +#ifndef TCP_WND +#define TCP_WND (20 * 1024) +#endif + +/* Maximum number of retransmissions of data segments. */ +#define TCP_MAXRTX 12 + +/* Maximum number of retransmissions of SYN segments. */ +#define TCP_SYNMAXRTX 4 + + +/* ---------- ARP options ---------- */ +#define LWIP_ARP 1 +#define ARP_TABLE_SIZE 10 +#define ARP_QUEUEING 1 +#ifndef ARP_PROXYARP_SUPPORT +#define ARP_PROXYARP_SUPPORT 1 +#endif + + +/* ---------- IP options ---------- */ +/* Define IP_FORWARD to 1 if you wish to have the ability to forward + IP packets across network interfaces. If you are going to run lwIP + on a device with only one network interface, define this to 0. */ +#ifndef IP_FORWARD +#define IP_FORWARD 1 +#endif + +/* + extra header space when forwarding for adding the ethernet header +*/ +#define PBUF_LINK_HLEN 16 + + +/* IP reassembly and segmentation.These are orthogonal even + * if they both deal with IP fragments */ +#define IP_REASSEMBLY 1 +#define IP_REASS_MAX_PBUFS (10 * ((1500 + PBUF_POOL_BUFSIZE - 1) / PBUF_POOL_BUFSIZE)) +#define MEMP_NUM_REASSDATA IP_REASS_MAX_PBUFS +#define IP_FRAG 1 +#define IPV6_FRAG_COPYHEADER 1 + +/* ---------- ICMP options ---------- */ +#define ICMP_TTL 255 + + +/* ---------- DHCP options ---------- */ +/* Define LWIP_DHCP to 1 if you want DHCP configuration of + interfaces. */ +#ifndef LWIP_DHCP +#define LWIP_DHCP 1 +#endif + +/* 1 if you want to do an ARP check on the offered address + (recommended). */ +#define DHCP_DOES_ARP_CHECK (LWIP_DHCP) + + +/* ---------- AUTOIP options ------- */ +#define LWIP_AUTOIP (LWIP_DHCP) +#define LWIP_DHCP_AUTOIP_COOP (LWIP_DHCP && LWIP_AUTOIP) + + +/* ---------- UDP options ---------- */ +#ifndef LWIP_UDP +#define LWIP_UDP 1 +#endif +#define LWIP_UDPLITE LWIP_UDP +#define UDP_TTL 255 + + +/* ---------- RAW options ---------- */ +#define LWIP_RAW 0 + + +/* ---------- Statistics options ---------- */ + +#define LWIP_STATS 0 +#define LWIP_STATS_DISPLAY 0 + +/* ---------- NETBIOS options ---------- */ +#define LWIP_NETBIOS_RESPOND_NAME_QUERY 0 + +/* ---------- PPP options ---------- */ + +#ifndef PPP_SUPPORT +#define PPP_SUPPORT 1 /* Set > 0 for PPP */ +#endif + +#if PPP_SUPPORT + +#define NUM_PPP 1 /* Max PPP sessions. */ + + +/* Select modules to enable. Ideally these would be set in the makefile but + * we're limited by the command line length so you need to modify the settings + * in this file. + */ +#define PPPOE_SUPPORT 0 +#define PPPOS_SUPPORT PPP_SUPPORT + +#define PAP_SUPPORT 0 /* Set > 0 for PAP. */ +#define CHAP_SUPPORT 0 /* Set > 0 for CHAP. */ +#define MSCHAP_SUPPORT 0 /* Set > 0 for MSCHAP */ +#define CBCP_SUPPORT 0 /* Set > 0 for CBCP (NOT FUNCTIONAL!) */ +#define CCP_SUPPORT 0 /* Set > 0 for CCP */ +/* + VJ support disabled due to bugs with IP forwarding + */ +#define VJ_SUPPORT 0 /* Set > 0 for VJ header compression. */ +#define MD5_SUPPORT 0 /* Set > 0 for MD5 (see also CHAP) */ + +#endif /* PPP_SUPPORT */ + +#endif /* LWIP_OPTTEST_FILE */ + +/* The following defines must be done even in OPTTEST mode: */ + +#if !defined(NO_SYS) || !NO_SYS /* default is 0 */ +void sys_check_core_locking(void); +#define LWIP_ASSERT_CORE_LOCKED() sys_check_core_locking() +#endif + +#ifndef LWIP_PLATFORM_ASSERT +/* Define LWIP_PLATFORM_ASSERT to something to catch missing stdio.h includes */ +void ap_networking_platform_assert(const char *msg, int line, const char *file); +#define LWIP_PLATFORM_ASSERT(x) ap_networking_platform_assert(x, __LINE__, __FILE__) +#endif + +/* + map LWIP debugging onto ap_networking_printf to allow for easier + redirection to a file or dedicated serial port + */ +#ifdef __cplusplus +extern "C" { +#endif +int ap_networking_printf(const char *fmt, ...); +#ifdef __cplusplus +} +#endif +#define LWIP_PLATFORM_DIAG(x) do {ap_networking_printf x; } while(0) + +// #define AP_NETWORKING_LWIP_DEBUG_FILE "lwip.log" + +#ifdef __cplusplus +} +#endif + diff --git a/libraries/AP_Networking/lwip_hal/arch/evtimer.c b/libraries/AP_Networking/lwip_hal/arch/evtimer.c new file mode 100644 index 00000000000000..56657904ed7619 --- /dev/null +++ b/libraries/AP_Networking/lwip_hal/arch/evtimer.c @@ -0,0 +1,11 @@ +/* + wrapper around evtimer.c so we only build when events are + enabled. This prevents a complex check in the ChibiOS mk layer + */ +#include + +#if CH_CFG_USE_EVENTS +// this include relies on -I for modules/ChibiOS/os/various/cpp_wrappers +#include <../evtimer.c> +#endif + diff --git a/libraries/AP_Networking/lwip_hal/arch/sys_arch.cpp b/libraries/AP_Networking/lwip_hal/arch/sys_arch.cpp new file mode 100644 index 00000000000000..c84133a294bbd5 --- /dev/null +++ b/libraries/AP_Networking/lwip_hal/arch/sys_arch.cpp @@ -0,0 +1,396 @@ +/* + port of lwip to ArduPilot AP_HAL + This is partly based on ChibiOS/os/various/lwip_bindings + */ + +#include +#include + +#if AP_NETWORKING_NEED_LWIP +#include +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include + +extern "C" { +#include "lwip/debug.h" +#include "lwip/def.h" +#include "lwip/sys.h" +#include "lwip/opt.h" +#include "lwip/stats.h" +#include "lwip/tcpip.h" +} + +extern const AP_HAL::HAL &hal; + +unsigned int +lwip_port_rand(void) +{ + return (u32_t)rand(); +} + +static HAL_Semaphore lwprot_mutex; +static HAL_Semaphore tcpip_mutex; + +struct sys_mbox_msg { + struct sys_mbox_msg *next; + void *msg; +}; + +#define SYS_MBOX_SIZE 128 + +struct sys_mbox { + int first, last; + void *msgs[SYS_MBOX_SIZE]; + HAL_BinarySemaphore not_empty; + HAL_BinarySemaphore not_full; + HAL_BinarySemaphore mutex; + int wait_send; +}; + +class ThreadWrapper { +public: + ThreadWrapper(lwip_thread_fn fn, void *_arg) : + function(fn), + arg(_arg) + {} + bool create(const char *name, int stacksize, int prio) { +#ifdef HAL_BOOTLOADER_BUILD + return thread_create_alloc(MAX(stacksize,2048), name, 60, function, arg); +#else + return hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&ThreadWrapper::run, void), name, MAX(stacksize,2048), AP_HAL::Scheduler::PRIORITY_NET, prio); +#endif + } +private: + void run(void) { + function(arg); + } + lwip_thread_fn function; + void *arg; +}; + +sys_thread_t +sys_thread_new(const char *name, lwip_thread_fn function, void *arg, int stacksize, int prio) +{ + auto *thread_data = new ThreadWrapper(function, arg); + if (!thread_data->create(name, stacksize, prio)) { + AP_HAL::panic("lwip: Failed to start thread %s", name); + } + return (sys_thread_t)thread_data; +} + +void sys_lock_tcpip_core(void) +{ + tcpip_mutex.take_blocking(); +} + +void sys_unlock_tcpip_core(void) +{ + tcpip_mutex.give(); +} + +void sys_mark_tcpip_thread(void) +{ +} + +void sys_check_core_locking(void) +{ + /* Embedded systems should check we are NOT in an interrupt + * context here */ +} + +/*-----------------------------------------------------------------------------------*/ +/* Mailbox */ +err_t +sys_mbox_new(struct sys_mbox **mb, int size) +{ + struct sys_mbox *mbox; + LWIP_UNUSED_ARG(size); + + mbox = new sys_mbox; + if (mbox == NULL) { + return ERR_MEM; + } + mbox->first = mbox->last = 0; + mbox->mutex.signal(); + mbox->wait_send = 0; + + *mb = mbox; + return ERR_OK; +} + +void +sys_mbox_free(struct sys_mbox **mb) +{ + if ((mb != NULL) && (*mb != SYS_MBOX_NULL)) { + struct sys_mbox *mbox = *mb; + mbox->mutex.wait_blocking(); + delete mbox; + } +} + +err_t +sys_mbox_trypost(struct sys_mbox **mb, void *msg) +{ + u8_t first; + struct sys_mbox *mbox; + LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL)); + mbox = *mb; + + mbox->mutex.wait_blocking(); + + LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_trypost: mbox %p msg %p\n", + (void *)mbox, (void *)msg)); + + if ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) { + mbox->mutex.signal(); + return ERR_MEM; + } + + mbox->msgs[mbox->last % SYS_MBOX_SIZE] = msg; + + if (mbox->last == mbox->first) { + first = 1; + } else { + first = 0; + } + + mbox->last++; + + if (first) { + mbox->not_empty.signal(); + } + + mbox->mutex.signal(); + + return ERR_OK; +} + +void +sys_mbox_post(struct sys_mbox **mb, void *msg) +{ + u8_t first; + struct sys_mbox *mbox; + LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL)); + mbox = *mb; + + mbox->mutex.wait_blocking(); + + LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_post: mbox %p msg %p\n", (void *)mbox, (void *)msg)); + + while ((mbox->last + 1) >= (mbox->first + SYS_MBOX_SIZE)) { + mbox->wait_send++; + mbox->mutex.signal(); + mbox->not_full.wait_blocking(); + mbox->mutex.wait_blocking(); + mbox->wait_send--; + } + + mbox->msgs[mbox->last % SYS_MBOX_SIZE] = msg; + + if (mbox->last == mbox->first) { + first = 1; + } else { + first = 0; + } + + mbox->last++; + + if (first) { + mbox->not_empty.signal(); + } + + mbox->mutex.signal(); +} + +u32_t +sys_arch_mbox_tryfetch(struct sys_mbox **mb, void **msg) +{ + struct sys_mbox *mbox = *mb; + + mbox->mutex.wait_blocking(); + + if (mbox->first == mbox->last) { + mbox->mutex.signal(); + return SYS_MBOX_EMPTY; + } + + if (msg != NULL) { + LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_tryfetch: mbox %p msg %p\n", (void *)mbox, *msg)); + *msg = mbox->msgs[mbox->first % SYS_MBOX_SIZE]; + } else { + LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_tryfetch: mbox %p, null msg\n", (void *)mbox)); + } + + mbox->first++; + + if (mbox->wait_send) { + mbox->not_full.signal(); + } + + mbox->mutex.signal(); + + return 0; +} + +u32_t +sys_arch_mbox_fetch(struct sys_mbox **mb, void **msg, u32_t timeout_ms) +{ + struct sys_mbox *mbox; + LWIP_ASSERT("invalid mbox", (mb != NULL) && (*mb != NULL)); + mbox = *mb; + + mbox->mutex.wait_blocking(); + + while (mbox->first == mbox->last) { + mbox->mutex.signal(); + + /* We block while waiting for a mail to arrive in the mailbox. We + must be prepared to timeout. */ + if (timeout_ms != 0) { + if (!mbox->not_empty.wait(timeout_ms*1000U)) { + return SYS_ARCH_TIMEOUT; + } + } else { + mbox->not_empty.wait_blocking(); + } + + mbox->mutex.wait_blocking(); + } + + if (msg != NULL) { + LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_fetch: mbox %p msg %p\n", (void *)mbox, *msg)); + *msg = mbox->msgs[mbox->first % SYS_MBOX_SIZE]; + } else { + LWIP_DEBUGF(SYS_DEBUG, ("sys_mbox_fetch: mbox %p, null msg\n", (void *)mbox)); + } + + mbox->first++; + + if (mbox->wait_send) { + mbox->not_full.signal(); + } + + mbox->mutex.signal(); + + return 0; +} + +err_t +sys_sem_new(sys_sem_t *sem, u8_t count) +{ + *sem = (sys_sem_t)new HAL_BinarySemaphore(count); + if (*sem == NULL) { + return ERR_MEM; + } + return ERR_OK; +} + +u32_t +sys_arch_sem_wait(sys_sem_t *s, u32_t timeout_ms) +{ + HAL_BinarySemaphore *sem = (HAL_BinarySemaphore *)*s; + if (timeout_ms == 0) { + return sem->wait_blocking()?0:SYS_ARCH_TIMEOUT; + } + return sem->wait(timeout_ms*1000U)?0:SYS_ARCH_TIMEOUT; +} + +void +sys_sem_signal(sys_sem_t *s) +{ + HAL_BinarySemaphore *sem = (HAL_BinarySemaphore *)*s; + sem->signal(); +} + +void +sys_sem_free(sys_sem_t *sem) +{ + delete ((HAL_BinarySemaphore *)*sem); +} + +/*-----------------------------------------------------------------------------------*/ +/* Mutex */ +/** Create a new mutex + * @param mutex pointer to the mutex to create + * @return a new mutex */ +err_t +sys_mutex_new(sys_mutex_t *mutex) +{ + *mutex = (sys_mutex_t)new HAL_Semaphore; + if (*mutex == nullptr) { + return ERR_MEM; + } + return ERR_OK; +} + +/** Lock a mutex + * @param mutex the mutex to lock */ +void +sys_mutex_lock(sys_mutex_t *mutex) +{ + ((HAL_Semaphore*)*mutex)->take_blocking(); +} + +/** Unlock a mutex + * @param mutex the mutex to unlock */ +void +sys_mutex_unlock(sys_mutex_t *mutex) +{ + ((HAL_Semaphore*)*mutex)->give(); +} + +/** Delete a mutex + * @param mutex the mutex to delete */ +void +sys_mutex_free(sys_mutex_t *mutex) +{ + delete (HAL_Semaphore*)*mutex; +} + +u32_t +sys_now(void) +{ + return AP_HAL::millis(); +} + +u32_t +sys_jiffies(void) +{ + return AP_HAL::micros(); +} + +void +sys_init(void) +{ +} + +sys_prot_t +sys_arch_protect(void) +{ + lwprot_mutex.take_blocking(); + return 0; +} + +void +sys_arch_unprotect(sys_prot_t pval) +{ + LWIP_UNUSED_ARG(pval); + lwprot_mutex.give(); +} + +#endif // AP_NETWORKING_NEED_LWIP + diff --git a/libraries/AP_Networking/lwip_hal/include/arch/cc.h b/libraries/AP_Networking/lwip_hal/include/arch/cc.h new file mode 100644 index 00000000000000..56747da1ffef0f --- /dev/null +++ b/libraries/AP_Networking/lwip_hal/include/arch/cc.h @@ -0,0 +1,21 @@ +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define LWIP_ERRNO_STDINCLUDE 1 + +extern unsigned int lwip_port_rand(void); +#define LWIP_RAND() (lwip_port_rand()) + +typedef uint32_t sys_prot_t; + +#ifdef __cplusplus +} +#endif + diff --git a/libraries/AP_Networking/lwip_hal/include/arch/sys_arch.h b/libraries/AP_Networking/lwip_hal/include/arch/sys_arch.h new file mode 100644 index 00000000000000..f376b2f8c42f88 --- /dev/null +++ b/libraries/AP_Networking/lwip_hal/include/arch/sys_arch.h @@ -0,0 +1,47 @@ +#pragma once + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define SYS_MBOX_NULL NULL +#define SYS_SEM_NULL NULL + +struct sys_sem; +typedef struct sys_sem * sys_sem_t; +#define sys_sem_valid(sem) (((sem) != NULL) && (*(sem) != NULL)) +#define sys_sem_valid_val(sem) ((sem) != NULL) +#define sys_sem_set_invalid(sem) do { if((sem) != NULL) { *(sem) = NULL; }}while(0) +#define sys_sem_set_invalid_val(sem) do { (sem) = NULL; }while(0) + +struct sys_mutex; +typedef struct sys_mutex * sys_mutex_t; +#define sys_mutex_valid(mutex) sys_sem_valid(mutex) +#define sys_mutex_set_invalid(mutex) sys_sem_set_invalid(mutex) + +struct sys_mbox; +typedef struct sys_mbox * sys_mbox_t; +#define sys_mbox_valid(mbox) sys_sem_valid(mbox) +#define sys_mbox_valid_val(mbox) sys_sem_valid_val(mbox) +#define sys_mbox_set_invalid(mbox) sys_sem_set_invalid(mbox) +#define sys_mbox_set_invalid_val(mbox) sys_sem_set_invalid_val(mbox) + +typedef void *sys_thread_t; + +void sys_mark_tcpip_thread(void); +#define LWIP_MARK_TCPIP_THREAD() sys_mark_tcpip_thread() + +#if LWIP_TCPIP_CORE_LOCKING +void sys_lock_tcpip_core(void); +#define LOCK_TCPIP_CORE() sys_lock_tcpip_core() +void sys_unlock_tcpip_core(void); +#define UNLOCK_TCPIP_CORE() sys_unlock_tcpip_core() +#endif + +struct timeval; + +#ifdef __cplusplus +} +#endif + diff --git a/libraries/AP_Networking/wscript b/libraries/AP_Networking/wscript new file mode 100644 index 00000000000000..454a350ba24c15 --- /dev/null +++ b/libraries/AP_Networking/wscript @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 + +import pathlib +import platform + +def configure(cfg): + + if not cfg.env.BOARD_CLASS in ['SITL', 'LINUX', 'ChibiOS']: + return + + # networking doesn't build with clang unless using macOS + if platform.system() != 'Darwin' and 'clang++' in cfg.env.COMPILER_CXX: + return + + extra_src = [ + 'modules/lwip/src/core/*c', + 'modules/lwip/src/core/ipv4/*c', + 'modules/lwip/src/api/*c', + 'modules/lwip/src/netif/*c', + 'modules/lwip/src/netif/ppp/*c', + ] + + extra_src_inc = [ + 'modules/lwip/src/include', + ] + + extra_src.extend(['libraries/AP_Networking/lwip_hal/arch/*cpp']) + + if cfg.env.BOARD_CLASS == 'ChibiOS': + extra_src.extend(['libraries/AP_Networking/lwip_hal/arch/evtimer.c']) + + extra_src_inc.extend(['libraries/AP_Networking/config', + 'libraries/AP_Networking/lwip_hal/include']) + + cfg.env.AP_LIB_EXTRA_SOURCES['AP_Networking'] = [] + for pattern in extra_src: + s = cfg.srcnode.ant_glob(pattern, dir=False, src=True) + for x in s: + cfg.env.AP_LIB_EXTRA_SOURCES['AP_Networking'].append(str(x)) + + for inc in extra_src_inc: + cfg.env.INCLUDES += [str(cfg.srcnode.make_node(inc))] diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index bf3b7b1024510a..1a1b6d2158276f 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -535,6 +535,27 @@ void AP_Notify::send_text(const char *str) _send_text_updated_millis = AP_HAL::millis(); } +#if AP_SCRIPTING_ENABLED +void AP_Notify::send_text_scripting(const char *str, uint8_t r) +{ + for (uint8_t i = 0; i < _num_devices; i++) { + if (_devices[i] != nullptr) { + _devices[i]->send_text_blocking(str, r); + } + } +} + +void AP_Notify::release_text_scripting(uint8_t r) +{ + for (uint8_t i = 0; i < _num_devices; i++) { + if (_devices[i] != nullptr) { + _devices[i]->release_text(r); + } + } +} +#endif + + // convert 0-3 to 0-100 int8_t AP_Notify::get_rgb_led_brightness_percent() const { diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 0db8c825bb8ce5..b71988bdf13097 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -210,6 +210,12 @@ class AP_Notify void send_text(const char *str); const char* get_text() const { return _send_text; } uint32_t get_text_updated_millis() const {return _send_text_updated_millis; } + +#if AP_SCRIPTING_ENABLED + // send text to the display using scripting + void send_text_scripting(const char *str, uint8_t r); + void release_text_scripting(uint8_t r); +#endif static const struct AP_Param::GroupInfo var_info[]; int8_t get_buzz_pin() const { return _buzzer_pin; } diff --git a/libraries/AP_Notify/DShotLED.cpp b/libraries/AP_Notify/DShotLED.cpp index 798905a990e931..6b895b88bee6da 100644 --- a/libraries/AP_Notify/DShotLED.cpp +++ b/libraries/AP_Notify/DShotLED.cpp @@ -23,7 +23,8 @@ extern const AP_HAL::HAL& hal; bool DShotLED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) { // don't play the motor LEDs while flying - if (hal.util->get_soft_armed() || hal.rcout->get_dshot_esc_type() != AP_HAL::RCOutput::DSHOT_ESC_BLHELI) { + if (hal.util->get_soft_armed() || (hal.rcout->get_dshot_esc_type() != AP_HAL::RCOutput::DSHOT_ESC_BLHELI + && hal.rcout->get_dshot_esc_type() != AP_HAL::RCOutput::DSHOT_ESC_BLHELI_EDT)) { return false; } diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp index 301cc862f3cc63..a17b72637996e4 100644 --- a/libraries/AP_Notify/Display.cpp +++ b/libraries/AP_Notify/Display.cpp @@ -404,15 +404,53 @@ void Display::update() void Display::update_all() { - update_text(0); - update_mode(1); - update_battery(2); + if(!BIT_IS_SET(_send_text_scr_override, 0)) { + update_text(0); + } + + if(!BIT_IS_SET(_send_text_scr_override, 1)) { + update_mode(1); + } + +#if AP_BATTERY_ENABLED + if(!BIT_IS_SET(_send_text_scr_override, 2)) { + update_battery(2); + } +#endif #if AP_GPS_ENABLED - update_gps(3); + if(!BIT_IS_SET(_send_text_scr_override, 3)) { + update_gps(3); + } #endif - //update_gps_sats(4); - update_prearm(4); - update_ekf(5); + + if(!BIT_IS_SET(_send_text_scr_override, 4)) { + //update_gps_sats(4); + update_prearm(4); + } + + if(!BIT_IS_SET(_send_text_scr_override, 5)) { + update_ekf(5); + } +} + +void Display::send_text_blocking(const char *text, uint8_t r) +{ + if (text == nullptr) { + return; + } + if (r >= DISPLAY_TEXT_NUM_ROWS) { + return; + } + BIT_SET(_send_text_scr_override, r); + char txt [DISPLAY_MESSAGE_SIZE] = {}; + memset(txt, ' ', DISPLAY_MESSAGE_SIZE); + memcpy(txt, text, strnlen(text, DISPLAY_MESSAGE_SIZE)); + draw_text(COLUMN(0), ROW(r), txt); +} + +void Display::release_text(uint8_t r) +{ + BIT_CLEAR(_send_text_scr_override, r); } void Display::draw_text(uint16_t x, uint16_t y, const char* c) @@ -530,6 +568,7 @@ void Display::update_ekf(uint8_t r) } } +#if AP_BATTERY_ENABLED void Display::update_battery(uint8_t r) { char msg [DISPLAY_MESSAGE_SIZE]; @@ -541,7 +580,8 @@ void Display::update_battery(uint8_t r) snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT:%4.2fV --%% ", (double)battery.voltage()) ; } draw_text(COLUMN(0), ROW(r), msg); - } +} +#endif // AP_BATTERY_ENABLED void Display::update_mode(uint8_t r) { diff --git a/libraries/AP_Notify/Display.h b/libraries/AP_Notify/Display.h index 5c0fbb3e857b58..6243a3696e6a08 100644 --- a/libraries/AP_Notify/Display.h +++ b/libraries/AP_Notify/Display.h @@ -9,6 +9,7 @@ #define ROW(Y) ((Y * 10) + 6) #define COLUMN(X) ((X * 7) + 0) +#define DISPLAY_TEXT_NUM_ROWS 6 #define DISPLAY_MESSAGE_SIZE 19 class Display_Backend; @@ -19,7 +20,8 @@ class Display: public NotifyDevice { bool init(void) override; void update() override; - + void send_text_blocking(const char *text, uint8_t line) override; + void release_text(uint8_t line) override; private: void draw_char(uint16_t x, uint16_t y, const char c); void draw_text(uint16_t x, uint16_t y, const char *c); @@ -42,6 +44,9 @@ class Display: public NotifyDevice { // stop showing text in display after this many millis: const uint16_t _send_text_valid_millis = 20000; + + //Bitmask of what lines send_text_scripting should override + uint8_t _send_text_scr_override; }; #endif // HAL_DISPLAY_ENABLED diff --git a/libraries/AP_Notify/NotifyDevice.h b/libraries/AP_Notify/NotifyDevice.h index 2f2f76ca92a306..f5f6d89d804bb4 100644 --- a/libraries/AP_Notify/NotifyDevice.h +++ b/libraries/AP_Notify/NotifyDevice.h @@ -30,6 +30,10 @@ class NotifyDevice { // give RGB value for single led virtual void rgb_set_id(uint8_t r, uint8_t g, uint8_t b, uint8_t id) {} + // Allow text to be sent or removed from a display + virtual void send_text_blocking(const char *text, uint8_t line) {} + virtual void release_text(uint8_t line) {} + // this pointer is used to read the parameters relative to devices const AP_Notify *pNotify; }; diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index d4284cc7208981..4329c423e55d7d 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -446,6 +446,7 @@ void AP_OSD::update_stats() // maximum altitude alt = -alt; _stats.max_alt_m = fmaxf(_stats.max_alt_m, alt); +#if AP_BATTERY_ENABLED // maximum current AP_BattMonitor &battery = AP::battery(); float amps; @@ -457,11 +458,14 @@ void AP_OSD::update_stats() if (voltage > 0) { _stats.min_voltage_v = fminf(_stats.min_voltage_v, voltage); } +#endif +#if AP_RSSI_ENABLED // minimum rssi AP_RSSI *ap_rssi = AP_RSSI::get_singleton(); if (ap_rssi) { _stats.min_rssi = fminf(_stats.min_rssi, ap_rssi->read_receiver_rssi()); } +#endif // max airspeed either true or synthetic if (have_airspeed_estimate) { _stats.max_airspeed_mps = fmaxf(_stats.max_airspeed_mps, aspd_mps); @@ -509,6 +513,7 @@ void AP_OSD::update_current_screen() return; } +#if AP_RC_CHANNEL_ENABLED RC_Channel *channel = RC_Channels::rc_channel(rc_channel-1); if (channel == nullptr) { return; @@ -564,6 +569,7 @@ void AP_OSD::update_current_screen() break; } switch_debouncer = false; +#endif // AP_RC_CHANNEL_ENABLED } //select next avaliable screen, do nothing if all screens disabled diff --git a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp index 0e46dfc2cd7848..dccc059b570cfa 100644 --- a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp +++ b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp @@ -39,12 +39,12 @@ bool AP_OSD_MSP_DisplayPort::init(void) // check if we have a DisplayPort backend to use const AP_MSP *msp = AP::msp(); if (msp == nullptr) { - gcs().send_text(MAV_SEVERITY_WARNING,"MSP backend not available"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP backend not available"); return false; } _displayport = msp->find_protocol(AP_SerialManager::SerialProtocol_MSP_DisplayPort); if (_displayport == nullptr) { - gcs().send_text(MAV_SEVERITY_WARNING,"MSP DisplayPort uart not available"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"MSP DisplayPort uart not available"); return false; } // re-init port here for use in this thread diff --git a/libraries/AP_OSD/AP_OSD_ParamScreen.cpp b/libraries/AP_OSD/AP_OSD_ParamScreen.cpp index f61e2d87125cca..9dd4e77bedfdf2 100644 --- a/libraries/AP_OSD/AP_OSD_ParamScreen.cpp +++ b/libraries/AP_OSD/AP_OSD_ParamScreen.cpp @@ -164,11 +164,11 @@ static const AP_OSD_ParamSetting::Initializer PARAM_DEFAULTS[AP_OSD_NUM_PARAM_SC }, { { 1, { 185, 0, 0 }, OSD_PARAM_NONE }, // TRIM_THROTTLE - { 2, { 155, 0, 0 }, OSD_PARAM_NONE }, // TRIM_ARSPD_CM + { 2, { 155, 0, 0 }, OSD_PARAM_NONE }, // AIRSPEED_CRUISE { 3, { 4, 0, 1094 }, OSD_PARAM_NONE }, // SERVO_AUTO_TRIM - { 4, { 120, 0, 0 }, OSD_PARAM_NONE}, // ARSPD_FBW_MIN - { 5, { 121, 0, 0 }, OSD_PARAM_NONE }, // ARSPD_FBW_MAX - { 6, { 156, 0, 0 }, OSD_PARAM_NONE }, // ALT_HOLD_RTL + { 4, { 120, 0, 0 }, OSD_PARAM_NONE}, // AIRSPEED_MIN + { 5, { 121, 0, 0 }, OSD_PARAM_NONE }, // AIRSPEED_MAX + { 6, { 156, 0, 0 }, OSD_PARAM_NONE }, // RTL_ALTITUDE { 7, { 140, 2, 8 }, OSD_PARAM_NONE }, // AHRS_TRIM_Y { 8, { 182, 0, 0 }, OSD_PARAM_NONE }, // THR_MAX { 9, { 189, 0, 0 }, OSD_PARAM_NONE } // THR_SLEWRATE diff --git a/libraries/AP_OSD/AP_OSD_ParamSetting.cpp b/libraries/AP_OSD/AP_OSD_ParamSetting.cpp index 6c3d7723a67b92..9efdef9b3659a9 100644 --- a/libraries/AP_OSD/AP_OSD_ParamSetting.cpp +++ b/libraries/AP_OSD/AP_OSD_ParamSetting.cpp @@ -117,7 +117,7 @@ static const char* SERIAL_PROTOCOL_VALUES[] = { "FSKY_TX", "LID360", "", "BEACN", "VOLZ", "SBUS", "ESC_TLM", "DEV_TLM", "OPTFLW", "RBTSRV", "NMEA", "WNDVNE", "SLCAN", "RCIN", "MGSQRT", "LTM", "RUNCAM", "HOT_TLM", "SCRIPT", "CRSF", "GEN", "WNCH", "MSP", "DJI", "AIRSPD", "ADSB", "AHRS", "AUDIO", "FETTEC", "TORQ", - "AIS", "CD_ESC", "MSP_DP", "MAV_HL", "TRAMP", "DDS", "IMUOUT", + "AIS", "CD_ESC", "MSP_DP", "MAV_HL", "TRAMP", "DDS", "IMUOUT", "IQ", "PPP", }; static_assert(AP_SerialManager::SerialProtocol_NumProtocols == ARRAY_SIZE(SERIAL_PROTOCOL_VALUES), "Wrong size SerialProtocol_NumProtocols"); diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index cdc0e2a59a9c19..1309707f9c2a9b 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -1334,6 +1334,7 @@ void AP_OSD_Screen::draw_altitude(uint8_t x, uint8_t y) backend->write(x, y, false, "%4d%c", (int)u_scale(ALTITUDE, alt), u_icon(ALTITUDE)); } +#if AP_BATTERY_ENABLED void AP_OSD_Screen::draw_bat_volt(uint8_t instance, VoltageType type, uint8_t x, uint8_t y) { AP_BattMonitor &battery = AP::battery(); @@ -1409,7 +1410,9 @@ void AP_OSD_Screen::draw_restvolt(uint8_t x, uint8_t y) { draw_bat_volt(0,VoltageType::RESTING_VOLTAGE,x,y); } +#endif // AP_BATTERY_ENABLED +#if AP_RSSI_ENABLED void AP_OSD_Screen::draw_rssi(uint8_t x, uint8_t y) { AP_RSSI *ap_rssi = AP_RSSI::get_singleton(); @@ -1431,7 +1434,9 @@ void AP_OSD_Screen::draw_link_quality(uint8_t x, uint8_t y) } } } +#endif // AP_RSSI_ENABLED +#if AP_BATTERY_ENABLED void AP_OSD_Screen::draw_current(uint8_t instance, uint8_t x, uint8_t y) { float amps; @@ -1452,6 +1457,7 @@ void AP_OSD_Screen::draw_current(uint8_t x, uint8_t y) { draw_current(0, x, y); } +#endif void AP_OSD_Screen::draw_fltmode(uint8_t x, uint8_t y) { @@ -1475,6 +1481,7 @@ void AP_OSD_Screen::draw_sats(uint8_t x, uint8_t y) backend->write(x, y, flash, "%c%c%2u", SYMBOL(SYM_SAT_L), SYMBOL(SYM_SAT_R), nsat); } +#if AP_BATTERY_ENABLED void AP_OSD_Screen::draw_batused(uint8_t instance, uint8_t x, uint8_t y) { float mah; @@ -1493,6 +1500,7 @@ void AP_OSD_Screen::draw_batused(uint8_t x, uint8_t y) { draw_batused(0, x, y); } +#endif //Autoscroll message is the same as in minimosd-extra. //Thanks to night-ghost for the approach. @@ -1569,7 +1577,7 @@ void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y) float angle = 0; const float length = v.length(); if (length > 1.0f) { - angle = atan2f(v.y, v.x) - ahrs.yaw; + angle = atan2f(v.y, v.x) - ahrs.get_yaw(); } draw_speed(x + 1, y, angle, length); } @@ -1820,7 +1828,7 @@ void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y) if (check_option(AP_OSD::OPTION_INVERTED_WIND)) { angle = M_PI; } - angle = angle + atan2f(v.y, v.x) - ahrs.yaw; + angle = angle + atan2f(v.y, v.x) - ahrs.get_yaw(); } draw_speed(x + 1, y, angle, length); @@ -2033,6 +2041,7 @@ void AP_OSD_Screen::draw_flightime(uint8_t x, uint8_t y) } } +#if AP_BATTERY_ENABLED void AP_OSD_Screen::draw_eff(uint8_t x, uint8_t y) { AP_BattMonitor &battery = AP::battery(); @@ -2050,7 +2059,9 @@ void AP_OSD_Screen::draw_eff(uint8_t x, uint8_t y) backend->write(x, y, false, "%c---%c", SYMBOL(SYM_EFF),SYMBOL(SYM_MAH)); } } +#endif // AP_BATTERY_ENABLED +#if AP_BATTERY_ENABLED void AP_OSD_Screen::draw_climbeff(uint8_t x, uint8_t y) { char unit_icon = u_icon(DISTANCE); @@ -2080,6 +2091,7 @@ void AP_OSD_Screen::draw_climbeff(uint8_t x, uint8_t y) backend->write(x, y, false,"%c%c---%c",SYMBOL(SYM_PTCHUP),SYMBOL(SYM_EFF),unit_icon); } } +#endif #if BARO_MAX_INSTANCES > 1 void AP_OSD_Screen::draw_btemp(uint8_t x, uint8_t y) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 137a5778b55c8f..0a5c89aa6689d0 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -14,6 +14,7 @@ #include "AP_OpticalFlow_UPFLOW.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -267,6 +268,7 @@ void AP_OpticalFlow::update_state(const OpticalFlow_state &state) _state = state; _last_update_ms = AP_HAL::millis(); +#if AP_AHRS_ENABLED // write to log and send to EKF if new data has arrived AP::ahrs().writeOptFlowMeas(quality(), _state.flowRate, @@ -274,6 +276,7 @@ void AP_OpticalFlow::update_state(const OpticalFlow_state &state) _last_update_ms, get_pos_offset(), get_height_override()); +#endif #if HAL_LOGGING_ENABLED Log_Write_Optflow(); #endif diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow.h index c27498c6dacf9b..8a68827cfd0c0b 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.h @@ -79,10 +79,10 @@ class AP_OpticalFlow // quality - returns the surface quality as a measure from 0 ~ 255 uint8_t quality() const { return _state.surface_quality; } - // raw - returns the raw movement from the sensor + // flowRate - returns the raw movement from the sensor in rad/s const Vector2f& flowRate() const { return _state.flowRate; } - // velocity - returns the velocity in m/s + // bodyRate - returns the IMU-adjusted movement in rad/s const Vector2f& bodyRate() const { return _state.bodyRate; } // last_update() - returns system time of last sensor update diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.cpp index a23397881e3aaa..6b28d68d4c5448 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.cpp @@ -22,6 +22,7 @@ #include #include +#include const uint32_t AP_OPTICALFLOW_CAL_TIMEOUT_SEC = 120; // calibration timesout after 120 seconds const uint32_t AP_OPTICALFLOW_CAL_STATUSINTERVAL_SEC = 3; // status updates printed at 3 second intervals diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.h index 13b9c5c0dc8c99..3f894ce4751472 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Calibrator.h @@ -1,6 +1,7 @@ #pragma once #include +#include #define AP_OPTICALFLOW_CAL_MAX_SAMPLES 50 // number of samples required before calibration begins @@ -53,8 +54,10 @@ class AP_OpticalFlow_Calibrator { // calculate mean squared residual for all samples of a single axis (0 or 1) given a scalar parameter float calc_mean_squared_residuals(uint8_t axis, float scalar) const; +#if HAL_LOGGING_ENABLED // log a sample void log_sample(uint8_t axis, uint8_t sample_num, float flow_rate, float body_rate, float los_pred); +#endif // calibration states enum class CalState { diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp index e7a94704420d00..123b3408c3abd3 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp @@ -53,8 +53,8 @@ void AP_OpticalFlow_HereFlow::handle_measurement(AP_DroneCAN *ap_dronecan, const if (_ap_dronecan == ap_dronecan && _node_id == transfer.source_node_id) { WITH_SEMAPHORE(_driver->_sem); _driver->new_data = true; - _driver->flowRate = Vector2f(msg.flow_integral[0], msg.flow_integral[1]); - _driver->bodyRate = Vector2f(msg.rate_gyro_integral[0], msg.rate_gyro_integral[1]); + _driver->flow_integral = Vector2f(msg.flow_integral[0], msg.flow_integral[1]); + _driver->rate_gyro_integral = Vector2f(msg.rate_gyro_integral[0], msg.rate_gyro_integral[1]); _driver->integral_time = msg.integration_interval; _driver->surface_quality = msg.quality; } @@ -79,9 +79,11 @@ void AP_OpticalFlow_HereFlow::_push_state(void) float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y; float integralToRate = 1.0f / integral_time; //Convert to Raw Flow measurement to Flow Rate measurement - state.flowRate = Vector2f(flowRate.x * flowScaleFactorX, - flowRate.y * flowScaleFactorY) * integralToRate; - state.bodyRate = bodyRate * integralToRate; + state.flowRate = Vector2f{ + flow_integral.x * flowScaleFactorX, + flow_integral.y * flowScaleFactorY + } * integralToRate; + state.bodyRate = rate_gyro_integral * integralToRate; state.surface_quality = surface_quality; _applyYaw(state.flowRate); _applyYaw(state.bodyRate); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h index 8b61a87bf7cd91..afc75841631a07 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h @@ -21,7 +21,7 @@ class AP_OpticalFlow_HereFlow : public OpticalFlow_backend { private: - Vector2f flowRate, bodyRate; + Vector2f flow_integral, rate_gyro_integral; uint8_t surface_quality; float integral_time; bool new_data; diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp index 1095e44227293b..5a976c8a3eb2f4 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_Pixart.cpp @@ -287,7 +287,11 @@ void AP_OpticalFlow_Pixart::timer(void) uint32_t dt_us = last_burst_us - integral.last_frame_us; float dt = dt_us * 1.0e-6; +#if AP_AHRS_ENABLED const Vector3f &gyro = AP::ahrs().get_gyro(); +#else + const Vector3f &gyro = AP::ins().get_gyro(); +#endif { WITH_SEMAPHORE(_sem); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp index 5ac842daba2f2a..d2382543134db1 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_SITL.cpp @@ -64,9 +64,9 @@ void AP_OpticalFlow_SITL::update(void) // estimate range to centre of image float range; - if (rotmat.c.z > 0.05f && _sitl->height_agl > 0) { + if (rotmat.c.z > 0.05f && _sitl->state.height_agl > 0) { Vector3f relPosSensorEF = rotmat * posRelSensorBF; - range = (_sitl->height_agl - relPosSensorEF.z) / rotmat.c.z; + range = (_sitl->state.height_agl - relPosSensorEF.z) / rotmat.c.z; } else { range = 1e38f; } diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index 9b4075dc719642..c570f8bf6cfa85 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -24,8 +24,9 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @Param: TYPE // @DisplayName: Parachute release mechanism type (relay or servo) - // @Description: Parachute release mechanism type (relay or servo) - // @Values: 0:First Relay,1:Second Relay,2:Third Relay,3:Fourth Relay,10:Servo + // @Description: Parachute release mechanism type (relay number in versions prior to 4.5, or servo). Values 0-3 all are relay. Relay number used for release is set by RELAYx_FUNCTION in 4.5 or later. + // @Values: 0: Relay,10:Servo + // @User: Standard AP_GROUPINFO("TYPE", 1, AP_Parachute, _release_type, AP_PARACHUTE_TRIGGER_TYPE_RELAY_0), @@ -92,7 +93,7 @@ void AP_Parachute::enabled(bool on_off) // clear release_time _release_time = 0; - AP::logger().Write_Event(_enabled ? LogEvent::PARACHUTE_ENABLED : LogEvent::PARACHUTE_DISABLED); + LOGGER_WRITE_EVENT(_enabled ? LogEvent::PARACHUTE_ENABLED : LogEvent::PARACHUTE_DISABLED); } /// release - release parachute @@ -104,7 +105,7 @@ void AP_Parachute::release() } GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Parachute: Released"); - AP::logger().Write_Event(LogEvent::PARACHUTE_RELEASED); + LOGGER_WRITE_EVENT(LogEvent::PARACHUTE_RELEASED); // set release time to current system time if (_release_time == 0) { @@ -142,7 +143,7 @@ void AP_Parachute::update() // set relay AP_Relay*_relay = AP::relay(); if (_relay != nullptr) { - _relay->on(_release_type); + _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, true); } #endif } @@ -159,7 +160,7 @@ void AP_Parachute::update() // set relay back to zero volts AP_Relay*_relay = AP::relay(); if (_relay != nullptr) { - _relay->off(_release_type); + _relay->set(AP_Relay_Params::FUNCTION::PARACHUTE, false); } #endif } @@ -218,8 +219,8 @@ bool AP_Parachute::arming_checks(size_t buflen, char *buffer) const } else { #if AP_RELAY_ENABLED AP_Relay*_relay = AP::relay(); - if (_relay == nullptr || !_relay->enabled(_release_type)) { - hal.util->snprintf(buffer, buflen, "Chute invalid relay %d", int(_release_type)); + if (_relay == nullptr || !_relay->enabled(AP_Relay_Params::FUNCTION::PARACHUTE)) { + hal.util->snprintf(buffer, buflen, "Chute has no relay"); return false; } #else @@ -235,6 +236,29 @@ bool AP_Parachute::arming_checks(size_t buflen, char *buffer) const return true; } +#if AP_RELAY_ENABLED +// Return the relay index that would be used for param conversion to relay functions +bool AP_Parachute::get_legacy_relay_index(int8_t &index) const +{ + // PARAMETER_CONVERSION - Added: Dec-2023 + if (_release_type > AP_PARACHUTE_TRIGGER_TYPE_RELAY_3) { + // Not relay type + return false; + } + if (!enabled() && !_enabled.configured()) { + // Disabled and parameter never changed + // Copter manual parachute release enables and deploys in one step + // This means it is possible for parachute to still function correctly if disabled at boot + // Checking if the enable param is configured means the user must have setup chute at some point + // this means relay parm conversion will be done if parachute has ever been enabled + // Parachute has priority in relay param conversion, so this might mean we overwrite some other function + return false; + } + index = _release_type; + return true; +} +#endif + // singleton instance AP_Parachute *AP_Parachute::_singleton; diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 7793a202d23bbd..30005e7543067e 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -85,7 +85,10 @@ class AP_Parachute { // check settings are valid bool arming_checks(size_t buflen, char *buffer) const; - + + // Return the relay index that would be used for param conversion to relay functions + bool get_legacy_relay_index(int8_t &index) const; + static const struct AP_Param::GroupInfo var_info[]; // get singleton instance diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp index 56d9cbf8ecbf63..e3983e2812b9a6 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -35,10 +35,14 @@ #include #include #include +#include + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #endif +#include "AP_Param_config.h" + extern const AP_HAL::HAL &hal; uint16_t AP_Param::sentinal_offset; @@ -46,6 +50,15 @@ uint16_t AP_Param::sentinal_offset; // singleton instance AP_Param *AP_Param::_singleton; +#ifndef AP_PARAM_STORAGE_BAK_ENABLED +// we only have a storage region for backup storage if we have at +// least 32768 bytes or storage. We also don't enable when using flash +// storage as this can lead to loss of storage when updating to a +// larger storage size +#define AP_PARAM_STORAGE_BAK_ENABLED (HAL_STORAGE_SIZE>=32768) && !defined(STORAGE_FLASH_PAGE) +#endif + + #define ENABLE_DEBUG 0 #if ENABLE_DEBUG @@ -102,6 +115,8 @@ uint16_t AP_Param::num_read_only; ObjectBuffer_TS AP_Param::save_queue{30}; bool AP_Param::registered_save_handler; +bool AP_Param::done_all_default_params; + AP_Param::defaults_list *AP_Param::default_list; // we need a dummy object for the parameter save callback @@ -124,8 +139,10 @@ const AP_Param::param_defaults_struct AP_Param::param_defaults_data = { // storage object StorageAccess AP_Param::_storage(StorageManager::StorageParam); +#if AP_PARAM_STORAGE_BAK_ENABLED // backup storage object StorageAccess AP_Param::_storage_bak(StorageManager::StorageParamBak); +#endif // flags indicating frame type uint16_t AP_Param::_frame_type_flags; @@ -134,7 +151,9 @@ uint16_t AP_Param::_frame_type_flags; void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size) { _storage.write_block(ofs, ptr, size); +#if AP_PARAM_STORAGE_BAK_ENABLED _storage_bak.write_block(ofs, ptr, size); +#endif } bool AP_Param::_hide_disabled_groups = true; @@ -333,14 +352,19 @@ void AP_Param::check_var_info(void) bool AP_Param::setup(void) { struct EEPROM_header hdr {}; - struct EEPROM_header hdr2 {}; // check the header _storage.read_block(&hdr, 0, sizeof(hdr)); + +#if AP_PARAM_STORAGE_BAK_ENABLED + struct EEPROM_header hdr2 {}; _storage_bak.read_block(&hdr2, 0, sizeof(hdr2)); +#endif + if (hdr.magic[0] != k_EEPROM_magic0 || hdr.magic[1] != k_EEPROM_magic1 || hdr.revision != k_EEPROM_revision) { +#if AP_PARAM_STORAGE_BAK_ENABLED if (hdr2.magic[0] == k_EEPROM_magic0 && hdr2.magic[1] == k_EEPROM_magic1 && hdr2.revision == k_EEPROM_revision && @@ -349,14 +373,17 @@ bool AP_Param::setup(void) INTERNAL_ERROR(AP_InternalError::error_t::params_restored); return true; } +#endif // AP_PARAM_STORAGE_BAK_ENABLED // header doesn't match. We can't recover any variables. Wipe // the header and setup the sentinal directly after the header Debug("bad header in setup - erasing"); erase_all(); } +#if AP_PARAM_STORAGE_BAK_ENABLED // ensure that backup is in sync with primary _storage_bak.copy_area(_storage); +#endif return true; } @@ -1543,32 +1570,75 @@ bool AP_Param::load_all() */ void AP_Param::reload_defaults_file(bool last_pass) { -#if AP_PARAM_MAX_EMBEDDED_PARAM > 0 - if (param_defaults_data.length != 0) { - load_embedded_param_defaults(last_pass); - return; - } -#endif +#if AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED -#if AP_FILESYSTEM_POSIX_ENABLED /* if the HAL specifies a defaults parameter file then override defaults using that file */ const char *default_file = hal.util->get_custom_defaults_file(); if (default_file) { - if (load_defaults_file(default_file, last_pass)) { - printf("Loaded defaults from %s\n", default_file); - } else { - AP_HAL::panic("Failed to load defaults from %s\n", default_file); - } +#if AP_FILESYSTEM_FILE_READING_ENABLED + load_defaults_file_from_filesystem(default_file, last_pass); +#elif defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H) + load_defaults_file_from_romfs(default_file, last_pass); +#endif + } + +#endif // AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED + +#if AP_PARAM_MAX_EMBEDDED_PARAM > 0 + if (param_defaults_data.length != 0) { + load_embedded_param_defaults(last_pass); } #endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH) hal.util->set_cmdline_parameters(); #endif } +#if AP_FILESYSTEM_FILE_READING_ENABLED +void AP_Param::load_defaults_file_from_filesystem(const char *default_file, bool last_pass) +{ + if (load_defaults_file(default_file, last_pass)) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + printf("Loaded defaults from %s\n", default_file); +#endif + } else { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + AP_HAL::panic("Failed to load defaults from %s\n", default_file); +#else + printf("Failed to load defaults from %s\n", default_file); +#endif + } +} +#endif // AP_FILESYSTEM_FILE_READING_ENABLED + +#if defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H) +void AP_Param::load_defaults_file_from_romfs(const char *default_file, bool last_pass) +{ + const char *prefix = "@ROMFS/"; + if (strncmp(default_file, prefix, strlen(prefix)) != 0) { + // does not start with ROMFS, do not attempt to retrieve from it + return; + } + + // filename without the prefix: + const char *trimmed_filename = &default_file[strlen(prefix)]; + + uint32_t string_length; + const uint8_t *text = AP_ROMFS::find_decompress(trimmed_filename, string_length); + if (text == nullptr) { + return; + } + + load_param_defaults((const char*)text, string_length, last_pass); + + AP_ROMFS::free(text); + +} +#endif // HAL_HAVE_AP_ROMFS_EMBEDDED_H /* Load all variables from EEPROM for a particular object. This is @@ -1621,6 +1691,13 @@ void AP_Param::load_object_from_eeprom(const void *object_pointer, const struct } } + if (!done_all_default_params) { + /* + the new subtree may need defaults from defaults.parm + */ + reload_defaults_file(false); + } + // reset cached param counter as we may be loading a dynamic var_info invalidate_count(); } @@ -1875,7 +1952,6 @@ bool AP_Param::find_old_parameter(const struct ConversionInfo *info, AP_Param *v return true; } - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wformat" // convert one old vehicle parameter to new object parameter @@ -1936,11 +2012,17 @@ void AP_Param::convert_old_parameter(const struct ConversionInfo *info, float sc #pragma GCC diagnostic pop -// convert old vehicle parameters to new object parametersv +// convert old vehicle parameters to new object parameters void AP_Param::convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags) +{ + convert_old_parameters_scaled(conversion_table, table_size, 1.0f, flags); +} + +// convert old vehicle parameters to new object parameters with scaling - assumes all parameters will have the same scaling factor +void AP_Param::convert_old_parameters_scaled(const struct ConversionInfo *conversion_table, uint8_t table_size, float scaler, uint8_t flags) { for (uint8_t i=0; icast_to_float(old_ptype); - set_value(new_ptype, this, old_float_value); + set_value(new_ptype, this, old_float_value*scale_factor); // force save as the new type save(true); @@ -2051,7 +2133,6 @@ bool AP_Param::convert_parameter_width(ap_var_type old_ptype) return true; } - /* set a parameter to a float value */ @@ -2136,8 +2217,7 @@ bool AP_Param::parse_param_line(char *line, char **vname, float &value, bool &re } -// FIXME: make this AP_FILESYSTEM_FILE_READING_ENABLED -#if AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED +#if AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED || AP_PARAM_DYNAMIC_ENABLED // increments num_defaults for each default found in filename bool AP_Param::count_defaults_in_file(const char *filename, uint16_t &num_defaults) @@ -2179,6 +2259,7 @@ bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass, ui return false; } + bool done_all = true; char line[100]; while (AP::FS().fgets(line, sizeof(line)-1, file_apfs)) { char *pname; @@ -2199,6 +2280,7 @@ bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass, ui pname, filename); #endif } + done_all = false; continue; } if (idx >= param_overrides_len) { @@ -2218,6 +2300,8 @@ bool AP_Param::read_param_defaults_file(const char *filename, bool last_pass, ui } AP::FS().close(file_apfs); + done_all_default_params = done_all; + return true; } @@ -2283,17 +2367,15 @@ bool AP_Param::load_defaults_file(const char *filename, bool last_pass) return true; } +#endif // AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED -#endif // AP_FILESYSTEM_FATFS_ENABLED || AP_FILESYSTEM_POSIX_ENABLED -#if AP_PARAM_MAX_EMBEDDED_PARAM > 0 +#if AP_PARAM_MAX_EMBEDDED_PARAM > 0 || (!AP_FILESYSTEM_FILE_READING_ENABLED && defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H)) /* - count the number of embedded parameter defaults + count the number of parameter defaults present in supplied string */ -bool AP_Param::count_embedded_param_defaults(uint16_t &count) +bool AP_Param::count_param_defaults(const volatile char *ptr, int32_t length, uint16_t &count) { - const volatile char *ptr = param_defaults_data.data; - int32_t length = param_defaults_data.length; count = 0; while (length>0) { @@ -2334,12 +2416,22 @@ bool AP_Param::count_embedded_param_defaults(uint16_t &count) return true; } +#if AP_PARAM_MAX_EMBEDDED_PARAM > 0 /* * load a default set of parameters from a embedded parameter region * @last_pass: if this is the last pass on defaults - unknown parameters are * ignored but if this is set a warning will be emitted */ void AP_Param::load_embedded_param_defaults(bool last_pass) +{ + load_param_defaults(param_defaults_data.data, param_defaults_data.length, last_pass); +} +#endif // AP_PARAM_MAX_EMBEDDED_PARAM > 0 + +/* + * load parameter defaults from supplied string + */ +void AP_Param::load_param_defaults(const volatile char *ptr, int32_t length, bool last_pass) { delete[] param_overrides; param_overrides = nullptr; @@ -2348,7 +2440,7 @@ void AP_Param::load_embedded_param_defaults(bool last_pass) num_read_only = 0; uint16_t num_defaults = 0; - if (!count_embedded_param_defaults(num_defaults)) { + if (!count_param_defaults(ptr, length, num_defaults)) { return; } @@ -2360,8 +2452,6 @@ void AP_Param::load_embedded_param_defaults(bool last_pass) param_overrides_len = num_defaults; - const volatile char *ptr = param_defaults_data.data; - int32_t length = param_defaults_data.length; uint16_t idx = 0; while (idx < num_defaults && length > 0) { diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index db8fbde15f4607..85d533c1fd1487 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -29,6 +29,8 @@ #include #include +#include "AP_Param_config.h" + #include "float.h" #define AP_MAX_NAME_SIZE 16 @@ -46,11 +48,15 @@ maximum size of embedded parameter file */ #ifndef AP_PARAM_MAX_EMBEDDED_PARAM -#if BOARD_FLASH_SIZE <= 1024 -# define AP_PARAM_MAX_EMBEDDED_PARAM 1024 -#else -# define AP_PARAM_MAX_EMBEDDED_PARAM 8192 -#endif + #if FORCE_APJ_DEFAULT_PARAMETERS + #if BOARD_FLASH_SIZE <= 1024 + #define AP_PARAM_MAX_EMBEDDED_PARAM 1024 + #else + #define AP_PARAM_MAX_EMBEDDED_PARAM 8192 + #endif + #else + #define AP_PARAM_MAX_EMBEDDED_PARAM 0 + #endif #endif // allow for dynamically added tables when scripting enabled @@ -461,13 +467,18 @@ class AP_Param // convert old vehicle parameters to new object parameters static void convert_old_parameters(const struct ConversionInfo *conversion_table, uint8_t table_size, uint8_t flags=0); + // convert old vehicle parameters to new object parameters with scaling - assumes we use the same scaling factor for all values in the table + static void convert_old_parameters_scaled(const ConversionInfo *conversion_table, uint8_t table_size, float scaler, uint8_t flags); /* convert width of a parameter, allowing update to wider scalar values without changing the parameter indexes. This will return true if the parameter was converted from an old parameter value */ - bool convert_parameter_width(ap_var_type old_ptype); + bool convert_parameter_width(ap_var_type old_ptype, float scale_factor=1.0); + bool convert_centi_parameter(ap_var_type old_ptype) { + return convert_parameter_width(old_ptype, 0.01f); + } // convert a single parameter with scaling enum { @@ -625,6 +636,11 @@ class AP_Param static uint16_t _frame_type_flags; + /* + this is true if when scanning a defaults file we find all of the parameters + */ + static bool done_all_default_params; + /* structure for built-in defaults file that can be modified using apj_tool.py */ @@ -726,8 +742,17 @@ class AP_Param load a parameter defaults file. This happens as part of load_all() */ static bool count_defaults_in_file(const char *filename, uint16_t &num_defaults); + static bool count_param_defaults(const volatile char *ptr, int32_t length, uint16_t &count); static bool read_param_defaults_file(const char *filename, bool last_pass, uint16_t &idx); + // load a defaults.parm using AP_FileSystem: + static void load_defaults_file_from_filesystem(const char *filename, bool lastpass); + // load an @ROMFS defaults.parm using ROMFS API: + static void load_defaults_file_from_romfs(const char *filename, bool lastpass); + + // load defaults from supplied string: + static void load_param_defaults(const volatile char *ptr, int32_t length, bool last_pass); + /* load defaults from embedded parameters */ diff --git a/libraries/AP_Param/AP_Param_config.h b/libraries/AP_Param/AP_Param_config.h new file mode 100644 index 00000000000000..181c9f04c39b70 --- /dev/null +++ b/libraries/AP_Param/AP_Param_config.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +#include + +#ifndef AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED +#define AP_PARAM_DEFAULTS_FILE_PARSING_ENABLED AP_FILESYSTEM_FILE_READING_ENABLED +#endif + +#ifndef FORCE_APJ_DEFAULT_PARAMETERS +#define FORCE_APJ_DEFAULT_PARAMETERS 0 +#endif diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 1cd68ac08aa0a2..bbaec88f3e4d03 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp @@ -140,7 +140,7 @@ bool AP_PiccoloCAN::add_interface(AP_HAL::CANIface* can_iface) { return false; } - if (!_can_iface->set_event_handle(&_event_handle)) { + if (!_can_iface->set_event_handle(&sem_handle)) { debug_can(AP_CANManager::LOG_ERROR, "PiccoloCAN: Cannot add event handle\n\r"); return false; } @@ -361,6 +361,7 @@ void AP_PiccoloCAN::update() } #endif // AP_EFI_CURRAWONG_ECU_ENABLED +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); // Push received telemetry data into the logging system @@ -397,6 +398,9 @@ void AP_PiccoloCAN::update() } } } +#else + (void)timestamp; +#endif // HAL_LOGGING_ENABLED } #if HAL_GCS_ENABLED diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h index 906cb772d09b6b..770f05dcf53bf6 100644 --- a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h +++ b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.h @@ -114,7 +114,7 @@ class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend char _thread_name[16]; uint8_t _driver_index; AP_HAL::CANIface* _can_iface; - HAL_EventHandle _event_handle; + HAL_BinarySemaphore sem_handle; AP_PiccoloCAN_Servo _servos[PICCOLO_CAN_MAX_NUM_SERVO]; AP_PiccoloCAN_ESC _escs[PICCOLO_CAN_MAX_NUM_ESC]; diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index ac1d6786e6c6ba..82dc97e7919d06 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -30,6 +30,8 @@ #include "AP_Proximity_DroneCAN.h" #include "AP_Proximity_Scripting.h" #include "AP_Proximity_LD06.h" +#include "AP_Proximity_MR72_CAN.h" + #include @@ -81,27 +83,45 @@ const AP_Param::GroupInfo AP_Proximity::var_info[] = { // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[0], "1", 21, AP_Proximity, AP_Proximity_Params), + // @Group: 1_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[0], "1_", 26, AP_Proximity, backend_var_info[0]), + #if PROXIMITY_MAX_INSTANCES > 1 // @Group: 2 // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[1], "2", 22, AP_Proximity, AP_Proximity_Params), + + // @Group: 2_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[1], "2_", 27, AP_Proximity, backend_var_info[1]), #endif #if PROXIMITY_MAX_INSTANCES > 2 // @Group: 3 // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[2], "3", 23, AP_Proximity, AP_Proximity_Params), + + // @Group: 3_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[2], "3_", 28, AP_Proximity, backend_var_info[2]), #endif #if PROXIMITY_MAX_INSTANCES > 3 // @Group: 4 // @Path: AP_Proximity_Params.cpp AP_SUBGROUPINFO(params[3], "4", 24, AP_Proximity, AP_Proximity_Params), + + // @Group: 4_ + // @Path: AP_Proximity_MR72_CAN.cpp + AP_SUBGROUPVARPTR(drivers[3], "4_", 29, AP_Proximity, backend_var_info[3]), #endif AP_GROUPEND }; +const AP_Param::GroupInfo *AP_Proximity::backend_var_info[PROXIMITY_MAX_INSTANCES]; + AP_Proximity::AP_Proximity() { AP_Param::setup_object_defaults(this, var_info); @@ -206,6 +226,12 @@ void AP_Proximity::init() drivers[instance] = new AP_Proximity_Scripting(*this, state[instance], params[instance]); break; #endif +#if AP_PROXIMITY_MR72_ENABLED + case Type::MR72: + state[instance].instance = instance; + drivers[instance] = new AP_Proximity_MR72_CAN(*this, state[instance], params[instance]); + break; +# endif #if AP_PROXIMITY_SITL_ENABLED case Type::SITL: state[instance].instance = instance; @@ -237,6 +263,12 @@ void AP_Proximity::init() // initialise status state[instance].status = Status::NotConnected; + + // if the backend has some local parameters then make those available in the tree + if (drivers[instance] && state[instance].var_info) { + backend_var_info[instance] = state[instance].var_info; + AP_Param::load_object_from_eeprom(drivers[instance], backend_var_info[instance]); + } } } diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 0363fea8f4da97..92d6f123e07939 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -86,6 +86,9 @@ class AP_Proximity #endif #if AP_PROXIMITY_LD06_ENABLED LD06 = 16, +#endif +#if AP_PROXIMITY_MR72_ENABLED + MR72 = 17, #endif }; @@ -179,8 +182,12 @@ class AP_Proximity struct Proximity_State { uint8_t instance; // the instance number of this proximity sensor Status status; // sensor status + + const struct AP_Param::GroupInfo *var_info; // stores extra parameter information for the sensor (if it exists) }; + static const struct AP_Param::GroupInfo *backend_var_info[PROXIMITY_MAX_INSTANCES]; + // parameter list static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp new file mode 100644 index 00000000000000..4f0225c65bd6cc --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.cpp @@ -0,0 +1,146 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_MR72_ENABLED + +#include +#include +#include "AP_Proximity_MR72_CAN.h" + +const AP_Param::GroupInfo AP_Proximity_MR72_CAN::var_info[] = { + + // @Param: RECV_ID + // @DisplayName: CAN receive ID + // @Description: The receive ID of the CAN frames. A value of zero means all IDs are accepted. + // @Range: 0 65535 + // @User: Advanced + AP_GROUPINFO("RECV_ID", 1, AP_Proximity_MR72_CAN, receive_id, 0), + + AP_GROUPEND +}; + +MR72_MultiCAN *AP_Proximity_MR72_CAN::multican; + +AP_Proximity_MR72_CAN::AP_Proximity_MR72_CAN(AP_Proximity &_frontend, + AP_Proximity::Proximity_State &_state, + AP_Proximity_Params& _params): + AP_Proximity_Backend(_frontend, _state, _params) +{ + if (multican == nullptr) { + multican = new MR72_MultiCAN(); + if (multican == nullptr) { + AP_BoardConfig::allocation_error("MR72_CAN"); + } + } + + { + // add to linked list of drivers + WITH_SEMAPHORE(multican->sem); + auto *prev = multican->drivers; + next = prev; + multican->drivers = this; + } + + AP_Param::setup_object_defaults(this, var_info); + state.var_info = var_info; +} + +// update state +void AP_Proximity_MR72_CAN::update(void) +{ + WITH_SEMAPHORE(_sem); + const uint32_t now = AP_HAL::millis(); + if (now - last_update_ms > 500) { + // no new data. + set_status(AP_Proximity::Status::NoData); + } else { + set_status(AP_Proximity::Status::Good); + } +} + +// handler for incoming frames. These come in at 100Hz +bool AP_Proximity_MR72_CAN::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(_sem); + + + // check if message is coming from the right sensor ID + const uint16_t id = frame.id; + + if (receive_id > 0 && (get_radar_id(frame.id) != uint32_t(receive_id))) { + return false; + } + + switch (id & 0xFU) { + case 0xAU: + // number of objects + _object_count = frame.data[0]; + _current_object_index = 0; + _temp_boundary.update_3D_boundary(state.instance, frontend.boundary); + _temp_boundary.reset(); + last_update_ms = AP_HAL::millis(); + break; + case 0xBU: + // obstacle data + parse_distance_message(frame); + break; + default: + break; + } + + return true; + +} + +// parse a distance message from CAN frame +bool AP_Proximity_MR72_CAN::parse_distance_message(AP_HAL::CANFrame &frame) +{ + if (_current_object_index >= _object_count) { + // should never happen + return false; + } + _current_object_index++; + + Vector2f obstacle_fr; + // This parsing comes from the NanoRadar MR72 datasheet + obstacle_fr.x = ((frame.data[2] & 0x07U) * 256 + frame.data[3]) * 0.2 - 204.6; + obstacle_fr.y = (frame.data[1] * 32 + (frame.data[2] >> 3)) * 0.2 - 500; + const float yaw = correct_angle_for_orientation(wrap_360(degrees(atan2f(obstacle_fr.x, obstacle_fr.y)))); + + const float objects_dist = obstacle_fr.length(); + + if (ignore_reading(yaw, objects_dist)) { + // obstacle is probably near ground or out of range + return false; + } + + const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw); + _temp_boundary.add_distance(face, yaw, objects_dist); + return true; +} + +// handle frames from CANSensor, passing to the drivers +void MR72_MultiCAN::handle_frame(AP_HAL::CANFrame &frame) +{ + WITH_SEMAPHORE(sem); + for (auto *d = drivers; d; d=d->next) { + if (d->handle_frame(frame)) { + break; + } + } +} + +#endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h new file mode 100644 index 00000000000000..e2a3dcbb9df361 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_MR72_CAN.h @@ -0,0 +1,68 @@ +#pragma once +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_MR72_ENABLED + +#include "AP_Proximity.h" +#include "AP_Proximity_Backend.h" +#include +#include + +#define MR72_MAX_RANGE_M 50.0f // max range of the sensor in meters +#define MR72_MIN_RANGE_M 0.2f // min range of the sensor in meters + +class MR72_MultiCAN; + +class AP_Proximity_MR72_CAN : public AP_Proximity_Backend { +public: + friend class MR72_MultiCAN; + + AP_Proximity_MR72_CAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_Proximity_Params& _params); + + void update() override; + + // handler for incoming frames. Return true if consumed + bool handle_frame(AP_HAL::CANFrame &frame); + + // parse a distance message from CAN frame + bool parse_distance_message(AP_HAL::CANFrame &frame); + + // get maximum and minimum distances (in meters) of sensor + float distance_max() const override { return MR72_MAX_RANGE_M; } + float distance_min() const override { return MR72_MIN_RANGE_M; } + + static const struct AP_Param::GroupInfo var_info[]; + + AP_Proximity_Temp_Boundary _temp_boundary; + +private: + + uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); } + + uint32_t _object_count; // total number of objects to read + uint32_t _current_object_index; // current object index + uint32_t last_update_ms; // last update time in ms + + AP_Int32 receive_id; // ID of the sensor + + static MR72_MultiCAN *multican; // linked list + AP_Proximity_MR72_CAN *next; +}; + +// a class to allow for multiple MR_72 backends with one +// CANSensor driver +class MR72_MultiCAN : public CANSensor { +public: + MR72_MultiCAN() : CANSensor("MR72") { + register_driver(AP_CAN::Protocol::MR72); + } + + // handler for incoming frames + void handle_frame(AP_HAL::CANFrame &frame) override; + + HAL_Semaphore sem; + AP_Proximity_MR72_CAN *drivers; + +}; + +#endif // HAL_PROXIMITY_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Params.cpp b/libraries/AP_Proximity/AP_Proximity_Params.cpp index 046a29c3b71810..ad85936067fe45 100644 --- a/libraries/AP_Proximity/AP_Proximity_Params.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Proximity_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Proximity type // @Description: What type of proximity sensor is connected - // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06 + // @Values: 0:None,7:LightwareSF40c,2:MAVLink,3:TeraRangerTower,4:RangeFinder,5:RPLidarA2,6:TeraRangerTowerEvo,8:LightwareSF45B,10:SITL,12:AirSimSITL,13:CygbotD1, 14:DroneCAN, 15:Scripting, 16:LD06, 17: MR72_CAN // @RebootRequired: True // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Proximity_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index 1d72a8ebed41db..6c1e6dcdb72e62 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -131,7 +131,7 @@ float AP_Proximity_SITL::distance_min() const bool AP_Proximity_SITL::get_upward_distance(float &distance) const { // return distance to fence altitude - distance = MAX(0.0f, fence_alt_max->get() - sitl->height_agl); + distance = MAX(0.0f, fence_alt_max->get() - sitl->state.height_agl); return true; } diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index a2130b0b5c4f25..a05e94b653268c 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -37,6 +37,11 @@ #define AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif + +#ifndef AP_PROXIMITY_MR72_ENABLED +#define AP_PROXIMITY_MR72_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_MAX_CAN_PROTOCOL_DRIVERS +#endif + #ifndef AP_PROXIMITY_SCRIPTING_ENABLED #define AP_PROXIMITY_SCRIPTING_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED #endif diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 9c8ef679a1643b..2b501c888f8967 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -33,6 +33,7 @@ #include "AP_RCProtocol_FPort.h" #include "AP_RCProtocol_FPort2.h" #include "AP_RCProtocol_DroneCAN.h" +#include "AP_RCProtocol_GHST.h" #include #include @@ -54,7 +55,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_FASTSBUS_ENABLED backend[AP_RCProtocol::FASTSBUS] = new AP_RCProtocol_SBUS(*this, true, 200000); #endif +#if AP_RCPROTOCOL_DSM_ENABLED backend[AP_RCProtocol::DSM] = new AP_RCProtocol_DSM(*this); +#endif #if AP_RCPROTOCOL_SUMD_ENABLED backend[AP_RCProtocol::SUMD] = new AP_RCProtocol_SUMD(*this); #endif @@ -82,6 +85,9 @@ void AP_RCProtocol::init() #if AP_RCPROTOCOL_DRONECAN_ENABLED backend[AP_RCProtocol::DRONECAN] = new AP_RCProtocol_DroneCAN(*this); #endif +#if AP_RCPROTOCOL_GHST_ENABLED + backend[AP_RCProtocol::GHST] = new AP_RCProtocol_GHST(*this); +#endif } AP_RCProtocol::~AP_RCProtocol() @@ -297,7 +303,7 @@ static const AP_RCProtocol::SerialConfig serial_configs[] { // FastSBUS: { 200000, 2, 2, true }, #endif -#if AP_RCPROTOCOL_CRSF_ENABLED +#if AP_RCPROTOCOL_CRSF_ENABLED || AP_RCPROTOCOL_GHST_ENABLED // CrossFire: { 416666, 0, 1, false }, // CRSFv3 can negotiate higher rates which are sticky on soft reboot @@ -471,8 +477,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) case FASTSBUS: return "FastSBUS"; #endif +#if AP_RCPROTOCOL_DSM_ENABLED case DSM: return "DSM"; +#endif #if AP_RCPROTOCOL_SUMD_ENABLED case SUMD: return "SUMD"; @@ -504,6 +512,10 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol) #if AP_RCPROTOCOL_DRONECAN_ENABLED case DRONECAN: return "DroneCAN"; +#endif +#if AP_RCPROTOCOL_GHST_ENABLED + case GHST: + return "GHST"; #endif case NONE: break; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index eeacb6fa0a0b62..f5823739d6d09a 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -42,7 +42,9 @@ class AP_RCProtocol { #if AP_RCPROTOCOL_SBUS_NI_ENABLED SBUS_NI = 3, #endif +#if AP_RCPROTOCOL_DSM_ENABLED DSM = 4, +#endif #if AP_RCPROTOCOL_SUMD_ENABLED SUMD = 5, #endif @@ -69,6 +71,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_DRONECAN_ENABLED DRONECAN = 13, +#endif +#if AP_RCPROTOCOL_GHST_ENABLED + GHST = 14, #endif NONE //last enum always is None }; @@ -108,7 +113,9 @@ class AP_RCProtocol { // for protocols without strong CRCs we require 3 good frames to lock on bool requires_3_frames(enum rcprotocol_t p) { switch (p) { +#if AP_RCPROTOCOL_DSM_ENABLED case DSM: +#endif #if AP_RCPROTOCOL_FASTSBUS_ENABLED case FASTSBUS: #endif @@ -129,6 +136,9 @@ class AP_RCProtocol { #endif #if AP_RCPROTOCOL_CRSF_ENABLED case CRSF: +#endif +#if AP_RCPROTOCOL_GHST_ENABLED + case GHST: #endif return true; #if AP_RCPROTOCOL_IBUS_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index cbb0b1135be712..18aeb33cff90b9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -67,6 +67,10 @@ class AP_RCProtocol_Backend { return frontend.rc_protocols_mask; } + bool protocol_enabled(enum AP_RCProtocol::rcprotocol_t protocol) const { + return frontend.protocol_enabled(protocol); + } + // get RSSI int16_t get_RSSI(void) const { return rssi; @@ -95,6 +99,10 @@ class AP_RCProtocol_Backend { return true; } + bool is_detected() const { + return frontend._detected_protocol != AP_RCProtocol::NONE && frontend.backend[frontend._detected_protocol] == this; + } + #if AP_VIDEOTX_ENABLED // called by static methods to confiig video transmitters: static void configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index df47ab768d619e..2fd5a35a546c73 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -238,6 +238,10 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) return; } + if (_frame.device_address != DeviceAddress::CRSF_ADDRESS_FLIGHT_CONTROLLER) { + return; + } + // parse the length if (_frame_ofs == CSRF_HEADER_TYPE_LEN) { _frame_crc = crc8_dvb_s2(0, _frame.type); @@ -302,7 +306,7 @@ void AP_RCProtocol_CRSF::update(void) // never received RC frames, but have received CRSF frames so make sure we give the telemetry opportunity to run uint32_t now = AP_HAL::micros(); - if (_last_frame_time_us > 0 && (!get_rc_frame_count() || !is_tx_active()) + if (_last_frame_time_us > 0 && (!get_rc_input_count() || !is_tx_active()) && now - _last_frame_time_us > CRSF_INTER_FRAME_TIME_US_250HZ) { process_telemetry(false); _last_frame_time_us = now; @@ -617,7 +621,7 @@ void AP_RCProtocol_CRSF::process_handshake(uint32_t baudrate) || baudrate != CRSF_BAUDRATE || baudrate == get_bootstrap_baud_rate() || uart->get_baud_rate() == get_bootstrap_baud_rate() - || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::CRSF)+1))+1)) == 0) { + || !protocol_enabled(AP_RCProtocol::CRSF)) { return; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp index 6258f4069c541a..bcfd8f3f640fc4 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp @@ -20,7 +20,7 @@ #include "AP_RCProtocol_config.h" -#if AP_RCPROTOCOL_ENABLED +#if AP_RCPROTOCOL_DSM_ENABLED #include "AP_RCProtocol_DSM.h" #include @@ -539,4 +539,4 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate) _process_byte(AP_HAL::millis(), b); } -#endif // AP_RCPROTOCOL_ENABLED +#endif // AP_RCPROTOCOL_DSM_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h index 65a3e266c139cf..61141bf8f653f0 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_DSM.h @@ -19,7 +19,7 @@ #include "AP_RCProtocol_config.h" -#if AP_RCPROTOCOL_ENABLED +#if AP_RCPROTOCOL_DSM_ENABLED #include "AP_RCProtocol_Backend.h" @@ -83,4 +83,4 @@ class AP_RCProtocol_DSM : public AP_RCProtocol_Backend { SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1}; }; -#endif // AP_RCPROTOCOL_ENABLED +#endif // AP_RCPROTOCOL_DSM_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp new file mode 100644 index 00000000000000..a51c9dd795c88d --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp @@ -0,0 +1,458 @@ +/* + 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 . + */ +/* + GHST protocol decoder based on betaflight implementation + Code by Andy Piper + */ + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_GHST_ENABLED + +#define CRSF_BAUDRATE 416666U + +#include "AP_RCProtocol.h" +#include "AP_RCProtocol_GHST.h" +#include +#include +#include +#include +#include +#include + +/* + * GHST protocol + * + * GHST protocol uses a single wire half duplex uart connection. + * + * 420000 baud + * not inverted + * 8 Bit + * 1 Stop bit + * Big endian + * Max frame size is 14 bytes + * + * Every frame has the structure: + * + * + * Device address: (uint8_t) + * Frame length: length in bytes including Type (uint8_t) + * Type: (uint8_t) + * CRC: (uint8_t) + * + */ + +extern const AP_HAL::HAL& hal; + +//#define GHST_DEBUG +//#define GHST_DEBUG_CHARS +#ifdef GHST_DEBUG +# define debug(fmt, args...) hal.console->printf("GHST: " fmt "\n", ##args) +static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0) +{ + switch(byte) { + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_5TO8: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_5TO8: + return "RC5_8"; + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_9TO12: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_9TO12: + return "RC9_12"; + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_13TO16: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_HS4_12_13TO16: + return "RC13_16"; + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_RSSI: + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_12_RSSI: + return "RSSI"; + case AP_RCProtocol_GHST::GHST_UL_RC_VTX_CTRL: + return "VTX_CTRL"; + case AP_RCProtocol_GHST::GHST_UL_VTX_SETUP: + return "VTX_SETUP"; + } + return "UNKNOWN"; +} +#else +# define debug(fmt, args...) do {} while(0) +#endif + +#define GHST_MAX_FRAME_TIME_US 500U // 14 bytes @ 420k = ~450us +#define GHST_FRAME_TIMEOUT_US 10000U // 10ms to account for scheduling delays +#define GHST_INTER_FRAME_TIME_US 2000U // At fastest, frames are sent by the transmitter every 2 ms, 500 Hz +#define GHST_HEADER_TYPE_LEN (GHST_HEADER_LEN + 1) // header length including type + +const uint16_t AP_RCProtocol_GHST::RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES] = { + 55, 160, 250, 19, 250, 500, 150, 250, +}; + +AP_RCProtocol_GHST* AP_RCProtocol_GHST::_singleton; + +AP_RCProtocol_GHST::AP_RCProtocol_GHST(AP_RCProtocol &_frontend) : AP_RCProtocol_Backend(_frontend) +{ +#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) + if (_singleton != nullptr) { + AP_HAL::panic("Duplicate GHST handler"); + } + + _singleton = this; +#else + if (_singleton == nullptr) { + _singleton = this; + } +#endif +} + +AP_RCProtocol_GHST::~AP_RCProtocol_GHST() { + _singleton = nullptr; +} + +// get the protocol string +const char* AP_RCProtocol_GHST::get_protocol_string() const { + return "GHST"; +} + +// return the link rate as defined by the LinkStatistics +uint16_t AP_RCProtocol_GHST::get_link_rate() const { + return RF_MODE_RATES[_link_status.rf_mode - GHST_RF_MODE_NORMAL]; +} + +void AP_RCProtocol_GHST::_process_byte(uint32_t timestamp_us, uint8_t byte) +{ + //debug("process_byte(0x%x)", byte); + // we took too long decoding, start again - the RX will only send complete frames so this is unlikely to fail, + // however thread scheduling can introduce longer delays even when the data has been received + if (_frame_ofs > 0 && (timestamp_us - _start_frame_time_us) > GHST_FRAME_TIMEOUT_US) { + _frame_ofs = 0; + } + + // overflow check + if (_frame_ofs >= GHST_FRAMELEN_MAX) { + _frame_ofs = 0; + } + + // start of a new frame + if (_frame_ofs == 0) { + _start_frame_time_us = timestamp_us; + } + + add_to_buffer(_frame_ofs++, byte); + + // need a header to get the length + if (_frame_ofs < GHST_HEADER_TYPE_LEN) { + return; + } + + if (_frame.device_address != DeviceAddress::GHST_ADDRESS_FLIGHT_CONTROLLER) { + return; + } + + // parse the length + if (_frame_ofs == GHST_HEADER_TYPE_LEN) { + _frame_crc = crc8_dvb_s2(0, _frame.type); + // check for garbage frame + if (_frame.length > GHST_FRAME_PAYLOAD_MAX) { + _frame_ofs = 0; + } + return; + } + + // update crc + if (_frame_ofs < _frame.length + GHST_HEADER_LEN) { + _frame_crc = crc8_dvb_s2(_frame_crc, byte); + } + + // overflow check + if (_frame_ofs > _frame.length + GHST_HEADER_LEN) { + _frame_ofs = 0; + return; + } + + // decode whatever we got and expect + if (_frame_ofs == _frame.length + GHST_HEADER_LEN) { + log_data(AP_RCProtocol::GHST, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - GHST_HEADER_LEN); + + // we consumed the partial frame, reset + _frame_ofs = 0; + + // bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index) + if (_frame_crc != _frame.payload[_frame.length - 2]) { + return; + } + + _last_frame_time_us = _last_rx_frame_time_us = timestamp_us; + // decode here + if (decode_ghost_packet()) { + _last_tx_frame_time_us = timestamp_us; // we have received a frame from the transmitter + add_input(MAX_CHANNELS, _channels, false, _link_status.rssi, _link_status.link_quality); + } + } +} + +void AP_RCProtocol_GHST::update(void) +{ +} + +// write out a frame of any type +bool AP_RCProtocol_GHST::write_frame(Frame* frame) +{ + AP_HAL::UARTDriver *uart = get_current_UART(); + + if (!uart) { + return false; + } + + // check that we haven't been too slow in responding to the new UART data. If we respond too late then we will + // corrupt the next incoming control frame. incoming packets at max 126bits @500Hz @420k baud gives total budget of 2ms + // per packet of which we need 300us to receive a packet. outgoing packets are 126bits which require 300us to send + // leaving at most 1.4ms of delay that can be tolerated + uint64_t tend = uart->receive_time_constraint_us(1); + uint64_t now = AP_HAL::micros64(); + uint64_t tdelay = now - tend; + if (tdelay > 1000) { + // we've been too slow in responding + return false; + } + + // calculate crc + uint8_t crc = crc8_dvb_s2(0, frame->type); + for (uint8_t i = 0; i < frame->length - 2; i++) { + crc = crc8_dvb_s2(crc, frame->payload[i]); + } + frame->payload[frame->length - 2] = crc; + + uart->write((uint8_t*)frame, frame->length + 2); + uart->flush(); + +#ifdef GHST_DEBUG + hal.console->printf("GHST: writing %s:", get_frame_type(frame->type, frame->payload[0])); + for (uint8_t i = 0; i < frame->length + 2; i++) { + uint8_t val = ((uint8_t*)frame)[i]; +#ifdef GHST_DEBUG_CHARS + if (val >= 32 && val <= 126) { + hal.console->printf(" 0x%x '%c'", val, (char)val); + } else { +#endif + hal.console->printf(" 0x%x", val); +#ifdef GHST_DEBUG_CHARS + } +#endif + } + hal.console->printf("\n"); +#endif + return true; +} + +bool AP_RCProtocol_GHST::decode_ghost_packet() +{ +#ifdef GHST_DEBUG + hal.console->printf("GHST: received %s:", get_frame_type(_frame.type)); + uint8_t* fptr = (uint8_t*)&_frame; + for (uint8_t i = 0; i < _frame.length + 2; i++) { +#ifdef GHST_DEBUG_CHARS + if (fptr[i] >= 32 && fptr[i] <= 126) { + hal.console->printf(" 0x%x '%c'", fptr[i], (char)fptr[i]); + } else { +#endif + hal.console->printf(" 0x%x", fptr[i]); +#ifdef GHST_DEBUG_CHARS + } +#endif + } + hal.console->printf("\n"); +#endif + + const RadioFrame* radio_frame = (const RadioFrame*)(&_frame.payload); + const Channels12Bit_4Chan* channels = &(radio_frame->channels); + const uint8_t* lowres_channels = radio_frame->lowres_channels; + + // Scaling from Betaflight + // Scaling 12bit channels (8bit channels in brackets) + // OpenTx RC PWM (BF) + // min -1024 0( 0) 988us + // ctr 0 2048(128) 1500us + // max 1024 4096(256) 2012us + // + + // Scaling legacy (nearly 10bit) + // derived from original SBus scaling, with slight correction for offset + // now symmetrical around OpenTx 0 value + // scaling is: + // OpenTx RC PWM (BF) + // min -1024 172( 22) 988us + // ctr 0 992(124) 1500us + // max 1024 1811(226) 2012us + +#define CHANNEL_RESCALE(x) (((5 * x) >> 2) - 430) +#define CHANNEL_SCALE(x) (int32_t(x) / 4 + 988) +#define CHANNEL_SCALE_LEGACY(x) CHANNEL_SCALE(CHANNEL_RESCALE(x)) + + // legacy scaling + if (_frame.type >= GHST_UL_RC_CHANS_HS4_5TO8 && _frame.type <= GHST_UL_RC_CHANS_RSSI) { + _channels[0] = CHANNEL_SCALE_LEGACY(channels->ch0); + _channels[1] = CHANNEL_SCALE_LEGACY(channels->ch1); + _channels[2] = CHANNEL_SCALE_LEGACY(channels->ch2); + _channels[3] = CHANNEL_SCALE_LEGACY(channels->ch3); + } else { + _channels[0] = CHANNEL_SCALE(channels->ch0); + _channels[1] = CHANNEL_SCALE(channels->ch1); + _channels[2] = CHANNEL_SCALE(channels->ch2); + _channels[3] = CHANNEL_SCALE(channels->ch3); + } + +#define CHANNEL_LR_RESCALE(x) (5 * x - 108) +#define CHANNEL_LR_SCALE(x) (int32_t(x) * 2 + 988) +#define CHANNEL_LR_SCALE_LEGACY(x) (CHANNEL_LR_RESCALE(x) + 988) + + switch (_frame.type) { + case GHST_UL_RC_CHANS_HS4_5TO8: + case GHST_UL_RC_CHANS_HS4_9TO12: + case GHST_UL_RC_CHANS_HS4_13TO16: { + uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_5TO8 + 1) * 4; + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[0]); + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[1]); + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[2]); + _channels[offset++] = CHANNEL_LR_SCALE_LEGACY(lowres_channels[3]); + break; + } + case GHST_UL_RC_CHANS_HS4_12_5TO8: + case GHST_UL_RC_CHANS_HS4_12_9TO12: + case GHST_UL_RC_CHANS_HS4_12_13TO16: { + uint8_t offset = (_frame.type - GHST_UL_RC_CHANS_HS4_12_5TO8 + 1) * 4; + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[0]); + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[1]); + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[2]); + _channels[offset++] = CHANNEL_LR_SCALE(lowres_channels[3]); + break; + } + case GHST_UL_RC_CHANS_RSSI: + case GHST_UL_RC_CHANS_12_RSSI: + process_link_stats_frame((uint8_t*)&_frame.payload); + break; + + default: + break; + } + +#if AP_GHST_TELEM_ENABLED + if (AP_GHST_Telem::process_frame(FrameType(_frame.type), (uint8_t*)&_frame.payload)) { + process_telemetry(); + } +#endif + + return true; +} + +// send out telemetry +bool AP_RCProtocol_GHST::process_telemetry(bool check_constraint) +{ + + AP_HAL::UARTDriver *uart = get_current_UART(); + if (!uart) { + return false; + } + + if (!telem_available) { +#if AP_GHST_TELEM_ENABLED + if (AP_GHST_Telem::get_telem_data(&_telemetry_frame, is_tx_active())) { + telem_available = true; + } else { + return false; + } +#else + return false; +#endif + } + if (!write_frame(&_telemetry_frame)) { + return false; + } + + // get fresh telem_data in the next call + telem_available = false; + + return true; +} + +// process link statistics to get RSSI +void AP_RCProtocol_GHST::process_link_stats_frame(const void* data) +{ + const LinkStatisticsFrame* link = (const LinkStatisticsFrame*)data; + + uint8_t rssi_dbm; + rssi_dbm = link->rssi_dbm; + _link_status.link_quality = link->link_quality; + if (_use_lq_for_rssi) { + _link_status.rssi = derive_scaled_lq_value(link->link_quality); + } else{ + // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link + if (rssi_dbm < 50) { + _link_status.rssi = 255; + } else if (rssi_dbm > 120) { + _link_status.rssi = 0; + } else { + // this is an approximation recommended by Remo from TBS + _link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f)); + } + } + + _link_status.rf_mode = link->protocol; +} + +bool AP_RCProtocol_GHST::is_telemetry_supported() const +{ + return _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_NORMAL + || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE + || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_LR + || _link_status.rf_mode == AP_RCProtocol_GHST::GHST_RF_MODE_RACE250; +} + +// process a byte provided by a uart +void AP_RCProtocol_GHST::process_byte(uint8_t byte, uint32_t baudrate) +{ + // reject RC data if we have been configured for standalone mode + if (baudrate != CRSF_BAUDRATE && baudrate != GHST_BAUDRATE) { + return; + } + _process_byte(AP_HAL::micros(), byte); +} + +// change the bootstrap baud rate to Ghost standard if configured +void AP_RCProtocol_GHST::process_handshake(uint32_t baudrate) +{ + AP_HAL::UARTDriver *uart = get_current_UART(); + + // only change the baudrate if we are specifically bootstrapping Ghost + if (uart == nullptr + || baudrate != CRSF_BAUDRATE + || baudrate == GHST_BAUDRATE + || uart->get_baud_rate() == GHST_BAUDRATE + || !protocol_enabled(AP_RCProtocol::GHST) + || protocol_enabled(AP_RCProtocol::CRSF)) { + return; + } + + uart->begin(GHST_BAUDRATE); +} + +//returns uplink link quality on 0-255 scale +int16_t AP_RCProtocol_GHST::derive_scaled_lq_value(uint8_t uplink_lq) +{ + return int16_t(roundf(constrain_float(uplink_lq*2.5f,0,255))); +} + +namespace AP { + AP_RCProtocol_GHST* ghost() { + return AP_RCProtocol_GHST::get_singleton(); + } +}; + +#endif // AP_RCPROTOCOL_GHST_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h new file mode 100644 index 00000000000000..248fb3f1790d35 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.h @@ -0,0 +1,199 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#pragma once + +#include "AP_RCProtocol_config.h" + +#if AP_RCPROTOCOL_GHST_ENABLED + +#include "AP_RCProtocol.h" +#include +#include +#include "SoftSerial.h" + +#define GHST_MAX_CHANNELS 16U // Maximum number of channels from GHST datastream +#define GHST_FRAMELEN_MAX 14U // maximum possible framelength +#define GHST_HEADER_LEN 2U // header length +#define GHST_FRAME_PAYLOAD_MAX (GHST_FRAMELEN_MAX - GHST_HEADER_LEN) // maximum size of the frame length field in a packet +#define GHST_BAUDRATE 420000U +#define GHST_TX_TIMEOUT 500000U // the period after which the transmitter is considered disconnected (matches copters failsafe) +#define GHST_RX_TIMEOUT 150000U // the period after which the receiver is considered disconnected (>ping frequency) + +class AP_RCProtocol_GHST : public AP_RCProtocol_Backend { +public: + AP_RCProtocol_GHST(AP_RCProtocol &_frontend); + virtual ~AP_RCProtocol_GHST(); + void process_byte(uint8_t byte, uint32_t baudrate) override; + void process_handshake(uint32_t baudrate) override; + void update(void) override; + + // is the receiver active, used to detect power loss and baudrate changes + bool is_rx_active() const override { + return AP_HAL::micros() < _last_rx_frame_time_us + GHST_RX_TIMEOUT; + } + + // is the transmitter active, used to adjust telemetry data + bool is_tx_active() const { + // this is the same as the Copter failsafe timeout + return AP_HAL::micros() < _last_tx_frame_time_us + GHST_TX_TIMEOUT; + } + + // get singleton instance + static AP_RCProtocol_GHST* get_singleton() { + return _singleton; + } + + enum FrameType { + GHST_UL_RC_CHANS_HS4_5TO8 = 0x10, // Control packet with 4 primary channels + CH5-8 + GHST_UL_RC_CHANS_HS4_9TO12 = 0x11, // Control packet with 4 primary channels + CH9-12 + GHST_UL_RC_CHANS_HS4_13TO16 = 0x12, // Control packet with 4 primary channels + CH13-16 + GHST_UL_RC_CHANS_RSSI = 0x13, // Control packet with RSSI and LQ data + GHST_UL_RC_VTX_CTRL = 0x14, // Goggle/FC channel changing + // -> 0x1F reserved + GHST_UL_VTX_SETUP = 0x20, // vTx Setup w/o 4 primary channels (GECO Only) + GHST_UL_MSP_REQ = 0x21, // MSP frame, Request + GHST_UL_MSP_WRITE = 0x22, // MSP frame, Write + + GHST_DL_PACK_STAT = 0x23, // Battery Status + GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS Data + GHST_DL_GPS_SECONDARY = 0x26, // Secondary GPS Data + GHST_DL_MAGBARO = 0x27, // Magnetometer, Barometer (and Vario) Data + GHST_DL_MSP_RESP = 0x28, // MSP Response + + GHST_UL_RC_CHANS_HS4_12_5TO8 = 0x30, // Control packet with 4 primary channels + CH5-8 + GHST_UL_RC_CHANS_HS4_12_9TO12 = 0x31, // Control packet with 4 primary channels + CH9-12 + GHST_UL_RC_CHANS_HS4_12_13TO16 = 0x32, // Control packet with 4 primary channels + CH13-16 + GHST_UL_RC_CHANS_12_RSSI = 0x33, // Control packet with RSSI and LQ data + // 0x30 -> 0x3f - raw 12 bit packets + }; + + enum DeviceAddress { + GHST_ADDRESS_FLIGHT_CONTROLLER = 0x82, + GHST_ADDRESS_GOGGLES = 0x83, + GHST_ADDRESS_GHST_RECEIVER = 0x89, + }; + + struct Frame { + uint8_t device_address; + uint8_t length; + uint8_t type; + uint8_t payload[GHST_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for + } PACKED; + + struct Channels12Bit_4Chan { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + uint32_t ch0 : 12; + uint32_t ch1 : 12; + uint32_t ch2 : 12; + uint32_t ch3 : 12; + } PACKED; + + struct RadioFrame { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + Channels12Bit_4Chan channels; // high-res channels + uint8_t lowres_channels[4]; // low-res channels + } PACKED; + + struct LinkStatisticsFrame { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + Channels12Bit_4Chan channels; + uint8_t link_quality; // ( 0 - 100) + uint8_t rssi_dbm; // ( dBm * -1 ) + uint8_t protocol : 5; + uint8_t telemetry : 1; + uint8_t alt_scale : 1; + uint8_t reserved : 1; + int8_t tx_power; + } PACKED; + + enum RFMode { + GHST_RF_MODE_NORMAL = 5, // 55Hz + GHST_RF_MODE_RACE = 6, // 160Hz + GHST_RF_MODE_PURERACE = 7, // 250Hz + GHST_RF_MODE_LR = 8, // 19Hz + GHST_RF_MODE_RACE250 = 10, // 250Hz + GHST_RF_MODE_RACE500 = 11, // 500Hz + GHTS_RF_MODE_SOLID150 = 12, // 150Hz + GHST_RF_MODE_SOLID250 = 13, // 250Hz + RF_MODE_MAX_MODES, + RF_MODE_UNKNOWN, + }; + + struct LinkStatus { + int16_t rssi = -1; + int16_t link_quality = -1; + uint8_t rf_mode; + }; + + bool is_telemetry_supported() const; + + // this will be used by AP_GHST_Telem to access link status data + // from within AP_RCProtocol_GHST thread so no need for cross-thread synch + const volatile LinkStatus& get_link_status() const { + return _link_status; + } + + // return the link rate as defined by the LinkStatistics + uint16_t get_link_rate() const; + + // return the protocol string + const char* get_protocol_string() const; + +private: + struct Frame _frame; + struct Frame _telemetry_frame; + uint8_t _frame_ofs; + uint8_t _frame_crc; + + const uint8_t MAX_CHANNELS = MIN((uint8_t)GHST_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS); + + static AP_RCProtocol_GHST* _singleton; + + void _process_byte(uint32_t timestamp_us, uint8_t byte); + bool decode_ghost_packet(); + bool process_telemetry(bool check_constraint = true); + void process_link_stats_frame(const void* data); + bool write_frame(Frame* frame); + AP_HAL::UARTDriver* get_current_UART() { return get_available_UART(); } + + uint16_t _channels[GHST_MAX_CHANNELS]; /* buffer for extracted RC channel data as pulsewidth in microseconds */ + + void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; } + + uint32_t _last_frame_time_us; + uint32_t _last_tx_frame_time_us; + uint32_t _last_uart_start_time_ms; + uint32_t _last_rx_frame_time_us; + uint32_t _start_frame_time_us; + bool telem_available; + bool _use_lq_for_rssi; + int16_t derive_scaled_lq_value(uint8_t uplink_lq); + + volatile struct LinkStatus _link_status; + + static const uint16_t RF_MODE_RATES[RFMode::RF_MODE_MAX_MODES]; +}; + +namespace AP { + AP_RCProtocol_GHST* ghost(); +}; + +#endif // AP_RCPROTOCOL_GHST_ENABLED diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp index bf286f8985aaec..d35d12d0e2b434 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.cpp @@ -20,11 +20,6 @@ #include "AP_RCProtocol_IBUS.h" -// constructor -AP_RCProtocol_IBUS::AP_RCProtocol_IBUS(AP_RCProtocol &_frontend) : - AP_RCProtocol_Backend(_frontend) -{} - // decode a full IBUS frame bool AP_RCProtocol_IBUS::ibus_decode(const uint8_t frame[IBUS_FRAME_SIZE], uint16_t *values, bool *ibus_failsafe) { diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h index b3e759781662ae..179eb22e22032b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_IBUS.h @@ -30,7 +30,8 @@ class AP_RCProtocol_IBUS : public AP_RCProtocol_Backend { public: - AP_RCProtocol_IBUS(AP_RCProtocol &_frontend); + using AP_RCProtocol_Backend::AP_RCProtocol_Backend; + void process_pulse(uint32_t width_s0, uint32_t width_s1) override; void process_byte(uint8_t byte, uint32_t baudrate) override; private: diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 5619c5373c0819..7136a381e34ef9 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -19,6 +19,10 @@ #define AP_RCPROTOCOL_DRONECAN_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS #endif +#ifndef AP_RCPROTOCOL_DSM_ENABLED +#define AP_RCPROTOCOL_DSM_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED +#endif + #ifndef AP_RCPROTOCOL_FPORT_ENABLED #define AP_RCPROTOCOL_FPORT_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED #endif @@ -61,3 +65,7 @@ #ifndef AP_RCPROTOCOL_FASTSBUS_ENABLED #define AP_RCPROTOCOL_FASTSBUS_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED && AP_RCPROTOCOL_SBUS_ENABLED #endif + +#ifndef AP_RCPROTOCOL_GHST_ENABLED +#define AP_RCPROTOCOL_GHST_ENABLED AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/AP_RCProtocol/examples/RCProtocolDecoder/RCProtocolDecoder.cpp b/libraries/AP_RCProtocol/examples/RCProtocolDecoder/RCProtocolDecoder.cpp index 04836e01d8ef74..0d7ce675d2a71b 100644 --- a/libraries/AP_RCProtocol/examples/RCProtocolDecoder/RCProtocolDecoder.cpp +++ b/libraries/AP_RCProtocol/examples/RCProtocolDecoder/RCProtocolDecoder.cpp @@ -34,7 +34,7 @@ void loop(); #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include +#include #include #include #include @@ -73,7 +73,7 @@ class RC_Channels_RC : public RC_Channels #include RC_Channels_RC _rc; -SocketAPM rc_socket{true}; +SocketAPM_native rc_socket{true}; // change this to the device being tested. const char *devicename = "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A10596TP-if00-port0"; diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index 43f68c4c6a13f5..145f4b867cdce8 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -185,9 +185,14 @@ bool AP_CRSF_Telem::process_rf_mode_changes() if (crsf != nullptr) { uart = crsf->get_UART(); } + if (uart == nullptr) { return true; } + + if (!crsf->is_detected()) { + return false; + } // not ready yet if (!uart->is_initialized()) { return false; @@ -402,11 +407,11 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty) case GENERAL_COMMAND: return _baud_rate_request.pending; case VERSION_PING: - return _crsf_version.pending; + return _crsf_version.pending && AP::crsf()->is_detected(); // only send pings if protocol has been detected case HEARTBEAT: return true; // always send heartbeat if enabled case DEVICE_PING: - return !_crsf_version.pending; // only send pings if version has been negotiated + return !_crsf_version.pending; // only send pings if version has been negotiated default: return _enable_telemetry; } @@ -931,9 +936,9 @@ void AP_CRSF_Telem::calc_attitude() const int16_t INT_PI = 31415; // units are radians * 10000 - _telem.bcast.attitude.roll_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.roll) * 10000.0f), -INT_PI, INT_PI)); - _telem.bcast.attitude.pitch_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.pitch) * 10000.0f), -INT_PI, INT_PI)); - _telem.bcast.attitude.yaw_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.yaw) * 10000.0f), -INT_PI, INT_PI)); + _telem.bcast.attitude.roll_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_roll()) * 10000.0f), -INT_PI, INT_PI)); + _telem.bcast.attitude.pitch_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_pitch()) * 10000.0f), -INT_PI, INT_PI)); + _telem.bcast.attitude.yaw_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_yaw()) * 10000.0f), -INT_PI, INT_PI)); _telem_size = sizeof(AP_CRSF_Telem::AttitudeFrame); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_ATTITUDE; diff --git a/libraries/AP_RCTelemetry/AP_GHST_Telem.cpp b/libraries/AP_RCTelemetry/AP_GHST_Telem.cpp new file mode 100644 index 00000000000000..cdbcea0a543332 --- /dev/null +++ b/libraries/AP_RCTelemetry/AP_GHST_Telem.cpp @@ -0,0 +1,391 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +#include "AP_RCTelemetry_config.h" + +#if AP_GHST_TELEM_ENABLED + +#include "AP_GHST_Telem.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define GHST_DEBUG +#ifdef GHST_DEBUG +# define debug(fmt, args...) hal.console->printf("GHST: " fmt "\n", ##args) +#else +# define debug(fmt, args...) do {} while(0) +#endif + +extern const AP_HAL::HAL& hal; + +AP_GHST_Telem *AP_GHST_Telem::singleton; + +AP_GHST_Telem::AP_GHST_Telem() : AP_RCTelemetry(0) +{ + singleton = this; +} + +AP_GHST_Telem::~AP_GHST_Telem(void) +{ + singleton = nullptr; +} + +bool AP_GHST_Telem::init(void) +{ + // sanity check that we are using a UART for RC input + if (!AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_RCIN, 0)) { + return false; + } + + return AP_RCTelemetry::init(); +} + +/* + setup ready for passthrough telem + */ +void AP_GHST_Telem::setup_wfq_scheduler(void) +{ + // initialize packet weights for the WFQ scheduler + // priority[i] = 1/_scheduler.packet_weight[i] + // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) ) + + // CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit + add_scheduler_entry(50, 120); // Attitude and compass 8Hz + add_scheduler_entry(200, 1000); // VTX parameters 1Hz + add_scheduler_entry(1300, 500); // battery 2Hz + add_scheduler_entry(550, 280); // GPS 3Hz + add_scheduler_entry(550, 280); // GPS2 3Hz +} + +void AP_GHST_Telem::update_custom_telemetry_rates(AP_RCProtocol_GHST::RFMode rf_mode) +{ + if (is_high_speed_telemetry(rf_mode)) { + // standard telemetry for high data rates + set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz + set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz + // custom telemetry for high data rates + set_scheduler_entry(GPS, 550, 500); // 2.0Hz + set_scheduler_entry(GPS2, 550, 500); // 2.0Hz + } else { + // standard telemetry for low data rates + set_scheduler_entry(BATTERY, 1000, 2000); // 0.5Hz + set_scheduler_entry(ATTITUDE, 1000, 3000); // 0.33Hz + // GHST custom telemetry for low data rates + set_scheduler_entry(GPS, 550, 1000); // 1.0Hz + set_scheduler_entry(GPS2, 550, 1000); // 1.0Hz + } +} + +bool AP_GHST_Telem::process_rf_mode_changes() +{ + const AP_RCProtocol_GHST::RFMode current_rf_mode = get_rf_mode(); + uint32_t now = AP_HAL::millis(); + + AP_RCProtocol_GHST* ghost = AP::ghost(); + AP_HAL::UARTDriver* uart = nullptr; + if (ghost != nullptr) { + uart = ghost->get_UART(); + } + + if (uart == nullptr) { + return true; + } + + if (!ghost->is_detected()) { + return false; + } + // not ready yet + if (!uart->is_initialized()) { + return false; + } +#if !defined (STM32H7) + // warn the user if their setup is sub-optimal, H7 does not need DMA on serial port + if (_telem_bootstrap_msg_pending && !uart->is_dma_enabled()) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string()); + } +#endif + // note if option was set to show LQ in place of RSSI + bool current_lq_as_rssi_active = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI); + if(_telem_bootstrap_msg_pending || _noted_lq_as_rssi_active != current_lq_as_rssi_active){ + _noted_lq_as_rssi_active = current_lq_as_rssi_active; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally"); + } + _telem_bootstrap_msg_pending = false; + + const bool is_high_speed = is_high_speed_telemetry(current_rf_mode); + if ((now - _telem_last_report_ms > 5000)) { + // report an RF mode change or a change in telemetry rate if we haven't done so in the last 5s + if (!rc().option_is_enabled(RC_Channels::Option::SUPPRESS_CRSF_MESSAGE) && (_rf_mode != current_rf_mode)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: Link rate %dHz, Telemetry %s", + get_protocol_string(), ghost->get_link_rate(), _enable_telemetry ? "ON" : "OFF"); + } + // tune the scheduler based on telemetry speed high/low transitions + if (_telem_is_high_speed != is_high_speed) { + update_custom_telemetry_rates(current_rf_mode); + } + _telem_is_high_speed = is_high_speed; + _rf_mode = current_rf_mode; + _telem_last_avg_rate = _scheduler.avg_packet_rate; + if (_telem_last_report_ms == 0) { // only want to show bootstrap messages once + _telem_bootstrap_msg_pending = true; + } + _telem_last_report_ms = now; + } + return true; +} + +AP_RCProtocol_GHST::RFMode AP_GHST_Telem::get_rf_mode() const +{ + AP_RCProtocol_GHST* ghost = AP::ghost(); + if (ghost == nullptr) { + return AP_RCProtocol_GHST::RFMode::RF_MODE_UNKNOWN; + } + + return static_cast(ghost->get_link_status().rf_mode); +} + +bool AP_GHST_Telem::is_high_speed_telemetry(const AP_RCProtocol_GHST::RFMode rf_mode) const +{ + return rf_mode == AP_RCProtocol_GHST::RFMode::GHST_RF_MODE_RACE || rf_mode == AP_RCProtocol_GHST::RFMode::GHST_RF_MODE_RACE250; +} + +uint16_t AP_GHST_Telem::get_telemetry_rate() const +{ + return get_avg_packet_rate(); +} + +// WFQ scheduler +bool AP_GHST_Telem::is_packet_ready(uint8_t idx, bool queue_empty) +{ + return _enable_telemetry; +} + +// WFQ scheduler +void AP_GHST_Telem::process_packet(uint8_t idx) +{ + // send packet + switch (idx) { + case ATTITUDE: + calc_attitude(); + break; + case BATTERY: // BATTERY + calc_battery(); + break; + case GPS: // GPS + calc_gps(); + break; + case GPS2: // GPS secondary info + calc_gps2(); + break; + default: + break; + } +} + +// Process a frame from the GHST protocol decoder +bool AP_GHST_Telem::_process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data) { + switch (frame_type) { + // this means we are connected to an RC receiver and can send telemetry + case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_RSSI: { + process_rf_mode_changes(); + _enable_telemetry = AP::ghost()->is_telemetry_supported(); + break; + } + default: + break; + } + return true; +} + +// process any changed settings and schedule for transmission +void AP_GHST_Telem::update() +{ +} + +void AP_GHST_Telem::process_pending_requests() +{ + _pending_request.frame_type = 0; +} + +// prepare battery data +void AP_GHST_Telem::calc_battery() +{ + debug("BATTERY"); + const AP_BattMonitor &_battery = AP::battery(); + + _telem.battery.voltage = htole16(uint16_t(roundf(_battery.voltage(0) * 100.0f))); + + float current; + if (!_battery.current_amps(current, 0)) { + current = 0; + } + _telem.battery.current = htole16(uint16_t(roundf(current * 100.0f))); + + float used_mah; + if (!_battery.consumed_mah(used_mah, 0)) { + used_mah = 0; + } + + _telem.battery.consumed = htole16(uint16_t(roundf(used_mah * 100.0f)));; + _telem.battery.rx_voltage = htole16(uint16_t(roundf(hal.analogin->board_voltage() * 10))); + + _telem_size = sizeof(BatteryFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_PACK_STAT; + + _telem_pending = true; +} + +// prepare gps data +void AP_GHST_Telem::calc_gps() +{ + debug("GPS"); + const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw) + + _telem.gps.latitude = htole32(loc.lat); + _telem.gps.longitude = htole32(loc.lng); + _telem.gps.altitude = htole16(constrain_int16(loc.alt / 100, 0, 5000) + 1000); + + _telem_size = sizeof(AP_GHST_Telem::GPSFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_GPS_PRIMARY; + + _telem_pending = true; +} + +void AP_GHST_Telem::calc_gps2() +{ + debug("GPS2"); + _telem.gps2.groundspeed = htole16(roundf(AP::gps().ground_speed() * 100000 / 3600)); + _telem.gps2.gps_heading = htole16(roundf(AP::gps().ground_course() * 100.0f)); + _telem.gps2.satellites = AP::gps().num_sats(); + + AP_AHRS &_ahrs = AP::ahrs(); + WITH_SEMAPHORE(_ahrs.get_semaphore()); + Location loc; + + if (_ahrs.get_location(loc) && _ahrs.home_is_set()) { + const Location &home_loc = _ahrs.get_home(); + _telem.gps2.home_dist = home_loc.get_distance(loc) / 10; // 10m + _telem.gps2.home_heading = loc.get_bearing_to(home_loc) / 10; // deci-degrees + } else { + _telem.gps2.home_dist = 0; + _telem.gps2.home_heading = 0; + } + + AP_GPS::GPS_Status status = AP::gps().status(); + _telem.gps2.flags = status >= AP_GPS::GPS_OK_FIX_2D ? 0x1 : 0; + + _telem_size = sizeof(AP_GHST_Telem::GPSSecondaryFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_GPS_SECONDARY; + + _telem_pending = true; +} + +// prepare attitude data +void AP_GHST_Telem::calc_attitude() +{ + debug("MAGBARO"); + AP_AHRS &_ahrs = AP::ahrs(); + WITH_SEMAPHORE(_ahrs.get_semaphore()); + + float heading = AP::compass().calculate_heading(_ahrs.get_rotation_body_to_ned()); + _telem.sensor.compass_heading = htole16(degrees(wrap_PI(heading))); + + float alt = AP::baro().get_altitude(); + _telem.sensor.baro_alt = htole16(roundf(alt)); + _telem.sensor.vario = 0; + _telem.sensor.flags = 3; + _telem_size = sizeof(AP_GHST_Telem::SensorFrame); + _telem_type = AP_RCProtocol_GHST::GHST_DL_MAGBARO; + + _telem_pending = true; +} + +/* + fetch GHST frame data + if is_tx_active is true then this will be a request for telemetry after receiving an RC frame + */ +bool AP_GHST_Telem::_get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active) +{ + memset(&_telem, 0, sizeof(TelemetryPayload)); + _is_tx_active = is_tx_active; + + run_wfq_scheduler(); + if (!_telem_pending) { + return false; + } + memcpy(data->payload, &_telem, _telem_size); + data->device_address = AP_RCProtocol_GHST::GHST_ADDRESS_GHST_RECEIVER; + data->length = _telem_size + 2; + data->type = _telem_type; + + _telem_pending = false; + return true; +} + +/* + fetch data for an external transport, such as GHST + */ +bool AP_GHST_Telem::process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data) +{ + if (!get_singleton()) { + return false; + } + return singleton->_process_frame(frame_type, data); +} + +/* + fetch data for an external transport, such as GHST + */ +bool AP_GHST_Telem::get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active) +{ + if (!get_singleton()) { + return false; + } + return singleton->_get_telem_data(data, is_tx_active); +} + +AP_GHST_Telem *AP_GHST_Telem::get_singleton(void) { + if (!singleton && !hal.util->get_soft_armed()) { + // if telem data is requested when we are disarmed and don't + // yet have a AP_GHST_Telem object then try to allocate one + new AP_GHST_Telem(); + // initialize the passthrough scheduler + if (singleton) { + singleton->init(); + } + } + return singleton; +} + +namespace AP { + AP_GHST_Telem *ghost_telem() { + return AP_GHST_Telem::get_singleton(); + } +}; + +#endif // AP_GHST_TELEM_ENABLED diff --git a/libraries/AP_RCTelemetry/AP_GHST_Telem.h b/libraries/AP_RCTelemetry/AP_GHST_Telem.h new file mode 100644 index 00000000000000..768470d9976bdb --- /dev/null +++ b/libraries/AP_RCTelemetry/AP_GHST_Telem.h @@ -0,0 +1,166 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +#pragma once + +#include "AP_RCTelemetry_config.h" + +#if AP_GHST_TELEM_ENABLED + +#include +#include "AP_RCTelemetry.h" +#include + +class AP_GHST_Telem : public AP_RCTelemetry { +public: + AP_GHST_Telem(); + ~AP_GHST_Telem() override; + + /* Do not allow copies */ + CLASS_NO_COPY(AP_GHST_Telem); + + // init - perform required initialisation + virtual bool init() override; + + static AP_GHST_Telem *get_singleton(void); + + // Broadcast frame definitions courtesy of TBS + struct PACKED GPSFrame { + uint32_t latitude; // ( degree * 1e7 ) + uint32_t longitude; // (degree * 1e7 ) + int16_t altitude; // ( meter ) + }; + + struct PACKED GPSSecondaryFrame { + uint16_t groundspeed; // ( cm/s ) + uint16_t gps_heading; // ( degree * 10 ) + uint8_t satellites; // in use ( counter ) + uint16_t home_dist; // ( m / 10 ) + uint16_t home_heading; // ( degree * 10 ) + uint8_t flags; // GPS_FLAGS_FIX 0x01, GPS_FLAGS_FIX_HOME 0x2 + }; + + struct PACKED BatteryFrame { + uint16_t voltage; // ( mV / 10 ) + uint16_t current; // ( mA / 10 ) + uint16_t consumed; // ( mAh / 10 ) + uint8_t rx_voltage; // ( mV / 100 ) + uint8_t spare[3]; + }; + + struct PACKED VTXFrame { +#if __BYTE_ORDER != __LITTLE_ENDIAN +#error "Only supported on little-endian architectures" +#endif + uint8_t flags; + uint16_t frequency; // frequency in Mhz + uint16_t power; // power in mw, 0 == off + uint8_t band : 4; // A, B, E, AirWave, Race + uint8_t channel : 4; // 1x-8x + uint8_t spare[3]; + }; + + struct PACKED SensorFrame { + uint16_t compass_heading; // ( deg * 10 ) + int16_t baro_alt; // ( m ) + int16_t vario; // ( m/s * 100 ) + uint8_t spare[3]; + uint8_t flags; // MISC_FLAGS_MAGHEAD 0x01, MISC_FLAGS_BAROALT 0x02, MISC_FLAGS_VARIO 0x04 + }; + + union PACKED TelemetryPayload { + GPSFrame gps; + GPSSecondaryFrame gps2; + BatteryFrame battery; + VTXFrame vtx; + SensorFrame sensor; + }; + + // get the protocol string + const char* get_protocol_string() const { return AP::ghost()->get_protocol_string(); } + + // Process a frame from the CRSF protocol decoder + static bool process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data); + // process any changed settings and schedule for transmission + void update(); + // get next telemetry data for external consumers of SPort data + static bool get_telem_data(AP_RCProtocol_GHST::Frame* frame, bool is_tx_active); + +private: + + enum SensorType { + ATTITUDE, + VTX_PARAMETERS, + BATTERY, + GPS, + GPS2, + NUM_SENSORS + }; + + // passthrough WFQ scheduler + bool is_packet_ready(uint8_t idx, bool queue_empty) override; + void process_packet(uint8_t idx) override; + void setup_custom_telemetry(); + void update_custom_telemetry_rates(const AP_RCProtocol_GHST::RFMode rf_mode); + + void calc_battery(); + void calc_gps(); + void calc_gps2(); + void calc_attitude(); + void process_pending_requests(); + bool process_rf_mode_changes(); + AP_RCProtocol_GHST::RFMode get_rf_mode() const; + uint16_t get_telemetry_rate() const; + bool is_high_speed_telemetry(const AP_RCProtocol_GHST::RFMode rf_mode) const; + + // setup ready for passthrough operation + void setup_wfq_scheduler(void) override; + + // get next telemetry data for external consumers + bool _get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active); + bool _process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data); + + TelemetryPayload _telem; + uint8_t _telem_size; + uint8_t _telem_type; + + AP_RCProtocol_GHST::RFMode _rf_mode; + bool _enable_telemetry; + + // reporting telemetry rate + uint32_t _telem_last_report_ms; + uint16_t _telem_last_avg_rate; + // do we need to report the initial state + bool _telem_bootstrap_msg_pending; + + bool _telem_is_high_speed; + bool _telem_pending; + // used to limit telemetry when in a failsafe condition + bool _is_tx_active; + + struct { + uint8_t destination = 0; + uint8_t frame_type; + } _pending_request; + + bool _noted_lq_as_rssi_active; + + static AP_GHST_Telem *singleton; +}; + +namespace AP { + AP_GHST_Telem *ghost_telem(); +}; + +#endif // AP_GHST_TELEM_ENABLED diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h b/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h index 8cfb33467db44a..9dab308187a796 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry_config.h @@ -16,3 +16,7 @@ #ifndef HAL_SPEKTRUM_TELEM_ENABLED #define HAL_SPEKTRUM_TELEM_ENABLED 1 #endif + +#ifndef AP_GHST_TELEM_ENABLED +#define AP_GHST_TELEM_ENABLED AP_RCPROTOCOL_GHST_ENABLED +#endif diff --git a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp index 3e669206ec847f..083bb7af923409 100644 --- a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp @@ -296,12 +296,16 @@ void AP_Spektrum_Telem::calc_qos() // prepare rpm data - B/E mandatory frame that must be sent periodically void AP_Spektrum_Telem::calc_rpm() { +#if AP_BATTERY_ENABLED const AP_BattMonitor &_battery = AP::battery(); +#endif _telem.rpm.identifier = TELE_DEVICE_RPM; _telem.rpm.sID = 0; // battery voltage in centivolts, can have up to a 12S battery (4.25Vx12S = 51.0V) +#if AP_BATTERY_ENABLED _telem.rpm.volts = htobe16(((uint16_t)roundf(_battery.voltage(0) * 100.0f))); +#endif _telem.rpm.temperature = htobe16(int16_t(roundf(32.0f + AP::baro().get_temperature(0) * 9.0f / 5.0f))); #if AP_RPM_ENABLED const AP_RPM *rpm = AP::rpm(); @@ -328,10 +332,12 @@ void AP_Spektrum_Telem::send_msg_chunk(const MessageChunk& chunk) // prepare battery data - B/E but not supported by Spektrum void AP_Spektrum_Telem::calc_batt_volts(uint8_t instance) { +#if AP_BATTERY_ENABLED const AP_BattMonitor &_battery = AP::battery(); // battery voltage in centivolts, can have up to a 12S battery (4.25Vx12S = 51.0V) _telem.hv.volts = htobe16(uint16_t(roundf(_battery.voltage(instance) * 100.0f))); +#endif _telem.hv.identifier = TELE_DEVICE_VOLTAGE; _telem.hv.sID = 0; _telem_pending = true; @@ -340,6 +346,7 @@ void AP_Spektrum_Telem::calc_batt_volts(uint8_t instance) // prepare battery data - B/E but not supported by Spektrum void AP_Spektrum_Telem::calc_batt_amps(uint8_t instance) { +#if AP_BATTERY_ENABLED const AP_BattMonitor &_battery = AP::battery(); float current; @@ -349,6 +356,7 @@ void AP_Spektrum_Telem::calc_batt_amps(uint8_t instance) // Range: +/- 150A Resolution: 300A / 2048 = 0.196791 A/count _telem.amps.current = htobe16(int16_t(roundf(current * 2048.0f / 300.0f))); +#endif _telem.amps.identifier = TELE_DEVICE_AMPS; _telem.amps.sID = 0; _telem_pending = true; @@ -357,11 +365,14 @@ void AP_Spektrum_Telem::calc_batt_amps(uint8_t instance) // prepare battery data - L/E void AP_Spektrum_Telem::calc_batt_mah() { +#if AP_BATTERY_ENABLED const AP_BattMonitor &_battery = AP::battery(); +#endif _telem.fpMAH.identifier = TELE_DEVICE_FP_MAH; _telem.fpMAH.sID = 0; +#if AP_BATTERY_ENABLED float current; if (!_battery.current_amps(current, 0)) { current = 0; @@ -396,6 +407,10 @@ void AP_Spektrum_Telem::calc_batt_mah() } else { _telem.fpMAH.temp_B = 0x7FFF; } +#else + _telem.fpMAH.temp_A = 0x7FFF; + _telem.fpMAH.temp_B = 0x7FFF; +#endif _telem_pending = true; } diff --git a/libraries/AP_ROMFS/AP_ROMFS.cpp b/libraries/AP_ROMFS/AP_ROMFS.cpp index c56ce5e2db8987..67adb2313cc53a 100644 --- a/libraries/AP_ROMFS/AP_ROMFS.cpp +++ b/libraries/AP_ROMFS/AP_ROMFS.cpp @@ -34,48 +34,40 @@ const AP_ROMFS::embedded_file AP_ROMFS::files[] = {}; /* find an embedded file */ -const uint8_t *AP_ROMFS::find_file(const char *name, uint32_t &size, uint32_t &crc) +const AP_ROMFS::embedded_file *AP_ROMFS::find_file(const char *name) { for (uint16_t i=0; idecompressed_size; + return f->contents; #else - // last 4 bytes of gzip file are length of decompressed data - const uint8_t *p = &compressed_data[compressed_size-4]; - uint32_t decompressed_size = p[0] | p[1] << 8 | p[2] << 16 | p[3] << 24; - - uint8_t *decompressed_data = (uint8_t *)malloc(decompressed_size + 1); + uint8_t *decompressed_data = (uint8_t *)malloc(f->decompressed_size+1); if (!decompressed_data) { return nullptr; } - // explicitly null terimnate the data - decompressed_data[decompressed_size] = 0; + // explicitly null-terminate the data + decompressed_data[f->decompressed_size] = 0; TINF_DATA *d = (TINF_DATA *)malloc(sizeof(TINF_DATA)); if (!d) { @@ -84,23 +76,12 @@ const uint8_t *AP_ROMFS::find_decompress(const char *name, uint32_t &size) } uzlib_uncompress_init(d, NULL, 0); - d->source = compressed_data; - d->source_limit = compressed_data + compressed_size - 4; - - // assume gzip format - int res = uzlib_gzip_parse_header(d); - if (res != TINF_OK) { - ::free(decompressed_data); - ::free(d); - return nullptr; - } - + d->source = f->contents; + d->source_limit = f->contents + f->compressed_size; d->dest = decompressed_data; - d->destSize = decompressed_size; + d->destSize = f->decompressed_size; - // we don't check CRC, as it just wastes flash space for constant - // ROMFS data - res = uzlib_uncompress(d); + int res = uzlib_uncompress(d); ::free(d); @@ -109,12 +90,12 @@ const uint8_t *AP_ROMFS::find_decompress(const char *name, uint32_t &size) return nullptr; } - if (crc32_small(0, decompressed_data, decompressed_size) != crc) { + if (crc32_small(0, decompressed_data, f->decompressed_size) != f->crc) { ::free(decompressed_data); return nullptr; } - size = decompressed_size; + size = f->decompressed_size; return decompressed_data; #endif } diff --git a/libraries/AP_ROMFS/AP_ROMFS.h b/libraries/AP_ROMFS/AP_ROMFS.h index 78edc703dc264f..33a7c219fb7f03 100644 --- a/libraries/AP_ROMFS/AP_ROMFS.h +++ b/libraries/AP_ROMFS/AP_ROMFS.h @@ -24,14 +24,16 @@ class AP_ROMFS { static const char *dir_list(const char *dirname, uint16_t &ofs); private: - // find an embedded file - static const uint8_t *find_file(const char *name, uint32_t &size, uint32_t &crc); - struct embedded_file { const char *filename; - uint32_t size; + uint32_t compressed_size; + uint32_t decompressed_size; uint32_t crc; const uint8_t *contents; }; + + // find an embedded file + static const AP_ROMFS::embedded_file *find_file(const char *name); + static const struct embedded_file files[]; }; diff --git a/libraries/AP_ROMFS/tinfgzip.cpp b/libraries/AP_ROMFS/tinfgzip.cpp deleted file mode 100644 index f1afdd0b8d87a3..00000000000000 --- a/libraries/AP_ROMFS/tinfgzip.cpp +++ /dev/null @@ -1,110 +0,0 @@ -/* - * tinfgzip - tiny gzip decompressor - * - * Copyright (c) 2003 by Joergen Ibsen / Jibz - * All Rights Reserved - * - * http://www.ibsensoftware.com/ - * - * Copyright (c) 2014-2016 by Paul Sokolovsky - * - * This software is provided 'as-is', without any express - * or implied warranty. In no event will the authors be - * held liable for any damages arising from the use of - * this software. - * - * Permission is granted to anyone to use this software - * for any purpose, including commercial applications, - * and to alter it and redistribute it freely, subject to - * the following restrictions: - * - * 1. The origin of this software must not be - * misrepresented; you must not claim that you - * wrote the original software. If you use this - * software in a product, an acknowledgment in - * the product documentation would be appreciated - * but is not required. - * - * 2. Altered source versions must be plainly marked - * as such, and must not be misrepresented as - * being the original software. - * - * 3. This notice may not be removed or altered from - * any source distribution. - */ - -#include "tinf.h" - -#define FTEXT 1 -#define FHCRC 2 -#define FEXTRA 4 -#define FNAME 8 -#define FCOMMENT 16 - -void tinf_skip_bytes(TINF_DATA *d, int num); -uint16_t tinf_get_uint16(TINF_DATA *d); - -void tinf_skip_bytes(TINF_DATA *d, int num) -{ - while (num--) uzlib_get_byte(d); -} - -uint16_t tinf_get_uint16(TINF_DATA *d) -{ - unsigned int v = uzlib_get_byte(d); - v = (uzlib_get_byte(d) << 8) | v; - return v; -} - -int uzlib_gzip_parse_header(TINF_DATA *d) -{ - unsigned char flg; - - /* -- check format -- */ - - /* check id bytes */ - if (uzlib_get_byte(d) != 0x1f || uzlib_get_byte(d) != 0x8b) return TINF_DATA_ERROR; - - /* check method is deflate */ - if (uzlib_get_byte(d) != 8) return TINF_DATA_ERROR; - - /* get flag byte */ - flg = uzlib_get_byte(d); - - /* check that reserved bits are zero */ - if (flg & 0xe0) return TINF_DATA_ERROR; - - /* -- find start of compressed data -- */ - - /* skip rest of base header of 10 bytes */ - tinf_skip_bytes(d, 6); - - /* skip extra data if present */ - if (flg & FEXTRA) - { - unsigned int xlen = tinf_get_uint16(d); - tinf_skip_bytes(d, xlen); - } - - /* skip file name if present */ - if (flg & FNAME) { while (uzlib_get_byte(d)); } - - /* skip file comment if present */ - if (flg & FCOMMENT) { while (uzlib_get_byte(d)); } - - /* check header crc if present */ - if (flg & FHCRC) - { - /*unsigned int hcrc =*/ tinf_get_uint16(d); - - // TODO: Check! -// if (hcrc != (tinf_crc32(src, start - src) & 0x0000ffff)) -// return TINF_DATA_ERROR; - } - - /* initialize for crc32 checksum */ - d->checksum_type = TINF_CHKSUM_CRC; - d->checksum = ~0; - - return TINF_OK; -} diff --git a/libraries/AP_RSSI/AP_RSSI.cpp b/libraries/AP_RSSI/AP_RSSI.cpp index 883ae2662e0682..033e6122b76795 100644 --- a/libraries/AP_RSSI/AP_RSSI.cpp +++ b/libraries/AP_RSSI/AP_RSSI.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_RSSI_config.h" + +#if AP_RSSI_ENABLED + #include #include #include @@ -272,3 +276,5 @@ AP_RSSI *rssi() } }; + +#endif // AP_RSSI_ENABLED diff --git a/libraries/AP_RSSI/AP_RSSI.h b/libraries/AP_RSSI/AP_RSSI.h index f46438480013df..20f0db1b39ffb1 100644 --- a/libraries/AP_RSSI/AP_RSSI.h +++ b/libraries/AP_RSSI/AP_RSSI.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_RSSI_config.h" + +#if AP_RSSI_ENABLED + #include #include #include @@ -102,3 +106,5 @@ class AP_RSSI namespace AP { AP_RSSI *rssi(); }; + +#endif // AP_RSSI_ENABLED diff --git a/libraries/AP_RSSI/AP_RSSI_config.h b/libraries/AP_RSSI/AP_RSSI_config.h new file mode 100644 index 00000000000000..28f6e724f40656 --- /dev/null +++ b/libraries/AP_RSSI/AP_RSSI_config.h @@ -0,0 +1,7 @@ +#pragma once + +#include + +#ifndef AP_RSSI_ENABLED +#define AP_RSSI_ENABLED 1 +#endif diff --git a/libraries/AP_RTC/AP_RTC.cpp b/libraries/AP_RTC/AP_RTC.cpp index d695e57420ad99..36d78b4043c798 100644 --- a/libraries/AP_RTC/AP_RTC.cpp +++ b/libraries/AP_RTC/AP_RTC.cpp @@ -94,15 +94,8 @@ bool AP_RTC::get_utc_usec(uint64_t &usec) const return true; } -bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const +void AP_RTC::clock_ms_to_hms_fields(const uint64_t time_ms, uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const { - // get time of day in ms - uint64_t time_ms = 0; - if (!get_utc_usec(time_ms)) { - return false; - } - time_ms /= 1000U; - // separate time into ms, sec, min, hour and days but all expressed in milliseconds ms = time_ms % 1000; uint32_t sec_ms = (time_ms % (60 * 1000)) - ms; @@ -113,32 +106,104 @@ bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uin sec = sec_ms / 1000; min = min_ms / (60 * 1000); hour = hour_ms / (60 * 60 * 1000); +} +bool AP_RTC::clock_s_to_date_fields(const uint32_t utc_sec32, uint16_t& year, uint8_t& month, uint8_t& day, uint8_t &hour, uint8_t &min, uint8_t &sec, uint8_t &wday) const +{ + const time_t utc_sec = utc_sec32; + struct tm* tm = gmtime(&utc_sec); + if (tm == nullptr) { + return false; + } + year = tm->tm_year+1900; /* Year, 20xx. */ + month = tm->tm_mon; /* Month. [0-11] */ + day = tm->tm_mday; /* Day. [1-31] */ + hour = tm->tm_hour; /* Hours. [0-23] */ + min = tm->tm_min; /* Minutes. [0-59] */ + sec = tm->tm_sec; /* Seconds. [0-60] (1 leap second) */ + wday = tm->tm_wday; /* week day, [0-6] */ + return true; +} + +/* + return true for leap years + */ +bool AP_RTC::_is_leap(uint32_t y) +{ + y += 1900; + return (y % 4) == 0 && ((y % 100) != 0 || (y % 400) == 0); +} + +/* + implementation of timegm() (from Samba) +*/ +uint32_t AP_RTC::_timegm(struct tm &tm) +{ + static const uint8_t ndays[2][12] = { + {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}, + {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}}; + uint32_t res = 0; + + if (tm.tm_mon > 12 || + tm.tm_mday > 31 || + tm.tm_min > 60 || + tm.tm_sec > 60 || + tm.tm_hour > 24) { + /* invalid tm structure */ + return 0; + } + + for (auto i = 70; i < tm.tm_year; i++) { + res += _is_leap(i) ? 366 : 365; + } + + for (auto i = 0; i < tm.tm_mon; i++) { + res += ndays[_is_leap(tm.tm_year)][i]; + } + res += tm.tm_mday - 1U; + res *= 24U; + res += tm.tm_hour; + res *= 60U; + res += tm.tm_min; + res *= 60U; + res += tm.tm_sec; + return res; +} + +uint32_t AP_RTC::date_fields_to_clock_s(uint16_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec) const +{ + struct tm tm {}; + tm.tm_year = year - 1900; + tm.tm_mon = month; + tm.tm_mday = day; + tm.tm_hour = hour; + tm.tm_min = min; + tm.tm_sec = sec; + return _timegm(tm); +} + +bool AP_RTC::get_system_clock_utc(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const +{ + // get time of day in ms + uint64_t time_ms; + if (!get_utc_usec(time_ms)) { + return false; + } + time_ms /= 1000U; + clock_ms_to_hms_fields(time_ms, hour, min, sec, ms); return true; } bool AP_RTC::get_local_time(uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const { // get local time of day in ms - uint64_t time_ms = 0; - uint64_t ms_local = 0; + uint64_t time_ms; if (!get_utc_usec(time_ms)) { return false; } time_ms /= 1000U; - ms_local = time_ms + (tz_min * 60000); - - // separate time into ms, sec, min, hour and days but all expressed in milliseconds - ms = ms_local % 1000; - uint32_t sec_ms = (ms_local % (60 * 1000)) - ms; - uint32_t min_ms = (ms_local % (60 * 60 * 1000)) - sec_ms - ms; - uint32_t hour_ms = (ms_local % (24 * 60 * 60 * 1000)) - min_ms - sec_ms - ms; - - // convert times as milliseconds into appropriate units - sec = sec_ms / 1000; - min = min_ms / (60 * 1000); - hour = hour_ms / (60 * 60 * 1000); - + const uint64_t ms_local = time_ms + (tz_min * 60000); + clock_ms_to_hms_fields(ms_local, hour, min, sec, ms); return true; } @@ -178,7 +243,7 @@ uint32_t AP_RTC::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms total_delay_ms += ms - curr_ms; } if (largest_element == 1 && total_delay_ms < 0) { - return static_cast(total_delay_ms += 1000); + return static_cast(total_delay_ms + 1000); } // calculate sec to target @@ -186,7 +251,7 @@ uint32_t AP_RTC::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms total_delay_ms += (sec - curr_sec)*1000; } if (largest_element == 2 && total_delay_ms < 0) { - return static_cast(total_delay_ms += (60*1000)); + return static_cast(total_delay_ms + (60*1000)); } // calculate min to target @@ -194,7 +259,7 @@ uint32_t AP_RTC::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms total_delay_ms += (min - curr_min)*60*1000; } if (largest_element == 3 && total_delay_ms < 0) { - return static_cast(total_delay_ms += (60*60*1000)); + return static_cast(total_delay_ms + (60*60*1000)); } // calculate hours to target @@ -202,7 +267,7 @@ uint32_t AP_RTC::get_time_utc(int32_t hour, int32_t min, int32_t sec, int32_t ms total_delay_ms += (hour - curr_hour)*60*60*1000; } if (largest_element == 4 && total_delay_ms < 0) { - return static_cast(total_delay_ms += (24*60*60*1000)); + return static_cast(total_delay_ms + (24*60*60*1000)); } // total delay in milliseconds @@ -229,7 +294,7 @@ bool AP_RTC::get_date_and_time_utc(uint16_t& year, uint8_t& month, uint8_t& day, hour = tm->tm_hour; /* Hours. [0-23] */ min = tm->tm_min; /* Minutes. [0-59] */ sec = tm->tm_sec; /* Seconds. [0-60] (1 leap second) */ - ms = time_us / 1000U; /* milliseconds [0-999] */ + ms = (time_us / 1000U) % 1000U; /* milliseconds [0-999] */ return true; } diff --git a/libraries/AP_RTC/AP_RTC.h b/libraries/AP_RTC/AP_RTC.h index d03846bf6f29b1..a35887532b5473 100644 --- a/libraries/AP_RTC/AP_RTC.h +++ b/libraries/AP_RTC/AP_RTC.h @@ -61,7 +61,10 @@ class AP_RTC { HAL_Semaphore &get_semaphore(void) { return rsem; } - + + bool clock_s_to_date_fields(const uint32_t utc_sec32, uint16_t& year, uint8_t& month, uint8_t& day, uint8_t &hour, uint8_t &min, uint8_t &sec, uint8_t &wday) const; + uint32_t date_fields_to_clock_s(uint16_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec) const; + private: static AP_RTC *_singleton; @@ -70,6 +73,10 @@ class AP_RTC { source_type rtc_source_type = SOURCE_NONE; int64_t rtc_shift; + void clock_ms_to_hms_fields(const uint64_t time_ms, uint8_t &hour, uint8_t &min, uint8_t &sec, uint16_t &ms) const; + + static bool _is_leap(uint32_t y); + static uint32_t _timegm(struct tm &tm); }; namespace AP { diff --git a/libraries/AP_Radio/AP_Radio.cpp b/libraries/AP_Radio/AP_Radio.cpp index ae7f805922a8aa..5afaa5764b6032 100644 --- a/libraries/AP_Radio/AP_Radio.cpp +++ b/libraries/AP_Radio/AP_Radio.cpp @@ -1,6 +1,8 @@ -#include +#include "AP_Radio_config.h" + +#if AP_RADIO_ENABLED -#if HAL_RCINPUT_WITH_AP_RADIO +#include #include "AP_Radio.h" #include "AP_Radio_backend.h" @@ -151,40 +153,36 @@ AP_Radio::AP_Radio(void) bool AP_Radio::init(void) { switch (radio_type) { -#if (not defined AP_RADIO_CYRF6936 || AP_RADIO_CYRF6936) +#if AP_RADIO_CYRF6936_ENABLED case RADIO_TYPE_CYRF6936: driver = new AP_Radio_cypress(*this); break; #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#if (not defined AP_RADIO_CC2500 || AP_RADIO_CC2500) +#if AP_RADIO_CC2500_ENABLED case RADIO_TYPE_CC2500: driver = new AP_Radio_cc2500(*this); break; #endif -#endif -#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 -#if (not defined AP_RADIO_BK2425 || AP_RADIO_BK2425) +#if AP_RADIO_BK2425_ENABLED case RADIO_TYPE_BK2425: driver = new AP_Radio_beken(*this); break; #endif -#endif #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 case RADIO_TYPE_AUTO: // auto-detect between cc2500 and beken radios -#if (not defined AP_RADIO_CC2500 || AP_RADIO_CC2500) +#if AP_RADIO_CC2500_ENABLED if (AP_Radio_cc2500::probe()) { driver = new AP_Radio_cc2500(*this); } #endif -#if (not defined AP_RADIO_BK2425 || AP_RADIO_BK2425) +#if AP_RADIO_BK2425_ENABLED if (driver == nullptr) { driver = new AP_Radio_beken(*this); } #endif break; -#endif +#endif // CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 default: break; } @@ -301,5 +299,4 @@ void AP_Radio::change_txmode(void) } } -#endif // HAL_RCINPUT_WITH_AP_RADIO - +#endif // AP_RADIO_ENABLED diff --git a/libraries/AP_Radio/AP_Radio.h b/libraries/AP_Radio/AP_Radio.h index 0095457de26d33..450bcecd4a781c 100644 --- a/libraries/AP_Radio/AP_Radio.h +++ b/libraries/AP_Radio/AP_Radio.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_Radio_config.h" + +#if AP_RADIO_ENABLED + /* * base class for direct attached radios */ @@ -128,3 +132,5 @@ class AP_Radio static AP_Radio *_singleton; }; + +#endif // AP_RADIO_ENABLED diff --git a/libraries/AP_Radio/AP_Radio_backend.cpp b/libraries/AP_Radio/AP_Radio_backend.cpp index b30a509d972ef2..f3dcdaf2de2ef7 100644 --- a/libraries/AP_Radio/AP_Radio_backend.cpp +++ b/libraries/AP_Radio/AP_Radio_backend.cpp @@ -16,6 +16,10 @@ * backend class for direct attached radios */ +#include "AP_Radio_config.h" + +#if AP_RADIO_ENABLED + #include #include "AP_Radio_backend.h" @@ -27,3 +31,5 @@ AP_Radio_backend::AP_Radio_backend(AP_Radio &_radio) : AP_Radio_backend::~AP_Radio_backend(void) { } + +#endif // AP_RADIO_ENABLED diff --git a/libraries/AP_Radio/AP_Radio_backend.h b/libraries/AP_Radio/AP_Radio_backend.h index a1fd9e168e8de8..f360224603aa3e 100644 --- a/libraries/AP_Radio/AP_Radio_backend.h +++ b/libraries/AP_Radio/AP_Radio_backend.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_Radio_config.h" + +#if AP_RADIO_ENABLED + /* * backend class for direct attached radios */ @@ -156,3 +160,5 @@ class AP_Radio_backend AP_Radio &radio; }; + +#endif // AP_RADIO_ENABLED diff --git a/libraries/AP_Radio/AP_Radio_bk2425.cpp b/libraries/AP_Radio/AP_Radio_bk2425.cpp index 46c4ca079cabc1..2ed5b64f1b710a 100644 --- a/libraries/AP_Radio/AP_Radio_bk2425.cpp +++ b/libraries/AP_Radio/AP_Radio_bk2425.cpp @@ -1,3 +1,7 @@ +#include "AP_Radio_config.h" + +#if AP_RADIO_BK2425_ENABLED + /* driver for Beken_2425 radio */ @@ -5,8 +9,6 @@ //#pragma GCC optimize("O0") -#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 - #include #include "AP_Radio_bk2425.h" #include @@ -1421,7 +1423,4 @@ void SyncAdaptive::Miss(uint8_t channel) } } - - -#endif // HAL_RCINPUT_WITH_AP_RADIO - +#endif // AP_RADIO_BK2425_ENABLED diff --git a/libraries/AP_Radio/AP_Radio_bk2425.h b/libraries/AP_Radio/AP_Radio_bk2425.h index 5d35e250d7c3e1..4481b707791342 100644 --- a/libraries/AP_Radio/AP_Radio_bk2425.h +++ b/libraries/AP_Radio/AP_Radio_bk2425.h @@ -20,9 +20,12 @@ With thanks to cleanflight and betaflight projects */ +#include "AP_Radio_config.h" + +#if AP_RADIO_BK2425_ENABLED + #include "AP_Radio_backend.h" -#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 #include "hal.h" #include "telem_structure.h" #include "driver_bk2425.h" @@ -258,4 +261,4 @@ class AP_Radio_beken : public AP_Radio_backend uint8_t myDroneId[4]; // CRC of the flight boards UUID, to inform the tx }; -#endif // HAL_RCINPUT_WITH_AP_RADIO +#endif // AP_RADIO_BK2425_ENABLED diff --git a/libraries/AP_Radio/AP_Radio_cc2500.h b/libraries/AP_Radio/AP_Radio_cc2500.h index 1ddc608173ad32..b5eedad3fd36a6 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.h +++ b/libraries/AP_Radio/AP_Radio_cc2500.h @@ -20,8 +20,12 @@ With thanks to cleanflight and betaflight projects */ +#include "AP_Radio_config.h" + +#if AP_RADIO_CC2500_ENABLED + #include "AP_Radio_backend.h" -#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS + #include "hal.h" #include "telem_structure.h" #include "driver_cc2500.h" @@ -226,5 +230,4 @@ class AP_Radio_cc2500 : public AP_Radio_backend }; -#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS - +#endif // AP_RADIO_CC2500_ENABLED diff --git a/libraries/AP_Radio/AP_Radio_config.h b/libraries/AP_Radio/AP_Radio_config.h new file mode 100644 index 00000000000000..92ab9fbd789a29 --- /dev/null +++ b/libraries/AP_Radio/AP_Radio_config.h @@ -0,0 +1,23 @@ +#pragma once + +#include + +#ifndef AP_RADIO_ENABLED +#define AP_RADIO_ENABLED HAL_RCINPUT_WITH_AP_RADIO +#endif + +#ifndef AP_RADIO_BACKEND_DEFAULT_ENABLED +#define AP_RADIO_BACKEND_DEFAULT_ENABLED AP_RADIO_ENABLED +#endif + +#ifndef AP_RADIO_CC2500_ENABLED +#define AP_RADIO_CC2500_ENABLED AP_RADIO_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RADIO_CYRF6936_ENABLED +#define AP_RADIO_CYRF6936_ENABLED AP_RADIO_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_RADIO_BK2425_ENABLED +#define AP_RADIO_BK2425_ENABLED AP_RADIO_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp index ff6f99b4b65ef6..7d3d2101be0fa2 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.cpp +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -1,6 +1,8 @@ -#include +#include "AP_Radio_config.h" + +#if AP_RADIO_CYRF6936_ENABLED -#if HAL_RCINPUT_WITH_AP_RADIO +#include #include #include "AP_Radio_cypress.h" @@ -1681,5 +1683,4 @@ void AP_Radio_cypress::handle_data_packet(mavlink_channel_t chan, const mavlink_ } } -#endif // HAL_RCINPUT_WITH_AP_RADIO - +#endif // AP_RADIO_CYRF6936_ENABLED diff --git a/libraries/AP_Radio/AP_Radio_cypress.h b/libraries/AP_Radio/AP_Radio_cypress.h index 15c63d528d7071..504f9737e9b3c2 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.h +++ b/libraries/AP_Radio/AP_Radio_cypress.h @@ -14,7 +14,9 @@ */ #pragma once -#if HAL_RCINPUT_WITH_AP_RADIO +#include "AP_Radio_config.h" + +#if AP_RADIO_CYRF6936_ENABLED /* AP_Radio implementation for Cypress 2.4GHz radio. @@ -279,4 +281,4 @@ class AP_Radio_cypress : public AP_Radio_backend void setup_timeout(uint32_t timeout_ms); }; -#endif +#endif // AP_RADIO_CYRPRESS_ENABLED diff --git a/libraries/AP_Radio/driver_bk2425.cpp b/libraries/AP_Radio/driver_bk2425.cpp index 04452cde2beda5..4fb08653c887d5 100644 --- a/libraries/AP_Radio/driver_bk2425.cpp +++ b/libraries/AP_Radio/driver_bk2425.cpp @@ -1,11 +1,13 @@ +#include "AP_Radio_config.h" + +#if AP_RADIO_BK2425_ENABLED + // -------------------------------------------------------------------- // low level driver for the beken radio on SPI // -------------------------------------------------------------------- #include "driver_bk2425.h" -#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 - #include #include using namespace ChibiOS; @@ -722,4 +724,4 @@ void Radio_Beken::DumpRegisters(void) SetRBank(0); } -#endif // HAL_RCINPUT_WITH_AP_RADIO +#endif // AP_RADIO_BK2425_ENABLED diff --git a/libraries/AP_Radio/driver_bk2425.h b/libraries/AP_Radio/driver_bk2425.h index 14fbf5a264a147..9352a21ff75308 100644 --- a/libraries/AP_Radio/driver_bk2425.h +++ b/libraries/AP_Radio/driver_bk2425.h @@ -6,9 +6,11 @@ #pragma once -#include +#include "AP_Radio_config.h" + +#if AP_RADIO_BK2425_ENABLED -#if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412 +#include #define SUPPORT_BK_DEBUG_PINS 0 // 0=UART6 is for GPS, 1=UART6 is debug gpio #define TX_SPEED 250u // Default transmit speed in kilobits per second. @@ -414,4 +416,4 @@ class Radio_Beken BkRadioMode bkMode; }; -#endif +#endif // AP_RADIO_BK2425_ENABLED diff --git a/libraries/AP_Rally/AP_Rally.cpp b/libraries/AP_Rally/AP_Rally.cpp index d3b8284a963aa6..e2883152074ca1 100644 --- a/libraries/AP_Rally/AP_Rally.cpp +++ b/libraries/AP_Rally/AP_Rally.cpp @@ -12,8 +12,6 @@ // storage object StorageAccess AP_Rally::_storage(StorageManager::StorageRally); -assert_storage_size _assert_storage_size_RallyLocation; - #if APM_BUILD_COPTER_OR_HELI #define RALLY_LIMIT_KM_DEFAULT 0.3f #define RALLY_INCLUDE_HOME_DEFAULT 1 @@ -56,6 +54,8 @@ const AP_Param::GroupInfo AP_Rally::var_info[] = { // constructor AP_Rally::AP_Rally() { + ASSERT_STORAGE_SIZE(RallyLocation, 15); + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (_singleton != nullptr) { AP_HAL::panic("Rally must be singleton"); @@ -116,7 +116,9 @@ bool AP_Rally::set_rally_point_with_index(uint8_t i, const RallyLocation &rallyL _last_change_time_ms = AP_HAL::millis(); +#if HAL_LOGGING_ENABLED AP::logger().Write_RallyPoint(_rally_point_total_count, i, rallyLoc); +#endif return true; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index b9ea6f90765edd..8d245855c6e4b2 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -786,6 +786,7 @@ bool RangeFinder::get_temp(enum Rotation orientation, float &temp) const return backend->get_temp(temp); } +#if HAL_LOGGING_ENABLED // Write an RFND (rangefinder) packet void RangeFinder::Log_RFND() const { @@ -816,6 +817,7 @@ void RangeFinder::Log_RFND() const AP::logger().WriteBlock(&pkt, sizeof(pkt)); } } +#endif // HAL_LOGGING_ENABLED bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp index 0809cd516a49a9..bd0e9be2945ace 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.cpp @@ -53,30 +53,39 @@ bool AP_RangeFinder_Backend::has_data() const { } // update status based on distance measurement -void AP_RangeFinder_Backend::update_status() +void AP_RangeFinder_Backend::update_status(RangeFinder::RangeFinder_State &state_arg) const { // check distance - if (state.distance_m > max_distance_cm() * 0.01f) { - set_status(RangeFinder::Status::OutOfRangeHigh); - } else if (state.distance_m < min_distance_cm() * 0.01f) { - set_status(RangeFinder::Status::OutOfRangeLow); + if (state_arg.distance_m > max_distance_cm() * 0.01f) { + set_status(state_arg, RangeFinder::Status::OutOfRangeHigh); + } else if (state_arg.distance_m < min_distance_cm() * 0.01f) { + set_status(state_arg, RangeFinder::Status::OutOfRangeLow); } else { - set_status(RangeFinder::Status::Good); + set_status(state_arg, RangeFinder::Status::Good); } } // set status and update valid count -void AP_RangeFinder_Backend::set_status(RangeFinder::Status _status) +void AP_RangeFinder_Backend::set_status(RangeFinder::RangeFinder_State &state_arg, RangeFinder::Status _status) { - state.status = _status; + state_arg.status = _status; // update valid count if (_status == RangeFinder::Status::Good) { - if (state.range_valid_count < 10) { - state.range_valid_count++; + if (state_arg.range_valid_count < 10) { + state_arg.range_valid_count++; } } else { - state.range_valid_count = 0; + state_arg.range_valid_count = 0; } } +#if AP_SCRIPTING_ENABLED +// get a copy of state structure +void AP_RangeFinder_Backend::get_state(RangeFinder::RangeFinder_State &state_arg) +{ + WITH_SEMAPHORE(_sem); + state_arg = state; +} +#endif + diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h index 86f487364ba342..ca2decd044e309 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend.h @@ -36,9 +36,11 @@ class AP_RangeFinder_Backend virtual void handle_msg(const mavlink_message_t &msg) { return; } #if AP_SCRIPTING_ENABLED - // Returns false if scripting backing hasn't been setup - // Get distance from lua script - virtual bool handle_script_msg(float dist_m) { return false; } + void get_state(RangeFinder::RangeFinder_State &state_arg); + + // Returns false if scripting backing hasn't been setup. + virtual bool handle_script_msg(float dist_m) { return false; } // legacy interface + virtual bool handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) { return false; } #endif #if HAL_MSP_RANGEFINDER_ENABLED @@ -80,10 +82,12 @@ class AP_RangeFinder_Backend protected: // update status based on distance measurement - void update_status(); + void update_status(RangeFinder::RangeFinder_State &state_arg) const; + void update_status() { update_status(state); } // set status and update valid_count - void set_status(RangeFinder::Status status); + static void set_status(RangeFinder::RangeFinder_State &state_arg, RangeFinder::Status status); + void set_status(RangeFinder::Status status) { set_status(state, status); } RangeFinder::RangeFinder_State &state; AP_RangeFinder_Params ¶ms; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp index cd024575136d27..75e22b2aaba5f4 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.cpp @@ -24,31 +24,49 @@ AP_RangeFinder_Lua::AP_RangeFinder_Lua(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params) : AP_RangeFinder_Backend(_state, _params) { - set_status(RangeFinder::Status::NoData); } -// Set the distance based on a Lua Script -bool AP_RangeFinder_Lua::handle_script_msg(float dist_m) +// Process range finder data from a lua driver. The state structure needs to be completely +// filled in by the lua script. The data passed to this method is copied to a pending_state +// structure. The update() method periodically copies data from pending_state to state. The get_state() +// method returns data from state. +bool AP_RangeFinder_Lua::handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) { - state.last_reading_ms = AP_HAL::millis(); - _distance_m = dist_m; + WITH_SEMAPHORE(_sem); + _state_pending = state_arg; return true; } +// Process range finder data from a lua driver - legacy interface. This method takes +// a distance measurement and fills in the pending state structure. In this legacy mode +// the lua script only passes in a distance measurement and does not manage the rest +// of the fields in the state structure. +bool AP_RangeFinder_Lua::handle_script_msg(float dist_m) { -// update the state of the sensor -void AP_RangeFinder_Lua::update(void) -{ - //Time out on incoming data; if we don't get new - //data in 500ms, dump it - if (AP_HAL::millis() - state.last_reading_ms > AP_RANGEFINDER_LUA_TIMEOUT_MS) { - set_status(RangeFinder::Status::NoData); - state.distance_m = 0.0f; + const uint32_t now = AP_HAL::millis(); + + WITH_SEMAPHORE(_sem); + + // Time out on incoming data; if we don't get new data in 500ms, dump it + if (now - _state_pending.last_reading_ms > AP_RANGEFINDER_LUA_TIMEOUT_MS) { + set_status(_state_pending, RangeFinder::Status::NoData); } else { - state.distance_m = _distance_m; - update_status(); + _state_pending.last_reading_ms = now; + _state_pending.distance_m = dist_m; + _state_pending.signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN; + _state_pending.voltage_mv = 0; + update_status(_state_pending); } + + return true; +} + +// Update the state of the sensor +void AP_RangeFinder_Lua::update(void) +{ + WITH_SEMAPHORE(_sem); + state = _state_pending; } #endif // AP_RANGEFINDER_LUA_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h index 12e5fad3675a23..0bd7f7a1acb6b7 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lua.h @@ -21,6 +21,7 @@ class AP_RangeFinder_Lua : public AP_RangeFinder_Backend // Get update from Lua script bool handle_script_msg(float dist_m) override; + bool handle_script_msg(const RangeFinder::RangeFinder_State &state_arg) override; MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { return MAV_DISTANCE_SENSOR_UNKNOWN; @@ -28,7 +29,7 @@ class AP_RangeFinder_Lua : public AP_RangeFinder_Backend private: - float _distance_m; // stored data from lua script: + RangeFinder::RangeFinder_State _state_pending = {}; }; #endif // AP_RANGEFINDER_LUA_ENABLED diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 1c4cb143f7b0fc..4aa31ec215737a 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -12,9 +12,28 @@ #include "AP_Relay.h" #include +#include #include #include +#include +#include +#include + +#include +#if APM_BUILD_TYPE(APM_BUILD_Rover) +#include +#endif + +#if AP_RELAY_DRONECAN_ENABLED +#include +#include +#endif + +#if AP_SIM_ENABLED +#include +#endif + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define RELAY1_PIN_DEFAULT 13 @@ -60,54 +79,107 @@ const AP_Param::GroupInfo AP_Relay::var_info[] = { - // @Param: PIN - // @DisplayName: First Relay Pin - // @Description: Digital pin number for first relay control. This is the pin used for camera shutter control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,27:BBBMini Pin P8.17,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN", 0, AP_Relay, _pin[0], RELAY1_PIN_DEFAULT), - - // @Param: PIN2 - // @DisplayName: Second Relay Pin - // @Description: Digital pin number for 2nd relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,65:BBBMini Pin P8.18,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN2", 1, AP_Relay, _pin[1], RELAY2_PIN_DEFAULT), - - // @Param: PIN3 - // @DisplayName: Third Relay Pin - // @Description: Digital pin number for 3rd relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,22:BBBMini Pin P8.19,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN3", 2, AP_Relay, _pin[2], RELAY3_PIN_DEFAULT), - - // @Param: PIN4 - // @DisplayName: Fourth Relay Pin - // @Description: Digital pin number for 4th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,63:BBBMini Pin P8.34,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN4", 3, AP_Relay, _pin[3], RELAY4_PIN_DEFAULT), - - // @Param: DEFAULT - // @DisplayName: Default relay state - // @Description: The state of the relay on boot. - // @User: Standard - // @Values: 0:Off,1:On,2:NoChange - AP_GROUPINFO("DEFAULT", 4, AP_Relay, _default, 0), - - // @Param: PIN5 - // @DisplayName: Fifth Relay Pin - // @Description: Digital pin number for 5th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,62:BBBMini Pin P8.13,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN5", 5, AP_Relay, _pin[4], RELAY5_PIN_DEFAULT), - - // @Param: PIN6 - // @DisplayName: Sixth Relay Pin - // @Description: Digital pin number for 6th relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. - // @User: Standard - // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,37:BBBMini Pin P8.14,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 - AP_GROUPINFO("PIN6", 6, AP_Relay, _pin[5], RELAY6_PIN_DEFAULT), + // 0 was PIN + // 1 was PIN2 + // 2 was PIN3 + // 3 was PIN4 + // 4 was DEFAULT + // 5 was PIN5 + // 6 was PIN6 + + // @Group: 1_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[0], "1_", 7, AP_Relay, AP_Relay_Params), + +#if AP_RELAY_NUM_RELAYS > 1 + // @Group: 2_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[1], "2_", 8, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 2 + // @Group: 3_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[2], "3_", 9, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 3 + // @Group: 4_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[3], "4_", 10, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 4 + // @Group: 5_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[4], "5_", 11, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 5 + // @Group: 6_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[5], "6_", 12, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 6 + // @Group: 7_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[6], "7_", 13, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 7 + // @Group: 8_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[7], "8_", 14, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 8 + // @Group: 9_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[8], "9_", 15, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 9 + // @Group: 10_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[9], "10_", 16, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 10 + // @Group: 11_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[10], "11_", 17, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 11 + // @Group: 12_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[11], "12_", 18, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 12 + // @Group: 13_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[12], "13_", 19, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 13 + // @Group: 14_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[13], "14_", 20, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 14 + // @Group: 15_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[14], "15_", 21, AP_Relay, AP_Relay_Params), +#endif + +#if AP_RELAY_NUM_RELAYS > 15 + // @Group: 16_ + // @Path: AP_Relay_Params.cpp + AP_SUBGROUPINFO(_params[15], "16_", 22, AP_Relay, AP_Relay_Params), +#endif AP_GROUPEND }; @@ -128,61 +200,271 @@ AP_Relay::AP_Relay(void) singleton = this; } +void AP_Relay::convert_params() +{ + // PARAMETER_CONVERSION - Added: Dec-2023 +#ifndef HAL_BUILD_AP_PERIPH + // Dont need this conversion on periph as relay support is more recent + + // Before converting local params we must find any relays being used by index from external libs + int8_t ice_relay = -1; +#if AP_ICENGINE_ENABLED + AP_ICEngine *ice = AP::ice(); + int8_t ice_relay_index; + if (ice != nullptr && ice->get_legacy_ignition_relay_index(ice_relay_index)) { + ice_relay = ice_relay_index; + } +#endif + + int8_t chute_relay = -1; +#if HAL_PARACHUTE_ENABLED + AP_Parachute *parachute = AP::parachute(); + int8_t parachute_relay_index; + if (parachute != nullptr && parachute->get_legacy_relay_index(parachute_relay_index)) { + chute_relay = parachute_relay_index; + } +#endif + + int8_t cam_relay = -1; +#if AP_CAMERA_ENABLED + AP_Camera *camera = AP::camera(); + int8_t camera_relay_index; + if ((camera != nullptr) && (camera->get_legacy_relay_index(camera_relay_index))) { + cam_relay = camera_relay_index; + } +#endif + +#if APM_BUILD_TYPE(APM_BUILD_Rover) + int8_t rover_relay[] = { -1, -1, -1, -1 }; + AP_MotorsUGV *motors = AP::motors_ugv(); + if (motors != nullptr) { + motors->get_legacy_relay_index(rover_relay[0], rover_relay[1], rover_relay[2], rover_relay[3]); + } +#endif + + // Find old default param + int8_t default_state = 0; // off was the old behaviour + const bool have_default = AP_Param::get_param_by_index(this, 4, AP_PARAM_INT8, &default_state); + + // grab the old values if they were set + for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), 6U); i++) { + if (_params[i].function.configured()) { + // Conversion already done, or user has configured manually + continue; + } + + uint8_t param_index = i; + if (i > 3) { + // Skip over the old DEFAULT parameter at index 4 + param_index += 1; + } + + int8_t pin = 0; + if (!_params[i].pin.configured() && AP_Param::get_param_by_index(this, param_index, AP_PARAM_INT8, &pin) && (pin >= 0)) { + // Copy old pin parameter if valid + _params[i].pin.set_and_save(pin); + } + + // Work out what function this relay should be + AP_Relay_Params::FUNCTION new_fun; + if (i == chute_relay) { + new_fun = AP_Relay_Params::FUNCTION::PARACHUTE; + + } else if (i == ice_relay) { + new_fun = AP_Relay_Params::FUNCTION::IGNITION; + + } else if (i == cam_relay) { + new_fun = AP_Relay_Params::FUNCTION::CAMERA; + +#if APM_BUILD_TYPE(APM_BUILD_Rover) + } else if (i == rover_relay[0]) { + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1; + + } else if (i == rover_relay[1]) { + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2; + + } else if (i == rover_relay[2]) { + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3; + + } else if (i == rover_relay[3]) { + new_fun = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4; +#endif + + } else { + if (_params[i].pin < 0) { + // Don't enable as numbered relay if pin is invalid + // Other functions should be enabled with a invalid pin + // This will result in a pre-arm promoting the user to resolve + continue; + } + new_fun = AP_Relay_Params::FUNCTION::RELAY; + + } + _params[i].function.set_and_save(int8_t(new_fun)); + + + // Set the default state + if (have_default) { + _params[i].default_state.set_and_save(default_state); + } + } +#endif // HAL_BUILD_AP_PERIPH +} + +void AP_Relay::set_defaults() { + const int8_t pins[] = { RELAY1_PIN_DEFAULT, + RELAY2_PIN_DEFAULT, + RELAY3_PIN_DEFAULT, + RELAY4_PIN_DEFAULT, + RELAY5_PIN_DEFAULT, + RELAY6_PIN_DEFAULT }; + + for (uint8_t i = 0; i < MIN(ARRAY_SIZE(_params), ARRAY_SIZE(pins)); i++) { + // set the default + if (pins[i] != -1) { + _params[i].pin.set_default(pins[i]); + } + } +} + +// Return true is function is valid +bool AP_Relay::function_valid(AP_Relay_Params::FUNCTION function) const +{ + return (function > AP_Relay_Params::FUNCTION::NONE) && (function < AP_Relay_Params::FUNCTION::NUM_FUNCTIONS); +} void AP_Relay::init() { - if (_default != 0 && _default != 1) { + set_defaults(); + + convert_params(); + + // setup the actual default values of all the pins + for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) { + const int16_t pin = _params[instance].pin; + if (pin == -1) { + // no valid pin to set it on, skip it + continue; + } + + const AP_Relay_Params::FUNCTION function = _params[instance].function; + if (!function_valid(function)) { + // invalid function, skip it + continue; + } + + if (function == AP_Relay_Params::FUNCTION::RELAY) { + // relay by instance number, set the state to match our output + const AP_Relay_Params::DefaultState default_state = _params[instance].default_state; + if ((default_state == AP_Relay_Params::DefaultState::OFF) || + (default_state == AP_Relay_Params::DefaultState::ON)) { + + set_pin_by_instance(instance, (bool)default_state); + } + } else { + // all functions are supposed to be off by default + // this will need revisiting when we support inversion + set_pin_by_instance(instance, false); + } + + // Make sure any DroneCAN pin is enabled for streaming +#if AP_RELAY_DRONECAN_ENABLED + dronecan.enable_pin(pin); +#endif + + } +} + +void AP_Relay::set(const AP_Relay_Params::FUNCTION function, const bool value) { + if (!function_valid(function)) { + // invalid function return; } - for (uint8_t i=0; i= AP_RELAY_NUM_RELAYS) { - return; - } - if (_pin[instance] == -1) { + const int16_t pin = _params[instance].pin; + if (pin == -1) { + // no valid pin to set it on, skip it return; } - const uint32_t now = AP_HAL::millis(); - _pin_states = value ? _pin_states | (1U< 10)) { - AP::logger().Write("RELY", "TimeUS,State", "s-", "F-", "QB", - AP_HAL::micros64(), - _pin_states); - _last_log_ms = now; - _last_logged_pin_states = _pin_states; - } + #if AP_SIM_ENABLED if (!(AP::sitl()->on_hardware_relay_enable_mask & (1U << instance))) { return; } #endif - hal.gpio->pinMode(_pin[instance], HAL_GPIO_OUTPUT); - hal.gpio->write(_pin[instance], value); + + const bool initial_value = get_pin(pin); + + if (initial_value != value) { + set_pin(pin, value); +#if HAL_LOGGING_ENABLED + AP::logger().Write("RELY", "TimeUS,Instance,State", "s#-", "F--", "QBB", + AP_HAL::micros64(), + instance, + value); +#endif + } +} + +void AP_Relay::set(const uint8_t instance, const bool value) +{ + if (instance >= ARRAY_SIZE(_params)) { + return; + } + + if (_params[instance].function != AP_Relay_Params::FUNCTION::RELAY) { + return; + } + + set_pin_by_instance(instance, value); } void AP_Relay::toggle(uint8_t instance) { - if (instance < AP_RELAY_NUM_RELAYS && _pin[instance] != -1) { - bool ison = hal.gpio->read(_pin[instance]); - set(instance, !ison); + if (instance < ARRAY_SIZE(_params)) { + set(instance, !get(instance)); } } // check settings are valid bool AP_Relay::arming_checks(size_t buflen, char *buffer) const { - for (uint8_t i=0; ivalid_pin(pin)) { - char param_name_buf[11] = "RELAY_PIN"; - if (i > 0) { - hal.util->snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY_PIN%u", unsigned(i+1)); - } + for (uint8_t i=0; ivalid_pin(pin)) { + // Check GPIO pin is valid + char param_name_buf[14]; + hal.util->snprintf(param_name_buf, ARRAY_SIZE(param_name_buf), "RELAY%u_PIN", unsigned(i+1)); uint8_t servo_ch; if (hal.gpio->pin_to_servo_channel(pin, servo_ch)) { hal.util->snprintf(buffer, buflen, "%s=%d, set SERVO%u_FUNCTION=-1", param_name_buf, int(pin), unsigned(servo_ch+1)); @@ -191,30 +473,198 @@ bool AP_Relay::arming_checks(size_t buflen, char *buffer) const } return false; } + + // Each pin can only be used by a single relay + for (uint8_t j=i+1; jsnprintf(buffer, buflen, "pin conflict RELAY%u_PIN = RELAY%u_PIN", int(i+1), int(j+1)); + return false; + } + } } return true; } +bool AP_Relay::get(uint8_t instance) const +{ + if (instance >= ARRAY_SIZE(_params)) { + // invalid instance + return false; + } + + return get_pin(_params[instance].pin.get()); +} + +// Get relay state from pin number +bool AP_Relay::get_pin(const int16_t pin) const +{ + if (pin < 0) { + // invalid pin + return false; + } + +#if AP_RELAY_DRONECAN_ENABLED + if (dronecan.valid_pin(pin)) { + // Virtual DroneCAN pin + return dronecan.get_pin(pin); + } +#endif + + // Real GPIO pin + hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT); + return (bool)hal.gpio->read(pin); +} + +// Set relay state from pin number +void AP_Relay::set_pin(const int16_t pin, const bool value) +{ + if (pin < 0) { + // invalid pin + return; + } + +#if AP_RELAY_DRONECAN_ENABLED + if (dronecan.valid_pin(pin)) { + // Virtual DroneCAN pin + dronecan.set_pin(pin, value); + return; + } +#endif + + // Real GPIO pin + hal.gpio->pinMode(pin, HAL_GPIO_OUTPUT); + hal.gpio->write(pin, value); +} + +// see if the relay is enabled +bool AP_Relay::enabled(uint8_t instance) const +{ + // Must be a valid instance with function relay and pin set + return (instance < ARRAY_SIZE(_params)) && (_params[instance].pin != -1) && (_params[instance].function == AP_Relay_Params::FUNCTION::RELAY); +} + +// see if the relay is enabled +bool AP_Relay::enabled(AP_Relay_Params::FUNCTION function) const +{ + for (uint8_t instance = 0; instance < ARRAY_SIZE(_params); instance++) { + if ((_params[instance].function == function) && (_params[instance].pin != -1)) { + return true; + } + } + return false; +} + +#if AP_RELAY_DRONECAN_ENABLED +// Return true if pin number is a virtual DroneCAN pin +bool AP_Relay::DroneCAN::valid_pin(int16_t pin) const +{ + switch(pin) { + case (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0 ... (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_15: + return true; + default: + return false; + } +} + +// Enable streaming of pin number +void AP_Relay::DroneCAN::enable_pin(int16_t pin) +{ + if (!valid_pin(pin)) { + return; + } + + const uint8_t index = hardpoint_index(pin); + state[index].enabled = true; +} + +// Get the hardpoint index of given pin number +uint8_t AP_Relay::DroneCAN::hardpoint_index(const int16_t pin) const +{ + return pin - (int16_t)AP_Relay_Params::VIRTUAL_PINS::DroneCAN_0; +} + +// Set DroneCAN relay state from pin number +void AP_Relay::DroneCAN::set_pin(const int16_t pin, const bool value) +{ + const uint8_t index = hardpoint_index(pin); + + // Set pin and ensure enabled for streaming + state[index].enabled = true; + state[index].value = value; + + // Broadcast msg on all channels + // Just a single send, rely on streaming to fill in any lost packet + + uavcan_equipment_hardpoint_Command msg {}; + msg.hardpoint_id = index; + msg.command = state[index].value; + + uint8_t can_num_drivers = AP::can().get_num_drivers(); + for (uint8_t i = 0; i < can_num_drivers; i++) { + auto *ap_dronecan = AP_DroneCAN::get_dronecan(i); + if (ap_dronecan != nullptr) { + ap_dronecan->relay_hardpoint.broadcast(msg); + } + } + +} + +// Get relay state from pin number, this relies on a cached value, assume remote pin is in sync +bool AP_Relay::DroneCAN::get_pin(const int16_t pin) const +{ + const uint8_t index = hardpoint_index(pin); + return state[index].value; +} + +// Populate message and update index with the sent command +// This will allow the caller to cycle through each enabled pin +bool AP_Relay::DroneCAN::populate_next_command(uint8_t &index, uavcan_equipment_hardpoint_Command &msg) const +{ + // Find the next enabled index + for (uint8_t i = 0; i < ARRAY_SIZE(state); i++) { + // Look for next index, wrapping back to 0 as needed + const uint8_t new_index = (index + 1 + i) % ARRAY_SIZE(state); + if (!state[new_index].enabled) { + // This index is not being used + continue; + } + + // Update command and index then return + msg.hardpoint_id = new_index; + msg.command = state[new_index].value; + index = new_index; + return true; + } + + return false; +} +#endif // AP_RELAY_DRONECAN_ENABLED #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED // this method may only return false if there is no space in the // supplied link for the message. bool AP_Relay::send_relay_status(const GCS_MAVLINK &link) const { + static_assert(AP_RELAY_NUM_RELAYS <= 16, "Too many relays for MAVLink status reporting to work."); + if (!HAVE_PAYLOAD_SPACE(link.get_chan(), RELAY_STATUS)) { return false; } uint16_t present_mask = 0; uint16_t on_mask = 0; - for (auto i=0; i +#include -#define AP_RELAY_NUM_RELAYS 6 +#ifndef AP_RELAY_NUM_RELAYS + #define AP_RELAY_NUM_RELAYS 6 +#endif + +#if AP_RELAY_NUM_RELAYS < 1 + #error There must be at least one relay instance if using AP_Relay +#endif + +#if AP_RELAY_DRONECAN_ENABLED +#include +#endif /// @class AP_Relay /// @brief Class to manage the ArduPilot relay class AP_Relay { +#if AP_RELAY_DRONECAN_ENABLED + // Allow DroneCAN to directly access private DroneCAN state + friend class AP_DroneCAN; +#endif public: AP_Relay(); @@ -29,9 +44,6 @@ class AP_Relay { // setup the relay pin void init(); - // set relay to state - void set(uint8_t instance, bool value); - // activate the relay void on(uint8_t instance) { set(instance, true); } @@ -39,12 +51,10 @@ class AP_Relay { void off(uint8_t instance) { set(instance, false); } // get state of relay - uint8_t get(uint8_t instance) const { - return instance < AP_RELAY_NUM_RELAYS ? _pin_states & (1U< +#include "AP_Relay_Params.h" + +const AP_Param::GroupInfo AP_Relay_Params::var_info[] = { + // @Param: FUNCTION + // @DisplayName: Relay function + // @Description: The function the relay channel is mapped to. + // @Values{Copter, Rover, Plane, Blimp,Sub}: 0:None + // @Values{Copter, Rover, Plane, Blimp,Sub}: 1:Relay + // @Values{Plane}: 2:Ignition + // @Values{Plane, Copter}: 3:Parachute + // @Values{Copter, Rover, Plane, Blimp,Sub}: 4:Camera + // @Values{Rover}: 5:Bushed motor reverse 1 throttle or throttle-left or omni motor 1 + // @Values{Rover}: 6:Bushed motor reverse 2 throttle-right or omni motor 2 + // @Values{Rover}: 7:Bushed motor reverse 3 omni motor 3 + // @Values{Rover}: 8:Bushed motor reverse 4 omni motor 4 + // @Values{Plane}: 9:ICE Starter + // @Values{AP_Periph}: 10: DroneCAN Hardpoint ID 0 + // @Values{AP_Periph}: 11: DroneCAN Hardpoint ID 1 + // @Values{AP_Periph}: 12: DroneCAN Hardpoint ID 2 + // @Values{AP_Periph}: 13: DroneCAN Hardpoint ID 3 + // @Values{AP_Periph}: 14: DroneCAN Hardpoint ID 4 + // @Values{AP_Periph}: 15: DroneCAN Hardpoint ID 5 + // @Values{AP_Periph}: 16: DroneCAN Hardpoint ID 6 + // @Values{AP_Periph}: 17: DroneCAN Hardpoint ID 7 + // @Values{AP_Periph}: 18: DroneCAN Hardpoint ID 8 + // @Values{AP_Periph}: 19: DroneCAN Hardpoint ID 9 + // @Values{AP_Periph}: 20: DroneCAN Hardpoint ID 10 + // @Values{AP_Periph}: 21: DroneCAN Hardpoint ID 11 + // @Values{AP_Periph}: 22: DroneCAN Hardpoint ID 12 + // @Values{AP_Periph}: 23: DroneCAN Hardpoint ID 13 + // @Values{AP_Periph}: 24: DroneCAN Hardpoint ID 14 + // @Values{AP_Periph}: 25: DroneCAN Hardpoint ID 15 + + // @User: Standard + AP_GROUPINFO_FLAGS("FUNCTION", 1, AP_Relay_Params, function, (float)FUNCTION::NONE, AP_PARAM_FLAG_ENABLE), + + // @Param: PIN + // @DisplayName: Relay pin + // @Description: Digital pin number for relay control. Some common values are given, but see the Wiki's "GPIOs" page for how to determine the pin number for a given autopilot. + // @Values: -1:Disabled,49:BB Blue GP0 pin 4,50:AUXOUT1,51:AUXOUT2,52:AUXOUT3,53:AUXOUT4,54:AUXOUT5,55:AUXOUT6,57:BB Blue GP0 pin 3,113:BB Blue GP0 pin 6,116:BB Blue GP0 pin 5,62:BBBMini Pin P8.13,101:MainOut1,102:MainOut2,103:MainOut3,104:MainOut4,105:MainOut5,106:MainOut6,107:MainOut7,108:MainOut8 + // @Values: 1000: DroneCAN Hardpoint ID 0 + // @Values: 1001: DroneCAN Hardpoint ID 1 + // @Values: 1002: DroneCAN Hardpoint ID 2 + // @Values: 1003: DroneCAN Hardpoint ID 3 + // @Values: 1004: DroneCAN Hardpoint ID 4 + // @Values: 1005: DroneCAN Hardpoint ID 5 + // @Values: 1006: DroneCAN Hardpoint ID 6 + // @Values: 1007: DroneCAN Hardpoint ID 7 + // @Values: 1008: DroneCAN Hardpoint ID 8 + // @Values: 1009: DroneCAN Hardpoint ID 9 + // @Values: 1010: DroneCAN Hardpoint ID 10 + // @Values: 1011: DroneCAN Hardpoint ID 11 + // @Values: 1012: DroneCAN Hardpoint ID 12 + // @Values: 1013: DroneCAN Hardpoint ID 13 + // @Values: 1014: DroneCAN Hardpoint ID 14 + // @Values: 1015: DroneCAN Hardpoint ID 15 + // @User: Standard + AP_GROUPINFO("PIN", 2, AP_Relay_Params, pin, -1), + + // @Param: DEFAULT + // @DisplayName: Relay default state + // @Description: Should the relay default to on or off, this only applies to RELAYx_FUNC "Relay" (1). All other uses will pick the appropriate default output state from within the controlling function's parameters. + // @Values: 0: Off,1:On,2:NoChange + // @User: Standard + AP_GROUPINFO("DEFAULT", 3, AP_Relay_Params, default_state, (float)DefaultState::OFF), + + AP_GROUPEND + +}; + +AP_Relay_Params::AP_Relay_Params(void) { + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_Relay/AP_Relay_Params.h b/libraries/AP_Relay/AP_Relay_Params.h new file mode 100644 index 00000000000000..9b6a9d1cc5d8d0 --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_Params.h @@ -0,0 +1,73 @@ +#pragma once + +#include + +class AP_Relay_Params { +public: + static const struct AP_Param::GroupInfo var_info[]; + + AP_Relay_Params(void); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Relay_Params); + + enum class DefaultState : uint8_t { + OFF = 0, + ON = 1, + NO_CHANGE = 2, + }; + + enum class FUNCTION : uint8_t { + NONE = 0, + RELAY = 1, + IGNITION = 2, + PARACHUTE = 3, + CAMERA = 4, + BRUSHED_REVERSE_1 = 5, + BRUSHED_REVERSE_2 = 6, + BRUSHED_REVERSE_3 = 7, + BRUSHED_REVERSE_4 = 8, + ICE_STARTER = 9, + DroneCAN_HARDPOINT_0 = 10, + DroneCAN_HARDPOINT_1 = 11, + DroneCAN_HARDPOINT_2 = 12, + DroneCAN_HARDPOINT_3 = 13, + DroneCAN_HARDPOINT_4 = 14, + DroneCAN_HARDPOINT_5 = 15, + DroneCAN_HARDPOINT_6 = 16, + DroneCAN_HARDPOINT_7 = 17, + DroneCAN_HARDPOINT_8 = 18, + DroneCAN_HARDPOINT_9 = 19, + DroneCAN_HARDPOINT_10 = 20, + DroneCAN_HARDPOINT_11 = 21, + DroneCAN_HARDPOINT_12 = 22, + DroneCAN_HARDPOINT_13 = 23, + DroneCAN_HARDPOINT_14 = 24, + DroneCAN_HARDPOINT_15 = 25, + NUM_FUNCTIONS // must be the last entry + }; + + // Pins that do not go via GPIO + enum class VIRTUAL_PINS { + DroneCAN_0 = 1000, + DroneCAN_1 = 1001, + DroneCAN_2 = 1002, + DroneCAN_3 = 1003, + DroneCAN_4 = 1004, + DroneCAN_5 = 1005, + DroneCAN_6 = 1006, + DroneCAN_7 = 1007, + DroneCAN_8 = 1008, + DroneCAN_9 = 1009, + DroneCAN_10 = 1010, + DroneCAN_11 = 1011, + DroneCAN_12 = 1012, + DroneCAN_13 = 1013, + DroneCAN_14 = 1014, + DroneCAN_15 = 1015, + }; + + AP_Enum function; // relay function + AP_Int16 pin; // gpio pin number + AP_Enum default_state; // default state +}; diff --git a/libraries/AP_Relay/AP_Relay_config.h b/libraries/AP_Relay/AP_Relay_config.h index efc3b918f4e297..946be54dfe1460 100644 --- a/libraries/AP_Relay/AP_Relay_config.h +++ b/libraries/AP_Relay/AP_Relay_config.h @@ -5,3 +5,5 @@ #ifndef AP_RELAY_ENABLED #define AP_RELAY_ENABLED 1 #endif + +#define AP_RELAY_DRONECAN_ENABLED AP_RELAY_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS && !defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp index abfbf2fec5614f..5fd91703b114b3 100644 --- a/libraries/AP_Scheduler/AP_Scheduler.cpp +++ b/libraries/AP_Scheduler/AP_Scheduler.cpp @@ -31,6 +31,7 @@ #include #include #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include @@ -113,10 +114,12 @@ void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint _vehicle_tasks = tasks; _num_vehicle_tasks = num_tasks; +#if AP_VEHICLE_ENABLED AP_Vehicle* vehicle = AP::vehicle(); if (vehicle != nullptr) { vehicle->get_common_scheduler_tasks(_common_tasks, _num_common_tasks); } +#endif _num_tasks = _num_vehicle_tasks + _num_common_tasks; @@ -417,6 +420,7 @@ void AP_Scheduler::loop() #endif } +#if HAL_LOGGING_ENABLED void AP_Scheduler::update_logging() { if (debug_flags()) { @@ -459,6 +463,7 @@ void AP_Scheduler::Log_Write_Performance() }; AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); } +#endif // HAL_LOGGING_ENABLED // display task statistics as text buffer for @SYS/tasks.txt void AP_Scheduler::task_info(ExpandingString &str) diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 347a7d440fd2d2..86dab106f3fe09 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -53,6 +53,10 @@ static_assert(SCRIPTING_STACK_SIZE <= SCRIPTING_STACK_MAX_SIZE, "Scripting requi #define SCRIPTING_ENABLE_DEFAULT 0 #endif +#if AP_NETWORKING_ENABLED +#include +#endif + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_Scripting::var_info[] = { @@ -149,6 +153,14 @@ const AP_Param::GroupInfo AP_Scripting::var_info[] = { // @User: Advanced AP_GROUPINFO("RUN_CHECKSUM", 13, AP_Scripting, _required_running_checksum, -1), + // @Param: THD_PRIORITY + // @DisplayName: Scripting thread priority + // @Description: This sets the priority of the scripting thread. This is normally set to a low priority to prevent scripts from interfering with other parts of the system. Advanced users can change this priority if scripting needs to be prioritised for realtime applications. WARNING: changing this parameter can impact the stability of your flight controller. The scipting thread priority in this parameter is chosen based on a set of system level priorities for other subsystems. It is strongly recommended that you use the lowest priority that is sufficient for your application. Note that all scripts run at the same priority, so if you raise this priority you must carefully audit all lua scripts for behaviour that does not interfere with the operation of the system. + // @Values: 0:Normal, 1:IO Priority, 2:Storage Priority, 3:UART Priority, 4:I2C Priority, 5:SPI Priority, 6:Timer Priority, 7:Main Priority, 8:Boost Priority + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("THD_PRIORITY", 14, AP_Scripting, _thd_priority, uint8_t(ThreadPriority::NORMAL)), + AP_GROUPEND }; @@ -175,8 +187,29 @@ void AP_Scripting::init(void) { } } + AP_HAL::Scheduler::priority_base priority = AP_HAL::Scheduler::PRIORITY_SCRIPTING; + static const struct { + ThreadPriority scr_priority; + AP_HAL::Scheduler::priority_base hal_priority; + } priority_map[] = { + { ThreadPriority::NORMAL, AP_HAL::Scheduler::PRIORITY_SCRIPTING }, + { ThreadPriority::IO, AP_HAL::Scheduler::PRIORITY_IO }, + { ThreadPriority::STORAGE, AP_HAL::Scheduler::PRIORITY_STORAGE }, + { ThreadPriority::UART, AP_HAL::Scheduler::PRIORITY_UART }, + { ThreadPriority::I2C, AP_HAL::Scheduler::PRIORITY_I2C }, + { ThreadPriority::SPI, AP_HAL::Scheduler::PRIORITY_SPI }, + { ThreadPriority::TIMER, AP_HAL::Scheduler::PRIORITY_TIMER }, + { ThreadPriority::MAIN, AP_HAL::Scheduler::PRIORITY_MAIN }, + { ThreadPriority::BOOST, AP_HAL::Scheduler::PRIORITY_BOOST }, + }; + for (const auto &p : priority_map) { + if (p.scr_priority == _thd_priority) { + priority = p.hal_priority; + } + } + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Scripting::thread, void), - "Scripting", SCRIPTING_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_SCRIPTING, 0)) { + "Scripting", SCRIPTING_STACK_SIZE, priority, 0)) { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Scripting: %s", "failed to start"); _thread_failed = true; } @@ -287,6 +320,16 @@ void AP_Scripting::thread(void) { } num_pwm_source = 0; +#if AP_NETWORKING_ENABLED + // clear allocated sockets + for (uint8_t i=0; i #include #include "AP_Scripting_CANSensor.h" +#include #ifndef SCRIPTING_MAX_NUM_I2C_DEVICE #define SCRIPTING_MAX_NUM_I2C_DEVICE 4 @@ -31,6 +32,13 @@ #define SCRIPTING_MAX_NUM_PWM_SOURCE 4 +#if AP_NETWORKING_ENABLED +#ifndef SCRIPTING_MAX_NUM_NET_SOCKET +#define SCRIPTING_MAX_NUM_NET_SOCKET 50 +#endif +class SocketAPM; +#endif + class AP_Scripting { public: @@ -111,6 +119,11 @@ class AP_Scripting int get_current_ref() { return current_ref; } void set_current_ref(int ref) { current_ref = ref; } +#if AP_NETWORKING_ENABLED + // SocketAPM storage + SocketAPM *_net_sockets[SCRIPTING_MAX_NUM_NET_SOCKET]; +#endif + struct mavlink_msg { mavlink_message_t msg; mavlink_channel_t chan; @@ -145,6 +158,18 @@ class AP_Scripting // The full range of uint32 integers cannot be represented by a float. const uint32_t checksum_param_mask = 0x007FFFFF; + enum class ThreadPriority : uint8_t { + NORMAL = 0, + IO = 1, + STORAGE = 2, + UART = 3, + I2C = 4, + SPI = 5, + TIMER = 6, + MAIN = 7, + BOOST = 8 + }; + AP_Int8 _enable; AP_Int32 _script_vm_exec_count; AP_Int32 _script_heap_size; @@ -153,6 +178,7 @@ class AP_Scripting AP_Int32 _required_loaded_checksum; AP_Int32 _required_running_checksum; + AP_Enum _thd_priority; bool _thread_failed; // thread allocation failed bool _init_failed; // true if memory allocation failed diff --git a/libraries/AP_Scripting/AP_Scripting_CANSensor.cpp b/libraries/AP_Scripting/AP_Scripting_CANSensor.cpp index d3b0ab23756535..39f1bf9f870a09 100644 --- a/libraries/AP_Scripting/AP_Scripting_CANSensor.cpp +++ b/libraries/AP_Scripting/AP_Scripting_CANSensor.cpp @@ -16,6 +16,7 @@ Scripting CANSensor class, for easy scripting CAN support */ #include "AP_Scripting_CANSensor.h" +#include #if AP_SCRIPTING_CAN_SENSOR_ENABLED @@ -62,8 +63,25 @@ bool ScriptingCANBuffer::read_frame(AP_HAL::CANFrame &frame) // recursively add frame to buffer void ScriptingCANBuffer::handle_frame(AP_HAL::CANFrame &frame) { + // accept everything if no filters are setup + bool accept = num_filters == 0; + + // Check if frame matches any filters + for (uint8_t i = 0; i < MIN(num_filters, ARRAY_SIZE(filter)); i++) { + if ((frame.id & filter[i].mask) == filter[i].value) { + accept = true; + break; + } + } + WITH_SEMAPHORE(sem); - buffer.push(frame); + + // Add to buffer for scripting to read + if (accept) { + buffer.push(frame); + } + + // filtering is not applied to other buffers if (next != nullptr) { next->handle_frame(frame); } @@ -79,4 +97,19 @@ void ScriptingCANBuffer::add_buffer(ScriptingCANBuffer* new_buff) { next->add_buffer(new_buff); } +// Add a filter, will pass ID's that match value given the mask +bool ScriptingCANBuffer::add_filter(uint32_t mask, uint32_t value) { + + // Run out of filters + if (num_filters >= ARRAY_SIZE(filter)) { + return false; + } + + // Add to list and increment + filter[num_filters].mask = mask; + filter[num_filters].value = value & mask; + num_filters++; + return true; +} + #endif // AP_SCRIPTING_CAN_SENSOR_ENABLED diff --git a/libraries/AP_Scripting/AP_Scripting_CANSensor.h b/libraries/AP_Scripting/AP_Scripting_CANSensor.h index b6f19d28d83e7b..962aef8fbfa5f2 100644 --- a/libraries/AP_Scripting/AP_Scripting_CANSensor.h +++ b/libraries/AP_Scripting/AP_Scripting_CANSensor.h @@ -74,6 +74,9 @@ class ScriptingCANBuffer { // recursively add new buffer void add_buffer(ScriptingCANBuffer* new_buff); + // Add a filter to this buffer + bool add_filter(uint32_t mask, uint32_t value); + private: ObjectBuffer buffer; @@ -84,6 +87,12 @@ class ScriptingCANBuffer { HAL_Semaphore sem; + struct { + uint32_t mask; + uint32_t value; + } filter[8]; + uint8_t num_filters; + }; #endif // AP_SCRIPTING_CAN_SENSOR_ENABLED diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua index 7c1c7362b98dbe..c0b674e5ae8508 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/RateBased/sport_aerobatics.lua @@ -28,7 +28,6 @@ local THR_PIT_FF = bind_add_param("PIT_FF",4,80) -- throttle FF from pitch local SPD_P = bind_add_param("SPD_P",5,5) -- speed P gain local SPD_I = bind_add_param("SPD_I",6,25) -- speed I gain local TRIM_THROTTLE = Parameter("TRIM_THROTTLE") -local TRIM_ARSPD_CM = Parameter("TRIM_ARSPD_CM") local RLL2SRV_TCONST = Parameter("RLL2SRV_TCONST") local PITCH_TCONST = Parameter("PTCH2SRV_TCONST") diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_plane.sh b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_plane.sh index 64704f9cc70231..fa67876008e8e8 100755 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_plane.sh +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/dual_plane.sh @@ -15,12 +15,12 @@ LOC2="-35.36274593,149.16516256,585,353.8" } # setup multicast -#UARTA="tcp:0" -UARTA="mcast:" +#SERIAL0="tcp:0" +SERIAL0="mcast:" PLANE_DEFAULTS="$ROOTDIR/Tools/autotest/models/plane-3d.parm" -(cd ArduPlane/AeroBatics1 && $PLANE --instance 1 --home $LOC1 --model plane-3d --uartA $UARTA --defaults $PLANE_DEFAULTS) & -(cd ArduPlane/AeroBatics2 && $PLANE --instance 2 --home $LOC2 --model plane-3d --uartA $UARTA --defaults $PLANE_DEFAULTS) & +(cd ArduPlane/AeroBatics1 && $PLANE --instance 1 --home $LOC1 --model plane-3d --serial0 $SERIAL0 --defaults $PLANE_DEFAULTS) & +(cd ArduPlane/AeroBatics2 && $PLANE --instance 2 --home $LOC2 --model plane-3d --serial0 $SERIAL0 --defaults $PLANE_DEFAULTS) & wait diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index e7928c8c0bbcd9..72112bf30c5d81 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -276,7 +276,7 @@ end ACRO_ROLL_RATE = Parameter("ACRO_ROLL_RATE") ACRO_YAW_RATE = Parameter('ACRO_YAW_RATE') -ARSPD_FBW_MIN = Parameter("ARSPD_FBW_MIN") +AIRSPEED_MIN = Parameter("AIRSPEED_MIN") SCALING_SPEED = Parameter("SCALING_SPEED") SYSID_THISMAV = Parameter("SYSID_THISMAV") @@ -385,7 +385,7 @@ local DO_JUMP = 177 local k_throttle = 70 local NAME_FLOAT_RATE = 2 -local TRIM_ARSPD_CM = Parameter("TRIM_ARSPD_CM") +local AIRSPEED_CRUISE = Parameter("AIRSPEED_CRUISE") local last_id = 0 local current_task = nil @@ -2010,7 +2010,7 @@ end velocity at the start of the maneuver --]] function target_groundspeed() - return math.max(ahrs:get_EAS2TAS()*TRIM_ARSPD_CM:get()*0.01, ahrs:get_velocity_NED():length()) + return math.max(ahrs:get_EAS2TAS()*AIRSPEED_CRUISE:get(), ahrs:get_velocity_NED():length()) end --[[ @@ -2465,7 +2465,7 @@ function do_path() end -- airspeed, assume we don't go below min - local airspeed_constrained = math.max(ARSPD_FBW_MIN:get(), ahrs_airspeed) + local airspeed_constrained = math.max(AIRSPEED_MIN:get(), ahrs_airspeed) --[[ calculate positions and angles at previous, current and next time steps diff --git a/libraries/AP_Scripting/applets/Heli_idle_control.lua b/libraries/AP_Scripting/applets/Heli_idle_control.lua new file mode 100644 index 00000000000000..16e7de0a0056bf --- /dev/null +++ b/libraries/AP_Scripting/applets/Heli_idle_control.lua @@ -0,0 +1,212 @@ +-- idle_control.lua: a closed loop control throttle control while on ground idle (trad-heli) + +local PARAM_TABLE_KEY = 73 +local PARAM_TABLE_PREFIX = 'IDLE_' +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 30), 'could not add param table') + +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 + +-- parameters for idle control + +IDLE_GAIN_I = bind_add_param('GAIN_I', 1, 0.05) +IDLE_GAIN_P = bind_add_param('GAIN_P', 2, 0.25) +IDLE_GAIN_MAX = bind_add_param('GAIN_MAX', 3, 1) +IDLE_MAX = bind_add_param('MAX', 4, 17) +IDLE_RANGE = bind_add_param('RANGE', 5, 300) +IDLE_SETPOINT = bind_add_param('SETPOINT', 6, 600) +IDLE_RPM_ENABLE = bind_add_param('RPM_ENABLE', 7, 0) + +-- internal variables + +local thr_out = nil +local idle_control_active = false +local idle_control_active_last = false +local last_scaled_output = nil +local idle_default = nil +local ramp_up_complete = false +local idle_adjusted = false +local last_idc_time = nil +local time_now = nil +local pv = nil +local thr_ctl = nil +local thr_out_last = nil +local pot_input = rc:find_channel_for_option(301) +local switch_rsc = rc:find_channel_for_option(32) +local rsc_output = SRV_Channels:find_channel(31) +local SERVO_MAX = Parameter('SERVO' .. (rsc_output+1) .. '_MAX') +local SERVO_MIN = Parameter('SERVO' .. (rsc_output+1) .. '_MIN') +local SERVO_REV = Parameter('SERVO' .. (rsc_output+1) .. '_REVERSED') +local servo_range = SERVO_MAX:get() - SERVO_MIN:get() +local H_RSC_IDLE = Parameter('H_RSC_IDLE') +local H_RSC_RUNUP_TIME = Parameter('H_RSC_RUNUP_TIME') + +-- map function + +function map(x, in_min, in_max, out_min, out_max) + return out_min + (x - in_min)*(out_max - out_min)/(in_max - in_min) +end + +-- constrain function + +local function constrain(v, vmin, vmax) + if v < vmin then + v = vmin + end + if v > vmax then + v = vmax + end + return v +end + +-- PI controller function +local function PI_controller(kP,kI,iMax,min,max) + + local self = {} + + local _kP = kP + local _kI = kI + local _iMax = iMax + local _min = min + local _max = max + local _last_t = nil + local _I = 0 + local _total = 0 + local _counter = 0 + + function self.update(target, current) + local now = millis() + if not _last_t then + _last_t = now + end + local dt = (now - _last_t):tofloat()*0.001 + _last_t = now + local err = target - current + _counter = _counter + 1 + local P = _kP:get() * err + if ((_total < _max and _total > _min) or (_total >= _max and err < 0) or (_total <= _min and err > 0)) then + _I = _I + _kI:get() * err * dt + end + if _iMax:get() > 0 then + _I = constrain(_I, -_iMax:get(), iMax:get()) + end + local ret = P + _I + _total = ret + return ret + end + + function self.reset(integrator) + _I = integrator + end + + return self +end + +local thr_PI = PI_controller(IDLE_GAIN_P, IDLE_GAIN_I, IDLE_GAIN_MAX, 0, 1) + +-- main update function + +function update() + + local armed = arming:is_armed() + + -- aux potentiometer for manual adjusting of idle + + local pot_pos = pot_input:norm_input() + local thr_man = map(pot_pos,-1,1,0,1) + + if armed == false then + idle_default = H_RSC_IDLE:get() + idle_control_active = false + ramp_up_complete = false + idle_adjusted = false + thr_PI.reset(0) + else + if switch_rsc:get_aux_switch_pos() == 0 and vehicle:get_likely_flying() == false then + if H_RSC_IDLE:get()~= idle_default then + H_RSC_IDLE:set(idle_default) + gcs:send_text(5, "H_RSC_IDLE set to:".. tostring(H_RSC_IDLE:get())) + end + if IDLE_RPM_ENABLE:get() == 0 then + ramp_up_complete = false + idle_adjusted = false + thr_out = H_RSC_IDLE:get() + thr_man*(IDLE_MAX:get() - H_RSC_IDLE:get()) + else + if thr_man == 0 then + idle_control_active = false + if idle_control_active_last ~= idle_control_active then + gcs:send_text(5, "idle control: OFF") + end + thr_out = H_RSC_IDLE:get() + thr_ctl = 0 + thr_PI.reset(0) + else + local rpm_current = RPM:get_rpm((IDLE_RPM_ENABLE:get())-1) + ramp_up_complete = false + idle_adjusted = false + if rpm_current < (IDLE_SETPOINT:get() - IDLE_RANGE:get()) then + thr_out = H_RSC_IDLE:get() + thr_man*(IDLE_MAX:get() - H_RSC_IDLE:get()) + thr_out_last = thr_out + elseif rpm_current > (IDLE_SETPOINT:get() + IDLE_RANGE:get()) then + thr_out = H_RSC_IDLE:get() + thr_out_last = thr_out + else + -- throttle output set from the PI controller + pv = rpm_current/(IDLE_SETPOINT:get()) + thr_ctl = thr_PI.update(1, pv) + thr_ctl = constrain(thr_ctl,0,1) + thr_out = H_RSC_IDLE:get() + thr_ctl*(IDLE_MAX:get() - H_RSC_IDLE:get()) + if thr_out_last == nil then + thr_out_last = 0 + end + thr_out = constrain(thr_out, thr_out_last-0.05, thr_out_last+0.05) + thr_out_last = thr_out + idle_control_active = true + end + if idle_control_active_last ~= idle_control_active then + gcs:send_text(5, "idle control: ON") + end + end + end + last_idc_time = millis() + last_scaled_output = thr_out/100 + if SERVO_REV:get() == 0 then + SRV_Channels:set_output_pwm_chan_timeout(rsc_output, math.floor((last_scaled_output*servo_range)+SERVO_MIN:get()), 150) + else + SRV_Channels:set_output_pwm_chan_timeout(rsc_output, math.floor(SERVO_MAX:get()-(last_scaled_output*servo_range)), 150) + end + else + -- motor interlock disabled, armed state, flight + idle_control_active = false + if idle_control_active_last ~= idle_control_active then + gcs:send_text(5, "idle control: deactivated") + end + if ramp_up_complete ~= true then + time_now = millis() + if ((time_now-last_idc_time):tofloat()*0.001) < H_RSC_RUNUP_TIME:get() then + if idle_adjusted ~= true then + H_RSC_IDLE:set(last_scaled_output*100) + idle_adjusted = true + gcs:send_text(5, "H_RSC_IDLE updated for ramp up:".. tostring(H_RSC_IDLE:get())) + end + else + if H_RSC_IDLE:get()~= idle_default then + H_RSC_IDLE:set(idle_default) + gcs:send_text(5, "H_RSC_IDLE default restored:".. tostring(H_RSC_IDLE:get())) + end + ramp_up_complete = true + end + end + end + -- update notify variable + idle_control_active_last = idle_control_active + end + + return update, 100 -- 10Hz rate +end + +gcs:send_text(5, "idle_control_running") + +return update() diff --git a/libraries/AP_Scripting/applets/Heli_idle_control.md b/libraries/AP_Scripting/applets/Heli_idle_control.md new file mode 100644 index 00000000000000..39fb88684e0207 --- /dev/null +++ b/libraries/AP_Scripting/applets/Heli_idle_control.md @@ -0,0 +1,17 @@ +# Idle Control + +Allows manual or automatic rpm control for heli while on ground idle condition + +# Parameters + +IDLE_GAIN_I = integrative gain of the controller +IDLE_GAIN_P = proportional gain of the controller +IDLE_GAIN_MAX = IMAX for integrative +IDLE_MAX = maximum throttle position, shall be set low enough to prevent clutch engagement/slipping or lower than first point of throttle curve +IDLE_RANGE = rpm range of operation for the idle control +IDLE_SETPOINT = desired rpm setpoint +IDLE_RPM_ENABLE = 0 for manual throttle control // 1 for RPM1 targeting // 2 for RPM2 targeting + +# How To Use + +set RCx_OPTION to 301 to enable idle control from an auxiliary potentiometer diff --git a/libraries/AP_Scripting/applets/WebExamples/test.lua b/libraries/AP_Scripting/applets/WebExamples/test.lua new file mode 100644 index 00000000000000..97a5cc4a9e086c --- /dev/null +++ b/libraries/AP_Scripting/applets/WebExamples/test.lua @@ -0,0 +1,9 @@ +--[[ +example lua cgi file for cgi-bin/ folder +--]] +return [[ +test-from-cgi + +working!! +]] + diff --git a/libraries/AP_Scripting/applets/WebExamples/test.shtml b/libraries/AP_Scripting/applets/WebExamples/test.shtml new file mode 100644 index 00000000000000..96546a427fe521 --- /dev/null +++ b/libraries/AP_Scripting/applets/WebExamples/test.shtml @@ -0,0 +1,16 @@ + + + + + +

    Server Side Scripting Test

    + + + + + + + +
    RollPitchYaw
    + + diff --git a/libraries/AP_Scripting/applets/net_webserver.lua b/libraries/AP_Scripting/applets/net_webserver.lua new file mode 100644 index 00000000000000..cccbc6eb0c36fc --- /dev/null +++ b/libraries/AP_Scripting/applets/net_webserver.lua @@ -0,0 +1,967 @@ +--[[ + example script to test lua socket API +--]] + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +PARAM_TABLE_KEY = 47 +PARAM_TABLE_PREFIX = "WEB_" + +-- 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 Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'net_test: could not add param table') + +--[[ + // @Param: WEB_ENABLE + // @DisplayName: enable web server + // @Description: enable web server + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local WEB_ENABLE = bind_add_param('ENABLE', 1, 1) + +--[[ + // @Param: WEB_BIND_PORT + // @DisplayName: web server TCP port + // @Description: web server TCP port + // @Range: 1 65535 + // @User: Standard +--]] +local WEB_BIND_PORT = bind_add_param('BIND_PORT', 2, 8080) + +--[[ + // @Param: WEB_DEBUG + // @DisplayName: web server debugging + // @Description: web server debugging + // @Values: 0:Disabled,1:Enabled + // @User: Advanced +--]] +local WEB_DEBUG = bind_add_param('DEBUG', 3, 0) + +--[[ + // @Param: WEB_BLOCK_SIZE + // @DisplayName: web server block size + // @Description: web server block size for download + // @Range: 1 65535 + // @User: Advanced +--]] +local WEB_BLOCK_SIZE = bind_add_param('BLOCK_SIZE', 4, 10240) + +--[[ + // @Param: WEB_TIMEOUT + // @DisplayName: web server timeout + // @Description: timeout for inactive connections + // @Units: s + // @Range: 0.1 60 + // @User: Advanced +--]] +local WEB_TIMEOUT = bind_add_param('TIMEOUT', 5, 2.0) + +--[[ + // @Param: WEB_SENDFILE_MIN + // @DisplayName: web server minimum file size for sendfile + // @Description: sendfile is an offloading mechanism for faster file download. If this is non-zero and the file is larger than this size then sendfile will be used for file download + // @Range: 0 10000000 + // @User: Advanced +--]] +local WEB_SENDFILE_MIN = bind_add_param('SENDFILE_MIN', 6, 100000) + +if WEB_ENABLE:get() ~= 1 then + gcs:send_text(MAV_SEVERITY.INFO, "WebServer: disabled") + return +end + +local BRD_RTC_TZ_MIN = Parameter("BRD_RTC_TZ_MIN") + +gcs:send_text(MAV_SEVERITY.INFO, string.format("WebServer: starting on port %u", WEB_BIND_PORT:get())) + +local sock_listen = Socket(0) +local clients = {} + +local DOCTYPE = "" +local SERVER_VERSION = "net_webserver 1.0" +local CONTENT_TEXT_HTML = "text/html;charset=UTF-8" +local CONTENT_OCTET_STREAM = "application/octet-stream" + +local HIDDEN_FOLDERS = { "@SYS", "@ROMFS", "@MISSION", "@PARAM" } + +local MNT_PREFIX = "/mnt" +local MNT_PREFIX2 = MNT_PREFIX .. "/" + +local MIME_TYPES = { + ["apj"] = CONTENT_OCTET_STREAM, + ["dat"] = CONTENT_OCTET_STREAM, + ["o"] = CONTENT_OCTET_STREAM, + ["obj"] = CONTENT_OCTET_STREAM, + ["lua"] = "text/x-lua", + ["py"] = "text/x-python", + ["shtml"] = CONTENT_TEXT_HTML, + ["js"] = "text/javascript", + -- thanks to https://developer.mozilla.org/en-US/docs/Web/HTTP/Basics_of_HTTP/MIME_types/Common_types + ["aac"] = "audio/aac", + ["abw"] = "application/x-abiword", + ["arc"] = "application/x-freearc", + ["avif"] = "image/avif", + ["avi"] = "video/x-msvideo", + ["azw"] = "application/vnd.amazon.ebook", + ["bin"] = "application/octet-stream", + ["bmp"] = "image/bmp", + ["bz"] = "application/x-bzip", + ["bz2"] = "application/x-bzip2", + ["cda"] = "application/x-cdf", + ["csh"] = "application/x-csh", + ["css"] = "text/css", + ["csv"] = "text/csv", + ["doc"] = "application/msword", + ["docx"] = "application/vnd.openxmlformats-officedocument.wordprocessingml.document", + ["eot"] = "application/vnd.ms-fontobject", + ["epub"] = "application/epub+zip", + ["gz"] = "application/gzip", + ["gif"] = "image/gif", + ["htm"] = CONTENT_TEXT_HTML, + ["html"] = CONTENT_TEXT_HTML, + ["ico"] = "image/vnd.microsoft.icon", + ["ics"] = "text/calendar", + ["jar"] = "application/java-archive", + ["jpeg"] = "image/jpeg", + ["json"] = "application/json", + ["jsonld"] = "application/ld+json", + ["mid"] = "audio/x-midi", + ["mjs"] = "text/javascript", + ["mp3"] = "audio/mpeg", + ["mp4"] = "video/mp4", + ["mpeg"] = "video/mpeg", + ["mpkg"] = "application/vnd.apple.installer+xml", + ["odp"] = "application/vnd.oasis.opendocument.presentation", + ["ods"] = "application/vnd.oasis.opendocument.spreadsheet", + ["odt"] = "application/vnd.oasis.opendocument.text", + ["oga"] = "audio/ogg", + ["ogv"] = "video/ogg", + ["ogx"] = "application/ogg", + ["opus"] = "audio/opus", + ["otf"] = "font/otf", + ["png"] = "image/png", + ["pdf"] = "application/pdf", + ["php"] = "application/x-httpd-php", + ["ppt"] = "application/vnd.ms-powerpoint", + ["pptx"] = "application/vnd.openxmlformats-officedocument.presentationml.presentation", + ["rar"] = "application/vnd.rar", + ["rtf"] = "application/rtf", + ["sh"] = "application/x-sh", + ["svg"] = "image/svg+xml", + ["tar"] = "application/x-tar", + ["tif"] = "image/tiff", + ["tiff"] = "image/tiff", + ["ts"] = "video/mp2t", + ["ttf"] = "font/ttf", + ["txt"] = "text/plain", + ["vsd"] = "application/vnd.visio", + ["wav"] = "audio/wav", + ["weba"] = "audio/webm", + ["webm"] = "video/webm", + ["webp"] = "image/webp", + ["woff"] = "font/woff", + ["woff2"] = "font/woff2", + ["xhtml"] = "application/xhtml+xml", + ["xls"] = "application/vnd.ms-excel", + ["xlsx"] = "application/vnd.openxmlformats-officedocument.spreadsheetml.sheet", + ["xml"] = "default.", + ["xul"] = "application/vnd.mozilla.xul+xml", + ["zip"] = "application/zip", + ["3gp"] = "video", + ["3g2"] = "video", + ["7z"] = "application/x-7z-compressed", +} + +--[[ + builtin dynamic pages +--]] +local DYNAMIC_PAGES = { + +-- main home page +["/"] = [[ + + + + + + ArduPilot + + + +

    ArduPilot Web Server

    + + +
    + +
    +

    Controller Status

    +
    + + +]], + +-- board status section on front page +["@DYNAMIC/board_status.shtml"] = [[ + + + + + + + +
    Firmware
    GIT Hash
    Uptime
    Arm Status
    AHRS Location
    GPS Location
    +]] +} + +--[[ + builtin javascript library functions +--]] +JS_LIBRARY = { + ["dynamic_load"] = [[ + function dynamic_load(div_id, uri, period_ms) { + var xhr = new XMLHttpRequest(); + xhr.open('GET', uri); + + xhr.setRequestHeader("Cache-Control", "no-cache, no-store, max-age=0"); + xhr.setRequestHeader("Expires", "Tue, 01 Jan 1980 1:00:00 GMT"); + xhr.setRequestHeader("Pragma", "no-cache"); + + xhr.onload = function () { + if (xhr.status === 200) { + var output = document.getElementById(div_id); + if (uri.endsWith('.shtml') || uri.endsWith('.html')) { + output.innerHTML = xhr.responseText; + } else { + output.textContent = xhr.responseText; + } + } + setTimeout(function() { dynamic_load(div_id,uri, period_ms); }, period_ms); + } + xhr.send(); + } +]] +} + +if not sock_listen:bind("0.0.0.0", WEB_BIND_PORT:get()) then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("WebServer: failed to bind to TCP %u", WEB_BIND_PORT:get())) + return +end + +if not sock_listen:listen(20) then + gcs:send_text(MAV_SEVERITY.ERROR, "WebServer: failed to listen") + return +end + +function hms_uptime() + local s = (millis()/1000):toint() + local min = math.floor(s / 60) % 60 + local hr = math.floor(s / 3600) + return string.format("%u hours %u minutes %u seconds", hr, min, s%60) +end + +--[[ + split string by pattern +--]] +local function split(str, pattern) + local ret = {} + for s in string.gmatch(str, pattern) do + table.insert(ret, s) + end + return ret +end + +--[[ + return true if a string ends in the 2nd string +--]] +local function endswith(str, s) + local len1 = #str + local len2 = #s + return string.sub(str,1+len1-len2,len1) == s +end + +--[[ + return true if a string starts with the 2nd string +--]] +local function startswith(str, s) + return string.sub(str,1,#s) == s +end + +local debug_count=0 + +function DEBUG(txt) + if WEB_DEBUG:get() ~= 0 then + gcs:send_text(MAV_SEVERITY.DEBUG, txt .. string.format(" [%u]", debug_count)) + debug_count = debug_count + 1 + end +end + +--[[ + return index of element in a table +--]] +function table_index(t,el) + for i,v in ipairs(t) do + if v == el then + return i + end + end + return nil +end + +--[[ + return true if a table contains a given element +--]] +function table_contains(t,el) + local i = table_index(t, el) + return i ~= nil +end + +function is_hidden_dir(path) + return table_contains(HIDDEN_FOLDERS, path) +end + +local DAYS = { "Sun", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat" } +local MONTHS = { "Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec" } + +function isdirectory(path) + local s = fs:stat(path) + return s and s:is_directory() +end + +--[[ + time string for directory listings +--]] +function file_timestring(path) + local s = fs:stat(path) + if not s then + return "" + end + local mtime = s:mtime() + mtime = mtime + BRD_RTC_TZ_MIN:get()*60 + local year, month, day, hour, min, sec, _ = rtc:clock_s_to_date_fields(mtime) + if not year then + return "" + end + return string.format("%04u-%02u-%02u %02u:%02u", year, month+1, day, hour, min, sec) +end + +--[[ + time string for Last-Modified +--]] +function file_timestring_http(mtime) + local year, month, day, hour, min, sec, wday = rtc:clock_s_to_date_fields(mtime) + if not year then + return "" + end + return string.format("%s, %02u %s %u %02u:%02u:%02u GMT", + DAYS[wday+1], + day, + MONTHS[month+1], + year, + hour, + min, + sec) +end + +--[[ + parse a http time string to a uint32_t seconds timestamp +--]] +function file_timestring_http_parse(tstring) + local dayname, day, monthname, year, hour, min, sec = + string.match(tstring, + '(%w%w%w), (%d+) (%w%w%w) (%d%d%d%d) (%d%d):(%d%d):(%d%d) GMT') + if not dayname then + return nil + end + local mon = table_index(MONTHS, monthname) + return rtc:date_fields_to_clock_s(year, mon-1, day, hour, min, sec) +end + +--[[ + return true if path exists and is not a directory +--]] +function file_exists(path) + local s = fs:stat(path) + if not s then + return false + end + return not s:is_directory() +end + +--[[ + substitute variables of form {xxx} from a table + from http://lua-users.org/wiki/StringInterpolation +--]] +function substitute_vars(s, vars) + s = (string.gsub(s, "({([^}]+)})", + function(whole,i) + return vars[i] or whole + end)) + return s +end + +--[[ + lat or lon as a string, working around limited type in ftoa_engine +--]] +function latlon_str(ll) + local ipart = tonumber(string.match(tostring(ll*1.0e-7), '(.*[.]).*')) + local fpart = math.abs(ll - ipart*10000000) + return string.format("%d.%u", ipart, fpart, ipart*10000000, ll) +end + +--[[ + location string for home page +--]] +function location_string(loc) + return substitute_vars([[{lat} {lon} {alt}]], + { ["lat"] = latlon_str(loc:lat()), + ["lon"] = latlon_str(loc:lng()), + ["alt"] = string.format("%.1fm", loc:alt()*1.0e-2) }) +end + +--[[ + client class for open connections +--]] +local function Client(_sock, _idx) + local self = {} + + self.closed = false + + local sock = _sock + local idx = _idx + local have_header = false + local header = "" + local header_lines = {} + local header_vars = {} + local run = nil + local protocol = nil + local file = nil + local start_time = millis() + local offset = 0 + + function self.read_header() + local s = sock:recv(2048) + if not s then + local now = millis() + if not sock:is_connected() or now - start_time > WEB_TIMEOUT:get()*1000 then + -- EOF while looking for header + DEBUG(string.format("%u: EOF", idx)) + self.remove() + return false + end + return false + end + if not s or #s == 0 then + return false + end + header = header .. s + local eoh = string.find(s, '\r\n\r\n') + if eoh then + DEBUG(string.format("%u: got header", idx)) + have_header = true + header_lines = split(header, "[^\r\n]+") + -- blocking for reply + sock:set_blocking(true) + return true + end + return false + end + + function self.sendstring(s) + sock:send(s, #s) + end + + function self.sendline(s) + self.sendstring(s .. "\r\n") + end + + --[[ + send a string with variable substitution using {varname} + --]] + function self.sendstring_vars(s, vars) + self.sendstring(substitute_vars(s, vars)) + end + + function self.send_header(code, codestr, vars) + self.sendline(string.format("%s %u %s", protocol, code, codestr)) + self.sendline(string.format("Server: %s", SERVER_VERSION)) + for k,v in pairs(vars) do + self.sendline(string.format("%s: %s", k, v)) + end + self.sendline("Connection: close") + self.sendline("") + end + + -- get size of a file + function self.file_size(fname) + local s = fs:stat(fname) + if not s then + return 0 + end + local ret = s:size():toint() + DEBUG(string.format("%u: size of '%s' -> %u", idx, fname, ret)) + return ret + end + + + --[[ + return full path with .. resolution + --]] + function self.full_path(path, name) + DEBUG(string.format("%u: full_path(%s,%s)", idx, path, name)) + local ret = path + if path == "/" and startswith(name,"@") then + return name + end + if name == ".." then + if path == "/" then + return "/" + end + if endswith(path,"/") then + path = string.sub(path, 1, #path-1) + end + local dir, _ = string.match(path, '(.*/)(.*)') + if not dir then + return path + end + return dir + end + if not endswith(ret, "/") then + ret = ret .. "/" + end + ret = ret .. name + DEBUG(string.format("%u: full_path(%s,%s) -> %s", idx, path, name, ret)) + return ret + end + + function self.directory_list(path) + sock:set_blocking(true) + if startswith(path, "/@") then + path = string.sub(path, 2, #path-1) + end + DEBUG(string.format("%u: directory_list(%s)", idx, path)) + local dlist = dirlist(path) + if not dlist then + dlist = {} + end + if not table_contains(dlist, "..") then + -- on ChibiOS we don't get .. + table.insert(dlist, "..") + end + if path == "/" then + for _,v in ipairs(HIDDEN_FOLDERS) do + table.insert(dlist, v) + end + end + + table.sort(dlist) + self.send_header(200, "OK", {["Content-Type"]=CONTENT_TEXT_HTML}) + self.sendline(DOCTYPE) + self.sendstring_vars([[ + + + Index of {path} + + +

    Index of {path}

    + + +]], {path=path}) + for _,d in ipairs(dlist) do + local skip = d == "." + if not skip then + local fullpath = self.full_path(path, d) + local name = d + local sizestr = "0" + local stat = fs:stat(fullpath) + local size = stat and stat:size() or 0 + if is_hidden_dir(fullpath) or (stat and stat:is_directory()) then + name = name .. "/" + elseif size >= 100*1000*1000 then + sizestr = string.format("%uM", (size/(1000*1000)):toint()) + else + sizestr = tostring(size) + end + local modtime = file_timestring(fullpath) + self.sendstring_vars([[ +]], { name=name, size=sizestr, modtime=modtime }) + end + end + self.sendstring([[ +
    NameLast modifiedSize
    {name}{modtime}{size}
    + + +]]) + end + + -- send file content + function self.send_file() + if not sock:pollout(0) then + return + end + local chunk = WEB_BLOCK_SIZE:get() + local b = file:read(chunk) + sock:set_blocking(true) + if b and #b > 0 then + local sent = sock:send(b, #b) + if sent == -1 then + run = nil + self.remove() + return + end + if sent < #b then + file:seek(offset+sent) + end + offset = offset + sent + end + if not b or #b < chunk then + -- EOF + DEBUG(string.format("%u: sent file", idx)) + run = nil + self.remove() + return + end + end + + --[[ + load whole file as a string + --]] + function self.load_file() + local chunk = WEB_BLOCK_SIZE:get() + local ret = "" + while true do + local b = file:read(chunk) + if not b or #b == 0 then + break + end + ret = ret .. b + end + return ret + end + + --[[ + evaluate some lua code and return as a string + --]] + function self.evaluate(code) + local eval_code = "function eval_func()\n" .. code .. "\nend\n" + local f, errloc, err = load(eval_code, "eval_func", "t", _ENV) + if not f then + DEBUG(string.format("load failed: err=%s errloc=%s", err, errloc)) + return nil + end + local success, err2 = pcall(f) + if not success then + DEBUG(string.format("pcall failed: err=%s", err2)) + return nil + end + local ok, s2 = pcall(eval_func) + eval_func = nil + if ok then + return s2 + end + return nil + end + + --[[ + process a file as a lua CGI + --]] + function self.send_cgi() + sock:set_blocking(true) + local contents = self.load_file() + local s = self.evaluate(contents) + if s then + self.sendstring(s) + end + self.remove() + end + + --[[ + send file content with server side processsing + files ending in .shtml can have embedded lua lika this: + + + + Using 'lstr' a return tostring(yourcode) is added to the code + automatically + --]] + function self.send_processed_file(dynamic_page) + sock:set_blocking(true) + local contents + if dynamic_page then + contents = file + else + contents = self.load_file() + end + while #contents > 0 do + local pat1 = "(.-)[<][?]lua[ \n](.-)[?][>](.*)" + local pat2 = "(.-)[<][?]lstr[ \n](.-)[?][>](.*)" + local p1, p2, p3 = string.match(contents, pat1) + if not p1 then + p1, p2, p3 = string.match(contents, pat2) + if not p1 then + break + end + p2 = "return tostring(" .. p2 .. ")" + end + self.sendstring(p1) + local s2 = self.evaluate(p2) + if s2 then + self.sendstring(s2) + end + contents = p3 + end + self.sendstring(contents) + self.remove() + end + + -- return a content type + function self.content_type(path) + if path == "/" then + return MIME_TYPES["html"] + end + local _, ext = string.match(path, '(.*[.])(.*)') + ext = string.lower(ext) + local ret = MIME_TYPES[ext] + if not ret then + return CONTENT_OCTET_STREAM + end + return ret + end + + -- perform a file download + function self.file_download(path) + if startswith(path, "/@") then + path = string.sub(path, 2, #path) + end + DEBUG(string.format("%u: file_download(%s)", idx, path)) + file = DYNAMIC_PAGES[path] + dynamic_page = file ~= nil + if not dynamic_page then + file = io.open(path,"rb") + if not file then + DEBUG(string.format("%u: Failed to open '%s'", idx, path)) + return false + end + end + local vars = {["Content-Type"]=self.content_type(path)} + local cgi_processing = startswith(path, "/cgi-bin/") and endswith(path, ".lua") + local server_side_processing = endswith(path, ".shtml") + local stat = fs:stat(path) + if not startswith(path, "@") and + not server_side_processing and + not cgi_processing and stat and + not dynamic_page then + local fsize = stat:size() + local mtime = stat:mtime() + vars["Content-Length"]= tostring(fsize) + local modtime = file_timestring_http(mtime) + if modtime then + vars["Last-Modified"] = modtime + end + local if_modified_since = header_vars['If-Modified-Since'] + if if_modified_since then + local tsec = file_timestring_http_parse(if_modified_since) + if tsec and tsec >= mtime then + DEBUG(string.format("%u: Not modified: %s %s", idx, modtime, if_modified_since)) + self.send_header(304, "Not Modified", vars) + return true + end + end + end + self.send_header(200, "OK", vars) + if server_side_processing or dynamic_page then + DEBUG(string.format("%u: shtml processing %s", idx, path)) + run = self.send_processed_file(dynamic_page) + elseif cgi_processing then + DEBUG(string.format("%u: CGI processing %s", idx, path)) + run = self.send_cgi + elseif stat and + WEB_SENDFILE_MIN:get() > 0 and + stat:size() >= WEB_SENDFILE_MIN:get() and + sock:sendfile(file) then + return true + else + run = self.send_file + end + return true + end + + function self.not_found() + self.send_header(404, "Not found", {}) + end + + function self.moved_permanently(relpath) + if not startswith(relpath, "/") then + relpath = "/" .. relpath + end + local location = string.format("http://%s%s", header_vars['Host'], relpath) + DEBUG(string.format("%u: Redirect -> %s", idx, location)) + self.send_header(301, "Moved Permanently", {["Location"]=location}) + end + + -- process a single request + function self.process_request() + local h1 = header_lines[1] + if not h1 or #h1 == 0 then + DEBUG(string.format("%u: empty request", idx)) + return + end + local cmd = split(header_lines[1], "%S+") + if not cmd or #cmd < 3 then + DEBUG(string.format("bad request: %s", header_lines[1])) + return + end + if cmd[1] ~= "GET" then + DEBUG(string.format("bad op: %s", cmd[1])) + return + end + protocol = cmd[3] + if protocol ~= "HTTP/1.0" and protocol ~= "HTTP/1.1" then + DEBUG(string.format("bad protocol: %s", protocol)) + return + end + local path = cmd[2] + DEBUG(string.format("%u: path='%s'", idx, path)) + + -- extract header variables + for i = 2,#header_lines do + local key, var = string.match(header_lines[i], '(.*): (.*)') + if key then + header_vars[key] = var + end + end + + if DYNAMIC_PAGES[path] ~= nil then + self.file_download(path) + return + end + + if path == MNT_PREFIX then + path = "/" + end + if startswith(path, MNT_PREFIX2) then + path = string.sub(path,#MNT_PREFIX2,#path) + end + + if isdirectory(path) and + not endswith(path,"/") and + header_vars['Host'] and + not is_hidden_dir(path) then + self.moved_permanently(path .. "/") + return + end + + if path ~= "/" and endswith(path,"/") then + path = string.sub(path, 1, #path-1) + end + + if startswith(path,"/@") then + path = string.sub(path, 2, #path) + end + + -- see if we have an index file + if isdirectory(path) and file_exists(path .. "/index.html") then + DEBUG(string.format("%u: found index.html", idx)) + if self.file_download(path .. "/index.html") then + return + end + end + + -- see if it is a directory + if (path == "/" or + DYNAMIC_PAGES[path] == nil) and + (endswith(path,"/") or + isdirectory(path) or + is_hidden_dir(path)) then + self.directory_list(path) + return + end + + -- or a file + if self.file_download(path) then + return + end + self.not_found(path) + end + + -- update the client + function self.update() + if run then + run() + return + end + if not have_header then + if not self.read_header() then + return + end + end + self.process_request() + if not run then + -- nothing more to do + self.remove() + end + end + + function self.remove() + DEBUG(string.format("%u: removing client OFFSET=%u", idx, offset)) + sock:close() + self.closed = true + end + + -- return the instance + return self +end + +--[[ + see if any new clients want to connect +--]] +local function check_new_clients() + while sock_listen:pollin(0) do + local sock = sock_listen:accept() + if not sock then + return + end + -- non-blocking for header read + sock:set_blocking(false) + -- find free client slot + for i = 1, #clients+1 do + if clients[i] == nil then + local idx = i + local client = Client(sock, idx) + DEBUG(string.format("%u: New client", idx)) + clients[idx] = client + end + end + end +end + +--[[ + check for client activity +--]] +local function check_clients() + for idx,client in ipairs(clients) do + if not client.closed then + client.update() + end + if client.closed then + table.remove(clients,idx) + end + end +end + +local function update() + check_new_clients() + check_clients() + return update,5 +end + +return update,100 diff --git a/libraries/AP_Scripting/applets/net_webserver.md b/libraries/AP_Scripting/applets/net_webserver.md new file mode 100644 index 00000000000000..fa45efe780b467 --- /dev/null +++ b/libraries/AP_Scripting/applets/net_webserver.md @@ -0,0 +1,91 @@ +# Web Server Application + +This implements a web server for boards that have networking support. + +# Parameters + +The web server has a small number of parameters + +## WEB_ENABLE + +This must be set to 1 to enable the web server + +## WEB_BIND_PORT + +This sets the network port to use for the server. It defaults to 8080 + +## WEB_DEBUG + +This enables verbose debugging + +## WEB_BLOCK_SIZE + +This sets the block size for network and file read/write +operations. Setting a larger value can increase performance at the +cost of more memory + +## WEB_TIMEOUT + +This sets the timeout in seconds for inactive client connections. + +# Operation + +By default the web server serves the root of your microSD card. You +can include html, javascript (*.js), image files etc on your microSD +to create a full web server with any structure you want. + +## Server Side Scripting + +The web server supports embedding lua script elements inside html +files for files with a filename of *.shtml. Here is an example: + +``` + + + + + +

    Server Side Scripting Test

    + + + + + + + +
    RollPitchYaw
    + + +``` +In this example we are using two forms of embedded lua scripts. The +first form starts with " quality is unknown +---@return number +function RangeFinder_State_ud:signal_quality() end + +-- set measurement quality in percent 0-100, -1 -> quality is unknown +---@param value number +function RangeFinder_State_ud:signal_quality(value) end + +-- get voltage in millivolts, if applicable, otherwise 0 +---@return number +function RangeFinder_State_ud:voltage() end + +-- set voltage in millivolts, if applicable, otherwise 0 +---@param value number +function RangeFinder_State_ud:voltage(value) end + + -- RangeFinder backend ---@class AP_RangeFinder_Backend_ud local AP_RangeFinder_Backend_ud = {} --- Send distance to lua rangefinder backend. Returns false if failed ----@param distance number +-- Send range finder measurement to lua rangefinder backend. Returns false if failed +---@param state RangeFinder_State_ud|number ---@return boolean -function AP_RangeFinder_Backend_ud:handle_script_msg(distance) end +function AP_RangeFinder_Backend_ud:handle_script_msg(state) end -- Status of this rangefinder instance ---@return integer @@ -2450,6 +2624,15 @@ function AP_RangeFinder_Backend_ud:orientation() end ---@return number function AP_RangeFinder_Backend_ud:distance() end +-- Current distance measurement signal_quality of the sensor instance +---@return number +function AP_RangeFinder_Backend_ud:signal_quality() end + +-- State of most recent range finder measurment +---@return RangeFinder_State_ud +function AP_RangeFinder_Backend_ud:get_state() end + + -- desc ---@class rangefinder rangefinder = {} @@ -2494,6 +2677,11 @@ function rangefinder:max_distance_cm_orient(orientation) end ---@return integer function rangefinder:distance_cm_orient(orientation) end +-- Current distance measurement signal quality for range finder at this orientation +---@param orientation integer +---@return integer +function rangefinder:signal_quality_pct_orient(orientation) end + -- desc ---@param orientation integer ---@return boolean @@ -2590,6 +2778,14 @@ function notify:handle_rgb(red, green, blue, rate_hz) end ---@param tune string function notify:play_tune(tune) end +-- Display text on a notify display, text too long to fit will automatically be scrolled. +---@param text string -- upto 50 characters +---@param row integer -- row number to display on, 0 is at the top. +function notify:send_text(text, row) end + +-- desc +---@param row integer +function notify:release_text(row) end -- desc ---@class gps @@ -2699,11 +2895,64 @@ function gps:primary_sensor() end ---@return integer function gps:num_sensors() end +-- desc +---@class BattMonitorScript_State_ud +local BattMonitorScript_State_ud = {} + +---@return BattMonitorScript_State_ud +function BattMonitorScript_State() end + +-- set field +---@param value number +function BattMonitorScript_State_ud:temperature(value) end + +-- set field +---@param value number +function BattMonitorScript_State_ud:consumed_wh(value) end + +-- set field +---@param value number +function BattMonitorScript_State_ud:consumed_mah(value) end + +-- set field +---@param value number +function BattMonitorScript_State_ud:current_amps(value) end + +-- set field +---@param value integer +function BattMonitorScript_State_ud:cycle_count(value) end + +-- set array field +---@param index integer +---@param value integer +function BattMonitorScript_State_ud:cell_voltages(index, value) end + +-- set field +---@param value integer +function BattMonitorScript_State_ud:capacity_remaining_pct(value) end + +-- set field +---@param value integer +function BattMonitorScript_State_ud:cell_count(value) end + +-- set field +---@param value number +function BattMonitorScript_State_ud:voltage(value) end + +-- set field +---@param value boolean +function BattMonitorScript_State_ud:healthy(value) end -- desc ---@class battery battery = {} +-- desc +---@param idx integer +---@param state BattMonitorScript_State_ud +---@return boolean +function battery:handle_scripting(idx, state) end + -- desc ---@param instance integer ---@param percentage number @@ -2916,6 +3165,18 @@ function ahrs:groundspeed_vector() end ---@return Vector3f_ud function ahrs:wind_estimate() end +-- Determine how aligned heading_deg is with the wind. Return result +-- is 1.0 when perfectly aligned heading into wind, -1 when perfectly +-- aligned with-wind, and zero when perfect cross-wind. There is no +-- distinction between a left or right cross-wind. Wind speed is ignored +---@param heading_deg number +---@return number +function ahrs:wind_alignment(heading_deg) end + +-- Forward head-wind component in m/s. Negative means tail-wind +---@return number +function ahrs:head_wind() end + -- desc ---@return number|nil function ahrs:get_hagl() end @@ -3066,3 +3327,97 @@ function fence:get_breach_time() end ---| 4 # Polygon ---| 8 # Minimum altitude function fence:get_breaches() end + +-- desc +---@class stat_t_ud +local stat_t_ud = {} + +---@return stat_t_ud +function stat_t() end + +-- get creation time in seconds +---@return uint32_t_ud +function stat_t_ud:ctime() end + +-- get last access time in seconds +---@return uint32_t_ud +function stat_t_ud:atime() end + +-- get last modification time in seconds +---@return uint32_t_ud +function stat_t_ud:mtime() end + +-- get file mode +---@return integer +function stat_t_ud:mode() end + +-- get file size in bytes +---@return uint32_t_ud +function stat_t_ud:size() end + +-- return true if this is a directory +---@return boolean +function stat_t_ud:is_directory() end + +-- desc +---@class rtc +rtc = {} + +-- return a time since 1970 in seconds from GMT date elements +---@param year integer -- 20xx +---@param month integer -- 0-11 +---@param day integer -- 1-31 +---@param hour integer -- 0-23 +---@param min integer -- 0-60 +---@param sec integer -- 0-60 +---@return uint32_t_ud +function rtc:date_fields_to_clock_s(year, month, day, hour, min, sec) end + +-- break a time in seconds since 1970 to GMT date elements +---@param param1 uint32_t_ud +---@return integer|nil -- year 20xx +---@return integer|nil -- month 0-11 +---@return integer|nil -- day 1-31 +---@return integer|nil -- hour 0-23 +---@return integer|nil -- min 0-60 +---@return integer|nil -- sec 0-60 +---@return integer|nil -- weekday 0-6, sunday is 0 +function rtc:clock_s_to_date_fields(param1) end + +-- desc +---@class fs +fs = {} + +-- desc +---@param param1 string +---@return stat_t_ud|nil +function fs:stat(param1) end + +-- Format the SD card. This is a async operation, use get_format_status to get the status of the format +---@return boolean +function fs:format() end + +-- Get the current status of a format. 0=NOT_STARTED, 1=PENDING, 2=IN_PROGRESS, 3=SUCCESS, 4=FAILURE +---@return number +function fs:get_format_status() end + +-- desc +---@class networking +networking = {} + +-- conver uint32_t address to string +---@param ip4addr uint32_t_ud +---@return string +function networking:address_to_str(ip4addr) end + +-- desc +---@return uint32_t_ud +function networking:get_gateway_active() end + +-- desc +---@return uint32_t_ud +function networking:get_netmask_active() end + +-- desc +---@return uint32_t_ud +function networking:get_ip_active() end diff --git a/libraries/AP_Scripting/drivers/BattMon_ANX.lua b/libraries/AP_Scripting/drivers/BattMon_ANX.lua new file mode 100644 index 00000000000000..80b32b924dc8e8 --- /dev/null +++ b/libraries/AP_Scripting/drivers/BattMon_ANX.lua @@ -0,0 +1,253 @@ +--[[ + device driver for ANX CAN battery monitor +--]] + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +local PARAM_TABLE_KEY = 45 +local PARAM_TABLE_PREFIX = "BATT_ANX_" + +-- 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 + +-- Type conversion functions, little endian +function get_uint16(frame, ofs) + return frame:data(ofs) + (frame:data(ofs + 1) << 8) +end + +-- Setup EFI Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 15), 'could not add param table') + +--[[ + // @Param: BATT_ANX_ENABLE + // @DisplayName: Enable ANX battery support + // @Description: Enable ANX battery support + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local BATT_ANX_ENABLE = bind_add_param('ENABLE', 1, 0) + +--[[ + // @Param: BATT_ANX_CANDRV + // @DisplayName: Set ANX CAN driver + // @Description: Set ANX CAN driver + // @Values: 0:None,1:1stCANDriver,2:2ndCanDriver + // @User: Standard +--]] +local BATT_ANX_CANDRV = bind_add_param('CANDRV', 2, 1) + +--[[ + // @Param: BATT_ANX_INDEX + // @DisplayName: ANX CAN battery index + // @Description: ANX CAN battery index + // @Range: 1 10 + // @User: Standard +--]] +local BATT_ANX_INDEX = bind_add_param('INDEX', 3, 1) + +--[[ + // @Param: BATT_ANX_OPTIONS + // @DisplayName: ANX CAN battery options + // @Description: ANX CAN battery options + // @Bitmask: 0:LogAllFrames + // @User: Advanced +--]] +local BATT_ANX_OPTIONS = bind_add_param('OPTIONS', 4, 0) + +local OPTION_LOGALLFRAMES = 0x01 + +if BATT_ANX_ENABLE:get() == 0 then + gcs:send_text(0, string.format("BATT_ANX: disabled")) + return +end + +-- Register for the CAN drivers +local driver + +local CAN_BUF_LEN = 25 +if BATT_ANX_CANDRV:get() == 1 then + driver = CAN.get_device(CAN_BUF_LEN) +elseif BATT_ANX_CANDRV:get() == 2 then + driver = CAN.get_device2(CAN_BUF_LEN) +end + +if not driver then + gcs:send_text(0, string.format("BATT_ANX: Failed to load driver")) + return +end + +local assembly = {} +assembly.num_frames = 0 +assembly.frames = {} + +--[[ + xmodem CRC implementation thanks to https://github.com/cloudwu/skynet + under MIT license +--]] +local XMODEMCRC16Lookup = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 +} + +local function crc_ANX(bytes) + -- ANX CRC uses xmodem with a seed of 0xa635 + local crc = 0xa635 + for i=1,#bytes do + local b = string.byte(bytes,i,i) + crc = ((crc<<8) & 0xffff) ~ XMODEMCRC16Lookup[(((crc>>8)~b) & 0xff) + 1] + end + return crc +end + +local frame_count = 0 + +local function log_can_frame(frame) + logger.write("CANF",'Id,DLC,FC,B0,B1,B2,B3,B4,B5,B6,B7','IBIBBBBBBBB', + frame:id(), + frame:dlc(), + frame_count, + frame:data(0), frame:data(1), frame:data(2), frame:data(3), + frame:data(4), frame:data(5), frame:data(6), frame:data(7)) + frame_count = frame_count + 1 +end + + +local function parse_volt_frame(payload) + if #payload < 12 then + -- invalid length + return + end + local total_volt, current, rem_cap, temperature, _ = string.unpack(" 32 then + num_cells = 32 + end + + local state = BattMonitorScript_State() + state:healthy(true) + state:voltage(total_volt) + state:cell_count(num_cells) + state:capacity_remaining_pct(math.floor(rem_cap)) + for i = 1, num_cells do + state:cell_voltages(i-1, cells[i]) + end + state:current_amps(current) + state:temperature(temperature) + + battery:handle_scripting(BATT_ANX_INDEX:get()-1, state) +end + + +--[[ + process a set of frames for a whole packet +--]] +local function process_frames(msg_type_id) + local bytes = "" + for i = 1, assembly.num_frames do + local dlc = assembly.frames[i]:dlc() + for ofs = 1, dlc do + bytes = bytes .. string.char(assembly.frames[i]:data(ofs-1)) + end + end + local crc = string.unpack("= 721 and msg_type_id <= 727 then + parse_volt_frame(payload) + end +end + +--[[ + read from CAN bus, updating battery backend +--]] +local function read_can() + while true do + local frame = driver:read_frame() + if not frame then + return + end + if BATT_ANX_OPTIONS:get() & OPTION_LOGALLFRAMES ~= 0 then + log_can_frame(frame) + end + if not frame:isExtended() then + -- only want extended frames + break + end + local id = frame:id_signed() + -- local sender_id = id&0x7 + local last_pkt_id = (id>>3) & 1 + local pkt_count = (id>>4) & 0x3F + -- local pkt_id = (id>>10) & 0x7f + -- local trans_type = (id>>17) & 0x03 + local msg_type_id = (id>>19) & 0x3FF + + if pkt_count ~= assembly.num_frames then + -- reset, non-contiguous packets + assembly.num_frames = 0 + end + + assembly.num_frames = assembly.num_frames + 1 + assembly.frames[assembly.num_frames] = frame + if last_pkt_id == 1 then + process_frames(msg_type_id) + -- reset for next frame + assembly.num_frames = 0 + end + end +end + +function update() + read_can() + return update,10 +end + +gcs:send_text(MAV_SEVERITY.INFO, "BATT_ANX: Started") + +return update() diff --git a/libraries/AP_Scripting/drivers/BattMon_ANX.md b/libraries/AP_Scripting/drivers/BattMon_ANX.md new file mode 100644 index 00000000000000..1deb4d9d665891 --- /dev/null +++ b/libraries/AP_Scripting/drivers/BattMon_ANX.md @@ -0,0 +1,20 @@ +# ANX Battery Driver + +This driver implements support for the ANX CAN battery protocol + +# Parameters + +The script used the following parameters: + +## BATT_ANX_ENABLE + +this must be set to 1 to enable the driver + +## BATT_ANX_CANDRV + +This sets the scripting CAN driver to use, this should be 1 or 2. + +## BATT_ANX_INDEX + +This sets the battery monitor index to use. Set to 1 for BATT1, 2 for +BATT2 etc diff --git a/libraries/AP_Scripting/drivers/EFI_SkyPower.lua b/libraries/AP_Scripting/drivers/EFI_SkyPower.lua index ba75519a99b6b5..569295ff1bac88 100644 --- a/libraries/AP_Scripting/drivers/EFI_SkyPower.lua +++ b/libraries/AP_Scripting/drivers/EFI_SkyPower.lua @@ -11,7 +11,6 @@ CAN_P1_DRIVER 1 (First driver) CAN_D1_BITRATE 500000 (500 kbit/s) --]] --- luacheck: only 0 -- Check Script uses a miniumum firmware version local SCRIPT_AP_VERSION = 4.3 @@ -27,6 +26,9 @@ local MAV_SEVERITY_ERROR = 3 local K_THROTTLE = 70 local K_HELIRSC = 31 +local MODEL_DEFAULT = 0 +local MODEL_SP_275 = 1 + PARAM_TABLE_KEY = 36 PARAM_TABLE_PREFIX = "EFI_SP_" @@ -62,10 +64,6 @@ function constrain(v, vmin, vmax) return v end ----- GLOBAL VARIABLES -local GKWH_TO_LBS_HP_HR = 0.0016439868 -local LITRES_TO_LBS = 1.6095 -- 6.1 lbs of fuel per gallon -> 1.6095 - local efi_backend = nil -- Setup EFI Parameters @@ -174,6 +172,34 @@ local EFI_SP_LOG_RT = bind_add_param('LOG_RT', 10, 10) -- rate for logg --]] local EFI_SP_ST_DISARM = bind_add_param('ST_DISARM', 11, 0) -- allow start when disarmed +--[[ + // @Param: EFI_SP_MODEL + // @DisplayName: SkyPower EFI ECU model + // @Description: SkyPower EFI ECU model + // @Values: 0:Default,1:SP_275 + // @User: Standard +--]] +local EFI_SP_MODEL = bind_add_param('MODEL', 12, MODEL_DEFAULT) + +--[[ + // @Param: EFI_SP_GEN_CTRL + // @DisplayName: SkyPower EFI enable generator control + // @Description: SkyPower EFI enable generator control + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local EFI_SP_GEN_CTRL = bind_add_param('GEN_CRTL', 13, 1) + +--[[ + // @Param: EFI_SP_RST_TIME + // @DisplayName: SkyPower EFI restart time + // @Description: SkyPower EFI restart time. If engine should be running and it has stopped for this amount of time then auto-restart. To disable this feature set this value to zero. + // @Range: 0 10 + // @User: Standard + // @Units: s +--]] +local EFI_SP_RST_TIME = bind_add_param('RST_TIME', 14, 2) + if EFI_SP_ENABLE:get() == 0 then gcs:send_text(0, string.format("EFISP: disabled")) return @@ -211,7 +237,7 @@ end --[[ EFI Engine Object --]] -local function engine_control(_driver, _idx) +local function engine_control(_driver) local self = {} -- Build up the EFI_State that is passed into the EFI Scripting backend @@ -221,21 +247,16 @@ local function engine_control(_driver, _idx) -- private fields as locals local rpm = 0 local air_pressure = 0 - local inj_ang = 0 local inj_time = 0 local target_load = 0 local current_load = 0 local throttle_angle = 0 local ignition_angle = 0 - local sfc = 0 - local sfc_icao = 0 - local last_sfc_t = 0 local supply_voltage = 0 local fuel_consumption_lph = 0 local fuel_total_l = 0 local last_fuel_s = 0 local driver = _driver - local idx = _idx local last_rpm_t = get_time_sec() local last_state_update_t = get_time_sec() local last_thr_update = get_time_sec() @@ -246,6 +267,9 @@ local function engine_control(_driver, _idx) local generator_started = false local engine_start_t = 0.0 local last_throttle = 0.0 + local sensor_error_flags = 0 + local thermal_limit_flags = 0 + local starter_rpm = 0 -- frames for sending commands local FRM_500 = uint32_t(0x500) @@ -264,6 +288,10 @@ local function engine_control(_driver, _idx) temps.cht = 0.0 -- Cylinder Head Temperature temps.imt = 0.0 -- intake manifold temperature temps.oilt = 0.0 -- oil temperature + temps.cht2 = 0.0 + temps.egt2 = 0.0 + temps.imt2 = 0.0 + temps.oil2 = 0.0 -- read telemetry packets function self.update_telemetry() @@ -276,9 +304,9 @@ local function engine_control(_driver, _idx) break end - -- All Frame IDs for this EFI Engine are extended - if frame:isExtended() then - self.handle_EFI_packet(frame, _idx) + -- All Frame IDs for this EFI Engine are not extended + if not frame:isExtended() then + self.handle_EFI_packet(frame) end end if last_rpm_t > last_state_update_t then @@ -289,36 +317,79 @@ local function engine_control(_driver, _idx) end -- handle an EFI packet - function self.handle_EFI_packet(frame, idx) + function self.handle_EFI_packet(frame) local id = frame:id_signed() - if id == 0x100 then - rpm = get_uint16(frame, 0) - ignition_angle = get_uint16(frame, 2) * 0.1 - throttle_angle = get_uint16(frame, 4) * 0.1 - last_rpm_t = get_time_sec() - elseif id == 0x101 then - current_load = get_uint16(frame, 0) * 0.1 - target_load = get_uint16(frame, 2) * 0.1 - inj_time = get_uint16(frame, 4) - inj_ang = get_uint16(frame, 6) * 0.1 - elseif id == 0x102 then - -- unused fields - elseif id == 0x104 then - supply_voltage = get_uint16(frame, 0) * 0.1 - ecu_temp = get_uint16(frame, 2) * 0.1 - air_pressure = get_uint16(frame, 4) - fuel_consumption_lph = get_uint16(frame,6)*0.001 - if last_fuel_s > 0 then - local dt = now_s - last_fuel_s - local fuel_lps = fuel_consumption_lph / 3600.0 - fuel_total_l = fuel_total_l + fuel_lps * dt + if EFI_SP_MODEL:get() == MODEL_SP_275 then + -- updated telemetry for SP-275 ECU + if id == 0x100 then + rpm = get_uint16(frame, 0) + ignition_angle = get_uint16(frame, 2) * 0.1 + throttle_angle = get_uint16(frame, 4) * 0.1 + last_rpm_t = get_time_sec() + elseif id == 0x101 then + air_pressure = get_uint16(frame, 4) + elseif id == 0x102 then + inj_time = get_uint16(frame, 4) + -- inj_ang = get_uint16(frame, 2) * 0.1 + elseif id == 0x104 then + supply_voltage = get_uint16(frame, 0) * 0.1 + elseif id == 0x105 then + temps.cht = get_uint16(frame, 0) * 0.1 + temps.imt = get_uint16(frame, 2) * 0.1 + temps.egt = get_uint16(frame, 4) * 0.1 + temps.oilt = get_uint16(frame, 6) * 0.1 + elseif id == 0x107 then + sensor_error_flags = get_uint16(frame, 0) + thermal_limit_flags = get_uint16(frame, 2) + elseif id == 0x107 then + target_load = get_uint16(frame, 6) * 0.1 + elseif id == 0x10C then + temps.cht2 = get_uint16(frame, 4) * 0.1 + temps.egt2 = get_uint16(frame, 6) * 0.1 + elseif id == 0x10D then + current_load = get_uint16(frame, 2) * 0.1 + elseif id == 0x113 then + gen.amps = get_uint16(frame, 2) + elseif id == 0x2E0 then + starter_rpm = get_uint16(frame, 4) + elseif id == 0x10B then + fuel_consumption_lph = get_uint16(frame,6)*0.001 + if last_fuel_s > 0 then + local dt = now_s - last_fuel_s + local fuel_lps = fuel_consumption_lph / 3600.0 + fuel_total_l = fuel_total_l + fuel_lps * dt + end + last_fuel_s = now_s + end + else + -- original SkyPower driver + if id == 0x100 then + rpm = get_uint16(frame, 0) + ignition_angle = get_uint16(frame, 2) * 0.1 + throttle_angle = get_uint16(frame, 4) * 0.1 + last_rpm_t = get_time_sec() + elseif id == 0x101 then + current_load = get_uint16(frame, 0) * 0.1 + target_load = get_uint16(frame, 2) * 0.1 + inj_time = get_uint16(frame, 4) + -- inj_ang = get_uint16(frame, 6) * 0.1 + elseif id == 0x104 then + supply_voltage = get_uint16(frame, 0) * 0.1 + ecu_temp = get_uint16(frame, 2) * 0.1 + air_pressure = get_uint16(frame, 4) + fuel_consumption_lph = get_uint16(frame,6)*0.001 + if last_fuel_s > 0 then + local dt = now_s - last_fuel_s + local fuel_lps = fuel_consumption_lph / 3600.0 + fuel_total_l = fuel_total_l + fuel_lps * dt + end + last_fuel_s = now_s + elseif id == 0x105 then + temps.cht = get_uint16(frame, 0) * 0.1 + temps.imt = get_uint16(frame, 2) * 0.1 + temps.egt = get_uint16(frame, 4) * 0.1 + temps.oilt = get_uint16(frame, 6) * 0.1 end - last_fuel_s = now_s - elseif id == 0x105 then - temps.cht = get_uint16(frame, 0) * 0.1 - temps.imt = get_uint16(frame, 2) * 0.1 - temps.egt = get_uint16(frame, 4) * 0.1 - temps.oilt = get_uint16(frame, 6) * 0.1 end end @@ -344,7 +415,7 @@ local function engine_control(_driver, _idx) -- copy cylinder_state to efi_state efi_state:cylinder_status(cylinder_state) - last_efi_state_time = millis() + local last_efi_state_time = millis() efi_state:last_updated_ms(last_efi_state_time) @@ -368,6 +439,10 @@ local function engine_control(_driver, _idx) -- send an engine start command function self.send_engine_start() + if EFI_SP_MODEL:get() == MODEL_SP_275 then + -- the SP-275 needs a stop before a start will work + self.send_engine_stop() + end local msg = CANFrame() msg:id(FRM_505) msg:data(0,10) @@ -437,7 +512,7 @@ local function engine_control(_driver, _idx) if min_rpm > 0 and engine_started and rpm < min_rpm and allow_run_engine() then local now = get_time_sec() local dt = now - engine_start_t - if dt > 2.0 then + if EFI_SP_RST_TIME:get() > 0 and dt > EFI_SP_RST_TIME:get() then gcs:send_text(0, string.format("EFISP: re-starting engine")) self.send_engine_start() engine_start_t = get_time_sec() @@ -455,6 +530,9 @@ local function engine_control(_driver, _idx) -- update generator control function self.update_generator() + if EFI_SP_GEN_CTRL:get() == 0 then + return + end local gen_state = rc:get_aux_cached(EFI_SP_GEN_FN:get()) if gen_state == 0 and generator_started then generator_started = false @@ -504,6 +582,9 @@ local function engine_control(_driver, _idx) gcs:send_named_float('EFI_OILTMP', temps.oilt) gcs:send_named_float('EFI_TRLOAD', target_load) gcs:send_named_float('EFI_VOLTS', supply_voltage) + gcs:send_named_float('EFI_GEN_AMPS', gen.amps) + gcs:send_named_float('EFI_CHT2', temps.cht2) + gcs:send_named_float('EFI_STARTRPM', starter_rpm) end -- update custom logging @@ -517,18 +598,18 @@ local function engine_control(_driver, _idx) return end last_log_t = now - logger.write('EFSP','Thr,CLoad,TLoad,OilT,RPM,gRPM,gAmp,gCur', 'ffffffff', + logger.write('EFSP','Thr,CLoad,TLoad,OilT,RPM,gRPM,gAmp,gCur,SErr,TLim,STRPM', 'ffffffffHHH', last_throttle, current_load, target_load, temps.oilt, rpm, - gen.rpm, gen.amps, gen.batt_current) + gen.rpm, gen.amps, gen.batt_current, + sensor_error_flags, thermal_limit_flags, + starter_rpm) end -- return the instance return self -end -- end function engine_control(_driver, _idx) - -local engine1 = engine_control(driver1, 1) +end -- end function engine_control -local last_efi_state_time = 0.0 +local engine1 = engine_control(driver1) function update() now_s = get_time_sec() diff --git a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua index ac050cabc66339..61113d05d7d545 100644 --- a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua @@ -74,7 +74,7 @@ -- global definitions local INIT_INTERVAL_MS = 3000 -- attempt to initialise the gimbal at this interval local UPDATE_INTERVAL_MS = 1 -- update interval in millis -local REPLY_TIMEOUT_MS = 1000 -- timeout waiting for reply after 1 sec +local REPLY_TIMEOUT_MS = 100 -- timeout waiting for reply after 0.1 sec local REQUEST_ATTITUDE_INTERVAL_MS = 100-- request attitude at 10hz local SET_ATTITUDE_INTERVAL_MS = 100 -- set attitude at 10hz local MOUNT_INSTANCE = 0 -- always control the first mount/gimbal @@ -118,6 +118,9 @@ local MNT1_TYPE = Parameter("MNT1_TYPE") -- should be 9:Scripting -- message definitions local HEADER = 0xAA local RETURN_CODE = {SUCCESS=0x00, PARSE_ERROR=0x01, EXECUTION_FAILED=0x02, UNDEFINED=0xFF} +local ATTITUDE_PACKET_LEN = {LEGACY=24, LATEST=26} -- attitude packet expected length. Legacy must be less than latest +local POSITION_CONTROL_PACKET_LEN = {LEGACY=17, LATEST=19} -- position control packet expected length. Legacy must be less than latest +local SPEED_CONTROL_PACKET_LEN = {LEGACY=17, LATEST=19} -- speed control packet expected length. Legacy must be less than latest -- parsing state definitions local PARSE_STATE_WAITING_FOR_HEADER = 0 @@ -653,12 +656,27 @@ function parse_byte(b) end -- parse attitude reply message - if (expected_reply == REPLY_TYPE.ATTITUDE) and (parse_length >= 20) then - local ret_code = parse_buff[13] + if (expected_reply == REPLY_TYPE.ATTITUDE) and (parse_length >= ATTITUDE_PACKET_LEN.LEGACY) then + -- default to legacy format but also handle latest format + local ret_code_field = 13 + local yaw_field = 15 + local pitch_field = 17 + local roll_field = 19 + if (parse_length >= ATTITUDE_PACKET_LEN.LATEST) then + ret_code_field = 15 + yaw_field = 17 + pitch_field = 21 + roll_field = 19 + end + local ret_code = parse_buff[ret_code_field] if ret_code == RETURN_CODE.SUCCESS then - local yaw_deg = int16_value(parse_buff[16],parse_buff[15]) * 0.1 - local pitch_deg = int16_value(parse_buff[18],parse_buff[17]) * 0.1 - local roll_deg = int16_value(parse_buff[20],parse_buff[19]) * 0.1 + local yaw_deg = int16_value(parse_buff[yaw_field+1],parse_buff[yaw_field]) * 0.1 + local pitch_deg = int16_value(parse_buff[pitch_field+1],parse_buff[pitch_field]) * 0.1 + local roll_deg = int16_value(parse_buff[roll_field+1],parse_buff[roll_field]) * 0.1 + -- if upsidedown, subtract 180deg from yaw to undo addition of target + if DJIR_UPSIDEDOWN:get() > 0 then + yaw_deg = wrap_180(yaw_deg - 180) + end mount:set_attitude_euler(MOUNT_INSTANCE, roll_deg, pitch_deg, yaw_deg) if DJIR_DEBUG:get() > 1 then gcs:send_text(MAV_SEVERITY.INFO, string.format("DJIR: roll:%4.1f pitch:%4.1f yaw:%4.1f", roll_deg, pitch_deg, yaw_deg)) @@ -669,16 +687,26 @@ function parse_byte(b) end -- parse position control reply message - if (expected_reply == REPLY_TYPE.POSITION_CONTROL) and (parse_length >= 13) then - local ret_code = parse_buff[13] + if (expected_reply == REPLY_TYPE.POSITION_CONTROL) and (parse_length >= POSITION_CONTROL_PACKET_LEN.LEGACY) then + -- default to legacy format but also handle latest format + local ret_code_field = 13 + if (parse_length >= POSITION_CONTROL_PACKET_LEN.LATEST) then + ret_code_field = 15 + end + local ret_code = parse_buff[ret_code_field] if ret_code ~= RETURN_CODE.SUCCESS then execute_fails = execute_fails + 1 end end -- parse speed control reply message - if (expected_reply == REPLY_TYPE.SPEED_CONTROL) and (parse_length >= 13) then - local ret_code = parse_buff[13] + if (expected_reply == REPLY_TYPE.SPEED_CONTROL) and (parse_length >= SPEED_CONTROL_PACKET_LEN.LEGACY) then + -- default to legacy format but also handle latest format + local ret_code_field = 13 + if (parse_length >= SPEED_CONTROL_PACKET_LEN.LATEST) then + ret_code_field = 15 + end + local ret_code = parse_buff[ret_code_field] if ret_code ~= RETURN_CODE.SUCCESS then execute_fails = execute_fails + 1 end diff --git a/libraries/AP_Scripting/drivers/mount-djirs2-driver.md b/libraries/AP_Scripting/drivers/mount-djirs2-driver.md index d4b19886e2354f..7f4d6e058c36b9 100644 --- a/libraries/AP_Scripting/drivers/mount-djirs2-driver.md +++ b/libraries/AP_Scripting/drivers/mount-djirs2-driver.md @@ -12,3 +12,10 @@ DJI RS2 and RS3-Pro gimbal mount driver lua script - Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver - Reboot the autopilot - Copy the mount-djirs2-driver.lua script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot + +## Issues + +If the ground station reports "Pre-arm: Mount not healthy", update the +gimbal firmware using the DJI Ronin phone app to version 01.04.00.20 or +later to correct a mismatch in the way data is received from the gimbal. +Completing this update may take more than an hour. diff --git a/libraries/AP_Scripting/examples/CAN_logger.lua b/libraries/AP_Scripting/examples/CAN_logger.lua new file mode 100644 index 00000000000000..aa3c5ff990a643 --- /dev/null +++ b/libraries/AP_Scripting/examples/CAN_logger.lua @@ -0,0 +1,52 @@ +--[[ + This script captures raw packets to a log file for later playback using Tools/scripts/CAN/CAN_playback.py onto a CAN bus. + Set CAN_D1_PROTOCOL to 10 for scripting. + Also need LOG_DISARMED set to 1 if running this while disarmed. +--]] + +local can_driver = CAN:get_device(25) + +if not can_driver then + gcs:send_text(0,"No scripting CAN interface found") + return +end + +local last_print_ms = millis() +local frame_count = 0 +local last_frame_count = 0 + +function update() + + local more_frames = true + for _ = 1, 25 do + local frame = can_driver:read_frame() + if not frame then + more_frames = false + break + end + local id = frame:id() + logger.write("CANF",'Id,DLC,FC,B0,B1,B2,B3,B4,B5,B6,B7','IBIBBBBBBBB', + id, + frame:dlc(), + frame_count, + frame:data(0), frame:data(1), frame:data(2), frame:data(3), + frame:data(4), frame:data(5), frame:data(6), frame:data(7)) + frame_count = frame_count + 1 + end + + local now = millis() + if now - last_print_ms >= 1000 then + local dt = (now - last_print_ms):tofloat()*0.001 + gcs:send_text(0, string.format("CAN: %.2f fps", (frame_count-last_frame_count)/dt)) + last_print_ms = now + last_frame_count = frame_count + end + + if more_frames then + -- sleep for min possible time to try not to lose frames + return update, 0 + end + return update, 2 +end + +return update() diff --git a/libraries/AP_Scripting/examples/CAN_read.lua b/libraries/AP_Scripting/examples/CAN_read.lua index ca310faf3e0e47..789f9e32002f4d 100644 --- a/libraries/AP_Scripting/examples/CAN_read.lua +++ b/libraries/AP_Scripting/examples/CAN_read.lua @@ -10,6 +10,11 @@ if not driver1 and not driver2 then return end +-- Only accept DroneCAN node status msg on second driver +-- node status is message ID 341 +-- Message ID is 16 bits left shifted by 8 in the CAN frame ID. +driver2:add_filter(uint32_t(0xFFFF) << 8, uint32_t(341) << 8) + function show_frame(dnum, frame) gcs:send_text(0,string.format("CAN[%u] msg from " .. tostring(frame:id()) .. ": %i, %i, %i, %i, %i, %i, %i, %i", dnum, frame:data(0), frame:data(1), frame:data(2), frame:data(3), frame:data(4), frame:data(5), frame:data(6), frame:data(7))) end diff --git a/libraries/AP_Scripting/examples/OOP_example.lua b/libraries/AP_Scripting/examples/OOP_example.lua index d555cb4a0278e7..a6b7cf0638d91c 100644 --- a/libraries/AP_Scripting/examples/OOP_example.lua +++ b/libraries/AP_Scripting/examples/OOP_example.lua @@ -1,5 +1,4 @@ -- this is an example of how to do object oriented programming in Lua --- luacheck: only 0 function constrain(v, minv, maxv) -- constrain a value between two limits @@ -25,7 +24,6 @@ local function PIFF(kFF,kP,kI,iMax) local _kFF = kFF local _kP = kP or 0.0 local _kI = kI or 0.0 - local _kD = kD or 0.0 local _iMax = iMax local _last_t = nil local _log_data = {} diff --git a/libraries/AP_Scripting/examples/SN-GCJA5-particle-sensor.lua b/libraries/AP_Scripting/examples/SN-GCJA5-particle-sensor.lua index 08a1f2ad8d27e1..163ae6ca8aa85e 100644 --- a/libraries/AP_Scripting/examples/SN-GCJA5-particle-sensor.lua +++ b/libraries/AP_Scripting/examples/SN-GCJA5-particle-sensor.lua @@ -14,6 +14,9 @@ local file_name while true do file_name = string.format('Particle %i.csv',index) local file = io.open(file_name) + if file == nil then + break + end local first_line = file:read(1) -- try and read the first character io.close(file) if first_line == nil then diff --git a/libraries/AP_Scripting/examples/hello_world_display.lua b/libraries/AP_Scripting/examples/hello_world_display.lua new file mode 100644 index 00000000000000..cf356da6252637 --- /dev/null +++ b/libraries/AP_Scripting/examples/hello_world_display.lua @@ -0,0 +1,66 @@ +-- This script is an example of printing to a display via scripting +-- Connect a supported display to the autopilot, and configure the NTF_DISPLAY_TYPE parameter seen at https://ardupilot.org/copter/docs/common-display-onboard.html +-- The notify:send_text(text, row) method will override default on the display, disabling the default messages + +local switchTimeA +local switchTimeB +local displayWidth = 18 +local function update() + -- Just keep track of when we should switch to a smiley :) + if switchTimeA == nil then + switchTimeA = millis() + 5000 + switchTimeB = switchTimeA + 10000 + end + + -- Example of overriding a line keeping some defaults, here we will replace the battery(1) and GPS(2) rows + if switchTimeA > millis() then + notify:send_text("Hello, World!", 1) + notify:send_text(tostring(millis()), 2) + + -- Next demonstrate we can release the text, and the default will be shown again + elseif switchTimeB > millis() then + notify:release_text(1) + notify:release_text(2) + + -- Example of overriding all lines, a smiley, try moving the autopilot around to see it change + else + -- Generate the smiley + local width = (displayWidth / 2) + local roll = math.floor(width + (ahrs:get_roll() * width)) - 4 + local pitch = math.floor(ahrs:get_pitch() * 6) + 2; + local sub = 5 - roll + if sub < 0 then + sub = 0 + end + local rows = {} + if pitch - 2 >= 0 and pitch - 2 <= 5 then + rows[pitch - 2] = (string.rep(" ", roll) .. " ##"):sub(sub); + end + if pitch - 1 >= 0 and pitch - 1 <= 5 then + rows[pitch - 1] = (string.rep(" ", roll) .. " # #"):sub(sub); + end + if pitch >= 0 and pitch <= 5 then + rows[pitch] = (string.rep(" ", roll) .. " #"):sub(sub); + end + if pitch + 1 >= 0 and pitch + 1 <= 5 then + rows[pitch + 1] = (string.rep(" ", roll) .. " # #"):sub(sub); + end + if pitch + 2 >= 0 and pitch + 2 <= 5 then + rows[pitch + 2] = (string.rep(" ", roll) .. " ##"):sub(sub); + end + if pitch + 3 >= 0 and pitch + 3 <= 5 then + rows[pitch + 3] = ""; + end + + -- Send it out to the display + for i = 0, 5 do + if rows[i] == nil then + rows[i] = "" + end + notify:send_text(rows[i], i) + end + end + + return update, 10 + end + return update, 1000 -- Wait a few seconds before starting diff --git a/libraries/AP_Scripting/examples/mission-save.lua b/libraries/AP_Scripting/examples/mission-save.lua index 559d9199923468..84d19da296f49b 100644 --- a/libraries/AP_Scripting/examples/mission-save.lua +++ b/libraries/AP_Scripting/examples/mission-save.lua @@ -14,6 +14,9 @@ local function save_to_SD() while true do file_name = string.format('%i.waypoints',index) local file = io.open(file_name) + if file == nil then + break + end local first_line = file:read(1) -- try and read the first character io.close(file) if first_line == nil then diff --git a/libraries/AP_Scripting/examples/net_test.lua b/libraries/AP_Scripting/examples/net_test.lua new file mode 100644 index 00000000000000..b83fa2f8f4ba97 --- /dev/null +++ b/libraries/AP_Scripting/examples/net_test.lua @@ -0,0 +1,119 @@ +--[[ + example script to test lua socket API +--]] + +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +PARAM_TABLE_KEY = 46 +PARAM_TABLE_PREFIX = "NT_" + +-- 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 Parameters +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 6), 'net_test: could not add param table') + +--[[ + // @Param: NT_ENABLE + // @DisplayName: enable network tests + // @Description: Enable network tests + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local NT_ENABLE = bind_add_param('ENABLE', 1, 0) +if NT_ENABLE:get() == 0 then + return +end + +local NT_TEST_IP = { bind_add_param('TEST_IP0', 2, 192), + bind_add_param('TEST_IP1', 3, 168), + bind_add_param('TEST_IP2', 4, 13), + bind_add_param('TEST_IP3', 5, 15) } + +local NT_BIND_PORT = bind_add_param('BIND_PORT', 6, 15001) + +local PORT_ECHO = 7 + +gcs:send_text(MAV_SEVERITY.INFO, "net_test: starting") + +local function test_ip() + return string.format("%u.%u.%u.%u", NT_TEST_IP[1]:get(), NT_TEST_IP[2]:get(), NT_TEST_IP[3]:get(), NT_TEST_IP[4]:get()) +end + +local counter = 0 +local sock_tcp_echo = Socket(0) +local sock_udp_echo = Socket(1) +local sock_tcp_in = Socket(0) +local sock_tcp_in2 = nil +local sock_udp_in = Socket(1) + +assert(sock_tcp_echo, "net_test: failed to create tcp echo socket") +assert(sock_udp_echo, "net_test: failed to create udp echo socket") +assert(sock_tcp_in:bind("0.0.0.0", + NT_BIND_PORT:get()), string.format("net_test: failed to bind to TCP %u", NT_BIND_PORT:get())) +assert(sock_tcp_in:listen(1), "net_test: failed to listen") +assert(sock_udp_in:bind("0.0.0.0", + NT_BIND_PORT:get()), string.format("net_test: failed to bind to UDP %u", NT_BIND_PORT:get())) + +--[[ + test TCP or UDP echo +--]] +local function test_echo(name, sock) + if not sock:is_connected() then + if not sock:connect(test_ip(), PORT_ECHO) then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("test_echo(%s): failed to connect", name)) + return + end + + if not sock:set_blocking(true) then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("test_echo(%s): failed to set blocking", name)) + return + end + end + + local s = string.format("testing %u", counter) + local nsent = sock:send(s, #s) + if nsent ~= #s then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("test_echo(%s): failed to send", name)) + return + end + local r = sock:recv(#s) + if r then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("test_echo(%s): got reply '%s'", name, r)) + end +end + +--[[ + test a simple server +--]] +local function test_server(name, sock) + if name == "TCP" then + if not sock_tcp_in2 then + sock_tcp_in2 = sock:accept() + if not sock_tcp_in2 then + return + end + gcs:send_text(MAV_SEVERITY.ERROR, string.format("test_server(%s): new connection", name)) + end + sock = sock_tcp_in2 + end + + local r = sock:recv(1024) + if r and #r > 0 then + gcs:send_text(MAV_SEVERITY.ERROR, string.format("test_server(%s): got input '%s'", name, r)) + end +end + +local function update() + test_echo("TCP", sock_tcp_echo) + test_echo("UDP", sock_udp_echo) + test_server("TCP", sock_tcp_in) + test_server("UDP", sock_udp_in) + counter = counter + 1 + return update,1000 +end + +return update,100 diff --git a/libraries/AP_Scripting/examples/plane-doublets.lua b/libraries/AP_Scripting/examples/plane-doublets.lua index 60618b66aa5f18..2c34a381f6cd55 100644 --- a/libraries/AP_Scripting/examples/plane-doublets.lua +++ b/libraries/AP_Scripting/examples/plane-doublets.lua @@ -7,7 +7,6 @@ -- It is suggested to allow the aircraft to trim for straight, level, unaccelerated flight (SLUF) in FBWB mode before -- starting a doublet -- Charlie Johnson, Oklahoma State University 2020 --- luacheck: only 0 local DOUBLET_ACTION_CHANNEL = 6 -- RCIN channel to start a doublet when high (>1700) local DOUBLET_CHOICE_CHANNEL = 7 -- RCIN channel to choose elevator (low) or rudder (high) @@ -19,8 +18,6 @@ local DOUBLET_TIME = 500 -- period of doublet signal in ms -- flight mode numbers for plane https://mavlink.io/en/messages/ardupilotmega.html local MODE_MANUAL = 0 local MODE_FBWA = 5 -local MODE_FBWB = 6 -local MODE_RTL = 11 local K_AILERON = 4 local K_ELEVATOR = 19 local K_THROTTLE = 70 @@ -30,7 +27,6 @@ local K_RUDDER = 21 local start_time = -1 local end_time = -1 local now = -1 -local desired_mode = -1 -- store information about the doublet channel local doublet_srv_chan = SRV_Channels:find_channel(DOUBLET_FUCNTION) @@ -65,7 +61,7 @@ function doublet() pre_doublet_mode = vehicle:get_mode() -- are we doing a doublet on elevator or rudder? set the other controls to trim local doublet_choice_pwm = rc:get_pwm(DOUBLET_CHOICE_CHANNEL) - local trim_funcs = {} + local trim_funcs local pre_doublet_elevator = SRV_Channels:get_output_pwm(K_ELEVATOR) if doublet_choice_pwm < 1500 then -- doublet on elevator diff --git a/libraries/AP_Scripting/examples/plane-wind-fs.lua b/libraries/AP_Scripting/examples/plane-wind-fs.lua index 908d9af64278e4..ef5fcfccd2eb26 100644 --- a/libraries/AP_Scripting/examples/plane-wind-fs.lua +++ b/libraries/AP_Scripting/examples/plane-wind-fs.lua @@ -30,29 +30,25 @@ local SITL_wind = false -- Read in required params -local value = param:get('TRIM_ARSPD_CM') +local value = param:get('AIRSPEED_CRUISE') if value then - air_speed = value / 100 + air_speed = value else - error('LUA: get TRIM_ARSPD_CM failed') + error('LUA: get AIRSPEED_CRUISE failed') end -value = param:get('ARSPD_FBW_MIN') +value = param:get('AIRSPEED_MIN') if value then min_air_speed = value else - error('LUA: get ARSPD_FBW_MIN failed') + error('LUA: get AIRSPEED_MIN failed') end -value = param:get('MIN_GNDSPD_CM') -if value then - min_ground_speed = value / 100 -else - error('LUA: get MIN_GNDSPD_CM failed') +min_ground_speed = param:get('MIN_GROUNDSPEED') +if not min_groundspeed then + error('LUA: get MIN_GROUNDSPEED failed') end -value = param:get('LIM_ROLL_CD') -if value then - max_bank_angle = value / 100 -else - error('LUA: get LIM_ROLL_CD failed') +max_bank_angle = param:get('ROLL_LIMIT_DEG') +if not max_bank_angle then + error('LUA: get ROLL_LIMIT_DEG failed') end -- https://en.wikipedia.org/wiki/Standard_rate_turn#Radius_of_turn_formula diff --git a/libraries/AP_Scripting/examples/protected_call.lua b/libraries/AP_Scripting/examples/protected_call.lua index 7446092c5f4e2c..ff7cf45cd7a402 100644 --- a/libraries/AP_Scripting/examples/protected_call.lua +++ b/libraries/AP_Scripting/examples/protected_call.lua @@ -1,7 +1,6 @@ -- this shows how to protect against faults in your scripts -- you can wrap your update() call (or any other call) in a pcall() -- which catches errors, allowing you to take an appropriate action --- luacheck: only 0 -- example main loop function @@ -12,6 +11,7 @@ function update() -- deliberately make a bad call to cause a fault, asking for the 6th GPS status -- as this is done inside a pcall() the error will be caught instead of stopping the script local status = gps:status(5) + gcs:send_text(0, "GPS status: " .. tostring(status)) end end diff --git a/libraries/AP_Scripting/examples/quadruped.lua b/libraries/AP_Scripting/examples/quadruped.lua index b36391e9899040..a8d5536512d7de 100644 --- a/libraries/AP_Scripting/examples/quadruped.lua +++ b/libraries/AP_Scripting/examples/quadruped.lua @@ -18,7 +18,6 @@ -- Output12: back right tibia (shin) servo -- -- CAUTION: This script should only be used with ArduPilot Rover's firmware --- luacheck: only 0 local FRAME_LEN = 80 -- frame length in mm @@ -183,7 +182,7 @@ end -- a) body rotations: body_rot_x, body_rot_y, body_rot_z -- b) body position: body_pos_x, body_pos_y, body_pos_z -- c) offset of the center of body -function body_forward_kinematics(X, Y, Z, Xdist, Ydist, Zrot) +function body_forward_kinematics(X, Y, _, Xdist, Ydist, Zrot) local totaldist_x = X + Xdist + body_pos_x local totaldist_y = Y + Ydist + body_pos_y local distBodyCenterFeet = math.sqrt(totaldist_x^2 + totaldist_y^2) diff --git a/libraries/AP_Scripting/examples/rangefinder_quality_test.lua b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua new file mode 100644 index 00000000000000..28c78e7ff25156 --- /dev/null +++ b/libraries/AP_Scripting/examples/rangefinder_quality_test.lua @@ -0,0 +1,304 @@ + +-- This test uses the Range Finder driver interface to simulate Range Finder +-- hardware and uses the Range Finder client interface to simulate a +-- client of the driver. The test sends distance data through the driver +-- interface and validates that it can be read through the client interface. + +-- Parameters should be set as follows before this test is loaded. +-- "RNGFND1_TYPE": 36, +-- "RNGFND1_ORIENT": 25, +-- "RNGFND1_MIN_CM": 10, +-- "RNGFND1_MAX_CM": 5000, + +-- UPDATE_PERIOD_MS is the time between when a distance is set and +-- when it is read. There is a periodic task that copies the set distance to +-- the state structure that it is read from. If UPDATE_PERIOD_MS is too short this periodic +-- task might not get a chance to run. A value of 25 seems to be too quick for sub. +local UPDATE_PERIOD_MS = 50 +local TIMEOUT_MS = 5000 + +-- These strings must match the strings used by the test driver for interpreting the output from this test. +local TEST_ID_STR = "RQTL" +local COMPLETE_STR = "#complete#" +local SUCCESS_STR = "!!success!!" +local FAILURE_STR = "!!failure!!" + + +-- Copied from libraries/AP_Math/rotation.h enum Rotation {}. +local RNGFND_ORIENTATION_DOWN = 25 +local RNGFND_ORIENTATION_FORWARD = 0 +-- Copied from libraries/AP_RangeFinder/AP_RanggeFinder.h enum RangeFinder::Type {}. +local RNGFND_TYPE_LUA = 36.0 +-- Copied from libraries/AP_RangeFinder/AP_RangeFinder.h enum RangeFinder::Status {}. +local RNDFND_STATUS_NOT_CONNECTED = 0 +local RNDFND_STATUS_OUT_OF_RANGE_LOW = 2 +local RNDFND_STATUS_OUT_OF_RANGE_HIGH = 3 +local RNDFND_STATUS_GOOD = 4 +-- Copied from libraries/AP_RangeFinder/AP_RangeFinder.h +local SIGNAL_QUALITY_MIN = 0 +local SIGNAL_QUALITY_MAX = 100 +local SIGNAL_QUALITY_UNKNOWN = -1 + +-- Read parameters for min and max valid range finder ranges. +local RNGFND1_MIN_CM = Parameter("RNGFND1_MIN_CM"):get() +local RNGFND1_MAX_CM = Parameter("RNGFND1_MAX_CM"):get() + +local function send(str) + gcs:send_text(3, string.format("%s %s", TEST_ID_STR, str)) +end + + +-- The range finder backend is initialized in the update_prepare function. +---@type AP_RangeFinder_Backend_ud +local rngfnd_backend + + +local function test_dist_equal(dist_m_in, dist_in_factor, dist_out, signal_quality_pct_in, signal_quality_pct_out) + if math.abs(dist_out - dist_m_in * dist_in_factor) > 1.0e-3 then + return false + end + if signal_quality_pct_in < 0 and signal_quality_pct_out == -1 then + return true + end + if signal_quality_pct_in > 100 and signal_quality_pct_out == -1 then + return true + end + if signal_quality_pct_in == signal_quality_pct_out then + return true + end + return false +end + +local function get_and_eval(test_idx, dist_m_in, signal_quality_pct_in, status_expected) + local status_actual = rangefinder:status_orient(RNGFND_ORIENTATION_DOWN) + + -- Check that the status is as expected + if status_expected ~= status_actual then + return string.format("Status test %i status incorrect - expected %i, actual %i", test_idx, status_expected, status_actual) + end + + -- Not more checks if the status is poor + if status_actual ~= RNDFND_STATUS_GOOD then + send(string.format("Status test %i status correct - expected: %i actual: %i", test_idx, status_expected, status_actual)) + return nil + end + + -- L U A I N T E R F A C E T E S T + -- Check that the distance and signal_quality from the frontend are as expected + local distance1_cm_out = rangefinder:distance_cm_orient(RNGFND_ORIENTATION_DOWN) + local signal_quality1_pct_out = rangefinder:signal_quality_pct_orient(RNGFND_ORIENTATION_DOWN) + + -- Make sure data was returned + if not distance1_cm_out or not signal_quality1_pct_out then + return "No data returned from rangefinder:distance_cm_orient()" + end + + send(string.format("Frontend test %i dist in_m: %.2f out_cm: %.2f, signal_quality_pct in: %.1f out: %.1f", + test_idx, dist_m_in, distance1_cm_out, signal_quality_pct_in, signal_quality1_pct_out)) + + if not test_dist_equal(dist_m_in, 100.0, distance1_cm_out, signal_quality_pct_in, signal_quality1_pct_out) then + return "Frontend expected and actual do not match" + end + + -- L U A I N T E R F A C E T E S T + -- Check that the distance and signal_quality from the backend are as expected + local disttance2_m_out = rngfnd_backend:distance() + local signal_quality2_pct_out = rngfnd_backend:signal_quality() + + send(string.format("Backend test %i dist in_m: %.2f out_m: %.2f, signal_quality_pct in: %.1f out: %.1f", + test_idx, dist_m_in, disttance2_m_out, signal_quality_pct_in, signal_quality2_pct_out)) + + if not test_dist_equal(dist_m_in, 1.0, disttance2_m_out, signal_quality_pct_in, signal_quality2_pct_out) then + return "Backend expected and actual do not match" + end + + -- L U A I N T E R F A C E T E S T + -- Check that the state from the backend is as expected + local rf_state = rngfnd_backend:get_state() + local distance3_m_out = rf_state:distance() + local signal_quality3_pct_out = rf_state:signal_quality() + + send(string.format("State test %i dist in_m: %.2f out_m: %.2f, signal_quality_pct in: %.1f out: %.1f", + test_idx, dist_m_in, distance3_m_out, signal_quality_pct_in, signal_quality3_pct_out)) + + if not test_dist_equal(dist_m_in, 1.0, distance3_m_out, signal_quality_pct_in, signal_quality3_pct_out) then + return "State expected and actual do not match" + end + + return nil +end + +-- Test various status states +local function do_status_tests() + send("Test initial status") + local status_actual = rangefinder:status_orient(RNGFND_ORIENTATION_DOWN) + if status_actual ~= RNDFND_STATUS_NOT_CONNECTED then + return string.format("DOWN Status '%i' not NOT_CONNECTED on initialization.", status_actual) + end + status_actual = rangefinder:status_orient(RNGFND_ORIENTATION_FORWARD) + if status_actual ~= RNDFND_STATUS_NOT_CONNECTED then + return string.format("FORWARD Status '%i' not NOT_CONNECTED on initialization.", status_actual) + end + return nil +end + + +local test_data = { + {20.0, -1, RNDFND_STATUS_GOOD}, + {20.5, -2, RNDFND_STATUS_GOOD}, + {21.0, 0, RNDFND_STATUS_GOOD}, + {22.0, 50, RNDFND_STATUS_GOOD}, + {23.0, 100, RNDFND_STATUS_GOOD}, + {24.0, 101, RNDFND_STATUS_GOOD}, + {25.0, -3, RNDFND_STATUS_GOOD}, + {26.0, 127, RNDFND_STATUS_GOOD}, + {27.0, 3, RNDFND_STATUS_GOOD}, + {28.0, 100, RNDFND_STATUS_GOOD}, + {29.0, 99, RNDFND_STATUS_GOOD}, + {100.0, 100, RNDFND_STATUS_OUT_OF_RANGE_HIGH}, + {0.0, 100, RNDFND_STATUS_OUT_OF_RANGE_LOW}, + {100.0, -2, RNDFND_STATUS_OUT_OF_RANGE_HIGH}, + {0.0, -2, RNDFND_STATUS_OUT_OF_RANGE_LOW}, +} + +-- Record the start time so we can timeout if initialization takes too long. +local time_start_ms = millis():tofloat() +local test_idx = 0 + + +-- Called when tests are completed. +local function complete(error_str) + -- Send a message indicating the success or failure of the test + local status_str + if not error_str or #error_str == 0 then + status_str = SUCCESS_STR + else + send(error_str) + status_str = FAILURE_STR + end + send(string.format("%s: %s", COMPLETE_STR, status_str)) + + -- Returning nil will not queue an update routine so the test will stop running. +end + + +-- A state machine of update functions. The states progress: +-- prepare, wait, begin_test, eval_test, begin_test, eval_test, ... complete + +local update_prepare +local update_wait +local update_begin_test +local update_eval_test + +local function _update_prepare() + if Parameter('RNGFND1_TYPE'):get() ~= RNGFND_TYPE_LUA then + return complete("LUA range finder driver not enabled") + end + if rangefinder:num_sensors() < 1 then + return complete("LUA range finder driver not connected") + end + rngfnd_backend = rangefinder:get_backend(0) + if not rngfnd_backend then + return complete("Range Finder 1 does not exist") + end + if (rngfnd_backend:type() ~= RNGFND_TYPE_LUA) then + return complete("Range Finder 1 is not a LUA driver") + end + + return update_wait() +end + +local function _update_wait() + -- Check for timeout while initializing + if millis():tofloat() - time_start_ms > TIMEOUT_MS then + return complete("Timeout while trying to initialize") + end + + -- Wait until the prearm check passes. This ensures that the system is mostly initialized + -- before starting the tests. + if not arming:pre_arm_checks() then + return update_wait, UPDATE_PERIOD_MS + end + + -- Do some one time tests + local error_str = do_status_tests() + if error_str then + return complete(error_str) + end + + -- Continue on to the main list of tests. + return update_begin_test() +end + +local function _update_begin_test() + test_idx = test_idx + 1 + if test_idx > #test_data then + return complete() + end + + local dist_m_in = test_data[test_idx][1] + local signal_quality_pct_in = test_data[test_idx][2] + + -- L U A I N T E R F A C E T E S T + -- Use the driver interface to simulate a data measurement being received and being passed to AP. + local result + -- -2 => use legacy interface + if signal_quality_pct_in == -2 then + result = rngfnd_backend:handle_script_msg(dist_m_in) -- number as arg (compatibility mode) + + else + -- The full state udata must be initialized. + local rf_state = RangeFinder_State() + -- Set the status + if dist_m_in < RNGFND1_MIN_CM * 0.01 then + rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_LOW) + elseif dist_m_in > RNGFND1_MAX_CM * 0.01 then + rf_state:status(RNDFND_STATUS_OUT_OF_RANGE_HIGH) + else + rf_state:status(RNDFND_STATUS_GOOD) + end + -- Sanitize signal_quality_pct_in + if signal_quality_pct_in < SIGNAL_QUALITY_MIN or signal_quality_pct_in > SIGNAL_QUALITY_MAX then + signal_quality_pct_in = SIGNAL_QUALITY_UNKNOWN + end + rf_state:last_reading(millis():toint()) + rf_state:range_valid_count(10) + rf_state:distance(dist_m_in) + rf_state:signal_quality(signal_quality_pct_in) + rf_state:voltage(0) + result = rngfnd_backend:handle_script_msg(rf_state) -- state as arg + end + + if not result then + return complete(string.format("Test %i, dist_m: %.2f, quality_pct: %3i failed to handle_script_msg2", + test_idx, dist_m_in, signal_quality_pct_in)) + end + + return update_eval_test, UPDATE_PERIOD_MS +end + +local function _update_eval_test() + local dist_m_in = test_data[test_idx][1] + local signal_quality_pct_in = test_data[test_idx][2] + local status_expected = test_data[test_idx][3] + + -- Use the client interface to get distance data and ensure it matches the distance data + -- that was sent through the driver interface. + local error_str = get_and_eval(test_idx, dist_m_in, signal_quality_pct_in, status_expected) + if error_str then + return complete(string.format("Test %i, dist_m: %.2f, quality_pct: %3i failed because %s", + test_idx, dist_m_in, signal_quality_pct_in, error_str)) + end + + -- Move to the next test in the list. + return update_begin_test() +end + +update_prepare = _update_prepare +update_wait = _update_wait +update_begin_test = _update_begin_test +update_eval_test = _update_eval_test + +send("Loaded rangefinder_quality_test.lua") + +return update_prepare, 0 diff --git a/libraries/AP_Scripting/examples/test_load.lua b/libraries/AP_Scripting/examples/test_load.lua index 0178cf0ed9708e..f09d6a48274036 100644 --- a/libraries/AP_Scripting/examples/test_load.lua +++ b/libraries/AP_Scripting/examples/test_load.lua @@ -7,7 +7,6 @@ gcs:send_text(0,"Testing load() method") -- a function written as a string. This could come from a file -- or any other source (eg. mavlink) -- Note that the [[ xxx ]] syntax is just a multi-line string --- luacheck: only 0 local func_str = [[ function TestFunc(x,y) @@ -23,9 +22,9 @@ function test_load() return end -- run the code within a protected call to catch any errors - local success, err = pcall(f) + local success, err2 = pcall(f) if not success then - gcs:send_text(0, string.format("Failed to load TestFunc: %s", err)) + gcs:send_text(0, string.format("Failed to load TestFunc: %s", err2)) return end diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index a37476a5252d93..f8860dce6d707b 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -38,6 +38,8 @@ singleton AP_AHRS method get_gyro Vector3f singleton AP_AHRS method get_accel Vector3f singleton AP_AHRS method get_hagl boolean float'Null singleton AP_AHRS method wind_estimate Vector3f +singleton AP_AHRS method wind_alignment float'skip_check float'skip_check +singleton AP_AHRS method head_wind float'skip_check singleton AP_AHRS method groundspeed_vector Vector2f singleton AP_AHRS method get_velocity_NED boolean Vector3f'Null singleton AP_AHRS method get_relative_position_NED_home boolean Vector3f'Null @@ -74,8 +76,21 @@ singleton AP_Arming method set_aux_auth_failed void uint8_t'skip_check string include AP_BattMonitor/AP_BattMonitor.h +include AP_BattMonitor/AP_BattMonitor_Scripting.h +userdata BattMonitorScript_State depends AP_BATTERY_SCRIPTING_ENABLED == 1 +userdata BattMonitorScript_State field healthy boolean write +userdata BattMonitorScript_State field voltage float'skip_check write +userdata BattMonitorScript_State field cell_count uint8_t'skip_check write +userdata BattMonitorScript_State field capacity_remaining_pct uint8_t'skip_check write +userdata BattMonitorScript_State field cell_voltages'array int(ARRAY_SIZE(ud->cell_voltages)) uint16_t'skip_check write +userdata BattMonitorScript_State field cycle_count uint16_t'skip_check write +userdata BattMonitorScript_State field current_amps float'skip_check write +userdata BattMonitorScript_State field consumed_mah float'skip_check write +userdata BattMonitorScript_State field consumed_wh float'skip_check write +userdata BattMonitorScript_State field temperature float'skip_check write + singleton AP_BattMonitor rename battery -singleton AP_BattMonitor depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY)) +singleton AP_BattMonitor depends AP_BATTERY_ENABLED singleton AP_BattMonitor method num_instances uint8_t singleton AP_BattMonitor method healthy boolean uint8_t 0 ud->num_instances() singleton AP_BattMonitor method voltage float uint8_t 0 ud->num_instances() @@ -91,6 +106,7 @@ singleton AP_BattMonitor method get_temperature boolean float'Null uint8_t 0 ud- singleton AP_BattMonitor method get_cycle_count boolean uint8_t 0 ud->num_instances() uint16_t'Null singleton AP_BattMonitor method reset_remaining boolean uint8_t 0 ud->num_instances() float 0 100 singleton AP_BattMonitor method get_cell_voltage boolean uint8_t'skip_check uint8_t'skip_check float'Null +singleton AP_BattMonitor method handle_scripting boolean uint8_t'skip_check BattMonitorScript_State include AP_GPS/AP_GPS.h @@ -175,6 +191,10 @@ singleton AP_Notify depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH singleton AP_Notify method play_tune void string singleton AP_Notify method handle_rgb void uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check singleton AP_Notify method handle_rgb_id void uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check +singleton AP_Notify method send_text_scripting void string uint8_t'skip_check +singleton AP_Notify method send_text_scripting rename send_text +singleton AP_Notify method release_text_scripting void uint8_t'skip_check +singleton AP_Notify method release_text_scripting rename release_text include AP_Proximity/AP_Proximity.h include AP_Proximity/AP_Proximity_Backend.h @@ -198,18 +218,35 @@ singleton AP_Proximity method get_backend AP_Proximity_Backend uint8_t'skip_chec include AP_RangeFinder/AP_RangeFinder.h include AP_RangeFinder/AP_RangeFinder_Backend.h +userdata RangeFinder::RangeFinder_State depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) +userdata RangeFinder::RangeFinder_State rename RangeFinder_State +userdata RangeFinder::RangeFinder_State field last_reading_ms uint32_t'skip_check read write +userdata RangeFinder::RangeFinder_State field last_reading_ms rename last_reading +userdata RangeFinder::RangeFinder_State field status RangeFinder::Status'enum read write RangeFinder::Status::NotConnected RangeFinder::Status::Good +userdata RangeFinder::RangeFinder_State field range_valid_count uint8_t'skip_check read write +userdata RangeFinder::RangeFinder_State field distance_m float'skip_check read write +userdata RangeFinder::RangeFinder_State field distance_m rename distance +userdata RangeFinder::RangeFinder_State field signal_quality_pct int8_t'skip_check read write +userdata RangeFinder::RangeFinder_State field signal_quality_pct rename signal_quality +userdata RangeFinder::RangeFinder_State field voltage_mv uint16_t'skip_check read write +userdata RangeFinder::RangeFinder_State field voltage_mv rename voltage + ap_object AP_RangeFinder_Backend depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) ap_object AP_RangeFinder_Backend method distance float +ap_object AP_RangeFinder_Backend method signal_quality_pct float +ap_object AP_RangeFinder_Backend method signal_quality_pct rename signal_quality ap_object AP_RangeFinder_Backend method orientation Rotation'enum ap_object AP_RangeFinder_Backend method type uint8_t ap_object AP_RangeFinder_Backend method status uint8_t -ap_object AP_RangeFinder_Backend method handle_script_msg boolean float'skip_check +ap_object AP_RangeFinder_Backend manual handle_script_msg lua_range_finder_handle_script_msg 1 +ap_object AP_RangeFinder_Backend method get_state void RangeFinder::RangeFinder_State'Ref singleton RangeFinder depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) singleton RangeFinder rename rangefinder singleton RangeFinder method num_sensors uint8_t singleton RangeFinder method has_orientation boolean Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 +singleton RangeFinder method signal_quality_pct_orient int8_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method max_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method min_distance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 singleton RangeFinder method ground_clearance_cm_orient uint16_t Rotation'enum ROTATION_NONE ROTATION_MAX-1 @@ -354,9 +391,9 @@ singleton RC_Channels method get_aux_cached boolean RC_Channel::AUX_FUNC'enum 0 include AP_SerialManager/AP_SerialManager.h -ap_object AP_HAL::UARTDriver depends HAL_GCS_ENABLED ap_object AP_HAL::UARTDriver method begin void uint32_t 1U UINT32_MAX ap_object AP_HAL::UARTDriver method read int16_t +ap_object AP_HAL::UARTDriver manual readstring AP_HAL__UARTDriver_readstring 1 ap_object AP_HAL::UARTDriver method write uint32_t uint8_t'skip_check ap_object AP_HAL::UARTDriver method available uint32_t ap_object AP_HAL::UARTDriver method set_flow_control void AP_HAL::UARTDriver::flow_control'enum AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE AP_HAL::UARTDriver::FLOW_CONTROL_AUTO @@ -451,6 +488,8 @@ singleton AP_Mission method cmd_has_location boolean uint16_t'skip_check singleton AP_Mission method jump_to_tag boolean uint16_t 0 UINT16_MAX singleton AP_Mission method get_index_of_jump_tag uint16_t uint16_t 0 UINT16_MAX singleton AP_Mission method get_last_jump_tag boolean uint16_t'Null uint16_t'Null +singleton AP_Mission method jump_to_landing_sequence boolean +singleton AP_Mission method jump_to_abort_landing_sequence boolean userdata mavlink_mission_item_int_t depends AP_MISSION_ENABLED @@ -536,6 +575,23 @@ ap_object AP_HAL::I2CDevice method write_register boolean uint8_t'skip_check uin ap_object AP_HAL::I2CDevice manual read_registers AP_HAL__I2CDevice_read_registers 2 ap_object AP_HAL::I2CDevice method set_address void uint8_t'skip_check +include AP_HAL/utility/Socket.h depends (AP_NETWORKING_ENABLED==1) +global manual Socket lua_get_SocketAPM 1 depends (AP_NETWORKING_ENABLED==1) + +ap_object SocketAPM depends (AP_NETWORKING_ENABLED==1) +ap_object SocketAPM method connect boolean string uint16_t'skip_check +ap_object SocketAPM method bind boolean string uint16_t'skip_check +ap_object SocketAPM method send int32_t string uint32_t'skip_check +ap_object SocketAPM method listen boolean uint8_t'skip_check +ap_object SocketAPM method set_blocking boolean boolean +ap_object SocketAPM method is_connected boolean +ap_object SocketAPM method pollout boolean uint32_t'skip_check +ap_object SocketAPM method pollin boolean uint32_t'skip_check +ap_object SocketAPM method reuseaddress boolean +ap_object SocketAPM manual sendfile SocketAPM_sendfile 1 +ap_object SocketAPM manual close SocketAPM_close 0 +ap_object SocketAPM manual recv SocketAPM_recv 1 +ap_object SocketAPM manual accept SocketAPM_accept 1 ap_object AP_HAL::AnalogSource depends !defined(HAL_DISABLE_ADC_DRIVER) ap_object AP_HAL::AnalogSource method set_pin boolean uint8_t'skip_check @@ -581,6 +637,9 @@ singleton AP_InertialSensor rename ins singleton AP_InertialSensor method get_temperature float uint8_t 0 INS_MAX_INSTANCES singleton AP_InertialSensor method get_gyro_health boolean uint8_t'skip_check singleton AP_InertialSensor method get_accel_health boolean uint8_t'skip_check +singleton AP_InertialSensor method calibrating boolean +singleton AP_InertialSensor method get_gyro Vector3f uint8_t'skip_check +singleton AP_InertialSensor method get_accel Vector3f uint8_t'skip_check singleton CAN manual get_device lua_get_CAN_device 1 singleton CAN manual get_device2 lua_get_CAN_device2 1 @@ -602,7 +661,7 @@ userdata AP_HAL::CANFrame method isErrorFrame boolean ap_object ScriptingCANBuffer depends AP_SCRIPTING_CAN_SENSOR_ENABLED ap_object ScriptingCANBuffer method write_frame boolean AP_HAL::CANFrame uint32_t'skip_check ap_object ScriptingCANBuffer method read_frame boolean AP_HAL::CANFrame'Null - +ap_object ScriptingCANBuffer method add_filter boolean uint32_t'skip_check uint32_t'skip_check include ../Tools/AP_Periph/AP_Periph.h depends defined(HAL_BUILD_AP_PERIPH) singleton AP_Periph_FW depends defined(HAL_BUILD_AP_PERIPH) @@ -610,6 +669,7 @@ singleton AP_Periph_FW rename periph singleton AP_Periph_FW method get_yaw_earth float singleton AP_Periph_FW method get_vehicle_state uint32_t singleton AP_Periph_FW method can_printf void "%s"'literal string +singleton AP_Periph_FW method reboot void boolean include AP_Motors/AP_Motors_Class.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI singleton AP::motors() depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_COPTER_OR_HELI @@ -814,3 +874,35 @@ singleton AC_Fence depends AP_FENCE_ENABLED singleton AC_Fence rename fence singleton AC_Fence method get_breaches uint8_t singleton AC_Fence method get_breach_time uint32_t + +include AP_Filesystem/AP_Filesystem.h depends AP_FILESYSTEM_FILE_READING_ENABLED +include AP_Filesystem/AP_Filesystem_config.h +userdata AP_Filesystem::stat_t depends AP_FILESYSTEM_FILE_READING_ENABLED +userdata AP_Filesystem::stat_t rename stat_t +userdata AP_Filesystem::stat_t field size uint32_t read +userdata AP_Filesystem::stat_t field mode int32_t read +userdata AP_Filesystem::stat_t field mtime uint32_t read +userdata AP_Filesystem::stat_t field atime uint32_t read +userdata AP_Filesystem::stat_t field ctime uint32_t read +userdata AP_Filesystem::stat_t method is_directory boolean + +singleton AP_Filesystem rename fs +singleton AP_Filesystem method stat boolean string AP_Filesystem::stat_t'Null +singleton AP_Filesystem method format boolean +singleton AP_Filesystem method get_format_status uint8_t'skip_check + +include AP_RTC/AP_RTC.h depends AP_RTC_ENABLED +include AP_RTC/AP_RTC_config.h +singleton AP_RTC depends AP_RTC_ENABLED +singleton AP_RTC rename rtc +singleton AP_RTC method clock_s_to_date_fields boolean uint32_t'skip_check uint16_t'Null uint8_t'Null uint8_t'Null uint8_t'Null uint8_t'Null uint8_t'Null uint8_t'Null +singleton AP_RTC method date_fields_to_clock_s uint32_t uint16_t'skip_check int8_t'skip_check uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check uint8_t'skip_check + +include AP_Networking/AP_Networking.h depends AP_NETWORKING_ENABLED +include AP_Networking/AP_Networking_Config.h +singleton AP_Networking depends AP_NETWORKING_ENABLED +singleton AP_Networking rename networking +singleton AP_Networking method get_ip_active uint32_t +singleton AP_Networking method get_netmask_active uint32_t +singleton AP_Networking method get_gateway_active uint32_t +singleton AP_Networking method address_to_str string uint32_t'skip_check diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index 1ff491ed740785..ffa3e349fe00b9 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -4,6 +4,10 @@ #include #include +#include +#if AP_NETWORKING_ENABLED +#include +#endif #include #include @@ -15,6 +19,8 @@ #include #include +#include "lua/src/lauxlib.h" + extern const AP_HAL::HAL& hal; extern "C" { @@ -632,6 +638,30 @@ int AP_HAL__I2CDevice_read_registers(lua_State *L) { return success; } +int AP_HAL__UARTDriver_readstring(lua_State *L) { + binding_argcheck(L, 2); + + AP_HAL::UARTDriver * ud = *check_AP_HAL__UARTDriver(L, 1); + + const uint16_t count = get_uint16_t(L, 2); + uint8_t *data = (uint8_t*)malloc(count); + if (data == nullptr) { + return 0; + } + + const auto ret = ud->read(data, count); + if (ret < 0) { + free(data); + return 0; + } + + // push to lua string + lua_pushlstring(L, (const char *)data, ret); + free(data); + + return 1; +} + #if AP_SCRIPTING_CAN_SENSOR_ENABLED int lua_get_CAN_device(lua_State *L) { @@ -755,6 +785,134 @@ int lua_get_PWMSource(lua_State *L) { return 1; } +#if AP_NETWORKING_ENABLED +/* + allocate a SocketAPM + */ +int lua_get_SocketAPM(lua_State *L) { + binding_argcheck(L, 1); + const uint8_t datagram = get_uint8_t(L, 1); + auto *scripting = AP::scripting(); + + auto *sock = new SocketAPM(datagram); + if (sock == nullptr) { + return luaL_argerror(L, 1, "SocketAPM device nullptr"); + } + for (uint8_t i=0; i_net_sockets[i] == nullptr) { + scripting->_net_sockets[i] = sock; + new_SocketAPM(L); + *((SocketAPM**)luaL_checkudata(L, -1, "SocketAPM")) = scripting->_net_sockets[i]; + return 1; + } + } + + return luaL_argerror(L, 1, "no sockets available"); +} + +/* + socket close + */ +int SocketAPM_close(lua_State *L) { + binding_argcheck(L, 1); + + SocketAPM *ud = *check_SocketAPM(L, 1); + + auto *scripting = AP::scripting(); + + // clear allocated socket + for (uint8_t i=0; i_net_sockets[i] == ud) { + ud->close(); + delete ud; + scripting->_net_sockets[i] = nullptr; + break; + } + } + + return 0; +} + +/* + socket sendfile, for offloading file send to AP_Networking + */ +int SocketAPM_sendfile(lua_State *L) { + binding_argcheck(L, 2); + + SocketAPM *ud = *check_SocketAPM(L, 1); + + auto *p = (luaL_Stream *)luaL_checkudata(L, 2, LUA_FILEHANDLE); + int fd = p->f->fd; + + bool ret = fd != -1 && AP::network().sendfile(ud, fd); + if (ret) { + // the fd is no longer valid. The lua script must + // still call close() to release the memory from the + // socket + p->f->fd = -1; + } + + lua_pushboolean(L, ret); + return 1; +} + +/* + receive from a socket to a lua string + */ +int SocketAPM_recv(lua_State *L) { + binding_argcheck(L, 2); + + SocketAPM * ud = *check_SocketAPM(L, 1); + + const uint16_t count = get_uint16_t(L, 2); + uint8_t *data = (uint8_t*)malloc(count); + if (data == nullptr) { + return 0; + } + + const auto ret = ud->recv(data, count, 0); + if (ret < 0) { + free(data); + return 0; + } + + // push to lua string + lua_pushlstring(L, (const char *)data, ret); + free(data); + + return 1; +} + +/* + TCP socket accept() call + */ +int SocketAPM_accept(lua_State *L) { + binding_argcheck(L, 1); + + SocketAPM * ud = *check_SocketAPM(L, 1); + + auto *scripting = AP::scripting(); + + // find an empty slot + for (uint8_t i=0; i_net_sockets[i] == nullptr) { + scripting->_net_sockets[i] = ud->accept(0); + if (scripting->_net_sockets[i] == nullptr) { + return 0; + } + new_SocketAPM(L); + *((SocketAPM**)luaL_checkudata(L, -1, "SocketAPM")) = scripting->_net_sockets[i]; + return 1; + } + } + + // out of socket slots, return nil, caller can retry + return 0; +} + +#endif // AP_NETWORKING_ENABLED + + int lua_get_current_ref() { auto *scripting = AP::scripting(); @@ -771,4 +929,30 @@ int lua_print(lua_State *L) { return 0; } +#if (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_RANGEFINDER)) +int lua_range_finder_handle_script_msg(lua_State *L) { + // Arg 1 => self (an instance of rangefinder_backend) + // Arg 2 => a float distance or a RangeFinder_State user data + binding_argcheck(L, 2); + + // check_AP_RangeFinder_Backend aborts if not found. No need to check for null + AP_RangeFinder_Backend * ud = *check_AP_RangeFinder_Backend(L, 1); + + bool result = false; + + // Check to see if the first argument is the state structure. + const void *state_arg = luaL_testudata(L, 2, "RangeFinder_State"); + if (state_arg != nullptr) { + result = ud->handle_script_msg(*static_cast(state_arg)); + + } else { + // Otherwise assume the argument is a number and set the measurement. + result = ud->handle_script_msg(luaL_checknumber(L, 2)); + } + + lua_pushboolean(L, result); + return 1; +} +#endif + #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index 8eef08bca35ac9..add40faa0a7bd8 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -8,15 +8,22 @@ int lua_mission_receive(lua_State *L); int AP_Logger_Write(lua_State *L); int lua_get_i2c_device(lua_State *L); int AP_HAL__I2CDevice_read_registers(lua_State *L); +int AP_HAL__UARTDriver_readstring(lua_State *L); int lua_get_CAN_device(lua_State *L); int lua_get_CAN_device2(lua_State *L); int lua_dirlist(lua_State *L); int lua_removefile(lua_State *L); int SRV_Channels_get_safety_state(lua_State *L); int lua_get_PWMSource(lua_State *L); +int lua_get_SocketAPM(lua_State *L); +int SocketAPM_recv(lua_State *L); +int SocketAPM_accept(lua_State *L); +int SocketAPM_close(lua_State *L); +int SocketAPM_sendfile(lua_State *L); int lua_mavlink_init(lua_State *L); int lua_mavlink_receive_chan(lua_State *L); int lua_mavlink_register_rx_msgid(lua_State *L); int lua_mavlink_send_chan(lua_State *L); int lua_mavlink_block_command(lua_State *L); int lua_print(lua_State *L); +int lua_range_finder_handle_script_msg(lua_State *L); diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp index d1c8badff71d95..b1b60a29c8a711 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/libraries/AP_Scripting/lua_scripts.cpp @@ -402,19 +402,18 @@ void lua_scripts::remove_script(lua_State *L, script_info *script) { } } + { + // Remove from running checksum + WITH_SEMAPHORE(crc_sem); + running_checksum ^= script->crc; + } + if (L != nullptr) { // state could be null if we are force killing all scripts luaL_unref(L, LUA_REGISTRYINDEX, script->lua_ref); } _heap.deallocate(script->name); _heap.deallocate(script); - - { - // Remove from running checksum - WITH_SEMAPHORE(crc_sem); - running_checksum ^= script->crc; - } - } void lua_scripts::reschedule_script(script_info *script) { diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 5b961da0722eac..3d2ed06f5a061d 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -28,37 +28,6 @@ #include "AP_SerialManager.h" #include -#ifndef HAL_HAVE_SERIAL0 -#define HAL_HAVE_SERIAL0 HAL_NUM_SERIAL_PORTS > 0 -#endif -#ifndef HAL_HAVE_SERIAL1 -#define HAL_HAVE_SERIAL1 HAL_NUM_SERIAL_PORTS > 1 -#endif -#ifndef HAL_HAVE_SERIAL2 -#define HAL_HAVE_SERIAL2 HAL_NUM_SERIAL_PORTS > 2 -#endif -#ifndef HAL_HAVE_SERIAL3 -#define HAL_HAVE_SERIAL3 HAL_NUM_SERIAL_PORTS > 3 -#endif -#ifndef HAL_HAVE_SERIAL4 -#define HAL_HAVE_SERIAL4 HAL_NUM_SERIAL_PORTS > 4 -#endif -#ifndef HAL_HAVE_SERIAL5 -#define HAL_HAVE_SERIAL5 HAL_NUM_SERIAL_PORTS > 5 -#endif -#ifndef HAL_HAVE_SERIAL6 -#define HAL_HAVE_SERIAL6 HAL_NUM_SERIAL_PORTS > 6 -#endif -#ifndef HAL_HAVE_SERIAL7 -#define HAL_HAVE_SERIAL7 HAL_NUM_SERIAL_PORTS > 7 -#endif -#ifndef HAL_HAVE_SERIAL8 -#define HAL_HAVE_SERIAL8 HAL_NUM_SERIAL_PORTS > 8 -#endif -#ifndef HAL_HAVE_SERIAL9 -#define HAL_HAVE_SERIAL9 HAL_NUM_SERIAL_PORTS > 9 -#endif - extern const AP_HAL::HAL& hal; #ifndef DEFAULT_SERIAL0_PROTOCOL @@ -611,7 +580,14 @@ void AP_SerialManager::init() uart->set_unbuffered_writes(true); break; #endif - +#if AP_NETWORKING_BACKEND_PPP + case SerialProtocol_PPP: + uart->begin(state[i].baudrate(), + AP_SERIALMANAGER_PPP_BUFSIZE_RX, + AP_SERIALMANAGER_PPP_BUFSIZE_TX); + break; +#endif + default: uart->begin(state[i].baudrate()); } diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index b548656297f91c..d569d1f066d43a 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -80,6 +80,8 @@ class AP_SerialManager { SerialProtocol_Tramp = 44, SerialProtocol_DDS_XRCE = 45, SerialProtocol_IMUOUT = 46, + // Reserving Serial Protocol 47 for SerialProtocol_IQ + SerialProtocol_PPP = 48, SerialProtocol_NumProtocols // must be the last value }; diff --git a/libraries/AP_SerialManager/AP_SerialManager_config.h b/libraries/AP_SerialManager/AP_SerialManager_config.h index 801b52a163ae0a..a09a2995a09f2e 100644 --- a/libraries/AP_SerialManager/AP_SerialManager_config.h +++ b/libraries/AP_SerialManager/AP_SerialManager_config.h @@ -27,8 +27,7 @@ #if HAL_UART_NUM_SERIAL_PORTS >= 4 #define SERIALMANAGER_NUM_PORTS HAL_UART_NUM_SERIAL_PORTS #else -// we need a minimum of 4 to allow for a GPS due to the odd ordering -// of hal.uartB as SERIAL3 +// we want a minimum of 4 as the default GPS port is SERIAL3 #define SERIALMANAGER_NUM_PORTS 4 #endif #else @@ -137,3 +136,39 @@ #define AP_SERIALMANAGER_IMUOUT_BAUD 921600 #define AP_SERIALMANAGER_IMUOUT_BUFSIZE_RX 128 #define AP_SERIALMANAGER_IMUOUT_BUFSIZE_TX 2048 + +// PPP protocol +#define AP_SERIALMANAGER_PPP_BAUD 921600 +#define AP_SERIALMANAGER_PPP_BUFSIZE_RX 4096 +#define AP_SERIALMANAGER_PPP_BUFSIZE_TX 4096 + +#ifndef HAL_HAVE_SERIAL0 +#define HAL_HAVE_SERIAL0 HAL_NUM_SERIAL_PORTS > 0 +#endif +#ifndef HAL_HAVE_SERIAL1 +#define HAL_HAVE_SERIAL1 HAL_NUM_SERIAL_PORTS > 1 +#endif +#ifndef HAL_HAVE_SERIAL2 +#define HAL_HAVE_SERIAL2 HAL_NUM_SERIAL_PORTS > 2 +#endif +#ifndef HAL_HAVE_SERIAL3 +#define HAL_HAVE_SERIAL3 HAL_NUM_SERIAL_PORTS > 3 +#endif +#ifndef HAL_HAVE_SERIAL4 +#define HAL_HAVE_SERIAL4 HAL_NUM_SERIAL_PORTS > 4 +#endif +#ifndef HAL_HAVE_SERIAL5 +#define HAL_HAVE_SERIAL5 HAL_NUM_SERIAL_PORTS > 5 +#endif +#ifndef HAL_HAVE_SERIAL6 +#define HAL_HAVE_SERIAL6 HAL_NUM_SERIAL_PORTS > 6 +#endif +#ifndef HAL_HAVE_SERIAL7 +#define HAL_HAVE_SERIAL7 HAL_NUM_SERIAL_PORTS > 7 +#endif +#ifndef HAL_HAVE_SERIAL8 +#define HAL_HAVE_SERIAL8 HAL_NUM_SERIAL_PORTS > 8 +#endif +#ifndef HAL_HAVE_SERIAL9 +#define HAL_HAVE_SERIAL9 HAL_NUM_SERIAL_PORTS > 9 +#endif diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp index 049e7506b6aba0..173d12c8f4079c 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.cpp +++ b/libraries/AP_SmartRTL/AP_SmartRTL.cpp @@ -118,7 +118,7 @@ void AP_SmartRTL::init() // check if memory allocation failed if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) { log_action(SRTL_DEACTIVATED_INIT_FAILED); - gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed"); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed"); free(_path); free(_prune.loops); free(_simplify.stack); @@ -390,7 +390,7 @@ void AP_SmartRTL::run_background_cleanup() // warn if buffer is about to be filled uint32_t now_ms = AP_HAL::millis(); if ((path_points_count >0) && (path_points_count >= _path_points_max - 9) && (now_ms - _last_low_space_notify_ms > 10000)) { - gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL Low on space!"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SmartRTL Low on space!"); _last_low_space_notify_ms = now_ms; } @@ -866,9 +866,10 @@ void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason) { _active = false; log_action(action); - gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL deactivated: %s", reason); + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: %s", reason); } +#if HAL_LOGGING_ENABLED // logging void AP_SmartRTL::log_action(SRTL_Actions action, const Vector3f &point) const { @@ -876,6 +877,7 @@ void AP_SmartRTL::log_action(SRTL_Actions action, const Vector3f &point) const AP::logger().Write_SRTL(_active, _path_points_count, _path_points_max, action, point); } } +#endif // returns true if the two loops overlap (used within add_loop to determine which loops to keep or throw away) bool AP_SmartRTL::loops_overlap(const prune_loop_t &loop1, const prune_loop_t &loop2) const diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h index a83cad692a0344..144762e512839d 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.h +++ b/libraries/AP_SmartRTL/AP_SmartRTL.h @@ -3,6 +3,7 @@ #include #include #include +#include // definitions and macros #define SMARTRTL_ACCURACY_DEFAULT 2.0f // default _ACCURACY parameter value. Points will be no closer than this distance (in meters) together. @@ -172,8 +173,12 @@ class AP_SmartRTL { // de-activate SmartRTL, send warning to GCS and logger void deactivate(SRTL_Actions action, const char *reason); +#if HAL_LOGGING_ENABLED // logging void log_action(SRTL_Actions action, const Vector3f &point = Vector3f()) const; +#else + void log_action(SRTL_Actions action, const Vector3f &point = Vector3f()) const {} +#endif // parameters AP_Float _accuracy; diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 53e61652848043..bb51cd3948a10f 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -144,14 +144,14 @@ const AP_Param::GroupInfo SoaringController::var_info[] = { // @Param: THML_ARSPD // @DisplayName: Specific setting for airspeed when soaring in THERMAL mode. - // @Description: If non-zero this airspeed will be used when thermalling. A value of 0 will use TRIM_ARSPD_CM. + // @Description: If non-zero this airspeed will be used when thermalling. A value of 0 will use AIRSPEED_CRUISE. // @Range: 0 50 // @User: Advanced AP_GROUPINFO("THML_ARSPD", 20, SoaringController, soar_thermal_airspeed, 0), // @Param: CRSE_ARSPD // @DisplayName: Specific setting for airspeed when soaring in AUTO mode. - // @Description: If non-zero this airspeed will be used when cruising between thermals in AUTO. If set to -1, airspeed will be selected based on speed-to-fly theory. If set to 0, then TRIM_ARSPD_CM will be used while cruising between thermals. + // @Description: If non-zero this airspeed will be used when cruising between thermals in AUTO. If set to -1, airspeed will be selected based on speed-to-fly theory. If set to 0, then AIRSPEED_CRUISE will be used while cruising between thermals. // @Range: -1 50 // @User: Advanced AP_GROUPINFO("CRSE_ARSPD", 21, SoaringController, soar_cruise_airspeed, 0), @@ -280,8 +280,8 @@ void SoaringController::init_thermalling() // New state vector filter will be reset. Thermal location is placed in front of a/c const float init_xr[4] = {_vario.get_trigger_value(), INITIAL_THERMAL_RADIUS, - position.x + thermal_distance_ahead * cosf(_ahrs.yaw), - position.y + thermal_distance_ahead * sinf(_ahrs.yaw)}; + position.x + thermal_distance_ahead * cosf(_ahrs.get_yaw()), + position.y + thermal_distance_ahead * sinf(_ahrs.get_yaw())}; const VectorN xr{init_xr}; @@ -337,6 +337,7 @@ void SoaringController::update_thermalling() _position_x_filter.apply(_ekf.X[2], deltaT); _position_y_filter.apply(_ekf.X[3], deltaT); +#if HAL_LOGGING_ENABLED // write log - save the data. // @LoggerMessage: SOAR // @Vehicles: Plane @@ -367,6 +368,7 @@ void SoaringController::update_thermalling() (double)wind_drift.x, (double)wind_drift.y, (double)_thermalability); +#endif } void SoaringController::update_cruising() @@ -388,6 +390,7 @@ void SoaringController::update_cruising() // Update the calculation. _speedToFly.update(wx, wz, thermal_vspeed, CLmin, CLmax); +#if HAL_LOGGING_ENABLED AP::logger().WriteStreaming("SORC", "TimeUS,wx,wz,wexp,CLmin,CLmax,Vopt", "Qffffff", AP_HAL::micros64(), (double)wx, @@ -396,6 +399,7 @@ void SoaringController::update_cruising() (double)CLmin, (double)CLmax, (double)_speedToFly.speed_to_fly()); +#endif } void SoaringController::update_vario() diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index a7a59f789e1623..712a6c3b4d5dcc 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -21,7 +21,7 @@ void Variometer::update(const float thermal_bank) float aspd = 0; if (!_ahrs.airspeed_estimate(aspd)) { - aspd = _aparm.airspeed_cruise_cm * 0.01f; + aspd = _aparm.airspeed_cruise; } float aspd_filt = _sp_filter.apply(aspd); @@ -62,7 +62,7 @@ void Variometer::update(const float thermal_bank) float smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, dt); // Compute still-air sinkrate - float roll = _ahrs.roll; + float roll = _ahrs.get_roll(); float sinkrate = calculate_aircraft_sinkrate(roll); reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate; @@ -79,6 +79,7 @@ void Variometer::update(const float thermal_bank) _expected_thermalling_sink = calculate_aircraft_sinkrate(radians(thermal_bank)); +#if HAL_LOGGING_ENABLED // @LoggerMessage: VAR // @Vehicles: Plane // @Description: Variometer data @@ -107,9 +108,12 @@ void Variometer::update(const float thermal_bank) (double)_expected_thermalling_sink, (double)dsp, (double)dsp_bias); +#else + (void)filtered_reading; + (void)smoothed_climb_rate; +#endif } - float Variometer::calculate_aircraft_sinkrate(float phi) const { // Remove aircraft sink rate diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index f5892d68585728..dd9c8b428e68d5 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -20,7 +20,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: CLMB_MAX // @DisplayName: Maximum Climb Rate (metres/sec) - // @Description: Maximum demanded climb rate. Do not set higher than the climb speed at THR_MAX at TRIM_ARSPD_CM when the battery is at low voltage. Reduce value if airspeed cannot be maintained on ascent. Increase value if throttle does not increase significantly to ascend. + // @Description: Maximum demanded climb rate. Do not set higher than the climb speed at THR_MAX at AIRSPEED_CRUISE when the battery is at low voltage. Reduce value if airspeed cannot be maintained on ascent. Increase value if throttle does not increase significantly to ascend. // @Increment: 0.1 // @Range: 0.1 20.0 // @User: Standard @@ -28,7 +28,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: SINK_MIN // @DisplayName: Minimum Sink Rate (metres/sec) - // @Description: Minimum sink rate when at THR_MIN and TRIM_ARSPD_CM. + // @Description: Minimum sink rate when at THR_MIN and AIRSPEED_CRUISE. // @Increment: 0.1 // @Range: 0.1 10.0 // @User: Standard @@ -108,7 +108,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: SINK_MAX // @DisplayName: Maximum Descent Rate (metres/sec) - // @Description: Maximum demanded descent rate. Do not set higher than the vertical speed the aircraft can maintain at THR_MIN, TECS_PITCH_MIN, and ARSPD_FBW_MAX. + // @Description: Maximum demanded descent rate. Do not set higher than the vertical speed the aircraft can maintain at THR_MIN, TECS_PITCH_MIN, and AIRSPEED_MAX. // @Increment: 0.1 // @Range: 0.0 20.0 // @User: Standard @@ -116,7 +116,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: LAND_ARSPD // @DisplayName: Airspeed during landing approach (m/s) - // @Description: When performing an autonomus landing, this value is used as the goal airspeed during approach. Max airspeed allowed is Trim Airspeed or ARSPD_FBW_MAX as defined by LAND_OPTIONS bitmask. Note that this parameter is not useful if your platform does not have an airspeed sensor (use TECS_LAND_THR instead). If negative then this value is halfway between ARSPD_FBW_MIN and TRIM_CRUISE_CM speed for fixed wing autolandings. + // @Description: When performing an autonomus landing, this value is used as the goal airspeed during approach. Max airspeed allowed is Trim Airspeed or AIRSPEED_MAX as defined by LAND_OPTIONS bitmask. Note that this parameter is not useful if your platform does not have an airspeed sensor (use TECS_LAND_THR instead). If negative then this value is halfway between AIRSPEED_MIN and TRIM_CRUISE_CM speed for fixed wing autolandings. // @Range: -1 127 // @Increment: 1 // @User: Standard @@ -140,7 +140,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: PITCH_MAX // @DisplayName: Maximum pitch in auto flight - // @Description: Overrides LIM_PITCH_MAX in automatic throttle modes to reduce climb rates. Uses LIM_PITCH_MAX if set to 0. For proper TECS tuning, set to the angle that the aircraft can climb at TRIM_ARSPD_CM and THR_MAX. + // @Description: Overrides PTCH_LIM_MAX_DEG in automatic throttle modes to reduce climb rates. Uses PTCH_LIM_MAX_DEG if set to 0. For proper TECS tuning, set to the angle that the aircraft can climb at AIRSPEED_CRUISE and THR_MAX. // @Range: 0 45 // @Increment: 1 // @User: Advanced @@ -148,7 +148,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: PITCH_MIN // @DisplayName: Minimum pitch in auto flight - // @Description: Overrides LIM_PITCH_MIN in automatic throttle modes to reduce descent rates. Uses LIM_PITCH_MIN if set to 0. For proper TECS tuning, set to the angle that the aircraft can descend at without overspeeding. + // @Description: Overrides PTCH_LIM_MIN_DEG in automatic throttle modes to reduce descent rates. Uses PTCH_LIM_MIN_DEG if set to 0. For proper TECS tuning, set to the angle that the aircraft can descend at without overspeeding. // @Range: -45 0 // @Increment: 1 // @User: Advanced @@ -403,7 +403,7 @@ void AP_TECS::_update_speed(float DT) // at the maximum speed const float velRateMin = 0.5f * _STEdot_min / MAX(_TAS_state, aparm.airspeed_min * EAS2TAS); _TASmax += _DT * velRateMin; - _TASmax = MAX(_TASmax, 0.01f * (float)aparm.airspeed_cruise_cm * EAS2TAS); + _TASmax = MAX(_TASmax, aparm.airspeed_cruise * EAS2TAS); } else { // wind airspeed upper limit back to parameter defined value const float velRateMax = 0.5f * _STEdot_max / MAX(_TAS_state, aparm.airspeed_min * EAS2TAS); @@ -426,7 +426,7 @@ void AP_TECS::_update_speed(float DT) // airspeed sensor data cannot be used if (!use_airspeed || !_ahrs.airspeed_estimate(_EAS)) { // If no airspeed available use average of min and max - _EAS = constrain_float(0.01f * (float)aparm.airspeed_cruise_cm.get(), (float)aparm.airspeed_min.get(), (float)aparm.airspeed_max.get()); + _EAS = constrain_float(aparm.airspeed_cruise.get(), (float)aparm.airspeed_min.get(), (float)aparm.airspeed_max.get()); } // limit the airspeed to a minimum of 3 m/s @@ -470,11 +470,19 @@ void AP_TECS::_update_speed_demand(void) // Constrain speed demand, taking into account the load factor _TAS_dem = constrain_float(_TAS_dem, _TASmin, _TASmax); + // Determine the true cruising airspeed (m/s) + const float TAScruise = aparm.airspeed_cruise * _ahrs.get_EAS2TAS(); + // calculate velocity rate limits based on physical performance limits // provision to use a different rate limit if bad descent or underspeed condition exists - // Use 50% of maximum energy rate to allow margin for total energy contgroller + // Use 50% of maximum energy rate on gain, 90% on dissipation to allow margin for total energy controller const float velRateMax = 0.5f * _STEdot_max / _TAS_state; - const float velRateMin = 0.5f * _STEdot_min / _TAS_state; + // Maximum permissible rate of deceleration value at max airspeed + const float velRateNegMax = 0.9f * _STEdot_neg_max / _TASmax; + // Maximum permissible rate of deceleration value at cruise speed + const float velRateNegCruise = 0.9f * _STEdot_min / TAScruise; + // Linear interpolation between velocity rate at cruise and max speeds, capped at those speeds + const float velRateMin = linear_interpolate(velRateNegMax, velRateNegCruise, _TAS_state, _TASmax, TAScruise); const float TAS_dem_previous = _TAS_dem_adj; // Apply rate limit @@ -780,6 +788,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) // Sum the components. _throttle_dem = _throttle_dem + _integTHR_state; +#if HAL_LOGGING_ENABLED if (AP::logger().should_log(_log_bitmask)){ AP::logger().WriteStreaming("TEC3","TimeUS,KED,PED,KEDD,PEDD,TEE,TEDE,FFT,Imin,Imax,I,Emin,Emax", "Qffffffffffff", @@ -797,6 +806,7 @@ void AP_TECS::_update_throttle_with_airspeed(void) (double)SPE_err_min, (double)SPE_err_max); } +#endif } // Constrain throttle demand and record clipping @@ -848,8 +858,8 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge) // so that the throttle mapping adjusts for the effect of pitch control errors _pitch_demand_lpf.apply(_pitch_dem, _DT); const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get(); - _pitch_measured_lpf.apply(_ahrs.pitch, _DT); - const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(0.01f * (float)aparm.pitch_trim_cd); + _pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT); + const float pitch_corrected_lpf = _pitch_measured_lpf.get(); const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf; if (pitch_blended > 0.0f && _PITCHmaxf > 0.0f) @@ -1049,6 +1059,7 @@ void AP_TECS::_update_pitch(void) _last_pitch_dem = _pitch_dem; +#if HAL_LOGGING_ENABLED if (AP::logger().should_log(_log_bitmask)){ // log to AP_Logger // @LoggerMessage: TEC2 @@ -1086,6 +1097,7 @@ void AP_TECS::_update_pitch(void) (double)_PITCHminf, (double)_PITCHmaxf); } +#endif } void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) @@ -1099,7 +1111,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _integSEBdot = 0.0f; _integKE = 0.0f; _last_throttle_dem = aparm.throttle_cruise * 0.01f; - _last_pitch_dem = _ahrs.pitch; + _last_pitch_dem = _ahrs.get_pitch(); _hgt_afe = hgt_afe; _hgt_dem_in_prev = hgt_afe; _hgt_dem_lpf = hgt_afe; @@ -1130,8 +1142,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) const float fc = 1.0f / (M_2PI * _timeConst); _pitch_demand_lpf.set_cutoff_frequency(fc); _pitch_measured_lpf.set_cutoff_frequency(fc); - _pitch_demand_lpf.reset(_ahrs.pitch); - _pitch_measured_lpf.reset(_ahrs.pitch); + _pitch_demand_lpf.reset(_ahrs.get_pitch()); + _pitch_measured_lpf.reset(_ahrs.get_pitch()); } else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { _PITCHminf = 0.000174533f * ptchMinCO_cd; @@ -1151,8 +1163,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) _max_climb_scaler = 1.0f; _max_sink_scaler = 1.0f; - _pitch_demand_lpf.reset(_ahrs.pitch); - _pitch_measured_lpf.reset(_ahrs.pitch); + _pitch_demand_lpf.reset(_ahrs.get_pitch()); + _pitch_measured_lpf.reset(_ahrs.get_pitch()); } if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { @@ -1163,10 +1175,12 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe) void AP_TECS::_update_STE_rate_lim(void) { - // Calculate Specific Total Energy Rate Limits + // Calculate Specific Total Energy Rate Limits & deceleration rate bounds + // Keep the 50% energy margin from the original velRate calculation for now // This is a trivial calculation at the moment but will get bigger once we start adding altitude effects _STEdot_max = _climb_rate_limit * GRAVITY_MSS; _STEdot_min = - _minSinkRate * GRAVITY_MSS; + _STEdot_neg_max = - _maxSinkRate * GRAVITY_MSS; } @@ -1235,15 +1249,15 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be // larger than LIM_PITCH_{MAX,MIN} if (_pitch_max == 0) { - _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f; + _PITCHmaxf = aparm.pitch_limit_max; } else { - _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); + _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max); } if (_pitch_min >= 0) { - _PITCHminf = aparm.pitch_limit_min_cd * 0.01f; + _PITCHminf = aparm.pitch_limit_min; } else { - _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f); + _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min); } // apply temporary pitch limit and clear @@ -1271,7 +1285,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, } const float pitch_limit_deg = (1.0f - p) * _pitch_min_at_flare_entry + p * 0.01f * _landing.get_pitch_cd(); - // in flare use min pitch from LAND_PITCH_CD + // in flare use min pitch from LAND_PITCH_DEG _PITCHminf = MAX(_PITCHminf, pitch_limit_deg); // and use max pitch from TECS_LAND_PMAX @@ -1283,7 +1297,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, // and allow zero throttle _THRminf = 0; } else if (_landing.is_on_approach()) { - _PITCHminf = MAX(_PITCHminf, 0.01f * aparm.pitch_limit_min_cd); + _PITCHminf = MAX(_PITCHminf, aparm.pitch_limit_min); _pitch_min_at_flare_entry = _PITCHminf; _flare_initialised = false; } else { @@ -1362,6 +1376,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, _flags.badDescent = false; } +#if HAL_LOGGING_ENABLED if (AP::logger().should_log(_log_bitmask)){ // log to AP_Logger // @LoggerMessage: TECS @@ -1404,4 +1419,5 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, (double)_TAS_rate_dem, _flags_byte); } +#endif } diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 4b83d133fa2df0..b392a0bfc555be 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -349,9 +349,10 @@ class AP_TECS { // pitch demand before limiting float _pitch_dem_unc; - // Maximum and minimum specific total energy rate limits - float _STEdot_max; - float _STEdot_min; + // Specific total energy rate limits + float _STEdot_max; // Specific total energy rate gain at cruise airspeed & THR_MAX (m/s/s) + float _STEdot_min; // Specific total energy rate loss at cruise airspeed & THR_MIN (m/s/s) + float _STEdot_neg_max; // Specific total energy rate loss at max airspeed & THR_MIN (m/s/s) // Maximum and minimum floating point throttle limits float _THRmaxf; diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp index 68ac7027abe3cb..69ef84e41a6aca 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp @@ -29,6 +29,7 @@ #include "AP_TemperatureSensor_MCP9600.h" #include "AP_TemperatureSensor_MAX31865.h" #include "AP_TemperatureSensor_Analog.h" +#include "AP_TemperatureSensor_DroneCAN.h" #include #include @@ -195,6 +196,11 @@ void AP_TemperatureSensor::init() case AP_TemperatureSensor_Params::Type::ANALOG: drivers[instance] = new AP_TemperatureSensor_Analog(*this, _state[instance], _params[instance]); break; +#endif +#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + case AP_TemperatureSensor_Params::Type::DRONECAN: + drivers[instance] = new AP_TemperatureSensor_DroneCAN(*this, _state[instance], _params[instance]); + break; #endif case AP_TemperatureSensor_Params::Type::NONE: default: diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h index 1a4bffa656ccbd..1a9538d775b5ae 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h @@ -36,6 +36,7 @@ class AP_TemperatureSensor friend class AP_TemperatureSensor_MAX31865; friend class AP_TemperatureSensor_TSYS03; friend class AP_TemperatureSensor_Analog; + friend class AP_TemperatureSensor_DroneCAN; public: diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp index 4aadb2cf49a03f..cab984c6496610 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp @@ -40,6 +40,7 @@ bool AP_TemperatureSensor_Backend::healthy(void) const return (_state.last_time_ms > 0) && (AP_HAL::millis() - _state.last_time_ms < 5000); } +#if HAL_LOGGING_ENABLED void AP_TemperatureSensor_Backend::Log_Write_TEMP() const { AP::logger().Write("TEMP", @@ -49,6 +50,7 @@ void AP_TemperatureSensor_Backend::Log_Write_TEMP() const "Q" "B" "f" , // types AP_HAL::micros64(), _state.instance, _state.temperature); } +#endif void AP_TemperatureSensor_Backend::set_temperature(const float temperature) { @@ -88,6 +90,9 @@ void AP_TemperatureSensor_Backend::update_external_libraries(const float tempera AP::battery().set_temperature_by_serial_number(temperature, _params.source_id); break; #endif + case AP_TemperatureSensor_Params::Source::DroneCAN: + // Label only, used by AP_Periph + break; case AP_TemperatureSensor_Params::Source::None: case AP_TemperatureSensor_Params::Source::Pitot_tube: diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp new file mode 100644 index 00000000000000..0092939a06566b --- /dev/null +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.cpp @@ -0,0 +1,80 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#include "AP_TemperatureSensor_config.h" + +#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + +#include "AP_TemperatureSensor_DroneCAN.h" +#include +#include + +AP_TemperatureSensor_DroneCAN* AP_TemperatureSensor_DroneCAN::_drivers[]; +uint8_t AP_TemperatureSensor_DroneCAN::_driver_instance; +HAL_Semaphore AP_TemperatureSensor_DroneCAN::_driver_sem; + +extern const AP_HAL::HAL &hal; + +const AP_Param::GroupInfo AP_TemperatureSensor_DroneCAN::var_info[] = { + + // @Param: MSG_ID + // @DisplayName: Temperature sensor DroneCAN message ID + // @Description: Sets the message device ID this backend listens for + // @Range: 0 65535 + AP_GROUPINFO("MSG_ID", 1, AP_TemperatureSensor_DroneCAN, _ID, 0), + + AP_GROUPEND +}; + +AP_TemperatureSensor_DroneCAN::AP_TemperatureSensor_DroneCAN(AP_TemperatureSensor &front, + AP_TemperatureSensor::TemperatureSensor_State &state, + AP_TemperatureSensor_Params ¶ms) : + AP_TemperatureSensor_Backend(front, state, params) +{ + AP_Param::setup_object_defaults(this, var_info); + _state.var_info = var_info; + + // Register self in static driver list + WITH_SEMAPHORE(_driver_sem); + _drivers[_driver_instance] = this; + _driver_instance++; +} + +// Subscript to incoming temperature messages +void AP_TemperatureSensor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +{ + if (ap_dronecan == nullptr) { + return; + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("temp_sub"); + } +} + +void AP_TemperatureSensor_DroneCAN::handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_device_Temperature &msg) +{ + WITH_SEMAPHORE(_driver_sem); + + for (uint8_t i = 0; i < _driver_instance; i++) { + if ((_drivers[i] != nullptr) && (_drivers[i]->_ID.get() == msg.device_id)) { + // Driver loaded and looking for this ID, set temp + _drivers[i]->set_temperature(KELVIN_TO_C(msg.temperature)); + } + } +} + +#endif // AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h new file mode 100644 index 00000000000000..fa2a3f086f0098 --- /dev/null +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_DroneCAN.h @@ -0,0 +1,52 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +#pragma once + +#include "AP_TemperatureSensor_config.h" + +#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + +#include "AP_TemperatureSensor_Backend.h" +#include + +#include + +class AP_TemperatureSensor_DroneCAN : public AP_TemperatureSensor_Backend { +public: + AP_TemperatureSensor_DroneCAN(AP_TemperatureSensor &front, AP_TemperatureSensor::TemperatureSensor_State &state, AP_TemperatureSensor_Params ¶ms); + + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + + // Don't do anything in update, but still need to override the pure virtual method. + void update(void) override {}; + + static const struct AP_Param::GroupInfo var_info[]; + +private: + + static void handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_device_Temperature &msg); + + // Static list of drivers + static AP_TemperatureSensor_DroneCAN *_drivers[AP_TEMPERATURE_SENSOR_MAX_INSTANCES]; + static uint8_t _driver_instance; + static HAL_Semaphore _driver_sem; + + // DroneCAN temperature ID to listen for + AP_Int32 _ID; + +}; + +#endif // AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp index 361eaf9c13f1ef..b1e15333d40f44 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp @@ -34,7 +34,7 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = { // @Param: TYPE // @DisplayName: Temperature Sensor Type // @Description: Enables temperature sensors - // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600, 3:MAX31865, 4:TSYS03, 5:Analog + // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600, 3:MAX31865, 4:TSYS03, 5:Analog, 6:DroneCAN // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("TYPE", 1, AP_TemperatureSensor_Params, type, (float)Type::NONE, AP_PARAM_FLAG_ENABLE), @@ -58,7 +58,7 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = { // @Param: SRC // @DisplayName: Sensor Source // @Description: Sensor Source is used to designate which device's temperature report will be replaced by this temperature sensor's data. If 0 (None) then the data is only available via log. In the future a new Motor temperature report will be created for returning data directly. - // @Values: 0: None, 1:ESC, 2:Motor(not implemented yet), 3:Battery Index, 4:Battery ID/SerialNumber, 5: CAN based Pitot tube + // @Values: 0: None, 1:ESC, 2:Motor, 3:Battery Index, 4:Battery ID/SerialNumber, 5:CAN based Pitot tube, 6:DroneCAN-out on AP_Periph // @User: Standard AP_GROUPINFO("SRC", 4, AP_TemperatureSensor_Params, source, (float)Source::None), diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h index 46d41e8c659ad7..f8c551ee5f6dc3 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h @@ -33,6 +33,7 @@ class AP_TemperatureSensor_Params { MAX31865 = 3, TSYS03 = 4, ANALOG = 5, + DRONECAN = 6, }; // option to map to another system component @@ -43,6 +44,7 @@ class AP_TemperatureSensor_Params { Battery_Index = 3, Battery_ID_SerialNumber = 4, Pitot_tube = 5, + DroneCAN = 6, }; AP_Enum type; // 0=disabled, others see frontend enum TYPE diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h index b34ac029c38678..8b7cbf59e47677 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h @@ -24,6 +24,12 @@ #define AP_TEMPERATURE_SENSOR_ANALOG_ENABLED AP_TEMPERATURE_SENSOR_ENABLED #endif +#ifndef AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED + #define AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED AP_TEMPERATURE_SENSOR_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS +#endif +#if AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED && !HAL_ENABLE_DRONECAN_DRIVERS + #error AP_TEMPERATURE_SENSOR_DRONECAN_ENABLED requires HAL_ENABLE_DRONECAN_DRIVERS +#endif // maximum number of Temperature Sensors #ifndef AP_TEMPERATURE_SENSOR_MAX_INSTANCES diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 7e9e75403662ec..d2aab8bf1d8aba 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -26,6 +26,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -439,6 +440,7 @@ bool AP_Terrain::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) cons return true; } +#if HAL_LOGGING_ENABLED void AP_Terrain::log_terrain_data() { if (!allocate()) { @@ -472,6 +474,7 @@ void AP_Terrain::log_terrain_data() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif /* allocate terrain cache. Making this dynamically allocated allows diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index ff55ce0dbbcb05..8a6be4579db06c 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -27,6 +27,7 @@ #include #include #include +#include #define TERRAIN_DEBUG 0 @@ -172,10 +173,12 @@ class AP_Terrain { */ float lookahead(float bearing, float distance, float climb_ratio); +#if HAL_LOGGING_ENABLED /* log terrain status to AP_Logger */ void log_terrain_data(); +#endif /* get some statistics for TERRAIN_REPORT diff --git a/libraries/AP_Torqeedo/AP_Torqeedo.cpp b/libraries/AP_Torqeedo/AP_Torqeedo.cpp index 1fd14eded794b3..0d4b80ec86396f 100644 --- a/libraries/AP_Torqeedo/AP_Torqeedo.cpp +++ b/libraries/AP_Torqeedo/AP_Torqeedo.cpp @@ -564,6 +564,7 @@ void AP_Torqeedo::parse_message() esc_temp, motor_temp); +#if HAL_LOGGING_ENABLED // log data if ((_options & options::LOG) != 0) { // @LoggerMessage: TRST @@ -592,6 +593,7 @@ void AP_Torqeedo::parse_message() _display_system_state.batt_voltage, _display_system_state.batt_current); } +#endif // send to GCS if ((_options & options::DEBUG_TO_GCS) != 0) { @@ -625,6 +627,7 @@ void AP_Torqeedo::parse_message() _display_system_setup.batt_type = _received_buff[9]; _display_system_setup.master_sw_version = UINT16_VALUE(_received_buff[10], _received_buff[11]); +#if HAL_LOGGING_ENABLED // log data if ((_options & options::LOG) != 0) { // @LoggerMessage: TRSE @@ -647,6 +650,7 @@ void AP_Torqeedo::parse_message() _display_system_setup.batt_type, _display_system_setup.master_sw_version); } +#endif // send to GCS if ((_options & options::DEBUG_TO_GCS) != 0) { @@ -694,6 +698,7 @@ void AP_Torqeedo::parse_message() _motor_param.pcb_temp, // esc temp _motor_param.stator_temp); // motor temp +#if HAL_LOGGING_ENABLED // log data if ((_options & options::LOG) != 0) { // @LoggerMessage: TRMP @@ -714,6 +719,7 @@ void AP_Torqeedo::parse_message() _motor_param.pcb_temp, _motor_param.stator_temp); } +#endif // send to GCS if ((_options & options::DEBUG_TO_GCS) != 0) { @@ -736,6 +742,7 @@ void AP_Torqeedo::parse_message() _motor_status.status_flags_value = _received_buff[2]; _motor_status.error_flags_value = UINT16_VALUE(_received_buff[3], _received_buff[4]); +#if HAL_LOGGING_ENABLED // log data if ((_options & options::LOG) != 0) { // @LoggerMessage: TRMS @@ -743,11 +750,12 @@ void AP_Torqeedo::parse_message() // @Field: TimeUS: Time since system startup // @Field: State: Motor status flags // @Field: Err: Motor error flags - AP::logger().Write("TRMS", "TimeUS,State,Err", "QBHH", + AP::logger().Write("TRMS", "TimeUS,State,Err", "QBH", AP_HAL::micros64(), _motor_status.status_flags_value, _motor_status.error_flags_value); } +#endif // send to GCS if ((_options & options::DEBUG_TO_GCS) != 0) { @@ -1084,6 +1092,7 @@ void AP_Torqeedo::log_TRQD(bool force_logging) const bool health = healthy(); int16_t actual_motor_speed = get_motor_speed_limited(); +#if HAL_LOGGING_ENABLED if ((_options & options::LOG) != 0) { // @LoggerMessage: TRQD // @Description: Torqeedo Status @@ -1101,6 +1110,7 @@ void AP_Torqeedo::log_TRQD(bool force_logging) _parse_success_count, _parse_error_count); } +#endif if ((_options & options::DEBUG_TO_GCS) != 0) { GCS_SEND_TEXT(MAV_SEVERITY_INFO,"TRQD h:%u dspd:%d spd:%d succ:%ld err:%ld", diff --git a/libraries/AP_Tuning/AP_Tuning.cpp b/libraries/AP_Tuning/AP_Tuning.cpp index b07aec2d8e6510..490416fdab8405 100644 --- a/libraries/AP_Tuning/AP_Tuning.cpp +++ b/libraries/AP_Tuning/AP_Tuning.cpp @@ -227,10 +227,14 @@ void AP_Tuning::check_input(uint8_t flightmode) changed = true; need_revert |= (1U << current_parm_index); set_value(current_parm, new_value); + +#if HAL_LOGGING_ENABLED Log_Write_Parameter_Tuning(new_value); +#endif } +#if HAL_LOGGING_ENABLED /* log a tuning change */ @@ -250,6 +254,7 @@ void AP_Tuning::Log_Write_Parameter_Tuning(float value) (double)value, (double)center_value); } +#endif /* save parameters in the set diff --git a/libraries/AP_Vehicle/AP_FixedWing.h b/libraries/AP_Vehicle/AP_FixedWing.h index df9bac196cab04..7aee38d5c5b35d 100644 --- a/libraries/AP_Vehicle/AP_FixedWing.h +++ b/libraries/AP_Vehicle/AP_FixedWing.h @@ -13,17 +13,16 @@ struct AP_FixedWing { AP_Int8 takeoff_throttle_max; AP_Int16 airspeed_min; AP_Int16 airspeed_max; - AP_Int32 airspeed_cruise_cm; - AP_Int32 min_gndspeed_cm; + AP_Float airspeed_cruise; + AP_Float min_groundspeed; AP_Int8 crash_detection_enable; - AP_Int16 roll_limit_cd; - AP_Int16 pitch_limit_max_cd; - AP_Int16 pitch_limit_min_cd; + AP_Float roll_limit; + AP_Float pitch_limit_max; + AP_Float pitch_limit_min; AP_Int8 autotune_level; AP_Int32 autotune_options; AP_Int8 stall_prevention; AP_Int16 loiter_radius; - AP_Int16 pitch_trim_cd; AP_Float takeoff_throttle_max_t; struct Rangefinder_State { diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 7a1595b7e5dc06..fcb78c2799e712 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -1,3 +1,7 @@ +#include "AP_Vehicle_config.h" + +#if AP_VEHICLE_ENABLED + #include "AP_Vehicle.h" #include @@ -247,6 +251,19 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { // @Path: ../Filter/AP_Filter.cpp AP_SUBGROUPINFO(filters, "FILT", 26, AP_Vehicle, AP_Filters), #endif + +#if AP_STATS_ENABLED + // @Group: STAT + // @Path: ../AP_Stats/AP_Stats.cpp + AP_SUBGROUPINFO(stats, "STAT", 27, AP_Vehicle, AP_Stats), +#endif + +#if AP_SCRIPTING_ENABLED + // @Group: SCR_ + // @Path: ../AP_Scripting/AP_Scripting.cpp + AP_SUBGROUPINFO(scripting, "SCR_", 28, AP_Vehicle, AP_Scripting), +#endif + AP_GROUPEND }; @@ -342,9 +359,24 @@ void AP_Vehicle::setup() generator.init(); #endif +#if AP_STATS_ENABLED + // initialise stats module + stats.init(); +#endif + + BoardConfig.init(); + +#if HAL_CANMANAGER_ENABLED + can_mgr.init(); +#endif + // init_ardupilot is where the vehicle does most of its initialisation. init_ardupilot(); +#if AP_SCRIPTING_ENABLED + scripting.init(); +#endif // AP_SCRIPTING_ENABLED + #if AP_AIRSPEED_ENABLED airspeed.init(); if (airspeed.enabled()) { @@ -472,7 +504,7 @@ void AP_Vehicle::loop() } const uint32_t new_internal_errors = AP::internalerror().errors(); if(_last_internal_errors != new_internal_errors) { - AP::logger().Write_Error(LogErrorSubsystem::INTERNAL_ERROR, LogErrorCode::INTERNAL_ERRORS_DETECTED); + LOGGER_WRITE_ERROR(LogErrorSubsystem::INTERNAL_ERROR, LogErrorCode::INTERNAL_ERRORS_DETECTED); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Internal Errors 0x%x", (unsigned)new_internal_errors); _last_internal_errors = new_internal_errors; } @@ -569,6 +601,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #endif #if AP_FILTER_ENABLED SCHED_TASK_CLASS(AP_Filters, &vehicle.filters, update, 1, 100, 252), +#endif +#if AP_STATS_ENABLED + SCHED_TASK_CLASS(AP_Stats, &vehicle.stats, update, 1, 100, 252), #endif SCHED_TASK(update_arming, 1, 50, 253), }; @@ -594,10 +629,12 @@ void AP_Vehicle::scheduler_delay_callback() static uint32_t last_1hz, last_50hz, last_5s; +#if HAL_LOGGING_ENABLED AP_Logger &logger = AP::logger(); // don't allow potentially expensive logging calls: logger.EnableWrites(false); +#endif const uint32_t tnow = AP_HAL::millis(); if (tnow - last_1hz > 1000) { @@ -622,7 +659,9 @@ void AP_Vehicle::scheduler_delay_callback() } } +#if HAL_LOGGING_ENABLED logger.EnableWrites(true); +#endif } // if there's been a watchdog reset, notify the world via a statustext: @@ -803,7 +842,7 @@ void AP_Vehicle::update_dynamic_notch_at_specified_rate() void AP_Vehicle::notify_no_such_mode(uint8_t mode_number) { GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No such mode %u", mode_number); - AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode_number)); + LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode_number)); } // reboot the vehicle in an orderly manner, doing various cleanups and @@ -846,6 +885,7 @@ void AP_Vehicle::reboot(bool hold_in_bootloader) #if OSD_ENABLED void AP_Vehicle::publish_osd_info() { +#if AP_MISSION_ENABLED AP_Mission *mission = AP::mission(); if (mission == nullptr) { return; @@ -868,13 +908,14 @@ void AP_Vehicle::publish_osd_info() } nav_info.wp_number = mission->get_current_nav_index(); osd->set_nav_info(nav_info); +#endif } #endif void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const { - roll = ahrs.roll; - pitch = ahrs.pitch; + roll = ahrs.get_roll(); + pitch = ahrs.get_pitch(); } #if HAL_INS_ACCELCAL_ENABLED @@ -945,10 +986,7 @@ void AP_Vehicle::one_Hz_update(void) } #if AP_SCRIPTING_ENABLED - AP_Scripting *scripting = AP_Scripting::get_singleton(); - if (scripting != nullptr) { - scripting->update(); - } + scripting.update(); #endif } @@ -1029,3 +1067,4 @@ AP_Vehicle *vehicle() }; +#endif // AP_VEHICLE_ENABLED diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 1b12f3505c3124..3e38ac9b63691c 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_Vehicle_config.h" + +#if AP_VEHICLE_ENABLED + /* this header holds a parameter structure for each vehicle type for parameters needed by multiple libraries @@ -30,6 +34,7 @@ #include #include #include +#include #include #include #include // Notify library @@ -63,6 +68,10 @@ #include #include #include +#include // statistics library +#if AP_SCRIPTING_ENABLED +#include +#endif class AP_DDS_Client; @@ -151,12 +160,15 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // returns true if the vehicle has crashed virtual bool is_crashed() const; +#if AP_EXTERNAL_CONTROL_ENABLED + // Method to control vehicle position for use by external control + virtual bool set_target_location(const Location& target_loc) { return false; } +#endif // AP_EXTERNAL_CONTROL_ENABLED #if AP_SCRIPTING_ENABLED /* methods to control vehicle for use by scripting */ virtual bool start_takeoff(float alt) { return false; } - virtual bool set_target_location(const Location& target_loc) { return false; } virtual bool set_target_pos_NED(const Vector3f& target_pos, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative, bool terrain_alt) { return false; } virtual bool set_target_posvel_NED(const Vector3f& target_pos, const Vector3f& target_vel) { return false; } virtual bool set_target_posvelaccel_NED(const Vector3f& target_pos, const Vector3f& target_vel, const Vector3f& target_accel, bool use_yaw, float yaw_deg, bool use_yaw_rate, float yaw_rate_degs, bool yaw_relative) { return false; } @@ -260,7 +272,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { */ virtual bool get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const { return false; } - // Returns roll and pitch for OSD Horizon, Plane overrides to correct for VTOL view and fixed wing TRIM_PITCH_CD + // Returns roll and pitch for OSD Horizon, Plane overrides to correct for VTOL view and fixed wing PTCH_TRIM_DEG virtual void get_osd_roll_pitch_rad(float &roll, float &pitch) const; /* @@ -301,7 +313,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { #endif RangeFinder rangefinder; +#if AP_RSSI_ENABLED AP_RSSI rssi; +#endif + #if HAL_RUNCAM_ENABLED AP_RunCam runcam; #endif @@ -377,6 +392,11 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_Airspeed airspeed; #endif +#if AP_STATS_ENABLED + // vehicle statistics + AP_Stats stats; +#endif + #if AP_AIS_ENABLED // Automatic Identification System - for tracking sea-going vehicles AP_AIS ais; @@ -398,6 +418,10 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_TemperatureSensor temperature_sensor; #endif +#if AP_SCRIPTING_ENABLED + AP_Scripting scripting; +#endif + static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Scheduler::Task scheduler_tasks[]; @@ -487,3 +511,5 @@ extern const AP_HAL::HAL& hal; extern const AP_Param::Info vehicle_var_info[]; #include "AP_Vehicle_Type.h" + +#endif // AP_VEHICLE_ENABLED diff --git a/libraries/AP_Vehicle/AP_Vehicle_config.h b/libraries/AP_Vehicle/AP_Vehicle_config.h new file mode 100644 index 00000000000000..8cdfa764a05294 --- /dev/null +++ b/libraries/AP_Vehicle/AP_Vehicle_config.h @@ -0,0 +1,5 @@ +#pragma once + +#ifndef AP_VEHICLE_ENABLED +#define AP_VEHICLE_ENABLED 1 +#endif diff --git a/libraries/AP_VideoTX/AP_VideoTX.cpp b/libraries/AP_VideoTX/AP_VideoTX.cpp index 8bfa7c14ca6462..ac57aade8319cf 100644 --- a/libraries/AP_VideoTX/AP_VideoTX.cpp +++ b/libraries/AP_VideoTX/AP_VideoTX.cpp @@ -515,7 +515,7 @@ void AP_VideoTX::change_power(int8_t position) // first find out how many possible levels there are uint8_t num_active_levels = 0; for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) { - if (_power_levels[i].active != PowerActive::Inactive) { + if (_power_levels[i].active != PowerActive::Inactive && _power_levels[i].mw <= _max_power_mw) { num_active_levels++; } } diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index b0974348e1467a..051981041d8421 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp @@ -36,6 +36,7 @@ bool AP_VisualOdom_Backend::healthy() const return ((AP_HAL::millis() - _last_update_ms) < AP_VISUALODOM_TIMEOUT_MS); } +#if HAL_GCS_ENABLED // consume vision_position_delta mavlink messages void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_message_t &msg) { @@ -55,6 +56,7 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa // send to EKF const float time_delta_sec = packet.time_delta_usec * 1.0E-6; +#if AP_AHRS_ENABLED AP::ahrs().writeBodyFrameOdom(packet.confidence, position_delta, angle_delta, @@ -62,13 +64,17 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa now_ms, _frontend.get_delay_ms(), _frontend.get_pos_offset()); +#endif +#if HAL_LOGGING_ENABLED // log sensor data Write_VisualOdom(time_delta_sec, angle_delta, position_delta, packet.confidence); +#endif } +#endif // HAL_GCS_ENABLED // returns the system time of the last reset if reset_counter has not changed // updates the reset timestamp to the current system time if the reset_counter has changed diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index b17eef76e73915..9f5225aa9e9a4d 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -18,6 +18,8 @@ #if HAL_VISUALODOM_ENABLED +#include + class AP_VisualOdom_Backend { public: @@ -27,8 +29,10 @@ class AP_VisualOdom_Backend // return true if sensor is basically healthy (we are receiving data) bool healthy() const; +#if HAL_GCS_ENABLED // consume vision_position_delta mavlink messages void handle_vision_position_delta_msg(const mavlink_message_t &msg); +#endif // consume vision pose estimate data and send to EKF. distances in meters virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0; @@ -55,10 +59,12 @@ class AP_VisualOdom_Backend return _frontend.get_type(); } +#if HAL_LOGGING_ENABLED // Logging Functions void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence); void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored); void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored); +#endif AP_VisualOdom &_frontend; // reference to frontend uint32_t _last_update_ms; // system time of last update from sensor (used by health checks) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index ebec6eebdd0e91..cc063d85901049 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -72,8 +72,10 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint float yaw; att.to_euler(roll, pitch, yaw); +#if HAL_LOGGING_ENABLED // log sensor data Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume); +#endif // store corrected attitude for use in pre-arm checks _attitude_last = att; @@ -99,7 +101,9 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_ // record time for health monitoring _last_update_ms = AP_HAL::millis(); +#if HAL_LOGGING_ENABLED Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume); +#endif } // apply rotation and correction to position diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp index 36486b1e4a9f20..4a92a983606f8c 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp @@ -1,6 +1,7 @@ #include "AP_VisualOdom_Backend.h" +#include -#if HAL_VISUALODOM_ENABLED +#if HAL_VISUALODOM_ENABLED && HAL_LOGGING_ENABLED #include diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 0117e56af84575..5e4af959fabcda 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp @@ -21,6 +21,7 @@ #include #include +#include // consume vision pose estimate data and send to EKF. distances in meters void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) @@ -39,8 +40,10 @@ void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t t float yaw; attitude.to_euler(roll, pitch, yaw); +#if HAL_LOGGING_ENABLED // log sensor data Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false); +#endif // record time for health monitoring _last_update_ms = AP_HAL::millis(); @@ -54,7 +57,9 @@ void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, ui // record time for health monitoring _last_update_ms = AP_HAL::millis(); +#if HAL_LOGGING_ENABLED Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false); +#endif } #endif // AP_VISUALODOM_MAV_ENABLED diff --git a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp index 2ca8cde3a34e38..610a7cd8e2bd7e 100644 --- a/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp +++ b/libraries/AP_WheelEncoder/AP_WheelEncoder.cpp @@ -201,6 +201,7 @@ void AP_WheelEncoder::update(void) } } +#if HAL_LOGGING_ENABLED // log wheel encoder information void AP_WheelEncoder::Log_Write() const { @@ -219,6 +220,7 @@ void AP_WheelEncoder::Log_Write() const }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif // check if an instance is healthy bool AP_WheelEncoder::healthy(uint8_t instance) const diff --git a/libraries/AP_Winch/AP_Winch.cpp b/libraries/AP_Winch/AP_Winch.cpp index 4a3a41db4021e9..5738fdfea957b0 100644 --- a/libraries/AP_Winch/AP_Winch.cpp +++ b/libraries/AP_Winch/AP_Winch.cpp @@ -159,7 +159,9 @@ bool AP_Winch::pre_arm_check(char *failmsg, uint8_t failmsg_len) const } PASS_TO_BACKEND(update) +#if HAL_LOGGING_ENABLED PASS_TO_BACKEND(write_log) +#endif #undef PASS_TO_BACKEND diff --git a/libraries/AP_Winch/AP_Winch.h b/libraries/AP_Winch/AP_Winch.h index df1ecf9c896c8c..8a3b0f72a82179 100644 --- a/libraries/AP_Winch/AP_Winch.h +++ b/libraries/AP_Winch/AP_Winch.h @@ -23,6 +23,7 @@ #include #include #include +#include class AP_Winch_Backend; @@ -64,8 +65,10 @@ class AP_Winch { // send status to ground station void send_status(const class GCS_MAVLINK &channel); +#if HAL_LOGGING_ENABLED // write log void write_log(); +#endif // returns true if pre arm checks have passed bool pre_arm_check(char *failmsg, uint8_t failmsg_len) const; diff --git a/libraries/AP_Winch/AP_Winch_Backend.h b/libraries/AP_Winch/AP_Winch_Backend.h index 0504a753db86b0..d03adde34595b1 100644 --- a/libraries/AP_Winch/AP_Winch_Backend.h +++ b/libraries/AP_Winch/AP_Winch_Backend.h @@ -19,6 +19,8 @@ #if AP_WINCH_ENABLED +#include + class AP_Winch_Backend { public: AP_Winch_Backend(struct AP_Winch::Backend_Config &_config) : @@ -39,8 +41,10 @@ class AP_Winch_Backend { // send status to ground station virtual void send_status(const GCS_MAVLINK &channel) = 0; +#if HAL_LOGGING_ENABLED // write log virtual void write_log() = 0; +#endif // helper to check if option enabled bool option_enabled(AP_Winch::Options option) const { diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.cpp b/libraries/AP_Winch/AP_Winch_Daiwa.cpp index d80a8f58760d3c..88f61844726317 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.cpp +++ b/libraries/AP_Winch/AP_Winch_Daiwa.cpp @@ -86,6 +86,7 @@ void AP_Winch_Daiwa::send_status(const GCS_MAVLINK &channel) status_bitmask); } +#if HAL_LOGGING_ENABLED // write log void AP_Winch_Daiwa::write_log() { @@ -102,6 +103,7 @@ void AP_Winch_Daiwa::write_log() latest.voltage, constrain_int16(latest.motor_temp, INT8_MIN, INT8_MAX)); } +#endif // read incoming data from winch and update intermediate and latest structures void AP_Winch_Daiwa::read_data_from_winch() diff --git a/libraries/AP_Winch/AP_Winch_Daiwa.h b/libraries/AP_Winch/AP_Winch_Daiwa.h index 301a73d9e9b070..a49eb9e58fa3f2 100644 --- a/libraries/AP_Winch/AP_Winch_Daiwa.h +++ b/libraries/AP_Winch/AP_Winch_Daiwa.h @@ -52,8 +52,10 @@ class AP_Winch_Daiwa : public AP_Winch_Backend { // send status to ground station void send_status(const GCS_MAVLINK &channel) override; +#if HAL_LOGGING_ENABLED // write log void write_log() override; +#endif private: diff --git a/libraries/AP_Winch/AP_Winch_PWM.cpp b/libraries/AP_Winch/AP_Winch_PWM.cpp index ba8d89a8c4498b..7607cd97e26a46 100644 --- a/libraries/AP_Winch/AP_Winch_PWM.cpp +++ b/libraries/AP_Winch/AP_Winch_PWM.cpp @@ -93,6 +93,7 @@ void AP_Winch_PWM::send_status(const GCS_MAVLINK &channel) } // write log +#if HAL_LOGGING_ENABLED void AP_Winch_PWM::write_log() { AP::logger().Write_Winch( @@ -108,5 +109,6 @@ void AP_Winch_PWM::write_log() 0, // voltage (unsupported) 0); // temp (unsupported) } +#endif #endif // AP_WINCH_PWM_ENABLED diff --git a/libraries/AP_Winch/AP_Winch_PWM.h b/libraries/AP_Winch/AP_Winch_PWM.h index 58ef71f3c6bbc1..9fd6ff2eda6aad 100644 --- a/libraries/AP_Winch/AP_Winch_PWM.h +++ b/libraries/AP_Winch/AP_Winch_PWM.h @@ -36,8 +36,10 @@ class AP_Winch_PWM : public AP_Winch_Backend { // send status to ground station void send_status(const GCS_MAVLINK &channel) override; +#if HAL_LOGGING_ENABLED // write log void write_log() override; +#endif private: diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index cd541779016eec..7d336a41c570c2 100644 --- a/libraries/AP_WindVane/AP_WindVane.cpp +++ b/libraries/AP_WindVane/AP_WindVane.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_ENABLED + #include "AP_WindVane.h" #include "AP_WindVane_Home.h" @@ -202,36 +206,46 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager) case WindVaneType::WINDVANE_NONE: // WindVane disabled return; +#if AP_WINDVANE_HOME_ENABLED case WindVaneType::WINDVANE_HOME_HEADING: case WindVaneType::WINDVANE_PWM_PIN: _direction_driver = new AP_WindVane_Home(*this); break; +#endif +#if AP_WINDVANE_ANALOG_ENABLED case WindVaneType::WINDVANE_ANALOG_PIN: _direction_driver = new AP_WindVane_Analog(*this); break; -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif +#if AP_WINDVANE_SIM_ENABLED case WindVaneType::WINDVANE_SITL_TRUE: case WindVaneType::WINDVANE_SITL_APPARENT: _direction_driver = new AP_WindVane_SITL(*this); break; #endif +#if AP_WINDVANE_NMEA_ENABLED case WindVaneType::WINDVANE_NMEA: _direction_driver = new AP_WindVane_NMEA(*this); _direction_driver->init(serial_manager); break; +#endif } // wind speed switch (_speed_sensor_type) { case Speed_type::WINDSPEED_NONE: break; +#if AP_WINDVANE_AIRSPEED_ENABLED case Speed_type::WINDSPEED_AIRSPEED: _speed_driver = new AP_WindVane_Airspeed(*this); break; +#endif +#if AP_WINDVANE_MODERNDEVICE_ENABLED case Speed_type::WINDVANE_WIND_SENSOR_REV_P: _speed_driver = new AP_WindVane_ModernDevice(*this); break; -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif +#if AP_WINDVANE_SIM_ENABLED case Speed_type::WINDSPEED_SITL_TRUE: case Speed_type::WINDSPEED_SITL_APPARENT: // single driver does both speed and direction @@ -241,7 +255,8 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager) _speed_driver = _direction_driver; } break; -#endif +#endif // AP_WINDVANE_SIM_ENABLED +#if AP_WINDVANE_NMEA_ENABLED case Speed_type::WINDSPEED_NMEA: // single driver does both speed and direction if (_direction_type != WindVaneType::WINDVANE_NMEA) { @@ -251,9 +266,12 @@ void AP_WindVane::init(const AP_SerialManager& serial_manager) _speed_driver = _direction_driver; } break; +#endif // AP_WINDVANE_NMEA_ENABLED +#if AP_WINDVANE_RPM_ENABLED case Speed_type::WINDSPEED_RPM: _speed_driver = new AP_WindVane_RPM(*this); break; +#endif } } @@ -303,7 +321,7 @@ void AP_WindVane::update() } } else { // only have direction, can't do true wind calcs, set true direction to apparent + heading - _direction_true_raw = wrap_PI(_direction_apparent_raw + AP::ahrs().yaw); + _direction_true_raw = wrap_PI(_direction_apparent_raw + AP::ahrs().get_yaw()); _speed_true_raw = 0.0f; } @@ -354,6 +372,7 @@ void AP_WindVane::update() _direction_true = _direction_true_raw; } +#if HAL_LOGGING_ENABLED // @LoggerMessage: WIND // @Description: Windvane readings // @Field: TimeUS: Time since system startup @@ -372,12 +391,13 @@ void AP_WindVane::update() _speed_apparent_raw, _speed_apparent, _speed_true); +#endif } void AP_WindVane::record_home_heading() { - _home_heading = AP::ahrs().yaw; + _home_heading = AP::ahrs().get_yaw(); } // to start direction calibration from mavlink or other @@ -436,7 +456,7 @@ void AP_WindVane::update_true_wind_speed_and_direction() } // convert apparent wind speed and direction to 2D vector in same frame as vehicle velocity - const float wind_dir_180 = _direction_apparent_raw + AP::ahrs().yaw + radians(180); + const float wind_dir_180 = _direction_apparent_raw + AP::ahrs().get_yaw() + radians(180); const Vector2f wind_apparent_vec(cosf(wind_dir_180) * _speed_apparent, sinf(wind_dir_180) * _speed_apparent); // add vehicle velocity @@ -464,7 +484,7 @@ void AP_WindVane::update_apparent_wind_dir_from_true() Vector2f wind_apparent_vec = Vector2f(wind_true_vec.x - veh_velocity.x, wind_true_vec.y - veh_velocity.y); // calculate apartment speed and direction - _direction_apparent_raw = wrap_PI(atan2f(wind_apparent_vec.y, wind_apparent_vec.x) - radians(180) - AP::ahrs().yaw); + _direction_apparent_raw = wrap_PI(atan2f(wind_apparent_vec.y, wind_apparent_vec.x) - radians(180) - AP::ahrs().get_yaw()); _speed_apparent_raw = wind_apparent_vec.length(); } @@ -476,3 +496,5 @@ namespace AP { return AP_WindVane::get_singleton(); } }; + +#endif // AP_WINDVANE_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane.h b/libraries/AP_WindVane/AP_WindVane.h index e17edbaa49f7b0..148f072e867a53 100644 --- a/libraries/AP_WindVane/AP_WindVane.h +++ b/libraries/AP_WindVane/AP_WindVane.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_ENABLED + #include #include #include @@ -162,11 +166,17 @@ class AP_WindVane enum WindVaneType { WINDVANE_NONE = 0, +#if AP_WINDVANE_HOME_ENABLED WINDVANE_HOME_HEADING = 1, WINDVANE_PWM_PIN = 2, +#endif +#if AP_WINDVANE_ANALOG_ENABLED WINDVANE_ANALOG_PIN = 3, +#endif +#if AP_WINDVANE_NMEA_ENABLED WINDVANE_NMEA = 4, -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif +#if AP_WINDVANE_SIM_ENABLED WINDVANE_SITL_TRUE = 10, WINDVANE_SITL_APPARENT = 11, #endif @@ -174,11 +184,17 @@ class AP_WindVane enum Speed_type { WINDSPEED_NONE = 0, +#if AP_WINDVANE_AIRSPEED_ENABLED WINDSPEED_AIRSPEED = 1, +#endif WINDVANE_WIND_SENSOR_REV_P = 2, +#if AP_WINDVANE_RPM_ENABLED WINDSPEED_RPM = 3, +#endif +#if AP_WINDVANE_NMEA_ENABLED WINDSPEED_NMEA = 4, -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif +#if AP_WINDVANE_SIM_ENABLED WINDSPEED_SITL_TRUE = 10, WINDSPEED_SITL_APPARENT = 11, #endif @@ -190,3 +206,5 @@ class AP_WindVane namespace AP { AP_WindVane *windvane(); }; + +#endif // AP_WINDVANE_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp b/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp index 094d27cc496925..54496e2f15857d 100644 --- a/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Airspeed.cpp @@ -13,14 +13,18 @@ along with this program. If not, see . */ +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_AIRSPEED_ENABLED + #include "AP_WindVane_Airspeed.h" void AP_WindVane_Airspeed::update_speed() { -#if AP_AIRSPEED_ENABLED const AP_Airspeed* airspeed = AP_Airspeed::get_singleton(); if (airspeed != nullptr) { _frontend._speed_apparent_raw = airspeed->get_raw_airspeed(); } -#endif } + +#endif // AP_WINDVANE_AIRSPEED_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Airspeed.h b/libraries/AP_WindVane/AP_WindVane_Airspeed.h index 8f69151bc442fd..d64c8badb7c0e4 100644 --- a/libraries/AP_WindVane/AP_WindVane_Airspeed.h +++ b/libraries/AP_WindVane/AP_WindVane_Airspeed.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_AIRSPEED_ENABLED + #include "AP_WindVane_Backend.h" #include @@ -27,3 +31,5 @@ class AP_WindVane_Airspeed : public AP_WindVane_Backend // update state void update_speed() override; }; + +#endif // AP_WINDVANE_AIRSPEED_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.cpp b/libraries/AP_WindVane/AP_WindVane_Analog.cpp index cee49bdd1ef3c6..cebd664f51b502 100644 --- a/libraries/AP_WindVane/AP_WindVane_Analog.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Analog.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_ANALOG_ENABLED + #include "AP_WindVane_Analog.h" #include @@ -55,7 +59,7 @@ void AP_WindVane_Analog::calibrate() _cal_start_ms = AP_HAL::millis(); _cal_volt_max = _current_analog_voltage; _cal_volt_min = _current_analog_voltage; - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration started, rotate wind vane"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: Calibration started, rotate wind vane"); } // record min and max voltage @@ -70,11 +74,11 @@ void AP_WindVane_Analog::calibrate() // save min and max voltage _frontend._dir_analog_volt_max.set_and_save(_cal_volt_max); _frontend._dir_analog_volt_min.set_and_save(_cal_volt_min); - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration complete (volt min:%.1f max:%1.f)", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: Calibration complete (volt min:%.1f max:%1.f)", (double)_cal_volt_min, (double)_cal_volt_max); } else { - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: Calibration failed (volt diff %.1f below %.1f)", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: Calibration failed (volt diff %.1f below %.1f)", (double)volt_diff, (double)WINDVANE_CALIBRATION_VOLT_DIFF_MIN); } @@ -82,3 +86,5 @@ void AP_WindVane_Analog::calibrate() _cal_start_ms = 0; } } + +#endif // AP_WINDVANE_ANALOG_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Analog.h b/libraries/AP_WindVane/AP_WindVane_Analog.h index 017266b0be9a66..658761c5381f76 100644 --- a/libraries/AP_WindVane/AP_WindVane_Analog.h +++ b/libraries/AP_WindVane/AP_WindVane_Analog.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_ANALOG_ENABLED + #include "AP_WindVane_Backend.h" class AP_WindVane_Analog : public AP_WindVane_Backend @@ -35,3 +39,5 @@ class AP_WindVane_Analog : public AP_WindVane_Backend float _cal_volt_min; float _cal_volt_max; }; + +#endif // AP_WINDVANE_ANALOG_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.cpp b/libraries/AP_WindVane/AP_WindVane_Backend.cpp index 39f9ac9ed79174..a0fa4e10582452 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Backend.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_ENABLED + #include "AP_WindVane.h" #include "AP_WindVane_Backend.h" @@ -27,7 +31,9 @@ AP_WindVane_Backend::AP_WindVane_Backend(AP_WindVane &frontend) : // calibrate WindVane void AP_WindVane_Backend::calibrate() { - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: No cal required"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: No cal required"); _frontend._calibration.set_and_save(0); return; } + +#endif // AP_WINDVANE_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Backend.h b/libraries/AP_WindVane/AP_WindVane_Backend.h index e347de49723509..54b9b227e26bd7 100644 --- a/libraries/AP_WindVane/AP_WindVane_Backend.h +++ b/libraries/AP_WindVane/AP_WindVane_Backend.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_ENABLED + #include "AP_WindVane.h" class AP_WindVane_Backend @@ -39,3 +43,5 @@ class AP_WindVane_Backend AP_WindVane &_frontend; }; + +#endif // AP_WINDVANE_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Home.cpp b/libraries/AP_WindVane/AP_WindVane_Home.cpp index 042d4ca0c1889f..3271f6c1808c0c 100644 --- a/libraries/AP_WindVane/AP_WindVane_Home.cpp +++ b/libraries/AP_WindVane/AP_WindVane_Home.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_HOME_ENABLED + #include "AP_WindVane_Home.h" #include @@ -28,5 +32,7 @@ void AP_WindVane_Home::update_direction() } } - _frontend._direction_apparent_raw = wrap_PI(direction_apparent_ef - AP::ahrs().yaw); + _frontend._direction_apparent_raw = wrap_PI(direction_apparent_ef - AP::ahrs().get_yaw()); } + +#endif // AP_WINDVANE_HOME_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_Home.h b/libraries/AP_WindVane/AP_WindVane_Home.h index 8a60d807a364e5..32dfe1a9920fb5 100644 --- a/libraries/AP_WindVane/AP_WindVane_Home.h +++ b/libraries/AP_WindVane/AP_WindVane_Home.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_HOME_ENABLED + #include "AP_WindVane_Backend.h" #include @@ -26,3 +30,5 @@ class AP_WindVane_Home : public AP_WindVane_Backend // update state void update_direction() override; }; + +#endif // AP_WINDVANE_HOME_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp index 9daacb6c86bdf3..e01d6aad199eb1 100644 --- a/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp +++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_MODERNDEVICE_ENABLED + #include "AP_WindVane_ModernDevice.h" // read wind speed from Modern Device rev p wind sensor // https://moderndevice.com/news/calibrating-rev-p-wind-sensor-new-regression/ @@ -66,7 +70,9 @@ void AP_WindVane_ModernDevice::update_speed() void AP_WindVane_ModernDevice::calibrate() { - gcs().send_text(MAV_SEVERITY_INFO, "WindVane: rev P. zero wind voltage offset set to %.1f",double(_current_analog_voltage)); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "WindVane: rev P. zero wind voltage offset set to %.1f",double(_current_analog_voltage)); _frontend._speed_sensor_voltage_offset.set_and_save(_current_analog_voltage); _frontend._calibration.set_and_save(0); } + +#endif // AP_WINDVANE_MODERNDEVICE_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_ModernDevice.h b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h index 1d32708881666f..e94690fe3a5850 100644 --- a/libraries/AP_WindVane/AP_WindVane_ModernDevice.h +++ b/libraries/AP_WindVane/AP_WindVane_ModernDevice.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_MODERNDEVICE_ENABLED + #include "AP_WindVane_Backend.h" class AP_WindVane_ModernDevice : public AP_WindVane_Backend @@ -33,3 +37,5 @@ class AP_WindVane_ModernDevice : public AP_WindVane_Backend AP_HAL::AnalogSource *_speed_analog_source; AP_HAL::AnalogSource *_temp_analog_source; }; + +#endif // AP_WINDVANE_MODERNDEVICE_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_NMEA.cpp b/libraries/AP_WindVane/AP_WindVane_NMEA.cpp index 7501571afb69a7..ef6f693740cde4 100644 --- a/libraries/AP_WindVane/AP_WindVane_NMEA.cpp +++ b/libraries/AP_WindVane/AP_WindVane_NMEA.cpp @@ -13,6 +13,10 @@ along with this program. If not, see . */ +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_NMEA_ENABLED + #include #include "AP_WindVane_NMEA.h" #include @@ -197,3 +201,5 @@ bool AP_WindVane_NMEA::decode_latest_term() } return false; } + +#endif // AP_WINDVANE_NMEA_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_NMEA.h b/libraries/AP_WindVane/AP_WindVane_NMEA.h index f23f35b8b3cf58..71c4e0a86e82c6 100644 --- a/libraries/AP_WindVane/AP_WindVane_NMEA.h +++ b/libraries/AP_WindVane/AP_WindVane_NMEA.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_NMEA_ENABLED + #include "AP_WindVane_Backend.h" class AP_WindVane_NMEA : public AP_WindVane_Backend @@ -54,3 +58,5 @@ class AP_WindVane_NMEA : public AP_WindVane_Backend bool _sentence_valid; // is current sentence valid so far bool _sentence_done; // true if this sentence has already been decoded }; + +#endif // AP_WINDVANE_NMEA_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_RPM.cpp b/libraries/AP_WindVane/AP_WindVane_RPM.cpp index 069f30b657e973..b80b366c5fb94a 100644 --- a/libraries/AP_WindVane/AP_WindVane_RPM.cpp +++ b/libraries/AP_WindVane/AP_WindVane_RPM.cpp @@ -15,11 +15,14 @@ #include "AP_WindVane_RPM.h" +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_RPM_ENABLED + #include void AP_WindVane_RPM::update_speed() { -#if AP_RPM_ENABLED const AP_RPM* rpm = AP_RPM::get_singleton(); if (rpm != nullptr) { float temp_speed; @@ -28,5 +31,6 @@ void AP_WindVane_RPM::update_speed() _frontend._speed_apparent_raw = temp_speed; } } -#endif // AP_RPM_ENABLED } + +#endif // AP_WINDVANE_RPM_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_RPM.h b/libraries/AP_WindVane/AP_WindVane_RPM.h index 3d2c9e2dfe80c1..b68f7e8b3bf0bb 100644 --- a/libraries/AP_WindVane/AP_WindVane_RPM.h +++ b/libraries/AP_WindVane/AP_WindVane_RPM.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_RPM_ENABLED + #include "AP_WindVane_Backend.h" class AP_WindVane_RPM : public AP_WindVane_Backend @@ -25,3 +29,5 @@ class AP_WindVane_RPM : public AP_WindVane_Backend // update state void update_speed() override; }; + +#endif // AP_WINDVANE_RPM_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.cpp b/libraries/AP_WindVane/AP_WindVane_SITL.cpp index 8460bbf5cddd59..8fb24172df71eb 100644 --- a/libraries/AP_WindVane/AP_WindVane_SITL.cpp +++ b/libraries/AP_WindVane/AP_WindVane_SITL.cpp @@ -13,9 +13,11 @@ along with this program. If not, see . */ -#include "AP_WindVane_SITL.h" +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_SIM_ENABLED -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include "AP_WindVane_SITL.h" #include #include @@ -39,7 +41,7 @@ void AP_WindVane_SITL::update_direction() wind_vector_ef.x += AP::sitl()->state.speedN; wind_vector_ef.y += AP::sitl()->state.speedE; - _frontend._direction_apparent_raw = wrap_PI(atan2f(wind_vector_ef.y, wind_vector_ef.x) - AP::ahrs().yaw); + _frontend._direction_apparent_raw = wrap_PI(atan2f(wind_vector_ef.y, wind_vector_ef.x) - AP::ahrs().get_yaw()); } else { // WINDVANE_SITL_APARRENT // directly read the body frame apparent wind set by physics backend @@ -71,4 +73,4 @@ void AP_WindVane_SITL::update_speed() _frontend._speed_apparent_raw = AP::sitl()->get_apparent_wind_spd(); } } -#endif +#endif // AP_WINDVANE_SIM_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_SITL.h b/libraries/AP_WindVane/AP_WindVane_SITL.h index c9d3bd68ae4733..bdfe547f4e088b 100644 --- a/libraries/AP_WindVane/AP_WindVane_SITL.h +++ b/libraries/AP_WindVane/AP_WindVane_SITL.h @@ -14,6 +14,10 @@ */ #pragma once +#include "AP_WindVane_config.h" + +#if AP_WINDVANE_SIM_ENABLED + #include "AP_WindVane_Backend.h" class AP_WindVane_SITL : public AP_WindVane_Backend @@ -24,8 +28,8 @@ class AP_WindVane_SITL : public AP_WindVane_Backend using AP_WindVane_Backend::AP_WindVane_Backend; // update state - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - void update_direction() override; - void update_speed() override; - #endif + void update_direction() override; + void update_speed() override; }; + +#endif // AP_WINDVANE_SIM_ENABLED diff --git a/libraries/AP_WindVane/AP_WindVane_config.h b/libraries/AP_WindVane/AP_WindVane_config.h new file mode 100644 index 00000000000000..2c1e0f46c83826 --- /dev/null +++ b/libraries/AP_WindVane/AP_WindVane_config.h @@ -0,0 +1,42 @@ +#pragma once + +#include + +#include +#include + +#ifndef AP_WINDVANE_ENABLED +#define AP_WINDVANE_ENABLED 1 +#endif + +#ifndef AP_WINDVANE_BACKEND_DEFAULT_ENABLED +#define AP_WINDVANE_BACKEND_DEFAULT_ENABLED AP_WINDVANE_ENABLED +#endif + +#ifndef AP_WINDVANE_AIRSPEED_ENABLED +#define AP_WINDVANE_AIRSPEED_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_AIRSPEED_ENABLED +#endif + +#ifndef AP_WINDVANE_ANALOG_ENABLED +#define AP_WINDVANE_ANALOG_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_WINDVANE_HOME_ENABLED +#define AP_WINDVANE_HOME_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_WINDVANE_MODERNDEVICE_ENABLED +#define AP_WINDVANE_MODERNDEVICE_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_WINDVANE_NMEA_ENABLED +#define AP_WINDVANE_NMEA_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_WINDVANE_RPM_ENABLED +#define AP_WINDVANE_RPM_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_RPM_ENABLED +#endif + +#ifndef AP_WINDVANE_SIM_ENABLED +#define AP_WINDVANE_SIM_ENABLED AP_WINDVANE_BACKEND_DEFAULT_ENABLED && AP_SIM_ENABLED +#endif diff --git a/libraries/AR_Motors/AP_MotorsUGV.cpp b/libraries/AR_Motors/AP_MotorsUGV.cpp index 357815613e6ae2..b211384e1cb9d2 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.cpp +++ b/libraries/AR_Motors/AP_MotorsUGV.cpp @@ -16,7 +16,7 @@ #include #include #include "AP_MotorsUGV.h" -#include +#include #define SERVO_MAX 4500 // This value represents 45 degrees and is just an arbitrary representation of servo max travel. @@ -142,11 +142,38 @@ void AP_MotorsUGV::init(uint8_t frtype) setup_safety_output(); // setup for omni vehicles - if (is_omni()) { + if (_frame_type != FRAME_TYPE_UNDEFINED) { setup_omni(); } } +bool AP_MotorsUGV::get_legacy_relay_index(int8_t &index1, int8_t &index2, int8_t &index3, int8_t &index4) const +{ + if (_pwm_type != PWM_TYPE_BRUSHED_WITH_RELAY) { + // Relays only used if PWM type is set to brushed with relay + return false; + } + + // First relay is always used, throttle, throttle left or motor 1 + index1 = 0; + + // Second relay is used for right throttle on skid steer and motor 2 for omni + if (have_skid_steering()) { + index2 = 1; + } + + // Omni can have a variable number of motors + if (is_omni()) { + // Omni has at least 3 motors + index2 = 2; + if (_motors_num >= 4) { + index2 = 3; + } + } + + return true; +} + // setup output in case of main CPU failure void AP_MotorsUGV::setup_safety_output() { @@ -457,29 +484,34 @@ bool AP_MotorsUGV::output_test_pwm(motor_test_order motor_seq, float pwm) // returns true if checks pass, false if they fail. report should be true to send text messages to GCS bool AP_MotorsUGV::pre_arm_check(bool report) const { + const bool have_throttle = SRV_Channels::function_assigned(SRV_Channel::k_throttle); + const bool have_throttle_left = SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft); + const bool have_throttle_right = SRV_Channels::function_assigned(SRV_Channel::k_throttleRight); + // check that there's defined outputs, inc scripting and sail - if(!SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) && - !SRV_Channels::function_assigned(SRV_Channel::k_throttleRight) && - !SRV_Channels::function_assigned(SRV_Channel::k_throttle) && + if(!have_throttle_left && + !have_throttle_right && + !have_throttle && !SRV_Channels::function_assigned(SRV_Channel::k_steering) && !SRV_Channels::function_assigned(SRV_Channel::k_scripting1) && - !has_sail()) { + !has_sail() && + !is_omni()) { if (report) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: no motor, sail or scripting outputs defined"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: no motor, sail or scripting outputs defined"); } return false; } // check if only one of skid-steering output has been configured - if (SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft) != SRV_Channels::function_assigned(SRV_Channel::k_throttleRight)) { + if (have_throttle_left != have_throttle_right) { if (report) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check skid steering config"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: check skid steering config"); } return false; } // check if only one of throttle or steering outputs has been configured, if has a sail allow no throttle - if ((has_sail() || SRV_Channels::function_assigned(SRV_Channel::k_throttle)) != SRV_Channels::function_assigned(SRV_Channel::k_steering)) { + if ((has_sail() || have_throttle) != SRV_Channels::function_assigned(SRV_Channel::k_steering)) { if (report) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: check steering and throttle config"); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: check steering and throttle config"); } return false; } @@ -488,11 +520,40 @@ bool AP_MotorsUGV::pre_arm_check(bool report) const SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i); if (!SRV_Channels::function_assigned(function)) { if (report) { - gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: servo function %u unassigned", function); + GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PreArm: servo function %u unassigned", function); } return false; } } + + // Check relays are configured for brushed with relay outputs +#if AP_RELAY_ENABLED + AP_Relay*relay = AP::relay(); + if ((_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) && (relay != nullptr)) { + // If a output is configured its relay must be enabled + struct RelayTable { + bool output_assigned; + AP_Relay_Params::FUNCTION fun; + }; + + const RelayTable relay_table[] = { + { have_throttle || have_throttle_left || (SRV_Channels::function_assigned(SRV_Channel::k_motor1) && (_motors_num >= 1)), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1 }, + { have_throttle_right || (SRV_Channels::function_assigned(SRV_Channel::k_motor2) && (_motors_num >= 2)), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2 }, + { SRV_Channels::function_assigned(SRV_Channel::k_motor3) && (_motors_num >= 3), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3 }, + { SRV_Channels::function_assigned(SRV_Channel::k_motor4) && (_motors_num >= 4), AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4 }, + }; + + for (uint8_t i=0; ienabled(relay_table[i].fun)) { + if (report) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: relay function %u unassigned", uint8_t(relay_table[i].fun)); + } + return false; + } + } + } +#endif + return true; } @@ -613,7 +674,7 @@ void AP_MotorsUGV::add_omni_motor_num(int8_t motor_num) SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num); SRV_Channels::set_aux_channel_default(function, motor_num); if (!SRV_Channels::find_channel(function, chan)) { - gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num); } } } @@ -864,7 +925,7 @@ void AP_MotorsUGV::output_omni(bool armed, float steering, float throttle, float const float scaled_steering = steering / 4500.0f; const float scaled_lateral = lateral * 0.01f; - float thr_str_ltr_out[AP_MOTORS_NUM_MOTORS_MAX]; + float thr_str_ltr_out[_motors_num]; float thr_str_ltr_max = 1; for (uint8_t i=0; i<_motors_num; i++) { // Each motor outputs throttle + steering + lateral @@ -918,9 +979,9 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f throttle = get_rate_controlled_throttle(function, throttle, dt); // set relay if necessary -#if AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED - if (_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) { - auto &_relayEvents { *AP::servorelayevents() }; +#if AP_RELAY_ENABLED + AP_Relay*relay = AP::relay(); + if ((_pwm_type == PWM_TYPE_BRUSHED_WITH_RELAY) && (relay != nullptr)) { // find the output channel, if not found return const SRV_Channel *out_chan = SRV_Channels::get_channel_for(function); @@ -930,30 +991,31 @@ void AP_MotorsUGV::output_throttle(SRV_Channel::Aux_servo_function_t function, f const int8_t reverse_multiplier = out_chan->get_reversed() ? -1 : 1; bool relay_high = is_negative(reverse_multiplier * throttle); + AP_Relay_Params::FUNCTION relay_function; switch (function) { case SRV_Channel::k_throttle: case SRV_Channel::k_throttleLeft: case SRV_Channel::k_motor1: - _relayEvents.do_set_relay(0, relay_high); + default: + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_1; break; case SRV_Channel::k_throttleRight: case SRV_Channel::k_motor2: - _relayEvents.do_set_relay(1, relay_high); + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_2; break; case SRV_Channel::k_motor3: - _relayEvents.do_set_relay(2, relay_high); + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_3; break; case SRV_Channel::k_motor4: - _relayEvents.do_set_relay(3, relay_high); - break; - default: - // do nothing + relay_function = AP_Relay_Params::FUNCTION::BRUSHED_REVERSE_4; break; } + relay->set(relay_function, relay_high); + // invert the output to always have positive value calculated by calc_pwm throttle = reverse_multiplier * fabsf(throttle); } -#endif // AP_SERVORELAYEVENTS_ENABLED && AP_RELAY_ENABLED +#endif // AP_RELAY_ENABLED // output to servo channel switch (function) { diff --git a/libraries/AR_Motors/AP_MotorsUGV.h b/libraries/AR_Motors/AP_MotorsUGV.h index df61e8734d974f..42c2df5377ae98 100644 --- a/libraries/AR_Motors/AP_MotorsUGV.h +++ b/libraries/AR_Motors/AP_MotorsUGV.h @@ -114,6 +114,9 @@ class AP_MotorsUGV { // returns true if the vehicle is omni bool is_omni() const { return _frame_type != FRAME_TYPE_UNDEFINED && _motors_num > 0; } + // Return the relay index that would be used for param conversion to relay functions + bool get_legacy_relay_index(int8_t &index1, int8_t &index2, int8_t &index3, int8_t &index4) const; + // structure for holding motor limit flags struct AP_MotorsUGV_limit { uint8_t steer_left : 1; // we have reached the steering controller's left most limit diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 1ab6c817d86fb5..66b4041dce920f 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -18,6 +18,7 @@ #include #include "AR_WPNav.h" #include +#include #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 1bfb2864aaf9b9..64bbeb9a25e057 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -168,6 +168,7 @@ class AR_WPNav { uint32_t _last_update_ms; // system time of last call to update Location _origin; // origin Location (vehicle will travel from the origin to the destination) Location _destination; // destination Location when in Guided_WP + Location _next_destination; // next destination Location when in Guided_WP bool _orig_and_dest_valid; // true if the origin and destination have been set bool _reversed; // execute the mission by backing up enum class NavControllerType { diff --git a/libraries/AR_WPNav/AR_WPNav_OA.cpp b/libraries/AR_WPNav/AR_WPNav_OA.cpp index dc7c8f29be3af9..f1fd9886ff8810 100644 --- a/libraries/AR_WPNav/AR_WPNav_OA.cpp +++ b/libraries/AR_WPNav/AR_WPNav_OA.cpp @@ -18,6 +18,7 @@ #include #include "AR_WPNav_OA.h" #include +#include extern const AP_HAL::HAL& hal; @@ -38,17 +39,28 @@ void AR_WPNav_OA::update(float dt) // run path planning around obstacles bool stop_vehicle = false; - // backup _origin and _destination when not doing oa + // backup _origin, _destination and _next_destination when not doing oa if (!_oa_active) { _origin_oabak = _origin; _destination_oabak = _destination; + _next_destination_oabak = _next_destination; } AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton(); if (oa != nullptr) { - Location oa_origin_new, oa_destination_new; + Location oa_origin_new, oa_destination_new, oa_next_destination_new; AP_OAPathPlanner::OAPathPlannerUsed path_planner_used; - const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin_oabak, _destination_oabak, oa_origin_new, oa_destination_new, path_planner_used); + bool dest_to_next_dest_clear; + const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, + _origin_oabak, + _destination_oabak, + _next_destination_oabak, + oa_origin_new, + oa_destination_new, + oa_next_destination_new, + dest_to_next_dest_clear, + path_planner_used); + switch (oa_retstate) { case AP_OAPathPlanner::OA_NOT_REQUIRED: diff --git a/libraries/AR_WPNav/AR_WPNav_OA.h b/libraries/AR_WPNav/AR_WPNav_OA.h index 195ed3028f2815..2130579eebaa97 100644 --- a/libraries/AR_WPNav/AR_WPNav_OA.h +++ b/libraries/AR_WPNav/AR_WPNav_OA.h @@ -36,8 +36,10 @@ class AR_WPNav_OA : public AR_WPNav { bool _oa_active; // true if we should use alternative destination to avoid obstacles Location _origin_oabak; // backup of _origin so it can be restored when oa completes Location _destination_oabak; // backup of _desitnation so it can be restored when oa completes + Location _next_destination_oabak; // backup of _next_destination so it can be restored when oa completes Location _oa_origin; // intermediate origin during avoidance Location _oa_destination; // intermediate destination during avoidance + Location _oa_next_destination; // intermediate next destination during avoidance float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters float _oa_wp_bearing_cd; // OA adjusted heading to _oa_destination in centi-degrees }; diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 0393f04930bb0a..5d730b7c89e2a4 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "MissionItemProtocol_Waypoints.h" #include "MissionItemProtocol_Rally.h" @@ -24,16 +25,16 @@ extern const AP_HAL::HAL& hal; +void GCS::get_sensor_status_flags(uint32_t &present, + uint32_t &enabled, + uint32_t &health) +{ // if this assert fails then fix it and the comment in GCS.h where // _statustext_queue is declared #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -assert_storage_size _assert_statustext_t_size; +ASSERT_STORAGE_SIZE(GCS::statustext_t, 58); #endif -void GCS::get_sensor_status_flags(uint32_t &present, - uint32_t &enabled, - uint32_t &health) -{ update_sensor_status_flags(); present = control_sensors_present; @@ -261,7 +262,7 @@ void GCS::update_sensor_status_flags() control_sensors_health |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; #endif -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && AP_AHRS_ENABLED if (ahrs.get_ekf_type() == 10) { // always show EKF type 10 as healthy. This prevents spurious error // messages in xplane and other simulators that use EKF type 10 @@ -290,7 +291,12 @@ void GCS::update_sensor_status_flags() if (airspeed && airspeed->enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; const bool use = airspeed->use(); +#if AP_AHRS_ENABLED const bool enabled = AP::ahrs().airspeed_sensor_enabled(); +#else + const AP_Airspeed *_airspeed = AP::airspeed(); + const bool enabled = (_airspeed != nullptr && _airspeed->use()); +#endif if (use) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 867ec889031087..87a76c3823f3f3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -573,11 +573,14 @@ class GCS_MAVLINK #endif void handle_fence_message(const mavlink_message_t &msg); void handle_param_value(const mavlink_message_t &msg); - void handle_radio_status(const mavlink_message_t &msg, bool log_radio); +#if HAL_LOGGING_ENABLED + virtual uint32_t log_radio_bit() const { return 0; } +#endif + void handle_radio_status(const mavlink_message_t &msg); void handle_serial_control(const mavlink_message_t &msg); void handle_vision_position_delta(const mavlink_message_t &msg); - void handle_common_message(const mavlink_message_t &msg); + virtual void handle_message(const mavlink_message_t &msg); void handle_set_gps_global_origin(const mavlink_message_t &msg); void handle_setup_signing(const mavlink_message_t &msg) const; virtual MAV_RESULT handle_preflight_reboot(const mavlink_command_int_t &packet, const mavlink_message_t &msg); @@ -643,9 +646,10 @@ class GCS_MAVLINK virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg); virtual MAV_RESULT _handle_command_preflight_calibration_baro(const mavlink_message_t &msg); +#if AP_MISSION_ENABLED virtual MAV_RESULT handle_command_do_set_mission_current(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_jump_tag(const mavlink_command_int_t &packet); - +#endif MAV_RESULT handle_command_battery_reset(const mavlink_command_int_t &packet); void handle_command_long(const mavlink_message_t &msg); MAV_RESULT handle_command_accelcal_vehicle_pos(const mavlink_command_int_t &packet); @@ -657,8 +661,6 @@ class GCS_MAVLINK MAV_RESULT handle_command_mag_cal(const mavlink_command_int_t &packet); MAV_RESULT handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet); - virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const; - MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg); MAV_RESULT handle_command_camera(const mavlink_command_int_t &packet); MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet); virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc); @@ -730,12 +732,16 @@ class GCS_MAVLINK */ uint32_t correct_offboard_timestamp_usec_to_ms(uint64_t offboard_usec, uint16_t payload_size); +#if AP_MAVLINK_COMMAND_LONG_ENABLED // converts a COMMAND_LONG packet to a COMMAND_INT packet, where // the command-long packet is assumed to be in the supplied frame. // If location is not present in the command then just omit frame. // this method ensures the passed-in structure is entirely // initialised. virtual void convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame = MAV_FRAME_GLOBAL_RELATIVE_ALT); + virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const; + MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg); +#endif // methods to extract a Location object from a command_int bool location_from_command_t(const mavlink_command_int_t &in, Location &out); @@ -773,8 +779,6 @@ class GCS_MAVLINK void service_statustext(void); - virtual void handleMessage(const mavlink_message_t &msg) = 0; - MAV_RESULT handle_servorelay_message(const mavlink_command_int_t &packet); bool send_relay_status() const; @@ -1010,7 +1014,7 @@ class GCS_MAVLINK void send_distance_sensor(const class AP_RangeFinder_Backend *sensor, const uint8_t instance) const; - virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) = 0; + virtual bool handle_guided_request(AP_Mission::Mission_Command &cmd) { return false; }; virtual void handle_change_alt_request(AP_Mission::Mission_Command &cmd) {}; void handle_common_mission_message(const mavlink_message_t &msg); @@ -1334,11 +1338,13 @@ GCS &gcs(); // send text when we do have a GCS #if !defined(HAL_BUILD_AP_PERIPH) #define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args) +#define AP_HAVE_GCS_SEND_TEXT 1 #else extern "C" { void can_printf(const char *fmt, ...); } #define GCS_SEND_TEXT(severity, format, args...) (void)severity; can_printf(format, ##args) +#define AP_HAVE_GCS_SEND_TEXT 1 #endif #define GCS_SEND_MESSAGE(msg) gcs().send_message(msg) @@ -1351,11 +1357,13 @@ void can_printf(const char *fmt, ...); } #define GCS_SEND_TEXT(severity, format, args...) can_printf(format, ##args) #define GCS_SEND_MESSAGE(msg) +#define AP_HAVE_GCS_SEND_TEXT 1 #else // HAL_GCS_ENABLED // empty send text when we have no GCS #define GCS_SEND_TEXT(severity, format, args...) #define GCS_SEND_MESSAGE(msg) +#define AP_HAVE_GCS_SEND_TEXT 0 #endif // HAL_GCS_ENABLED diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a81f3d5a38cb7b..ec4ff7eae0d762 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -22,6 +22,7 @@ #include "GCS.h" #include +#include #include #include #include @@ -38,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -63,11 +65,15 @@ #include #include #include +#include #include "MissionItemProtocol_Waypoints.h" #include "MissionItemProtocol_Rally.h" #include "MissionItemProtocol_Fence.h" +#include +#include + #include #if HAL_RCINPUT_WITH_AP_RADIO @@ -548,6 +554,7 @@ void GCS_MAVLINK::send_proximity() // report AHRS2 state void GCS_MAVLINK::send_ahrs2() { +#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); Vector3f euler; Location loc {}; @@ -562,6 +569,7 @@ void GCS_MAVLINK::send_ahrs2() loc.lat, loc.lng); } +#endif } MissionItemProtocol *GCS::get_prot_for_mission_type(const MAV_MISSION_TYPE mission_type) const @@ -652,7 +660,11 @@ void GCS_MAVLINK::send_mission_current(const class AP_Mission &mission, uint16_t num_commands -= 1; } +#if AP_VEHICLE_ENABLED const uint8_t mission_mode = AP::vehicle()->current_mode_requires_mission() ? 1 : 0; +#else + const uint8_t mission_mode = 0; +#endif mavlink_msg_mission_current_send( chan, @@ -815,7 +827,7 @@ float GCS_MAVLINK::telemetry_radio_rssi() return last_radio_status.rssi/254.0f; } -void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg, bool log_radio) +void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg) { mavlink_radio_t packet; mavlink_msg_radio_decode(&msg, &packet); @@ -860,10 +872,12 @@ void GCS_MAVLINK::handle_radio_status(const mavlink_message_t &msg, bool log_rad } #endif +#if HAL_LOGGING_ENABLED //log rssi, noise, etc if logging Performance monitoring data - if (log_radio) { + if (AP::logger().should_log(log_radio_bit())) { AP::logger().Write_Radio(packet); } +#endif } void GCS_MAVLINK::handle_mission_item(const mavlink_message_t &msg) @@ -1749,7 +1763,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status, // e.g. enforce-sysid says we shouldn't look at this packet return; } - handleMessage(msg); + handle_message(msg); } void @@ -1826,6 +1840,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) } } +#if HAL_LOGGING_ENABLED // consider logging mavlink stats: if (is_active() || is_streaming()) { if (tnow - last_mavlink_stats_logged > 1000) { @@ -1833,6 +1848,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) last_mavlink_stats_logged = tnow; } } +#endif #if GCS_DEBUG_SEND_MESSAGE_TIMINGS @@ -1909,6 +1925,7 @@ GCS_MAVLINK::update_receive(uint32_t max_time_us) #endif } +#if HAL_LOGGING_ENABLED /* record stats about this link to logger */ @@ -1945,6 +1962,7 @@ void GCS_MAVLINK::log_mavlink_stats() AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif /* send the SYSTEM_TIME message @@ -1993,7 +2011,12 @@ void GCS_MAVLINK::send_rc_channels() const values[15], values[16], values[17], - receiver_rssi()); +#if AP_RSSI_ENABLED + receiver_rssi() +#else + 255 // meaning "unknown" +#endif + ); } bool GCS_MAVLINK::sending_mavlink1() const @@ -2024,23 +2047,28 @@ void GCS_MAVLINK::send_rc_channels_raw() const values[5], values[6], values[7], - receiver_rssi()); +#if AP_RSSI_ENABLED + receiver_rssi() +#else + 255 // meaning "unknown" +#endif +); } void GCS_MAVLINK::send_raw_imu() { #if AP_INERTIALSENSOR_ENABLED const AP_InertialSensor &ins = AP::ins(); - const Compass &compass = AP::compass(); const Vector3f &accel = ins.get_accel(0); const Vector3f &gyro = ins.get_gyro(0); Vector3f mag; +#if AP_COMPASS_ENABLED + const Compass &compass = AP::compass(); if (compass.get_count() >= 1) { mag = compass.get_field(0); - } else { - mag.zero(); } +#endif mavlink_msg_raw_imu_send( chan, @@ -2063,7 +2091,6 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan { #if AP_INERTIALSENSOR_ENABLED const AP_InertialSensor &ins = AP::ins(); - const Compass &compass = AP::compass(); int16_t _temperature = 0; bool have_data = false; @@ -2078,11 +2105,14 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan gyro = ins.get_gyro(instance); have_data = true; } - Vector3f mag{}; + Vector3f mag; +#if AP_COMPASS_ENABLED + const Compass &compass = AP::compass(); if (compass.get_count() > instance) { mag = compass.get_field(instance); have_data = true; } +#endif if (!have_data) { return; } @@ -2169,6 +2199,7 @@ void GCS_MAVLINK::send_scaled_pressure3() void GCS_MAVLINK::send_ahrs() { +#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); const Vector3f &omega_I = ahrs.get_gyro_drift(); mavlink_msg_ahrs_send( @@ -2180,6 +2211,7 @@ void GCS_MAVLINK::send_ahrs() 0, ahrs.get_error_rp(), ahrs.get_error_yaw()); +#endif } /* @@ -2253,12 +2285,14 @@ void GCS::send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list, u } } while (false); +#if HAL_LOGGING_ENABLED // given we don't really know what these methods get up to, we // don't hold the statustext semaphore while doing them: AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { logger->Write_Message(first_piece_of_text); } +#endif #if AP_FRSKY_TELEM_ENABLED frsky = AP::frsky_telem(); @@ -2587,6 +2621,7 @@ void GCS_MAVLINK::handle_set_mode(const mavlink_message_t &msg) MAV_RESULT GCS_MAVLINK::_set_mode_common(const MAV_MODE _base_mode, const uint32_t _custom_mode) { // only accept custom modes because there is no easy mapping from Mavlink flight modes to AC flight modes +#if AP_VEHICLE_ENABLED if (uint32_t(_base_mode) & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { if (!AP::vehicle()->set_mode(_custom_mode, ModeReason::GCS_COMMAND)) { // often we should be returning DENIED rather than FAILED @@ -2596,6 +2631,7 @@ MAV_RESULT GCS_MAVLINK::_set_mode_common(const MAV_MODE _base_mode, const uint32 } return MAV_RESULT_ACCEPTED; } +#endif if (_base_mode == (MAV_MODE)MAV_MODE_FLAG_DECODE_POSITION_SAFETY) { // set the safety switch position. Must be in a command by itself @@ -2728,6 +2764,7 @@ void GCS_MAVLINK::send_autopilot_version() const */ void GCS_MAVLINK::send_local_position() const { +#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); Vector3f local_position, velocity; @@ -2746,6 +2783,7 @@ void GCS_MAVLINK::send_local_position() const velocity.x, velocity.y, velocity.z); +#endif } /* @@ -3110,11 +3148,13 @@ float GCS_MAVLINK::vfr_hud_airspeed() const float GCS_MAVLINK::vfr_hud_climbrate() const { +#if AP_AHRS_ENABLED Vector3f velned; if (AP::ahrs().get_velocity_NED(velned) || AP::ahrs().get_vert_pos_rate_D(velned.z)) { return -velned.z; } +#endif return 0; } @@ -3125,6 +3165,7 @@ float GCS_MAVLINK::vfr_hud_alt() const void GCS_MAVLINK::send_vfr_hud() { +#if AP_AHRS_ENABLED AP_AHRS &ahrs = AP::ahrs(); // return values ignored; we send stale data @@ -3138,6 +3179,7 @@ void GCS_MAVLINK::send_vfr_hud() abs(vfr_hud_throttle()), vfr_hud_alt(), vfr_hud_climbrate()); +#endif } /* @@ -3269,7 +3311,11 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_int_t &pac // when packet.param1 == 3 we reboot to hold in bootloader const bool hold_in_bootloader = is_equal(packet.param1, 3.0f); +#if AP_VEHICLE_ENABLED AP::vehicle()->reboot(hold_in_bootloader); // not expected to return +#else + hal.scheduler->reboot(hold_in_bootloader); +#endif return MAV_RESULT_FAILED; } @@ -3356,13 +3402,15 @@ void GCS_MAVLINK::handle_timesync(const mavlink_message_t &msg) // response to an ancient request... return; } - const uint64_t round_trip_time_us = (timesync_receive_timestamp_ns() - _timesync_request.sent_ts1)*0.001f; #if 0 gcs().send_text(MAV_SEVERITY_INFO, "timesync response sysid=%u (latency=%fms)", msg.sysid, round_trip_time_us*0.001f); #endif + +#if HAL_LOGGING_ENABLED + const uint64_t round_trip_time_us = (timesync_receive_timestamp_ns() - _timesync_request.sent_ts1)*0.001f; AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { AP::logger().Write( @@ -3376,6 +3424,7 @@ void GCS_MAVLINK::handle_timesync(const mavlink_message_t &msg) round_trip_time_us ); } +#endif // HAL_LOGGING_ENABLED return; } @@ -3414,6 +3463,7 @@ void GCS_MAVLINK::send_timesync() void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const { +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr) { return; @@ -3438,6 +3488,7 @@ void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const memcpy(&text[offset], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); logger->Write_Message(text); +#endif } @@ -3446,6 +3497,7 @@ void GCS_MAVLINK::handle_statustext(const mavlink_message_t &msg) const */ void GCS_MAVLINK::handle_named_value(const mavlink_message_t &msg) const { +#if HAL_LOGGING_ENABLED auto *logger = AP_Logger::get_singleton(); if (logger == nullptr) { return; @@ -3461,6 +3513,7 @@ void GCS_MAVLINK::handle_named_value(const mavlink_message_t &msg) const p.value, msg.sysid, msg.compid); +#endif } #if AP_RTC_ENABLED @@ -3486,6 +3539,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_int_t &packe #endif +#if AP_AHRS_ENABLED // sets ekf_origin if it has not been set. // should only be used when there is no GPS to provide an absolute position void GCS_MAVLINK::set_ekf_origin(const Location& loc) @@ -3507,7 +3561,9 @@ void GCS_MAVLINK::set_ekf_origin(const Location& loc) return; } +#if HAL_LOGGING_ENABLED ahrs.Log_Write_Home_And_Origin(); +#endif // send ekf origin to GCS if (!try_send_message(MSG_ORIGIN)) { @@ -3533,6 +3589,7 @@ void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t &msg) ekf_origin.alt = packet.altitude / 10; set_ekf_origin(ekf_origin); } +#endif // AP_AHRS_ENABLED /* handle a DATA96 message @@ -3775,19 +3832,24 @@ void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg) #endif -#if COMPASS_CAL_ENABLED +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED /* handle MAV_CMD_FIXED_MAG_CAL_YAW */ MAV_RESULT GCS_MAVLINK::handle_command_fixed_mag_cal_yaw(const mavlink_command_int_t &packet) { Compass &compass = AP::compass(); - return compass.mag_cal_fixed_yaw(packet.param1, - uint8_t(packet.param2), - packet.param3, - packet.param4); + if (!compass.mag_cal_fixed_yaw(packet.param1, + uint8_t(packet.param2), + packet.param3, + packet.param4)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; } +#endif // AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED +#if COMPASS_CAL_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_int_t &packet) { return AP::compass().handle_mag_cal_command(packet); @@ -3879,7 +3941,7 @@ void GCS_MAVLINK::handle_heartbeat(const mavlink_message_t &msg) const /* handle messages which don't require vehicle specific data */ -void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) +void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) { switch (msg.msgid) { @@ -3903,9 +3965,11 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_common_param_message(msg); break; +#if AP_AHRS_ENABLED case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN: handle_set_gps_global_origin(msg); break; +#endif #if AP_MAVLINK_MSG_DEVICE_OP_ENABLED case MAVLINK_MSG_ID_DEVICE_OP_READ: @@ -3919,6 +3983,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_TIMESYNC: handle_timesync(msg); break; +#if HAL_LOGGING_ENABLED case MAVLINK_MSG_ID_LOG_REQUEST_LIST: case MAVLINK_MSG_ID_LOG_REQUEST_DATA: case MAVLINK_MSG_ID_LOG_ERASE: @@ -3926,6 +3991,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: AP::logger().handle_mavlink_msg(*this, msg); break; +#endif case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: handle_file_transfer_protocol(msg); @@ -4009,6 +4075,11 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) handle_param_value(msg); break; + case MAVLINK_MSG_ID_RADIO: + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_radio_status(msg); + break; + #if AP_MAVLINK_MSG_SERIAL_CONTROL_ENABLED case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg); @@ -4156,11 +4227,11 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) break; #endif - case MAVLINK_MSG_ID_CAN_FILTER_MODIFY: #if HAL_CANMANAGER_ENABLED + case MAVLINK_MSG_ID_CAN_FILTER_MODIFY: AP::can().handle_can_filter_modify(msg); -#endif break; +#endif #if AP_OPENDRONEID_ENABLED case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS: @@ -4420,9 +4491,11 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_comm #endif #if AP_INERTIALSENSOR_ENABLED +#if AP_AHRS_ENABLED if (packet.x == 2) { return AP::ins().calibrate_trim(); } +#endif if (packet.x == 4) { // simple accel calibration @@ -4470,6 +4543,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_run_prearm_checks(const mavlink_command_i return MAV_RESULT_ACCEPTED; } +#if AP_MISSION_ENABLED // changes the current waypoint; at time of writing GCS // implementations use the mavlink message MISSION_SET_CURRENT to set // the current waypoint, rather than this DO command. It is hoped we @@ -4528,6 +4602,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_jump_tag(const mavlink_command_int_t & return MAV_RESULT_ACCEPTED; } +#endif #if AP_BATTERY_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_battery_reset(const mavlink_command_int_t &packet) @@ -4593,6 +4668,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_debug_trap(const mavlink_command_int_t &p return MAV_RESULT_UNSUPPORTED; } +#if AP_AHRS_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_int_t &packet) { // source set must be between 1 and 3 @@ -4604,6 +4680,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_set_ekf_source_set(const mavlink_command_ } return MAV_RESULT_DENIED; } +#endif #if AP_GRIPPER_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_do_gripper(const mavlink_command_int_t &packet) @@ -4730,6 +4807,49 @@ MAV_RESULT GCS_MAVLINK::handle_command_component_arm_disarm(const mavlink_comman return MAV_RESULT_UNSUPPORTED; } +bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Location &out) +{ + if (!command_long_stores_location((MAV_CMD)in.command)) { + return false; + } + + // integer storage imposes limits on the altitudes we can accept: + if (isnan(in.z) || fabsf(in.z) > LOCATION_ALT_MAX_M) { + return false; + } + + Location::AltFrame frame; + if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in.frame, frame)) { + // unknown coordinate frame + return false; + } + + out.lat = in.x; + out.lng = in.y; + + out.set_alt_cm(int32_t(in.z * 100), frame); + + return true; +} + +bool GCS_MAVLINK::command_long_stores_location(const MAV_CMD command) +{ + switch(command) { + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_SET_ROI: + case MAV_CMD_DO_SET_ROI_LOCATION: + // case MAV_CMD_NAV_TAKEOFF: // technically yes, but we don't do lat/lng + // case MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_DO_REPOSITION: + case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: + return true; + default: + return false; + } + return false; +} + +#if AP_MAVLINK_COMMAND_LONG_ENABLED // when conveyed via COMMAND_LONG, a command doesn't come with an // explicit frame. When conveying a location they do have an assumed // frame in ArduPilot, and this function returns that frame. @@ -4774,49 +4894,6 @@ MAV_RESULT GCS_MAVLINK::try_command_long_as_command_int(const mavlink_command_lo return handle_command_int_packet(command_int, msg); } -bool GCS_MAVLINK::location_from_command_t(const mavlink_command_int_t &in, Location &out) -{ - if (!command_long_stores_location((MAV_CMD)in.command)) { - return false; - } - - // integer storage imposes limits on the altitudes we can accept: - if (isnan(in.z) || fabsf(in.z) > LOCATION_ALT_MAX_M) { - return false; - } - - Location::AltFrame frame; - if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)in.frame, frame)) { - // unknown coordinate frame - return false; - } - - out.lat = in.x; - out.lng = in.y; - - out.set_alt_cm(int32_t(in.z * 100), frame); - - return true; -} - -#if AP_MAVLINK_COMMAND_LONG_ENABLED -bool GCS_MAVLINK::command_long_stores_location(const MAV_CMD command) -{ - switch(command) { - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_DO_SET_ROI: - case MAV_CMD_DO_SET_ROI_LOCATION: - // case MAV_CMD_NAV_TAKEOFF: // technically yes, but we don't do lat/lng - // case MAV_CMD_NAV_VTOL_TAKEOFF: - case MAV_CMD_DO_REPOSITION: - case MAV_CMD_EXTERNAL_POSITION_ESTIMATE: - return true; - default: - return false; - } - return false; -} - // returns a value suitable for COMMAND_INT.x or y based on a value // coming in from COMMAND_LONG.p5 or p6: static int32_t convert_COMMAND_LONG_loc_param(float param, bool stores_location) @@ -4875,13 +4952,35 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) msg.sysid, msg.compid); +#if HAL_LOGGING_ENABLED // log the packet: mavlink_command_int_t packet_int; convert_COMMAND_LONG_to_COMMAND_INT(packet, packet_int); AP::logger().Write_Command(packet_int, msg.sysid, msg.compid, result, true); +#endif hal.util->persistent_data.last_mavlink_cmd = 0; } + +#else +void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) +{ + // decode packet + mavlink_command_long_t packet; + mavlink_msg_command_long_decode(&msg, &packet); + + // send ACK or NAK + mavlink_msg_command_ack_send( + chan, + packet.command, + MAV_RESULT_COMMAND_INT_ONLY, + 0, + 0, + msg.sysid, + msg.compid + ); + +} #endif // AP_MAVLINK_COMMAND_LONG_ENABLED MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc) @@ -5069,11 +5168,13 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_command_do_gripper(packet); #endif +#if AP_MISSION_ENABLED case MAV_CMD_DO_JUMP_TAG: return handle_command_do_jump_tag(packet); case MAV_CMD_DO_SET_MISSION_CURRENT: return handle_command_do_set_mission_current(packet); +#endif case MAV_CMD_DO_SET_MODE: return handle_command_do_set_mode(packet); @@ -5130,9 +5231,11 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p case MAV_CMD_COMPONENT_ARM_DISARM: return handle_command_component_arm_disarm(packet); -#if COMPASS_CAL_ENABLED +#if AP_COMPASS_CALIBRATION_FIXED_YAW_ENABLED case MAV_CMD_FIXED_MAG_CAL_YAW: return handle_command_fixed_mag_cal_yaw(packet); +#endif +#if COMPASS_CAL_ENABLED case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: @@ -5190,8 +5293,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p } #endif // AP_SCRIPTING_ENABLED +#if AP_AHRS_ENABLED case MAV_CMD_SET_EKF_SOURCE_SET: return handle_command_set_ekf_source_set(packet); +#endif case MAV_CMD_START_RX_PAIR: return handle_START_RX_PAIR(packet); @@ -5241,7 +5346,9 @@ void GCS_MAVLINK::handle_command_int(const mavlink_message_t &msg) msg.sysid, msg.compid); +#if HAL_LOGGING_ENABLED AP::logger().Write_Command(packet, msg.sysid, msg.compid, result); +#endif hal.util->persistent_data.last_mavlink_cmd = 0; } @@ -5257,6 +5364,7 @@ void GCS::try_send_queued_message_for_type(MAV_MISSION_TYPE type) const { bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id) { switch (id) { +#if AP_MISSION_ENABLED case MSG_CURRENT_WAYPOINT: { CHECK_PAYLOAD_SIZE(MISSION_CURRENT); @@ -5274,6 +5382,7 @@ bool GCS_MAVLINK::try_send_mission_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(MISSION_REQUEST); gcs().try_send_queued_message_for_type(MAV_MISSION_TYPE_MISSION); break; +#endif #if HAL_RALLY_ENABLED case MSG_NEXT_MISSION_REQUEST_RALLY: CHECK_PAYLOAD_SIZE(MISSION_REQUEST); @@ -5384,21 +5493,24 @@ void GCS_MAVLINK::send_extended_sys_state() const void GCS_MAVLINK::send_attitude() const { +#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); const Vector3f omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, AP_HAL::millis(), - ahrs.roll, - ahrs.pitch, - ahrs.yaw, + ahrs.get_roll(), + ahrs.get_pitch(), + ahrs.get_yaw(), omega.x, omega.y, omega.z); +#endif } void GCS_MAVLINK::send_attitude_quaternion() const { +#if AP_AHRS_ENABLED const AP_AHRS &ahrs = AP::ahrs(); Quaternion quat; if (!ahrs.get_quaternion(quat)) { @@ -5418,19 +5530,26 @@ void GCS_MAVLINK::send_attitude_quaternion() const omega.z, // yawspeed repr_offseq_q ); +#endif } int32_t GCS_MAVLINK::global_position_int_alt() const { return global_position_current_loc.alt * 10UL; } int32_t GCS_MAVLINK::global_position_int_relative_alt() const { +#if AP_AHRS_ENABLED float posD; AP::ahrs().get_relative_position_D_home(posD); posD *= -1000.0f; // change from down to up and metres to millimeters return posD; +#else + return 0; +#endif } + void GCS_MAVLINK::send_global_position_int() { +#if AP_AHRS_ENABLED AP_AHRS &ahrs = AP::ahrs(); UNUSED_RESULT(ahrs.get_location(global_position_current_loc)); // return value ignored; we send stale data @@ -5451,6 +5570,7 @@ void GCS_MAVLINK::send_global_position_int() vel.y * 100, // Y speed cm/s (+ve East) vel.z * 100, // Z speed cm/s (+ve Down) ahrs.yaw_sensor); // compass heading in 1/100 degree +#endif // AP_AHRS_ENABLED } #if HAL_MOUNT_ENABLED @@ -5613,10 +5733,12 @@ void GCS_MAVLINK::send_autopilot_state_for_gimbal_device() const // get vehicle earth-frame rotation rate targets Vector3f rate_ef_targets; +#if AP_VEHICLE_ENABLED const AP_Vehicle *vehicle = AP::vehicle(); if (vehicle != nullptr) { vehicle->get_rate_ef_targets(rate_ef_targets); } +#endif // get estimator flags uint16_t est_status_flags = 0; @@ -5746,10 +5868,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) #endif #endif // AP_BATTERY_ENABLED +#if AP_AHRS_ENABLED case MSG_EKF_STATUS_REPORT: CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT); AP::ahrs().send_ekf_status_report(*this); break; +#endif case MSG_MEMINFO: CHECK_PAYLOAD_SIZE(MEMINFO); @@ -6637,6 +6761,7 @@ void GCS_MAVLINK::handle_manual_control(const mavlink_message_t &msg) } +#if AP_RSSI_ENABLED uint8_t GCS_MAVLINK::receiver_rssi() const { AP_RSSI *aprssi = AP::rssi(); @@ -6651,6 +6776,7 @@ uint8_t GCS_MAVLINK::receiver_rssi() const // scale across the full valid range return aprssi->read_receiver_rssi() * 254; } +#endif GCS &gcs() { @@ -6663,6 +6789,7 @@ GCS &gcs() #if HAL_HIGH_LATENCY2_ENABLED void GCS_MAVLINK::send_high_latency2() const { +#if AP_AHRS_ENABLED AP_AHRS &ahrs = AP::ahrs(); Location global_position_current; UNUSED_RESULT(ahrs.get_location(global_position_current)); @@ -6670,11 +6797,13 @@ void GCS_MAVLINK::send_high_latency2() const const int8_t battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE); #endif - AP_Mission *mission = AP::mission(); uint16_t current_waypoint = 0; +#if AP_MISSION_ENABLED + AP_Mission *mission = AP::mission(); if (mission != nullptr) { current_waypoint = mission->get_current_nav_index(); } +#endif uint32_t present; uint32_t enabled; @@ -6739,6 +6868,7 @@ void GCS_MAVLINK::send_high_latency2() const base_mode(), // Field for custom payload. base mode (arming status) in ArduPilot's case 0, // Field for custom payload. 0); // Field for custom payload. +#endif } int8_t GCS_MAVLINK::high_latency_air_temperature() const diff --git a/libraries/GCS_MAVLink/GCS_Dummy.h b/libraries/GCS_MAVLink/GCS_Dummy.h index 2d273d58bdb5b2..e40968ca82231e 100644 --- a/libraries/GCS_MAVLink/GCS_Dummy.h +++ b/libraries/GCS_MAVLink/GCS_Dummy.h @@ -24,9 +24,7 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK private: uint32_t telem_delay() const override { return 0; } - void handleMessage(const mavlink_message_t &msg) override {} bool try_send_message(enum ap_message id) override { return true; } - bool handle_guided_request(AP_Mission::Mission_Command &cmd) override { return true; } uint8_t sysid_my_gcs() const override { return 1; } protected: diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index 436a26f5f6e1a9..ffbc50207f5757 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -41,6 +41,30 @@ extern const AP_HAL::HAL& hal; #pragma GCC diagnostic pop #endif +mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { +#if HAL_GCS_ENABLED + GCS_MAVLINK *link = gcs().chan(chan); + if (link == nullptr) { + return nullptr; + } + return link->channel_buffer(); +#else + return nullptr; +#endif +} + +mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { +#if HAL_GCS_ENABLED + GCS_MAVLINK *link = gcs().chan(chan); + if (link == nullptr) { + return nullptr; + } + return link->channel_status(); +#else + return nullptr; +#endif +} + #endif // HAL_MAVLINK_BINDINGS_ENABLED #if HAL_GCS_ENABLED @@ -65,22 +89,6 @@ GCS_MAVLINK *GCS_MAVLINK::find_by_mavtype_and_compid(uint8_t mav_type, uint8_t c return gcs().chan(channel); } -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { - GCS_MAVLINK *link = gcs().chan(chan); - if (link == nullptr) { - return nullptr; - } - return link->channel_buffer(); -} - -mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { - GCS_MAVLINK *link = gcs().chan(chan); - if (link == nullptr) { - return nullptr; - } - return link->channel_status(); -} - // set a channel as private. Private channels get sent heartbeats, but // don't get broadcast packets or forwarded packets void GCS_MAVLINK::set_channel_private(mavlink_channel_t _chan) @@ -140,7 +148,7 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len) } const size_t written = mavlink_comm_port[chan]->write(buf, len); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL - if (written < len) { + if (written < len && !mavlink_comm_port[chan]->is_write_locked()) { AP_HAL::panic("Short write on UART: %lu < %u", (unsigned long)written, len); } #else diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index 26a30d068d312c..36e0cd3fb0851b 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -43,7 +43,7 @@ #include "include/mavlink/v2.0/mavlink_types.h" -/// MAVLink stream used for uartA +/// MAVLink streams used for each telemetry port extern AP_HAL::UARTDriver *mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS]; extern bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS]; diff --git a/libraries/GCS_MAVLink/GCS_Param.cpp b/libraries/GCS_MAVLink/GCS_Param.cpp index 75cc85b63f6c0a..a4c8c1df5182cb 100644 --- a/libraries/GCS_MAVLink/GCS_Param.cpp +++ b/libraries/GCS_MAVLink/GCS_Param.cpp @@ -314,11 +314,13 @@ void GCS_MAVLINK::handle_param_set(const mavlink_message_t &msg) if (force_save && (parameter_flags & AP_PARAM_FLAG_ENABLE)) { AP_Param::invalidate_count(); } - + +#if HAL_LOGGING_ENABLED AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { logger->Write_Parameter(key, vp->cast_to_float(var_type)); } +#endif } void GCS_MAVLINK::send_parameter_value(const char *param_name, ap_var_type param_type, float param_value) @@ -351,11 +353,13 @@ void GCS::send_parameter_value(const char *param_name, ap_var_type param_type, f gcs().send_to_active_channels(MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet); +#if HAL_LOGGING_ENABLED // also log to AP_Logger AP_Logger *logger = AP_Logger::get_singleton(); if (logger != nullptr) { logger->Write_Parameter(param_name, param_value); } +#endif } diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 7a9e38b51922f6..8960a881ab45cb 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -2,6 +2,7 @@ #include #include +#include #ifndef HAL_GCS_ENABLED #define HAL_GCS_ENABLED 1 @@ -25,7 +26,7 @@ // The command was added to the spec in January 2019 and to MAVLink in // ArduPilot in 4.1.x #ifndef AP_MAVLINK_MISSION_SET_CURRENT_ENABLED -#define AP_MAVLINK_MISSION_SET_CURRENT_ENABLED 1 +#define AP_MAVLINK_MISSION_SET_CURRENT_ENABLED AP_MISSION_ENABLED #endif // AUTOPILOT_VERSION_REQUEST is slated to be removed; an instance of diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp index bea8f3dd89dba9..3d8cd593d02ee7 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.cpp @@ -95,7 +95,7 @@ uint16_t MissionItemProtocol_Fence::item_count() const return _fence.polyfence().num_stored_items(); } -static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(const mavlink_mission_item_int_t &mission_item_int, AC_PolyFenceItem &ret) +MAV_MISSION_RESULT MissionItemProtocol_Fence::convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(const mavlink_mission_item_int_t &mission_item_int, AC_PolyFenceItem &ret) { if (mission_item_int.frame != MAV_FRAME_GLOBAL && mission_item_int.frame != MAV_FRAME_GLOBAL_INT && diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h index 56cadc02eb3024..58a984f60a821a 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Fence.h @@ -2,6 +2,8 @@ #include "MissionItemProtocol.h" +#include + class AC_PolyFence_loader; class MissionItemProtocol_Fence : public MissionItemProtocol { @@ -21,7 +23,9 @@ class MissionItemProtocol_Fence : public MissionItemProtocol { static function to format mission item as mavlink_mission_item_int_t */ static bool get_item_as_mission_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet); - + + static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_AC_PolyFenceItem(const mavlink_mission_item_int_t &mission_item_int, class AC_PolyFenceItem &ret); + protected: ap_message next_item_ap_message_id() const override { diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp index 8c9271884537cd..ee33e55188e35c 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.cpp @@ -43,7 +43,9 @@ MAV_MISSION_RESULT MissionItemProtocol_Rally::append_item(const mavlink_mission_ MAV_MISSION_RESULT MissionItemProtocol_Rally::complete(const GCS_MAVLINK &_link) { +#if HAL_LOGGING_ENABLED AP::logger().Write_Rally(); +#endif return MAV_MISSION_ACCEPTED; } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h index f527f109bc5185..add6efa6ca53e5 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Rally.h @@ -20,7 +20,9 @@ class MissionItemProtocol_Rally : public MissionItemProtocol { static function to get rally item as mavlink_mission_item_int_t */ static bool get_item_as_mission_item(uint16_t seq, mavlink_mission_item_int_t &ret_packet); - + + static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, class RallyLocation &ret) WARN_IF_UNUSED; + protected: ap_message next_item_ap_message_id() const override { @@ -42,8 +44,6 @@ class MissionItemProtocol_Rally : public MissionItemProtocol { const mavlink_mission_request_int_t &packet, mavlink_mission_item_int_t &ret_packet) override WARN_IF_UNUSED; - static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_RallyLocation(const mavlink_mission_item_int_t &cmd, class RallyLocation &ret) WARN_IF_UNUSED; - }; #endif // HAL_RALLY_ENABLED diff --git a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp index 311b27e157d85e..3605dc152b85f1 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol_Waypoints.cpp @@ -58,7 +58,9 @@ bool MissionItemProtocol_Waypoints::clear_all_items() MAV_MISSION_RESULT MissionItemProtocol_Waypoints::complete(const GCS_MAVLINK &_link) { _link.send_text(MAV_SEVERITY_INFO, "Flight plan received"); +#if HAL_LOGGING_ENABLED AP::logger().Write_EntireMission(); +#endif return MAV_MISSION_ACCEPTED; } diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index fb85273f26eccb..ef80a7ab849cf0 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -107,6 +107,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Param: OPTION // @DisplayName: RC input option // @Description: Function assigned to this RC channel + // @SortValues: AlphabeticalZeroAtTop // @Values{Copter, Rover, Plane, Blimp}: 0:Do Nothing // @Values{Copter}: 2:FLIP Mode // @Values{Copter}: 3:Simple Mode @@ -131,7 +132,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Copter}: 25:AttCon Feed Forward // @Values{Copter}: 26:AttCon Accel Limits // @Values{Copter, Rover, Plane}: 27:Retract Mount1 - // @Values{Copter, Rover, Plane}: 28:Relay On/Off + // @Values{Copter, Rover, Plane}: 28:Relay1 On/Off // @Values{Copter, Plane}: 29:Landing Gear // @Values{Copter}: 30:Lost Copter Sound // @Values{Rover}: 30:Lost Rover Sound @@ -207,6 +208,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Plane}: 108:QRTL Mode // @Values{Copter}: 109:use Custom Controller // @Values{Copter, Rover, Plane, Blimp}: 110:KillIMU3 + // @Values{Copter,Plane,Rover,Blimp,Sub,Tracker}: 112:SwitchExternalAHRS // @Values{Plane}: 150:CRUISE Mode // @Values{Copter}: 151:TURTLE Mode // @Values{Copter}: 152:SIMPLE heading reset @@ -700,6 +702,7 @@ void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPo case AUX_FUNC::CAMERA_MANUAL_FOCUS: case AUX_FUNC::CAMERA_AUTO_FOCUS: case AUX_FUNC::CAMERA_LENS: + case AUX_FUNC::AHRS_TYPE: run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT); break; default: @@ -875,14 +878,14 @@ void RC_Channel::do_aux_function_avoid_adsb(const AuxSwitchPos ch_flag) return; } avoidance->enable(); - AP::logger().Write_Event(LogEvent::AVOIDANCE_ADSB_ENABLE); + LOGGER_WRITE_EVENT(LogEvent::AVOIDANCE_ADSB_ENABLE); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB Avoidance Enabled"); return; } // disable AP_Avoidance avoidance->disable(); - AP::logger().Write_Event(LogEvent::AVOIDANCE_ADSB_DISABLE); + LOGGER_WRITE_EVENT(LogEvent::AVOIDANCE_ADSB_DISABLE); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "ADSB Avoidance Disabled"); #endif } @@ -1214,6 +1217,7 @@ bool RC_Channel::run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFun #endif const bool ret = do_aux_function(ch_option, pos); +#if HAL_LOGGING_ENABLED // @LoggerMessage: AUXF // @Description: Auxiliary function invocation information // @Field: TimeUS: Time since system startup @@ -1236,6 +1240,8 @@ bool RC_Channel::run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFun uint8_t(source), uint8_t(ret) ); +#endif + return ret; } @@ -1313,11 +1319,13 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos break; #endif +#if AP_BATTERY_ENABLED case AUX_FUNC::BATTERY_MPPT_ENABLE: if (ch_flag != AuxSwitchPos::MIDDLE) { AP::battery().MPPT_set_powered_state_to_all(ch_flag == AuxSwitchPos::HIGH); } break; +#endif #if HAL_SPRAYER_ENABLED case AUX_FUNC::SPRAYER: @@ -1542,6 +1550,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos } #endif +#if HAL_LOGGING_ENABLED case AUX_FUNC::LOG_PAUSE: { AP_Logger *logger = AP_Logger::get_singleton(); switch (ch_flag) { @@ -1557,6 +1566,7 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos } break; } +#endif #if COMPASS_CAL_ENABLED case AUX_FUNC::MAG_CAL: { @@ -1613,6 +1623,14 @@ bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos AP::ahrs().request_yaw_reset(); break; + case AUX_FUNC::AHRS_TYPE: { +#if HAL_NAVEKF3_AVAILABLE && HAL_EXTERNAL_AHRS_ENABLED + AP::ahrs().set_ekf_type(ch_flag==AuxSwitchPos::HIGH? AP_AHRS::EKFType::EXTERNAL : AP_AHRS::EKFType::THREE); +#endif + break; + } + + #if HAL_TORQEEDO_ENABLED // clear torqeedo error case AUX_FUNC::TORQEEDO_CLEAR_ERR: { diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index bf9d70f5bcf4ed..5216f2a4ac7ab3 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -190,7 +190,7 @@ class RC_Channel { TER_DISABLE = 86, // disable terrain following in CRUISE/FBWB modes CROW_SELECT = 87, // select CROW mode for diff spoilers;high disables,mid forces progressive SOARING = 88, // three-position switch to set soaring mode - LANDING_FLARE = 89, // force flare, throttle forced idle, pitch to LAND_PITCH_CD, tilts up + LANDING_FLARE = 89, // force flare, throttle forced idle, pitch to LAND_PITCH_DEG, tilts up EKF_POS_SOURCE = 90, // change EKF position source between primary, secondary and tertiary sources ARSPD_CALIBRATE= 91, // calibrate airspeed ratio FBWA = 92, // Fly-By-Wire-A @@ -216,6 +216,7 @@ class RC_Channel { CUSTOM_CONTROLLER = 109, // use Custom Controller KILL_IMU3 = 110, // disable third IMU (for IMU failure testing) LOWEHEISER_STARTER = 111, // allows for manually running starter + AHRS_TYPE = 112, // change AHRS_EKF_TYPE // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp! // also, if you add an option >255, you will need to fix duplicate_options_exist diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 8578904f5a4603..0ecdaba6d9fbf9 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -169,10 +169,12 @@ void RC_Channels::read_aux_all() } need_log |= c->read_aux(); } +#if HAL_LOGGING_ENABLED if (need_log) { // guarantee that we log when a switch changes AP::logger().Write_RCIN(); } +#endif } void RC_Channels::init_aux_all() diff --git a/libraries/RC_Channel/RC_Channels_VarInfo.h b/libraries/RC_Channel/RC_Channels_VarInfo.h index 8302be931d6a52..a824cc1ececc34 100644 --- a/libraries/RC_Channel/RC_Channels_VarInfo.h +++ b/libraries/RC_Channel/RC_Channels_VarInfo.h @@ -101,7 +101,7 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = { // @DisplayName: RC protocols enabled // @Description: Bitmask of enabled RC protocols. Allows narrowing the protocol detection to only specific types of RC receivers which can avoid issues with incorrect detection. Set to 1 to enable all protocols. // @User: Advanced - // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS + // @Bitmask: 0:All,1:PPM,2:IBUS,3:SBUS,4:SBUS_NI,5:DSM,6:SUMD,7:SRXL,8:SRXL2,9:CRSF,10:ST24,11:FPORT,12:FPORT2,13:FastSBUS,14:DroneCAN,15:Ghost AP_GROUPINFO("_PROTOCOLS", 34, RC_CHANNELS_SUBCLASS, _protocols, 1), // @Param: _FS_TIMEOUT diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp index 784d032902642a..2168b922946e0c 100644 --- a/libraries/SITL/SIM_ADSB.cpp +++ b/libraries/SITL/SIM_ADSB.cpp @@ -164,7 +164,7 @@ void ADSB::send_report(const class Aircraft &aircraft) { if (AP_HAL::millis() < 10000) { // simulated aircraft don't appear until 10s after startup. This avoids a windows - // threading issue with non-blocking sockets and the initial wait on uartA + // threading issue with non-blocking sockets and the initial wait on SERIAL0 return; } diff --git a/libraries/SITL/SIM_ADSB.h b/libraries/SITL/SIM_ADSB.h index 9b9682c15b515a..c80cf2668e0dbd 100644 --- a/libraries/SITL/SIM_ADSB.h +++ b/libraries/SITL/SIM_ADSB.h @@ -22,7 +22,7 @@ #if HAL_SIM_ADSB_ENABLED -#include +#include #include "SIM_Aircraft.h" diff --git a/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp b/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp index 88a76717d21b97..68ed4470efe895 100644 --- a/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp +++ b/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp @@ -143,8 +143,7 @@ void ADSB_Sagetech_MXS::handle_message() bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::Installation& opmsg) { - assert_storage_size _assert_storage_size_Installation; - (void)_assert_storage_size_Installation; + ASSERT_STORAGE_SIZE(Installation, 36); if (operating_mode != OperatingMode::OFF && operating_mode != OperatingMode::MAINTENANCE) { @@ -181,8 +180,7 @@ void ADSB_Sagetech_MXS::assert_good_flight_id() bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::FlightIDMessage& _msg) { - assert_storage_size _assert_storage_size_FlightIDMessage; - (void)_assert_storage_size_FlightIDMessage; + ASSERT_STORAGE_SIZE(FlightIDMessage, 12); // do something! flight_id_set_time_ms = AP_HAL::millis(); @@ -194,8 +192,7 @@ bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::FlightIDMe bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::Operating& _msg) { - assert_storage_size _assert_storage_size_Operating; - (void)_assert_storage_size_Operating; + ASSERT_STORAGE_SIZE(Operating, 12); // do something! @@ -222,8 +219,7 @@ double ADSB_Sagetech_MXS::lat_string_to_double(const uint8_t *str) bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::GPS& _msg) { - assert_storage_size _assert_storage_size_GPS; - (void)_assert_storage_size_GPS; + ASSERT_STORAGE_SIZE(GPS, 63); // store data to transmit via ADSB info_from_vehicle.gps.lat = lat_string_to_double(_msg.latitude); @@ -234,8 +230,7 @@ bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::GPS& _msg) bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::DataRequest& _msg) { - assert_storage_size _assert_storage_size_DataRequest; - (void)_assert_storage_size_DataRequest; + ASSERT_STORAGE_SIZE(DataRequest, 4); // handle request to send data to vehicle. Note that the // specification says (on page 32) that the ack is sent before the @@ -246,8 +241,7 @@ bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::DataReques bool ADSB_Sagetech_MXS::handle_message(const SITL::ADSB_Sagetech_MXS::TargetRequest& _msg) { - assert_storage_size _assert_storage_size_TargetRequest; - (void)_assert_storage_size_TargetRequest; + ASSERT_STORAGE_SIZE(TargetRequest, 7); // handle request to send adsb data to vehicle as it is received diff --git a/libraries/SITL/SIM_AIS.cpp b/libraries/SITL/SIM_AIS.cpp index 4372cabd5dd4f3..6aae9c3717331f 100644 --- a/libraries/SITL/SIM_AIS.cpp +++ b/libraries/SITL/SIM_AIS.cpp @@ -14,7 +14,7 @@ */ /* Dump logged AIS data to the serial port - ./Tools/autotest/sim_vehicle.py -v Rover -A --uartF=sim:AIS --custom-location 51.58689798356386,-3.9044570193067965,0,0 + ./Tools/autotest/sim_vehicle.py -v Rover -A --serial5=sim:AIS --custom-location 51.58689798356386,-3.9044570193067965,0,0 param set SERIAL5_PROTOCOL 40 param set AIS_TYPE 1 diff --git a/libraries/SITL/SIM_AIS.h b/libraries/SITL/SIM_AIS.h index 14bc6c90159f9f..395f0f11e5bec5 100644 --- a/libraries/SITL/SIM_AIS.h +++ b/libraries/SITL/SIM_AIS.h @@ -14,7 +14,7 @@ */ /* Dump logged AIS data to the serial port - ./Tools/autotest/sim_vehicle.py -v Rover --no-mavproxy -A --uartF=sim:AIS --custom-location 51.58689798356386,-3.9044570193067965,0,0 + ./Tools/autotest/sim_vehicle.py -v Rover --no-mavproxy -A --serial5=sim:AIS --custom-location 51.58689798356386,-3.9044570193067965,0,0 param set SERIAL5_PROTOCOL 40 param set AIS_TYPE 1 diff --git a/libraries/SITL/SIM_AirSim.h b/libraries/SITL/SIM_AirSim.h index 7e3b447817dee1..1e9b75c0e648ae 100644 --- a/libraries/SITL/SIM_AirSim.h +++ b/libraries/SITL/SIM_AirSim.h @@ -26,7 +26,7 @@ #if HAL_SIM_AIRSIM_ENABLED -#include +#include #include "SIM_Aircraft.h" namespace SITL { @@ -78,7 +78,7 @@ class AirSim : public Aircraft { // connection_info_.sitl_ip_port uint16_t airsim_control_port = 9002; - SocketAPM sock; + SocketAPM_native sock; double average_frame_time; uint64_t frame_counter; diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 07d0f6064e66ec..ed43366ab6c0b4 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -456,6 +456,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) last_speedup = sitl->speedup; } +#if HAL_LOGGING_ENABLED // for EKF comparison log relhome pos and velocity at loop rate static uint16_t last_ticks; uint16_t ticks = AP::scheduler().ticks(); @@ -482,6 +483,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) airspeed_pitot, achieved_rate_hz/rate_hz); } +#endif } // returns perpendicular height to surface downward-facing rangefinder @@ -490,7 +492,7 @@ float Aircraft::perpendicular_distance_to_rangefinder_surface() const { switch ((Rotation)sitl->sonar_rot.get()) { case Rotation::ROTATION_PITCH_270: - return sitl->height_agl; + return sitl->state.height_agl; case ROTATION_NONE ... ROTATION_YAW_315: return sitl->measure_distance_at_angle_bf(location, sitl->sonar_rot.get()*45); default: @@ -607,7 +609,11 @@ void Aircraft::update_home() void Aircraft::update_model(const struct sitl_input &input) { local_ground_level = 0.0f; - update(input); + if (sitl != nullptr) { + update(input); + } else { + time_advance(); + } } /* @@ -1008,6 +1014,13 @@ void Aircraft::update_external_payload(const struct sitl_input &input) richenpower->update(input); } +#if AP_SIM_LOWEHEISER_ENABLED + // update Loweheiser generator + if (loweheiser) { + loweheiser->update(); + } +#endif + if (fetteconewireesc) { fetteconewireesc->update(*this); } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 1c867d56c11286..8b40a4eba281fa 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -30,6 +30,7 @@ #include "SIM_Parachute.h" #include "SIM_Precland.h" #include "SIM_RichenPower.h" +#include "SIM_Loweheiser.h" #include "SIM_FETtecOneWireESC.h" #include "SIM_I2C.h" #include "SIM_Buzzer.h" @@ -146,6 +147,9 @@ class Aircraft { void set_parachute(Parachute *_parachute) { parachute = _parachute; } void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; } void set_adsb(class ADSB *_adsb) { adsb = _adsb; } +#if AP_SIM_LOWEHEISER_ENABLED + void set_loweheiser(Loweheiser *_loweheiser) { loweheiser = _loweheiser; } +#endif void set_fetteconewireesc(FETtecOneWireESC *_fetteconewireesc) { fetteconewireesc = _fetteconewireesc; } void set_ie24(IntelligentEnergy24 *_ie24) { ie24 = _ie24; } void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; } @@ -341,6 +345,9 @@ class Aircraft { Gripper_EPM *gripper_epm; Parachute *parachute; RichenPower *richenpower; +#if AP_SIM_LOWEHEISER_ENABLED + Loweheiser *loweheiser; +#endif FETtecOneWireESC *fetteconewireesc; IntelligentEnergy24 *ie24; diff --git a/libraries/SITL/SIM_BattMonitor_SMBus.cpp b/libraries/SITL/SIM_BattMonitor_SMBus.cpp index 2f9fe08df5c300..cd9003e9827bbe 100644 --- a/libraries/SITL/SIM_BattMonitor_SMBus.cpp +++ b/libraries/SITL/SIM_BattMonitor_SMBus.cpp @@ -1,7 +1,5 @@ #include "SIM_BattMonitor_SMBus.h" -#include - SITL::SIM_BattMonitor_SMBus::SIM_BattMonitor_SMBus() : SMBusDevice() { diff --git a/libraries/SITL/SIM_Blimp.cpp b/libraries/SITL/SIM_Blimp.cpp index c5f8835b851b50..844fed4f8d6736 100644 --- a/libraries/SITL/SIM_Blimp.cpp +++ b/libraries/SITL/SIM_Blimp.cpp @@ -106,6 +106,7 @@ void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc, Vector3f rot_T{0,0,0}; +#if HAL_LOGGING_ENABLED AP::logger().WriteStreaming("SFT", "TimeUS,f0,f1,f2,f3", "Qffff", AP_HAL::micros64(), @@ -138,6 +139,7 @@ void Blimp::calculate_forces(const struct sitl_input &input, Vector3f &body_acc, "Qfff", AP_HAL::micros64(), rot_T.x, rot_T.y, rot_T.z); +#endif // HAL_LOGGING_ENABLED #if 0 //"Wobble" attempt rot_T.y = fin[0].Fz * radius + fin[1].Fz * radius; diff --git a/libraries/SITL/SIM_Blimp.h b/libraries/SITL/SIM_Blimp.h index 042341c5a1bc52..1cdfc504df2c82 100644 --- a/libraries/SITL/SIM_Blimp.h +++ b/libraries/SITL/SIM_Blimp.h @@ -68,7 +68,7 @@ class Blimp : public Aircraft { float drag_gyr_constant; void calculate_forces(const struct sitl_input &input, Vector3f &rot_accel, Vector3f &body_accel); - float sq(float a) {return pow(a,2);} + float sq(float a) {return powf(a,2);} }; } diff --git a/libraries/SITL/SIM_CRRCSim.h b/libraries/SITL/SIM_CRRCSim.h index eb6d01c3b4a98a..b3a5699f99156e 100644 --- a/libraries/SITL/SIM_CRRCSim.h +++ b/libraries/SITL/SIM_CRRCSim.h @@ -26,7 +26,7 @@ #if HAL_SIM_CRRCSIM_ENABLED -#include +#include #include "SIM_Aircraft.h" @@ -81,7 +81,7 @@ class CRRCSim : public Aircraft { bool heli_servos; double last_timestamp; - SocketAPM sock; + SocketAPM_native sock; }; } // namespace SITL diff --git a/libraries/SITL/SIM_CRSF.h b/libraries/SITL/SIM_CRSF.h index bd02e92dded553..c4fcda7efabf87 100644 --- a/libraries/SITL/SIM_CRSF.h +++ b/libraries/SITL/SIM_CRSF.h @@ -15,7 +15,7 @@ /* Simulated CRSF device -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:crsf --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:crsf --speedup=1 param set SERIAL5_PROTOCOL 23 reboot diff --git a/libraries/SITL/SIM_EFI_Hirth.cpp b/libraries/SITL/SIM_EFI_Hirth.cpp index ba6faece7ee29f..2dbafba610b01a 100644 --- a/libraries/SITL/SIM_EFI_Hirth.cpp +++ b/libraries/SITL/SIM_EFI_Hirth.cpp @@ -206,8 +206,7 @@ void SITL::EFI_Hirth::send_record1() write_to_autopilot((char*)&packed_record1, sizeof(packed_record1)); - assert_storage_size _assert_storage_size_Record1; - (void)_assert_storage_size_Record1; + ASSERT_STORAGE_SIZE(Record1, 84); } void SITL::EFI_Hirth::send_record2() @@ -224,8 +223,7 @@ void SITL::EFI_Hirth::send_record2() write_to_autopilot((char*)&packed_record2, sizeof(packed_record2)); - assert_storage_size _assert_storage_size_Record2; - (void)_assert_storage_size_Record2; + ASSERT_STORAGE_SIZE(Record2, 98); } @@ -243,6 +241,5 @@ void SITL::EFI_Hirth::send_record3() write_to_autopilot((char*)&packed_record3, sizeof(packed_record3)); - assert_storage_size _assert_storage_size_Record3; - (void)_assert_storage_size_Record3; + ASSERT_STORAGE_SIZE(Record3, 100); } diff --git a/libraries/SITL/SIM_EFI_Hirth.h b/libraries/SITL/SIM_EFI_Hirth.h index cfc32517e6a76e..0aec264f765a9c 100644 --- a/libraries/SITL/SIM_EFI_Hirth.h +++ b/libraries/SITL/SIM_EFI_Hirth.h @@ -15,7 +15,7 @@ /* simulate Hirth EFI system -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:hirth --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --serial5=sim:hirth --speedup=1 param set SERIAL5_PROTOCOL 24 param set SIM_EFI_TYPE 6 param set EFI_TYPE 6 @@ -29,7 +29,7 @@ status EFI_STATUS #pragma once #include -#include +#include #include "SIM_SerialDevice.h" namespace SITL { diff --git a/libraries/SITL/SIM_EFI_MegaSquirt.h b/libraries/SITL/SIM_EFI_MegaSquirt.h index 813e22e5e65d90..7c0151a1657198 100644 --- a/libraries/SITL/SIM_EFI_MegaSquirt.h +++ b/libraries/SITL/SIM_EFI_MegaSquirt.h @@ -15,7 +15,7 @@ /* simulate MegaSquirt EFI system -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --uartF=sim:megasquirt --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduPlane -A --serial5=sim:megasquirt --speedup=1 param set SERIAL5_PROTOCOL 24 param set SIM_EFI_TYPE 1 param set EFI_TYPE 1 @@ -28,8 +28,8 @@ status EFI_STATUS #pragma once +#include #include -#include #include "SIM_SerialDevice.h" namespace SITL { diff --git a/libraries/SITL/SIM_FETtecOneWireESC.h b/libraries/SITL/SIM_FETtecOneWireESC.h index 2d983e3b0e6cc5..a434d066987bf7 100644 --- a/libraries/SITL/SIM_FETtecOneWireESC.h +++ b/libraries/SITL/SIM_FETtecOneWireESC.h @@ -15,7 +15,7 @@ /* Simulator for the FETtecOneWire ESCs -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:fetteconewireesc --speedup=1 --console +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:fetteconewireesc --speedup=1 --console param set SERIAL5_PROTOCOL 38 param set SERIAL5_BAUD 500000 diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index afd89d2be125e2..36d63e0d67cdc3 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -590,7 +590,7 @@ void FlightAxis::socket_creator(void) pthread_cond_wait(&sockcond2, &sockmtx); } pthread_mutex_unlock(&sockmtx); - auto *sck = new SocketAPM(false); + auto *sck = new SocketAPM_native(false); if (sck == nullptr) { usleep(500); continue; diff --git a/libraries/SITL/SIM_FlightAxis.h b/libraries/SITL/SIM_FlightAxis.h index 592cc741a52848..0567997e1fee5f 100644 --- a/libraries/SITL/SIM_FlightAxis.h +++ b/libraries/SITL/SIM_FlightAxis.h @@ -26,7 +26,7 @@ #if HAL_SIM_FLIGHTAXIS_ENABLED -#include +#include #include "SIM_Aircraft.h" @@ -193,8 +193,8 @@ class FlightAxis : public Aircraft { const char *controller_ip = "127.0.0.1"; uint16_t controller_port = 18083; - SocketAPM *socknext; - SocketAPM *sock; + SocketAPM_native *socknext; + SocketAPM_native *sock; char replybuf[10000]; pid_t socket_pid; uint32_t sock_error_count; diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index e16ceabb8f9f80..a9b49040427371 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -413,7 +413,7 @@ void Frame::load_frame_params(const char *model_json) // use default value continue; } - if (vars[i].t == VarType::FLOAT) { + if (per_motor_vars[i].t == VarType::FLOAT) { parse_float(v, label_name, *(((float *)per_motor_vars[i].ptr) + j)); } else if (per_motor_vars[i].t == VarType::VECTOR3F) { diff --git a/libraries/SITL/SIM_Frsky_D.h b/libraries/SITL/SIM_Frsky_D.h index 5d5c8fa96a36a8..eccd37db325a06 100644 --- a/libraries/SITL/SIM_Frsky_D.h +++ b/libraries/SITL/SIM_Frsky_D.h @@ -15,7 +15,7 @@ /* Simulated Frsky D device -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:frsky-d --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:frsky-d --speedup=1 param set SERIAL5_PROTOCOL 3 reboot diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 63e4b99441c85d..87eafb00c43081 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -18,7 +18,7 @@ Usage example: param set SERIAL5_PROTOCOL 5 - sim_vehicle.py -D --console --map -A --uartB=sim:gps:2 + sim_vehicle.py -D --console --map -A --serial5=sim:gps:2 */ #pragma once @@ -74,13 +74,6 @@ class GPS_Backend { virtual void update_read(); // writing fix information to autopilot (e.g.) virtual void publish(const GPS_Data *d) = 0; - -protected: - - uint8_t instance; - GPS &front; - - class SIM *_sitl; struct GPS_TOW { // Number of weeks since midnight 5-6 January 1980 @@ -90,6 +83,14 @@ class GPS_Backend { }; static GPS_TOW gps_time(); + +protected: + + uint8_t instance; + GPS &front; + + class SIM *_sitl; + static void simulation_timeval(struct timeval *tv); }; diff --git a/libraries/SITL/SIM_GPS_FILE.cpp b/libraries/SITL/SIM_GPS_FILE.cpp index e4c096f1eba7c8..60868d4355cb6a 100644 --- a/libraries/SITL/SIM_GPS_FILE.cpp +++ b/libraries/SITL/SIM_GPS_FILE.cpp @@ -6,6 +6,9 @@ #include #include +#include +#include +#include extern const AP_HAL::HAL& hal; diff --git a/libraries/SITL/SIM_Gazebo.h b/libraries/SITL/SIM_Gazebo.h index 4da05353bae182..6a13d8b8e51441 100644 --- a/libraries/SITL/SIM_Gazebo.h +++ b/libraries/SITL/SIM_Gazebo.h @@ -27,7 +27,7 @@ #if HAL_SIM_GAZEBO_ENABLED #include "SIM_Aircraft.h" -#include +#include namespace SITL { @@ -76,7 +76,7 @@ class Gazebo : public Aircraft { double last_timestamp; - SocketAPM socket_sitl; + SocketAPM_native socket_sitl; const char *_gazebo_address = "127.0.0.1"; int _gazebo_port = 9002; static const uint64_t GAZEBO_TIMEOUT_US = 5000000; diff --git a/libraries/SITL/SIM_GeneratorEngine.cpp b/libraries/SITL/SIM_GeneratorEngine.cpp new file mode 100644 index 00000000000000..5e862cf1d78167 --- /dev/null +++ b/libraries/SITL/SIM_GeneratorEngine.cpp @@ -0,0 +1,48 @@ +#include "SIM_GeneratorEngine.h" + +#include + +#include + +using namespace SITL; + +void SIM_GeneratorEngine::update() +{ + if (current_current > 1 && is_zero(desired_rpm)) { + AP_HAL::panic("Generator stalled due to high current demand (%u > 1)", (unsigned)current_current); + } else if (current_current > max_current) { + AP_HAL::panic("Generator stalled due to high current demand (run)"); + } + + // linear degradation in RPM up to maximum load + if (!is_zero(desired_rpm)) { + desired_rpm -= 1500 * (current_current/max_current); + } + + const uint32_t now = AP_HAL::millis(); + + const float max_slew_rpm = max_slew_rpm_per_second * ((now - last_rpm_update_ms) / 1000.0f); + last_rpm_update_ms = now; + const float rpm_delta = current_rpm - desired_rpm; + if (rpm_delta > 0) { + current_rpm -= MIN(max_slew_rpm, rpm_delta); + } else { + current_rpm += MIN(max_slew_rpm, abs(rpm_delta)); + } + + // update the temperature + const uint32_t time_delta_ms = now - last_heat_update_ms; + last_heat_update_ms = now; + + constexpr float heat_environment_loss_factor = 0.15f; + + const float factor = 0.0035; + temperature += (current_rpm * time_delta_ms * (1/1000.0f) * factor); + // cap the heat of the motor: + temperature = MIN(temperature, 150); + // now lose some heat to the environment + const float heat_loss = ((temperature * heat_environment_loss_factor * (time_delta_ms * (1/1000.0f)))); // lose some % of heat per second + // gcs().send_text(MAV_SEVERITY_INFO, "heat=%f loss=%f", temperature, heat_loss); + temperature -= heat_loss; + +} diff --git a/libraries/SITL/SIM_GeneratorEngine.h b/libraries/SITL/SIM_GeneratorEngine.h new file mode 100644 index 00000000000000..d4e3c885913b86 --- /dev/null +++ b/libraries/SITL/SIM_GeneratorEngine.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +namespace SITL { + +class SIM_GeneratorEngine +{ +public: + void update(); + + // input variables: + float desired_rpm; + float current_current; + float max_current; + float max_slew_rpm_per_second; + float max_rpm = 8000; + + // output variables: + float current_rpm; + float temperature = 20; + +private: + uint32_t last_rpm_update_ms; + uint32_t last_heat_update_ms; +}; + +}; // namespace SITL diff --git a/libraries/SITL/SIM_Gimbal.cpp b/libraries/SITL/SIM_Gimbal.cpp index a1997dd00fbd19..5aede11b3b40a3 100644 --- a/libraries/SITL/SIM_Gimbal.cpp +++ b/libraries/SITL/SIM_Gimbal.cpp @@ -258,7 +258,7 @@ void Gimbal::send_report(void) if (now < 10000) { // don't send gimbal reports until 10s after startup. This // avoids a windows threading issue with non-blocking sockets - // and the initial wait on uartA + // and the initial wait on SERIAL0 return; } if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { diff --git a/libraries/SITL/SIM_Gimbal.h b/libraries/SITL/SIM_Gimbal.h index 53a2212a29f028..2cedd3a1cf9c3f 100644 --- a/libraries/SITL/SIM_Gimbal.h +++ b/libraries/SITL/SIM_Gimbal.h @@ -26,7 +26,7 @@ #if HAL_SIM_GIMBAL_ENABLED -#include +#include #include "SIM_Aircraft.h" @@ -101,7 +101,7 @@ class Gimbal { uint8_t vehicle_system_id; uint8_t vehicle_component_id; - SocketAPM mav_socket; + SocketAPM_native mav_socket; struct { // socket to telem2 on aircraft bool connected; diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 51450870d74396..196301942d9654 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -36,6 +36,7 @@ #include "SIM_LM2755.h" #include "SIM_MS5525.h" #include "SIM_MS5611.h" +#include "SIM_QMC5883L.h" #include @@ -81,6 +82,9 @@ static LM2755 lm2755; static IS31FL3195 is31fl3195; #define SIM_IS31FL3195_ADDR 0x54 #endif +#if AP_SIM_COMPASS_QMC5883L_ENABLED +static QMC5883L qmc5883l; +#endif struct i2c_device_at_address { uint8_t bus; @@ -116,6 +120,9 @@ struct i2c_device_at_address { { 2, 0x40, tsys03 }, #endif { 2, 0x77, ms5611 }, // MS5611: BARO_PROBE_EXT = 2 +#if AP_SIM_COMPASS_QMC5883L_ENABLED + { 2, 0x0D, qmc5883l }, +#endif }; void I2C::init() diff --git a/libraries/SITL/SIM_InertialLabs.cpp b/libraries/SITL/SIM_InertialLabs.cpp new file mode 100644 index 00000000000000..728c7934f9cf7c --- /dev/null +++ b/libraries/SITL/SIM_InertialLabs.cpp @@ -0,0 +1,124 @@ +/* + 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 InertialLabs external AHRS +*/ +#include "SIM_InertialLabs.h" +#include +#include "SIM_GPS.h" + +using namespace SITL; + +InertialLabs::InertialLabs() : SerialDevice::SerialDevice() +{ +} + +void InertialLabs::send_packet(void) +{ + const auto &fdm = _sitl->state; + + pkt.msg_len = sizeof(pkt)-2; + + pkt.accel_data_hr.x = (fdm.yAccel * 1.0e6)/GRAVITY_MSS; + pkt.accel_data_hr.y = (fdm.xAccel * 1.0e6)/GRAVITY_MSS; + pkt.accel_data_hr.z = (-fdm.zAccel * 1.0e6)/GRAVITY_MSS; + + pkt.gyro_data_hr.y = fdm.rollRate*1.0e5; + pkt.gyro_data_hr.x = fdm.pitchRate*1.0e5; + pkt.gyro_data_hr.z = -fdm.yawRate*1.0e5; + + float sigma, delta, theta; + AP_Baro::SimpleAtmosphere((fdm.altitude+rand_float()*0.25) * 0.001, sigma, delta, theta); + pkt.baro_data.pressure_pa2 = SSL_AIR_PRESSURE * delta * 0.5; + pkt.baro_data.baro_alt = fdm.altitude; + pkt.temperature = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta); + + pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS)*0.1; + pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS)*0.1; + pkt.mag_data.z = (-fdm.bodyMagField.z / NTESLA_TO_MGAUSS)*0.1; + + pkt.orientation_angles.roll = fdm.rollDeg*100; + pkt.orientation_angles.pitch = fdm.pitchDeg*100; + pkt.orientation_angles.yaw = fdm.yawDeg*100; + + pkt.velocity.x = fdm.speedE*100; + pkt.velocity.y = fdm.speedN*100; + pkt.velocity.z = -fdm.speedD*100; + + pkt.position.lat = fdm.latitude*1e7; + pkt.position.lon = fdm.longitude*1e7; + pkt.position.alt = fdm.altitude*1e2; + + pkt.kf_vel_covariance.x = 10; + pkt.kf_vel_covariance.z = 10; + pkt.kf_vel_covariance.z = 10; + + pkt.kf_pos_covariance.x = 20; + pkt.kf_pos_covariance.z = 20; + pkt.kf_pos_covariance.z = 20; + + const auto gps_tow = GPS_Backend::gps_time(); + + pkt.gps_ins_time_ms = gps_tow.ms; + + pkt.gnss_new_data = 0; + + if (packets_sent % gps_frequency == 0) { + // update GPS data at 5Hz + pkt.gps_week = gps_tow.week; + pkt.gnss_pos_timestamp = gps_tow.ms; + pkt.gnss_new_data = 3; + pkt.gps_position.lat = pkt.position.lat; + pkt.gps_position.lon = pkt.position.lon; + pkt.gps_position.alt = pkt.position.alt; + + pkt.num_sats = 32; + pkt.gnss_vel_track.hor_speed = norm(fdm.speedN,fdm.speedE)*100; + Vector2d track{fdm.speedN,fdm.speedE}; + pkt.gnss_vel_track.track_over_ground = wrap_360(degrees(track.angle()))*100; + pkt.gnss_vel_track.ver_speed = -fdm.speedD*100; + + pkt.gnss_extended_info.fix_type = 2; + } + + pkt.differential_pressure = 0.5*sq(fdm.airspeed+fabsf(rand_float()*0.25))*1.0e4; + pkt.supply_voltage = 12.3*100; + pkt.temperature = 23.4*10; + + const uint8_t *buffer = (const uint8_t *)&pkt; + pkt.crc = crc_sum_of_bytes_16(&buffer[2], sizeof(pkt)-4); + + write_to_autopilot((char *)&pkt, sizeof(pkt)); + + packets_sent++; +} + + +/* + send InertialLabs data + */ +void InertialLabs::update(void) +{ + if (!init_sitl_pointer()) { + return; + } + const uint32_t us_between_packets = 5000; // 200Hz + const uint32_t now = AP_HAL::micros(); + if (now - last_pkt_us >= us_between_packets) { + last_pkt_us = now; + send_packet(); + } + +} diff --git a/libraries/SITL/SIM_InertialLabs.h b/libraries/SITL/SIM_InertialLabs.h new file mode 100644 index 00000000000000..448d5bf48bd192 --- /dev/null +++ b/libraries/SITL/SIM_InertialLabs.h @@ -0,0 +1,123 @@ +//usage: +//PARAMS: +// param set AHRS_EKF_TYPE 11 +// param set EAHRS_TYPE 5 +// param set SERIAL4_PROTOCOL 36 +// param set SERIAL4_BAUD 460800 +// sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:ILabs +#pragma once + +#include "SIM_Aircraft.h" + +#include +#include "SIM_SerialDevice.h" + +namespace SITL +{ + +class InertialLabs : public SerialDevice +{ +public: + + InertialLabs(); + + // update state + void update(void); + +private: + void send_packet(void); + + struct PACKED vec3_16_t { + int16_t x,y,z; + }; + struct PACKED vec3_32_t { + int32_t x,y,z; + }; + struct PACKED vec3_u8_t { + uint8_t x,y,z; + }; + struct PACKED vec3_u16_t { + uint16_t x,y,z; + }; + + struct gnss_extended_info_t { + uint8_t fix_type; + uint8_t spoofing_status; + }; + + struct gnss_info_short_t { + uint8_t info1; + uint8_t info2; + }; + + struct PACKED ILabsPacket { + uint16_t magic = 0x55AA; + uint8_t msg_type = 1; + uint8_t msg_id = 0x95; + uint16_t msg_len; // total packet length-2 + + // send Table4, 27 messages + uint8_t num_messages = 27; + uint8_t messages[27] = { + 0x01, 0x3C, 0x23, 0x21, 0x25, 0x24, 0x07, 0x12, 0x10, 0x58, 0x57, 0x53, 0x4a, + 0x3b, 0x30, 0x32, 0x3e, 0x36, 0x41, 0xc0, 0x28, 0x86, 0x8a, 0x8d, 0x50, + 0x52, 0x5a + }; + uint32_t gps_ins_time_ms; // ms since start of GPS week for IMU data + uint16_t gps_week; + vec3_32_t accel_data_hr; // g * 1e6 + vec3_32_t gyro_data_hr; // deg/s * 1e5 + struct PACKED { + uint16_t pressure_pa2; // Pascals/2 + int32_t baro_alt; // meters*100 + } baro_data; + vec3_16_t mag_data; // nT/10 + struct PACKED { + int16_t yaw; // deg*100 + int16_t pitch; // deg*100 + int16_t roll; // deg*100 + } orientation_angles; // 321 euler order + vec3_32_t velocity; // m/s * 100 + struct PACKED { + int32_t lat; // deg*1e7 + int32_t lon; // deg*1e7 + int32_t alt; // m*100, AMSL + } position; + vec3_u8_t kf_vel_covariance; // mm/s + vec3_u16_t kf_pos_covariance; + uint16_t unit_status; + gnss_extended_info_t gnss_extended_info; + uint8_t num_sats; + struct PACKED { + int32_t lat; // deg*1e7 + int32_t lon; // deg*1e7 + int32_t alt; // m*100, AMSL + } gps_position; + struct PACKED { + int32_t hor_speed; // m/s*100 + uint16_t track_over_ground; // deg*100 + int32_t ver_speed; // m/s*100 + } gnss_vel_track; + uint32_t gnss_pos_timestamp; // ms + gnss_info_short_t gnss_info_short; + uint8_t gnss_new_data; + uint8_t gnss_jam_status; + int32_t differential_pressure; // mbar*1e4 + int16_t true_airspeed; // m/s*100 + vec3_16_t wind_speed; // m/s*100 + uint16_t air_data_status; + uint16_t supply_voltage; // V*100 + int16_t temperature; // degC*10 + uint16_t unit_status2; + uint16_t crc; + } pkt; + + uint32_t last_pkt_us; + const uint16_t pkt_rate_hz = 200; + const uint16_t gps_rate_hz = 10; + const uint16_t gps_frequency = pkt_rate_hz / gps_rate_hz; + uint32_t packets_sent; +}; + +} + diff --git a/libraries/SITL/SIM_IntelligentEnergy24.h b/libraries/SITL/SIM_IntelligentEnergy24.h index 68d8782937f627..794addc7a9b033 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.h +++ b/libraries/SITL/SIM_IntelligentEnergy24.h @@ -15,7 +15,7 @@ /* Simulator for the IntelligentEnergy 2.4kW FuelCell -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:ie24 --speedup=1 --console +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:ie24 --speedup=1 --console param set SERIAL5_PROTOCOL 30 # Generator param set SERIAL5_BAUD 115200 diff --git a/libraries/SITL/SIM_Invensense_v3.cpp b/libraries/SITL/SIM_Invensense_v3.cpp index 2724daf90ad7f5..a13dfa134894b8 100644 --- a/libraries/SITL/SIM_Invensense_v3.cpp +++ b/libraries/SITL/SIM_Invensense_v3.cpp @@ -4,8 +4,7 @@ void SITL::InvensenseV3::update(const class Aircraft &aircraft) { - assert_storage_size _assert_fifo_size; - (void)_assert_fifo_size; + ASSERT_STORAGE_SIZE(FIFOData, 16); const SIM *sitl = AP::sitl(); const int16_t xAccel = sitl->state.xAccel / accel_scale(); diff --git a/libraries/SITL/SIM_JEDEC.cpp b/libraries/SITL/SIM_JEDEC.cpp index 9b4255f78139c1..9db090b7b665e8 100644 --- a/libraries/SITL/SIM_JEDEC.cpp +++ b/libraries/SITL/SIM_JEDEC.cpp @@ -4,6 +4,9 @@ #include #include +#include +#include +#include #include diff --git a/libraries/SITL/SIM_JSBSim.h b/libraries/SITL/SIM_JSBSim.h index fda323e943f294..bbb2c1a46a0f4b 100644 --- a/libraries/SITL/SIM_JSBSim.h +++ b/libraries/SITL/SIM_JSBSim.h @@ -26,7 +26,7 @@ #if HAL_SIM_JSBSIM_ENABLED -#include +#include #include "SIM_Aircraft.h" @@ -49,10 +49,10 @@ class JSBSim : public Aircraft { private: // tcp input control socket to JSBSIm - SocketAPM sock_control; + SocketAPM_native sock_control; // UDP packets from JSBSim in fgFDM format - SocketAPM sock_fgfdm; + SocketAPM_native sock_fgfdm; bool initialised; diff --git a/libraries/SITL/SIM_JSON.h b/libraries/SITL/SIM_JSON.h index 22582f0ae48083..b168219d9d351f 100644 --- a/libraries/SITL/SIM_JSON.h +++ b/libraries/SITL/SIM_JSON.h @@ -22,7 +22,7 @@ #if HAL_SIM_JSON_ENABLED -#include +#include #include "SIM_Aircraft.h" namespace SITL { @@ -64,7 +64,7 @@ class JSON : public Aircraft { // default connection_info_.sitl_ip_port uint16_t control_port = 9002; - SocketAPM sock; + SocketAPM_native sock; uint32_t frame_counter; double last_timestamp_s; diff --git a/libraries/SITL/SIM_JSON_Master.cpp b/libraries/SITL/SIM_JSON_Master.cpp index 835dae013644b5..0130a7067dc068 100644 --- a/libraries/SITL/SIM_JSON_Master.cpp +++ b/libraries/SITL/SIM_JSON_Master.cpp @@ -22,6 +22,8 @@ #include #include +#include +#include using namespace SITL; @@ -97,6 +99,7 @@ void JSON_Master::receive(struct sitl_input &input) } } +#if HAL_LOGGING_ENABLED const bool use_servos = list->instance == master_instance; // @LoggerMessage: SLV1 @@ -156,6 +159,7 @@ void JSON_Master::receive(struct sitl_input &input) buffer.pwm[11], buffer.pwm[12], buffer.pwm[13]); +#endif if (list->instance == master_instance) { // Use the servo outs from this instance diff --git a/libraries/SITL/SIM_JSON_Master.h b/libraries/SITL/SIM_JSON_Master.h index 9a568f6ed14466..f4bcc1399417c4 100644 --- a/libraries/SITL/SIM_JSON_Master.h +++ b/libraries/SITL/SIM_JSON_Master.h @@ -27,7 +27,7 @@ #if HAL_SIM_JSON_MASTER_ENABLED #include "SITL_Input.h" -#include +#include #include namespace SITL { @@ -48,8 +48,8 @@ class JSON_Master { private: struct socket_list { - SocketAPM sock_in{true}; - SocketAPM sock_out{true}; + SocketAPM_native sock_in{true}; + SocketAPM_native sock_out{true}; uint8_t instance; bool connected; socket_list *next; diff --git a/libraries/SITL/SIM_Loweheiser.cpp b/libraries/SITL/SIM_Loweheiser.cpp new file mode 100644 index 00000000000000..20779e2e20a136 --- /dev/null +++ b/libraries/SITL/SIM_Loweheiser.cpp @@ -0,0 +1,344 @@ +/* + 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 . + */ +/* + Simulator for the Loweheiser generators +*/ + +#include "SIM_config.h" + +#if AP_SIM_LOWEHEISER_ENABLED + +#include + +#include "SIM_Loweheiser.h" +#include "SITL.h" + +#include + +#include +#include + +using namespace SITL; + +Loweheiser::Loweheiser() : SerialDevice::SerialDevice() +{ +} + +void Loweheiser::update() +{ + // if (!_enabled.get()) { + // return; + // } + maybe_send_heartbeat(); + update_receive(); + update_send(); +} + +void Loweheiser::maybe_send_heartbeat() +{ + const uint32_t now = AP_HAL::millis(); + + if (now - last_heartbeat_ms < 100) { + // we only provide a heartbeat every so often + return; + } + last_heartbeat_ms = now; + + mavlink_message_t msg; + mavlink_msg_heartbeat_pack(system_id, + component_id, + &msg, + MAV_TYPE_GCS, + MAV_AUTOPILOT_INVALID, + 0, + 0, + 0); + + uint8_t buf[300]; + uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg); + + if (write_to_autopilot((const char*)&buf, buf_len) != buf_len) { + // ::fprintf(stderr, "write failure\n"); + } +} + +void Loweheiser::handle_message(const mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_COMMAND_LONG: { + mavlink_command_long_t pkt; + mavlink_msg_command_long_decode(&msg, &pkt); + + if (pkt.target_system != system_id || + pkt.target_component != component_id) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Not for me"); + return; + } + + switch (pkt.command) { + case MAV_CMD_LOWEHEISER_SET_STATE: + // decode the desired run state: + // param2 physically turns power on/off to the EFI! + switch ((uint8_t)pkt.param2) { + case 0: + autopilot_desired_engine_run_state = EngineRunState::OFF; + break; + case 1: + autopilot_desired_engine_run_state = EngineRunState::ON; + break; + default: + AP_HAL::panic("Bad desired engine run state"); + } + switch ((uint8_t)pkt.param3) { + case 0: + autopilot_desired_governor_state = GovernorState::OFF; + break; + case 1: + autopilot_desired_governor_state = GovernorState::ON; + break; + default: + AP_HAL::panic("Bad desired governor state"); + } + switch ((uint8_t)pkt.param5) { + case 0: + autopilot_desired_startup_state = StartupState::OFF; + break; + case 1: + autopilot_desired_startup_state = StartupState::ON; + break; + default: + AP_HAL::panic("Bad electronic startup state"); + } + manual_throttle_pct = pkt.param4; + mavlink_message_t ack; + mavlink_msg_command_ack_pack( + system_id, + component_id, + &ack, + MAV_CMD_LOWEHEISER_SET_STATE, + MAV_RESULT_ACCEPTED, + 0, + 0, + msg.sysid, + msg.compid + ); + uint8_t buf[300]; + uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &ack); + + if (write_to_autopilot((const char*)&buf, buf_len) != buf_len) { + // ::fprintf(stderr, "write failure\n"); + } + break; + } + }; + } +} + +void Loweheiser::update_receive() +{ + char buf[128]; + const ssize_t bytes_read = read_from_autopilot(buf, sizeof(buf)); + if (bytes_read <= 0) { + return; + } + for (uint16_t i=0; iefi_type == SIM::EFIType::EFI_TYPE_NONE) { + return; + } + + float throttle = 0; + float throttle_output = 0; + + switch (autopilot_desired_engine_run_state) { + case EngineRunState::OFF: + generatorengine.desired_rpm = 0; + break; + case EngineRunState::ON: + switch (autopilot_desired_governor_state) { + case GovernorState::OFF: { + throttle = manual_throttle_pct; + throttle_output = throttle; + if (is_positive(generatorengine.desired_rpm) || + autopilot_desired_startup_state == StartupState::ON) { + const uint16_t manual_rpm_min = 0; + const uint16_t manual_rpm_max = 8000; + generatorengine.desired_rpm = linear_interpolate( + manual_rpm_min, + manual_rpm_max, + throttle, + 0, + 100 + ); + } + break; + } + case GovernorState::ON: + // should probably base this on current draw from battery + // / motor throttle output? + throttle = 80; + throttle_output = 80; + generatorengine.desired_rpm = 8000; + break; + } + } + + // a simulation for a stuck throttle - once the egine starts it + // won't stop based on servo position,, only engine run state + const bool stuck_throttle_failure_simulation = false; + static bool throttle_is_stuck; + if (stuck_throttle_failure_simulation) { + if (generatorengine.desired_rpm > 7000) { + throttle_is_stuck = true; + } + // if the throttle is stuck then the engine runs - except if + // the autopilot is saying the desired runstate is off, + // because that just shuts down the spark entirely, so the + // engine will not run + if (throttle_is_stuck && + autopilot_desired_engine_run_state == EngineRunState::ON) { + // same numbers as "on" case, above + throttle = 80; + throttle_output = 80; + generatorengine.desired_rpm = 8000; + } + } + + _current_current = AP::sitl()->state.battery_current; + _current_current = MIN(_current_current, max_current); + + generatorengine.current_current = _current_current; + generatorengine.max_current = max_current; + generatorengine.max_slew_rpm_per_second = 2000; + + generatorengine.update(); + + update_fuel_level(); + + float efi_pw = std::numeric_limits::quiet_NaN(); + float efi_fuel_flow = std::numeric_limits::quiet_NaN(); + float efi_fuel_consumed = std::numeric_limits::quiet_NaN(); + float efi_fuel_level = std::numeric_limits::quiet_NaN(); + float efi_baro = std::numeric_limits::quiet_NaN(); + float efi_mat = std::numeric_limits::quiet_NaN(); + float efi_clt = std::numeric_limits::quiet_NaN(); + float efi_tps = std::numeric_limits::quiet_NaN(); + float efi_egt = std::numeric_limits::quiet_NaN(); + float efi_batt = std::numeric_limits::quiet_NaN(); + + float curr_batt = -0.3; + float curr_gen = 10.12; + + // Current from the generator is the battery charging current (defined to be negative) plus the generator current + float curr_rot = (curr_batt < 0 ? -curr_batt : 0.0) + curr_gen; + + // controlled by param2, this turns on/off the DC/DC component which + // powers the efi + if (autopilot_desired_engine_run_state == EngineRunState::ON) { + efi_baro = AP::baro().get_pressure() / 1000.0; + efi_mat = AP::baro().get_temperature(); + efi_clt = generatorengine.temperature; + efi_tps = MAX(throttle_output, 40); + efi_batt = 12.5; + efi_fuel_flow = fuel_flow_lps * 3600; // litres/second -> litres/hour + } + + if (false) { + efi_fuel_level = fuel_level; + efi_fuel_consumed = fuel_consumed; + } + + // +/- 3V, depending on draw + const float volt_batt = base_supply_voltage - (3 * (_current_current / max_current)); + + const mavlink_loweheiser_gov_efi_t loweheiser_efi_gov{ + volt_batt, // volt_batt + curr_batt, // curr_batt + curr_gen, // curr_gen + curr_rot, // curr_rot + efi_fuel_level, // fuel_level in litres + throttle, // throttle + UINT32_MAX, // runtime in seconds + INT32_MAX, // time until maintenance + std::numeric_limits::quiet_NaN(), // rectifier temperature + std::numeric_limits::quiet_NaN(), // generator temperature + efi_batt, // efi_batt + generatorengine.current_rpm, // efi_rpm + efi_pw, // efi_pw + efi_fuel_flow, // efi_fuelflow + efi_fuel_consumed, // efi_fuel_consumed + efi_baro, // efi_baro + efi_mat, // efi_mat + efi_clt, // efi_clt + efi_tps, // efi_tps + efi_egt, // efi_exhaust_gas_temperature + 1, // EFI index + 0, // generator_status + 0 // EFI status + }; + + mavlink_message_t msg; + mavlink_msg_loweheiser_gov_efi_encode_status( + system_id, + component_id, + &mav_status, + &msg, + &loweheiser_efi_gov + ); + + uint8_t buf[300]; + uint16_t buf_len = mavlink_msg_to_send_buffer(buf, &msg); + + if (write_to_autopilot((char*)buf, buf_len) != buf_len) { + AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno)); + } +} + +#endif // AP_SIM_LOWEHEISER_ENABLED diff --git a/libraries/SITL/SIM_Loweheiser.h b/libraries/SITL/SIM_Loweheiser.h new file mode 100644 index 00000000000000..ae4f28cdd73bf7 --- /dev/null +++ b/libraries/SITL/SIM_Loweheiser.h @@ -0,0 +1,180 @@ +/* + 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 . + */ +/* + Simulator for the Loweheiser EFI/generator + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:loweheiser --speedup=1 --console + +param set SIM_EFI_TYPE 2 +param set SERIAL5_PROTOCOL 2 +param set GEN_TYPE 4 +param set EFI_TYPE 4 +param set BATT2_MONITOR 17 # generator (elec) +param set BATT3_MONITOR 18 # generator (fuel level) +param fetch + +param set BATT3_CAPACITY 10000 +param set BATT3_LOW_MAH 1000 +param set BATT3_CRT_MAH 500 +param set BATT3_FS_LOW_ACT 2 # RTL +param set BATT3_FS_CRT_ACT 1 # LAND +param set BATT3_LOW_VOLT 0 + +param set RC9_OPTION 85 # generator control + +param set GEN_IDLE_TH_H 40 # NOTE without this the engine never warms up past 36 deg C +param set GEN_IDLE_TH 25 + +param set RC10_OPTION 212 # loweheiser manual throttle control +param set RC10_DZ 20 +param set RC10_TRIM 1000 +param set RC10_MIN 1000 +param set RC10_MAX 2000 + +param set RC11_OPTION 109 # loweheiser starter channel + +reboot + +# for testing failsafes: +param set BATT3_CAPACITY 200 +param set BATT3_LOW_MAH 100 +param set BATT3_CRT_MAH 50 + +# stream EFI_STATUS at 10Hz: +long SET_MESSAGE_INTERVAL 225 100000 + +# run SITL against real generator: +DEV=/dev/serial/by-id/usb-Prolific_Technology_Inc._USB-Serial_Controller-if00-port0 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=uart:$DEV:115200 --speedup=1 --console + +# run generator test script against simulator: +python ./libraries/AP_Generator/scripts/test-loweheiser.py tcp:localhost:5762 + +# use the generator test script to control the generator: +./libraries/AP_Generator/scripts/test-loweheiser.py $DEV + +# observe RPM + +# observe remaining fuel: +graph BATTERY_STATUS[2].battery_remaining +graph BATTERY_STATUS[2].current_consumed + +# autotest suite: + +./Tools/autotest/autotest.py --gdb --debug build.Copter test.Copter.Loweheiser + +# use a usb-ttl cable to connect directly to mavlink-speaking generator: +DEV=/dev/serial/by-id/usb-Prolific_Technology_Inc._USB-Serial_Controller-if00-port0 +mavproxy.py --master $DEV --baud 115200 + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_LOWEHEISER_ENABLED + +#include + +#include "SITL_Input.h" + +#include "SIM_SerialDevice.h" +#include "SIM_GeneratorEngine.h" + +#include + +#include + +namespace SITL { + +class Loweheiser : public SerialDevice { +public: + + Loweheiser(); + + // update state + void update(); + +private: + + // TODO: make these parameters: + const uint8_t system_id = 17; + const uint8_t component_id = 18; + + const float max_current = 50.0f; + const float base_supply_voltage = 50.0; + + // we share channels with the ArduPilot binary! + // Beware: the mavlink rangefinder and other stuff shares this channel. + const mavlink_channel_t mavlink_ch = (mavlink_channel_t)(MAVLINK_COMM_0+5); + + class SIM *_sitl; + + uint32_t last_sent_ms; + + void update_receive(); + void update_send(); + + void maybe_send_heartbeat(); + uint32_t last_heartbeat_ms; + + void handle_message(const mavlink_message_t &msg); + + enum class EngineRunState : uint8_t { + OFF = 0, + ON = 1, + }; + EngineRunState autopilot_desired_engine_run_state = EngineRunState::OFF; + + enum class GovernorState : uint8_t { + OFF = 0, + ON = 1, + }; + GovernorState autopilot_desired_governor_state = GovernorState::OFF; + + float manual_throttle_pct; + + enum class StartupState : uint8_t { + OFF = 0, + ON = 1, + }; + StartupState autopilot_desired_startup_state = StartupState::OFF; + + mavlink_message_t rxmsg; + mavlink_status_t rxstatus; + + SIM_GeneratorEngine generatorengine; + + float _current_current; + + // fuel + const float initial_fuel_level = 10; // litres, must match battery setup + float fuel_level = initial_fuel_level; // litres + float fuel_consumed = 0; // litres + float fuel_flow_lps = 0; // litres/second + void update_fuel_level(); + + uint32_t last_fuel_update_ms; + + mavlink_status_t mav_status; + + // parameters + // AP_Int8 _enabled; +}; + +} + +#endif // AP_SIM_LOWEHEISER_ENABLED diff --git a/libraries/SITL/SIM_Morse.cpp b/libraries/SITL/SIM_Morse.cpp index 81ba8ce34972f7..a23669b0f5fb76 100644 --- a/libraries/SITL/SIM_Morse.cpp +++ b/libraries/SITL/SIM_Morse.cpp @@ -29,7 +29,6 @@ #include #include -#include #include "pthread.h" #include @@ -261,7 +260,7 @@ bool Morse::parse_sensors(const char *json) bool Morse::connect_sockets(void) { if (!sensors_sock) { - sensors_sock = new SocketAPM(false); + sensors_sock = new SocketAPM_native(false); if (!sensors_sock) { AP_HAL::panic("Out of memory for sensors socket"); } @@ -280,7 +279,7 @@ bool Morse::connect_sockets(void) printf("Sensors connected\n"); } if (!control_sock) { - control_sock = new SocketAPM(false); + control_sock = new SocketAPM_native(false); if (!control_sock) { AP_HAL::panic("Out of memory for control socket"); } @@ -602,7 +601,7 @@ void Morse::send_report(void) if (now < 10000) { // don't send lidar reports until 10s after startup. This // avoids a windows threading issue with non-blocking sockets - // and the initial wait on uartA + // and the initial wait on SERIAL0 return; } #endif diff --git a/libraries/SITL/SIM_Morse.h b/libraries/SITL/SIM_Morse.h index 5e45ab9f93095a..9445c26d5817a3 100644 --- a/libraries/SITL/SIM_Morse.h +++ b/libraries/SITL/SIM_Morse.h @@ -26,7 +26,7 @@ #if HAL_SIM_MORSE_ENABLED -#include +#include #include "SIM_Aircraft.h" namespace SITL { @@ -51,7 +51,7 @@ class Morse : public Aircraft { // loopback to convert inbound Morse lidar data into inbound mavlink msgs const char *mavlink_loopback_address = "127.0.0.1"; const uint16_t mavlink_loopback_port = 5762; - SocketAPM mav_socket { false }; + SocketAPM_native mav_socket { false }; struct { // socket to telem2 on aircraft bool connected; @@ -91,8 +91,8 @@ class Morse : public Aircraft { uint8_t sensor_buffer[50000]; uint32_t sensor_buffer_len; - SocketAPM *sensors_sock; - SocketAPM *control_sock; + SocketAPM_native *sensors_sock; + SocketAPM_native *control_sock; uint32_t no_data_counter; uint32_t connect_counter; diff --git a/libraries/SITL/SIM_PS_LightWare_SF45B.h b/libraries/SITL/SIM_PS_LightWare_SF45B.h index 901a6c7988ebc5..16fd9955734efd 100644 --- a/libraries/SITL/SIM_PS_LightWare_SF45B.h +++ b/libraries/SITL/SIM_PS_LightWare_SF45B.h @@ -15,7 +15,7 @@ /* Simulator for the LightWare S45B proximity sensor -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:sf45b --speedup=1 -l 51.8752066,14.6487840,54.15,0 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:sf45b --speedup=1 -l 51.8752066,14.6487840,54.15,0 param set SERIAL5_PROTOCOL 11 # proximity param set PRX1_TYPE 8 # s45b diff --git a/libraries/SITL/SIM_PS_RPLidar.h b/libraries/SITL/SIM_PS_RPLidar.h index bf2444f5cd5172..bb694a7bab31cb 100644 --- a/libraries/SITL/SIM_PS_RPLidar.h +++ b/libraries/SITL/SIM_PS_RPLidar.h @@ -17,7 +17,7 @@ /* Simulator for the RPLidarA2 proximity sensor -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,54.15,0 --map +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,54.15,0 --map param set SERIAL5_PROTOCOL 11 param set PRX1_TYPE 5 diff --git a/libraries/SITL/SIM_PS_RPLidarA1.h b/libraries/SITL/SIM_PS_RPLidarA1.h index d374354e6b37aa..ca0b625270ea5f 100644 --- a/libraries/SITL/SIM_PS_RPLidarA1.h +++ b/libraries/SITL/SIM_PS_RPLidarA1.h @@ -15,7 +15,7 @@ /* Simulator for the RPLidarA2 proximity sensor -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rplidara1 --speedup=1 -l 51.8752066,14.6487840,0,0 --map +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rplidara1 --speedup=1 -l 51.8752066,14.6487840,0,0 --map param set SERIAL5_PROTOCOL 11 param set PRX1_TYPE 5 diff --git a/libraries/SITL/SIM_PS_RPLidarA2.h b/libraries/SITL/SIM_PS_RPLidarA2.h index b5beb50f3cf022..785c2bd6bce337 100644 --- a/libraries/SITL/SIM_PS_RPLidarA2.h +++ b/libraries/SITL/SIM_PS_RPLidarA2.h @@ -15,7 +15,7 @@ /* Simulator for the RPLidarA2 proximity sensor -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,0,0 --map +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,0,0 --map param set SERIAL5_PROTOCOL 11 param set PRX1_TYPE 5 diff --git a/libraries/SITL/SIM_PS_TeraRangerTower.h b/libraries/SITL/SIM_PS_TeraRangerTower.h index f17f491bceb67b..71b0798d52a355 100644 --- a/libraries/SITL/SIM_PS_TeraRangerTower.h +++ b/libraries/SITL/SIM_PS_TeraRangerTower.h @@ -15,7 +15,7 @@ /* Simulator for the TeraRangerTower proximity sensor -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:terarangertower --speedup=1 -l 51.8752066,14.6487840,54.15,0 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:terarangertower --speedup=1 -l 51.8752066,14.6487840,54.15,0 param set SERIAL5_PROTOCOL 11 param set PRX1_TYPE 3 # terarangertower diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 4b32c10406267b..8dbdfc6fc4a872 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/libraries/SITL/SIM_Plane.cpp @@ -20,6 +20,7 @@ #include "SIM_Plane.h" #include +#include using namespace SITL; @@ -80,6 +81,7 @@ Plane::Plane(const char *frame_str) : thrust_scale *= 1.5; } +#if AP_FILESYSTEM_FILE_READING_ENABLED if (strstr(frame_str, "-3d")) { aerobatic = true; thrust_scale *= 1.5; @@ -87,7 +89,8 @@ Plane::Plane(const char *frame_str) : AP_Param::load_defaults_file("@ROMFS/models/plane.parm", false); AP_Param::load_defaults_file("@ROMFS/models/plane-3d.parm", false); } - +#endif + if (strstr(frame_str, "-ice")) { ice_engine = true; } diff --git a/libraries/SITL/SIM_QMC5883L.cpp b/libraries/SITL/SIM_QMC5883L.cpp new file mode 100644 index 00000000000000..e9e93813a33daa --- /dev/null +++ b/libraries/SITL/SIM_QMC5883L.cpp @@ -0,0 +1,128 @@ +#include "SIM_config.h" + +#if AP_SIM_COMPASS_QMC5883L_ENABLED + +#include "SIM_QMC5883L.h" + +// FIXME: read the datasheet :-) + +#include +#include + +#include + +#define QMC5883L_REG_CONF1 0x09 +#define QMC5883L_REG_CONF2 0x0A + +#define QMC5883L_MODE_STANDBY 0x00 +#define QMC5883L_MODE_CONTINUOUS 0x01 + +#define QMC5883L_ODR_100HZ (0x02 << 2) + +#define QMC5883L_OSR_512 (0x00 << 6) + +#define QMC5883L_RNG_8G (0x01 << 4) + +SITL::QMC5883L::QMC5883L() +{ + writable_registers.set(0); + writable_registers.set(0x0b); + writable_registers.set(0x20); + writable_registers.set(0x21); + writable_registers.set(QMC5883L_REG_CONF1); + writable_registers.set(QMC5883L_REG_CONF2); + + reset(); +} + +void SITL::QMC5883L::reset() +{ + memset(registers.byte, 0, ARRAY_SIZE(registers.byte)); + registers.byname.ZEROX_ZEROC = 0x01; + registers.byname.REG_STATUS = 0x0; + registers.byname.REG_ID = 0xFF; +} + + +int SITL::QMC5883L::rdwr(I2C::i2c_rdwr_ioctl_data *&data) +{ + if (data->nmsgs == 2) { + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + if (data->msgs[1].flags != I2C_M_RD) { + AP_HAL::panic("Unexpected flags"); + } + const uint8_t reg_addr = data->msgs[0].buf[0]; + for (uint8_t i=0; imsgs[1].len; i++) { + const uint8_t register_value = registers.byte[reg_addr+i]; + data->msgs[1].buf[i] = register_value; + + // FIXME: is this really how the status data-ready bit works? + if (reg_addr == 0x05) { // that's the last data register... + registers.byname.REG_STATUS &= ~0x04; + } + } + return 0; + } + + if (data->nmsgs == 1) { + if (data->msgs[0].flags != 0) { + AP_HAL::panic("Unexpected flags"); + } + const uint8_t reg_addr = data->msgs[0].buf[0]; + if (!writable_registers.get(reg_addr)) { + AP_HAL::panic("Register 0x%02x is not writable!", reg_addr); + } + const uint8_t register_value = data->msgs[0].buf[1]; + ::fprintf(stderr, "Setting register (0x%x) to 0x%x\n", reg_addr, register_value); + registers.byte[reg_addr] = register_value; + return 0; + } + + return -1; +}; + +void SITL::QMC5883L::update(const Aircraft &aircraft) +{ + + // FIXME: swipe stuff from AP_Compass_SITL here. + + // FIXME: somehow decide to use the simulated compass offsets etc + // from SITL + + Vector3f f = AP::sitl()->state.bodyMagField; + + // Vector3 field + // int16_t str_x = 123; + // int16_t str_y = -56; + // int16_t str_z = 1; + + f.rotate_inverse(ROTATION_ROLL_180_YAW_270); + + f.x = -f.x; + f.z = -f.z; + + f.x *= 3; + f.y *= 3; + f.z *= 3; + + Vector3 k { + k.x = htole16((int16_t)f.x), + k.y = htole16((int16_t)f.y), + k.z = htole16((int16_t)f.z) + }; + + if (registers.byname.REG_CONF1 & QMC5883L_MODE_CONTINUOUS) { + // FIXME: clock according to configuration here + registers.byname.REG_STATUS |= 0x04; + registers.byname.DATA_OUTPUT_X_low = k.x & 0xFF; + registers.byname.DATA_OUTPUT_X_high = k.x >> 8; + registers.byname.DATA_OUTPUT_Y_low = k.y & 0xFF; + registers.byname.DATA_OUTPUT_Y_high = k.y >> 8; + registers.byname.DATA_OUTPUT_Z_low = k.z & 0xFF; + registers.byname.DATA_OUTPUT_Z_high = k.z >> 8; + } +} + +#endif // AP_SIM_COMPASS_QMC5883L_ENABLED diff --git a/libraries/SITL/SIM_QMC5883L.h b/libraries/SITL/SIM_QMC5883L.h new file mode 100644 index 00000000000000..10aa1719656c44 --- /dev/null +++ b/libraries/SITL/SIM_QMC5883L.h @@ -0,0 +1,55 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_COMPASS_QMC5883L_ENABLED + +#include "SIM_I2CDevice.h" + +namespace SITL { + +class QMC5883L : public I2CDevice +{ +public: + + QMC5883L(); + + void update(const class Aircraft &aircraft) override; + + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + +private: + + union Registers { + uint8_t byte[256]; + struct PACKED ByName { + uint8_t DATA_OUTPUT_X_low; + uint8_t DATA_OUTPUT_X_high; + uint8_t DATA_OUTPUT_Y_low; + uint8_t DATA_OUTPUT_Y_high; + uint8_t DATA_OUTPUT_Z_low; + uint8_t DATA_OUTPUT_Z_high; + uint8_t REG_STATUS; + uint8_t unused1[2]; // unused + uint8_t REG_CONF1; + uint8_t REG_CONF2; + uint8_t ZEROX_ZEROB; // magic register number from driver + uint8_t ZEROX_ZEROC; // magic register from driver should always be 0x01 + uint8_t REG_ID; + uint8_t unused2[242]; + } byname; + } registers; + + // 256 1-byte registers: + assert_storage_size assert_storage_size_registers_reg; + + Bitmask<256> writable_registers; + + void reset(); + + uint32_t cmd_take_reading_received_ms; +}; + +} // namespace SITL + +#endif // AP_SIM_COMPASS_QMC5883L_ENABLED diff --git a/libraries/SITL/SIM_RAMTRON.cpp b/libraries/SITL/SIM_RAMTRON.cpp index dee0fca6b784a0..9706dee62a318e 100644 --- a/libraries/SITL/SIM_RAMTRON.cpp +++ b/libraries/SITL/SIM_RAMTRON.cpp @@ -4,6 +4,9 @@ #include #include +#include +#include +#include #include diff --git a/libraries/SITL/SIM_RF_BLping.h b/libraries/SITL/SIM_RF_BLping.h index 7c76b31d1fc650..762e5de9611992 100644 --- a/libraries/SITL/SIM_RF_BLping.h +++ b/libraries/SITL/SIM_RF_BLping.h @@ -15,7 +15,7 @@ /* Simulator for the BLping rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduSub -A --uartF=sim:blping --speedup=1 -l 33.810313,-118.393867,0,185 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduSub -A --serial5=sim:blping --speedup=1 -l 33.810313,-118.393867,0,185 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 23 diff --git a/libraries/SITL/SIM_RF_Benewake_TF02.h b/libraries/SITL/SIM_RF_Benewake_TF02.h index fac8443f6bf656..3d9604559c5ae0 100644 --- a/libraries/SITL/SIM_RF_Benewake_TF02.h +++ b/libraries/SITL/SIM_RF_Benewake_TF02.h @@ -15,7 +15,7 @@ /* Simulator for the Benewake TF02 RangeFinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tf02 --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:benewake_tf02 --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 19 diff --git a/libraries/SITL/SIM_RF_Benewake_TF03.h b/libraries/SITL/SIM_RF_Benewake_TF03.h index f8d86fdbd59d45..cd01419e3338c0 100644 --- a/libraries/SITL/SIM_RF_Benewake_TF03.h +++ b/libraries/SITL/SIM_RF_Benewake_TF03.h @@ -15,7 +15,7 @@ /* Simulator for the Benewake TF03 RangeFinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tf03 --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:benewake_tf03 --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 27 diff --git a/libraries/SITL/SIM_RF_Benewake_TFmini.h b/libraries/SITL/SIM_RF_Benewake_TFmini.h index b204fb6bf023f1..5589b5f303bc14 100644 --- a/libraries/SITL/SIM_RF_Benewake_TFmini.h +++ b/libraries/SITL/SIM_RF_Benewake_TFmini.h @@ -15,7 +15,7 @@ /* Simulator for the TFMini RangeFinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:benewake_tfmini --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:benewake_tfmini --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 20 diff --git a/libraries/SITL/SIM_RF_GYUS42v2.h b/libraries/SITL/SIM_RF_GYUS42v2.h index 308123dd1813c8..7188cd30f9f92c 100644 --- a/libraries/SITL/SIM_RF_GYUS42v2.h +++ b/libraries/SITL/SIM_RF_GYUS42v2.h @@ -15,7 +15,7 @@ /* Simulator for the GY-US42-v2 serial rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:gyus42v2 --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:gyus42v2 --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 31 diff --git a/libraries/SITL/SIM_RF_JRE.h b/libraries/SITL/SIM_RF_JRE.h index d10c77a5258909..4dbdbfe2c0ffdc 100644 --- a/libraries/SITL/SIM_RF_JRE.h +++ b/libraries/SITL/SIM_RF_JRE.h @@ -15,7 +15,7 @@ /* Simulator for the JAE JRE radio altimiter -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:jre --speedup=1 -L KalaupapaCliffs --map +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:jre --speedup=1 -L KalaupapaCliffs --map param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 38 diff --git a/libraries/SITL/SIM_RF_Lanbao.h b/libraries/SITL/SIM_RF_Lanbao.h index a3bc74e87e05db..d773287a3ed45e 100644 --- a/libraries/SITL/SIM_RF_Lanbao.h +++ b/libraries/SITL/SIM_RF_Lanbao.h @@ -15,7 +15,7 @@ /* Simulator for the Lanbao rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lanbao --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:lanbao --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 26 diff --git a/libraries/SITL/SIM_RF_LeddarOne.h b/libraries/SITL/SIM_RF_LeddarOne.h index ad54ce38991441..21f74ac8a70cee 100644 --- a/libraries/SITL/SIM_RF_LeddarOne.h +++ b/libraries/SITL/SIM_RF_LeddarOne.h @@ -15,7 +15,7 @@ /* Simulator for the LeddarOne rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:leddarone --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:leddarone --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 12 diff --git a/libraries/SITL/SIM_RF_LightWareSerial.h b/libraries/SITL/SIM_RF_LightWareSerial.h index 4591fa4a5c0ac8..686b448bcf2384 100644 --- a/libraries/SITL/SIM_RF_LightWareSerial.h +++ b/libraries/SITL/SIM_RF_LightWareSerial.h @@ -15,7 +15,7 @@ /* Simulator for the serial LightWare rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lightwareserial --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:lightwareserial --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 8 diff --git a/libraries/SITL/SIM_RF_LightWareSerialBinary.h b/libraries/SITL/SIM_RF_LightWareSerialBinary.h index 992a7b160aacad..0454fb28904f25 100644 --- a/libraries/SITL/SIM_RF_LightWareSerialBinary.h +++ b/libraries/SITL/SIM_RF_LightWareSerialBinary.h @@ -15,7 +15,7 @@ /* Simulator for the serial LightWare rangefinder - binary mode -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:lightwareserial-binary --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:lightwareserial-binary --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 8 diff --git a/libraries/SITL/SIM_RF_MAVLink.h b/libraries/SITL/SIM_RF_MAVLink.h index 23b04bdaf0ea00..9d901ba75a4e65 100644 --- a/libraries/SITL/SIM_RF_MAVLink.h +++ b/libraries/SITL/SIM_RF_MAVLink.h @@ -15,7 +15,7 @@ /* Simulator for the NMEA serial rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rf_mavlink --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rf_mavlink --speedup=1 param set SERIAL5_PROTOCOL 1 param set RNGFND1_TYPE 10 diff --git a/libraries/SITL/SIM_RF_MaxsonarSerialLV.h b/libraries/SITL/SIM_RF_MaxsonarSerialLV.h index 6397637f33ac40..d23a865f7f8fee 100644 --- a/libraries/SITL/SIM_RF_MaxsonarSerialLV.h +++ b/libraries/SITL/SIM_RF_MaxsonarSerialLV.h @@ -15,7 +15,7 @@ /* Simulator for the MaxsonarSerialLV rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:maxsonarseriallv --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:maxsonarseriallv --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 13 diff --git a/libraries/SITL/SIM_RF_NMEA.h b/libraries/SITL/SIM_RF_NMEA.h index d9dbb8079bc3e3..4ba8feaf61244e 100644 --- a/libraries/SITL/SIM_RF_NMEA.h +++ b/libraries/SITL/SIM_RF_NMEA.h @@ -15,7 +15,7 @@ /* Simulator for the NMEA serial rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:nmea --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:nmea --speedup=1 param set SERIAL5_PROTOCOL 9 param set SERIAL5_BAUD 9600 diff --git a/libraries/SITL/SIM_RF_RDS02UF.h b/libraries/SITL/SIM_RF_RDS02UF.h index a96bb9b56564d0..44a387370db6b7 100644 --- a/libraries/SITL/SIM_RF_RDS02UF.h +++ b/libraries/SITL/SIM_RF_RDS02UF.h @@ -15,7 +15,7 @@ /* Simulator for the RDS02UF rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rds02uf --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:rds02uf --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 36 diff --git a/libraries/SITL/SIM_RF_TeraRanger_Serial.h b/libraries/SITL/SIM_RF_TeraRanger_Serial.h index 314460699fe083..31544f913826a3 100644 --- a/libraries/SITL/SIM_RF_TeraRanger_Serial.h +++ b/libraries/SITL/SIM_RF_TeraRanger_Serial.h @@ -12,7 +12,7 @@ */ /* Simulator for the TeraRanger NEO RangeFinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:teraranger_serial --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:teraranger_serial --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 35 graph RANGEFINDER.distance diff --git a/libraries/SITL/SIM_RF_USD1_v0.h b/libraries/SITL/SIM_RF_USD1_v0.h index 866c5a8ea3f295..7a3498adb64311 100644 --- a/libraries/SITL/SIM_RF_USD1_v0.h +++ b/libraries/SITL/SIM_RF_USD1_v0.h @@ -15,7 +15,7 @@ /* Simulator for the USD1 v0 Serial RangeFinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:USD1_v0 --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:USD1_v0 --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 11 diff --git a/libraries/SITL/SIM_RF_USD1_v1.h b/libraries/SITL/SIM_RF_USD1_v1.h index 2b3b4901d1c22b..d2982e2cb863b8 100644 --- a/libraries/SITL/SIM_RF_USD1_v1.h +++ b/libraries/SITL/SIM_RF_USD1_v1.h @@ -15,7 +15,7 @@ /* Simulator for the USD1 v1 Serial RangeFinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:USD1_v1 --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:USD1_v1 --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 11 diff --git a/libraries/SITL/SIM_RF_Wasp.h b/libraries/SITL/SIM_RF_Wasp.h index e52d9aba989d46..a1e9b37acf5e5f 100644 --- a/libraries/SITL/SIM_RF_Wasp.h +++ b/libraries/SITL/SIM_RF_Wasp.h @@ -15,7 +15,7 @@ /* Simulator for the Wasp serial rangefinder -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:wasp --speedup=1 +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:wasp --speedup=1 param set SERIAL5_PROTOCOL 9 param set RNGFND1_TYPE 18 diff --git a/libraries/SITL/SIM_RichenPower.cpp b/libraries/SITL/SIM_RichenPower.cpp index f5affa7d277471..f45e29f17f909b 100644 --- a/libraries/SITL/SIM_RichenPower.cpp +++ b/libraries/SITL/SIM_RichenPower.cpp @@ -51,6 +51,8 @@ const AP_Param::GroupInfo RichenPower::var_info[] = { RichenPower::RichenPower() : SerialDevice::SerialDevice() { + ASSERT_STORAGE_SIZE(RichenPacket, 70); + AP_Param::setup_object_defaults(this, var_info); u.packet.magic1 = 0xAA; @@ -126,47 +128,29 @@ void RichenPower::update_control_pin(const struct sitl_input &input) // RICHENPOWER, 13:49 // Idle RMP 4800 +-300, RUN RPM 13000 +- 1500 - uint16_t desired_rpm = 0; switch (_state) { case State::STOP: - desired_rpm = 0; + generatorengine.desired_rpm = 0; break; case State::IDLE: case State::STOPPING: - desired_rpm = 4800; // +/- 300 + generatorengine.desired_rpm = 4800; // +/- 300 break; case State::RUN: - desired_rpm = 13000; // +/- 1500 + generatorengine.desired_rpm = 13000; // +/- 1500 break; } _current_current = AP::sitl()->state.battery_current; _current_current = MIN(_current_current, max_current); - if (_current_current > 1 && _state != State::RUN) { - AP_HAL::panic("Generator stalled due to high current demand"); - } else if (_current_current > max_current) { - AP_HAL::panic("Generator stalled due to high current demand (run)"); - } - - // linear degradation in RPM up to maximum load - if (desired_rpm) { - desired_rpm -= 1500 * (_current_current/max_current); - } - const float max_slew_rpm_per_second = 2000; - const float max_slew_rpm = max_slew_rpm_per_second * ((now - last_rpm_update_ms) / 1000.0f); - last_rpm_update_ms = now; - const float rpm_delta = _current_rpm - desired_rpm; - if (rpm_delta > 0) { - _current_rpm -= MIN(max_slew_rpm, rpm_delta); - } else { - _current_rpm += MIN(max_slew_rpm, abs(rpm_delta)); - } + generatorengine.current_current = _current_current; + generatorengine.max_current = max_current; + generatorengine.max_slew_rpm_per_second = 2000; - // if (!is_zero(rpm_delta)) { - // ::fprintf(stderr, "richenpower pwm: %f\n", _current_rpm); - // } + generatorengine.update(); + _current_rpm = generatorengine.current_rpm; } void RichenPower::RichenUnion::update_checksum() diff --git a/libraries/SITL/SIM_RichenPower.h b/libraries/SITL/SIM_RichenPower.h index f0d604c60528e3..ed9217ba924eec 100644 --- a/libraries/SITL/SIM_RichenPower.h +++ b/libraries/SITL/SIM_RichenPower.h @@ -15,7 +15,7 @@ /* Simulator for the RichenPower Hybrid generators -./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:richenpower --speedup=1 --console +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:richenpower --speedup=1 --console param set SERIAL5_PROTOCOL 30 param set SERIAL5_BAUD 9600 @@ -45,6 +45,7 @@ arm throttle (denied because generator not running) #include "SITL_Input.h" #include "SIM_SerialDevice.h" +#include "SIM_GeneratorEngine.h" namespace SITL { @@ -94,8 +95,6 @@ class RichenPower : public SerialDevice { float _current_current; - uint32_t last_rpm_update_ms; - enum class Errors { MaintenanceRequired = 0, }; @@ -124,7 +123,6 @@ class RichenPower : public SerialDevice { uint8_t footermagic1; uint8_t footermagic2; }; - assert_storage_size _assert_storage_size_RichenPacket UNUSED_PRIVATE_MEMBER; union RichenUnion { uint8_t parse_buffer[70]; @@ -137,6 +135,8 @@ class RichenPower : public SerialDevice { // time we were asked to stop; uint32_t stop_start_ms; + + SIM_GeneratorEngine generatorengine; }; } diff --git a/libraries/SITL/SIM_Scrimmage.h b/libraries/SITL/SIM_Scrimmage.h index e6228cb281a79d..2fa8dfcbaf3ddd 100644 --- a/libraries/SITL/SIM_Scrimmage.h +++ b/libraries/SITL/SIM_Scrimmage.h @@ -28,7 +28,7 @@ #include -#include +#include #include "SIM_Aircraft.h" @@ -84,8 +84,8 @@ class Scrimmage : public Aircraft { void send_servos(const struct sitl_input &input); uint64_t prev_timestamp_us; - SocketAPM recv_sock; - SocketAPM send_sock; + SocketAPM_native recv_sock; + SocketAPM_native send_sock; }; } // namespace SITL diff --git a/libraries/SITL/SIM_Ship.h b/libraries/SITL/SIM_Ship.h index cbb91ccba40554..afdff43b37c877 100644 --- a/libraries/SITL/SIM_Ship.h +++ b/libraries/SITL/SIM_Ship.h @@ -22,7 +22,7 @@ #if AP_SIM_SHIP_ENABLED -#include +#include #include #include #include @@ -83,7 +83,7 @@ class ShipSim { uint32_t last_report_ms; uint32_t last_heartbeat_ms; - SocketAPM mav_socket { false }; + SocketAPM_native mav_socket { false }; bool mavlink_connected; mavlink_status_t mav_status; diff --git a/libraries/SITL/SIM_SilentWings.h b/libraries/SITL/SIM_SilentWings.h index 50739387a36952..75fdd3984fff32 100644 --- a/libraries/SITL/SIM_SilentWings.h +++ b/libraries/SITL/SIM_SilentWings.h @@ -23,7 +23,7 @@ #if HAL_SIM_SILENTWINGS_ENABLED -#include +#include #include "SIM_Aircraft.h" @@ -109,7 +109,7 @@ class SilentWings : public Aircraft { /* ArduPlane's internal time when the first packet from Silent Wings is received. */ uint64_t time_base_us; - SocketAPM sock; + SocketAPM_native sock; const char *_sw_address = "127.0.0.1"; int _port_in = 6060; int _sw_port = 6070; diff --git a/libraries/SITL/SIM_VectorNav.h b/libraries/SITL/SIM_VectorNav.h index 436e19cd22d0b5..589539e5d9a8f3 100644 --- a/libraries/SITL/SIM_VectorNav.h +++ b/libraries/SITL/SIM_VectorNav.h @@ -20,7 +20,7 @@ AHRS_EKF_TYPE = 11 EAHRS_TYPE=1 - sim_vehicle.py -D --console --map -A --uartF=sim:VectorNav + sim_vehicle.py -D --console --map -A --serial5=sim:VectorNav */ #pragma once diff --git a/libraries/SITL/SIM_Webots.cpp b/libraries/SITL/SIM_Webots.cpp index ac6c4655e9b81b..80a8583c410922 100644 --- a/libraries/SITL/SIM_Webots.cpp +++ b/libraries/SITL/SIM_Webots.cpp @@ -29,7 +29,6 @@ #include #include -#include #include "pthread.h" #include @@ -291,7 +290,7 @@ bool Webots::parse_sensors(const char *json) bool Webots::connect_sockets(void) { if (!sim_sock) { - sim_sock = new SocketAPM(false); + sim_sock = new SocketAPM_native(false); if (!sim_sock) { AP_HAL::panic("Out of memory for sensors socket"); } diff --git a/libraries/SITL/SIM_Webots.h b/libraries/SITL/SIM_Webots.h index ef6ea1454ab7f3..5457d6cd962a66 100644 --- a/libraries/SITL/SIM_Webots.h +++ b/libraries/SITL/SIM_Webots.h @@ -27,7 +27,7 @@ #if HAL_SIM_WEBOTS_ENABLED #include -#include +#include #include "SIM_Aircraft.h" namespace SITL { @@ -79,7 +79,7 @@ class Webots : public Aircraft { uint8_t sensor_buffer[50000]; uint32_t sensor_buffer_len; - SocketAPM *sim_sock; + SocketAPM_native *sim_sock; uint32_t connect_counter; diff --git a/libraries/SITL/SIM_Webots_Python.h b/libraries/SITL/SIM_Webots_Python.h index 194d8cd648884f..ec3e8ba4b56274 100644 --- a/libraries/SITL/SIM_Webots_Python.h +++ b/libraries/SITL/SIM_Webots_Python.h @@ -27,7 +27,7 @@ #if HAL_SIM_WEBOTSPYTHON_ENABLED #include "SIM_Aircraft.h" -#include +#include namespace SITL { @@ -78,7 +78,7 @@ class WebotsPython : public Aircraft { double last_timestamp; - SocketAPM socket_sitl; + SocketAPM_native socket_sitl; const char* _webots_address = "127.0.0.1"; int _webots_port = 9002; static const uint64_t WEBOTS_TIMEOUT_US = 5000000; diff --git a/libraries/SITL/SIM_XPlane.h b/libraries/SITL/SIM_XPlane.h index 84051439328d47..8c90ea6c2172e3 100644 --- a/libraries/SITL/SIM_XPlane.h +++ b/libraries/SITL/SIM_XPlane.h @@ -26,7 +26,7 @@ #if HAL_SIM_XPLANE_ENABLED -#include +#include #include #include "SIM_Aircraft.h" @@ -70,8 +70,8 @@ class XPlane : public Aircraft { uint16_t xplane_port = 49000; uint16_t bind_port = 49001; // udp socket, input and output - SocketAPM socket_in{true}; - SocketAPM socket_out{true}; + SocketAPM_native socket_in{true}; + SocketAPM_native socket_out{true}; uint64_t time_base_us; uint32_t last_data_time_ms; diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 4eb427c0ae3f10..419cac92262ba2 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -1,6 +1,7 @@ #pragma once #include +#include #ifndef HAL_SIM_ADSB_ENABLED #define HAL_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) @@ -22,6 +23,10 @@ #define AP_SIM_IS31FL3195_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_LOWEHEISER_ENABLED +#define AP_SIM_LOWEHEISER_ENABLED AP_SIM_ENABLED && HAL_MAVLINK_BINDINGS_ENABLED +#endif + #ifndef AP_SIM_SHIP_ENABLED #define AP_SIM_SHIP_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif @@ -78,3 +83,13 @@ #ifndef AP_SIM_GPS_UBLOX_ENABLED #define AP_SIM_GPS_UBLOX_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED #endif + + +// simulated compass support; currently only in SITL, not SimOnHW: +#ifndef AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED +#define AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_SIM_COMPASS_QMC5883L_ENABLED +#define AP_SIM_COMPASS_QMC5883L_ENABLED AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED +#endif diff --git a/libraries/SITL/SIM_last_letter.h b/libraries/SITL/SIM_last_letter.h index ae7cae5ecf8c22..d8afa83ba0c694 100644 --- a/libraries/SITL/SIM_last_letter.h +++ b/libraries/SITL/SIM_last_letter.h @@ -26,7 +26,7 @@ #if HAL_SIM_LAST_LETTER_ENABLED -#include +#include #include "SIM_Aircraft.h" @@ -77,7 +77,7 @@ class last_letter : public Aircraft { void start_last_letter(void); uint64_t last_timestamp_us; - SocketAPM sock; + SocketAPM_native sock; }; } // namespace SITL diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 2ff2220aa7ed7f..2aeed73ab6ed28 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -1169,6 +1169,7 @@ void SIM::sim_state_send(mavlink_channel_t chan) const (int32_t)(state.longitude*1.0e7)); } +#if HAL_LOGGING_ENABLED /* report SITL state to AP_Logger */ void SIM::Log_Write_SIMSTATE() { @@ -1196,6 +1197,7 @@ void SIM::Log_Write_SIMSTATE() }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif /* convert a set of roll rates from earth frame to body frame diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 24dbdac91a6aec..4d7394a7c4bb3a 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -22,6 +22,7 @@ #include "SIM_ToneAlarm.h" #include "SIM_EFI_MegaSquirt.h" #include "SIM_RichenPower.h" +#include "SIM_Loweheiser.h" #include "SIM_FETtecOneWireESC.h" #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" @@ -92,6 +93,10 @@ struct sitl_fdm { // earthframe wind, from backends that know it Vector3f wind_ef; + + // AGL altitude, usually derived from the terrain database in simulation: + float height_agl; + }; // number of rc output channels @@ -152,9 +157,6 @@ class SIM { // throttle when motors are active float throttle; - // height above ground - float height_agl; - static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info2[]; static const struct AP_Param::GroupInfo var_info3[]; @@ -297,6 +299,7 @@ class SIM { enum EFIType { EFI_TYPE_NONE = 0, EFI_TYPE_MS = 1, + EFI_TYPE_LOWEHEISER = 2, EFI_TYPE_HIRTH = 8, }; @@ -454,6 +457,9 @@ class SIM { ToneAlarm tonealarm_sim; SIM_Precland precland_sim; RichenPower richenpower_sim; +#if AP_SIM_LOWEHEISER_ENABLED + Loweheiser loweheiser_sim; +#endif IntelligentEnergy24 ie24_sim; FETtecOneWireESC fetteconewireesc_sim; #if AP_TEST_DRONECAN_DRIVERS diff --git a/libraries/SITL/examples/Airsim/follow-copter.sh b/libraries/SITL/examples/Airsim/follow-copter.sh index 29fe62d0b7b64f..47625999c82c9e 100755 --- a/libraries/SITL/examples/Airsim/follow-copter.sh +++ b/libraries/SITL/examples/Airsim/follow-copter.sh @@ -46,7 +46,7 @@ BASE_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/copter.parm,$ROOTDIR/Tools } # start up main copter in the current directory -$COPTER --model airsim-copter --uartA udpclient:$GCS_IP --uartC mcast:$MCAST_IP_PORT --defaults $BASE_DEFAULTS & +$COPTER --model airsim-copter --serial0 udpclient:$GCS_IP --serial1 mcast:$MCAST_IP_PORT --defaults $BASE_DEFAULTS & # Set number of extra copters to be simulated, change this for increasing the count NCOPTERS="1" @@ -71,7 +71,7 @@ FOLL_SYSID $FOLL_SYSID FOLL_DIST_MAX 1000 EOF pushd copter$i - $COPTER --model airsim-copter --uartA tcp:0 --uartC mcast:$MCAST_IP_PORT --instance $i --defaults $BASE_DEFAULTS,follow.parm & + $COPTER --model airsim-copter --serial0 tcp:0 --serial1 mcast:$MCAST_IP_PORT --instance $i --defaults $BASE_DEFAULTS,follow.parm & popd done wait diff --git a/libraries/SITL/examples/Airsim/multi_vehicle.sh b/libraries/SITL/examples/Airsim/multi_vehicle.sh index 4fdd20301754b8..f02717e6fc3895 100755 --- a/libraries/SITL/examples/Airsim/multi_vehicle.sh +++ b/libraries/SITL/examples/Airsim/multi_vehicle.sh @@ -28,7 +28,7 @@ BASE_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/copter.parm,$ROOTDIR/Tools } # start up main copter in the current directory -$COPTER --model airsim-copter --uartA udpclient:$GCS_IP:14550 --uartB tcp:0 --defaults $BASE_DEFAULTS & +$COPTER --model airsim-copter --serial0 udpclient:$GCS_IP:14550 --serial3 tcp:0 --defaults $BASE_DEFAULTS & # Set number of "extra" copters to be simulated, change this for increasing the count NCOPTERS="1" @@ -49,7 +49,7 @@ SYSID_THISMAV $SYSID EOF pushd copter$i - $COPTER --model airsim-copter --uartA udpclient:$GCS_IP:$UDP_PORT --uartB tcp:$i \ + $COPTER --model airsim-copter --serial0 udpclient:$GCS_IP:$UDP_PORT --serial3 tcp:$i \ --instance $i --defaults $BASE_DEFAULTS,identity.parm & popd done diff --git a/libraries/SITL/examples/Follow/plane_quad.sh b/libraries/SITL/examples/Follow/plane_quad.sh index 49dd4fd02aa46a..171bcdff20cbd6 100755 --- a/libraries/SITL/examples/Follow/plane_quad.sh +++ b/libraries/SITL/examples/Follow/plane_quad.sh @@ -13,14 +13,14 @@ PLANE=$ROOTDIR/build/sitl/bin/arduplane } # setup for either TCP or multicast -#UARTA="tcp:0" -UARTA="mcast:" +#SERIAL0="tcp:0" +SERIAL0="mcast:" PLANE_DEFAULTS="$ROOTDIR/Tools/autotest/models/plane.parm" COPTER_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/copter.parm" mkdir -p swarm/plane swarm/copter -(cd swarm/plane && $PLANE --model plane --uartA $UARTA --defaults $PLANE_DEFAULTS) & +(cd swarm/plane && $PLANE --model plane --serial0 $SERIAL0 --defaults $PLANE_DEFAULTS) & # create default parameter file for the follower cat < swarm/copter/follow.parm @@ -32,5 +32,5 @@ FOLL_SYSID 1 FOLL_DIST_MAX 1000 EOF -(cd swarm/copter && $COPTER --model quad --uartA $UARTA --instance 1 --defaults $COPTER_DEFAULTS,follow.parm) & +(cd swarm/copter && $COPTER --model quad --serial0 $SERIAL0 --instance 1 --defaults $COPTER_DEFAULTS,follow.parm) & wait diff --git a/libraries/SITL/examples/Morse/start_follow.sh b/libraries/SITL/examples/Morse/start_follow.sh index 7e5300fcaa6cc5..e213eaeb8daadf 100755 --- a/libraries/SITL/examples/Morse/start_follow.sh +++ b/libraries/SITL/examples/Morse/start_follow.sh @@ -9,7 +9,7 @@ GCS_IP=192.168.2.48 BASE_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/rover.parm,$ROOTDIR/Tools/autotest/default_params/rover-skid.parm" # start up main rover in the current directory -$ROVER --model morse-skid --uartA udpclient:$GCS_IP --uartC mcast: --defaults $BASE_DEFAULTS & +$ROVER --model morse-skid --serial0 udpclient:$GCS_IP --serial1 mcast: --defaults $BASE_DEFAULTS & # now start 2 rovers to follow the first, using # a separate directory for each to keep the eeprom.bin @@ -37,7 +37,7 @@ FOLL_SYSID $FOLL_SYSID FOLL_DIST_MAX 1000 EOF pushd rov$i - $ROVER --model "morse-skid:127.0.0.1:$port1:$port2" --uartA tcp:0 --uartC mcast: --instance $i --defaults $BASE_DEFAULTS,follow.parm & + $ROVER --model "morse-skid:127.0.0.1:$port1:$port2" --serial0 tcp:0 --serial1 mcast: --instance $i --defaults $BASE_DEFAULTS,follow.parm & popd done wait diff --git a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param index 0c7de8b67c26fc..fa828dfc49a490 100644 --- a/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param +++ b/libraries/SITL/examples/SilentWings/Params/Rolladen-Schneider-LS8b.param @@ -18,8 +18,8 @@ AHRS_TRIM_Y,0 AHRS_TRIM_Z,0 AHRS_WIND_MAX,0 AHRS_YAW_P,0.2 -ALT_HOLD_FBWCM,0 -ALT_HOLD_RTL,10000 +CRUISE_ALT_FLOOR,0.00 +RTL_ALTITUDE,100.00 ALT_OFFSET,0 ARMING_ACCTHRESH,0.75 ARMING_CHECK,0 @@ -29,8 +29,8 @@ ARMING_VOLT_MIN,0 ARMING_VOLT2_MIN,0 ARSPD_AUTOCAL,0 ARSPD_BUS,1 -ARSPD_FBW_MAX,40 -ARSPD_FBW_MIN,23 +AIRSPEED_MAX,40 +AIRSPEED_MIN,23 ARSPD_OFFSET,2013.475 ARSPD_PIN,1 ARSPD_PRIMARY,0 @@ -117,7 +117,7 @@ COMPASS_ORIENT2,0 COMPASS_ORIENT3,0 COMPASS_PMOT_EN,0 COMPASS_PRIMARY,0 -COMPASS_TYPEMASK,0 +COMPASS_DISBLMSK,0 COMPASS_USE,0 COMPASS_USE2,0 COMPASS_USE3,0 @@ -292,15 +292,15 @@ LAND_FLARE_SEC,2 LAND_PF_ALT,10 LAND_PF_ARSPD,0 LAND_PF_SEC,6 -LAND_PITCH_CD,0 +LAND_PITCH_DEG,0.00 LAND_SLOPE_RCALC,2 LAND_THEN_NEUTRL,0 LAND_THR_SLEW,0 LAND_TYPE,0 LEVEL_ROLL_LIMIT,5 -LIM_PITCH_MAX,2000 -LIM_PITCH_MIN,-2500 -LIM_ROLL_CD,5000 +PTCH_LIM_MAX_DEG,20.00 +PTCH_LIM_MIN_DEG,-25.00 +ROLL_LIMIT_DEG,50.00 LOG_BACKEND_TYPE,1 LOG_BITMASK,65535 LOG_DISARMED,0 @@ -310,7 +310,7 @@ LOG_MAV_BUFSIZE,8 LOG_REPLAY,0 MAG_ENABLE,1 MANUAL_RCMASK,0 -MIN_GNDSPD_CM,0 +MIN_GROUNDSPEED,0 MIS_OPTIONS,0 MIS_RESTART,0 MIS_TOTAL,12 @@ -849,9 +849,9 @@ TKOFF_THR_MAX,0 TKOFF_THR_MINACC,0 TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,0 -TRIM_ARSPD_CM,3000 +AIRSPEED_CRUISE,30.00 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PTCH_TRIM_DEG,0.00 TRIM_THROTTLE,45 TUNE_CHAN,0 TUNE_CHAN_MAX,2000 diff --git a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param index 82d518a4e12466..cb6b93e17612c5 100644 --- a/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param +++ b/libraries/SITL/examples/SilentWings/Params/Schleicher-ASW27B.param @@ -18,8 +18,8 @@ AHRS_TRIM_Y,0 AHRS_TRIM_Z,0 AHRS_WIND_MAX,0 AHRS_YAW_P,0.2 -ALT_HOLD_FBWCM,0 -ALT_HOLD_RTL,10000 +CRUISE_ALT_FLOOR,0.00 +RTL_ALTITUDE,100.00 ALT_OFFSET,0 ARMING_ACCTHRESH,0.75 ARMING_CHECK,0 @@ -29,8 +29,8 @@ ARMING_VOLT_MIN,0 ARMING_VOLT2_MIN,0 ARSPD_AUTOCAL,0 ARSPD_BUS,1 -ARSPD_FBW_MAX,40 -ARSPD_FBW_MIN,23 +AIRSPEED_MAX,40 +AIRSPEED_MIN,23 ARSPD_OFFSET,2013.475 ARSPD_PIN,1 ARSPD_PRIMARY,0 @@ -120,7 +120,7 @@ COMPASS_ORIENT2,0 COMPASS_ORIENT3,0 COMPASS_PMOT_EN,0 COMPASS_PRIMARY,0 -COMPASS_TYPEMASK,0 +COMPASS_DISBLMSK,0 COMPASS_USE,0 COMPASS_USE2,0 COMPASS_USE3,0 @@ -295,15 +295,15 @@ LAND_FLARE_SEC,2 LAND_PF_ALT,10 LAND_PF_ARSPD,0 LAND_PF_SEC,6 -LAND_PITCH_CD,0 +LAND_PITCH_DEG,0.00 LAND_SLOPE_RCALC,2 LAND_THEN_NEUTRL,0 LAND_THR_SLEW,0 LAND_TYPE,0 LEVEL_ROLL_LIMIT,5 -LIM_PITCH_MAX,2000 -LIM_PITCH_MIN,-2500 -LIM_ROLL_CD,5000 +PTCH_LIM_MAX_DEG,20.00 +PTCH_LIM_MIN_DEG,-25.00 +ROLL_LIMIT_DEG,50.00 LOG_BACKEND_TYPE,1 LOG_BITMASK,65535 LOG_DISARMED,0 @@ -313,7 +313,7 @@ LOG_MAV_BUFSIZE,8 LOG_REPLAY,0 MAG_ENABLE,1 MANUAL_RCMASK,0 -MIN_GNDSPD_CM,0 +MIN_GROUNDSPEED,0 MIS_OPTIONS,0 MIS_RESTART,0 MIS_TOTAL,11 @@ -843,9 +843,9 @@ TKOFF_THR_MAX,0 TKOFF_THR_MINACC,0 TKOFF_THR_MINSPD,0 TKOFF_THR_SLEW,0 -TRIM_ARSPD_CM,2700 +AIRSPEED_CRUISE,27.00 TRIM_AUTO,0 -TRIM_PITCH_CD,0 +PTCH_TRIM_DEG,0.00 TRIM_THROTTLE,45 TUNE_CHAN,0 TUNE_CHAN_MAX,2000 diff --git a/libraries/SITL/examples/follow-copter.sh b/libraries/SITL/examples/follow-copter.sh index 88067001719521..8dbeebc75f0d68 100755 --- a/libraries/SITL/examples/follow-copter.sh +++ b/libraries/SITL/examples/follow-copter.sh @@ -75,7 +75,7 @@ AUTO_OPTIONS 7 EOF pushd copter1 -$COPTER --model quad --home=$HOMELAT,$HOMELONG,$HOMEALT,0 --uartA udpclient:$GCS_IP --uartC mcast:$MCAST_IP_PORT --defaults $BASE_DEFAULTS,leader.parm & +$COPTER --model quad --home=$HOMELAT,$HOMELONG,$HOMEALT,0 --serial0 udpclient:$GCS_IP --serial1 mcast:$MCAST_IP_PORT --defaults $BASE_DEFAULTS,leader.parm & popd # now start other copters to follow the first, using @@ -103,7 +103,7 @@ EOF pushd copter$i LAT=$(echo "$HOMELAT + 0.0005*$i" | bc -l) LONG=$(echo "$HOMELONG + 0.0005*$i" | bc -l) - $COPTER --model quad --home=$LAT,$LONG,$HOMEALT,0 --uartA tcp:0 --uartC mcast:$MCAST_IP_PORT --instance $i --defaults $BASE_DEFAULTS,follow.parm & + $COPTER --model quad --home=$LAT,$LONG,$HOMEALT,0 --serial0 tcp:0 --serial1 mcast:$MCAST_IP_PORT --instance $i --defaults $BASE_DEFAULTS,follow.parm & popd done wait diff --git a/modules/ChibiOS b/modules/ChibiOS index d8c45abde5fb39..cc37d7f2f772c5 160000 --- a/modules/ChibiOS +++ b/modules/ChibiOS @@ -1 +1 @@ -Subproject commit d8c45abde5fb398ef9da6fc8fc36992f2ae7b15c +Subproject commit cc37d7f2f772c5eb7c17225c6467cc2ce4c49b29 diff --git a/modules/lwip b/modules/lwip new file mode 160000 index 00000000000000..143a6a5cb80239 --- /dev/null +++ b/modules/lwip @@ -0,0 +1 @@ +Subproject commit 143a6a5cb8023921b5dced55c30551ffb752b640 diff --git a/modules/mavlink b/modules/mavlink index 71af5c43fc24ff..130a836efbfef0 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit 71af5c43fc24ffb237d3a93dc0c8358d67aa0cc0 +Subproject commit 130a836efbfef0eb3287a92cd5e187de7facdce2 diff --git a/wscript b/wscript index 19630462ecdd70..c21a031e52f920 100644 --- a/wscript +++ b/wscript @@ -86,6 +86,29 @@ def _set_build_context_variant(board): continue c.variant = board +# Remove all submodules and then sync +@conf +def submodule_force_clean(ctx): + whitelist = { + 'COLCON_IGNORE', + 'esp_idf', + } + + # Get all items in the modules folder + module_list = os.scandir('modules') + + # Delete all directories except those in the whitelist + for module in module_list: + if (module.is_dir()) and (module.name not in whitelist): + shutil.rmtree(module) + + submodulesync(ctx) + +# run Tools/gittools/submodule-sync.sh to sync submodules +@conf +def submodulesync(ctx): + subprocess.call(['Tools/gittools/submodule-sync.sh']) + def init(ctx): # Generate Task List, so that VS Code extension can keep track # of changes to possible build targets @@ -180,6 +203,11 @@ def options(opt): action='store_true', default=False, help='enable OS level thread statistics.') + + g.add_option('--enable-ppp', + action='store_true', + default=False, + help='enable PPP networking.') g.add_option('--bootloader', action='store_true', @@ -548,6 +576,8 @@ def configure(cfg): cfg.recurse('libraries/AP_HAL_SITL') cfg.recurse('libraries/SITL') + cfg.recurse('libraries/AP_Networking') + cfg.start_msg('Scripting runtime checks') if cfg.options.scripting_checks: cfg.end_msg('enabled')