diff --git a/.github/workflows/cygwin_build.yml b/.github/workflows/cygwin_build.yml index 9ae7ffba31fac7..f836b9c09a923b 100644 --- a/.github/workflows/cygwin_build.yml +++ b/.github/workflows/cygwin_build.yml @@ -189,7 +189,7 @@ jobs: shell: C:\cygwin\bin\bash.exe -eo pipefail '{0}' run: >- ln -sf /usr/bin/python3.7 /usr/bin/python && ln -sf /usr/bin/pip3.7 /usr/bin/pip && - python -m pip install --progress-bar off empy pexpect && + python -m pip install --progress-bar off empy==3.3.4 pexpect && python -m pip install --progress-bar off dronecan --upgrade && cp /usr/bin/ccache /usr/local/bin/ && cd /usr/local/bin && ln -s ccache /usr/local/bin/gcc && diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml index c2a438891f7da7..7f2a11bf060ca8 100644 --- a/.github/workflows/esp32_build.yml +++ b/.github/workflows/esp32_build.yml @@ -161,11 +161,26 @@ jobs: - 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 update + sudo apt 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 + rm -rf /usr/local/bin/cmake sudo apt remove --purge --auto-remove cmake sudo apt update && \ sudo apt install -y software-properties-common lsb-release && \ @@ -183,7 +198,7 @@ jobs: cd modules/esp_idf echo "Installing ESP-IDF tools...." - ./install.sh 2>&1 > /dev/null + ./install.sh esp32,esp32s3 2>&1 > /dev/null cd ../.. @@ -200,7 +215,7 @@ jobs: ./install.sh source ./export.sh cd ../.. - python -m pip install --progress-bar off future lxml pymavlink MAVProxy pexpect flake8 geocoder empy dronecan + 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 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_linux_sbc.yml b/.github/workflows/test_linux_sbc.yml index d55a75b3f42c19..a66bc24de37b85 100644 --- a/.github/workflows/test_linux_sbc.yml +++ b/.github/workflows/test_linux_sbc.yml @@ -149,7 +149,8 @@ jobs: bebop, erlebrain2, pxfmini, - pxf + pxf, + canzero, ] toolchain: [ armhf, 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/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 0fbe19edaa8ca8..919a7c7305cd95 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -313,6 +313,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 diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 79d1739ed21f0f..2d20396ecfc694 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -23,6 +23,7 @@ const AP_Param::Info Tracker::var_info[] = { // @DisplayName: Ground station MAVLink system ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index 972c36f52505e8..e43876f8ded8d5 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, diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index d2e4838f8ce30e..72bc9addf073bf 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) 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/Copter.cpp b/ArduCopter/Copter.cpp index f5242feb0d95d1..c1218651342394 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -404,13 +404,7 @@ bool Copter::set_circle_rate(float rate_dps) // set desired speed (m/s). Used for scripting. bool Copter::set_desired_speed(float speed) { - // exit if vehicle is not in auto mode - if (!flightmode->is_autopilot()) { - return false; - } - - wp_nav->set_speed_xy(speed * 100.0f); - return true; + return flightmode->set_speed_xy(speed * 100.0f); } #if MODE_AUTO_ENABLED == ENABLED diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0827e2a09ca9e7..bb468a39939a52 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -77,12 +77,6 @@ #include "defines.h" #include "config.h" -#if FRAME_CONFIG == HELI_FRAME - #define AC_AttitudeControl_t AC_AttitudeControl_Heli -#else - #define AC_AttitudeControl_t AC_AttitudeControl_Multi -#endif - #if FRAME_CONFIG == HELI_FRAME #define MOTOR_CLASS AP_MotorsHeli #else @@ -485,7 +479,8 @@ class Copter : public AP_Vehicle { // Attitude, Position and Waypoint navigation objects // To-Do: move inertial nav up or other navigation variables down here - AC_AttitudeControl_t *attitude_control; + AC_AttitudeControl *attitude_control; + const struct AP_Param::GroupInfo *attitude_control_var_info; AC_PosControl *pos_control; AC_WPNav *wp_nav; AC_Loiter *loiter_nav; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 7c09a42b07f7c4..e3b949b7c322f5 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -46,6 +46,7 @@ const AP_Param::Info Copter::var_info[] = { // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -450,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 @@ -497,11 +498,7 @@ const AP_Param::Info Copter::var_info[] = { // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp,../libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp -#if FRAME_CONFIG == HELI_FRAME - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Heli), -#else - GOBJECTPTR(attitude_control, "ATC_", AC_AttitudeControl_Multi), -#endif + GOBJECTVARPTR(attitude_control, "ATC_", &copter.attitude_control_var_info), // @Group: PSC // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -699,6 +696,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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e59bf2238788b6..3d8b62b96d936c 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 diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 70cf715b617ead..6ccc0923aa6fe0 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,20 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +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 + - RADIX2HD supports external I2C compasses + - SpeedyBeeF405v4 support +2) Bug fixes + - DroneCAN battery monitor with cell monitor SoC reporting fix + - NTF_LED_TYPES parameter description fixed (was missing IS31FL3195) + - ProfiLED output fixed in both Notify and Scripting + - Scripting bug that could cause crash if parameters were added in flight + - STAT_BOOTCNT param fix (was not updating in some cases) + - Takeoff RPM check fixed where motors are attached to AUX channels + - don't query hobbywing DroneCAN ESC IDs while armed +------------------------------------------------------------------ Copter 4.4.3 14-Nov-2023 Changes from 4.4.3-beta1 1) AP_GPS: correct uBlox M10 configuration on low flash boards diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 40acda0f05c592..749a830f4dce65 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -114,9 +114,9 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di } else { // absolute angle _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd); - if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) { + if (direction < 0 && is_positive(_fixed_yaw_offset_cd)) { _fixed_yaw_offset_cd -= 36000.0; - } else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) { + } else if (direction > 0 && is_negative(_fixed_yaw_offset_cd)) { _fixed_yaw_offset_cd += 36000.0; } } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 329678e1b889ca..ace8cc43f2edc3 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -433,10 +433,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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 3de08e4766a69f..afb348c1dc6526 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -241,7 +241,7 @@ class Mode { AC_PosControl *&pos_control; AP_InertialNav &inertial_nav; AP_AHRS &ahrs; - AC_AttitudeControl_t *&attitude_control; + AC_AttitudeControl *&attitude_control; MOTOR_CLASS *&motors; RC_Channel *&channel_roll; RC_Channel *&channel_pitch; @@ -679,6 +679,7 @@ class ModeAuto : public Mode { int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) uint32_t condition_start; + // Land within Auto state enum class State { FlyToLocation = 0, Descending = 1 @@ -713,6 +714,13 @@ class ModeAuto : public Mode { float climb_rate; // climb rate in m/s. provided by mission command uint32_t start_ms; // system time that nav attitude time command was received (used for timeout) } nav_attitude_time; + + // desired speeds + struct { + float xy; // desired speed horizontally in m/s. 0 if unset + float up; // desired speed upwards in m/s. 0 if unset + float down; // desired speed downwards in m/s. 0 if unset + } desired_speed_override; }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b79b008c2e6b64..c33c9d98d7ecba 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -41,6 +41,9 @@ bool ModeAuto::init(bool ignore_checks) // initialise waypoint and spline controller wp_nav->wp_and_spline_init(); + // initialise desired speed overrides + desired_speed_override = {0, 0, 0}; + // set flag to start mission waiting_to_start = true; @@ -370,7 +373,16 @@ bool ModeAuto::wp_start(const Location& dest_loc) stopping_point = takeoff_complete_pos.tofloat(); } } - wp_nav->wp_and_spline_init(0, stopping_point); + float des_speed_xy_cm = is_positive(desired_speed_override.xy) ? (desired_speed_override.xy * 100) : 0; + wp_nav->wp_and_spline_init(des_speed_xy_cm, stopping_point); + + // override speeds up and down if necessary + if (is_positive(desired_speed_override.up)) { + wp_nav->set_speed_up(desired_speed_override.up * 100.0); + } + if (is_positive(desired_speed_override.down)) { + wp_nav->set_speed_down(desired_speed_override.down * 100.0); + } } if (!wp_nav->set_wp_destination_loc(dest_loc)) { @@ -585,18 +597,21 @@ bool ModeAuto::use_pilot_yaw(void) const bool ModeAuto::set_speed_xy(float speed_xy_cms) { copter.wp_nav->set_speed_xy(speed_xy_cms); + desired_speed_override.xy = speed_xy_cms * 0.01; return true; } bool ModeAuto::set_speed_up(float speed_up_cms) { copter.wp_nav->set_speed_up(speed_up_cms); + desired_speed_override.up = speed_up_cms * 0.01; return true; } bool ModeAuto::set_speed_down(float speed_down_cms) { copter.wp_nav->set_speed_down(speed_down_cms); + desired_speed_override.down = speed_down_cms * 0.01; return true; } @@ -1515,14 +1530,14 @@ bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const switch (next_cmd.id) { case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_LOITER_TIME: #if AP_MISSION_NAV_PAYLOAD_PLACE_ENABLED - case MAV_CMD_NAV_PAYLOAD_PLACE: { + case MAV_CMD_NAV_PAYLOAD_PLACE: +#endif + case MAV_CMD_NAV_LOITER_TIME: { const Location dest_loc = loc_from_cmd(current_cmd, default_loc); const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc); return wp_nav->set_wp_destination_next_loc(next_dest_loc); } -#endif case MAV_CMD_NAV_SPLINE_WAYPOINT: { // get spline's location and next location from command and send to wp_nav Location next_dest_loc, next_next_dest_loc; @@ -1850,17 +1865,18 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) // Do (Now) commands /********************************************************************************/ - - void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) { if (cmd.content.speed.target_ms > 0) { if (cmd.content.speed.speed_type == 2) { copter.wp_nav->set_speed_up(cmd.content.speed.target_ms * 100.0f); + desired_speed_override.up = cmd.content.speed.target_ms; } else if (cmd.content.speed.speed_type == 3) { copter.wp_nav->set_speed_down(cmd.content.speed.target_ms * 100.0f); + desired_speed_override.down = cmd.content.speed.target_ms; } else { copter.wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f); + desired_speed_override.xy = cmd.content.speed.target_ms; } } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 7c74838a3fe742..a8421d9009741e 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -483,14 +483,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_throw.cpp b/ArduCopter/mode_throw.cpp index c9ff702e3fd887..a2f71ff42f24d4 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -272,8 +272,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/system.cpp b/ArduCopter/system.cpp index 6679766a3b41d7..4c740a2e3b3e1c 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -444,26 +444,24 @@ void Copter::allocate_motors(void) AP_BoardConfig::allocation_error("AP_AHRS_View"); } - const struct AP_Param::GroupInfo *ac_var_info; - #if FRAME_CONFIG != HELI_FRAME if ((AP_Motors::motor_frame_class)g2.frame_class.get() == AP_Motors::MOTOR_FRAME_6DOF_SCRIPTING) { #if AP_SCRIPTING_ENABLED attitude_control = new AC_AttitudeControl_Multi_6DoF(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Multi_6DoF::var_info; + attitude_control_var_info = AC_AttitudeControl_Multi_6DoF::var_info; #endif // AP_SCRIPTING_ENABLED } else { attitude_control = new AC_AttitudeControl_Multi(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Multi::var_info; + attitude_control_var_info = AC_AttitudeControl_Multi::var_info; } #else attitude_control = new AC_AttitudeControl_Heli(*ahrs_view, aparm, *motors); - ac_var_info = AC_AttitudeControl_Heli::var_info; + attitude_control_var_info = AC_AttitudeControl_Heli::var_info; #endif if (attitude_control == nullptr) { AP_BoardConfig::allocation_error("AttitudeControl"); } - AP_Param::load_object_from_eeprom(attitude_control, ac_var_info); + AP_Param::load_object_from_eeprom(attitude_control, attitude_control_var_info); pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); if (pos_control == nullptr) { diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index b4e868b2ff288c..2905175fdeb151 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -66,8 +66,10 @@ 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"); 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..dd6af0ab6e30bc 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -798,8 +798,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 +816,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) { @@ -914,8 +916,8 @@ void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const return; } #endif - pitch = ahrs.pitch; - roll = ahrs.roll; + pitch = ahrs.get_pitch(); + roll = ahrs.get_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; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 9427b99e0cdac9..9a571e3b5e0ce9 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -129,9 +129,9 @@ 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); @@ -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) { diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 55d5ab4c46b495..a5e97fe2ae4970 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -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,12 +386,11 @@ 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 @@ -406,6 +408,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 +417,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. diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 720eed4c273694..dd55010d46adce 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -23,6 +23,7 @@ const AP_Param::Info Plane::var_info[] = { // @DisplayName: Ground station MAVLink system ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -416,7 +417,7 @@ const AP_Param::Info Plane::var_info[] = { // @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), @@ -432,7 +433,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), @@ -756,9 +757,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 @@ -1174,7 +1175,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 @@ -1183,14 +1184,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 diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7210bfe7771267..ef126900dfccf1 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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 @@ -542,9 +542,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.h b/ArduPlane/Plane.h index 80e3cb8636ce1e..8dcb90e47d8dd0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -85,7 +85,7 @@ #include #include #if AP_EXTERNAL_CONTROL_ENABLED -#include +#include "AP_ExternalControl_Plane.h" #endif #include "GCS_Mavlink.h" @@ -167,6 +167,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: @@ -776,9 +780,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[]; @@ -1091,7 +1095,7 @@ class Plane : public AP_Vehicle { // servos.cpp void set_servos_idle(void); void set_servos(); - void set_servos_controlled(void); + void set_throttle(void); void set_takeoff_expected(void); void set_servos_old_elevons(void); void set_servos_flaps(void); @@ -1103,7 +1107,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 +1244,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 ffc5323e418edf..9f965c9b002398 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,3 +1,19 @@ +Release 4.4.4 19th December 2023 +-------------------------------- + +Changes from 4.4.3: + +- CubeOrange Sim-on-hardware compilation fix +- RADIX2HD supports external I2C compasses +- SpeedyBeeF405v4 support +- DroneCAN battery monitor with cell monitor SoC reporting fix +- ProfiLED output fixed in both Notify and Scripting +- NTF_LED_TYPES parameter description fixed (was missing IS31FL3195) +- Scripting bug that could cause crash if parameters were added in flight +- STAT_BOOTCNT param fix (was not updating in some cases) +- don't query hobbywing DroneCAN ESC IDs while armed +- clamp empy version to prevent build errors + Release 4.4.3 14th November 2023 -------------------------------- diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 823f1e4da93bb5..972da6f765c8e3 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -348,6 +348,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; @@ -1089,7 +1090,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/defines.h b/ArduPlane/defines.h index c7389daf49bdca..a480162eff722e 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) diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index ff124dc6ba3dd1..6eef3fb0fab389 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -113,6 +113,9 @@ bool Mode::enter() plane.adsb.set_is_auto_mode(does_auto_navigation()); #endif + // set the nav controller stale AFTER _enter() so that we can check if we're currently in a loiter during the mode change + plane.nav_controller->set_data_is_stale(); + // reset steering integrator on mode change plane.steerController.reset_I(); @@ -132,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; diff --git a/ArduPlane/mode_LoiterAltQLand.cpp b/ArduPlane/mode_LoiterAltQLand.cpp index 0cc8b9457c11e7..8fbfd59735cd24 100644 --- a/ArduPlane/mode_LoiterAltQLand.cpp +++ b/ArduPlane/mode_LoiterAltQLand.cpp @@ -10,17 +10,13 @@ bool ModeLoiterAltQLand::_enter() return true; } + // If we were already in a loiter then use that waypoint. Else, use the current point + const bool already_in_a_loiter = plane.nav_controller->reached_loiter_target() && !plane.nav_controller->data_is_stale(); + const Location loiter_wp = already_in_a_loiter ? plane.next_WP_loc : plane.current_loc; + ModeLoiter::_enter(); -#if AP_TERRAIN_AVAILABLE - if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN); - } else { - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); - } -#else - plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME); -#endif + handle_guided_request(loiter_wp); switch_qland(); 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_guided.cpp b/ArduPlane/mode_guided.cpp index a523ddf87065ed..b4ae1241e757f6 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); diff --git a/ArduPlane/mode_takeoff.cpp b/ArduPlane/mode_takeoff.cpp index db212361c398e8..b807c64777ec00 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 @@ -81,7 +81,7 @@ void ModeTakeoff::update() const float dist = target_dist; if (!takeoff_started) { 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) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 0a5c724fc82b65..25c7ff533ba2a2 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1219,7 +1219,7 @@ bool QuadPlane::is_flying_vtol(void) const } if (plane.control_mode->is_vtol_man_mode()) { // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero - return is_positive(plane.get_throttle_input()); + return is_positive(get_throttle_input()); } if (in_vtol_mode() && millis() - landing_detect.lower_limit_start_ms > 5000) { // use landing detector @@ -1289,7 +1289,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const const auto rudder_in = plane.channel_rudder->get_control_in(); bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active(); if (!manual_air_mode && - !is_positive(plane.get_throttle_input()) && + !is_positive(get_throttle_input()) && (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) && plane.arming.get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM && rudder_in < 0 && @@ -1366,7 +1366,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const */ void QuadPlane::init_throttle_wait(void) { - if (plane.get_throttle_input() >= 10 || + if (get_throttle_input() >= 10 || plane.is_flying()) { throttle_wait = false; } else { @@ -1496,7 +1496,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; } @@ -1541,7 +1541,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)); } @@ -1920,7 +1920,7 @@ void QuadPlane::update_throttle_suppression(void) consider non-zero throttle to mean that pilot is commanding takeoff unless in a manual thottle mode */ - if (!is_zero(plane.get_throttle_input()) && + if (!is_zero(get_throttle_input()) && (rc().arming_check_throttle() || plane.control_mode->is_vtol_man_throttle() || plane.channel_throttle->norm_input_dz() > 0)) { @@ -2636,7 +2636,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", @@ -3046,7 +3046,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 @@ -3663,7 +3663,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, }; @@ -4061,7 +4060,7 @@ void QuadPlane::update_throttle_mix(void) if (plane.control_mode->is_vtol_man_throttle()) { // manual throttle - if (!is_positive(plane.get_throttle_input()) && !air_mode_active()) { + if (!is_positive(get_throttle_input()) && !air_mode_active()) { attitude_control->set_throttle_mix_min(); } else { attitude_control->set_throttle_mix_man(); @@ -4729,4 +4728,18 @@ bool QuadPlane::should_disable_TECS() const return false; } +// Get pilot throttle input with deadzone, this will return 50% throttle in failsafe! +// This is a re-implmentation of Plane::get_throttle_input +// Ignoring the no_deadzone case means we don't need to check for valid RC +// This is handled by Plane::control_failsafe setting of control in +float QuadPlane::get_throttle_input() const +{ + float ret = plane.channel_throttle->get_control_in(); + if (plane.reversed_throttle) { + // RC option for reverse throttle has been set + ret = -ret; + } + return ret; +} + #endif // HAL_QUADPLANE_ENABLED diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index b919be2c445869..2c5a624ff4a4e4 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; }; @@ -190,6 +189,9 @@ class QuadPlane */ bool should_disable_TECS() const; + // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe! + float get_throttle_input() const; + private: AP_AHRS &ahrs; diff --git a/ArduPlane/reverse_thrust.cpp b/ArduPlane/reverse_thrust.cpp index f64f37b276d6b3..16dc514b547646 100644 --- a/ArduPlane/reverse_thrust.cpp +++ b/ArduPlane/reverse_thrust.cpp @@ -125,6 +125,10 @@ bool Plane::have_reverse_thrust(void) const */ float Plane::get_throttle_input(bool no_deadzone) const { + if (!rc().has_valid_input()) { + // Return 0 if there is no valid input + return 0.0; + } float ret; if (no_deadzone) { ret = channel_throttle->get_control_in_zero_dz(); @@ -143,6 +147,10 @@ float Plane::get_throttle_input(bool no_deadzone) const */ float Plane::get_adjusted_throttle_input(bool no_deadzone) const { + if (!rc().has_valid_input()) { + // Return 0 if there is no valid input + return 0.0; + } if ((plane.channel_throttle->get_type() != RC_Channel::ControlType::RANGE) || (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)) == 0) { return get_throttle_input(no_deadzone); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 729e67089e528a..c8f03bb8968486 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); + } /* @@ -472,13 +499,8 @@ void Plane::throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle) /* setup output channels all non-manual modes */ -void Plane::set_servos_controlled(void) +void Plane::set_throttle(void) { - 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,16 +531,14 @@ void Plane::set_servos_controlled(void) } // compensate for battery voltage drop - throttle_voltage_comp(min_throttle, max_throttle); + g2.fwd_batt_cmp.update(); + 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 - 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 (!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 @@ -532,14 +552,18 @@ void Plane::set_servos_controlled(void) SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); } } else if (suppress_throttle()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default - // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: - if (landing.is_flaring() && landing.use_thr_min_during_flare() ) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get()); - } 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)); + + } else if (landing.is_flaring() && landing.use_thr_min_during_flare() ) { + // throttle is suppressed (above) to zero in final flare in auto mode, but we allow instead thr_min if user prefers, eg turbines: + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, aparm.throttle_min.get()); + + } else { + // default + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); + } #if AP_SCRIPTING_ENABLED } else if (nav_scripting_active()) { @@ -551,9 +575,7 @@ void Plane::set_servos_controlled(void) control_mode == &mode_fbwa || control_mode == &mode_autotune) { // a manual throttle mode - if (!rc().has_valid_input()) { - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_passthru_stabilize ? 0.0 : MAX(min_throttle,0)); - } else if (g.throttle_passthru_stabilize) { + 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)); @@ -577,6 +599,11 @@ void Plane::set_servos_controlled(void) } SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, fwd_thr); #endif // HAL_QUADPLANE_ENABLED + + } else { + // Apply min/max limits and 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, constrain_float(throttle, min_throttle, max_throttle)); } } @@ -825,11 +852,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/tailsitter.cpp b/ArduPlane/tailsitter.cpp index f40fa33d1f26eb..a94e4cb8c5ea0a 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -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,6 +775,29 @@ 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; + +} + +// 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)); } // return true if pitch control should be relaxed 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/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 78a3e288b9466b..aa4c39b3268a73 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -583,7 +583,23 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) break; // only accept control aimed at us } - sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons); + sub.transform_manual_control_to_rc_override( + packet.x, + packet.y, + packet.z, + packet.r, + packet.buttons, + packet.buttons2, + packet.enabled_extensions, + packet.s, + packet.t, + packet.aux1, + packet.aux2, + packet.aux3, + packet.aux4, + packet.aux5, + packet.aux6 + ); sub.failsafe.last_pilot_input_ms = AP_HAL::millis(); // a RC override message is considered to be a 'heartbeat' diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 73ce3dbcb02f54..5711f4fe31dc8e 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -47,6 +47,8 @@ const AP_Param::Info Sub::var_info[] = { // @Param: SYSID_MYGCS // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station + // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -348,6 +350,70 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp GGROUP(jbtn_15, "BTN15_", JSButton), + // @Group: BTN16_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_16, "BTN16_", JSButton), + + // @Group: BTN17_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_17, "BTN17_", JSButton), + + // @Group: BTN18_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_18, "BTN18_", JSButton), + + // @Group: BTN19_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_19, "BTN19_", JSButton), + + // @Group: BTN20_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_20, "BTN20_", JSButton), + + // @Group: BTN21_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_21, "BTN21_", JSButton), + + // @Group: BTN22_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_22, "BTN22_", JSButton), + + // @Group: BTN23_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_23, "BTN23_", JSButton), + + // @Group: BTN24_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_24, "BTN24_", JSButton), + + // @Group: BTN25_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_25, "BTN25_", JSButton), + + // @Group: BTN26_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_26, "BTN26_", JSButton), + + // @Group: BTN27_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_27, "BTN27_", JSButton), + + // @Group: BTN28_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_28, "BTN28_", JSButton), + + // @Group: BTN29_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_29, "BTN29_", JSButton), + + // @Group: BTN30_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_30, "BTN30_", JSButton), + + // @Group: BTN31_ + // @Path: ../libraries/AP_JSButton/AP_JSButton.cpp + GGROUP(jbtn_31, "BTN31_", JSButton), + // @Param: RC_SPEED // @DisplayName: ESC Update Speed // @Description: This is the speed in Hertz that your ESCs will receive updates @@ -410,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_ diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index f170667859e8f5..95ca7923ab86fd 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -149,6 +149,23 @@ class Parameters { k_param_jbtn_14, k_param_jbtn_15, + // 16 more for MANUAL_CONTROL extensions + k_param_jbtn_16, + k_param_jbtn_17, + k_param_jbtn_18, + k_param_jbtn_19, + k_param_jbtn_20, + k_param_jbtn_21, + k_param_jbtn_22, + k_param_jbtn_23, + k_param_jbtn_24, + k_param_jbtn_25, + k_param_jbtn_26, + k_param_jbtn_27, + k_param_jbtn_28, + k_param_jbtn_29, + k_param_jbtn_30, + k_param_jbtn_31, // PID Controllers k_param_p_pos_xy = 126, // deprecated @@ -295,6 +312,23 @@ class Parameters { JSButton jbtn_13; JSButton jbtn_14; JSButton jbtn_15; + // 16 - 31 from manual_control extension + JSButton jbtn_16; + JSButton jbtn_17; + JSButton jbtn_18; + JSButton jbtn_19; + JSButton jbtn_20; + JSButton jbtn_21; + JSButton jbtn_22; + JSButton jbtn_23; + JSButton jbtn_24; + JSButton jbtn_25; + JSButton jbtn_26; + JSButton jbtn_27; + JSButton jbtn_28; + JSButton jbtn_29; + JSButton jbtn_30; + JSButton jbtn_31; // Acro parameters AP_Float acro_rp_p; @@ -358,6 +392,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = { AP_Arming::ARMING_CHECK_BATTERY}, { "CIRCLE_RATE", 2.0f}, { "ATC_ACCEL_Y_MAX", 110000.0f}, + { "ATC_RATE_Y_MAX", 180.0f}, { "RC3_TRIM", 1100}, { "COMPASS_OFFS_MAX", 1000}, { "INS_GYR_CAL", 0}, @@ -368,4 +403,24 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = { { "RC8_OPTION", 213}, // MOUNT1_PITCH { "MOT_PWM_MIN", 1100}, { "MOT_PWM_MAX", 1900}, + { "PSC_JERK_Z", 50.0f}, + { "WPNAV_SPEED", 100.0f}, + { "PILOT_SPEED_UP", 100.0f}, + { "PSC_VELXY_P", 6.0f}, + { "EK3_SRC1_VELZ", 0}, +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR + { "BARO_PROBE_EXT", 0}, + { "BATT_MONITOR", 4}, + { "BATT_CAPACITY", 0}, + { "LEAK1_PIN", 27}, + { "SCHED_LOOP_RATE", 200}, + { "SERVO13_FUNCTION", 59}, // k_rcin9, lights 1 + { "SERVO14_FUNCTION", 60}, // k_rcin10, lights 2 + { "SERVO16_FUNCTION", 7}, // k_mount_tilt + { "SERVO16_REVERSED", 1}, +#else + { "BARO_PROBE_EXT", 768}, + { "SERVO9_FUNCTION", 59}, // k_rcin9, lights 1 + { "SERVO10_FUNCTION", 7}, // k_mount_tilt +#endif }; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 2e4c3a41360444..482aabfc388920 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -455,7 +455,15 @@ class Sub : public AP_Vehicle { void init_rc_out(); void enable_motor_output(); void init_joystick(); - void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons); + void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, + int16_t s, + int16_t t, + int16_t aux1, + int16_t aux2, + int16_t aux3, + int16_t aux4, + int16_t aux5, + int16_t aux6); void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false); void handle_jsbutton_release(uint8_t button, bool shift); JSButton* get_button(uint8_t index); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index f10d7d3a451089..0eeafe94fb403a 100644 --- a/ArduSub/joystick.cpp +++ b/ArduSub/joystick.cpp @@ -17,7 +17,7 @@ int16_t xTrim = 0; int16_t yTrim = 0; int16_t video_switch = 1100; int16_t x_last, y_last, z_last; -uint16_t buttons_prev; +uint32_t buttons_prev; // Servo control output channels // TODO: Allow selecting output channels @@ -51,7 +51,15 @@ void Sub::init_joystick() gain = constrain_float(gain, 0.1, 1.0); } -void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons, uint16_t buttons2, uint8_t enabled_extensions, + int16_t s, + int16_t t, + int16_t aux1, + int16_t aux2, + int16_t aux3, + int16_t aux4, + int16_t aux5, + int16_t aux6) { float rpyScale = 0.4*gain; // Scale -1000-1000 to -400-400 with gain @@ -65,17 +73,18 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t cam_tilt = 1500; cam_pan = 1500; + uint32_t all_buttons = buttons | (buttons2 << 16); // Detect if any shift button is pressed - for (uint8_t i = 0 ; i < 16 ; i++) { - if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) { + for (uint8_t i = 0 ; i < 32 ; i++) { + if ((all_buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) { shift = true; } } // Act if button is pressed // Only act upon pressing button and ignore holding. This provides compatibility with Taranis as joystick. - for (uint8_t i = 0 ; i < 16 ; i++) { - if ((buttons & (1 << i))) { + for (uint8_t i = 0 ; i < 32 ; i++) { + if ((all_buttons & (1 << i))) { handle_jsbutton_press(i,shift,(buttons_prev & (1 << i))); // buttonDebounce = tnow_ms; } else if (buttons_prev & (1 << i)) { @@ -83,7 +92,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t } } - buttons_prev = buttons; + buttons_prev = all_buttons; // attitude mode: if (roll_pitch_flag == 1) { @@ -110,8 +119,8 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t xTot = x + xTrim; } - RC_Channels::set_override(0, constrain_int16(pitchTrim + rpyCenter,1100,1900), tnow); // pitch - RC_Channels::set_override(1, constrain_int16(rollTrim + rpyCenter,1100,1900), tnow); // roll + RC_Channels::set_override(0, constrain_int16(s + pitchTrim + rpyCenter,1100,1900), tnow); // pitch + RC_Channels::set_override(1, constrain_int16(t + rollTrim + rpyCenter,1100,1900), tnow); // roll RC_Channels::set_override(2, constrain_int16((zTot)*throttleScale+throttleBase,1100,1900), tnow); // throttle RC_Channels::set_override(3, constrain_int16(r*rpyScale+rpyCenter,1100,1900), tnow); // yaw @@ -706,6 +715,40 @@ JSButton* Sub::get_button(uint8_t index) return &g.jbtn_14; case 15: return &g.jbtn_15; + + // add 16 more cases for 32 buttons with MANUAL_CONTROL extensions + case 16: + return &g.jbtn_16; + case 17: + return &g.jbtn_17; + case 18: + return &g.jbtn_18; + case 19: + return &g.jbtn_19; + case 20: + return &g.jbtn_20; + case 21: + return &g.jbtn_21; + case 22: + return &g.jbtn_22; + case 23: + return &g.jbtn_23; + case 24: + return &g.jbtn_24; + case 25: + return &g.jbtn_25; + case 26: + return &g.jbtn_26; + case 27: + return &g.jbtn_27; + case 28: + return &g.jbtn_28; + case 29: + return &g.jbtn_29; + case 30: + return &g.jbtn_30; + case 31: + return &g.jbtn_31; default: return &g.jbtn_0; } 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/Blimp/AP_Arming.cpp b/Blimp/AP_Arming.cpp index 23ba7790f9a175..adbc072e4bf834 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) diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index 26971ef2e65392..593e9f26687f74 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -41,6 +41,7 @@ const AP_Param::Info Blimp::var_info[] = { // @DisplayName: My ground station number // @Description: Allows restricting radio overrides to only come from my ground station // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 313cec0c21dc62..ac1bdb29abdc14 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -42,6 +42,7 @@ const AP_Param::Info Rover::var_info[] = { // @DisplayName: MAVLink ground station ID // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match. // @Range: 1 255 + // @Increment: 1 // @User: Advanced GSCALAR(sysid_my_gcs, "SYSID_MYGCS", 255), @@ -222,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_ diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 035d16f4d0ffb4..b4b7b019845cd8 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 diff --git a/Rover/RC_Channel.cpp b/Rover/RC_Channel.cpp index e799f38a70fd24..9a89b66d613764 100644 --- a/Rover/RC_Channel.cpp +++ b/Rover/RC_Channel.cpp @@ -33,6 +33,7 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSw // the following functions do not need initialising: case AUX_FUNC::ACRO: case AUX_FUNC::AUTO: + case AUX_FUNC::CIRCLE: case AUX_FUNC::FOLLOW: case AUX_FUNC::GUIDED: case AUX_FUNC::HOLD: @@ -226,6 +227,10 @@ bool RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const AuxSwit do_aux_function_change_mode(rover.mode_simple, ch_flag); break; + case AUX_FUNC::CIRCLE: + do_aux_function_change_mode(rover.g2.mode_circle, ch_flag); + break; + // trigger sailboat tack case AUX_FUNC::SAILBOAT_TACK: // any switch movement interpreted as request to tack diff --git a/Rover/crash_check.cpp b/Rover/crash_check.cpp index 284c125a4b9533..feafdf7f6f6ef7 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; } 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 439f4a014b034e..5d2029b8201d25 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -82,21 +82,15 @@ void ModeAuto::update() switch (_submode) { case SubMode::WP: { - // check if we've reached the destination - if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) { - // update navigation controller + // boats loiter once the waypoint is reached + bool keep_navigating = true; + if (rover.is_boat() && g2.wp_nav.reached_destination() && !g2.wp_nav.is_fast_waypoint()) { + keep_navigating = !start_loiter(); + } + + // update navigation controller + if (keep_navigating) { navigate_to_waypoint(); - } else { - // we have reached the destination so stay here - if (rover.is_boat()) { - if (!start_loiter()) { - start_stop(); - } - } else { - start_stop(); - } - // update distance to destination - _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination()); } break; } 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/release-notes.txt b/Rover/release-notes.txt index ed572d2ea1e262..5d6c5cae88c073 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,5 +1,23 @@ Rover Release Notes: ------------------------------------------------------------------ +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 + - RADIX2HD supports external I2C compasses + - SpeedyBeeF405v4 support +2) Bug fixes + - DroneCAN battery monitor with cell monitor SoC reporting fix + - NTF_LED_TYPES parameter description fixed (was missing IS31FL3195) + - ProfiLED output fixed in both Notify and Scripting + - Scripting bug that could cause crash if parameters were added in flight + - STAT_BOOTCNT param fix (was not updating in some cases) + - don't query hobbywing DroneCAN ESC IDs while armed +3) Rover specific changes + - Auto mode keeps navigating while paused at waypoints (reduces overshoot) + - QuikTune script simplification (only tunes rate and speed controllers) + - Torqeedo battery nearly empty reporting fix +------------------------------------------------------------------ Rover 4.4.0-beta10 14-Nov-2023 Changes from 4.4.0-beta9 1) AP_GPS: correct uBlox M10 configuration on low flash boards 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/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index f42271208073ba..7e4b80cad15a4b 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -109,6 +109,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 +117,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 diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 27bfe00e6a5ad0..63f3bba28ff906 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -272,6 +272,9 @@ AP_HW_VIMDRONES_MOSAIC_X5 1405 AP_HW_VIMDRONES_MOSAIC_H 1406 AP_HW_VIMDRONES_PERIPH 1407 +AP_HW_PIXHAWK6X_PERIPH 1408 + + # IDs 5000-5099 reserved for Carbonix # IDs 5100-5199 reserved for SYPAQ Systems # IDs 5200-5209 reserved for Airvolute @@ -281,6 +284,17 @@ AP_HW_AIRVOLUTE_DCS2 5200 AP_HW_AOCODA-RC-H743DUAL 5210 AP_HW_AOCODA-RC-F405V3 5211 +#IDs 5301-5399 reserved for Sierra Aerospace +AP_HW_Sierra-TrueNavPro-G4 5301 +AP_HW_Sierra-TrueNavIC 5302 +AP_HW_Sierra-TrueNorth-G4 5303 +AP_HW_Sierra-TrueSpeed-G4 5304 +AP_HW_Sierra-PrecisionPoint-G4 5305 +AP_HW_Sierra-AeroNex 5306 +AP_HW_Sierra-TrueFlow 5307 +AP_HW_Sierra-TrueNavIC-Pro 5308 +AP_HW_Sierra-F1-Pro 5309 + # IDs 6000-6099 reserved for SpektreWorks # OpenDroneID enabled boards. Use 10000 + the base board ID diff --git a/Tools/AP_Bootloader/support.cpp b/Tools/AP_Bootloader/support.cpp index ddf1041da48e37..c9f37b2b60e2e4 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) diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 4d5cfdcbea1558..41bf37b92c9d20 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -100,16 +100,16 @@ void AP_Periph_FW::init() can_start(); -#ifdef HAL_PERIPH_ENABLE_NETWORKING - networking_periph.init(); -#endif - #if HAL_GCS_ENABLED stm32_watchdog_pat(); gcs().init(); #endif serial_manager.init(); +#ifdef HAL_PERIPH_ENABLE_NETWORKING + networking_periph.init(); +#endif + #if HAL_GCS_ENABLED gcs().setup_console(); gcs().setup_uarts(); @@ -140,6 +140,10 @@ void AP_Periph_FW::init() node_stats.init(); #endif +#ifdef HAL_PERIPH_ENABLE_SERIAL_OPTIONS + serial_options.init(); +#endif + #ifdef HAL_PERIPH_ENABLE_GPS if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE && g.gps_port >= 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 diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 376e7245ba0e0d..d0e0c279be8ffc 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -26,17 +26,34 @@ #include #include #include -#include #include #include #include #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 #include #if HAL_NMEA_OUTPUT_ENABLED && !(HAL_GCS_ENABLED && defined(HAL_PERIPH_ENABLE_GPS)) @@ -77,6 +94,15 @@ #define HAL_PERIPH_CAN_MIRROR 0 #endif +#if defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT) && !defined(HAL_DEBUG_BUILD) && !defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG) +/* this checking for reboot can lose bytes on GPS modules and other + * serial devices. It is really only relevent on a debug build if you + * really want it for non-debug build then define + * HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG in hwdef.dat + */ +#undef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT +#endif + #include "Parameters.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -323,6 +349,10 @@ 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; @@ -367,6 +397,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}; @@ -416,8 +451,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, @@ -458,6 +501,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); @@ -465,7 +509,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/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 27c985a3bf6218..d425ad6c7c2525 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,12 @@ 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 + AP_VAREND }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index ff64f16d1e9508..884f1c4e64baa0 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -88,6 +88,8 @@ class Parameters { k_param_can_terminate0, k_param_can_terminate1, k_param_can_terminate2, + k_param_serial_options, + k_param_relay, }; AP_Int16 format_version; diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 89611c5cac5477..9000ab4e33431a 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -62,6 +62,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 @@ -207,25 +210,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 +306,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,22 +355,12 @@ 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) @@ -419,22 +388,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 +848,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 +963,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 +986,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 +1029,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 +1042,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 +1172,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 +1237,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 +1258,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 +1279,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); @@ -1351,14 +1374,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 +1513,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 +1546,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; 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..41fb6ee0426036 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 diff --git a/Tools/AP_Periph/networking.h b/Tools/AP_Periph/networking.h index d25dc5aa7aaa77..71cdc1754ea3af 100644 --- a/Tools/AP_Periph/networking.h +++ b/Tools/AP_Periph/networking.h @@ -1,13 +1,23 @@ #pragma once -#include "AP_Periph.h" +#include #ifdef HAL_PERIPH_ENABLE_NETWORKING +#include + #ifndef HAL_PERIPH_NETWORK_NUM_PASSTHRU #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() { @@ -52,7 +62,11 @@ class Networking_Periph { #endif // HAL_PERIPH_NETWORK_NUM_PASSTHRU AP_Networking networking_lib; -}; -#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/networking_passthru.cpp b/Tools/AP_Periph/networking_passthru.cpp index 18ed58bf68bac5..3d2f65a1cad7b7 100644 --- a/Tools/AP_Periph/networking_passthru.cpp +++ b/Tools/AP_Periph/networking_passthru.cpp @@ -13,7 +13,7 @@ along with this program. If not, see . */ -#include "networking.h" +#include "AP_Periph.h" #if defined(HAL_PERIPH_ENABLE_NETWORKING) && HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0 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/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 8f238ce14b0d50..a3b92b0150a111 100644 --- a/Tools/AP_Periph/wscript +++ b/Tools/AP_Periph/wscript @@ -7,7 +7,7 @@ import sys try: import em except ImportError: - print("you need to install empy with 'python -m pip install empy'") + print("you need to install empy with 'python -m pip install empy==3.3.4'") sys.exit(1) try: @@ -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/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/QuadPlanes/XPlane-Alia.parm b/Tools/Frame_params/QuadPlanes/XPlane-Alia.parm new file mode 100644 index 00000000000000..b9e54aae830ea2 --- /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 +ARSPD_FBW_MIN,30 +ARSPD_FBW_MAX,65 +TERRAIN_FOLLOW,1 +THR_MIN,-25 +THR_SLEWRATE,30 +TRIM_THROTTLE,60 +GROUND_STEER_ALT,10 +TRIM_ARSPD_CM,5200 +SCALING_SPEED,60 +TRIM_PITCH_CD,400 +ALT_HOLD_RTL,25000 +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_VELZ_MAX,500 +Q_WP_SPEED,1500 +Q_WP_SPEED_DN,500 +Q_WP_ACCEL,200 +Q_ASSIST_SPEED,25 +Q_LAND_SPEED,100 +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/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/boards.py b/Tools/ardupilotwaf/boards.py index 776a1cd9223724..9330bb6e02804f 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: @@ -462,8 +467,8 @@ def configure_env(self, cfg, env): # We always want to use PRI format macros cfg.define('__STDC_FORMAT_MACROS', 1) - if cfg.options.disable_ekf2: - env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] + if cfg.options.enable_ekf2: + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] if cfg.options.disable_ekf3: env.CXXFLAGS += ['-DHAL_NAVEKF3_AVAILABLE=0'] @@ -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) @@ -654,6 +661,12 @@ def configure_env(self, cfg, env): cfg.define('AP_NOTIFY_LP5562_BUS', 2) cfg.define('AP_NOTIFY_LP5562_ADDR', 0x30) + try: + env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=0') + except ValueError: + pass + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=1'] + if self.with_can: cfg.define('HAL_NUM_CAN_IFACES', 2) env.DEFINES.update(CANARD_MULTI_IFACE=1, @@ -674,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: @@ -793,6 +807,7 @@ def configure_env(self, cfg, env): # whitelist of compilers which we should build with -Werror gcc_whitelist = frozenset([ ('11','3','0'), + ('11','4','0'), ('12','1','0'), ]) @@ -840,6 +855,7 @@ def configure_env(self, cfg, env): HAL_PERIPH_ENABLE_RPM = 1, HAL_PERIPH_ENABLE_RC_OUT = 1, HAL_PERIPH_ENABLE_ADSB = 1, + HAL_PERIPH_ENABLE_SERIAL_OPTIONS = 1, AP_ICENGINE_ENABLED = 0, AP_AIRSPEED_ENABLED = 1, AP_AIRSPEED_AUTOCAL_ENABLE = 0, @@ -871,18 +887,24 @@ def configure_env(self, cfg, env): AP_CAN_SLCAN_ENABLED = 0, HAL_PROXIMITY_ENABLED = 0, AP_SCRIPTING_ENABLED = 0, - HAL_NAVEKF2_AVAILABLE = 0, HAL_NAVEKF3_AVAILABLE = 0, HAL_PWM_COUNT = 32, HAL_WITH_ESC_TELEM = 1, AP_RTC_ENABLED = 0, ) + try: + env.CXXFLAGS.remove('-DHAL_NAVEKF2_AVAILABLE=1') + except ValueError: + pass + env.CXXFLAGS += ['-DHAL_NAVEKF2_AVAILABLE=0'] 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() @@ -895,7 +917,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 @@ -907,6 +928,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', ] @@ -914,6 +944,7 @@ def expand_path(p): env.CFLAGS += [ '-fno-inline-functions', '-mlongcalls', + '-fsingle-precision-constant', ] env.CFLAGS.remove('-Werror=undef') @@ -929,6 +960,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') @@ -974,6 +1006,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', @@ -1138,7 +1171,8 @@ def configure_env(self, cfg, env): ] env.INCLUDES += [ - cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath() + cfg.srcnode.find_dir('libraries/AP_GyroFFT/CMSIS_5/include').abspath(), + cfg.srcnode.find_dir('modules/lwip/src/include/compat/posix').abspath() ] # whitelist of compilers which we should build with -Werror @@ -1149,6 +1183,7 @@ def configure_env(self, cfg, env): ('9','3','1'), ('10','2','1'), ('11','3','0'), + ('11','4','0'), ]) if cfg.env.HAL_CANFD_SUPPORTED: @@ -1213,6 +1248,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', @@ -1515,6 +1552,19 @@ def configure_env(self, cfg, env): CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_OBAL_V1', ) +class canzero(linux): + toolchain = 'arm-linux-gnueabihf' + + def __init__(self): + self.with_can = True + + def configure_env(self, cfg, env): + super(canzero, self).configure_env(cfg, env) + + env.DEFINES.update( + CONFIG_HAL_BOARD_SUBTYPE = 'HAL_BOARD_SUBTYPE_LINUX_CANZERO', + ) + class SITL_static(sitl): def configure_env(self, cfg, env): super(SITL_static, self).configure_env(cfg, env) diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 819b3474c540f1..b84d6e89514e4b 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -114,7 +114,7 @@ def get_full_wsl2_error_msg(self, error_msg): and make sure to add it to your path during the installation. Once installed, run this command in Powershell or Command Prompt to install some packages: - pip.exe install empy pyserial + pip.exe install empy==3.3.4 pyserial **************************************** **************************************** """ % error_msg) @@ -468,6 +468,20 @@ def setup_canmgr_build(cfg): 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', + ] + + 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 load_env_vars(env): '''optionally load extra environment variables from env.py in the build directory''' print("Checking for env.py") @@ -578,6 +592,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) 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/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt b/Tools/autotest/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt index f4ef0ef2a1f1ff..846be46dd5c002 100644 --- a/Tools/autotest/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt +++ b/Tools/autotest/ArduCopter_Tests/DO_CHANGE_SPEED/mission.txt @@ -1,11 +1,12 @@ -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 0.000000 0.000000 20.000000 1 -2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.362656 149.165072 20.000000 1 -3 0 0 178 1.000000 15.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.360174 149.167908 14.000000 1 -5 0 0 178 1.000000 10.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -6 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363296 149.169244 18.000000 1 -7 0 0 178 1.000000 16.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 -8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364115 149.165747 20.000000 1 -9 0 0 20 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1 +QGC WPL 110 +0 1 0 16 0 0 0 0 -35.3632620 149.1652370 584.090000 1 +1 0 3 22 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 20.000000 1 +2 0 3 178 0.00000000 4.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +3 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36265600 149.16507200 20.000000 1 +4 0 0 178 1.00000000 15.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +5 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36017400 149.16790800 14.000000 1 +6 0 0 178 1.00000000 10.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +7 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36329600 149.16924400 18.000000 1 +8 0 0 178 1.00000000 16.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 +9 0 3 16 0.00000000 0.00000000 0.00000000 0.00000000 -35.36411500 149.16574700 20.000000 1 +10 0 0 20 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.00000000 0.000000 1 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/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/ap1.txt new file mode 100644 index 00000000000000..ab2be0d1a45470 --- /dev/null +++ b/Tools/autotest/ArduPlane_Tests/MicroStrainEAHRS7/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 c7898e2b755353..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) @@ -7678,6 +7748,12 @@ def RichenPower(self): raise NotAchievedException("Did not find expected GEN message") def IE24(self): + '''Test IntelligentEnergy 2.4kWh generator with V1 and V2 telemetry protocols''' + protocol_ver = (1, 2) + for ver in protocol_ver: + self.run_IE24(ver) + + def run_IE24(self, proto_ver): '''Test IntelligentEnergy 2.4kWh generator''' elec_battery_instance = 2 fuel_battery_instance = 1 @@ -7687,14 +7763,14 @@ def IE24(self): "GEN_TYPE": 2, "BATT%u_MONITOR" % (fuel_battery_instance + 1): 18, # fuel-based generator "BATT%u_MONITOR" % (elec_battery_instance + 1): 17, - "SIM_IE24_ENABLE": 1, + "SIM_IE24_ENABLE": proto_ver, "LOG_DISARMED": 1, }) - self.customise_SITL_commandline(["--uartF=sim:ie24"]) + self.customise_SITL_commandline(["--serial5=sim:ie24"]) - self.start_subtest("ensure that BATTERY_STATUS for electrical generator message looks right") - self.start_subsubtest("Checking original voltage (electrical)") + 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) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: self.delay_sim_time(30) @@ -7712,13 +7788,13 @@ def IE24(self): "battery_remaining": original_elec_m.battery_remaining - 1, }, instance=elec_battery_instance) - self.start_subtest("ensure that BATTERY_STATUS for fuel generator message looks right") - self.start_subsubtest("Checking original voltage (fuel)") + self.start_subtest("Protocol %i: ensure that BATTERY_STATUS for fuel generator message looks right" % proto_ver) + self.start_subsubtest("Protocol %i: Checking original voltage (fuel)" % proto_ver) # ArduPilot spits out essentially uninitialised battery # messages until we read things fromthe battery: if original_fuel_m.battery_remaining <= 90: raise NotAchievedException("Bad original percentage (want=>%f got %f" % (90, original_fuel_m.battery_remaining)) - self.start_subsubtest("Ensure percentage is counting down") + self.start_subsubtest("Protocol %i: Ensure percentage is counting down" % proto_ver) self.wait_message_field_values('BATTERY_STATUS', { "battery_remaining": original_fuel_m.battery_remaining - 1, }, instance=fuel_battery_instance) @@ -7728,7 +7804,7 @@ def IE24(self): self.disarm_vehicle() # Test for pre-arm check fail when state is not running - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without takeoff generator error should cause failsafe and disarm" % proto_ver) self.set_parameter("SIM_IE24_STATE", 8) self.wait_statustext("Status not running", timeout=40) self.try_arm(result=False, @@ -7736,7 +7812,7 @@ def IE24(self): self.set_parameter("SIM_IE24_STATE", 2) # Explicitly set state to running # Test that error code does result in failsafe - self.start_subtest("If you haven't taken off generator error should cause instant failsafe and disarm") + self.start_subtest("Protocol %i: Without taken off generator error should cause failsafe and disarm" % proto_ver) self.change_mode("STABILIZE") self.set_parameter("DISARM_DELAY", 0) self.arm_vehicle() @@ -8176,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() @@ -8323,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, @@ -8374,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) @@ -8828,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''' @@ -9499,14 +9574,21 @@ def DO_CHANGE_SPEED(self): self.wait_ready_to_arm() self.arm_vehicle() - self.wait_current_waypoint(2) + self.wait_current_waypoint(1) + self.wait_groundspeed( + 3.5, 4.5, + minimum_duration=5, + timeout=60, + ) + + self.wait_current_waypoint(3) self.wait_groundspeed( 14.5, 15.5, minimum_duration=10, timeout=60, ) - self.wait_current_waypoint(4) + self.wait_current_waypoint(5) self.wait_groundspeed( 9.5, 11.5, minimum_duration=10, @@ -9514,7 +9596,7 @@ def DO_CHANGE_SPEED(self): ) self.set_parameter("ANGLE_MAX", 6000) - self.wait_current_waypoint(6) + self.wait_current_waypoint(7) self.wait_groundspeed( 15.5, 16.5, minimum_duration=10, @@ -9763,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() @@ -10055,6 +10137,7 @@ def tests1d(self): self.ModeCircle, self.MagFail, self.OpticalFlow, + self.OpticalFlowLocation, self.OpticalFlowLimits, self.OpticalFlowCalibration, self.MotorFail, @@ -10238,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 31a127f3e01170..be66a0447821f3 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -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: @@ -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, @@ -3275,6 +3278,14 @@ def MicroStrainEAHRS5(self): '''Test MicroStrain EAHRS series 5 support''' self.fly_external_AHRS("MicroStrain5", 2, "ap1.txt") + 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 @@ -4210,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() @@ -4511,7 +4522,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() @@ -5353,6 +5364,8 @@ def tests(self): self.TerrainLoiter, self.VectorNavEAHRS, self.MicroStrainEAHRS5, + self.MicroStrainEAHRS7, + self.InertialLabsEAHRS, self.Deadreckoning, self.DeadreckoningNoAirSpeed, self.EKFlaneswitch, diff --git a/Tools/autotest/ardusub.py b/Tools/autotest/ardusub.py index d1fc238c8263b9..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() @@ -317,12 +355,14 @@ def DiveMission(self): def GripperMission(self): '''Test gripper mission items''' self.load_mission("sub-gripper-mission.txt") - self.change_mode('GUIDED') self.wait_ready_to_arm() self.arm_vehicle() self.change_mode('AUTO') + self.wait_waypoint(1, 2, max_dist=5) self.wait_statustext("Gripper Grabbed", timeout=60) + self.wait_waypoint(1, 4, max_dist=5) self.wait_statustext("Gripper Released", timeout=60) + self.wait_waypoint(1, 6, max_dist=5) self.disarm_vehicle() def SET_POSITION_TARGET_GLOBAL_INT(self): @@ -522,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/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..6e5143632d452c 100644 --- a/Tools/autotest/default_params/vee-gull 005.param +++ b/Tools/autotest/default_params/vee-gull 005.param @@ -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 diff --git a/Tools/autotest/logger_metadata/enum_parse.py b/Tools/autotest/logger_metadata/enum_parse.py index 50c026a21651f3..3826160de20048 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): diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index 7f5f9c21202dee..fdb1465e29cc4d 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -109,11 +109,12 @@ def lua_applets(): libraries = [] -# AP_Vehicle also has parameters rooted at "", but isn't referenced -# from the vehicle in any way: -ap_vehicle_lib = Library("", reference="VEHICLE") # the "" is tacked onto the front of param name -setattr(ap_vehicle_lib, "Path", os.path.join('..', 'libraries', 'AP_Vehicle', 'AP_Vehicle.cpp')) -libraries.append(ap_vehicle_lib) +if args.vehicle != "AP_Periph": + # AP_Vehicle also has parameters rooted at "", but isn't referenced + # from the vehicle in any way: + ap_vehicle_lib = Library("", reference="VEHICLE") # the "" is tacked onto the front of param name + setattr(ap_vehicle_lib, "Path", os.path.join('..', 'libraries', 'AP_Vehicle', 'AP_Vehicle.cpp')) + libraries.append(ap_vehicle_lib) libraries.append(lua_applets()) diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index beed41e777c75c..6a0f6c9dff7ebd 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -539,9 +539,9 @@ def start_SITL(binary, if unhide_parameters: cmd.extend(['--unhide-groups']) # somewhere for MAVProxy to connect to: - cmd.append('--uartC=tcp:2') - if not enable_fgview_output: - cmd.append("--disable-fgview") + cmd.append('--serial1=tcp:2') + if enable_fgview_output: + cmd.append("--enable-fgview") if defaults_filepath is not None: if isinstance(defaults_filepath, list): @@ -675,6 +675,20 @@ def start_MAVProxy_SITL(atype, return ret +def start_PPP_daemon(ips, sockaddr): + """Start pppd for networking""" + + global close_list + cmd = "sudo pppd socket %s debug noauth nodetach %s" % (sockaddr, ips) + cmd = cmd.split() + print("Running: %s" % cmd_as_shell(cmd)) + + ret = pexpect.spawn(cmd[0], cmd[1:], logfile=sys.stdout, encoding=ENCODING, timeout=30) + ret.delaybeforesend = 0 + pexpect_autoclose(ret) + return ret + + def expect_setup_callback(e, callback): """Setup a callback that is called once a second while waiting for patterns.""" diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index d2e61b4f07ce0e..ebb0c2f9cf8592 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -1513,6 +1513,41 @@ def Q_GUIDED_MODE(self): self.fly_home_land_and_disarm() + def DCMClimbRate(self): + '''Test the climb rate measurement in DCM with and without GPS''' + self.wait_ready_to_arm() + + self.change_mode('QHOVER') + self.arm_vehicle() + self.set_rc(3, 2000) + self.wait_altitude(30, 50, relative=True) + + # Start Descending + self.set_rc(3, 1000) + self.wait_climbrate(-5, -0.5, timeout=10) + + # Switch to DCM + self.set_parameter('AHRS_EKF_TYPE', 0) + self.delay_sim_time(5) + + # Start Climbing + self.set_rc(3, 2000) + self.wait_climbrate(0.5, 5, timeout=10) + + # Kill any GPSs + self.set_parameters({ + 'SIM_GPS_DISABLE': 1, + 'SIM_GPS2_DISABLE': 1, + }) + self.delay_sim_time(5) + + # Start Descending + self.set_rc(3, 1000) + self.wait_climbrate(-5, -0.5, timeout=10) + + # Force disarm + self.disarm_vehicle(force=True) + def tests(self): '''return list of all tests''' @@ -1554,5 +1589,6 @@ def tests(self): self.mavlink_MAV_CMD_DO_VTOL_TRANSITION, self.MAV_CMD_NAV_TAKEOFF, self.Q_GUIDED_MODE, + self.DCMClimbRate, ]) return ret diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index c82dca3c0210e6..8fbf383e10ee41 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -568,6 +568,13 @@ def ServoRelayEvents(self): '''Test ServoRelayEvents''' for method in self.run_cmd, self.run_cmd_int: self.context_push() + + self.set_parameters({ + "RELAY1_FUNCTION": 1, # Enable relay 1 as a standard relay pin + "RELAY2_FUNCTION": 1, # Enable relay 2 as a standard relay pin + }) + self.reboot_sitl() # Needed for relay functions to take effect + method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=0) off = self.get_parameter("SIM_PIN_MASK") method(mavutil.mavlink.MAV_CMD_DO_SET_RELAY, p1=0, p2=1) @@ -602,8 +609,14 @@ def ServoRelayEvents(self): "on": 0, }) - # add another servo: - self.set_parameter("RELAY_PIN6", 14) + # add another relay and ensure that it changes the "present field" + self.set_parameters({ + "RELAY6_FUNCTION": 1, # Enable relay 6 as a standard relay pin + "RELAY6_PIN": 14, # Set pin number + }) + self.reboot_sitl() # Needed for relay function to take effect + self.set_message_rate_hz("RELAY_STATUS", 10) # Need to re-request the message since reboot + self.assert_received_message_field_values('RELAY_STATUS', { "present": 35, "on": 0, @@ -5269,21 +5282,6 @@ def test_scripting_internal_test(self): self.context_push() - test_scripts = ["scripting_test.lua", "math.lua", "strings.lua", "mavlink_test.lua"] - success_text = ["Internal tests passed", "Math tests passed", "String tests passed", "Received heartbeat from"] - named_value_float_types = ["test"] - - messages = [] - named_value_float = [] - - def my_message_hook(mav, message): - if message.get_type() == 'STATUSTEXT': - messages.append(message) - # also sniff for named value float messages - if message.get_type() == 'NAMED_VALUE_FLOAT': - named_value_float.append(message) - - self.install_message_hook_context(my_message_hook) self.set_parameters({ "SCR_ENABLE": 1, "SCR_HEAP_SIZE": 1024000, @@ -5291,39 +5289,37 @@ def my_message_hook(mav, message): }) self.install_test_modules_context() self.install_mavlink_module_context() - for script in test_scripts: + for script in [ + "scripting_test.lua", + "math.lua", + "strings.lua", + "mavlink_test.lua", + ]: self.install_test_script_context(script) + + self.context_collect('STATUSTEXT') + self.context_collect('NAMED_VALUE_FLOAT') + self.reboot_sitl() - self.delay_sim_time(10) + for success_text in [ + "Internal tests passed", + "Math tests passed", + "String tests passed", + "Received heartbeat from" + ]: + self.wait_statustext(success_text, check_context=True) + + for success_nvf in [ + "test", + ]: + self.assert_received_message_field_values("NAMED_VALUE_FLOAT", { + "name": success_nvf, + }, check_context=True) self.context_pop() self.reboot_sitl() - # check all messages to see if we got our message - success = True - for text in success_text: - script_success = False - for m in messages: - if text in m.text: - script_success = True - success = script_success and success - if not success: - raise NotAchievedException("Failed to receive STATUS_TEXT") - else: - self.progress("Success STATUS_TEXT") - - for type in named_value_float_types: - script_success = False - for m in named_value_float: - if type == m.name: - script_success = True - success = script_success and success - if not success: - raise NotAchievedException("Failed to receive NAMED_VALUE_FLOAT") - else: - self.progress("Success NAMED_VALUE_FLOAT") - def test_scripting_hello_world(self): self.start_subtest("Scripting hello world") @@ -5369,7 +5365,8 @@ def test_scripting_auxfunc(self): self.context_collect("STATUSTEXT") self.set_parameters({ "SCR_ENABLE": 1, - "RELAY_PIN": 1, + "RELAY1_FUNCTION": 1, + "RELAY1_PIN": 1 }) self.install_example_script_context("RCIN_test.lua") self.reboot_sitl() @@ -6131,14 +6128,14 @@ def DepthFinder(self): '''Test mulitple depthfinders for boats''' # Setup rangefinders self.customise_SITL_commandline([ - "--uartH=sim:nmea", # NMEA Rangefinder + "--serial7=sim:nmea", # NMEA Rangefinder ]) # RANGEFINDER_INSTANCES = [0, 2, 5] self.set_parameters({ "RNGFND1_TYPE" : 17, # NMEA must attach uart to SITL "RNGFND1_ORIENT" : 25, # Set to downward facing - "SERIAL7_PROTOCOL" : 9, # Rangefinder on uartH + "SERIAL7_PROTOCOL" : 9, # Rangefinder on serial7 "SERIAL7_BAUD" : 9600, # Rangefinder specific baudrate "RNGFND3_TYPE" : 2, # MaxbotixI2C @@ -6628,6 +6625,108 @@ def MAV_CMD_BATTERY_RESET(self): "poll": True, }) + def TestWebServer(self, url): + '''test active web server''' + self.progress("Accessing webserver main page") + import urllib.request + + main_page = urllib.request.urlopen(url).read().decode('utf-8') + if main_page.find('ArduPilot Web Server') == -1: + raise NotAchievedException("Expected banner on main page") + + board_status = urllib.request.urlopen(url + '/@DYNAMIC/board_status.shtml').read().decode('utf-8') + if board_status.find('0 hours') == -1: + raise NotAchievedException("Expected uptime in board status") + if board_status.find('40.713') == -1: + raise NotAchievedException("Expected lattitude in board status") + + self.progress("WebServer tests OK") + + def NetworkingWebServer(self): + '''web server''' + applet_script = "net_webserver.lua" + + self.context_push() + self.install_applet_script_context(applet_script) + + self.set_parameters({ + "SCR_ENABLE": 1, + "SCR_VM_I_COUNT": 1000000, + "SIM_SPEEDUP": 20, + "NET_ENABLED": 1, + }) + + self.reboot_sitl() + + self.context_push() + self.context_collect('STATUSTEXT') + + self.set_parameters({ + "WEB_BIND_PORT": 8081, + }) + + self.scripting_restart() + self.wait_text("WebServer: starting on port 8081", check_context=True) + + self.wait_ready_to_arm() + + self.TestWebServer("http://127.0.0.1:8081") + + self.context_pop() + self.context_pop() + self.reboot_sitl() + + def NetworkingWebServerPPP(self): + '''web server over PPP''' + applet_script = "net_webserver.lua" + + self.context_push() + self.install_applet_script_context(applet_script) + + self.set_parameters({ + "SCR_ENABLE": 1, + "SCR_VM_I_COUNT": 1000000, + "SIM_SPEEDUP": 20, + "NET_ENABLED": 1, + "SERIAL5_PROTOCOL": 48, + }) + + self.progress('rebuilding rover with ppp enabled') + import shutil + shutil.copy('build/sitl/bin/ardurover', 'build/sitl/bin/ardurover.noppp') + util.build_SITL('bin/ardurover', clean=False, configure=True, extra_configure_args=['--enable-ppp', '--debug']) + + self.reboot_sitl() + + self.progress("Starting PPP daemon") + pppd = util.start_PPP_daemon("192.168.14.15:192.168.14.13", '127.0.0.1:5765') + + self.context_push() + self.context_collect('STATUSTEXT') + + pppd.expect("remote IP address 192.168.14.13") + + self.progress("PPP daemon started") + + self.set_parameters({ + "WEB_BIND_PORT": 8081, + }) + + self.scripting_restart() + self.wait_text("WebServer: starting on port 8081", check_context=True) + + self.wait_ready_to_arm() + + self.TestWebServer("http://192.168.14.13:8081") + + self.context_pop() + self.context_pop() + + # restore rover without ppp enabled for next test + os.unlink('build/sitl/bin/ardurover') + shutil.copy('build/sitl/bin/ardurover.noppp', 'build/sitl/bin/ardurover') + self.reboot_sitl() + def tests(self): '''return list of all tests''' ret = super(AutoTestRover, self).tests() @@ -6712,6 +6811,8 @@ def tests(self): self.MAV_CMD_GET_HOME_POSITION, self.MAV_CMD_DO_FENCE_ENABLE, self.MAV_CMD_BATTERY_RESET, + self.NetworkingWebServer, + self.NetworkingWebServerPPP, ]) return ret diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 89113e04108a34..1b46b85a87e31d 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -378,8 +378,8 @@ def do_build(opts, frame_options): if opts.math_check_indexes: cmd_configure.append("--enable-math-check-indexes") - if opts.disable_ekf2: - cmd_configure.append("--disable-ekf2") + if opts.enable_ekf2: + cmd_configure.append("--enable-ekf2") if opts.disable_ekf3: cmd_configure.append("--disable-ekf3") @@ -414,6 +414,9 @@ def do_build(opts, frame_options): if opts.disable_networking: cmd_configure.append("--disable-networking") + if opts.enable_ppp: + cmd_configure.append("--enable-ppp") + if opts.enable_networking_tests: cmd_configure.append("--enable-networking-tests") @@ -635,13 +638,13 @@ def run_in_terminal_window(name, cmd, **kw): subprocess.Popen(runme, **kw) -tracker_uarta = None # blemish +tracker_serial0 = None # blemish def start_antenna_tracker(opts): """Compile and run the AntennaTracker, add tracker to mavproxy""" - global tracker_uarta + global tracker_serial0 progress("Preparing antenna tracker") tracker_home = find_location_by_name(opts.tracker_location) vehicledir = os.path.join(autotest_dir, "../../" + "AntennaTracker") @@ -652,7 +655,7 @@ def start_antenna_tracker(opts): tracker_instance = 1 oldpwd = os.getcwd() os.chdir(vehicledir) - tracker_uarta = "tcp:127.0.0.1:" + str(5760 + 10 * tracker_instance) + tracker_serial0 = "tcp:127.0.0.1:" + str(5760 + 10 * tracker_instance) binary_basedir = "build/sitl" exe = os.path.join(root_dir, binary_basedir, @@ -669,7 +672,6 @@ def start_antenna_tracker(opts): def start_CAN_Periph(opts, frame_info): """Compile and run the sitl_periph""" - global can_uarta progress("Preparing sitl_periph_gps") options = vinfo.options["sitl_periph_gps"]['frames']['gps'] defaults_path = frame_info.get('periph_params_filename', None) @@ -838,9 +840,9 @@ def start_vehicle(binary, opts, stuff, spawns=None): if spawns is not None: c.extend(["--home", spawns[i]]) if opts.mcast: - c.extend(["--uartA", "mcast:"]) + c.extend(["--serial0", "mcast:"]) elif opts.udp: - c.extend(["--uartA", "udpclient:127.0.0.1:" + str(5760+i*10)]) + c.extend(["--serial0", "udpclient:127.0.0.1:" + str(5760+i*10)]) if opts.auto_sysid: if opts.sysid is not None: raise ValueError("Can't use auto-sysid and sysid together") @@ -901,12 +903,12 @@ def start_mavproxy(opts, stuff): if opts.tracker: cmd.extend(["--load-module", "tracker"]) - global tracker_uarta - # tracker_uarta is set when we start the tracker... + global tracker_serial0 + # tracker_serial0 is set when we start the tracker... extra_cmd += ("module load map;" "tracker set port %s; " "tracker start; " - "tracker arm;" % (tracker_uarta,)) + "tracker arm;" % (tracker_serial0,)) if opts.mavlink_gimbal: cmd.extend(["--load-module", "gimbal"]) @@ -1295,7 +1297,7 @@ def generate_frame_help(): group_sim.add_option("--fram-storage", action='store_true', help="use fram storage emulation") -group_sim.add_option("--disable-ekf2", +group_sim.add_option("--enable-ekf2", action='store_true', help="disable EKF2 in build") group_sim.add_option("--disable-ekf3", @@ -1334,6 +1336,8 @@ def generate_frame_help(): help="Enable the dds client to connect with ROS2/DDS") group_sim.add_option("--disable-networking", action='store_true', help="Disable networking APIs") +group_sim.add_option("--enable-ppp", action='store_true', + help="Enable PPP networking") group_sim.add_option("--enable-networking-tests", action='store_true', help="Enable networking tests") diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index c44aef485f5842..5b7e4f54b7ecdc 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/Tools/autotest/vehicle_test_suite.py @@ -2942,7 +2942,7 @@ def customise_SITL_commandline(self, wipe=False, set_streamrate_callback=None, binary=None): - '''customisations could be "--uartF=sim:nmea" ''' + '''customisations could be "--serial5=sim:nmea" ''' self.contexts[-1].sitl_commandline_customised = True self.mav.close() self.stop_SITL() @@ -3972,15 +3972,19 @@ def assert_reach_imu_temperature(self, target, timeout): if not temp_ok: raise NotAchievedException("target temperature") + def message_has_field_values_field_values_equal(self, fieldname, value, got, epsilon=None): + if isinstance(value, float): + if math.isnan(value) or math.isnan(got): + return math.isnan(value) and math.isnan(got) + + if type(value) is not str and epsilon is not None: + return abs(got - value) <= epsilon + + return got == value + def message_has_field_values(self, m, fieldvalues, verbose=True, epsilon=None): for (fieldname, value) in fieldvalues.items(): got = getattr(m, fieldname) - if math.isnan(value) or math.isnan(got): - equal = math.isnan(value) and math.isnan(got) - elif epsilon is not None: - equal = abs(got - value) <= epsilon - else: - equal = got == value value_string = value got_string = got @@ -3994,7 +3998,9 @@ def message_has_field_values(self, m, fieldvalues, verbose=True, epsilon=None): value_string = "%s (%s)" % (value, enum[value].name) got_string = "%s (%s)" % (got, enum[got].name) - if not equal: + if not self.message_has_field_values_field_values_equal( + fieldname, value, got, epsilon=epsilon + ): # see if this is an enumerated field: self.progress(self.dump_message_verbose(m)) self.progress("Expected %s.%s to be %s, got %s" % @@ -4267,28 +4273,104 @@ def TestLogDownloadMAVProxyNetwork(self, upload_logs=False): self.context_push() self.set_parameters({ "NET_ENABLED": 1, - "NET_DHCP": 0, + "LOG_DISARMED": 1, + "LOG_DARM_RATEMAX": 1, # make small logs + # UDP client "NET_P1_TYPE": 1, "NET_P1_PROTOCOL": 2, - "NET_P1_PORT": 15004, + "NET_P1_PORT": 16001, "NET_P1_IP0": 127, "NET_P1_IP1": 0, "NET_P1_IP2": 0, - "NET_P1_IP3": 1 + "NET_P1_IP3": 1, + # UDP server + "NET_P2_TYPE": 2, + "NET_P2_PROTOCOL": 2, + "NET_P2_PORT": 16002, + "NET_P2_IP0": 0, + "NET_P2_IP1": 0, + "NET_P2_IP2": 0, + "NET_P2_IP3": 0, + # TCP client + "NET_P3_TYPE": 3, + "NET_P3_PROTOCOL": 2, + "NET_P3_PORT": 16003, + "NET_P3_IP0": 127, + "NET_P3_IP1": 0, + "NET_P3_IP2": 0, + "NET_P3_IP3": 1, + # TCP server + "NET_P4_TYPE": 4, + "NET_P4_PROTOCOL": 2, + "NET_P4_PORT": 16004, + "NET_P4_IP0": 0, + "NET_P4_IP1": 0, + "NET_P4_IP2": 0, + "NET_P4_IP3": 0, }) self.reboot_sitl() - filename = "MAVProxy-downloaded-net-log.BIN" - mavproxy = self.start_mavproxy(master=':15004') - self.mavproxy_load_module(mavproxy, 'log') - mavproxy.send("log list\n") - mavproxy.expect("numLogs") - self.wait_heartbeat() - self.wait_heartbeat() - mavproxy.send("set shownoise 0\n") - mavproxy.send("log download latest %s\n" % filename) - mavproxy.expect("Finished downloading", timeout=120) - self.mavproxy_unload_module(mavproxy, 'log') - self.stop_mavproxy(mavproxy) + endpoints = [('UDPClient', ':16001') , + ('UDPServer', 'udpout:127.0.0.1:16002'), + ('TCPClient', 'tcpin:0.0.0.0:16003'), + ('TCPServer', 'tcp:127.0.0.1:16004')] + for name, e in endpoints: + self.progress("Downloading log with %s %s" % (name, e)) + filename = "MAVProxy-downloaded-net-log-%s.BIN" % name + + mavproxy = self.start_mavproxy(master=e, options=['--source-system=123']) + self.mavproxy_load_module(mavproxy, 'log') + self.wait_heartbeat() + mavproxy.send("log list\n") + mavproxy.expect("numLogs") + # ensure the full list of logs has come out + for i in range(5): + self.wait_heartbeat() + mavproxy.send("log download latest %s\n" % filename) + mavproxy.expect("Finished downloading", timeout=120) + self.mavproxy_unload_module(mavproxy, 'log') + self.stop_mavproxy(mavproxy) + + self.set_parameters({ + # multicast UDP client + "NET_P1_TYPE": 1, + "NET_P1_PROTOCOL": 2, + "NET_P1_PORT": 16005, + "NET_P1_IP0": 239, + "NET_P1_IP1": 255, + "NET_P1_IP2": 145, + "NET_P1_IP3": 50, + # Broadcast UDP client + "NET_P2_TYPE": 1, + "NET_P2_PROTOCOL": 2, + "NET_P2_PORT": 16006, + "NET_P2_IP0": 255, + "NET_P2_IP1": 255, + "NET_P2_IP2": 255, + "NET_P2_IP3": 255, + "NET_P3_TYPE": -1, + "NET_P4_TYPE": -1, + "LOG_DISARMED": 0, + }) + self.reboot_sitl() + endpoints = [('UDPMulticast', 'mcast:16005') , + ('UDPBroadcast', ':16006')] + for name, e in endpoints: + self.progress("Downloading log with %s %s" % (name, e)) + filename = "MAVProxy-downloaded-net-log-%s.BIN" % name + + mavproxy = self.start_mavproxy(master=e, options=['--source-system=123']) + self.mavproxy_load_module(mavproxy, 'log') + self.wait_heartbeat() + mavproxy.send("log list\n") + mavproxy.expect("numLogs") + # ensure the full list of logs has come out + for i in range(5): + self.wait_heartbeat() + mavproxy.send("log download latest %s\n" % filename) + mavproxy.expect("Finished downloading", timeout=120) + self.mavproxy_unload_module(mavproxy, 'log') + self.stop_mavproxy(mavproxy) + self.context_pop() def TestLogDownloadMAVProxyCAN(self, upload_logs=False): @@ -4314,8 +4396,9 @@ def TestLogDownloadMAVProxyCAN(self, upload_logs=False): self.mavproxy_load_module(mavproxy, 'log') mavproxy.send("log list\n") mavproxy.expect("numLogs") - self.wait_heartbeat() - self.wait_heartbeat() + # ensure the full list of logs has come out + for i in range(5): + self.wait_heartbeat() mavproxy.send("set shownoise 0\n") mavproxy.send("log download latest %s\n" % filename) mavproxy.expect("Finished downloading", timeout=120) @@ -8362,7 +8445,7 @@ def run_one_test_attempt(self, test, interact=False, attempt=1, suppress_stdout= def defaults_filepath(self): return None - def start_mavproxy(self, sitl_rcin_port=None, master=None): + def start_mavproxy(self, sitl_rcin_port=None, master=None, options=None): self.start_mavproxy_count += 1 if self.mavproxy is not None: return self.mavproxy @@ -8380,11 +8463,18 @@ def start_mavproxy(self, sitl_rcin_port=None, master=None): if master is None: master = 'tcp:127.0.0.1:%u' % self.adjust_ardupilot_port(5762) + if options is None: + options = self.mavproxy_options() + else: + op = self.mavproxy_options().copy() + op.extend(options) + options = op + mavproxy = util.start_MAVProxy_SITL( self.vehicleinfo_key(), master=master, logfile=self.mavproxy_logfile, - options=self.mavproxy_options(), + options=options, pexpect_timeout=pexpect_timeout, sitl_rcin_port=sitl_rcin_port, ) @@ -11867,8 +11957,8 @@ def NMEAOutput(self): self.set_parameter("GPS_TYPE2", 5) # GPS2 is NMEA port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartE=tcp:%u" % port, # GPS2 is NMEA.... - "--uartF=tcpclient:127.0.0.1:%u" % port, # serial5 spews to localhost port + "--serial4=tcp:%u" % port, # GPS2 is NMEA.... + "--serial5=tcpclient:127.0.0.1:%u" % port, # serial5 spews to localhost port ]) self.do_timesync_roundtrip() self.wait_gps_fix_type_gte(3) @@ -12484,7 +12574,7 @@ def FRSkyPassThroughStatustext(self): }) port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) frsky = FRSkyPassThrough(("127.0.0.1", port), get_time=self.get_sim_time_cached) @@ -12575,7 +12665,7 @@ def FRSkyPassThroughSensorIDs(self): }) port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) frsky = FRSkyPassThrough(("127.0.0.1", port), get_time=self.get_sim_time_cached) @@ -12730,7 +12820,7 @@ def FRSkyMAVlite(self): self.set_parameter("SERIAL5_PROTOCOL", 10) # serial5 is FRSky passthrough port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) frsky = FRSkyPassThrough(("127.0.0.1", port)) frsky.connect() @@ -13005,7 +13095,7 @@ def FRSkySPort(self): self.set_parameter("RPM1_TYPE", 10) # enable SITL RPM sensor port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) frsky = FRSkySPort(("127.0.0.1", port), verbose=True) self.wait_ready_to_arm() @@ -13078,7 +13168,7 @@ def FRSkyD(self): self.set_parameter("SERIAL5_PROTOCOL", 3) # serial5 is FRSky output port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) frsky = FRSkyD(("127.0.0.1", port)) self.wait_ready_to_arm() @@ -13197,7 +13287,7 @@ def LTM(self): self.set_parameter("SERIAL5_PROTOCOL", 25) # serial5 is LTM output port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) ltm = LTM(("127.0.0.1", port)) self.wait_ready_to_arm() @@ -13240,7 +13330,7 @@ def DEVO(self): self.set_parameter("SERIAL5_PROTOCOL", 17) # serial5 is DEVO output port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) devo = DEVO(("127.0.0.1", port)) self.wait_ready_to_arm() @@ -13313,7 +13403,7 @@ def MSP_DJI(self): self.set_parameter("MSP_OPTIONS", 1) # telemetry (unpolled) mode port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 spews to localhost port + "--serial5=tcp:%u" % port # serial5 spews to localhost port ]) msp = MSP_DJI(("127.0.0.1", port)) self.wait_ready_to_arm() @@ -13341,7 +13431,7 @@ def CRSF(self): self.set_parameter("SERIAL5_PROTOCOL", 23) # serial5 is RCIN input port = self.spare_network_port() self.customise_SITL_commandline([ - "--uartF=tcp:%u" % port # serial5 reads from to localhost port + "--serial5=tcp:%u" % port # serial5 reads from to localhost port ]) crsf = CRSF(("127.0.0.1", port)) crsf.connect() diff --git a/Tools/bootloaders/Airvolute-DCS2_bl.bin b/Tools/bootloaders/Airvolute-DCS2_bl.bin new file mode 100644 index 00000000000000..1c9935095b9f45 Binary files /dev/null and b/Tools/bootloaders/Airvolute-DCS2_bl.bin differ diff --git a/Tools/bootloaders/Airvolute-DCS2_bl.hex b/Tools/bootloaders/Airvolute-DCS2_bl.hex new file mode 100644 index 00000000000000..2a7d8442644c08 --- /dev/null +++ b/Tools/bootloaders/Airvolute-DCS2_bl.hex @@ -0,0 +1,1111 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E3020008053B0008C1 +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E3020008ED3E0008193F000833 +:10006000453F0008713F00089D3F0008E30200087B +:10007000E3020008E3020008E3020008E3020008CC +:10008000E3020008E3020008E3020008E3020008BC +:10009000E3020008E3020008E3020008C93F000889 +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008A1400008E3020008E302000870 +:1000E0002D400008E3020008E3020008E3020008D4 +:1000F000E3020008E3020008E3020008E30200084C +:10010000E3020008E3020008B5400008E30200082B +:10011000E3020008E3020008E3020008E30200082B +:10012000E3020008E3020008E3020008E30200081B +:10013000E3020008E3020008E3020008E30200080B +:10014000E3020008E3020008E3020008E3020008FB +:10015000E3020008E3020008E3020008E3020008EB +:10016000E3020008E3020008E3020008E3020008DB +:10017000E302000885340008E3020008E3020008F7 +:10018000E3020008E3020008E3020008E3020008BB +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E302000871340008E3020008E3020008AB +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F02F006FB6D +:1003500003F02AFB4FF055301F491B4A91423CBF26 +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE702F01EFB03F088FBB8 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E702F006BB000600200022002063 +:1003D0000000000808ED00E00000002000060020FA +:1003E000E04400080022002064220020682200204F +:1003F000C4470020E0020008E0020008E002000814 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002001F01EFEFEE701F0DB +:10043000ADFD00DFFEE7000038B500F0DBFB02F0A9 +:100440004DFA054602F080FA0446D0B90F4B9D42A2 +:1004500019D001339D4241F2883512BF0446002570 +:100460000124002002F044FA0CB100F077F800F00B +:1004700089FD00F03BFC284600F01EF900F06EF804 +:10048000F9E70025EDE70546EBE700BF010007B0FF +:1004900008B500F0F7FBA0F120035842584108BD11 +:1004A00007B541F21203022101A8ADF8043000F0B3 +:1004B00007FC03B05DF804FB38B5302383F31188E3 +:1004C000174803680BB101F09BFE0023154A4FF457 +:1004D0007A71134801F08AFE002383F31188124CCD +:1004E000236813B12368013B2360636813B1636819 +:1004F000013B63600D4D2B7833B963687BB90220F3 +:1005000000F0BAFC322363602B78032B07D16368B9 +:100510002BB9022000F0B0FC4FF47A73636038BD51 +:1005200068220020B90400088823002080220020CF +:10053000084B187003280CD8DFE800F00805020803 +:10054000022000F08FBC022000F084BC024B00228D +:100550005A6070478022002088230020F8B5504B55 +:10056000504A1C461968013100F0998004339342C7 +:10057000F8D162684C4B9A4240F291804B4B9B6899 +:1005800003F1006303F500339A4280F08880002075 +:1005900000F0D2FB0220FFF7CBFF454B0021D3F840 +:1005A000E820C3F8E810D3F81021C3F81011D3F8ED +:1005B0001021D3F8EC20C3F8EC10D3F81421C3F8C1 +:1005C0001411D3F81421D3F8F020C3F8F010D3F8A5 +:1005D0001821C3F81811D3F81821D3F8802042F05D +:1005E0000062C3F88020D3F8802022F00062C3F8B4 +:1005F0008020D3F88020D3F8802042F00072C3F826 +:100600008020D3F8802022F00072C3F88020D3F835 +:10061000803072B64FF0E023C3F8084DD4E90004EF +:10062000BFF34F8FBFF36F8F224AC2F88410BFF31E +:100630004F8F536923F480335361BFF34F8FD2F848 +:10064000803043F6E076C3F3C905C3F34E335B0154 +:1006500003EA060C29464CEA81770139C2F8747224 +:10066000F9D2203B13F1200FF2D1BFF34F8FBFF32C +:100670006F8FBFF34F8FBFF36F8F536923F4003336 +:1006800053610023C2F85032BFF34F8FBFF36F8F17 +:10069000302383F31188854680F308882047F8BD0E +:1006A0000000020820000208FFFF010800220020CD +:1006B0000044025800ED00E02DE9F04F93B0B44B38 +:1006C0002022FF2100900AA89D6800F005FCB14A95 +:1006D0001378A3B90121B04811700360302383F36C +:1006E000118803680BB101F08BFD0023AB4A4FF476 +:1006F0007A71A94801F07AFD002383F31188009BE9 +:1007000013B1A74B009A1A60A64A1378032B03D0A3 +:1007100000231370A24A53604FF0000A009CD34696 +:100720005646D146012000F09DFB24B19C4B1B682E +:10073000002B00F02682002000F0A4FA0390039B17 +:10074000002BF2DB012000F085FB039B213B1F2BDC +:10075000E8D801A252F823F0D907000801080008E0 +:100760009508000825070008250700082507000848 +:1007700027090008F70A0008110A0008730A000890 +:100780009B0A0008C10A000825070008D30A0008D0 +:1007900025070008450B0008790800082507000810 +:1007A000890B0008E50700087908000825070008FC +:1007B000730A000825070008250700082507000818 +:1007C0002507000825070008250700082507000859 +:1007D00025070008950800080220FFF759FE0028A9 +:1007E00040F0F981009B022105A8BAF1000F08BF73 +:1007F0001C4641F21233ADF8143000F061FA91E773 +:100800004FF47A7000F03EFA071EEBDB0220FFF790 +:100810003FFE0028E6D0013F052F00F2DE81DFE831 +:1008200007F0030A0D1013360523042105A80593CC +:1008300000F046FA17E004215548F9E704215A4828 +:10084000F6E704215948F3E74FF01C08404608F149 +:10085000040800F073FA0421059005A800F030FAAE +:10086000B8F12C0FF2D101204FF0000900FA07F780 +:1008700047EA0B0B5FFA8BFB00F07AFB26B10BF01B +:100880000B030B2B08BF0024FFF70AFE4AE70421E5 +:100890004748CDE7002EA5D00BF00B030B2BA1D1C1 +:1008A0000220FFF7F5FD074600289BD00120002617 +:1008B00000F042FA0220FFF73BFE1FFA86F840469E +:1008C00000F04AFA0446B0B1039940460136A1F15E +:1008D00040025142514100F04FFA0028EDD1BA4692 +:1008E000044641F21213022105A83E46ADF8143029 +:1008F00000F0E6F916E725460120FFF719FE244B24 +:100900009B68AB4207D9284600F018FA013040F046 +:1009100067810435F3E70025224BBA463E461D7039 +:100920001F4B5D60A8E7002E3FF45CAF0BF00B039C +:100930000B2B7FF457AF0220FFF7FAFD322000F0B7 +:10094000A1F9B0F10008FFF64DAF18F003077FF4EE +:1009500049AF0F4A08EB0503926893423FF642AF56 +:10096000B8F5807F3FF73EAF124BB845019323DDCA +:100970004FF47A7000F086F90390039A002AFFF68C +:1009800031AF039A0137019B03F8012BEDE700BF5C +:10099000002200208423002068220020B9040008DF +:1009A000882300208022002004220020082200202A +:1009B0000C22002084220020C820FFF769FD074692 +:1009C00000283FF40FAF1F2D11D8C5F120020AAB4C +:1009D00025F0030084494245184428BF424601924D +:1009E00000F054FA019AFF217F4800F075FA4FEAAF +:1009F000A803C8F387027C492846019300F074FAE3 +:100A0000064600283FF46DAF019B05EB830533E7F5 +:100A10000220FFF73DFD00283FF4E4AE00F0CAF9E4 +:100A200000283FF4DFAE0027B846704B9B68BB42FE +:100A300018D91F2F11D80A9B01330ED027F00303BA +:100A400012AA134453F8203C05934046042205A9FA +:100A5000043700F0F9FA8046E7E7384600F06EF90F +:100A60000590F2E7CDF81480042105A800F028F9DC +:100A700002E70023642104A8049300F017F900287A +:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA +:100A9000049800F085F90590E6E70023642104A896 +:100AA000049300F003F900287FF49CAE0220FFF7C6 +:100AB000EFFC00283FF496AE049800F073F9EAE7E3 +:100AC0000220FFF7E5FC00283FF48CAE00F082F92D +:100AD000E1E70220FFF7DCFC00283FF483AE05A924 +:100AE000142000F07DF907460421049004A800F0CA +:100AF000E7F83946B9E7322000F0C4F8071EFFF6E0 +:100B000071AEBB077FF46EAE384A07EB09039268FB +:100B100093423FF667AE0220FFF7BAFC00283FF48D +:100B200061AE27F003074F44B9453FF4A5AE4846F0 +:100B300009F1040900F002F90421059005A800F06C +:100B4000BFF8F1E74FF47A70FFF7A2FC00283FF4FA +:100B500049AE00F02FF9002844D00A9B01330BD096 +:100B600008220AA9002000F0BFF900283AD020226C +:100B7000FF210AA800F0B0F9FFF792FC1C4801F031 +:100B800079FA13B0BDE8F08F002E3FF42BAE0BF0D6 +:100B90000B030B2B7FF426AE0023642105A80593DD +:100BA00000F084F8074600287FF41CAE0220FFF70F +:100BB0006FFC804600283FF415AEFFF771FC41F250 +:100BC000883001F057FA059800F01CFA46463C467A +:100BD00000F0CEF9A6E506464EE64FF0000901E624 +:100BE000BA467EE637467CE68422002000220020BA +:100BF000A0860100704700002DE9F84F4FF47A738A +:100C000006460D46002402FB03F7DFF85080DFF8AC +:100C1000509098F900305FFA84FA5A1C01D0A34230 +:100C200012D159F824002A4631460368D3F820B07F +:100C30003B46D847854207D1074B012083F800A0E7 +:100C4000BDE8F88F0124E4E7002CFBD04FF4FA70E4 +:100C500001F010FA0020F3E7D42300201022002036 +:100C60001422002070B504464FF47A76412C2546B4 +:100C700028BF412506FB05F001F0FCF9641BF5D106 +:100C800070BD0000002307B5024601210DF10700E9 +:100C90008DF80730FFF7B0FF20B19DF8070003B0D3 +:100CA0005DF804FB4FF0FF30F9E700000A4604212D +:100CB00008B5FFF7A1FF80F00100C0B2404208BDB7 +:100CC000074B0A4630B41978064B53F821400146C9 +:100CD00023682046DD69044BAC4630BC604700BF4A +:100CE000D423002014220020A086010070B5104CEF +:100CF0000025104E01F0DAFC208030682388834202 +:100D00000CD800252088013801F0CCFC238805444C +:100D1000013BB5F5802F2380F4D370BD01F0C2FCF8 +:100D2000336805440133B5F5003F3360E5D3E8E7A8 +:100D3000D62300209023002001F086BD00F1006042 +:100D400000F500300068704700F10060920000F587 +:100D5000003001F007BD0000054B1A68054B1B88E9 +:100D60009B1A834202D9104401F09CBC00207047BA +:100D700090230020D623002038B50446074D29B122 +:100D800028682044BDE8384001F0A4BC286820440D +:100D900001F08EFC0028F3D038BD00BF9023002066 +:100DA0000020704700F1FF5000F58F10D0F80008C8 +:100DB00070470000064991F8243033B1002308221F +:100DC000086A81F82430FFF7BFBF0120704700BFD9 +:100DD00094230020014B1868704700BF0010005C8E +:100DE000194B01380322084470B51D68174BC5F331 +:100DF0000B042D0C1E88A6420BD15C680A46013CF0 +:100E0000824213460FD214F9016F4EB102F8016B02 +:100E1000F6E7013A03F10803ECD181420B4602D216 +:100E20002C2203F8012B0424094A1688AE4204D16F +:100E3000984284BF967803F8016B013C02F10402EA +:100E4000F3D1581A70BD00BF0010005C1C220020B6 +:100E5000B8410008022801D1014B9861704700BFDA +:100E600000040258022803D1024B4FF400329A6169 +:100E7000704700BF00040258022804D1024A536997 +:100E800083F002035361704700040258002310B539 +:100E9000934203D0CC5CC4540133F9E710BD000089 +:100EA000013810B510F9013F3BB191F900409C4267 +:100EB00003D11AB10131013AF4E71AB191F90020D6 +:100EC000981A10BD1046FCE703460246D01A12F9E4 +:100ED000011B0029FAD1704702440346934202D015 +:100EE00003F8011BFAE770472DE9F8431F4D14463C +:100EF0000746884695F8242052BBDFF870909CB3D3 +:100F000095F824302BB92022FF2148462F62FFF7A5 +:100F1000E3FF95F824004146C0F1080205EB80008C +:100F2000A24228BF2246D6B29200FFF7AFFF95F843 +:100F30002430A41B17441E449044E4B2F6B2082E99 +:100F400085F82460DBD1FFF735FF0028D7D108E012 +:100F50002B6A03EB82038342CFD0FFF72BFF0028DD +:100F6000CBD10020BDE8F8830120FBE794230020CB +:100F7000024B1A78024B1A70704700BFD42300202E +:100F80001022002038B51A4C1A4D204600F0C6FB3E +:100F90002946204600F0EEFB2D684FF47A70D5F814 +:100FA0009020D2F8043843F00203C2F80438FFF767 +:100FB00059FE1149284600F0EBFCD5F890200F4D62 +:100FC000D2F80438286823F002030D49A042C2F881 +:100FD00004384FF4E1330B6001D000F0FDFA68688B +:100FE000A04204D00649BDE8384000F0F5BA38BD4B +:100FF000B82A0020B4420008BC4200081422002095 +:10100000BC2300200C4B70B50C4D04461E780C4BD5 +:1010100055F826209A420DD00A4B00211822184676 +:10102000FFF75AFF0460014655F82600BDE87040FE +:1010300000F0D2BA70BD00BFD423002014220020DB +:10104000B82A0020BC23002030B50A44084D914244 +:101050000DD011F8013B5840082340F30004013B38 +:101060002C4013F0FF0384EA5000F6D1EFE730BDC7 +:101070002083B8ED026843681143016003B118474B +:1010800070470000024A136843F0C00313607047C2 +:101090000010014013B50E4C204600F091FA04F107 +:1010A000140000234FF400720A49009400F04EF936 +:1010B000094B4FF40072094904F13800009400F024 +:1010C000C7F9074A074BC4E9172302B010BD00BF98 +:1010D000D823002044240020851000084426002046 +:1010E0000010014000E1F505037C30B5244C0029D7 +:1010F00018BF0C46012B11D1224B98420ED1224B26 +:10110000D3F8F02042F01002C3F8F020D3F81821F1 +:1011100042F01002C3F81821D3F818312268036E88 +:10112000C16D03EB52038466B3FBF2F362681504EE +:1011300042BF23F0070503F0070343EA4503CB60F2 +:10114000A36843F040034B60E36843F001038B6006 +:1011500042F4967343F001030B604FF0FF330B62D0 +:10116000510505D512F0102205D0B2F1805F04D0F0 +:1011700080F8643030BD7F23FAE73F23F8E700BFF3 +:10118000C8410008D8230020004402582DE9F04748 +:10119000C66D05463768F469210734621AD014F029 +:1011A000080118BF4FF48071E20748BF41F02001E9 +:1011B000A3074FF0300348BF41F04001600748BF2C +:1011C00041F0800183F31188281DFFF753FF0023AE +:1011D00083F31188E2050AD5302383F311884FF495 +:1011E0008061281DFFF746FF002383F311884FF02D +:1011F00030094FF0000A14F0200838D13B0616D50C +:101200004FF0300905F1380A200610D589F311880E +:10121000504600F051F9002836DA0821281DFFF762 +:1012200029FF27F080033360002383F311887906B8 +:1012300014D5620612D5302383F31188D5E9132320 +:101240009A4208D12B6C33B127F040071021281D9A +:10125000FFF710FF3760002383F31188E30618D5EA +:10126000AA6E1369ABB15069BDE8F047184789F31E +:101270001188736A284695F86410194000F0BAF98D +:101280008AF31188F469B6E7B06288F31188F469CB +:10129000BAE7BDE8F0870000F8B515468268044655 +:1012A0000B46AA4200D28568A1692669761AB54222 +:1012B0000BD218462A46FFF7E9FDA3692B44A36128 +:1012C0002846A3685B1BA360F8BD0CD9AF1B18466A +:1012D0003246FFF7DBFD3A46E1683044FFF7D6FDC2 +:1012E000E3683B44EBE718462A46FFF7CFFDE36887 +:1012F000E5E7000083689342F7B50446154600D23F +:101300008568D4E90460361AB5420BD22A46FFF745 +:10131000BDFD63692B4463612846A3685B1BA36022 +:1013200003B0F0BD0DD93246AF1B0191FFF7AEFD02 +:1013300001993A46E0683144FFF7A8FDE3683B4471 +:10134000E9E72A46FFF7A2FDE368E4E710B50A449F +:101350000024C361029B8460C16002610362C0E932 +:101360000000C0E9051110BD08B5D0E9053293426F +:1013700001D1826882B98268013282605A1C42615E +:1013800019700021D0E904329A4224BFC368436136 +:1013900000F08CFE002008BD4FF0FF30FBE700009E +:1013A00070B5302304460E4683F31188A568A5B1B5 +:1013B000A368A269013BA360531CA361157822694D +:1013C000934224BFE368A361E3690BB120469847C9 +:1013D000002383F31188284607E03146204600F0B9 +:1013E00055FE0028E2DA85F3118870BD2DE9F74F2C +:1013F00004460E4617469846D0F81C904FF0300A27 +:101400008AF311884FF0000B154665B12A46314624 +:101410002046FFF741FF034660B94146204600F0F1 +:1014200035FE0028F1D0002383F31188781B03B028 +:10143000BDE8F08FB9F1000F03D001902046C847F6 +:10144000019B8BF31188ED1A1E448AF31188DCE7A7 +:10145000C160C361009B82600362C0E90511114451 +:10146000C0E9000001617047F8B504460D46164614 +:10147000302383F31188A768A7B1A368013BA36059 +:1014800063695A1C62611D70D4E904329A4224BF18 +:10149000E3686361E3690BB120469847002080F35D +:1014A000118807E03146204600F0F0FD0028E2DA1E +:1014B00087F31188F8BD0000D0E9052310B59A42E2 +:1014C00001D182687AB982680021013282605A1C97 +:1014D00082611C7803699A4224BFC368836100F06B +:1014E000E5FD204610BD4FF0FF30FBE72DE9F74F3B +:1014F00004460E4617469846D0F81C904FF0300A26 +:101500008AF311884FF0000B154665B12A46314623 +:101510002046FFF7EFFE034660B94146204600F043 +:10152000B5FD0028F1D0002383F31188781B03B0A8 +:10153000BDE8F08FB9F1000F03D001902046C847F5 +:10154000019B8BF31188ED1A1E448AF31188DCE7A6 +:10155000026843681143016003B1184770470000F7 +:101560001430FFF743BF00004FF0FF331430FFF794 +:101570003DBF00003830FFF7B9BF00004FF0FF3328 +:101580003830FFF7B3BF00001430FFF709BF000089 +:101590004FF0FF311430FFF703BF00003830FFF782 +:1015A00063BF00004FF0FF323830FFF75DBF00002F +:1015B000012914BF6FF0130000207047FFF76ABDC8 +:1015C000044B036000234360C0E90233012303742A +:1015D000704700BFE041000810B53023044683F394 +:1015E0001188FFF781FD02230020237480F3118806 +:1015F00010BD000038B5C36904460D461BB904216F +:101600000844FFF7A5FF294604F11400FFF7ACFEDC +:10161000002806DA201D4FF40061BDE83840FFF7CE +:1016200097BF38BD026843681143016003B1184792 +:101630007047000013B5406B00F58054D4F8A4380F +:101640001A681178042914D1017C022911D1197961 +:10165000012312898B4013420BD101A94C3002F0B7 +:1016600059F8D4F8A4480246019B2179206800F07B +:10167000DFF902B010BD0000143001F0DBBF000044 +:101680004FF0FF33143001F0D5BF00004C3002F0B2 +:10169000ADB800004FF0FF334C3002F0A7B80000A7 +:1016A000143001F0A9BF00004FF0FF31143001F0F9 +:1016B000A3BF00004C3002F079B800004FF0FF32B9 +:1016C0004C3002F073B800000020704710B500F5F0 +:1016D0008054D4F8A4381A681178042917D1017CF1 +:1016E000022914D15979012352898B4013420ED11A +:1016F000143001F03BFF024648B1D4F8A4484FF43F +:10170000407361792068BDE8104000F07FB910BDDA +:10171000406BFFF7DBBF0000704700007FB5124B46 +:1017200001250426044603600023057400F18402A9 +:1017300043602946C0E902330C4B029014300193F8 +:101740004FF44073009601F0EDFE094B04F694420D +:10175000294604F14C000294CDE900634FF4407334 +:1017600001F0B4FF04B070BD084200081117000872 +:10177000351600080A68302383F311880B790B3380 +:1017800042F823004B79133342F823008B7913B1CD +:101790000B3342F8230000F58053C3F8A41802234A +:1017A0000374002080F311887047000038B5037F70 +:1017B000044613B190F85430ABB90125201D022125 +:1017C000FFF730FF04F114006FF00101257700F0FE +:1017D00079FC04F14C0084F854506FF00101BDE82D +:1017E000384000F06FBC38BD10B50121044604300C +:1017F000FFF718FF0023237784F8543010BD000052 +:1018000038B504460025143001F0A4FE04F14C0064 +:10181000257701F073FF201D84F854500121FFF754 +:1018200001FF2046BDE83840FFF750BF90F88030F8 +:1018300003F06003202B06D190F881200023212A99 +:1018400003D81F2A06D800207047222AFBD1C0E9FE +:101850001D3303E0034A426707228267C367012002 +:10186000704700BF3422002037B500F58055D5F809 +:10187000A4381A68117804291AD1017C022917D1D9 +:101880001979012312898B40134211D100F14C04C4 +:10189000204601F0F3FF58B101A9204601F03AFFBC +:1018A000D5F8A4480246019B2179206800F0C0F8D1 +:1018B00003B030BD01F10B03F0B550F8236085B0E3 +:1018C00004460D46FEB1302383F3118804EB8507EF +:1018D000301D0821FFF7A6FEFB6806F14C005B698E +:1018E0001B681BB1019001F023FF019803A901F0CF +:1018F00011FF024648B1039B2946204600F098F8A4 +:10190000002383F3118805B0F0BDFB685A691268A3 +:10191000002AF5D01B8A013B1340F1D104F180026B +:10192000EAE70000133138B550F82140ECB130231C +:1019300083F3118804F58053D3F8A42813685279EF +:1019400003EB8203DB689B695D6845B10421601885 +:10195000FFF768FE294604F1140001F011FE20464D +:10196000FFF7B4FE002383F3118838BD70470000F1 +:1019700001F0C8B801234022002110B5044600F848 +:10198000303BFFF7A9FA0023C4E9013310BD000082 +:1019900010B53023044683F31188242241600021CE +:1019A0000C30FFF799FA204601F0CEF80223002010 +:1019B000237080F3118810BD70B500EB81030546DC +:1019C00050690E461446DA6018B110220021FFF764 +:1019D00083FAA06918B110220021FFF77DFA314681 +:1019E0002846BDE8704001F0B5B9000083682022A8 +:1019F000002103F0011310B5044683601030FFF797 +:101A00006BFA2046BDE8104001F030BAF0B4012571 +:101A100000EB810447898D40E4683D43A46945811A +:101A200023600023A2606360F0BC01F04DBA0000A7 +:101A3000F0B4012500EB810407898D40E4683D4343 +:101A40006469058123600023A2606360F0BC01F03B +:101A5000C3BA000070B50223002504462422037097 +:101A60002946C0F888500C3040F8045CFFF734FA7F +:101A7000204684F8705001F001F963681B6823B1B7 +:101A800029462046BDE87040184770BD0378052BF5 +:101A900010B504460AD080F88C30052303704368E3 +:101AA0001B680BB1042198470023A36010BD000000 +:101AB0000178052906D190F88C20436802701B68D4 +:101AC00003B118477047000070B590F870300446B5 +:101AD00013B1002380F8703004F18002204601F039 +:101AE000E9F963689B68B3B994F8803013F0600536 +:101AF00035D00021204601F0DBFC0021204601F01A +:101B0000CBFC63681B6813B1062120469847062367 +:101B100084F8703070BD204698470028E4D0B4F8AF +:101B20008630A26F9A4288BFA36794F98030A56F70 +:101B3000002B4FF0300380F20381002D00F0F28083 +:101B4000092284F8702083F3118800212046D4E90B +:101B50001D23FFF76DFF002383F31188DAE794F864 +:101B6000812003F07F0343EA022340F202329342D2 +:101B700000F0C58021D8B3F5807F48D00DD8012B67 +:101B80003FD0022B00F09380002BB2D104F18802E9 +:101B900062670222A267E367C1E7B3F5817F00F0C5 +:101BA0009B80B3F5407FA4D194F88230012BA0D163 +:101BB000B4F8883043F0020332E0B3F5006F4DD043 +:101BC00017D8B3F5A06F31D0A3F5C063012B90D81F +:101BD0006368204694F882205E6894F88310B4F815 +:101BE0008430B047002884D0436863670368A367E4 +:101BF0001AE0B3F5106F36D040F6024293427FF4FC +:101C000078AF5C4B63670223A3670023C3E794F8B4 +:101C10008230012B7FF46DAFB4F8883023F00203DB +:101C2000A4F88830C4E91D55E56778E7B4F880303A +:101C3000B3F5A06F0ED194F88230204684F88A3034 +:101C400001F07AF863681B6813B1012120469847B8 +:101C5000032323700023C4E91D339CE704F18B03A5 +:101C600063670123C3E72378042B10D1302383F368 +:101C700011882046FFF7BAFE85F3118803216368B7 +:101C800084F88B5021701B680BB12046984794F85C +:101C90008230002BDED084F88B30042323706368FD +:101CA0001B68002BD6D0022120469847D2E794F833 +:101CB000843020461D0603F00F010AD501F0ECF830 +:101CC000012804D002287FF414AF2B4B9AE72B4B4A +:101CD00098E701F0D3F8F3E794F88230002B7FF413 +:101CE00008AF94F8843013F00F01B3D01A062046E1 +:101CF00002D501F0F5FBADE701F0E6FBAAE794F8A9 +:101D00008230002B7FF4F5AE94F8843013F00F018D +:101D1000A0D01B06204602D501F0CAFB9AE701F0CD +:101D2000BBFB97E7142284F8702083F311882B46BD +:101D30002A4629462046FFF769FE85F31188E9E621 +:101D40005DB1152284F8702083F3118800212046AC +:101D5000D4E91D23FFF75AFEFDE60B2284F870201C +:101D600083F311882B462A4629462046FFF760FE5A +:101D7000E3E700BF38420008304200083442000860 +:101D800038B590F870300446002B3ED0063BDAB2EE +:101D90000F2A34D80F2B32D8DFE803F0373131085F +:101DA000223231313131313131313737856FB0F84D +:101DB00086309D4214D2C3681B8AB5FBF3F203FB45 +:101DC00012556DB9302383F311882B462A462946D4 +:101DD000FFF72EFE85F311880A2384F870300EE099 +:101DE000142384F87030302383F3118800232046B5 +:101DF0001A461946FFF70AFE002383F3118838BDFF +:101E0000C36F03B198470023E7E70021204601F0A4 +:101E10004FFB0021204601F03FFB63681B6813B1B4 +:101E20000621204698470623D7E7000010B590F812 +:101E300070300446142B29D017D8062B05D001D8B2 +:101E40001BB110BD093B022BFBD80021204601F03D +:101E50002FFB0021204601F01FFB63681B6813B1B4 +:101E6000062120469847062319E0152BE9D10B23BC +:101E700080F87030302383F3118800231A46194606 +:101E8000FFF7D6FD002383F31188DAE7C3689B6967 +:101E90005B68002BD5D1C36F03B19847002384F84A +:101EA0007030CEE700238268037503691B68996868 +:101EB0009142FBD25A6803604260106058607047DC +:101EC00000238268037503691B6899689142FBD8F7 +:101ED0005A680360426010605860704708B5084651 +:101EE000302383F311880B7D032B05D0042B0DD0F9 +:101EF0002BB983F3118808BD8B6900221A604FF05B +:101F0000FF338361FFF7CEFF0023F2E7D1E9003210 +:101F100013605A60F3E70000FFF7C4BF054BD968B0 +:101F200008751868026853601A600122D86002754B +:101F3000FEF768BA482800200C4B30B5DD684B1C12 +:101F400087B004460FD02B46094A684600F04EF988 +:101F50002046FFF7E3FF009B13B1684600F050F9FD +:101F6000A86907B030BDFFF7D9FFF9E7482800207E +:101F7000DD1E0008044B1A68DB6890689B68984275 +:101F800094BF00200120704748280020084B10B55E +:101F90001C68D868226853601A600122DC602275D0 +:101FA000FFF78EFF01462046BDE81040FEF72ABA33 +:101FB0004828002038B5074C01230025064907486A +:101FC0002370656001F098FC0223237085F311886B +:101FD00038BD00BFB02A0020404200084828002039 +:101FE00000F044B9034A516853685B1A9842FBD821 +:101FF000704700BF001000E08B60022308610846B4 +:102000008B8270478368A3F1840243F8142C026921 +:1020100043F8442C426943F8402C094A43F8242CE5 +:10202000C268A3F1200043F8182C022203F80C2CFC +:10203000002203F80B2C034A43F8102C704700BF12 +:102040001D0400084828002008B5FFF7DBFFBDE8A5 +:102050000840FFF761BF0000024BDB6898610F206A +:10206000FFF75CBF48280020302383F31188FFF777 +:10207000F3BF000008B50146302383F31188082020 +:10208000FFF75AFF002383F3118808BD064BDB6876 +:1020900039B1426818605A60136043600420FFF74A +:1020A0004BBF4FF0FF30704748280020036898422C +:1020B00006D01A680260506018469961FFF72CBF7D +:1020C0007047000038B504460D462068844200D1B0 +:1020D00038BD036823605C608561FFF71DFFF4E78E +:1020E000036810B59C68A2420CD85C688A600B60DB +:1020F0004C602160596099688A1A9A604FF0FF33EA +:10210000836010BD121B1B68ECE700000A2938BF72 +:102110000A2170B504460D460A26601901F0E4FB59 +:1021200001F0CCFB041BA54203D8751C04462E46C7 +:10213000F3E70A2E04D90120BDE8704001F01CBC71 +:1021400070BD0000F8B5144B0D460A2A4FF00A077F +:10215000D96103F11001826038BF0A224160196918 +:102160001446016048601861A81801F0ADFB01F049 +:10217000A5FB431B0646A34206D37C1C2819274611 +:10218000354601F0B1FBF2E70A2F04D90120BDE882 +:10219000F84001F0F1BBF8BD48280020F8B506462C +:1021A0000D4601F08BFB0F4A134653F8107F9F42F8 +:1021B00006D12A4601463046BDE8F840FFF7C2BFC7 +:1021C000D169BB68441A2C1928BF2C46A34202D9F6 +:1021D0002946FFF79BFF224631460348BDE8F840F9 +:1021E000FFF77EBF4828002058280020C0E90323BD +:1021F000002310B45DF8044B4361FFF7CFBF00002C +:1022000010B5194C236998420DD08168D0E900328D +:1022100013605A609A680A449A60002303604FF082 +:10222000FF33A36110BD0268234643F8102F5360AB +:102230000022026022699A4203D1BDE8104001F0F9 +:102240004DBB936881680B44936001F037FB2269B2 +:10225000E1699268441AA242E4D91144BDE81040F1 +:10226000091AFFF753BF00BF482800202DE9F047A7 +:10227000DFF8BC8008F110072C4ED8F8105001F0A0 +:102280001DFBD8F81C40AA68031B9A423ED8144490 +:102290004FF00009D5E90032C8F81C4013605A60BD +:1022A000C5F80090D8F81030B34201D101F016FB08 +:1022B00089F31188D5E9033128469847302383F301 +:1022C00011886B69002BD8D001F0F8FA6A69A0EB8D +:1022D000040982464A450DD2022001F04DFB00223E +:1022E000D8F81030B34208D151462846BDE8F0472F +:1022F000FFF728BF121A2244F2E712EB0909294618 +:10230000384638BF4A46FFF7EBFEB5E7D8F810303D +:10231000B34208D01444C8F81C00211AA960BDE8D3 +:10232000F047FFF7F3BEBDE8F08700BF5828002054 +:102330004828002000207047FEE70000704700009A +:102340004FF0FF307047000002290CD0032904D061 +:102350000129074818BF00207047032A05D80548FF +:1023600000EBC2007047044870470020704700BF70 +:102370001843000844220020CC42000870B59AB0EF +:1023800005460846144601A900F0C2F801A8FEF768 +:102390009BFD431C0022C6B25B001046C5E9003419 +:1023A00023700323023404F8013C01ABD1B20234A0 +:1023B0008E4201D81AB070BD13F8011B013204F827 +:1023C000010C04F8021CF1E708B5302383F31188EF +:1023D0000348FFF749FA002383F3118808BD00BFC3 +:1023E000B82A002090F8803003F01F02012A07D19C +:1023F00090F881200B2A03D10023C0E91D3315E09A +:1024000003F06003202B08D1B0F884302BB990F88A +:102410008120212A03D81F2A04D8FFF707BA222ACD +:10242000EBD0FAE7034A426707228267C3670120BD +:10243000704700BF3B22002007B5052917D8DFE809 +:1024400001F0191603191920302383F31188104A5B +:1024500001210190FFF7B0FA019802210D4AFFF720 +:10246000ABFA0D48FFF7CCF9002383F3118803B0D2 +:102470005DF804FB302383F311880748FFF796F9D2 +:10248000F2E7302383F311880348FFF7ADF9EBE758 +:102490006C42000890420008B82A002038B50C4D64 +:1024A0000C4C2A460C4904F10800FFF767FF05F1C0 +:1024B000CA0204F110000949FFF760FF05F5CA726E +:1024C00004F118000649BDE83840FFF757BF00BFC8 +:1024D00090430020442200204C420008564200084D +:1024E0006142000870B5044608460D46FEF7ECFC54 +:1024F000C6B22046013403780BB9184670BD324687 +:102500002946FEF7CDFC0028F3D10120F6E70000B4 +:102510002DE9F04705460C46FEF7D6FC2B49C6B21E +:102520002846FFF7DFFF08B10736F6B228492846EC +:10253000FFF7D8FF08B11036F6B2632E0BD8DFF8DC +:102540008C80DFF88C90234FDFF894A02E7846B96A +:102550002670BDE8F08729462046BDE8F04701F027 +:102560000DBE252E2ED1072241462846FEF798FCA7 +:1025700070B9194B224603F10C0153F8040B8B423E +:1025800042F8040BF9D11B8807350E341380DDE7C0 +:10259000082249462846FEF783FC98B9A21C0F4B37 +:1025A000197802320909C95D02F8041C13F8011BED +:1025B00001F00F015345C95D02F8031CF0D1183436 +:1025C0000835C3E7013504F8016BBFE7384300085D +:1025D000614200084F4300084043000800E8F11F33 +:1025E0000CE8F11FBFF34F8F044B1A695107FCD160 +:1025F000D3F810215207F8D1704700BF00200052D5 +:1026000008B50D4B1B78ABB9FFF7ECFF0B4BDA6845 +:10261000D10704D50A4A5A6002F188325A60D3F8C9 +:102620000C21D20706D5064AC3F8042102F18832EC +:10263000C3F8042108BD00BFEE4500200020005271 +:102640002301674508B5114B1B78F3B9104B1A6984 +:10265000510703D5DA6842F04002DA60D3F810215E +:10266000520705D5D3F80C2142F04002C3F80C21E3 +:10267000FFF7B8FF064BDA6842F00102DA60D3F8E0 +:102680000C2142F00102C3F80C2108BDEE450020E8 +:10269000002000520F289ABF00F5806040040020FF +:1026A000704700004FF40030704700001020704762 +:1026B0000F2808B50BD8FFF7EDFF00F500330268CF +:1026C000013204D104308342F9D1012008BD002039 +:1026D000FCE700000F2838B505463FD8FFF782FF1A +:1026E0001F4CFFF78DFF4FF0FF3307286361C4F8DD +:1026F00014311DD82361FFF775FF030243F0240353 +:10270000E360E36843F08003E36023695A07FCD485 +:102710002846FFF767FFFFF7BDFF4FF4003100F0D9 +:10272000F5F82846FFF78EFFBDE83840FFF7C0BF39 +:10273000C4F81031FFF756FFA0F108031B0243F065 +:102740002403C4F80C31D4F80C3143F08003C4F8EE +:102750000C31D4F810315B07FBD4D9E7002038BD29 +:10276000002000522DE9F84F05460C46104645EA78 +:102770000203DE0602D00020BDE8F88F20F01F0023 +:10278000DFF8BCB0DFF8BCA0FFF73AFF04EB0008AD +:10279000444503D10120FFF755FFEDE720222946EC +:1027A000204601F0DBFC10B920352034F0E72B4641 +:1027B00005F120021F68791CDDD104339A42F9D15A +:1027C00005F178431B481C4EB3F5801F1B4B38BFE7 +:1027D000184603F1F80332BFD946D1461E46FFF72B +:1027E00001FF0760A5EB040C336804F11C0143F002 +:1027F00002033360231FD9F8007017F00507FAD1E0 +:1028000053F8042F8B424CF80320F4D1BFF34F8FC1 +:10281000FFF7E8FE4FF0FF332022214603602846F1 +:10282000336823F00203336001F098FC0028BBD02A +:102830003846B0E7142100520C20005214200052F8 +:10284000102000521021005210B5084C237828B1F6 +:102850001BB9FFF7D5FE0123237010BD002BFCD060 +:102860002070BDE81040FFF7EDBE00BFEE45002030 +:102870000244074BD2B210B5904200D110BD441CA7 +:1028800000B253F8200041F8040BE0B2F4E700BFB7 +:10289000504000580E4B30B51C6F240405D41C6FFB +:1028A0001C671C6F44F400441C670A4C02442368F4 +:1028B000D2B243F480732360074B904200D130BD05 +:1028C000441C51F8045B00B243F82050E0B2F4E736 +:1028D00000440258004802585040005807B50122F1 +:1028E00001A90020FFF7C4FF019803B05DF804FBC5 +:1028F00013B50446FFF7F2FFA04205D0012201A95B +:1029000000200194FFF7C6FF02B010BD0144BFF3E1 +:102910004F8F064B884204D3BFF34F8FBFF36F8FA7 +:102920007047C3F85C022030F4E700BF00ED00E020 +:10293000034B1A681AB9034AD2F8D0241A607047B8 +:10294000F04500200040025808B5FFF7F1FF024BA8 +:102950001868C0F3806008BDF045002070B5BFF373 +:102960004F8FBFF36F8F1A4A0021C2F85012BFF386 +:102970004F8FBFF36F8F536943F400335361BFF33D +:102980004F8FBFF36F8FC2F88410BFF34F8FD2F811 +:10299000803043F6E074C3F3C900C3F34E335B01E8 +:1029A00003EA0406014646EA81750139C2F860521D +:1029B000F9D2203B13F1200FF2D1BFF34F8F5369AF +:1029C00043F480335361BFF34F8FBFF36F8F70BDFC +:1029D00000ED00E0FEE70000214B2248224A70B5DE +:1029E000904237D3214BC11EDA1C121A22F0030287 +:1029F0008B4238BF00220021FEF76EFA1C4A0023EA +:102A0000C2F88430BFF34F8FD2F8803043F6E074C1 +:102A1000C3F3C900C3F34E335B0103EA0406014666 +:102A200046EA81750139C2F86C52F9D2203B13F1A4 +:102A3000200FF2D1BFF34F8FBFF36F8FBFF34F8FD4 +:102A4000BFF36F8F0023C2F85032BFF34F8FBFF335 +:102A50006F8F70BD53F8041B40F8041BC0E700BF24 +:102A600044450008C4470020C4470020C447002054 +:102A700000ED00E0074BD3F8D81021EA0001C3F8BD +:102A8000D810D3F8002122EA0002C3F80021D3F8BD +:102A9000003170470044025870B5D0E92443002249 +:102AA0004FF0FF359E6804EB42135101D3F8000943 +:102AB000002805DAD3F8000940F08040C3F8000987 +:102AC000D3F8000B002805DAD3F8000B40F0804063 +:102AD000C3F8000B013263189642C3F80859C3F8D3 +:102AE000085BE0D24FF00113C4F81C3870BD000041 +:102AF000890141F02001016103699B06FCD4122089 +:102B0000FFF770BA10B50A4C2046FEF733FF094BA9 +:102B1000C4F89030084BC4F89430084C2046FEF7B7 +:102B200029FF074BC4F89030064BC4F8943010BD11 +:102B3000F44500200000084084430008904600202F +:102B4000000004409043000870B503780546012B4F +:102B50005CD1434BD0F89040984258D1414B0E2164 +:102B60006520D3F8D82042F00062C3F8D820D3F80B +:102B7000002142F00062C3F80021D3F80021D3F80D +:102B8000802042F00062C3F88020D3F8802022F039 +:102B90000062C3F88020D3F8803000F0ADFC324BE7 +:102BA000E360324BC4F800380023D5F89060C4F8D5 +:102BB000003EC02323604FF40413A3633369002B4A +:102BC000FCDA01230C203361FFF70CFA3369DB07D1 +:102BD000FCD41220FFF706FA3369002BFCDA00263A +:102BE0002846A660FFF758FF6B68C4F81068DB68DA +:102BF000C4F81468C4F81C6883BB1D4BA3614FF074 +:102C0000FF336361A36843F00103A36070BD194BF8 +:102C10009842C9D1134B4FF08060D3F8D82042F0CE +:102C20000072C3F8D820D3F8002142F00072C3F834 +:102C30000021D3F80021D3F8802042F00072C3F8BD +:102C40008020D3F8802022F00072C3F88020D3F8CF +:102C50008030FFF70FFF0E214D209EE7064BCDE79A +:102C6000F4450020004402584014004003002002B4 +:102C7000003C30C090460020083C30C0F8B5D0F889 +:102C80009040054600214FF000662046FFF730FFD8 +:102C9000D5F8941000234FF001128F684FF0FF30E9 +:102CA000C4F83438C4F81C2804EB431201339F42A3 +:102CB000C2F80069C2F8006BC2F80809C2F8080B34 +:102CC000F2D20B68D5F89020C5F8983063621023D3 +:102CD0001361166916F01006FBD11220FFF782F976 +:102CE000D4F8003823F4FE63C4F80038A36943F431 +:102CF000402343F01003A3610923C4F81038C4F83B +:102D000014380B4BEB604FF0C043C4F8103B094B39 +:102D1000C4F8003BC4F81069C4F80039D5F89830FD +:102D200003F1100243F48013C5F89820A362F8BDA4 +:102D30006043000840800010D0F8902090F88A107E +:102D4000D2F8003823F4FE6343EA0113C2F80038D6 +:102D5000704700002DE9F84300EB8103D0F8905054 +:102D60000C468046DA680FFA81F94801166806F0C9 +:102D70000306731E022B05EB41134FF0000194BFB5 +:102D8000B604384EC3F8101B4FF0010104F11003D4 +:102D900098BF06F1805601FA03F3916998BF06F5D2 +:102DA000004600293AD0578A04F1580137434901B7 +:102DB0006F50D5F81C180B430021C5F81C382B1890 +:102DC0000127C3F81019A7405369611E9BB3138AEA +:102DD000928B9B08012A88BF5343D8F898209818F3 +:102DE00042EA034301F140022146C8F89800284610 +:102DF00005EB82025360FFF77BFE08EB8900C36896 +:102E00001B8A43EA845348341E4364012E51D5F88B +:102E10001C381F43C5F81C78BDE8F88305EB49173B +:102E2000D7F8001B21F40041C7F8001BD5F81C1887 +:102E300021EA0303C0E704F13F030B4A2846214679 +:102E400005EB83035A60FFF753FE05EB4910D0F8FA +:102E5000003923F40043C0F80039D5F81C3823EAC0 +:102E60000707D7E70080001000040002D0F8942084 +:102E70001268C0F89820FFF70FBE00005831D0F854 +:102E8000903049015B5813F4004004D013F4001F44 +:102E90000CBF0220012070474831D0F89030490122 +:102EA0005B5813F4004004D013F4001F0CBF022041 +:102EB0000120704700EB8101CB68196A0B68136031 +:102EC0004B6853607047000000EB810330B5DD684C +:102ED000AA691368D36019B9402B84BF40231360DB +:102EE0006B8A1468D0F890201C4402EB4110013C1E +:102EF00009B2B4FBF3F46343033323F0030343EA5F +:102F0000C44343F0C043C0F8103B2B6803F00303F5 +:102F1000012B0ED1D2F8083802EB411013F4807F58 +:102F2000D0F8003B14BF43F0805343F00053C0F887 +:102F3000003B02EB4112D2F8003B43F00443C2F8DD +:102F4000003B30BD2DE9F041D0F8906005460C46BD +:102F500006EB4113D3F8087B3A07C3F8087B08D582 +:102F6000D6F814381B0704D500EB8103DB685B68D7 +:102F70009847FA071FD5D6F81438DB071BD505EBA1 +:102F80008403D968CCB98B69488A5A68B2FBF0F6D9 +:102F900000FB16228AB91868DA6890420DD2121A1C +:102FA000C3E90024302383F3118821462846FFF724 +:102FB0008BFF84F31188BDE8F081012303FA04F24A +:102FC0006B8923EA02036B81CB68002BF3D0214687 +:102FD0002846BDE8F041184700EB81034A0170B56F +:102FE000DD68D0F890306C692668E66056BB1A44FC +:102FF0004FF40020C2F810092A6802F00302012AE7 +:103000000AB20ED1D3F8080803EB421410F4807F03 +:10301000D4F8000914BF40F0805040F00050C4F8CC +:10302000000903EB4212D2F8000940F00440C2F854 +:1030300000090122D3F8340802FA01F10143C3F870 +:10304000341870BD19B9402E84BF4020206020681C +:103050001A442E8A8419013CB4FBF6F440EAC440B9 +:1030600040F00050C6E700002DE9F843D0F890602A +:1030700005460C464F0106EB4113D3F8088918F0BA +:10308000010FC3F808891CD0D6F81038DB0718D513 +:1030900000EB8103D3F80CC0DCF81430D3F800E067 +:1030A000DA68964530D2A2EB0E024FF000091A60A2 +:1030B000C3F80490302383F31188FFF78DFF89F361 +:1030C000118818F0800F1DD0D6F834380126A6409C +:1030D000334217D005EB84030134D5F89050D3F870 +:1030E0000CC0E4B22F44DCF8142005EB0434D2F811 +:1030F00000E05168714514D3D5F8343823EA060648 +:10310000C5F83468BDE8F883012303FA01F20389A6 +:1031100023EA02030381DCF80830002BD1D0984762 +:10312000CFE7AEEB0103BCF81000834228BF034693 +:10313000D7F8180980B2B3EB800FE3D89068A0F1FC +:10314000040959F8048FC4F80080A0EB09089844DA +:10315000B8F1040FF5D818440B4490605360C8E7E9 +:103160002DE9F84FD0F8905004466E69AB691E40C7 +:1031700016F480586E6103D0BDE8F84FFEF76ABCC4 +:10318000002E12DAD5F8003E9B0705D0D5F8003E98 +:1031900023F00303C5F8003ED5F80438204623F099 +:1031A0000103C5F80438FEF783FC370505D5204632 +:1031B000FFF772FC2046FEF769FCB0040CD5D5F889 +:1031C000083813F0060FEB6823F470530CBF43F478 +:1031D000105343F4A053EB6031071BD56368DB68E1 +:1031E0001BB9AB6923F00803AB612378052B0CD125 +:1031F000D5F8003E9A0705D0D5F8003E23F003032A +:10320000C5F8003E2046FEF753FC6368DB680BB14F +:1032100020469847F30200F1BA80B70226D5D4F8C9 +:10322000909000274FF0010A09EB4712D2F8003BBB +:1032300003F44023B3F5802F11D1D2F8003B002BCB +:103240000DDA62890AFA07F322EA0303638104EBC9 +:103250008703DB68DB6813B139462046984701379E +:10326000D4F89430FFB29B689F42DDD9F00619D59F +:10327000D4F89000026AC2F30A1702F00F0302F4B6 +:10328000F012B2F5802F00F0CA80B2F5402F09D1BC +:1032900004EB8303002200F58050DB681B6A974231 +:1032A00040F0B0803003D5F8185835D5E90303D580 +:1032B00000212046FFF746FEAA0303D50121204640 +:1032C000FFF740FE6B0303D502212046FFF73AFECD +:1032D0002F0303D503212046FFF734FEE80203D570 +:1032E00004212046FFF72EFEA90203D50521204622 +:1032F000FFF728FE6A0203D506212046FFF722FECB +:103300002B0203D507212046FFF71CFEEF0103D552 +:1033100008212046FFF716FE700340F1A780E90759 +:1033200003D500212046FFF79FFEAA0703D5012100 +:103330002046FFF799FE6B0703D502212046FFF7D1 +:1033400093FE2F0703D503212046FFF78DFEEE06DF +:1033500003D504212046FFF787FEA80603D50521E3 +:103360002046FFF781FE690603D506212046FFF7B8 +:103370007BFE2A0603D507212046FFF775FEEB05E5 +:1033800074D520460821BDE8F84FFFF76DBED4F88C +:1033900090904FF0000B4FF0010AD4F894305FFA90 +:1033A0008BF79B689F423FF638AF09EB4713D3F882 +:1033B000002902F44022B2F5802F20D1D3F8002951 +:1033C000002A1CDAD3F8002942F09042C3F8002901 +:1033D000D3F80029002AFBDB3946D4F89000FFF728 +:1033E00087FB22890AFA07F322EA0303238104EB0D +:1033F0008703DB689B6813B13946204698470BF179 +:10340000010BCAE7910701D1D0F80080072A02F129 +:1034100001029CBF03F8018B4FEA18283FE704EB39 +:10342000830300F58050DA68D2F818C0DCF8082071 +:10343000DCE9001CA1EB0C0C00218F4208D1DB68F9 +:103440009B699A683A449A605A683A445A6029E7F4 +:1034500011F0030F01D1D0F800808C4501F101017A +:1034600084BF02F8018B4FEA1828E6E7BDE8F88F21 +:1034700008B50348FFF774FEBDE8084000F07ABBCA +:10348000F445002008B50348FFF76AFEBDE8084090 +:1034900000F070BB90460020D0F8903003EB411153 +:1034A000D1F8003B43F40013C1F8003B7047000023 +:1034B000D0F8903003EB4111D1F8003943F40013F8 +:1034C000C1F8003970470000D0F8903003EB41118B +:1034D000D1F8003B23F40013C1F8003B7047000013 +:1034E000D0F8903003EB4111D1F8003923F40013E8 +:1034F000C1F8003970470000090100F16043012262 +:1035000003F56143C9B283F8001300F01F039A402A +:1035100043099B0003F1604303F56143C3F8802135 +:103520001A60704730B50433039C0172002104FB1C +:103530000325C160C0E90653049B0363059BC0E9F2 +:103540000000C0E90422C0E90842C0E90A1143634F +:1035500030BD00000022416AC260C0E90411C0E928 +:103560000A226FF00101FEF7ADBD0000D0E9043280 +:10357000934201D1C2680AB9181D704700207047F4 +:10358000036919600021C2680132C260C269134434 +:1035900082699342036124BF436A0361FEF786BDDB +:1035A00038B504460D46E3683BB162690020131D3F +:1035B0001268A3621344E36207E0237A33B9294611 +:1035C0002046FEF763FD0028EDDA38BD6FF00100FC +:1035D000FBE70000C368C269013BC3604369134451 +:1035E00082699342436124BF436A4361002383623B +:1035F000036B03B11847704770B53023044683F35B +:103600001188866A3EB9FFF7CBFF054618B186F3ED +:103610001188284670BDA36AE26A13F8015B9342E1 +:10362000A36202D32046FFF7D5FF002383F311885E +:10363000EFE700002DE9F84F04460E46174698467E +:103640004FF0300989F311880025AA46D4F828B034 +:10365000BBF1000F09D141462046FFF7A1FF20B181 +:103660008BF311882846BDE8F88FD4E90A12A7EB3E +:10367000050B521A934528BF9346BBF1400F1BD947 +:10368000334601F1400251F8040B914243F8040B18 +:10369000F9D1A36A403640354033A362D4E90A2306 +:1036A0009A4202D32046FFF795FF8AF31188BD4264 +:1036B000D8D289F31188C9E730465A46FDF7E6FBB0 +:1036C000A36A5E445D445B44A362E7E710B5029CD5 +:1036D0000433017203FB0421C460C0E90613002314 +:1036E000C0E90A33039B0363049BC0E90000C0E9FF +:1036F0000422C0E90842436310BD0000026A6FF073 +:103700000101C260426AC0E904220022C0E90A2223 +:10371000FEF7D8BCD0E904239A4201D1C26822B98D +:10372000184650F8043B0B60704700231846FAE730 +:10373000C3680021C2690133C360436913448269CD +:103740009342436124BF436A4361FEF7AFBC00006C +:1037500038B504460D46E3683BB1236900201A1DC5 +:10376000A262E2691344E36207E0237A33B929468F +:103770002046FEF78BFC0028EDDA38BD6FF0010023 +:10378000FBE7000003691960C268013AC260C269C0 +:10379000134482699342036124BF436A0361002397 +:1037A0008362036B03B118477047000070B5302384 +:1037B0000D460446114683F31188866A2EB9FFF739 +:1037C000C7FF10B186F3118870BDA36A1D70A36A8C +:1037D000E26A01339342A36204D3E16920460439CB +:1037E000FFF7D0FF002080F31188EDE72DE9F84FB7 +:1037F00004460D46904699464FF0300A8AF31188E8 +:103800000026B346A76A4FB949462046FFF7A0FFF6 +:1038100020B187F311883046BDE8F88FD4E90A0754 +:103820003A1AA8EB0607974228BF1746402F1BD924 +:1038300005F1400355F8042B9D4240F8042BF9D1C3 +:10384000A36A40364033A362D4E90A239A4204D3E0 +:10385000E16920460439FFF795FF8BF3118846454F +:10386000D9D28AF31188CDE729463A46FDF70EFBF7 +:10387000A36A3D443E443B44A362E5E7D0E9042308 +:103880009A4217D1C3689BB1836A8BB1043B9B1AE0 +:103890000ED01360C368013BC360C3691A448369D7 +:1038A0009A42026124BF436A0361002383620123B9 +:1038B000184670470023FBE700F086B9014B586AB1 +:1038C000704700BF000C0040034B002258631A6190 +:1038D0000222DA60704700BF000C0040014B00225A +:1038E000DA607047000C0040014B5863704700BF1E +:1038F000000C0040FEE7000070B51B4B002504469D +:1039000086B058600E468562016300F00BF904F141 +:103910001003A560E562C4E904334FF0FF33C4E946 +:103920000044C4E90635FFF7C9FF2B46024604F1FF +:1039300034012046C4E9082380230C4A2565FEF79C +:103940005BFB01230A4AE0600092037568467268D7 +:103950000192B268CDE90223064BCDE90435FEF7AA +:1039600073FB06B070BD00BFB02A00209C43000866 +:10397000A1430008F5380008024AD36A1843D06210 +:10398000704700BF482800204B6843608B68836005 +:10399000CB68C3600B6943614B6903628B69436207 +:1039A0000B6803607047000008B53C4B40F2FF71A4 +:1039B0003B48D3F888200A43C3F88820D3F88820EE +:1039C00022F4FF6222F00702C3F88820D3F888208F +:1039D000D3F8E0200A43C3F8E020D3F808210A43D3 +:1039E000C3F808212F4AD3F808311146FFF7CCFF5E +:1039F00000F5806002F11C01FFF7C6FF00F5806052 +:103A000002F13801FFF7C0FF00F5806002F15401B8 +:103A1000FFF7BAFF00F5806002F17001FFF7B4FF15 +:103A200000F5806002F18C01FFF7AEFF00F58060C9 +:103A300002F1A801FFF7A8FF00F5806002F1C401C0 +:103A4000FFF7A2FF00F5806002F1E001FFF79CFFA5 +:103A500000F5806002F1FC01FFF796FF02F58C7122 +:103A600000F58060FFF790FF00F076F90E4BD3F879 +:103A7000902242F00102C3F89022D3F8942242F03F +:103A80000102C3F894220522C3F898204FF0605237 +:103A9000C3F89C20054AC3F8A02008BD0044025882 +:103AA00000000258A843000800ED00E01F000803D2 +:103AB00008B500F033FBFEF77DFA0F4BD3F8DC209E +:103AC00042F04002C3F8DC20D3F8042122F0400287 +:103AD000C3F80421D3F80431084B1A6842F00802F5 +:103AE0001A601A6842F004021A60FEF721FFBDE86E +:103AF0000840FEF7D3BC00BF00440258001802483B +:103B000070470000EFF30983054968334A6B22F0E0 +:103B100001024A6383F30988002383F31188704705 +:103B200000EF00E0302080F3118862B60D4B0E4AA2 +:103B3000D96821F4E0610904090C0A430B49DA60F1 +:103B4000D3F8FC2042F08072C3F8FC20084AC2F887 +:103B5000B01F116841F0010111602022DA7783F86B +:103B60002200704700ED00E00003FA0555CEACC519 +:103B7000001000E0302310B583F311880E4B5B6812 +:103B800013F4006314D0F1EE103AEFF309844FF010 +:103B90008073683CE361094BDB6B236684F309881F +:103BA000FEF7E8F910B1064BA36110BD054BFBE72A +:103BB00083F31188F9E700BF00ED00E000EF00E0BB +:103BC0002F04000832040008114BD3F8E82042F01B +:103BD0000802C3F8E820D3F8102142F00802C3F825 +:103BE00010210C4AD3F81031D36B43F00803D36390 +:103BF000C722094B9A624FF0FF32DA6200229A61C3 +:103C00005A63DA605A6001225A611A60704700BF35 +:103C1000004402580010005C000C0040094A08B53E +:103C20001169D3680B40D9B29B076FEA010111619A +:103C300007D5302383F31188FEF7D2F9002383F3ED +:103C4000118808BD000C0040064BD3F8DC2002436D +:103C5000C3F8DC20D3F804211043C3F80401D3F8DF +:103C600004317047004402583A4B4FF0FF31D3F80B +:103C7000802062F00042C3F88020D3F8802002F058 +:103C80000042C3F88020D3F88020D3F88420C3F802 +:103C90008410D3F884200022C3F88420D3F8840051 +:103CA000D86F40F0FF4040F4FF0040F4DF4040F0A8 +:103CB0007F00D867D86F20F0FF4020F4FF0020F489 +:103CC000DF4020F07F00D867D86FD3F888006FEA14 +:103CD00040506FEA5050C3F88800D3F88800C0F312 +:103CE0000A00C3F88800D3F88800D3F89000C3F81E +:103CF0009010D3F89000C3F89020D3F89000D3F838 +:103D00009400C3F89410D3F89400C3F89420D3F827 +:103D10009400D3F89800C3F89810D3F89800C3F82B +:103D20009820D3F89800D3F88C00C3F88C10D3F8FF +:103D30008C00C3F88C20D3F88C00D3F89C00C3F817 +:103D40009C10D3F89C10C3F89C20D3F89C3000F052 +:103D5000B9B900BF0044025808B50122534BC3F85B +:103D60000821534BD3F8F42042F00202C3F8F420A8 +:103D7000D3F81C2142F00202C3F81C210222D3F81E +:103D80001C314C4BDA605A689104FCD54A4A1A60DF +:103D900001229A60494ADA6000221A614FF44042D7 +:103DA0009A61444B9A699204FCD51A6842F4807275 +:103DB0001A603F4B1A6F12F4407F04D04FF48032E8 +:103DC0001A6700221A671A6842F001021A60384B1B +:103DD0001A685007FCD500221A611A6912F03802DD +:103DE000FBD1012119604FF0804159605A67344A74 +:103DF000DA62344A1A611A6842F480321A602C4B33 +:103E00001A689103FCD51A6842F480521A601A6845 +:103E10009204FCD52C4A2D499A6200225A631963F8 +:103E200001F57C01DA6301F2E71199635A64284ACB +:103E30001A64284ADA621A6842F0A8521A601C4BC7 +:103E40001A6802F02852B2F1285FF9D148229A612B +:103E50004FF48862DA6140221A621F4ADA641F4A0C +:103E60001A651F4A5A651F4A9A6532231E4A136013 +:103E7000136803F00F03022BFAD10D4A136943F0C4 +:103E800003031361136903F03803182BFAD14FF0C1 +:103E90000050FFF7D9FE4FF08040FFF7D5FE4FF0FE +:103EA0000040BDE80840FFF7CFBE00BF00800051D2 +:103EB000004402580048025800C000F0020000010F +:103EC0000000FF01008890082220400063020901E1 +:103ED000470E0508DD0BBF01200000200000011087 +:103EE0000910E00000010110002000524FF0B04224 +:103EF00008B5D2F8883003F00103C2F8883023B146 +:103F0000044A13680BB150689847BDE80840FFF7B2 +:103F100031BE00BF444700204FF0B04208B5D2F890 +:103F2000883003F00203C2F8883023B1044A936852 +:103F30000BB1D0689847BDE80840FFF71BBE00BF33 +:103F4000444700204FF0B04208B5D2F8883003F063 +:103F50000403C2F8883023B1044A13690BB15069D5 +:103F60009847BDE80840FFF705BE00BF4447002062 +:103F70004FF0B04208B5D2F8883003F00803C2F819 +:103F8000883023B1044A93690BB1D0699847BDE8E2 +:103F90000840FFF7EFBD00BF444700204FF0B0429C +:103FA00008B5D2F8883003F01003C2F8883023B186 +:103FB000044A136A0BB1506A9847BDE80840FFF7FE +:103FC000D9BD00BF444700204FF0B04310B5D3F82F +:103FD000884004F47872C3F88820A30604D5124AF6 +:103FE000936A0BB1D06A9847600604D50E4A136BEA +:103FF0000BB1506B9847210604D50B4A936B0BB15C +:10400000D06B9847E20504D5074A136C0BB1506C8E +:104010009847A30504D5044A936C0BB1D06C98471C +:10402000BDE81040FFF7A6BD444700204FF0B04365 +:1040300010B5D3F8884004F47C42C3F888206205A8 +:1040400004D5164A136D0BB1506D9847230504D55E +:10405000124A936D0BB1D06D9847E00404D50F4A16 +:10406000136E0BB1506E9847A10404D50B4A936EA2 +:104070000BB1D06E9847620404D5084A136F0BB198 +:10408000506F9847230404D5044A936F0BB1D06F47 +:104090009847BDE81040FFF76DBD00BF44470020C2 +:1040A00008B50348FDF772F8BDE80840FFF762BDA8 +:1040B000D823002008B5FFF7B1FDBDE80840FFF7A1 +:1040C00059BD0000062108B50846FFF715FA06217C +:1040D0000720FFF711FA06210820FFF70DFA062145 +:1040E0000920FFF709FA06210A20FFF705FA062141 +:1040F0001720FFF701FA06212820FFF7FDF9092113 +:104100007A20FFF7F9F907213220FFF7F5F90C21A2 +:104110002520BDE80840FFF7EFB9000008B5FFF71C +:10412000A3FD00F00DF8FDF749FAFDF721FCFDF7BE +:10413000F3FAFFF7E5FCBDE80840FFF7BDBB000060 +:104140000023054A19460133102BC2E9001102F180 +:104150000802F8D1704700BF4447002010B501396C +:104160000244904201D1002005E0037811F8014F8C +:10417000A34201D0181B10BD0130F2E7034611F82D +:10418000012B03F8012B002AF9D1704753544D330A +:104190003248373F3F3F0053544D3332483733782E +:1041A0002F3732780053544D3332483734332F375A +:1041B00035332F373530000001105A0003105900F5 +:1041C0000120580003205600009600000000000067 +:1041D00000000000000000000000000000000000DF +:1041E000000000007D15000869150008A5150008ED +:1041F000911500089D15000889150008751500081F +:1042000061150008B11500080000000095160008AF +:1042100081160008BD160008A9160008B51600088A +:10422000A11600088D16000879160008C9160008A6 +:1042300000000000010000000000000063300000EA +:104240003C420008A0280020B02A00204169727674 +:104250006F6C7574650025424F415244252D424CC8 +:10426000002553455249414C250000000200000042 +:1042700000000000B51800082519000840004000A3 +:104280006043002070430020020000000000000096 +:1042900003000000000000006D190008000000008D +:1042A000100000008043002000000000010000001A +:1042B00000000000F445002001010200392400083C +:1042C00049230008E5230008C92300084300000033 +:1042D000D442000809024300020100C03209040070 +:1042E0000001020201000524001001052401000163 +:1042F000042402020524060001070582030800FFCA +:1043000009040100020A0000000705010240000044 +:104310000705810240000000120000002043000851 +:10432000120110010200004009124157000201026F +:10433000030100000403090425424F4152442500B3 +:10434000416972766F6C7574652D44435332003049 +:1043500031323334353637383941424344454600EB +:1043600000000000C91A0008811D00082D1E000869 +:10437000400040002C4700202C4700200100000096 +:104380003C470020800000004001000008000000C1 +:104390000001000000100000080000006D61696E5F +:1043A0000069646C650000000000A02A00020000A3 +:1043B000AAAAAAAA00001024FFFF00000000000023 +:1043C00000A70A000400002000000000AAAAAAAA70 +:1043D00000000010FDFF00000000000000000004CD +:1043E0000000000000000000AAAAAAAA0000000025 +:1043F000FFFF00000000000000000000000050551A +:1044000000000000AAAAAAAA00005055FFFF000061 +:104410000000000000000000000000040000000098 +:10442000AAAAAAAA00000004FFFF000000000000E2 +:10443000000000000000000000000000AAAAAAAAD4 +:1044400000000000FFFF000000000000000000006E +:104450000000000000000000AAAAAAAA00000000B4 +:10446000FFFF00000000000000000000000000004E +:1044700000000000AAAAAAAA00000000FFFF000096 +:10448000000000000000000000000000000000002C +:10449000AAAAAAAA00000000FFFF00000000000076 +:1044A000000000000000000000000000AAAAAAAA64 +:1044B00000000000FFFF00000000000000000000FE +:1044C0000000000000000000AAAAAAAA0000000044 +:1044D000FFFF0000000000000000000000000000DE +:1044E000501400000000000000001A00000000004E +:1044F000FF000000B82A0020D823002000000000A0 +:104500008C4100088304000097410008500400001B +:10451000A541000800960000000008009600000079 +:104520000008000004000000344300080000000000 +:10453000000000000000000000000000000000007B +:044540000000000077 +:00000001FF diff --git a/Tools/bootloaders/Aocoda-RC-H743Dual_bl.bin b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.bin new file mode 100755 index 00000000000000..d806e3b8f53b95 Binary files /dev/null and b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.bin differ diff --git a/Tools/bootloaders/Aocoda-RC-H743Dual_bl.hex b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.hex new file mode 100644 index 00000000000000..ecc103e918efce --- /dev/null +++ b/Tools/bootloaders/Aocoda-RC-H743Dual_bl.hex @@ -0,0 +1,1004 @@ +:020000040800F2 +:1000000000060020E1020008E3020008E302000805 +:10001000E3020008E3020008E3020008E30200082C +:10002000E3020008E3020008E3020008BD34000810 +:10003000E3020008E3020008E3020008E30200080C +:10004000E3020008E3020008E3020008E3020008FC +:10005000E3020008E3020008A5380008D1380008D0 +:10006000FD3800082939000855390008E302000866 +:10007000E3020008E3020008E3020008E3020008CC +:10008000E3020008E3020008E3020008E3020008BC +:10009000E3020008E3020008E302000881390008D7 +:1000A000E3020008E3020008E3020008E30200089C +:1000B000E3020008E3020008E3020008E30200088C +:1000C000E3020008E3020008E3020008E30200087C +:1000D000E3020008E3020008E3020008E30200086C +:1000E000E5390008E3020008E3020008E302000823 +:1000F000E3020008E3020008E3020008E30200084C +:10010000E3020008E3020008593A0008E30200088D +:10011000E3020008E3020008E3020008E30200082B +:10012000E3020008E3020008E3020008E30200081B +:10013000E3020008E3020008E3020008E30200080B +:10014000E3020008E3020008E3020008E3020008FB +:10015000E3020008E3020008E3020008E3020008EB +:10016000E3020008E3020008E3020008E3020008DB +:10017000E30200083D2E0008E3020008E302000845 +:10018000E3020008E3020008E3020008E3020008BB +:10019000E3020008E3020008E3020008E3020008AB +:1001A000E3020008E3020008E3020008E30200089B +:1001B000E3020008E3020008E3020008E30200088B +:1001C000E3020008E3020008E3020008E30200087B +:1001D000E3020008292E0008E3020008E3020008F9 +:1001E000E3020008E3020008E3020008E30200085B +:1001F000E3020008E3020008E3020008E30200084B +:10020000E3020008E3020008E3020008E30200083A +:10021000E3020008E3020008E3020008E30200082A +:10022000E3020008E3020008E3020008E30200081A +:10023000E3020008E3020008E3020008E30200080A +:10024000E3020008E3020008E3020008E3020008FA +:10025000E3020008E3020008E3020008E3020008EA +:10026000E3020008E3020008E3020008E3020008DA +:10027000E3020008E3020008E3020008E3020008CA +:10028000E3020008E3020008E3020008E3020008BA +:10029000E3020008E3020008E3020008E3020008AA +:1002A000E3020008E3020008E3020008E30200089A +:1002B000E3020008E3020008E3020008E30200088A +:1002C000E3020008E3020008E3020008E30200087A +:1002D000E3020008E3020008E3020008E30200086A +:1002E00002E000F000F8FEE772B6374880F30888B5 +:1002F000364880F3098836483649086040F20000E5 +:10030000CCF200004EF63471CEF200010860BFF36B +:100310004F8FBFF36F8F40F20000C0F2F0004EF637 +:100320008851CEF200010860BFF34F8FBFF36F8F8B +:100330004FF00000E1EE100A4EF63C71CEF20001E3 +:100340000860062080F31488BFF36F8F01F0E2FF8E +:1003500003F006F84FF055301F491B4A91423CBF4D +:1003600041F8040BFAE71D49184A91423CBF41F895 +:10037000040BFAE71A491B4A1B4B9A423EBF51F83D +:10038000040B42F8040BF8E700201849184A914280 +:100390003CBF41F8040BFAE701F0FAFF03F064F800 +:1003A000144C154DAC4203DA54F8041B8847F9E7A6 +:1003B00000F042F8114C124DAC4203DA54F8041B21 +:1003C0008847F9E701F0E2BF000600200022002084 +:1003D0000000000808ED00E00000002000060020FA +:1003E000383E0008002200205C220020602200200D +:1003F00034430020E0020008E0020008E0020008A8 +:10040000E00200082DE9F04F2DED108AC1F80CD064 +:10041000D0F80CD0BDEC108ABDE8F08F002383F338 +:1004200011882846A047002001F0FAFAFEE701F003 +:1004300089FA00DFFEE7000038B500F0DBFB01F0D1 +:1004400029FF054601F05CFF0446D0B90F4B9D42E1 +:1004500019D001339D4241F2883512BF0446002570 +:100460000124002001F020FF0CB100F077F800F02B +:1004700073FD00F025FC284600F01EF900F06EF830 +:10048000F9E70025EDE70546EBE700BF010007B0FF +:1004900008B500F0E7FBA0F120035842584108BD21 +:1004A00007B541F21203022101A8ADF8043000F0B3 +:1004B000F7FB03B05DF804FB38B5302383F31188F4 +:1004C000174803680BB101F077FB0023154A4FF47E +:1004D0007A71134801F066FB002383F31188124CF4 +:1004E000236813B12368013B2360636813B1636819 +:1004F000013B63600D4D2B7833B963687BB90220F3 +:1005000000F0A4FC322363602B78032B07D16368CF +:100510002BB9022000F09AFC4FF47A73636038BD67 +:1005200060220020B90400088023002078220020E7 +:10053000084B187003280CD8DFE800F00805020803 +:10054000022000F07BBC022000F06EBC024B0022B7 +:100550005A6070477822002080230020F8B5504B65 +:10056000504A1C461968013100F0998004339342C7 +:10057000F8D162684C4B9A4240F291804B4B9B6899 +:1005800003F1006303F500339A4280F08880002075 +:1005900000F0BCFB0220FFF7CBFF454B0021D3F856 +:1005A000E820C3F8E810D3F81021C3F81011D3F8ED +:1005B0001021D3F8EC20C3F8EC10D3F81421C3F8C1 +:1005C0001411D3F81421D3F8F020C3F8F010D3F8A5 +:1005D0001821C3F81811D3F81821D3F8802042F05D +:1005E0000062C3F88020D3F8802022F00062C3F8B4 +:1005F0008020D3F88020D3F8802042F00072C3F826 +:100600008020D3F8802022F00072C3F88020D3F835 +:10061000803072B64FF0E023C3F8084DD4E90004EF +:10062000BFF34F8FBFF36F8F224AC2F88410BFF31E +:100630004F8F536923F480335361BFF34F8FD2F848 +:10064000803043F6E076C3F3C905C3F34E335B0154 +:1006500003EA060C29464CEA81770139C2F8747224 +:10066000F9D2203B13F1200FF2D1BFF34F8FBFF32C +:100670006F8FBFF34F8FBFF36F8F536923F4003336 +:1006800053610023C2F85032BFF34F8FBFF36F8F17 +:10069000302383F31188854680F308882047F8BD0E +:1006A0000000020820000208FFFF010800220020CD +:1006B0000044025800ED00E02DE9F04F93B0B44B38 +:1006C0002022FF2100900AA89D6800F0EFFBB14AAC +:1006D0001378A3B90121B04811700360302383F36C +:1006E000118803680BB101F067FA0023AB4A4FF49D +:1006F0007A71A94801F056FA002383F31188009B10 +:1007000013B1A74B009A1A60A64A1378032B03D0A3 +:1007100000231370A24A53604FF0000A009CD34696 +:100720005646D146012000F089FB24B19C4B1B6842 +:10073000002B00F02682002000F094FA0390039B27 +:10074000002BF2DB012000F06FFB039B213B1F2BF2 +:10075000E8D801A252F823F0D907000801080008E0 +:100760009508000825070008250700082507000848 +:1007700027090008F70A0008110A0008730A000890 +:100780009B0A0008C10A000825070008D30A0008D0 +:1007900025070008450B0008790800082507000810 +:1007A000890B0008E50700087908000825070008FC +:1007B000730A000825070008250700082507000818 +:1007C0002507000825070008250700082507000859 +:1007D00025070008950800080220FFF759FE0028A9 +:1007E00040F0F981009B022105A8BAF1000F08BF73 +:1007F0001C4641F21233ADF8143000F051FA91E783 +:100800004FF47A7000F02EFA071EEBDB0220FFF7A0 +:100810003FFE0028E6D0013F052F00F2DE81DFE831 +:1008200007F0030A0D1013360523042105A80593CC +:1008300000F036FA17E004215548F9E704215A4838 +:10084000F6E704215948F3E74FF01C08404608F149 +:10085000040800F05DFA0421059005A800F020FAD4 +:10086000B8F12C0FF2D101204FF0000900FA07F780 +:1008700047EA0B0B5FFA8BFB00F064FB26B10BF031 +:100880000B030B2B08BF0024FFF70AFE4AE70421E5 +:100890004748CDE7002EA5D00BF00B030B2BA1D1C1 +:1008A0000220FFF7F5FD074600289BD00120002617 +:1008B00000F02CFA0220FFF73BFE1FFA86F84046B4 +:1008C00000F034FA0446B0B1039940460136A1F174 +:1008D00040025142514100F039FA0028EDD1BA46A8 +:1008E000044641F21213022105A83E46ADF8143029 +:1008F00000F0D6F916E725460120FFF719FE244B34 +:100900009B68AB4207D9284600F002FA013040F05C +:1009100067810435F3E70025224BBA463E461D7039 +:100920001F4B5D60A8E7002E3FF45CAF0BF00B039C +:100930000B2B7FF457AF0220FFF7FAFD322000F0B7 +:1009400091F9B0F10008FFF64DAF18F003077FF4FE +:1009500049AF0F4A08EB0503926893423FF642AF56 +:10096000B8F5807F3FF73EAF124BB845019323DDCA +:100970004FF47A7000F076F90390039A002AFFF69C +:1009800031AF039A0137019B03F8012BEDE700BF5C +:10099000002200207C23002060220020B9040008EF +:1009A000802300207822002004220020082200203A +:1009B0000C2200207C220020C820FFF769FD07469A +:1009C00000283FF40FAF1F2D11D8C5F120020AAB4C +:1009D00025F0030084494245184428BF424601924D +:1009E00000F03EFA019AFF217F4800F05FFA4FEADB +:1009F000A803C8F387027C492846019300F05EFAF9 +:100A0000064600283FF46DAF019B05EB830533E7F5 +:100A10000220FFF73DFD00283FF4E4AE00F0B4F9FA +:100A200000283FF4DFAE0027B846704B9B68BB42FE +:100A300018D91F2F11D80A9B01330ED027F00303BA +:100A400012AA134453F8203C05934046042205A9FA +:100A5000043700F0ADFA8046E7E7384600F058F971 +:100A60000590F2E7CDF81480042105A800F018F9EC +:100A700002E70023642104A8049300F007F900288A +:100A80007FF4B0AE0220FFF703FD00283FF4AAAECA +:100A9000049800F06FF90590E6E70023642104A8AC +:100AA000049300F0F3F800287FF49CAE0220FFF7D7 +:100AB000EFFC00283FF496AE049800F05DF9EAE7F9 +:100AC0000220FFF7E5FC00283FF48CAE00F06CF943 +:100AD000E1E70220FFF7DCFC00283FF483AE05A924 +:100AE000142000F067F907460421049004A800F0E0 +:100AF000D7F83946B9E7322000F0B4F8071EFFF600 +:100B000071AEBB077FF46EAE384A07EB09039268FB +:100B100093423FF667AE0220FFF7BAFC00283FF48D +:100B200061AE27F003074F44B9453FF4A5AE4846F0 +:100B300009F1040900F0ECF80421059005A800F083 +:100B4000AFF8F1E74FF47A70FFF7A2FC00283FF40A +:100B500049AE00F019F9002844D00A9B01330BD0AC +:100B600008220AA9002000F0A9F900283AD0202282 +:100B7000FF210AA800F09AF9FFF792FC1C4800F048 +:100B800055FF13B0BDE8F08F002E3FF42BAE0BF0F5 +:100B90000B030B2B7FF426AE0023642105A80593DD +:100BA00000F074F8074600287FF41CAE0220FFF71F +:100BB0006FFC804600283FF415AEFFF771FC41F250 +:100BC000883000F033FF059800F0F0F946463C46C7 +:100BD00000F0B8F9A6E506464EE64FF0000901E63A +:100BE000BA467EE637467CE67C22002000220020C2 +:100BF000A0860100704700000F4B70B51B780C46B3 +:100C00000133DBB2012B11D80C4D4FF47A732968F4 +:100C1000A2FB033222460E6A01462846B0478442B0 +:100C200004D1074B002201201A7070BD4FF4FA70F6 +:100C300000F0FCFE0020F8E710220020282600200B +:100C4000B423002070B504464FF47A76412C254633 +:100C500028BF412506FB05F000F0E8FE641BF5D136 +:100C600070BD0000002307B5024601210DF1070009 +:100C70008DF80730FFF7C0FF20B19DF8070003B0E3 +:100C80005DF804FB4FF0FF30F9E700000A4604214D +:100C900008B5FFF7B1FF80F00100C0B2404208BDC7 +:100CA00030B4054C0A46014623682046DD69034BF3 +:100CB000AC4630BC604700BF28260020A08601005B +:100CC00070B5104C0025104E01F0CCF92080306832 +:100CD000238883420CD800252088013801F0BEF912 +:100CE00023880544013BB5F5802F2380F4D370BDE4 +:100CF00001F0B4F9336805440133B5F5003F3360C2 +:100D0000E5D3E8E7B62300208823002001F078BA75 +:100D100000F1006000F500300068704700F10060ED +:100D2000920000F5003001F0F9B90000054B1A6897 +:100D3000054B1B889B1A834202D9104401F08EB9DF +:100D40000020704788230020B623002038B50446D1 +:100D5000074D29B128682044BDE8384001F096B914 +:100D60002868204401F080F90028F3D038BD00BF86 +:100D7000882300200020704700F1FF5000F58F10FD +:100D8000D0F8000870470000064991F8243033B1CC +:100D900000230822086A81F82430FFF7BFBF012032 +:100DA000704700BF8C230020014B1868704700BFBC +:100DB0000010005C194B01380322084470B51D680F +:100DC000174BC5F30B042D0C1E88A6420BD15C6893 +:100DD0000A46013C824213460FD214F9016F4EB10C +:100DE00002F8016BF6E7013A03F10803ECD1814206 +:100DF0000B4602D22C2203F8012B0424094A168840 +:100E0000AE4204D1984284BF967803F8016B013C4E +:100E100002F10402F3D1581A70BD00BF0010005C4B +:100E200014220020503B0008022803D1024B4FF44B +:100E300000229A61704700BF00100258022802D1B8 +:100E4000014B08229A61704700100258022804D111 +:100E5000024A536983F00803536170470010025837 +:100E6000002310B5934203D0CC5CC4540133F9E79E +:100E700010BD0000013810B510F9013F3BB191F9E8 +:100E800000409C4203D11AB10131013AF4E71AB192 +:100E900091F90020981A10BD1046FCE7034602465F +:100EA000D01A12F9011B0029FAD1704702440346F7 +:100EB000934202D003F8011BFAE770472DE9F8438B +:100EC0001F4D14460746884695F8242052BBDFF88C +:100ED00070909CB395F824302BB92022FF2148460E +:100EE0002F62FFF7E3FF95F824004146C0F10802A6 +:100EF00005EB8000A24228BF2246D6B29200FFF73F +:100F0000AFFF95F82430A41B17441E449044E4B26C +:100F1000F6B2082E85F82460DBD1FFF735FF0028F4 +:100F2000D7D108E02B6A03EB82038342CFD0FFF7CF +:100F30002BFF0028CBD10020BDE8F8830120FBE780 +:100F40008C230020024B1A78024B1A70704700BFA6 +:100F5000B42300201022002010B5114C114800F0DD +:100F6000B9F821460F4800F0E1F824684FF47A7090 +:100F7000D4F89020D2F8043843F00203C2F80438C1 +:100F8000FFF760FE0849204600F0DEF9D4F8902013 +:100F9000D2F8043823F00203C2F8043810BD00BFB1 +:100FA0000C3C000828260020143C00087047000074 +:100FB00030B50A44084D91420DD011F8013B58401C +:100FC000082340F30004013B2C4013F0FF0384EAA4 +:100FD0005000F6D1EFE730BD2083B8ED02684368DA +:100FE0001143016003B118477047000013B5406B0F +:100FF00000F58054D4F8A4381A681178042914D163 +:10100000017C022911D11979012312898B401342E5 +:101010000BD101A94C3002F059F8D4F8A44802468B +:10102000019B2179206800F0DFF902B010BD0000BB +:10103000143001F0DBBF00004FF0FF33143001F03B +:10104000D5BF00004C3002F0ADB800004FF0FF33C8 +:101050004C3002F0A7B80000143001F0A9BF000026 +:101060004FF0FF31143001F0A3BF00004C3002F00C +:1010700079B800004FF0FF324C3002F073B8000036 +:101080000020704710B500F58054D4F8A4381A68D1 +:101090001178042917D1017C022914D1597901232F +:1010A00052898B4013420ED1143001F03BFF0246AF +:1010B00048B1D4F8A4484FF4407361792068BDE882 +:1010C000104000F07FB910BD406BFFF7DBBF0000A0 +:1010D000704700007FB5124B0125042604460360CB +:1010E0000023057400F1840243602946C0E90233FD +:1010F0000C4B0290143001934FF44073009601F0B2 +:10110000EDFE094B04F69442294604F14C0002948A +:10111000CDE900634FF4407301F0B4FF04B070BD3B +:10112000603B0008C9100008ED0F00080A68302372 +:1011300083F311880B790B3342F823004B79133377 +:1011400042F823008B7913B10B3342F8230000F5EA +:101150008053C3F8A41802230374002080F311887D +:101160007047000038B5037F044613B190F854303F +:10117000ABB90125201D0221FFF730FF04F1140057 +:101180006FF00101257700F079FC04F14C0084F840 +:1011900054506FF00101BDE8384000F06FBC38BD1D +:1011A00010B5012104460430FFF718FF0023237710 +:1011B00084F8543010BD000038B5044600251430C2 +:1011C00001F0A4FE04F14C00257701F073FF201D0F +:1011D00084F854500121FFF701FF2046BDE8384054 +:1011E000FFF750BF90F8803003F06003202B06D14A +:1011F00090F881200023212A03D81F2A06D8002036 +:101200007047222AFBD1C0E91D3303E0034A42673D +:1012100007228267C3670120704700BF2C2200208D +:1012200037B500F58055D5F8A4381A681178042927 +:101230001AD1017C022917D11979012312898B4017 +:10124000134211D100F14C04204601F0F3FF58B1D4 +:1012500001A9204601F03AFFD5F8A4480246019BB7 +:101260002179206800F0C0F803B030BD01F10B0314 +:10127000F0B550F8236085B004460D46FEB130232A +:1012800083F3118804EB8507301D0821FFF7A6FEC4 +:10129000FB6806F14C005B691B681BB1019001F013 +:1012A00023FF019803A901F011FF024648B1039BF7 +:1012B0002946204600F098F8002383F3118805B0F2 +:1012C000F0BDFB685A691268002AF5D01B8A013B01 +:1012D0001340F1D104F18002EAE70000133138B580 +:1012E00050F82140ECB1302383F3118804F580538A +:1012F000D3F8A4281368527903EB8203DB689B6957 +:101300005D6845B104216018FFF768FE294604F1C5 +:10131000140001F011FE2046FFF7B4FE002383F312 +:10132000118838BD7047000001F0C8B80123402281 +:10133000002110B5044600F8303BFFF7B7FD00234D +:10134000C4E9013310BD000010B53023044683F317 +:1013500011882422416000210C30FFF7A7FD2046B0 +:1013600001F0CEF802230020237080F3118810BD15 +:1013700070B500EB8103054650690E461446DA60ED +:1013800018B110220021FFF791FDA06918B11022B9 +:101390000021FFF78BFD31462846BDE8704001F083 +:1013A000B5B9000083682022002103F0011310B5B5 +:1013B000044683601030FFF779FD2046BDE81040F9 +:1013C00001F030BAF0B4012500EB810447898D406B +:1013D000E4683D43A469458123600023A260636003 +:1013E000F0BC01F04DBA0000F0B4012500EB81041F +:1013F00007898D40E4683D436469058123600023CB +:10140000A2606360F0BC01F0C3BA000070B50223B3 +:1014100000250446242203702946C0F888500C3069 +:1014200040F8045CFFF742FD204684F8705001F05C +:1014300001F963681B6823B129462046BDE8704066 +:10144000184770BD0378052B10B504460AD080F804 +:101450008C300523037043681B680BB10421984747 +:101460000023A36010BD00000178052906D190F883 +:101470008C20436802701B6803B118477047000056 +:1014800070B590F87030044613B1002380F87030C6 +:1014900004F18002204601F0E9F963689B68B3B962 +:1014A00094F8803013F0600535D00021204601F01B +:1014B000DBFC0021204601F0CBFC63681B6813B104 +:1014C000062120469847062384F8703070BD2046D8 +:1014D00098470028E4D0B4F88630A26F9A4288BFBB +:1014E000A36794F98030A56F002B4FF0300380F292 +:1014F0000381002D00F0F280092284F8702083F32C +:10150000118800212046D4E91D23FFF76DFF002339 +:1015100083F31188DAE794F8812003F07F0343EA2C +:10152000022340F20232934200F0C58021D8B3F585 +:10153000807F48D00DD8012B3FD0022B00F0938044 +:10154000002BB2D104F1880262670222A267E3672E +:10155000C1E7B3F5817F00F09B80B3F5407FA4D154 +:1015600094F88230012BA0D1B4F8883043F0020304 +:1015700032E0B3F5006F4DD017D8B3F5A06F31D07E +:10158000A3F5C063012B90D86368204694F88220AD +:101590005E6894F88310B4F88430B047002884D093 +:1015A000436863670368A3671AE0B3F5106F36D02A +:1015B00040F6024293427FF478AF5C4B63670223AC +:1015C000A3670023C3E794F88230012B7FF46DAF4B +:1015D000B4F8883023F00203A4F88830C4E91D551C +:1015E000E56778E7B4F88030B3F5A06F0ED194F8D2 +:1015F0008230204684F88A3001F07AF863681B68EC +:1016000013B1012120469847032323700023C4E926 +:101610001D339CE704F18B0363670123C3E7237841 +:10162000042B10D1302383F311882046FFF7BAFE34 +:1016300085F311880321636884F88B5021701B683F +:101640000BB12046984794F88230002BDED084F806 +:101650008B300423237063681B68002BD6D00221D3 +:1016600020469847D2E794F8843020461D0603F0C0 +:101670000F010AD501F0ECF8012804D002287FF40C +:1016800014AF2B4B9AE72B4B98E701F0D3F8F3E715 +:1016900094F88230002B7FF408AF94F8843013F074 +:1016A0000F01B3D01A06204602D501F0F5FBADE7D5 +:1016B00001F0E6FBAAE794F88230002B7FF4F5AE48 +:1016C00094F8843013F00F01A0D01B06204602D5F9 +:1016D00001F0CAFB9AE701F0BBFB97E7142284F8FC +:1016E000702083F311882B462A4629462046FFF7AF +:1016F00069FE85F31188E9E65DB1152284F8702052 +:1017000083F3118800212046D4E91D23FFF75AFEF8 +:10171000FDE60B2284F8702083F311882B462A46BD +:1017200029462046FFF760FEE3E700BF903B000834 +:10173000883B00088C3B000838B590F870300446B0 +:10174000002B3ED0063BDAB20F2A34D80F2B32D80A +:10175000DFE803F0373131082232313131313131B4 +:1017600031313737856FB0F886309D4214D2C36867 +:101770001B8AB5FBF3F203FB12556DB9302383F3DB +:1017800011882B462A462946FFF72EFE85F311883D +:101790000A2384F870300EE0142384F8703030236C +:1017A00083F31188002320461A461946FFF70AFEE4 +:1017B000002383F3118838BDC36F03B1984700231A +:1017C000E7E70021204601F04FFB0021204601F011 +:1017D0003FFB63681B6813B1062120469847062328 +:1017E000D7E7000010B590F870300446142B29D0CC +:1017F00017D8062B05D001D81BB110BD093B022B11 +:10180000FBD80021204601F02FFB0021204601F0EB +:101810001FFB63681B6813B1062120469847062307 +:1018200019E0152BE9D10B2380F87030302383F3B6 +:10183000118800231A461946FFF7D6FD002383F3CB +:101840001188DAE7C3689B695B68002BD5D1C36F49 +:1018500003B19847002384F87030CEE700238268F4 +:10186000037503691B6899689142FBD25A6803604B +:1018700042601060586070470023826803750369F6 +:101880001B6899689142FBD85A68036042601060F7 +:101890005860704708B50846302383F311880B7DE4 +:1018A000032B05D0042B0DD02BB983F3118808BD71 +:1018B0008B6900221A604FF0FF338361FFF7CEFF80 +:1018C0000023F2E7D1E9003213605A60F3E7000029 +:1018D000FFF7C4BF054BD9680875186802685360E4 +:1018E0001A600122D8600275FEF78CBDB823002073 +:1018F0000C4B30B5DD684B1C87B004460FD02B462F +:10190000094A684600F04EF92046FFF7E3FF009BC6 +:1019100013B1684600F050F9A86907B030BDFFF771 +:10192000D9FFF9E7B823002095180008044B1A687E +:10193000DB6890689B68984294BF00200120704744 +:10194000B8230020084B10B51C68D8682268536083 +:101950001A600122DC602275FFF78EFF01462046E7 +:10196000BDE81040FEF74EBDB823002038B5074C47 +:1019700001230025064907482370656001F098FCA3 +:101980000223237085F3118838BD00BF2026002074 +:10199000983B0008B823002000F044B9034A51687E +:1019A00053685B1A9842FBD8704700BF001000E0F4 +:1019B0008B600223086108468B8270478368A3F11D +:1019C000840243F8142C026943F8442C426943F81A +:1019D000402C094A43F8242CC268A3F1200043F8A4 +:1019E000182C022203F80C2C002203F80B2C034ABB +:1019F00043F8102C704700BF1D040008B8230020D6 +:101A000008B5FFF7DBFFBDE80840FFF761BF000046 +:101A1000024BDB6898610F20FFF75CBFB823002002 +:101A2000302383F31188FFF7F3BF000008B50146A8 +:101A3000302383F311880820FFF75AFF002383F334 +:101A4000118808BD064BDB6839B1426818605A60DE +:101A5000136043600420FFF74BBF4FF0FF30704727 +:101A6000B82300200368984206D01A6802605060CC +:101A700018469961FFF72CBF7047000038B504463F +:101A80000D462068844200D138BD036823605C6045 +:101A90008561FFF71DFFF4E7036810B59C68A2425B +:101AA0000CD85C688A600B604C6021605960996852 +:101AB0008A1A9A604FF0FF33836010BD121B1B68B7 +:101AC000ECE700000A2938BF0A2170B504460D462C +:101AD0000A26601901F0E4FB01F0CCFB041BA542CF +:101AE00003D8751C04462E46F3E70A2E04D90120BC +:101AF000BDE8704001F01CBC70BD0000F8B5144B8F +:101B00000D460A2A4FF00A07D96103F110018260DD +:101B100038BF0A22416019691446016048601861A3 +:101B2000A81801F0ADFB01F0A5FB431B0646A3423C +:101B300006D37C1C28192746354601F0B1FBF2E795 +:101B40000A2F04D90120BDE8F84001F0F1BBF8BD2F +:101B5000B8230020F8B506460D4601F08BFB0F4A6E +:101B6000134653F8107F9F4206D12A46014630465D +:101B7000BDE8F840FFF7C2BFD169BB68441A2C1911 +:101B800028BF2C46A34202D92946FFF79BFF2246D5 +:101B900031460348BDE8F840FFF77EBFB823002078 +:101BA000C8230020C0E90323002310B45DF8044BD0 +:101BB0004361FFF7CFBF000010B5194C236998426D +:101BC0000DD08168D0E9003213605A609A680A44E7 +:101BD0009A60002303604FF0FF33A36110BD0268D9 +:101BE000234643F8102F53600022026022699A4274 +:101BF00003D1BDE8104001F04DBB936881680B44F0 +:101C0000936001F037FB2269E1699268441AA242AD +:101C1000E4D91144BDE81040091AFFF753BF00BFD3 +:101C2000B82300202DE9F047DFF8BC8008F1100749 +:101C30002C4ED8F8105001F01DFBD8F81C40AA68B3 +:101C4000031B9A423ED814444FF00009D5E90032F4 +:101C5000C8F81C4013605A60C5F80090D8F81030DE +:101C6000B34201D101F016FB89F31188D5E90331A4 +:101C700028469847302383F311886B69002BD8D00E +:101C800001F0F8FA6A69A0EB040982464A450DD2D0 +:101C9000022001F04DFB0022D8F81030B34208D1E9 +:101CA00051462846BDE8F047FFF728BF121A2244E4 +:101CB000F2E712EB09092946384638BF4A46FFF7D2 +:101CC000EBFEB5E7D8F81030B34208D01444C8F89A +:101CD0001C00211AA960BDE8F047FFF7F3BEBDE87C +:101CE000F08700BFC8230020B823002000207047E1 +:101CF000FEE70000704700004FF0FF307047000023 +:101D000002290CD0032904D00129074818BF00205C +:101D10007047032A05D8054800EBC2007047044805 +:101D200070470020704700BF703C00083C22002034 +:101D3000243C000870B59AB005460846144601A92F +:101D400000F0C2F801A8FFF7A9F8431C0022C6B2B0 +:101D50005B001046C5E9003423700323023404F805 +:101D6000013C01ABD1B202348E4201D81AB070BD31 +:101D700013F8011B013204F8010C04F8021CF1E70E +:101D800008B5302383F311880348FFF749FA00238D +:101D900083F3118808BD00BF2826002090F880300A +:101DA00003F01F02012A07D190F881200B2A03D1EA +:101DB0000023C0E91D3315E003F06003202B08D198 +:101DC000B0F884302BB990F88120212A03D81F2A3B +:101DD00004D8FFF707BA222AEBD0FAE7034A426792 +:101DE00007228267C3670120704700BF33220020AB +:101DF00007B5052917D8DFE801F0191603191920CE +:101E0000302383F31188104A01210190FFF7B0FAC3 +:101E1000019802210D4AFFF7ABFA0D48FFF7CCF904 +:101E2000002383F3118803B05DF804FB302383F3B0 +:101E300011880748FFF796F9F2E7302383F31188FA +:101E40000348FFF7ADF9EBE7C43B0008E83B0008A7 +:101E50002826002038B50C4D0C4C2A460C4904F1BC +:101E60000800FFF767FF05F1CA0204F110000949F5 +:101E7000FFF760FF05F5CA7204F118000649BDE8D6 +:101E80003840FFF757BF00BF003F00203C22002032 +:101E9000A43B0008AE3B0008B93B000870B50446FF +:101EA00008460D46FEF7FAFFC6B220460134037815 +:101EB0000BB9184670BD32462946FEF7DBFF0028F5 +:101EC000F3D10120F6E700002DE9F04705460C4666 +:101ED000FEF7E4FF2B49C6B22846FFF7DFFF08B143 +:101EE0000B36F6B228492846FFF7D8FF08B110365E +:101EF000F6B2632E0BD8DFF88C80DFF88C90234F7E +:101F0000DFF894A02E7846B92670BDE8F087294600 +:101F10002046BDE8F04701F0FDBD252E2ED1072259 +:101F200041462846FEF7A6FF70B9194B224603F139 +:101F3000100153F8040B8B4242F8040BF9D11B88B3 +:101F4000073512341380DDE7082249462846FEF79C +:101F500091FF98B9A21C0F4B197802320909C95D8B +:101F600002F8041C13F8011B01F00F015345C95D71 +:101F700002F8031CF0D118340835C3E7013504F822 +:101F8000016BBFE7903C0008B93B0008AB3C000880 +:101F9000983C000800E8F11F0CE8F11FBFF34F8FD9 +:101FA000044B1A695107FCD1D3F810215207F8D11C +:101FB000704700BF0020005208B50D4B1B78ABB92D +:101FC000FFF7ECFF0B4BDA68D10704D50A4A5A60D9 +:101FD00002F188325A60D3F80C21D20706D5064A9E +:101FE000C3F8042102F18832C3F8042108BD00BF00 +:101FF0005E410020002000522301674508B5114BC7 +:102000001B78F3B9104B1A69510703D5DA6842F00F +:102010004002DA60D3F81021520705D5D3F80C211D +:1020200042F04002C3F80C21FFF7B8FF064BDA6814 +:1020300042F00102DA60D3F80C2142F00102C3F849 +:102040000C2108BD5E410020002000520F289ABFDD +:1020500000F5806040040020704700004FF400301D +:1020600070470000102070470F2808B50BD8FFF705 +:10207000EDFF00F500330268013204D104308342E1 +:10208000F9D1012008BD0020FCE700000F2838B579 +:1020900005463FD8FFF782FF1F4CFFF78DFF4FF03B +:1020A000FF3307286361C4F814311DD82361FFF79B +:1020B00075FF030243F02403E360E36843F0800309 +:1020C000E36023695A07FCD42846FFF767FFFFF750 +:1020D000BDFF4FF4003100F0F5F82846FFF78EFF02 +:1020E000BDE83840FFF7C0BFC4F81031FFF756FF16 +:1020F000A0F108031B0243F02403C4F80C31D4F808 +:102100000C3143F08003C4F80C31D4F810315B0774 +:10211000FBD4D9E7002038BD002000522DE9F84F4C +:1021200005460C46104645EA0203DE0602D00020B2 +:10213000BDE8F88F20F01F00DFF8BCB0DFF8BCA0CE +:10214000FFF73AFF04EB0008444503D10120FFF7F5 +:1021500055FFEDE720222946204601F0CBFC10B9BF +:1021600020352034F0E72B4605F120021F68791C4A +:10217000DDD104339A42F9D105F178431B481C4E56 +:10218000B3F5801F1B4B38BF184603F1F80332BF6D +:10219000D946D1461E46FFF701FF0760A5EB040CA8 +:1021A000336804F11C0143F002033360231FD9F8A4 +:1021B000007017F00507FAD153F8042F8B424CF842 +:1021C0000320F4D1BFF34F8FFFF7E8FE4FF0FF334A +:1021D0002022214603602846336823F0020333603F +:1021E00001F088FC0028BBD03846B0E7142100522B +:1021F0000C200052142000521020005210210052D6 +:1022000010B5084C237828B11BB9FFF7D5FE012380 +:10221000237010BD002BFCD02070BDE81040FFF7EC +:10222000EDBE00BF5E4100200244074BD2B210B5A4 +:10223000904200D110BD441C00B253F8200041F878 +:10224000040BE0B2F4E700BF504000580E4B30B52D +:102250001C6F240405D41C6F1C671C6F44F40044DD +:102260001C670A4C02442368D2B243F48073236093 +:10227000074B904200D130BD441C51F8045B00B2C2 +:1022800043F82050E0B2F4E70044025800480258F6 +:102290005040005807B5012201A90020FFF7C4FFF4 +:1022A000019803B05DF804FB13B50446FFF7F2FF95 +:1022B000A04205D0012201A900200194FFF7C6FF2A +:1022C00002B010BD0144BFF34F8F064B884204D3C8 +:1022D000BFF34F8FBFF36F8F7047C3F85C0220309E +:1022E000F4E700BF00ED00E0034B1A681AB9034A97 +:1022F000D2F8D0241A607047604100200040025894 +:1023000008B5FFF7F1FF024B1868C0F3806008BD05 +:102310006041002070B5BFF34F8FBFF36F8F1A4A33 +:102320000021C2F85012BFF34F8FBFF36F8F536974 +:1023300043F400335361BFF34F8FBFF36F8FC2F885 +:102340008410BFF34F8FD2F8803043F6E074C3F3AC +:10235000C900C3F34E335B0103EA0406014646EAB3 +:1023600081750139C2F86052F9D2203B13F1200F78 +:10237000F2D1BFF34F8F536943F480335361BFF3FE +:102380004F8FBFF36F8F70BD00ED00E0FEE70000E0 +:10239000214B2248224A70B5904237D3214BC11EAF +:1023A000DA1C121A22F003028B4238BF00220021ED +:1023B000FEF77CFD1C4A0023C2F88430BFF34F8F28 +:1023C000D2F8803043F6E074C3F3C900C3F34E3350 +:1023D0005B0103EA0406014646EA81750139C2F849 +:1023E0006C52F9D2203B13F1200FF2D1BFF34F8F83 +:1023F000BFF36F8FBFF34F8FBFF36F8F0023C2F810 +:102400005032BFF34F8FBFF36F8F70BD53F8041B73 +:1024100040F8041BC0E700BF943E0008344300208E +:10242000344300203443002000ED00E0074BD3F894 +:10243000D81021EA0001C3F8D810D3F8002122EA0D +:102440000002C3F80021D3F800317047004402585D +:1024500070B5D0E9244300224FF0FF359E6804EBAD +:1024600042135101D3F80009002805DAD3F8000916 +:1024700040F08040C3F80009D3F8000B002805DACB +:10248000D3F8000B40F08040C3F8000B0132631812 +:102490009642C3F80859C3F8085BE0D24FF0011325 +:1024A000C4F81C3870BD0000890141F020010161B1 +:1024B00003699B06FCD41220FFF770BA10B50A4CD2 +:1024C0002046FEF733FF094BC4F89030084BC4F8A0 +:1024D0009430084C2046FEF729FF074BC4F8903093 +:1024E000064BC4F8943010BD644100200000084041 +:1024F000E03C00080042002000000440EC3C0008E2 +:1025000070B503780546012B5CD1434BD0F8904061 +:10251000984258D1414B0E216520D3F8D82042F083 +:102520000062C3F8D820D3F8002142F00062C3F85B +:102530000021D3F80021D3F8802042F00062C3F8D4 +:102540008020D3F8802022F00062C3F88020D3F8E6 +:10255000803000F0ADFC324BE360324BC4F8003801 +:102560000023D5F89060C4F8003EC02323604FF4E8 +:102570000413A3633369002BFCDA01230C203361BD +:10258000FFF70CFA3369DB07FCD41220FFF706FAD9 +:102590003369002BFCDA00262846A660FFF758FFB7 +:1025A0006B68C4F81068DB68C4F81468C4F81C6869 +:1025B00083BB1D4BA3614FF0FF336361A36843F0FE +:1025C0000103A36070BD194B9842C9D1134B4FF062 +:1025D0008060D3F8D82042F00072C3F8D820D3F836 +:1025E000002142F00072C3F80021D3F80021D3F893 +:1025F000802042F00072C3F88020D3F8802022F0BF +:102600000072C3F88020D3F88030FFF70FFF0E214F +:102610004D209EE7064BCDE7644100200044025860 +:102620004014004003002002003C30C00042002063 +:10263000083C30C0F8B5D0F89040054600214FF076 +:1026400000662046FFF730FFD5F8941000234FF0C6 +:1026500001128F684FF0FF30C4F83438C4F81C28DA +:1026600004EB431201339F42C2F80069C2F8006BC9 +:10267000C2F80809C2F8080BF2D20B68D5F890200E +:10268000C5F89830636210231361166916F01006BE +:10269000FBD11220FFF782F9D4F8003823F4FE634F +:1026A000C4F80038A36943F4402343F01003A36146 +:1026B0000923C4F81038C4F814380B4BEB604FF002 +:1026C000C043C4F8103B094BC4F8003BC4F8106980 +:1026D000C4F80039D5F8983003F1100243F48013A0 +:1026E000C5F89820A362F8BDBC3C000840800010EB +:1026F000D0F8902090F88A10D2F8003823F4FE63C6 +:1027000043EA0113C2F80038704700002DE9F8438E +:1027100000EB8103D0F890500C468046DA680FFA3F +:1027200081F94801166806F00306731E022B05EBBB +:1027300041134FF0000194BFB604384EC3F8101B8C +:102740004FF0010104F1100398BF06F1805601FA21 +:1027500003F3916998BF06F5004600293AD0578ADD +:1027600004F15801374349016F50D5F81C180B4349 +:102770000021C5F81C382B180127C3F81019A740F1 +:102780005369611E9BB3138A928B9B08012A88BFF1 +:102790005343D8F89820981842EA034301F14002C5 +:1027A0002146C8F89800284605EB82025360FFF7DF +:1027B0007BFE08EB8900C3681B8A43EA84534834D4 +:1027C0001E4364012E51D5F81C381F43C5F81C78F0 +:1027D000BDE8F88305EB4917D7F8001B21F4004149 +:1027E000C7F8001BD5F81C1821EA0303C0E704F161 +:1027F0003F030B4A2846214605EB83035A60FFF747 +:1028000053FE05EB4910D0F8003923F40043C0F81B +:102810000039D5F81C3823EA0707D7E700800010F5 +:1028200000040002D0F894201268C0F89820FFF746 +:102830000FBE00005831D0F8903049015B5813F4B6 +:10284000004004D013F4001F0CBF02200120704789 +:102850004831D0F8903049015B5813F4004004D05F +:1028600013F4001F0CBF02200120704700EB810110 +:10287000CB68196A0B6813604B685360704700009F +:1028800000EB810330B5DD68AA691368D36019B91C +:10289000402B84BF402313606B8A1468D0F89020CB +:1028A0001C4402EB4110013C09B2B4FBF3F4634356 +:1028B000033323F0030343EAC44343F0C043C0F8A7 +:1028C000103B2B6803F00303012B0ED1D2F808381C +:1028D00002EB411013F4807FD0F8003B14BF43F0AB +:1028E000805343F00053C0F8003B02EB4112D2F892 +:1028F000003B43F00443C2F8003B30BD2DE9F041FA +:10290000D0F8906005460C4606EB4113D3F8087BDF +:102910003A07C3F8087B08D5D6F814381B0704D546 +:1029200000EB8103DB685B689847FA071FD5D6F890 +:102930001438DB071BD505EB8403D968CCB98B6948 +:10294000488A5A68B2FBF0F600FB16228AB918686A +:10295000DA6890420DD2121AC3E90024302383F3BF +:10296000118821462846FFF78BFF84F31188BDE8C4 +:10297000F081012303FA04F26B8923EA02036B81DD +:10298000CB68002BF3D021462846BDE8F04118471C +:1029900000EB81034A0170B5DD68D0F890306C69B6 +:1029A0002668E66056BB1A444FF40020C2F81009AE +:1029B0002A6802F00302012A0AB20ED1D3F80808ED +:1029C00003EB421410F4807FD4F8000914BF40F0E8 +:1029D000805040F00050C4F8000903EB4212D2F8D6 +:1029E000000940F00440C2F800090122D3F834087D +:1029F00002FA01F10143C3F8341870BD19B9402E31 +:102A000084BF4020206020681A442E8A8419013C2B +:102A1000B4FBF6F440EAC44040F00050C6E70000C2 +:102A20002DE9F843D0F8906005460C464F0106EBBF +:102A30004113D3F8088918F0010FC3F808891CD096 +:102A4000D6F81038DB0718D500EB8103D3F80CC09B +:102A5000DCF81430D3F800E0DA68964530D2A2EB07 +:102A60000E024FF000091A60C3F80490302383F37C +:102A70001188FFF78DFF89F3118818F0800F1DD0A2 +:102A8000D6F834380126A640334217D005EB84032C +:102A90000134D5F89050D3F80CC0E4B22F44DCF8E0 +:102AA000142005EB0434D2F800E05168714514D3CA +:102AB000D5F8343823EA0606C5F83468BDE8F8834B +:102AC000012303FA01F2038923EA02030381DCF8FC +:102AD0000830002BD1D09847CFE7AEEB0103BCF80C +:102AE0001000834228BF0346D7F8180980B2B3EB21 +:102AF000800FE3D89068A0F1040959F8048FC4F856 +:102B00000080A0EB09089844B8F1040FF5D81844E8 +:102B10000B4490605360C8E72DE9F84FD0F890500F +:102B200004466E69AB691E4016F480586E6103D08E +:102B3000BDE8F84FFEF76ABC002E12DAD5F8003E69 +:102B40009B0705D0D5F8003E23F00303C5F8003EEF +:102B5000D5F80438204623F00103C5F80438FEF701 +:102B600083FC370505D52046FFF772FC2046FEF7AB +:102B700069FCB0040CD5D5F8083813F0060FEB68E3 +:102B800023F470530CBF43F4105343F4A053EB6091 +:102B900031071BD56368DB681BB9AB6923F00803F9 +:102BA000AB612378052B0CD1D5F8003E9A0705D0F0 +:102BB000D5F8003E23F00303C5F8003E2046FEF79B +:102BC00053FC6368DB680BB120469847F30200F1C1 +:102BD000BA80B70226D5D4F8909000274FF0010AAA +:102BE00009EB4712D2F8003B03F44023B3F5802FE2 +:102BF00011D1D2F8003B002B0DDA62890AFA07F3F3 +:102C000022EA0303638104EB8703DB68DB6813B10B +:102C10003946204698470137D4F89430FFB29B6874 +:102C20009F42DDD9F00619D5D4F89000026AC2F3AC +:102C30000A1702F00F0302F4F012B2F5802F00F031 +:102C4000CA80B2F5402F09D104EB8303002200F5BE +:102C50008050DB681B6A974240F0B0803003D5F8A3 +:102C6000185835D5E90303D500212046FFF746FE65 +:102C7000AA0303D501212046FFF740FE6B0303D5CD +:102C800002212046FFF73AFE2F0303D503212046F9 +:102C9000FFF734FEE80203D504212046FFF72EFE9D +:102CA000A90203D505212046FFF728FE6A0203D5B5 +:102CB00006212046FFF722FE2B0203D507212046DE +:102CC000FFF71CFEEF0103D508212046FFF716FE93 +:102CD000700340F1A780E90703D500212046FFF7E4 +:102CE0009FFEAA0703D501212046FFF799FE6B0737 +:102CF00003D502212046FFF793FE2F0703D50321BA +:102D00002046FFF78DFEEE0603D504212046FFF78F +:102D100087FEA80603D505212046FFF781FE690638 +:102D200003D506212046FFF77BFE2A0603D507219F +:102D30002046FFF775FEEB0574D520460821BDE857 +:102D4000F84FFFF76DBED4F890904FF0000B4FF0A6 +:102D5000010AD4F894305FFA8BF79B689F423FF6E4 +:102D600038AF09EB4713D3F8002902F44022B2F53B +:102D7000802F20D1D3F80029002A1CDAD3F80029AB +:102D800042F09042C3F80029D3F80029002AFBDB67 +:102D90003946D4F89000FFF787FB22890AFA07F337 +:102DA00022EA0303238104EB8703DB689B6813B1EA +:102DB0003946204698470BF1010BCAE7910701D12C +:102DC000D0F80080072A02F101029CBF03F8018BB2 +:102DD0004FEA18283FE704EB830300F58050DA68D8 +:102DE000D2F818C0DCF80820DCE9001CA1EB0C0CC0 +:102DF00000218F4208D1DB689B699A683A449A6047 +:102E00005A683A445A6029E711F0030F01D1D0F80B +:102E100000808C4501F1010184BF02F8018B4FEA6B +:102E20001828E6E7BDE8F88F08B50348FFF774FEF9 +:102E3000BDE8084000F07ABB6441002008B50348B3 +:102E4000FFF76AFEBDE8084000F070BB00420020BA +:102E5000D0F8903003EB4111D1F8003B43F400135C +:102E6000C1F8003B70470000D0F8903003EB4111EF +:102E7000D1F8003943F40013C1F80039704700005D +:102E8000D0F8903003EB4111D1F8003B23F400134C +:102E9000C1F8003B70470000D0F8903003EB4111BF +:102EA000D1F8003923F40013C1F80039704700004D +:102EB000090100F16043012203F56143C9B283F8BF +:102EC000001300F01F039A4043099B0003F1604385 +:102ED00003F56143C3F880211A60704730B50433AD +:102EE000039C0172002104FB0325C160C0E9065365 +:102EF000049B0363059BC0E90000C0E90422C0E90C +:102F00000842C0E90A11436330BD00000022416A53 +:102F1000C260C0E90411C0E90A226FF00101FEF7A6 +:102F2000ADBD0000D0E90432934201D1C2680AB9B4 +:102F3000181D704700207047036919600021C2689E +:102F40000132C260C269134482699342036124BFA3 +:102F5000436A0361FEF786BD38B504460D46E36853 +:102F60003BB162690020131D1268A3621344E3623F +:102F700007E0237A33B929462046FEF763FD00288F +:102F8000EDDA38BD6FF00100FBE70000C368C269ED +:102F9000013BC3604369134482699342436124BF88 +:102FA000436A436100238362036B03B11847704790 +:102FB00070B53023044683F31188866A3EB9FFF763 +:102FC000CBFF054618B186F31188284670BDA36A69 +:102FD000E26A13F8015B9342A36202D32046FFF733 +:102FE000D5FF002383F31188EFE700002DE9F84FA8 +:102FF00004460E46174698464FF0300989F311886B +:103000000025AA46D4F828B0BBF1000F09D14146EB +:103010002046FFF7A1FF20B18BF311882846BDE8B9 +:10302000F88FD4E90A12A7EB050B521A934528BF73 +:103030009346BBF1400F1BD9334601F1400251F8D2 +:10304000040B914243F8040BF9D1A36A4036403592 +:103050004033A362D4E90A239A4202D32046FFF701 +:1030600095FF8AF31188BD42D8D289F31188C9E748 +:1030700030465A46FDF7F4FEA36A5E445D445B4465 +:10308000A362E7E710B5029C0433017203FB04213D +:10309000C460C0E906130023C0E90A33039B03633D +:1030A000049BC0E90000C0E90422C0E90842436370 +:1030B00010BD0000026A6FF00101C260426AC0E9FF +:1030C00004220022C0E90A22FEF7D8BCD0E904237A +:1030D0009A4201D1C26822B9184650F8043B0B60ED +:1030E000704700231846FAE7C3680021C26901331C +:1030F000C3604369134482699342436124BF436AB6 +:103100004361FEF7AFBC000038B504460D46E368E6 +:103110003BB1236900201A1DA262E2691344E362F5 +:1031200007E0237A33B929462046FEF78BFC0028B6 +:10313000EDDA38BD6FF00100FBE7000003691960AC +:10314000C268013AC260C269134482699342036152 +:1031500024BF436A036100238362036B03B11847F2 +:103160007047000070B530230D460446114683F3C6 +:103170001188866A2EB9FFF7C7FF10B186F3118850 +:1031800070BDA36A1D70A36AE26A01339342A36211 +:1031900004D3E16920460439FFF7D0FF002080F313 +:1031A0001188EDE72DE9F84F04460D469046994603 +:1031B0004FF0300A8AF311880026B346A76A4FB948 +:1031C00049462046FFF7A0FF20B187F3118830461B +:1031D000BDE8F88FD4E90A073A1AA8EB0607974228 +:1031E00028BF1746402F1BD905F1400355F8042B83 +:1031F0009D4240F8042BF9D1A36A40364033A362C4 +:10320000D4E90A239A4204D3E16920460439FFF73E +:1032100095FF8BF311884645D9D28AF31188CDE703 +:1032200029463A46FDF71CFEA36A3D443E443B4412 +:10323000A362E5E7D0E904239A4217D1C3689BB1A2 +:10324000836A8BB1043B9B1A0ED01360C368013BA9 +:10325000C360C3691A4483699A42026124BF436A06 +:103260000361002383620123184670470023FBE7B4 +:1032700000F086B9014B586A704700BF000C00404F +:10328000034B002258631A610222DA60704700BFC4 +:10329000000C0040014B0022DA607047000C004037 +:1032A000014B5863704700BF000C0040FEE7000070 +:1032B00070B51B4B0025044686B058600E468562EB +:1032C000016300F00BF904F11003A560E562C4E9A5 +:1032D00004334FF0FF33C4E90044C4E90635FFF777 +:1032E000C9FF2B46024604F134012046C4E90823F5 +:1032F00080230C4A2565FEF75BFB01230A4AE06048 +:1033000000920375684672680192B268CDE90223A3 +:10331000064BCDE90435FEF773FB06B070BD00BF68 +:1033200020260020F83C0008FD3C0008AD320008D3 +:10333000024AD36A1843D062704700BFB823002006 +:103340004B6843608B688360CB68C3600B694361E3 +:103350004B6903628B6943620B680360704700002E +:1033600008B53C4B40F2FF713B48D3F888200A4334 +:10337000C3F88820D3F8882022F4FF6222F00702E5 +:10338000C3F88820D3F88820D3F8E0200A43C3F894 +:10339000E020D3F808210A43C3F808212F4AD3F8C4 +:1033A00008311146FFF7CCFF00F5806002F11C01E7 +:1033B000FFF7C6FF00F5806002F13801FFF7C0FF9C +:1033C00000F5806002F15401FFF7BAFF00F580605C +:1033D00002F17001FFF7B4FF00F5806002F18C018B +:1033E000FFF7AEFF00F5806002F1A801FFF7A8FF2C +:1033F00000F5806002F1C401FFF7A2FF00F58060D4 +:1034000002F1E001FFF79CFF00F5806002F1FC0192 +:10341000FFF796FF02F58C7100F58060FFF790FFD3 +:1034200000F076F90E4BD3F8902242F00102C3F877 +:103430009022D3F8942242F00102C3F8942205228C +:10344000C3F898204FF06052C3F89C20054AC3F897 +:10345000A02008BD0044025800000258043D0008A6 +:1034600000ED00E01F00080308B500F025FBFEF7A3 +:103470007DFA0F4BD3F8DC2042F04002C3F8DC2089 +:10348000D3F8042122F04002C3F80421D3F8043118 +:10349000084B1A6842F008021A601A6842F00402E7 +:1034A0001A60FEF721FFBDE80840FEF7D3BC00BF5D +:1034B000004402580018024870470000EFF30983E7 +:1034C000054968334A6B22F001024A6383F3098895 +:1034D000002383F31188704700EF00E0302080F371 +:1034E000118862B60D4B0E4AD96821F4E0610904D7 +:1034F000090C0A430B49DA60D3F8FC2042F08072D1 +:10350000C3F8FC20084AC2F8B01F116841F001015D +:1035100011602022DA7783F82200704700ED00E086 +:103520000003FA0555CEACC5001000E0302310B5FD +:1035300083F311880E4B5B6813F4006314D0F1EE33 +:10354000103AEFF309844FF08073683CE361094B54 +:10355000DB6B236684F30988FEF7E8F910B1064BAC +:10356000A36110BD054BFBE783F31188F9E700BFAA +:1035700000ED00E000EF00E02F0400083204000836 +:10358000114BD3F8E82042F00802C3F8E820D3F842 +:10359000102142F00802C3F810210C4AD3F8103170 +:1035A000D36B43F00803D363C722094B9A624FF0F1 +:1035B000FF32DA6200229A615A63DA605A600122AD +:1035C0005A611A60704700BF004402580010005C46 +:1035D000000C0040094A08B51169D3680B40D9B204 +:1035E0009B076FEA0101116107D5302383F311882E +:1035F000FEF7D2F9002383F3118808BD000C0040C8 +:10360000064BD3F8DC200243C3F8DC20D3F80421B6 +:103610001043C3F80401D3F8043170470044025842 +:103620003A4B4FF0FF31D3F8802062F00042C3F8EC +:103630008020D3F8802002F00042C3F88020D3F825 +:103640008020D3F88420C3F88410D3F8842000228B +:10365000C3F88420D3F88400D86F40F0FF4040F4D2 +:10366000FF0040F4DF4040F07F00D867D86F20F0C3 +:10367000FF4020F4FF0020F4DF4020F07F00D867F7 +:10368000D86FD3F888006FEA40506FEA5050C3F803 +:103690008800D3F88800C0F30A00C3F88800D3F884 +:1036A0008800D3F89000C3F89010D3F89000C3F8C6 +:1036B0009020D3F89000D3F89400C3F89410D3F876 +:1036C0009400C3F89420D3F89400D3F89800C3F87A +:1036D0009810D3F89800C3F89820D3F89800D3F83E +:1036E0008C00C3F88C10D3F88C00C3F88C20D3F86E +:1036F0008C00D3F89C00C3F89C10D3F89C10C3F83E +:103700009C20D3F89C3000F0AFB900BF00440258B1 +:1037100008B50122534BC3F80821534BD3F8F420CA +:1037200042F00202C3F8F420D3F81C2142F0020256 +:10373000C3F81C210222D3F81C314C4BDA605A68C2 +:103740009104FCD54A4A1A6001229A60494ADA601B +:1037500000221A614FF440429A61444B9A699204E4 +:10376000FCD51A6842F480721A603F4B1A6F12F44B +:10377000407F04D04FF480321A6700221A671A681B +:1037800042F001021A60384B1A685007FCD500223B +:103790001A611A6912F03802FBD1012119604FF049 +:1037A000804159605A67344ADA62344A1A611A68A9 +:1037B00042F480321A602C4B1A689103FCD51A68C7 +:1037C00042F480521A601A689204FCD52C4A2D49A2 +:1037D0009A6200225A63196301F57C01DA6301F2EF +:1037E000E71199635A64284A1A64284ADA621A6807 +:1037F00042F0A8521A601C4B1A6802F02852B2F12B +:10380000285FF9D148229A614FF48862DA61402238 +:103810001A621F4ADA641F4A1A651F4A5A651F4A0C +:103820009A6532231E4A1360136803F00F03022BBC +:10383000FAD10D4A136943F003031361136903F0CE +:103840003803182BFAD14FF00050FFF7D9FE4FF094 +:103850008040FFF7D5FE4FF00040BDE80840FFF77D +:10386000CFBE00BF008000510044025800480258FB +:1038700000C000F0020000010000FF010088900875 +:103880001210200063020901470E0508DD0BBF017D +:1038900020000020000001100910E00000010110CC +:1038A000002000524FF0B04208B5D2F8883003F043 +:1038B0000103C2F8883023B1044A13680BB1506881 +:1038C0009847BDE80840FFF731BE00BFB442002072 +:1038D0004FF0B04208B5D2F8883003F00203C2F8C6 +:1038E000883023B1044A93680BB1D0689847BDE88B +:1038F0000840FFF71BBE00BFB44200204FF0B042AB +:1039000008B5D2F8883003F00403C2F8883023B138 +:10391000044A13690BB150699847BDE80840FFF7A6 +:1039200005BE00BFB44200204FF0B04208B5D2F847 +:10393000883003F00803C2F8883023B1044A936941 +:103940000BB1D0699847BDE80840FFF7EFBD00BF55 +:10395000B44200204FF0B04208B5D2F8883003F0EE +:103960001003C2F8883023B1044A136A0BB1506ABD +:103970009847BDE80840FFF7D9BD00BFB44200201A +:103980004FF0B04310B5D3F8884004F47872C3F810 +:103990008820A30604D5124A936A0BB1D06A9847CF +:1039A000600604D50E4A136B0BB1506B9847210685 +:1039B00004D50B4A936B0BB1D06B9847E20504D545 +:1039C000074A136C0BB1506C9847A30504D5044A01 +:1039D000936C0BB1D06C9847BDE81040FFF7A6BDC3 +:1039E000B44200204FF0B04310B5D3F8884004F43F +:1039F0007C42C3F88820620504D5164A136D0BB1CA +:103A0000506D9847230504D5124A936D0BB1D06DC4 +:103A10009847E00404D50F4A136E0BB1506E9847D7 +:103A2000A10404D50B4A936E0BB1D06E9847620483 +:103A300004D5084A136F0BB1506F9847230404D57F +:103A4000044A936F0BB1D06F9847BDE81040FFF761 +:103A50006DBD00BFB442002008B5FFF7BBFDBDE857 +:103A60000840FFF763BD0000062108B50846FFF7D0 +:103A70001FFA06210720FFF71BFA06210820FFF78F +:103A800017FA06210920FFF713FA06210A20FFF78B +:103A90000FFA06211720FFF70BFA06212820FFF75F +:103AA00007FA09217A20FFF703FA07213220BDE83F +:103AB0000840FFF7FDB9000008B5FFF7B1FD00F0C1 +:103AC0000BF8FDF731FCFDF703FBFFF7F5FCBDE854 +:103AD0000840FFF7CDBB00000023054A194601331B +:103AE000102BC2E9001102F10802F8D1704700BFA3 +:103AF000B442002010B501390244904201D10020A7 +:103B000005E0037811F8014FA34201D0181B10BD46 +:103B10000130F2E7034611F8012B03F8012B002ACC +:103B2000F9D1704753544D333248373F3F3F00532C +:103B3000544D3332483733782F3732780053544D51 +:103B40003332483734332F3735332F373530000091 +:103B500001105A000310590001205800032056009C +:103B6000000000004D100008391000087510000812 +:103B7000611000086D100008591000084510000879 +:103B80003110000881100008000000000100000052 +:103B90000000000063300000943B00081024002067 +:103BA000202600204172647550696C6F74002542B4 +:103BB0004F415244252D424C002553455249414C1A +:103BC0002500000002000000000000006D12000847 +:103BD000DD12000840004000D03E0020E03E002002 +:103BE00002000000000000000300000000000000D0 +:103BF000251300080000000010000000F03E002027 +:103C000000000000010000000000000064410020EE +:103C100001010200F11D0008011D00089D1D0008A2 +:103C2000811D0008430000002C3C000809024300ED +:103C3000020100C032090400000102020100052453 +:103C400000100105240100010424020205240600DD +:103C500001070582030800FF09040100020A0000B1 +:103C60000007050102400000070581024000000036 +:103C700012000000783C0008120110010200004010 +:103C80000912415700020102030100000403090464 +:103C900025424F4152442500416F636F64612D52AC +:103CA000432D483734334475616C0030313233343E +:103CB000353637383941424344454600000000005C +:103CC0008114000839170008E5170008400040007B +:103CD0009C4200209C42002001000000AC420020D9 +:103CE000800000004001000008000000000100000A +:103CF00000100000080000006D61696E0069646CCE +:103D0000650000000000802A00000000AAAAAAAAFC +:103D100000000024FFFF00000000000000A00A00D7 +:103D20000000000100000000AAAAAAAA00000001E9 +:103D3000FFFF000000000000000000000000004045 +:103D400000000000AAAAAAAA00000040FFFF00008D +:103D50000000000000000000400000000000000023 +:103D6000AAAAAAAA40000000FFFF0000000000006D +:103D7000000000004000400000000000AAAAAAAA1B +:103D800000004000F7FF00000000000000000000FD +:103D90000000000000000000AAAAAAAA000000007B +:103DA000FFFF000000000000000000000000000015 +:103DB00000000000AAAAAAAA00000000FFFF00005D +:103DC00000000000000000000000000000000000F3 +:103DD000AAAAAAAA00000000FFFF0000000000003D +:103DE000000000000000000000000000AAAAAAAA2B +:103DF00000000000FFFF00000000000000000000C5 +:103E00000000000000000000AAAAAAAA000000000A +:103E1000FFFF0000000000000000000000000000A4 +:103E200000000000AAAAAAAA00000000FFFF0000EC +:103E300000000000000000005A1400000000000014 +:103E400000001A0000000000FF0000000000000059 +:103E5000243B0008830400002F3B000850040000AE +:103E60003D3B00080096000000000800960000009E +:103E700000080000040000008C3C00080000000066 +:103E80000000000000000000000000000000000032 +:043E9000000000002E +:00000001FF diff --git a/Tools/bootloaders/CubePilot-PPPGW_bl.bin b/Tools/bootloaders/CubePilot-PPPGW_bl.bin new file mode 100755 index 00000000000000..91f4c3cc91a6b3 Binary files /dev/null and b/Tools/bootloaders/CubePilot-PPPGW_bl.bin differ diff --git a/Tools/bootloaders/Here4FC_bl.bin b/Tools/bootloaders/Here4FC_bl.bin new file mode 100755 index 00000000000000..72a593edecc451 Binary files /dev/null and b/Tools/bootloaders/Here4FC_bl.bin differ diff --git a/Tools/bootloaders/Here4FC_bl.hex b/Tools/bootloaders/Here4FC_bl.hex new file mode 100644 index 00000000000000..2ca9ef747bf0b0 --- /dev/null +++ b/Tools/bootloaders/Here4FC_bl.hex @@ -0,0 +1,1594 @@ +:020000040800F2 +:1000000000060020F50500081D350008D53400085D +:10001000FD340008D5340008F5340008F705000861 +:10002000F7050008F7050008F70500080145000876 +:10003000F7050008F7050008F7050008F7050008B0 +:10004000F7050008F7050008F7050008F7050008A0 +:10005000F7050008F7050008F15C00081D5D0008C1 +:10006000495D0008755D0008A15D0008F7050008FE +:10007000F7050008F7050008F7050008F705000870 +:10008000F7050008F7050008F705000885340008A3 +:10009000AD34000899340008C1340008CD5D000873 +:1000A000F7050008F7050008F7050008F705000840 +:1000B000F7050008F7050008F7050008F705000830 +:1000C000F7050008F7050008F7050008F705000820 +:1000D000F7050008A55E0008F7050008F705000809 +:1000E000315E0008F7050008F7050008F70500086D +:1000F000F7050008F7050008F7050008F7050008F0 +:10010000F7050008F7050008CD5E0008F7050008B0 +:10011000F7050008F7050008F7050008F7050008CF +:10012000F7050008F7050008F7050008F7050008BF +:10013000F7050008F7050008F7050008F7050008AF +:10014000F7050008F7050008F7050008F70500089F +:10015000F7050008F7050008F7050008F70500088F +:10016000F7050008F7050008F7050008F70500087F +:10017000F7050008F7050008F7050008F70500086F +:10018000F7050008F7050008F7050008B95E000844 +:10019000F7050008F7050008F7050008F70500084F +:1001A000F7050008F7050008F7050008F70500083F +:1001B000F7050008F7050008F7050008F70500082F +:1001C000F7050008F7050008F7050008F70500081F +:1001D000F7050008F7050008F7050008F70500080F +:1001E000F7050008F7050008F7050008F7050008FF +:1001F000F7050008F7050008F7050008F7050008EF +:10020000F7050008F7050008F7050008F7050008DE +:10021000F7050008F7050008F7050008F7050008CE +:10022000F7050008F7050008F7050008F7050008BE +:10023000F7050008F7050008F7050008F7050008AE +:10024000F7050008F7050008F7050008F70500089E +:10025000F7050008F7050008F7050008F70500088E +:10026000F7050008F7050008F7050008F70500087E +:10027000F7050008F7050008F7050008F70500086E +:10028000F7050008F7050008F7050008F70500085E +:10029000F7050008F7050008F7050008F70500084E +:1002A000F7050008F7050008F7050008F70500083E +:1002B000F7050008F7050008F7050008F70500082E +:1002C000F7050008F7050008F7050008F70500081E +:1002D000F7050008F7050008F7050008F70500080E +:1002E000CD19000800000000000000000000000020 +:1002F00053B94AB9002908BF00281CBF4FF0FF318D +:100300004FF0FF3000F074B9ADF1080C6DE904CE88 +:1003100000F006F8DDF804E0DDE9022304B07047E0 +:100320002DE9F047089D04468E46002B4DD18A42A8 +:10033000944669D9B2FA82F252B101FA02F3C2F1DB +:10034000200120FA01F10CFA02FC41EA030E94406C +:100350004FEA1C48210CBEFBF8F61FFA8CF708FB8D +:1003600016E341EA034306FB07F199420AD91CEB65 +:10037000030306F1FF3080F01F81994240F21C8197 +:10038000023E63445B1AA4B2B3FBF8F008FB1033DF +:1003900044EA034400FB07F7A7420AD91CEB040414 +:1003A00000F1FF3380F00A81A74240F207816444E4 +:1003B000023840EA0640E41B00261DB1D440002369 +:1003C000C5E900433146BDE8F0878B4209D9002DCD +:1003D00000F0EF800026C5E9000130463146BDE857 +:1003E000F087B3FA83F6002E4AD18B4202D38242C1 +:1003F00000F2F980841A61EB030301209E46002D70 +:10040000E0D0C5E9004EDDE702B9FFDEB2FA82F2C4 +:10041000002A40F09280A1EB0C014FEA1C471FFA22 +:100420008CFE0126200CB1FBF7F307FB131140EA09 +:1004300001410EFB03F0884208D91CEB010103F1D6 +:10044000FF3802D2884200F2CB804346091AA4B298 +:10045000B1FBF7F007FB101144EA01440EFB00FE6C +:10046000A64508D91CEB040400F1FF3102D2A645D1 +:1004700000F2BB800846A4EB0E0440EA03409CE770 +:10048000C6F12007B34022FA07FC4CEA030C20FA1D +:1004900007F401FA06F31C43F9404FEA1C4900FA3D +:1004A00006F3B1FBF9F8200C1FFA8CFE09FB1811BA +:1004B00040EA014108FB0EF0884202FA06F20BD92D +:1004C0001CEB010108F1FF3A80F08880884240F27D +:1004D0008580A8F102086144091AA4B2B1FBF9F0C1 +:1004E00009FB101144EA014100FB0EFE8E4508D9BC +:1004F0001CEB010100F1FF346CD28E456AD9023841 +:10050000614440EA0840A0FB0294A1EB0E01A14225 +:10051000C846A64656D353D05DB1B3EB080261EB93 +:100520000E0101FA07F722FA06F3F1401F43C5E96D +:10053000007100263146BDE8F087C2F12003D840A3 +:100540000CFA02FC21FA03F3914001434FEA1C47E5 +:100550001FFA8CFEB3FBF7F007FB10360B0C43EAD7 +:10056000064300FB0EF69E4204FA02F408D91CEB87 +:10057000030300F1FF382FD29E422DD90238634485 +:100580009B1B89B2B3FBF7F607FB163341EA034125 +:1005900006FB0EF38B4208D91CEB010106F1FF3874 +:1005A00016D28B4214D9023E6144C91A46EA00466B +:1005B00038E72E46284605E70646E3E61846F8E6FD +:1005C0004B45A9D2B9EB020864EB0C0E0138A3E746 +:1005D0004646EAE7204694E74046D1E7D0467BE727 +:1005E000023B614432E7304609E76444023842E79F +:1005F000704700BF02E000F000F8FEE772B637482F +:1006000080F30888364880F3098836483649086000 +:1006100040F20000CCF200004EF63471CEF2000140 +:100620000860BFF34F8FBFF36F8F40F20000C0F23E +:10063000F0004EF68851CEF200010860BFF34F8FF4 +:10064000BFF36F8F4FF00000E1EE100A4EF63C71E1 +:10065000CEF200010860062080F31488BFF36F8F8C +:1006600005F01AF805F01CF94FF055301F491B4AE8 +:1006700091423CBF41F8040BFAE71D49184A9142E8 +:100680003CBF41F8040BFAE71A491B4A1B4B9A423C +:100690003EBF51F8040B42F8040BF8E7002018495C +:1006A000184A91423CBF41F8040BFAE705F032F8D2 +:1006B00005F078F9144C154DAC4203DA54F8041BDC +:1006C0008847F9E700F042F8114C124DAC4203DACA +:1006D00054F8041B8847F9E705F01AB80006002013 +:1006E000002200200000000808ED00E000000020CB +:1006F00000060020E062000800220020A022002066 +:10070000A0220020086F0020E0020008E402000898 +:10071000E4020008E40200082DE9F04F2DED108AF4 +:10072000C1F80CD0D0F80CD0BDEC108ABDE8F08F29 +:10073000002383F311882846A047002004F0F0FB33 +:10074000FEE704F053FB00DFFEE70000F8B501F020 +:1007500053F904F025FF074604F094FF0546B8BBA3 +:10076000204B9F4234D001339F4234D027F0FF0208 +:100770001D4B9A4232D12E4642F21074F8B200F06C +:1007800071FF00F075FF08B10024264601F022FD3C +:1007900020B10024032000F07BF8264635B1134B2E +:1007A0009F4203D0002404F065FF2646002004F099 +:1007B00001FF0EB100F082F801F0ECFA00F08CFFBE +:1007C00001F0CAF9204600F00BF900F077F8F9E7DC +:1007D0002E460024D5E704460126D2E7064641F21C +:1007E0008834CEE7010007B0000008B0263A09B00F +:1007F00008B501F07DF9A0F120035842584108BD29 +:1008000007B541F21203022101A8ADF8043001F04E +:100810008DF903B05DF804FB38B5302383F31188FC +:10082000174803680BB104F039FC0023154A4FF454 +:100830007A71134804F028FC002383F31188124CCA +:10084000236813B12368013B2360636813B16368B5 +:10085000013B63600D4D2B7833B963687BB902208F +:1008600001F02EFA322363602B78032B07D16368E3 +:100870002BB9022001F024FA4FF47A73636038BD7B +:10088000A022002019080008C0230020B822002060 +:10089000084B187003280CD8DFE800F008050208A0 +:1008A000022001F00BBA022001F006BA024B00222E +:1008B0005A607047B8220020C0230020F8B501F02C +:1008C00089FC30B13E4B03221A7000223D4B5A6026 +:1008D000F8BD3D4B3D4A1C4619680131F8D0043340 +:1008E0009342F9D162683A4B9A42F1D9394B9B68ED +:1008F00003F1006303F580239A42E9D204F08EFEEF +:1009000004F0A0FE002001F04DF90220FFF7C0FF27 +:10091000314B00219A6C99641A6F19671A6FDA6C5F +:10092000D9645A6F59675A6F1A6D19659A6F99672A +:100930009B6F72B64FF0E023C3F8084DD4E9000472 +:10094000BFF34F8FBFF36F8F244AC2F88410BFF3F9 +:100950004F8F536923F480335361BFF34F8FD2F825 +:10096000803043F6E076C3F3C905C3F34E335B0131 +:1009700003EA060C29464CEA81770139C2F8747201 +:10098000F9D2203B13F1200FF2D1BFF34F8FBFF309 +:100990006F8FBFF34F8FBFF36F8F536923F4003313 +:1009A00053610023C2F85032BFF34F8FBFF36F8FF4 +:1009B000302383F31188854680F30888204787E732 +:1009C000B8220020C02300200000040820000408F2 +:1009D000FFFF0308002200200045025800ED00E060 +:1009E0002DE9F04F93B0B74B2022FF2100900AA8C9 +:1009F0009D6801F079F9B44A1378A3B90121B3488D +:100A000011700360302383F3118803680BB104F085 +:100A100045FB0023AE4A4FF47A71AC4804F034FB36 +:100A2000002383F31188009B13B1AA4B009A1A602C +:100A3000A94A1378032B03D000231370A54A5360EF +:100A40004FF0000A009CD3465646D146012001F0E3 +:100A500035F924B19F4B1B68002B00F02C8200203D +:100A600001F046F80390039B002B01DA00F088FEAA +:100A7000039B002BEDDB012001F01EF9039B213BC2 +:100A80001F2BE3D801A252F823F000BF0D0B000882 +:100A9000350B0008C90B00084D0A00084D0A000874 +:100AA0004D0A00085B0C00082B0E0008450D0008DD +:100AB000A70D0008CF0D0008F50D00084D0A00082D +:100AC000070E00084D0A0008790E0008AD0B00085B +:100AD0004D0A0008BD0E0008190B0008AD0B0008F8 +:100AE0004D0A0008A70D00084D0A00084D0A00082D +:100AF0004D0A00084D0A00084D0A00084D0A00087A +:100B00004D0A00084D0A0008C90B00080220FFF733 +:100B10006FFE002840F0F981009B022105A8BAF180 +:100B2000000F08BF1C4641F21233ADF8143000F03C +:100B3000FDFF8BE74FF47A7000F0DAFF071EEBDB66 +:100B40000220FFF755FE0028E6D0013F052F00F2F6 +:100B5000DE81DFE807F0030A0D10133605230421B8 +:100B600005A8059300F0E2FF17E004215548F9E7D6 +:100B700004215A48F6E704215948F3E74FF01C08CE +:100B8000404608F1040801F00FF80421059005A87B +:100B900000F0CCFFB8F12C0FF2D101204FF000098A +:100BA00000FA07F747EA0B0B5FFA8BFB01F0E8F856 +:100BB00026B10BF00B030B2B08BF0024FFF720FE20 +:100BC00044E704214748CDE7002EA5D00BF00B03E6 +:100BD0000B2BA1D10220FFF70BFE074600289BD06C +:100BE0000120002600F0DEFF0220FFF751FE1FFA71 +:100BF00086F8404600F0E6FF0446B0B1039940464F +:100C00000136A1F140025142514100F0F3FF0028AA +:100C1000EDD1BA46044641F21213022105A83E4620 +:100C2000ADF8143000F082FF10E725460120FFF7F1 +:100C30002FFE244B9B68AB4207D9284600F0B4FF37 +:100C4000013040F067810435F3E70025224BBA46B6 +:100C50003E461D701F4B5D60A8E7002E3FF45CAF61 +:100C60000BF00B030B2B7FF457AF0220FFF710FEA6 +:100C7000322000F03DFFB0F10008FFF64DAF18F054 +:100C800003077FF449AF0F4A08EB050392689342CC +:100C90003FF642AFB8F5807F3FF73EAF124BB84505 +:100CA000019323DD4FF47A7000F022FF0390039A42 +:100CB000002AFFF631AF039A0137019B03F8012B9D +:100CC000EDE700BF00220020BC230020A02200206E +:100CD00019080008C0230020B822002004220020A8 +:100CE000082200200C220020BC220020C820FFF790 +:100CF0007FFD074600283FF40FAF1F2D11D8C5F127 +:100D000020020AAB25F0030084494245184428BF5D +:100D10004246019200F0D6FF019AFF217F4800F081 +:100D2000E3FF4FEAA803C8F387027C4928460193F2 +:100D300000F0E2FF064600283FF46DAF019B05EB93 +:100D4000830533E70220FFF753FD00283FF4E4AEAC +:100D500000F062FF00283FF4DFAE0027B846704B7A +:100D60009B68BB4218D91F2F11D80A9B01330ED0A4 +:100D700027F0030312AA134453F8203C059340467E +:100D8000042205A9043701F00FFA8046E7E7384648 +:100D900000F00AFF0590F2E7CDF81480042105A8C1 +:100DA00000F0C4FE02E70023642104A8049300F0CD +:100DB000B3FE00287FF4B0AE0220FFF719FD002833 +:100DC0003FF4AAAE049800F01DFF0590E6E700236B +:100DD000642104A8049300F09FFE00287FF49CAED9 +:100DE0000220FFF705FD00283FF496AE049800F0BE +:100DF0000BFFEAE70220FFF7FBFC00283FF48CAE74 +:100E000000F01AFFE1E70220FFF7F2FC00283FF4B0 +:100E100083AE05A9142000F015FF074604210490B5 +:100E200004A800F083FE3946B9E7322000F060FEE6 +:100E3000071EFFF671AEBB077FF46EAE384A07EBB4 +:100E40000903926893423FF667AE0220FFF7D0FC99 +:100E500000283FF461AE27F003074F44B9453FF443 +:100E6000A5AE484609F1040900F09EFE0421059054 +:100E700005A800F05BFEF1E74FF47A70FFF7B8FCCD +:100E800000283FF449AE00F0C7FE002844D00A9B7A +:100E900001330BD008220AA9002000F02DFF002802 +:100EA0003AD02022FF210AA800F01EFFFFF7A8FC7D +:100EB0001C4804F03BF813B0BDE8F08F002E3FF45F +:100EC0002BAE0BF00B030B2B7FF426AE002364211B +:100ED00005A8059300F020FE074600287FF41CAE0D +:100EE0000220FFF785FC804600283FF415AEFFF78F +:100EF00087FC41F2883004F019F8059800F05EFF95 +:100F000046463C4600F03CFFA0E506464EE64FF064 +:100F1000000901E6BA467EE637467CE6BC220020A0 +:100F200000220020A0860100094A49F269001368E6 +:100F300099B21B0C00FB013344F250611360054B66 +:100F4000186882B2000C01FB0200186080B2704782 +:100F500014220020102200200021102210B5044687 +:100F600000F0C2FE034B03CB206061601868A060F4 +:100F700010BD00BF00E8F11F2DE9F041ADF54E7D39 +:100F80000D46002140F275126CAC07460EA80D917B +:100F900000F0AAFE4FF4C4720021204600F0A4FE27 +:100FA0000DF1340802F0C2FA4FF47A72264BB0FB0E +:100FB000F2F0186093E80700022384E807000DF5BB +:100FC000E9702382FFF7C8FF41F204331F4907A8E5 +:100FD000238405F011F815230DF2E3220DF12C0CFA +:100FE00084F8323107AB1E46083203CE664542F81C +:100FF000080C42F8041C3346F5D1306841461060B5 +:10100000204633791371012200F0F2FE002380B2F2 +:10101000E97E0393AB7E029305F11903019301234B +:10102000009307A3D3E90023CDE90480384602F0FA +:101030003BFE0DF54E7DBDE8F08100BFAFF30080B3 +:101040009E6AC421818A46EEC823002008600008F9 +:101050002DE9F0412C4CDAB080460D46237A5BBB7B +:1010600027A9284600F0D2FF0746002842D19DF864 +:101070009D60C82E3ED801464FF4A662204600F07F +:1010800033FE4FF4807332460DF19E01C4F8F831FF +:101090004FF4007304F109002644C4F80C334FF4F4 +:1010A0004073C4F8203400F00DFE9DF89C30777238 +:1010B00023720BB9EB7E237206AC8122002127A894 +:1010C00000F012FE0122214627A800F0DBFF0023DA +:1010D00080B2E97E0393AB7E029305F1190301937D +:1010E0002823CDE904400093404605A3D3E900231B +:1010F00002F0DAFD5AB0BDE8F08100BFAFF3008026 +:1011000026417272DF25D7B7905D0020F0B5254EDD +:101110004FF48A75F1B0002405FB006596F8D830CD +:10112000D822214685F8DC303AA885F8E84006AF99 +:1011300000F0DAFD06F1090000F0CEFDD5F8E4304C +:10114000C2B206F109018DF8F0000DF1F100CDE910 +:101150003A3400F0B7FD394601223AA800F0BEFF4C +:10116000082380B2317ACDE9047001270E48CDE919 +:10117000023706F1D80301933023009307A3D3E984 +:10118000002302F091FDA04206DD02F0CFF9C5F880 +:10119000E000384671B0F0BD2046FBE778F6339F9B +:1011A00093CACD8D905D0020E03300202DE9F04FF3 +:1011B000264F87B0DFF8A080254E384602F0A0FD0C +:1011C000034600283AD00024224DADF81440A14631 +:1011D0000294A246CDE90344027B8DF8142003AAB1 +:1011E0009968406803C21B6843F0004302932B6870 +:1011F00004F5A664D3F810B002F09AF910EB0802D7 +:10120000CDF800A0284605F5A65541F1000302A936 +:10121000D8470028C8BF49F00109B4F5266FE6D1C8 +:10122000B9F1000F05D0384602F06EFD86F800A037 +:10123000C3E73378072B04D80133337007B0BDE818 +:10124000F08F024802F060FDF8E700BFE0330020B5 +:10125000C56200201034002040420F0070B50D46DA +:1012600014461E4602F07CFC50B9022E10D1012C0F +:101270000ED112A3D3E900230120C5E9002307E022 +:10128000282C10D005D8012C09D0052C0FD0002017 +:1012900070BD302CFBD10BA3D3E90023ECE70BA3EB +:1012A000D3E90023E8E70BA3D3E90023E4E70BA38A +:1012B000D3E90023E0E700BFAFF30080401DA12089 +:1012C00026812A0B78F6339F93CACD8D9E6AC4215E +:1012D000818A46EE26417272DF25D7B7F017304A71 +:1012E00039059E5638B505460E4C0021013500F0F3 +:1012F00081FCA4F82C55B4F82C0500F063FC78B1FF +:10130000B4F82C0500F06EFC014648B9B4F82C0581 +:1013100000F070FCB4F82C350133A4F82C35EAE762 +:1013200038BD00BF905D0020F8B50D4C02260D4F72 +:10133000A4F5805343F8307C237E3BB965692DB119 +:10134000284600F0C5FF284604F00CFE2046A4F510 +:10135000A65400F0BDFF012E00D1F8BD0126E7E73D +:1013600000590020C06000082DE9F04F8FB00546FD +:101370000C4600AF02F0F4FB002849D1237E022B7B +:101380001BD1E38A012B18D102F0D0F80646FFF7F3 +:10139000CBFD03464FF4C87006F51676DFF8C08221 +:1013A000B3FBF0F202FB103316FA83F3C8F80030F7 +:1013B000E37E33B9A34B00221A703C37BD46BDE82B +:1013C000F08F07F12401204600F0DEFD0028F4D163 +:1013D00007F11400FFF7C0FD97F8264007F114014C +:1013E00007F12700224604F0D5FD0028E2D10F2C9A +:1013F00008D8944B1C70D8F80030A3F51673C8F8C1 +:101400000030DAE797F82410284602F0A1FBD4E771 +:10141000E38A282B2BD010D8012B23D0052BCCD13D +:10142000BFF34F8F8849894BCA6802F4E0621343C7 +:10143000CB60BFF34F8F00BFFDE7302BBDD1844E93 +:10144000E17E327A9142B8D1607E3146002291F835 +:10145000DC50854200F0A580013201F58A71042A32 +:10146000F5D1AAE721462846FFF786FDA5E72146E4 +:101470002846FFF7EDFDA0E7B2F8EC507B6005F1E0 +:1014800003094FEA99094FEA8902D11DC908A8EB5F +:10149000C10300219D46EB46584600F025FC04F1AF +:1014A000EE012A465846314400F00CFC7B6813B923 +:1014B000012000F077FB96F8D20000F083FB044691 +:1014C00030B9307200F0A8FB204600F06BFBB1E0B1 +:1014D000D6F8D4203AB996F8D200B6F82C25824234 +:1014E00001D8FFF7FFFED6F8D4202A44944208D250 +:1014F00096F8D200B6F82C250130824201D8FFF7C9 +:10150000F1FE5FFA89F25946706800F0F5FB08B900 +:10151000C54679E0726896F8D2002A447260D6F81F +:10152000D42005EB0209C6F8D49000F04BFB8145AE +:1015300009D396F8D220D6F8D4000132001B86F8E1 +:10154000D220C6F8D400FF2D0FD80024347200F04A +:1015500063FB204600F026FB00F03CFE3D4B18816B +:1015600008B9FFF7ABF9C54627E7BB6896F8D9007D +:101570000AFB0362FB68D2F8E41082F8E83001F558 +:101580008061C2F8E030C2F8E410FFF7BFFDFFF75A +:101590000DFE96F8D920013202F0030286F8D92018 +:1015A000B6E74FF48A7A20460AFB02F505F1EA0114 +:1015B000314400F0BFFDF86000287FF4FEAE012248 +:1015C000354485F8E82001F0B1FFD5F8E020D6EDEC +:1015D000007A801A40F6B832B8EE677ADFED1E6AFC +:1015E000192838BF1920904228BF104607EE900AEC +:1015F000F8EEE77A67EEA67ADFED186AE7EE267A6C +:10160000FCEEE77AC6ED007A96F8D930BB60BA688E +:1016100073680AFB02F4321992F8E81059B1D2F853 +:10162000E410E8468B423FF427AF002182F8E8102F +:10163000C2F8E010C5467368064A9B0A013313815D +:10164000BBE600BFD933002000ED00E00400FA053E +:10165000905D0020C8230020CDCCCC3D6666663F5F +:10166000DC330020014B1870704700BFD4230020EA +:1016700038B54FF04054144B22689A4221D1627D14 +:101680000025124B12481A70C922237D09301149D6 +:1016900000F8013C4FF48073C0F8DB50C0F8EF3124 +:1016A0004FF40073C0F803334FF44073C0F817349D +:1016B00000F008FBE0222946204600F015FB01203F +:1016C00038BD0020FCE700BF9AAD44C5D4230020FC +:1016D000905D00201600003037B500F07BFD1F4CF8 +:1016E0001F4D022301221F496B7123682881204668 +:1016F0005B68984704F5805301221A49D3F8C03437 +:1017000004F5A6505B689847002317494FF48052B0 +:101710000193164B16480093164B02F0F1F9164B45 +:10172000197811B1124802F013FA01F0FFFE0446D5 +:10173000FFF7FAFB4FF4C873B0FBF3F202FB13039D +:1017400004F5167010FA83F00C4B186003F09AFF42 +:1017500008B10F232B8103B030BD00BF103400202F +:10176000C823002040420F00D82300205D1200084B +:10177000E033002069130008D4230020DC3300206C +:101780002DE9F04F00242DED028B93B00DF12C08C4 +:101790004FF0010ADFF814B2FFF708FD0A94ADF824 +:1017A00034400B94C8F804409FED798B00267E4DA1 +:1017B00037462B680DF11D0207A928468DF81CA09D +:1017C0008DF81D408DED008BD3F808900023C8479D +:1017D0009DF81C90B9F1000F1ED0102200210EA818 +:1017E00000F082FA2B6808AA0AA95F6928460DF161 +:1017F0001E03B8470FAB4F4698E8030083E8030089 +:101800009DF834300EA958468DF844300A9B0E934B +:10181000DDE9082302F056FC06F5A66605F5A65597 +:10182000B6F5266FC5D1002FC0D1604802F098F9F7 +:1018300000283FD15E4E01F079FE3368984239D3DB +:1018400001F074FE0546FFF76FFB4FF4C8738DF887 +:101850002870B0FBF3F202FB130305F5167010FAC3 +:1018600083F03060534E377817B901238DF8283054 +:10187000C7F110050EA8FFF76FFB0EABEDB20DF12F +:101880002900D919062D28BF06252A46013500F062 +:1018900019FA0AABEDB24548039318230495029355 +:1018A000454B0193012300933BA3D3E9002302F0AE +:1018B0009DF9347001F03AFE404A414D1368C31A55 +:1018C000B3F57A7F2FD3106001F032FE02460B464B +:1018D000364802F023FA354802F042F918B32B7A61 +:1018E0000EAF384E002B14BF03230223737101F097 +:1018F0001DFE4FF47A7301223946B0FBF3F03060DD +:10190000304600F019FB182380B202932E4B01934E +:1019100040F25513CDE903700093244820A3D3E986 +:10192000002302F063F92B7A93B101F0FFFD00264A +:1019300007464FF48A7895F8D900304400F0030048 +:1019400008FB005393F8E82072B10136042EF2D15F +:10195000C82003F0EBFA2B7A002B7FF417AF13B0FB +:10196000BDEC028BBDE8F08FD3F8E02042B12B68CC +:10197000BA1AFA2B38BFFA230533B2EB430FE4D37C +:10198000FFF7C4FB0028E0D1E2E700BFAFF300801F +:101990000000000000000000401DA12026812A0B4D +:1019A000F1C6A7C1D068080F10340020E033002032 +:1019B000DC330020D9330020D8330020C06200205F +:1019C000905D0020C8230020C462002010B5074CA1 +:1019D000204601F0B1F804F5A65001F0ADF8044A34 +:1019E00004490020BDE8104004F0B6BA10340020CD +:1019F0002063002029130008F8B50A25204C012295 +:101A00004FF400712046192702F0EAFD204601221A +:101A10004FF4807102F0E4FD4FF4807665200138C8 +:101A200025D00822A2F10801B646332905D84FF087 +:101A3000000C66834FF000730DE0A1F13403668360 +:101A4000B3FBF7FC07FB1C33002B0CBF4FF0010C62 +:101A50004FF0000CEED1013143EA4C238A42A361DE +:101A6000A4F818E0E1D10832A02AD8D0DAE7013D85 +:101A700042F2107003F05AFA15F0FF05BFD1F8BD1D +:101A8000000402582DE9F84F4FF47A7306460D46CC +:101A9000002402FB03F7DFF85080DFF8509098F93C +:101AA00000305FFA84FA5A1C01D0A34212D159F8CF +:101AB00024002A4631460368D3F820B03B46D84775 +:101AC000854207D1074B012083F800A0BDE8F88FBD +:101AD0000124E4E7002CFBD04FF4FA7003F026FA5F +:101AE0000020F3E70C630020182200201C220020B5 +:101AF000002307B5024601210DF107008DF80730DC +:101B0000FFF7C0FF20B19DF8070003B05DF804FBAC +:101B10004FF0FF30F9E700000A46042108B5FFF74F +:101B2000B1FF80F00100C0B2404208BD074B0A4639 +:101B300030B41978064B53F82140014623682046FB +:101B4000DD69044BAC4630BC604700BF0C6300202D +:101B50001C220020A086010070B5104C0025104EFC +:101B600003F0A4FB20803068238883420CD8002532 +:101B70002088013803F096FB23880544013BB5F526 +:101B8000802F2380F4D370BD03F08CFB33680544B1 +:101B90000133B5F5802F3360E5D3E8E70E6300200D +:101BA000C862002003F050BC00F1006000F5802006 +:101BB0000068704700F10060920000F5802003F09B +:101BC000D1BB0000054B1A68054B1B889B1A83424A +:101BD00002D9104403F066BB00207047C8620020A1 +:101BE0000E630020024B1B68184403F061BB00BF6A +:101BF000C8620020024B1B68184403F06BBB00BF97 +:101C0000C86200200020704700F1FF5000F58F10DF +:101C1000D0F8000870470000064991F8243033B12D +:101C200000230822086A81F82430FFF7C3BF01208F +:101C3000704700BFCC620020014B1868704700BF9E +:101C40000010005C194B01380322084470B51D6870 +:101C5000174BC5F30B042D0C1E88A6420BD15C68F4 +:101C60000A46013C824213460FD214F9016F4EB16D +:101C700002F8016BF6E7013A03F10803ECD1814267 +:101C80000B4602D22C2203F8012B0424094A1688A1 +:101C9000AE4204D1984284BF967803F8016B013CB0 +:101CA00002F10402F3D1581A70BD00BF0010005CAD +:101CB000242200204C60000870470000704700009C +:101CC00070470000002310B5934203D0CC5CC4548D +:101CD0000133F9E710BD000003460246D01A12F99D +:101CE000011B0029FAD1704702440346934202D0F7 +:101CF00003F8011BFAE770472DE9F8431F4D14461E +:101D00000746884695F8242052BBDFF870909CB3B4 +:101D100095F824302BB92022FF2148462F62FFF787 +:101D2000E3FF95F824004146C0F1080205EB80006E +:101D3000A24228BF2246D6B29200FFF7C3FF95F811 +:101D40002430A41B17441E449044E4B2F6B2082E7B +:101D500085F82460DBD1FFF75FFF0028D7D108E0CA +:101D60002B6A03EB82038342CFD0FFF755FF002895 +:101D7000CBD10020BDE8F8830120FBE7CC62002036 +:101D8000024B1A78024B1A70704700BF0C63002098 +:101D90001822002010B5074C4FF4E13306492068A3 +:101DA0000B6002F089FF60680349BDE8104002F053 +:101DB00083BF00BF1C220020F4620020094B1822C0 +:101DC000002110B504461846FFF78EFF064A074B60 +:101DD00001461278046053F82200BDE8104002F07A +:101DE0006BBF00BFF46200200C6300201C220020A7 +:101DF0002DE9F0470D46044600219046284640F262 +:101E00007912FFF771FF234620220021284604F1B2 +:101E1000220702F0F9F8231D022220212846C026BD +:101E200002F0F2F8631D03222221284602F0ECF8AA +:101E3000A31D03222521284602F0E6F804F1080339 +:101E400010222821284602F0DFF804F110030822AE +:101E50003821284602F0D8F804F111030822402165 +:101E6000284602F0D1F804F112030822482128463E +:101E700002F0CAF804F1140320225021284602F08F +:101E8000C3F804F1180340227021284602F0BCF880 +:101E900004F120030822B021284602F0B5F804F12D +:101EA00021030822B821284602F0AEF83146083650 +:101EB0003B4608222846013702F0A6F8B6F5A07F77 +:101EC000F4D1002704F1330A04F132030822314629 +:101ED000284602F099F894F832304FEAC7099F4239 +:101EE00009F5A47615D3B8F1000F08D1314609F2EF +:101EF0004F1604F599730722284602F085F8274605 +:101F00003B1B94F8322193420CD3F01DC008BDE86E +:101F1000F0870AEB0703082231462846013702F012 +:101F200073F8D8E707F233133146082228460836FB +:101F3000013702F069F8E3E713B5044608460021CB +:101F40002022234601900160C0F8031002F05CF8E3 +:101F5000231D01980222202102F056F8631D0198EA +:101F60000322222102F050F8A31D0198032225210B +:101F700002F04AF8019804F108031022282102F027 +:101F800043F8072002B010BD0023F7B50E46047FCA +:101F9000072200911946054601F0FAFE731C012242 +:101FA000072100932846002301F0F2FEC4B9B31CB8 +:101FB000052208212846009323460D2401F0E8FE5F +:101FC0003746BB1BB278934211D307342B7FA88AC4 +:101FD000E408BBB9844294BF0020012003B0F0BDE7 +:101FE000AB8A0824DB00083BDB08B370E8E7FB1C86 +:101FF000214608222846009300230834013701F0C7 +:10200000C7FEDEE7201A18BF0120E7E70023F7B577 +:102010000E46047F082200911946054601F0B8FEDD +:10202000731CC4B908220093234610241146284685 +:1020300001F0AEFE01235F1C7278013B934211D385 +:1020400007342B7FA88AE408BBB9844294BF0020E0 +:10205000012003B0F0BDAB8A0824DB00083BDB089D +:102060007370E7E7F31921460822284600930023FE +:1020700001F08EFE08343B46DDE7201A18BF012030 +:10208000E7E70000F8B50E46054614460021812218 +:102090003046FFF729FE2B4608220021304601F08A +:1020A000B3FF7CB90F246B1C07220821304601F0D6 +:1020B000ABFF01235F1C6A78013B934204D3E01D10 +:1020C000C008F8BD0824F4E7EB1921460822304681 +:1020D00001F09AFF08343B46ECE70000F8B50E46E5 +:1020E000054614460021CE223046FFF7FDFD2B4663 +:1020F00028220021304601F087FF7CB9302405F109 +:10210000080308222821304601F07EFF2F467B1B62 +:102110002A7A934204D3E01DC008F8BD2824F5E7CD +:1021200007F109032146082230460834013701F03F +:102130006BFFECE7F7B5047F0E46009101231022F8 +:102140000021054601F024FEC4B9B31C0922102168 +:10215000284600932346192401F01AFE3746728858 +:10216000BB1B9A4211D807342B7FA88AE408BBB95D +:10217000844294BF0020012003B0F0BDAB8A10243C +:10218000DB00103BDB087380E8E73B1D214608229B +:102190002846009300230834013701F0F9FDDEE7FB +:1021A000201A18BF0120E7E730B50A44084D9142D4 +:1021B0000DD011F8013B5840082340F30004013BC7 +:1021C0002C4013F0FF0384EA5000F6D1EFE730BD56 +:1021D0002083B8EDF7B5384A6B46106851686A46F7 +:1021E00003C308233549364803F0E4FE04460028BB +:1021F00033D10A25334A6B46106851686A4603C3D7 +:10220000082331492E4803F0D5FE0446002849D062 +:102210000369B3F5E01F45D8B0F8661040F21342E9 +:1022200091423FD1294A024402F15C018B4239D3E9 +:102230005C3B234900209E1AFFF7B6FF04F16401BE +:10224000074632460020FFF7AFFFA3689F4229D11F +:10225000E368984208BF002524E00369B3F5E01F56 +:1022600027D8418B40F21342914220D1174A0244B1 +:1022700002F110018B4218D3103B114900209D1A26 +:10228000FFF792FF04F1180106462A460020FFF7E7 +:102290008BFFA3689E4202D1E368984201D00D25CE +:1022A000A8E70025284603B0F0BD1025A2E70C25BD +:1022B000A0E70B259EE700BF5C600008DCFF1B0069 +:1022C000000004086560000890FF1B000800FCF790 +:1022D00010B5037C044613B9006803F053FE204692 +:1022E00010BD00000023BFF35B8FC360BFF35B8FA3 +:1022F000BFF35B8F8360BFF35B8F7047BFF35B8F70 +:102300000068BFF35B8F704770B505460C30FFF770 +:10231000F5FF044605F108063046FFF7EFFFA0423F +:1023200006D96D683046FFF7E9FF2544281A70BDCD +:102330003046FFF7E3FF201AF9E7000070B50546C5 +:102340004068A0B105F10C0605F10800FFF7D6FFC3 +:1023500004463046FFF7D2FF844204F1FF34304692 +:1023600094BF6D680025FFF7C9FF2C44201A70BD8B +:1023700038B50C460546FFF7C7FFA04210D305F15C +:102380000800FFF7BBFF04446868BFF35B8FB4FB32 +:10239000F0F100FB11440120AC60BFF35B8F38BD4E +:1023A0000020FCE72DE9F041144607460D46FFF7F3 +:1023B000C5FF844228BF0446D4B1B84658F80C6B18 +:1023C0004046FFF79BFF3044286040467E68FFF799 +:1023D00095FF331A9C4203D801206C60BDE8F08160 +:1023E000A41B6B603B682044AB60E8600220F5E70B +:1023F0002046F3E738B50C460546FFF79FFFA0429D +:1024000010D305F10C00FFF779FF04446868BFF3AF +:102410005B8FB4FBF0F100FB11440120EC60BFF3D3 +:102420005B8F38BD0020FCE72DE9FF4188466946F7 +:1024300007466C46FFF7B6FF002506B204EBC6065A +:10244000B4420AD0626808EB050120680834FFF73F +:1024500039FC54F8043C1D44F2E729463846FFF79E +:10246000C9FF284604B0BDE8F0810000F8B5054674 +:102470000C300F46FFF742FF05F1080604463046D0 +:10248000FFF73CFFA042304688BF6C68FFF736FF7D +:10249000201A386020B12C683046FFF72FFF204407 +:1024A000F8BD000073B5144606460D46FFF72CFF35 +:1024B0008442019028BF0446DCB101A93046FFF7F1 +:1024C000D5FF019B33B93268C5E90233C5E9002461 +:1024D00001200CE09C42286038BF0194019884429E +:1024E0006860F5D93368241A0220AB60EC6002B052 +:1024F00070BD2046FBE700002DE9FF410F4669460D +:102500006C46FFF7CFFF00B2002604EBC005AC42DB +:1025100009D0D4F80480B81954F8081B4246464440 +:10252000FFF7D0FBF3E7304604B0BDE8F0810000D0 +:1025300038B50546FFF7E0FF044601462846FFF799 +:1025400017FF204638BD0000302383F3118862B6A0 +:1025500070470000002383F3118862B670470000C3 +:10256000012070477047000010B4134602681468D9 +:102570000022A4465DF8044B6047000000F580503F +:1025800090F859047047000000F5805090F852040C +:102590007047000000F5805090F958047047000023 +:1025A0004E20704700F5805208B5FFF7CDFFD2F8F6 +:1025B0009C34D2F898041844D2F894341844D2F8D1 +:1025C0007C341844D2F88C341844D2F88834184437 +:1025D000FFF7C0FF08BD00002DE9F04F0C4600F5E5 +:1025E000805185B01F4691F8523405469046BDF89B +:1025F00038909BB1D1F878340133C1F8783423682E +:102600009A0006D4237B082B0BD9627B0AB10F2BCF +:1026100007D9D1F87C340133C1F87C344FF0FF3056 +:102620000FE0FFF791FFEB6AD3F8C42012F4001A11 +:102630000AD0D1F8803400200133C1F88034FFF78C +:1026400089FF05B0BDE8F08F22684FF0480BD3F842 +:10265000C460002A6B6AC6F30446B4BF42F08042ED +:1026600092041BFB063BCBF8002023685B004FEA7B +:10267000066344BF42F00052CBF80020227B43EABD +:1026800002437201CBF80430607B18B343F440136B +:10269000CBF80430D1F8B0340133C1F8B034AB1802 +:1026A00003F58353197B41F020011973207B0392BA +:1026B00000F098FF0330039A80105FFA8AF30AF162 +:1026C000010A83420DDA04EB83010BEB83034968B3 +:1026D0009960F2E7AB1803F58353197B60F345115A +:1026E000E3E70121EB6A04F10C00B140C3F8D0101C +:1026F000AB18214603F58253C3E9048705EB461363 +:1027000003F58253183351F804CB814243F804CBCC +:10271000F9D109882A442846198041F26803D65025 +:1027200002F5805209F0030392F86C1043F0100395 +:1027300021F01F010B43214682F86C304246FFF71F +:1027400009FF3B46CDF8009000F012FF012078E72A +:102750002DE9F04700F58056044696F85254002DB6 +:1027600040F00181037C032B40F092802B462846E9 +:102770002F465FFA83FC944510DA01EBCC0E51F83A +:102780003CC0BCF1000F04DBDEF804C0BCF1000F5C +:1027900002DB01370133ECE70130FBE7FFF7D4FE42 +:1027A000E36AF0B9D3F8800040F00200C3F880007B +:1027B0004E23E06A002F6ED1D0F8803043F0010341 +:1027C000C0F88030694B6A4A1B6803F1805303F5F7 +:1027D0002C539B009342A36240F2AF80654800F007 +:1027E0009FFE4D2842D8DFF884814FEA004EDFF883 +:1027F0008891D8F800C04EEA8C0EC3F884E00CF142 +:10280000805303F52C539B00636100EB0C03D4F859 +:102810002CC0C8F80030C0F14E03DCF8800040F056 +:102820003000CCF880004FF0000CD4F81480E6465D +:102830005FFA8CF08242BCDD01EBC00A51F8300037 +:10284000002810DBDAF804A0BAF1000F0BDA09EA6D +:1028500000400AF07F0A40EA0A0040F0084048F8C9 +:102860002E000EF1010E0CF1010CE1E79A6922F045 +:1028700001029A6100F05AFE0646E36A9B69D90795 +:1028800004D500F053FE831BFA2BF6D9FFF762FE46 +:102890002846BDE8F087B7EB530F3DD2DFF8CCE018 +:1028A0004FEA074CDEF800304CEA830CC0F888C0D1 +:1028B00003F1805003EB4703002700F52C50CEF8BE +:1028C0000030BC468000A061E06AD0F8803043F060 +:1028D0000C03C0F88030D4F818E0FBB29A427FF7BE +:1028E00071AF51F8330001EBC3080028D8F8043069 +:1028F00001DB002B0EDB20F0604023F0604340F052 +:10290000005043F000434EF83C000EEBCC000CF1BD +:10291000010C43600137E0E7836923F00103836121 +:1029200000F004FE0646E36A9B69DA07AED500F0C4 +:10293000FDFD831BFA2BF6D9A8E7E26A936923F021 +:102940000103936100F0F2FD0746E36A9B69DB0730 +:1029500005D500F0EBFDC31BFA2BF6D996E7012550 +:1029600086F8525492E7002592E700BF1C630020CE +:10297000FCB50040706000080000FF0713B500F5CB +:1029800080540191606CFFF7D9FC1F280AD92022DE +:102990000199606CFFF748FDA0F1200358425841AF +:1029A00002B010BD0020FBE700F5805008B5FFF72E +:1029B000CBFD406CFFF796FCBDE80840FFF7CABDB1 +:1029C0000022026082814260826070470022002300 +:1029D00010B5C0E90023002304460C3020F8043C65 +:1029E000FFF7EEFF204610BD2DE9F047074688B0FF +:1029F0009A46884607F5805468469146FFF7A4FD3D +:102A0000FFF7E4FF606CFFF77FFC1F282DD9202221 +:102A10006946606CFFF78CFD202826D194F852346B +:102A20001BB303AD444605AB2E46083403CE9E428D +:102A300044F8080C44F8041C3546F5D1306841468A +:102A400020603846B388A380DDE90023C9E900236C +:102A5000BDF808304A46AAF80030FFF77BFD534620 +:102A600008B0BDE8F04700F071BD0020FFF772FD2F +:102A700008B0BDE8F08700002DE9F84F00230646B6 +:102A800000F58154054688461034C0E90133274BD0 +:102A900046F8303B374638462037FFF797FFA742C6 +:102AA000F9D105F580544FF4805305F5A3594FF043 +:102AB000000A26630026676405F5835709F11009AB +:102AC0004FF0000B1037E663C4E90D36012384F89C +:102AD000403084F84830A7F11800203747E910ABA0 +:102AE000FFF76EFF47F8286C4F45F4D1B8F1010F9E +:102AF00084F85884A4F85A64A4F85C64A4F85E646A +:102B000084F86064A4F86264A4F86464A4F8666459 +:102B100084F8686402D9064800F002FD054B284697 +:102B200053F82830EB62BDE8F88F00BFC0600008A2 +:102B300094600008B0600008044B10B51978044692 +:102B40004A1C1A70FFF798FF204610BD1963002039 +:102B50002DE9F04700295FD0304F3148B7FBF1F540 +:102B600081428CBF0A201120431EB5FBF0FC00FB04 +:102B70001C50DCB220B1022B1846F5D8002037E0FB +:102B80000CF1FF35B5F5806F32D2C4EBC4094FF4B8 +:102B90007A7009F103034FEAE308C3F3C70308F1AE +:102BA000010AA4EB030E08FB00085FFA8EF65AFA3E +:102BB0008EFEB8FBFEFEBEF5617F1BDC1FFA8EF4B5 +:102BC000581C56FA80F00CFB00FCB7FBFCFC61457E +:102BD000D4D1013BDBB20F2BD0D8711E0020C9B27B +:102BE000072905D810710120148055805371917107 +:102BF000BDE8F08709F1FF334FEAE30EC3F3C703E3 +:102C00000EF10108E41A0EFB0000E6B258FA84F453 +:102C1000B0FBF4F4A4B2D3E70846E9E700B4C40477 +:102C20003F420F0038B540F27772C36A154CC3F8C3 +:102C3000BC200722C36AC3F8C8202268C16A930077 +:102C400043F4C023C1F8A03002F1805302F16C01BB +:102C5000C56A03F52C53EA3289009B00226041F0DB +:102C6000E061094AC361C5F8C01003F5D87103F5E6 +:102C70006A7341629342836202D9044800F050FCB7 +:102C800038BD00BF1C630020FCB500407060000828 +:102C90002DE9F04F00F58055994689B0044695F826 +:102CA00058348A469046022B04D90027384609B08A +:102CB000BDE8F08F9B4A52F8231009B942F823006F +:102CC0009949C4F80CA00B7884F81090C3B9FFF7A9 +:102CD0003BFC964BD3F8EC2042F48072C3F8EC2016 +:102CE000D3F8942042F48072C3F89420D3F894204F +:102CF00022F48072C3F8942001230B70FFF72AFCA2 +:102D000095F851346BB9FFF71FFC894A95F8583490 +:102D1000D35CEBB1012B24D0012385F85134FFF7AC +:102D200019FCFFF711FCE26A936923F01003936129 +:102D300000F0FCFB0746E36A9E6916F0080617D010 +:102D400000F0F4FBC31BFA2BF5D9FFF703FCACE74B +:102D50000321132001F0FAFD0321152001F0F6FDF7 +:102D6000DAE70321142001F0F1FD03211620F5E735 +:102D70009A6942F001029A6100F0D8FB0746E36AC3 +:102D80009A69D00705D400F0D1FBC31BFA2BF6D902 +:102D9000DBE79A69002704F5825B42F002020BF13F +:102DA000100B9A61E36A5F65FFF7D4FB686CFFF76D +:102DB00099FA202200216846FEF796FF02A8FFF745 +:102DC000FFFD6A460BEB06030DF1180E06979446BD +:102DD0000833BCE80300F44543F8080C43F8041C2E +:102DE0006246F4D1DCF8000020361860B6F5806F3A +:102DF0009CF804201A71DCD1002304F5A25251463C +:102E000020461A3285F8503485F85334FFF7A0FE77 +:102E1000074690B9E26A936923F00103936100F0D9 +:102E200085FB0546E36A9B69D9077FF53EAF00F055 +:102E30007DFB431BFA2BF5D937E795F85F6495F8CE +:102E40005E243602C5F86CA4E36A46EA426695F849 +:102E500060241643B5F85C2446EA0246DE61B8F108 +:102E6000000F29D004F5A352414620460232FFF755 +:102E70006FFE90B9E26A936923F00103936100F059 +:102E800055FB0546E36A9B69DA077FF50EAF00F054 +:102E90004DFB431BFA2BF5D907E795F8683495F8F5 +:102EA00067141B01C5F87084E26A43EA0123B5F890 +:102EB000641443EA0143D360E36A00262046C3F862 +:102EC000BC60FFF7AFFE85F859646FF04042E36ADB +:102ED0001A65184AE36A5A654FF00222E36A9A6556 +:102EE0004FF0FF32E36AC3F8E0200322E36A914522 +:102EF000DA653FF4DBAEE26A936923F00103936184 +:102F000000F014FB0646E36A9B69DB0705D500F079 +:102F10000DFB831BFA2BF6D9C7E6012385F8523443 +:102F2000C4E600BF1063002018630020004402586C +:102F3000A8600008550200022DE9F04F054689B04F +:102F400090469946002741F2680A00F58056EB6AE0 +:102F5000D3F8D830FB40D8074ED505EB47124FEADF +:102F6000471B52441379190746D4D6F884340133E9 +:102F7000C6F8843405F5A553C3E9008913799A0688 +:102F800005EB0B0248BFD6F8B434524444BF0133BA +:102F9000C6F8B434137943F008031371DB0723D563 +:102FA00096F8533403B305F582546846FFF70EFDD7 +:102FB00003AB18345C4404F1080C2068083454F85E +:102FC000041C1A46644503C21346F6D120686946BC +:102FD00010602846A2889A800123ADF808302B683B +:102FE000CDE90089DB6B9847D6F8543423B1D6F885 +:102FF000A8340133C6F8A8340137202FA7D109B06F +:10300000BDE8F08F2DE9F04F0F468DB0044600F07B +:103010008FFA82468946002F5BD1E36AD3F8A0205D +:1030200012F4FE0F03D100200DB0BDE8F08FD3F8ED +:10303000A420920141BF04F58051D1F898240132B7 +:10304000C1F89824D3F8A4205606ECD0D3F8A450A5 +:10305000E669C5F305254823E8464FF0000B03FB5E +:1030600005664046FFF7ACFC326851004ABF22F0CB +:103070006043C2F38A4343F00043920048BF43F0E9 +:1030800080430093736813F400131BBF012304F5FE +:1030900080528DF80D308DF80D301EBFD2F8B83447 +:1030A0000133C2F8B834F38803F00F038DF80C3005 +:1030B0009DF80C0000F096FA5FFA8BF3984225D940 +:1030C000F2180CA90BF1010B127A0B4403F82C2C0B +:1030D000EEE7012FA7D1E36AD3F8B02012F4FE0F78 +:1030E000A1D0D3F8B420950141BF04F58051D1F8A7 +:1030F00098240132C1F89824D3F8B420500692D015 +:10310000D3F8B450266AC5F30525A4E7EFB9E36AFE +:10311000C3F8A85004A807ADFFF758FC98E80F00C3 +:1031200007C52B800023204604A9ADF8183023687A +:1031300004F58054DB6BCDE904A9984758B1D4F865 +:1031400090340133C4F890346EE7012FE2D1E36A82 +:10315000C3F8B850DEE7D4F8943401200133C4F842 +:10316000943461E7F8B505460F4600F58054012612 +:1031700039462846FFF746FF10B184F85364F7E755 +:10318000D4F8543423B1D4F8A8340133C4F8A834A3 +:10319000F8BD0000C36AF0B51A6C12F47F0F2BD093 +:1031A0001B6C00F5805441F268054FF0010CC4F827 +:1031B000AC340023471900EB43125E012A44117915 +:1031C00011F0020F15D0490713D4B959C66A0CFA89 +:1031D00001F1D6F8CCE011EA0E0F0AD0C6F8D410EF +:1031E000117941F004011171D4F88C240132C4F832 +:1031F0008C240133202BDED1F0BD00002B4B70B5A9 +:103200001E561B5C012B30D8294D2A4A55F8233015 +:1032100052F8264013B349B3236D9A0510D54FF4E5 +:103220000073236500F084F950EA01020B4602D0D6 +:10323000013861F10003024655F82600FFF77CFED5 +:10324000236D9B012CD54FF0007255F82630226576 +:1032500003F58053012283F8592421E001232365DB +:10326000102323654FF48053236570BD236DDA0767 +:1032700002D4236D5B0706D50523002155F82600EF +:103280002365FFF76FFF236DD80602D4236D59061F +:1032900006D55023012155F826002365FFF762FF6C +:1032A00055F82600BDE87040FFF774BFAC60000819 +:1032B00010630020B060000808B5FFF745F9FFF77C +:1032C00069FFBDE80840FFF745B90000C36AD3F8BD +:1032D000C00010F07C5005D0D3F8C40080F400107A +:1032E000C0F340507047000000F5805008B5FFF76C +:1032F0002BF9406CFFF708F8FFF72CF943090CBFD6 +:103300000120002008BD000000F5805393F85924E7 +:1033100062B1C16A8A6922F001028A61D3F89C24F1 +:103320000132C3F89C24002283F85924704700001E +:103330002DE9F74300F5825198460025FFF704F97F +:10334000103141F2680E4FF0010900F5805C00EB8E +:103350004514744423795E071CD4DB061AD58E69A4 +:10336000C36A09FA06F6D3F8CC703E4212D04F6811 +:1033700001970F689742019F77EB08070AD2C3F8BD +:10338000D460237943F004032371DCF888340133DB +:10339000CCF8883401352031202DD8D103B0BDE8D8 +:1033A000F043FFF7D7B80000F8B51E4600230F46DC +:1033B000054613701446FFF797FF80F00100387040 +:1033C0001EB12846FFF782FF2070F8BD2DE9F04FAF +:1033D00085B099460D468046164691F800B0DDE965 +:1033E0000EA302931378019300F0A2F82B78044601 +:1033F0000F4613B93378002B42D022463B46404655 +:10340000FFF796FFFFF758FFFFF77EFF4B46324668 +:103410002946FFF7C9FF2B7833B1BBF1000F03D06A +:10342000012005B0BDE8F08F337813B1019B002B6C +:10343000F6D108F5805303935445029B77EB0303C1 +:103440001ED2039BD3F85404D0B10368AAEB040145 +:10345000DB6889B298474B46324629464046FFF71B +:10346000A3FF2B7813B1BBF1000FD9D1337813B17F +:10347000019B002BD4D100F05BF804460F46DBE73C +:103480000020CEE7002108B50846FFF7B7FEBDE8EB +:10349000084001F06DB8000008B501210020FFF7D9 +:1034A000ADFEBDE8084001F063B8000008B500219A +:1034B0000120FFF7A3FEBDE8084001F059B8000065 +:1034C000012108B50846FFF799FEBDE8084001F064 +:1034D0004FB8000000B59BB0EFF309816822684641 +:1034E000FEF7F0FBEFF30583014B9B6BFEE700BF9C +:1034F00000ED00E008B5FFF7EDFF000000B59BB060 +:10350000EFF3098168226846FEF7DCFBEFF30583E1 +:10351000014B5B6BFEE700BF00ED00E0FEE7000043 +:103520000FB408B5029801F097FCFEE701F0D8BF90 +:1035300001F0B0BF13B56C46031D84E8060094E8A3 +:10354000030083E80500012002B010BD73B5856853 +:10355000019155B11B885B0707D4D0E900369B6BFE +:103560009847019AC1B23046A847012002B070BD09 +:10357000F0B5866889B005460C465EB1BDF83830B6 +:103580005B070AD4D0E900379B6B98472246C1B24B +:103590003846B047012009B0F0BD0022002301F1F8 +:1035A0000806CDE9002300230A46ADF8083003AB36 +:1035B0001068083252F8041C1C46B24203C4234669 +:1035C000F6D1106820609288A280FFF7B1FF042333 +:1035D000ADF808302B68CDE90001DB6B6946284661 +:1035E0009847D7E7082817D909280CD00A280CD003 +:1035F0000B280CD00C280CD00D280CD00E2814BF92 +:103600004020302070470C20704710207047142055 +:103610007047182070472020704700002DE9F041C6 +:10362000456A15B94162BDE8F0814B68AC4623F0AC +:103630006047C3F38A464FEAD37EC3F3807816EA25 +:10364000230638BF3E462B465A68BEEBD27F22F097 +:1036500060440AD0002A18DAA40CB44217D19D4263 +:103660000FD10D60DEE71346EEE7A74207D102F067 +:103670008044C2F3807242450BD054B1EFE708D2C8 +:10368000EDE7CCF800100B60CDE7B44201D0B442B6 +:10369000E5D81A689C46002AE5D11960C3E7000006 +:1036A0002DE9F047089D01F0070400EBD1004FF42D +:1036B0007F494FEAD508224405F00705944201D11D +:1036C000BDE8F08704F0070705F0070A111B08EBB7 +:1036D000D50E57453E4613F80EC038BF5646C6F1C4 +:1036E00008068E4228BF0E46E108415C344435444A +:1036F000B94029FA06F721FA0AF1FFB28CEA010172 +:1037000047FA0AF739408CEA010C03F80EC0D5E7F6 +:1037100080EA0120082341F2210201B2013B40006E +:10372000002980B2B8BF504013F0FF03F5D17047B5 +:1037300038B50C468D18A54200D138BD14F8011BD0 +:10374000FFF7E6FFF7E7000042684AB11368818996 +:103750004360438901339BB29942438138BF8381DF +:103760001046704770B588B0044620220D46684662 +:103770000021FEF7B9FA20460495FFF7E5FF02465F +:1037800060B16B46054608AE1C46083503CCB44212 +:1037900045F8080C45F8041C2346F5D1104608B03E +:1037A00070BD0000082817D909280CD00A280CD0B1 +:1037B0000B280CD00C280CD00D280CD00E2814BFD0 +:1037C0004020302070470C20704710207047142094 +:1037D000704718207047202070470000082817D92C +:1037E0000C280CD910280CD914280CD918280CD95D +:1037F00020280CD930288CBF0F200E2070470920BC +:1038000070470A2070470B2070470C2070470D202E +:10381000704700002DE9F843078C0446072F1ED996 +:1038200000254FF6FF73D0E90298C5F12006A5F1F7 +:10383000200029FA05F1083508FA06F628FA00F002 +:10384000314301431846C9B2FFF762FF402D0346DA +:10385000EBD13A46E169BDE8F843FFF769BF4FF69F +:10386000FF70BDE8F883000010B54B6823B9CA8A21 +:1038700063F30902CA8210BD04691A681C600361FF +:10388000C38A013BC3824A60EFE700002DE9F84F8D +:103890001D46CB8A0F468146C3F30901924605298E +:1038A0000B4630D00020AAB207F11A049EB21FFACC +:1038B00080F8042E0FD8904503F1010306D30A4483 +:1038C000FB8A62F309030120FB821AE01AF8006008 +:1038D0000130E654EAE79045F1D2A1F1050B1C2333 +:1038E0007C68BBFBF3F203FB12BB1FFA8BF6002CC8 +:1038F00045D14846FFF728FF044638B978606FF095 +:103900000200BDE8F88F4FF00008E6E700260660E9 +:103910007860ADB24FF0000B454510D90AEB0803B3 +:10392000221D13F8011B08F101089155B1B21FFACD +:1039300088F81B292BD0454506F10106F1D8FB8AF2 +:10394000C3F30902154465F30903BCE701321C46C1 +:1039500092B22368002BF9D16B1F0B441C21B3FBDF +:10396000F1F301339BB29A42D3D2BBF1000FD0D115 +:103970004846FFF7E9FE20B9C4F800B0BFE70122CE +:10398000E7E7C0F800B05E4620600446C1E7454561 +:10399000D5D94846FFF7D8FE08B92060AFE7C0F890 +:1039A00000B0002620600446B6E700002DE9F04F85 +:1039B0001C46074688462DED028B83B05B69019259 +:1039C000002B00F09A80238C2BB1E269002A00F0D2 +:1039D0009480072B35D807F10C00FFF7B5FE05469C +:1039E00038B96FF00205284603B0BDEC028BBDE884 +:1039F000F08F14220021FEF777F9228CE16905F19E +:103A00000800FEF75FF9208C48F00041013080B2D9 +:103A1000FFF7E4FEFFF7C6FE013880B220840130D4 +:103A2000287438466369228C1B782A4403F01F03EC +:103A300063F03F03137269602946FFF7EFFD01252C +:103A4000D1E74FF0000900F10C034FF0800A4E4619 +:103A50004D4608EE103A18EE100AFFF775FE834641 +:103A60000028BED014220021FEF73EF9002E3AD1E4 +:103A7000019B0222ABF808300BF1080E1FFA82FC02 +:103A8000218C0CF10100BCF1060F80B201D88E42EE +:103A90002BD3FFF7A3FE8E4208BF4FF0400AFFF77B +:103AA00081FE62690138013512781BFA80F101301C +:103AB00002F01F022DB242EA491289F001094AEAD6 +:103AC000020A48F0004281F808A059468BF810001D +:103AD0003846CBF804204FF0000AFFF79FFD238CF7 +:103AE000B342B8D17FE70022C6E7E169895D0136BC +:103AF0000EF80210B6B20132C0E76FF0010572E7AE +:103B0000F8B515460E463022002104461F46FEF742 +:103B1000EBF8069BB5F5001FA760636004F1100089 +:103B2000079B34BF6A094FF6FF72E661A362002368 +:103B300097B29A4206D800230360A782E3822383C8 +:103B4000E360F8BD0660013330462036F1E700003F +:103B500003781BB94BB2002BC8BF0170704700003F +:103B600000787047F8B50C46C969074611B9238C2F +:103B7000002B37D1257E1F2D34D8387828BB228CD6 +:103B8000072A2CD8268A36F003032BD14FF6FF7074 +:103B9000FFF7CEFD20F0010031024FF6FF72400426 +:103BA00041EA0561400C41EA402523462946384652 +:103BB000FFF7FCFE002807DD626913780133DBB2F2 +:103BC0001F2B88BF00231370F8BD218A2D0645EAFC +:103BD000012505432046FFF71DFE0246E5E76FF08D +:103BE0000300F1E76FF00100EEE7000070B58AB066 +:103BF000044616460021282268461D46FEF774F842 +:103C0000BDF8383069462046ADF810300F9B05935B +:103C10009DF840308DF81830119B0793BDF848305F +:103C2000CDE90265ADF82030FFF79CFF0AB070BD0A +:103C30002DE9F041D36905460C4616460BB9138CA5 +:103C40005BBB377E1F2F28D895F80080B8F1000F96 +:103C500026D03046FFF7DEFD3378210202462846A3 +:103C600041EAC331338A41EA080141EA076141EA86 +:103C70000341334641F08001FFF798FE00280ADD3A +:103C80003378012B07D1726913780133DBB21F2B14 +:103C900088BF00231370BDE8F0816FF00100FAE7E0 +:103CA0006FF00300F7E70000F0B58BB004460D4657 +:103CB00017460021282268461E46FEF715F89DF893 +:103CC0004C30294620465A1E534253416A468DF8CD +:103CD00000309DF84030ADF81030119B05939DF8F1 +:103CE00048308DF81830149B0793BDF85430CDE957 +:103CF0000276ADF82030FFF79BFF0BB0F0BD00005F +:103D0000406A00B104307047436A1A68426202692F +:103D10001A600361C38A013BC38270472DE9F041F9 +:103D2000D0F8208014461D46184E4146002709B998 +:103D3000BDE8F081D1E90223A21A65EB03039642A4 +:103D400077EB03031ED2036A8B420DD1FFF78CFD84 +:103D5000036A1B68036203690B60C38A0161013B4C +:103D6000016AC3828846E2E7FFF77EFD0B68C8F868 +:103D7000003003690B60C38A0161013BC382D8F83C +:103D80000010D4E788460968D1E700BF80841E0090 +:103D90002DE9F04F8BB00D4614469B46DDF8509050 +:103DA0008046002800F01881B9F1000F00F014815E +:103DB000531E3F2B00F21081012A03D1BBF1000FEB +:103DC00040F00A810023CDE90833B8F81430B5EB90 +:103DD000C30F4FEAC30703D300200BB0BDE8F08F39 +:103DE0002B199F42D8F80C3036BF7F1B2746FFB2F5 +:103DF0001BB9D8F81030002B7AD0272D4DD8C5F13B +:103E0000280600232946B742009308ABD8F80800DB +:103E10002CBFF6B23E46A7EB060A354432465FFA9F +:103E20008AFAFFF73DFCB8F81430282103F100535B +:103E3000053BDB000493D8F80C300393039B13B1CC +:103E4000BAF1000F2CD1D8F8100040B1BAF1000F30 +:103E500005D008AB5246691A0096FFF721FC38B22C +:103E6000002FB9D066070AD00AAB624203EBD40137 +:103E700002F0070211F8083C134101F8083C082C35 +:103E80003DD9102C40F2B580202C40F2B780BBF118 +:103E9000000F00F09C80082335E0BA460026C2E7F8 +:103EA000049BE02B28BFE02306930B44AB42059311 +:103EB00015D95A1B0398691A0096924508AB00F170 +:103EC000040034BF5246D2B20792FFF7E9FB079ACB +:103ED0001644AAEB020A1544F6B25FFA8AFA049B6A +:103EE000069A05999B1A0493039B1B680393A5E705 +:103EF00000933A4608AB2946D8F80800ADE7BBF175 +:103F0000000F13D00123B4EBC30F6BD0082C12D8D1 +:103F10009DF82030621E23FA02F2D50706D54FF035 +:103F2000FF3202FA04F423438DF820309DF820304C +:103F300089F8003051E7102C12D8BDF82030621EED +:103F400023FA02F2D10706D54FF0FF3202FA04F449 +:103F50002343ADF82030BDF82030A9F800303CE70D +:103F6000202C0FD80899631E21FA03F3DA0705D530 +:103F70004FF0FF3202FA04F40C430894089BC9F88E +:103F800000302AE7402C2AD0611EC4F12102A4F19E +:103F90002103DDE9086526FA01F105FA02F225FAA6 +:103FA00003F311431943CB0711D50122A4F12003D8 +:103FB000C4F1200102FA03F322FA01F1A2400B43FB +:103FC000524263EB430332432B43CDE90823DDE93F +:103FD0000823C9E9002300E76FF00100FDE66FF058 +:103FE0000800FAE6082CA1D9102CB4D9202CEED860 +:103FF000C4E7BBF1000FAED0022384E7BBF1000F92 +:10400000BCD004237FE70000012A30B5144638BF36 +:10401000012485B00025402C28BF4024012ACDE989 +:10402000025518D81B788DF8083063070AD004AB06 +:10403000624203EBD40502F0070215F8083C9340F6 +:1040400005F8083C034600912246002102A8FFF72C +:1040500027FB05B030BD082AE4D9102A03D81B88F5 +:10406000ADF80830E1E7202A95BF1B68D3E90023AB +:104070000293CDE90223D8E710B5CB681BB98B605A +:104080000B618B8210BD04691A681C600361C38ACE +:10409000013BC382CA60F0E703064CBFC0F3C030E7 +:1040A0000220704708B50246FFF7F6FF022806D146 +:1040B0005306C2F30F2001D100F0030008BDC2F384 +:1040C0000740FBE72DE9F04F93B004460D46CDE9DC +:1040D00003230A681046FFF7DFFF0228824614BF59 +:1040E000C2F306260026002A80F2F38112F0C049AE +:1040F00040F0EF81097B002900F0EB81022803D01A +:104100002378B34240F0E881C2F3046310462944A7 +:10411000069302F07F030593FFF7C4FF059B00227F +:1041200083464FEA8348002348EA0A4848EA46683B +:10413000CE78CDE90823F30948EA0008029368D055 +:10414000059B024608A92046009353466768B84776 +:10415000002800F0C481276A4FB9414604F10C00E1 +:10416000FFF700FB074690B96FF0020055E03B698E +:1041700098450DD03F68002FF9D1414604F10C005D +:10418000FFF7F0FA07460028EED0236A3B6027626B +:1041900097F817C006F01F08CCF3840CACEB0800AE +:1041A000A8EB0C031FFA80FE0028B8BF0EF1200018 +:1041B0001FFA83FEB8BF00B2002B0793B8BF0EF101 +:1041C0002003D7E90221BCBF1BB2079352EA0103C7 +:1041D00038D0039B4FF0000CDFF820E39A1A049BC1 +:1041E00063EB010196457CEB01032BD36B7B97F8C6 +:1041F0001AE0734519D1029B002B78D0012821DCED +:104200007868F8B9DFF8F0C2944570EB010316D373 +:1042100037E0276A27B96FF00C0013B0BDE8F08FC4 +:104220003B699845B4D03F68F4E7B34890427CEBD3 +:10423000010301D30020F0E7029B002BFAD0079B7B +:104240000F2B17DCFA7DB3003946204602F003023B +:1042500003F07C031343FB75FFF706FB6B7BBB7618 +:10426000029B3BB9FB7DC3F38402013262F38603F8 +:10427000FB75D0E76A7BBB7E9A42DBD1029B002BA9 +:1042800035D0B309022B32D0039B142200210DA894 +:10429000BB60049BFB60FDF727FD039B0AA920463A +:1042A0000A93049BADF83EB00B932B1D8DF840A0F4 +:1042B0000C932B7B8DF84180013BDBB2ADF83C3099 +:1042C000069B8DF84230059B8DF8433094F82C30D6 +:1042D00083F001038DF84430A3689847FB7DC3F356 +:1042E0008403013303F01F039B02FB82A2E7FB7DE3 +:1042F000C6F34012B2EBD31F40F0F480C3F3840343 +:10430000434540F0F280029AB6092B7B002A4DD03B +:10431000F2075DD4032B40F2EB80039BAE1D3946C0 +:1043200004F10C00BB603246049BFB602B7B033B1B +:10433000DBB2FFF7ABFA00280CDA39462046FFF76C +:1043400093FAFB7DC3F38403013303F01F039B0245 +:10435000FB8209E7AB88DDE908843B834FF6FF73F6 +:10436000C9F12000A9F1200228FA09F109F1080990 +:1043700004FA00F024FA02F2014318461143C9B2CC +:10438000FFF7C6F9B9F1400F0346E9D1B8823146CB +:104390002A7B033AD2B2FFF7CBF9FB7DB882DA432E +:1043A000C2F3C01262F3C713FB7543E786B92E1D33 +:1043B000013B394604F10C00DBB23246FFF766FAE6 +:1043C0000028BADB2A7B3146B88A013AD2B2E2E74A +:1043D000F98A013BC1F30901DAB204295BD8281D2F +:1043E000002307F11B069A4208D910F801CB0133CC +:1043F00006F801C00131DBB20529F4D103999342DB +:104400000A9138BF043304992CBF002355FA83F373 +:104410000B9107F11B010C9179680E930D91291DE9 +:10442000FB8AADF83EB0C3F309038DF840A08DF8C8 +:1044300041801A44069B8DF84230059BADF83C2024 +:104440008DF8433094F82C3083F001038DF844301C +:104450000023B88A7B602A7B013AFFF769F93B8B1E +:10446000B882834203D1A3680AA920469847204610 +:104470000AA9FFF701FEFB7DBA8AC3F38403013367 +:1044800003F01F039B02FB823B8B9A420CBF002070 +:104490006FF01000C1E67B68002BAFD0052001E073 +:1044A0001C3033461E68002EFAD1091A2E1D081D35 +:1044B000184401EB090C5FFA89F3BCF11B0F9DD87E +:1044C0009A429BD916F8013B09F1010900F8013B1A +:1044D000EFE76FF00900A0E66FF00A009DE66FF0CD +:1044E0000B009AE66FF00D0097E66FF00E0094E671 +:1044F0006FF00F0091E600BF40420F0080841E0065 +:10450000EFF30983054968334A6B22F001024A63DD +:1045100083F30988002383F31188704700EF00E0DC +:10452000302080F3118862B60D4B0E4AD96821F411 +:10453000E0610904090C0A430B49DA60D3F8FC2056 +:1045400042F08072C3F8FC20084AC2F8B01F11681C +:1045500041F0010111602022DA7783F822007047D0 +:1045600000ED00E00003FA0555CEACC5001000E0F8 +:10457000302310B583F311880E4B5B6813F400638E +:1045800014D0F1EE103AEFF309844FF08073683CD9 +:10459000E361094BDB6B236684F3098800F01AFCA6 +:1045A00010B1064BA36110BD054BFBE783F31188E7 +:1045B000F9E700BF00ED00E000EF00E0430700086E +:1045C000460700080023054A19460133102BC2E9AB +:1045D000001102F10802F8D1704700BF24630020E7 +:1045E0002DE9F74F02F0030AC2F3C313C2F38009A7 +:1045F000C2F3C108C2F3411E574600224FF00F0B11 +:104600000193CB0733D502F00703019C0125560027 +:104610009B00BAF1020F05FA02F504FA03FC4468A4 +:104620000BFA03F324EA05046FEA030344EA0904DE +:1046300044604FF00304856804FA06F625EA06058F +:104640006FEA060445EA08058560C56825EA06059F +:1046500045EA0E05C5601BD1072A13D8056A2B4011 +:1046600043EA0C03036203681C403C4304604908AE +:1046700022D04FEA490901324FEA88084FEA8E0EEC +:10468000BF00BEE7456A2B4043EA0C034362EAE7FA +:104690000568072A04EA050444EA0704046005D80B +:1046A000046A234043EA0C030362E0E7446A2340C0 +:1046B00043EA0C034362DAE703B0BDE8F08F000081 +:1046C000026843681143016003B118477047000056 +:1046D000024A136843F0C0031360704700100140A2 +:1046E000024A136843F0C00313607047007C004027 +:1046F00037B51A4C1A4D204600F0D2FA04F11400D6 +:10470000009400234FF40072164900F08FF94FF423 +:104710000072154904F138000094144B00F008FAB7 +:10472000134BC4E91735134C204600F0B9FA04F1D5 +:10473000140000234FF400720F49009400F076F942 +:104740000E4B4FF400720E4904F13800009400F053 +:10475000EFF90C4BC4E9173503B030BDA46300205A +:1047600000E1F5057C6400207C680020D14600084B +:1047700000100140106400207C660020E146000823 +:104780007C6A0020007C0040037C30B5284C002966 +:1047900018BF0C46012B0CD1264B984236D1264B24 +:1047A0001A6D42F010021A659A6F42F010029A6771 +:1047B0009B6F2268036EC16D03EB52038466B3FBEB +:1047C000F2F36268150442BF23F0070503F0070304 +:1047D00043EA4503CB60A36843F040034B60E368C2 +:1047E00043F001038B6042F4967343F001030B60C6 +:1047F0004FF0FF330B62510505D512F0102213D094 +:10480000B2F1805F12D080F8643030BD0B4B98421B +:10481000CFD1094B9A6C42F000429A641A6F42F071 +:1048200000421A671B6FC4E77F23ECE73F23EAE7E8 +:1048300000610008A46300200045025810640020B5 +:104840002DE9F047C66D05463768F46921073462E3 +:104850001AD014F0080118BF4FF48071E20748BF66 +:1048600041F02001A3074FF0300348BF41F0400161 +:10487000600748BF41F0800183F31188281DFFF7CE +:104880001FFF002383F31188E2050AD5302383F349 +:1048900011884FF48061281DFFF712FF002383F376 +:1048A00011884FF030094FF0000A14F0200838D179 +:1048B0003B0616D54FF0300905F1380A200610D511 +:1048C00089F31188504600F067F9002836DA08218C +:1048D000281DFFF7F5FE27F080033360002383F3E4 +:1048E0001188790614D5620612D5302383F3118816 +:1048F000D5E913239A4208D12B6C33B127F0400736 +:104900001021281DFFF7DCFE3760002383F3118898 +:10491000E30618D5AA6E1369ABB15069BDE8F0473C +:10492000184789F31188736A284695F8641019406E +:1049300000F0D0F98AF31188F469B6E7B06288F321 +:104940001188F469BAE7BDE8F0870000090100F1B9 +:104950006043012203F56143C9B283F8001300F0FC +:104960001F039A4043099B0003F1604303F5614331 +:10497000C3F880211A607047F8B51546826804466E +:104980000B46AA4200D28568A1692669761AB5420B +:104990000BD218462A46FDF795F9A3692B44A3616B +:1049A0002846A3685B1BA360F8BD0CD9AF1B184653 +:1049B0003246FDF787F93A46E1683044FDF782F95F +:1049C000E3683B44EBE718462A46FDF77BF9E368CA +:1049D000E5E7000083689342F7B50446154600D228 +:1049E0008568D4E90460361AB5420BD22A46FDF731 +:1049F00069F963692B4463612846A3685B1BA36064 +:104A000003B0F0BD0DD93246AF1B0191FDF75AF945 +:104A100001993A46E0683144FDF754F9E3683B44B4 +:104A2000E9E72A46FDF74EF9E368E4E710B50A44E2 +:104A30000024C361029B8460C16002610362C0E91B +:104A40000000C0E9051110BD08B5D0E90532934258 +:104A500001D1826882B98268013282605A1C426147 +:104A600019700021D0E904329A4224BFC36843611F +:104A700000F078FA002008BD4FF0FF30FBE700009F +:104A800070B5302304460E4683F31188A568A5B19E +:104A9000A368A269013BA360531CA3611578226936 +:104AA000934224BFE368A361E3690BB120469847B2 +:104AB000002383F31188284607E03146204600F0A2 +:104AC00041FA0028E2DA85F3118870BD2DE9F74F2D +:104AD00004460E4617469846D0F81C904FF0300A10 +:104AE0008AF311884FF0000B154665B12A4631460E +:104AF0002046FFF741FF034660B94146204600F0DB +:104B000021FA0028F1D0002383F31188781B03B029 +:104B1000BDE8F08FB9F1000F03D001902046C847DF +:104B2000019B8BF31188ED1A1E448AF31188DCE790 +:104B3000C160C361009B82600362C0E9051111443A +:104B4000C0E9000001617047F8B504460D461646FD +:104B5000302383F31188A768A7B1A368013BA36042 +:104B600063695A1C62611D70D4E904329A4224BF01 +:104B7000E3686361E3690BB120469847002080F346 +:104B8000118807E03146204600F0DCF90028E2DA1F +:104B900087F31188F8BD0000D0E9052310B59A42CB +:104BA00001D182687AB982680021013282605A1C80 +:104BB00082611C7803699A4224BFC368836100F054 +:104BC000D1F9204610BD4FF0FF30FBE72DE9F74F3C +:104BD00004460E4617469846D0F81C904FF0300A0F +:104BE0008AF311884FF0000B154665B12A4631460D +:104BF0002046FFF7EFFE034660B94146204600F02D +:104C0000A1F90028F1D0002383F31188781B03B0A9 +:104C1000BDE8F08FB9F1000F03D001902046C847DE +:104C2000019B8BF31188ED1A1E448AF31188DCE78F +:104C3000026843681143016003B1184770470000E0 +:104C40001430FFF743BF00004FF0FF331430FFF77D +:104C50003DBF00003830FFF7B9BF00004FF0FF3311 +:104C60003830FFF7B3BF00001430FFF709BF000072 +:104C70004FF0FF311430FFF703BF00003830FFF76B +:104C800063BF00004FF0FF323830FFF75DBF000018 +:104C9000012914BF6FF0130000207047FFF728BDF3 +:104CA000044B036000234360C0E902330123037413 +:104CB000704700BF1861000810B53023044683F325 +:104CC0001188FFF761FD02230020237480F311880F +:104CD00010BD000038B5C36904460D461BB9042158 +:104CE0000844FFF7A5FF294604F11400FFF7ACFEC6 +:104CF000002806DA201D4FF40061BDE83840FFF7B8 +:104D000097BF38BD00238268037503691B689968E3 +:104D10009142FBD25A68036042601060586070474D +:104D200000238268037503691B6899689142FBD868 +:104D30005A680360426010605860704708B50846C2 +:104D4000302383F311880B7D032B05D0042B0DD06A +:104D50002BB983F3118808BD8B6900221A604FF0CC +:104D6000FF338361FFF7CEFF0023F2E7D1E9003282 +:104D700013605A60F3E70000FFF7C4BF054BD96822 +:104D800008751868026853601A600122D8600275BD +:104D9000FBF7C2BC806C00200C4B30B5DD684B1CAF +:104DA00087B004460FD02B46094A684600F06CF9DC +:104DB0002046FFF7E3FF009B13B1684600F06EF951 +:104DC000A86907B030BDFFF7D9FFF9E7806C002074 +:104DD0003D4D0008044B1A68DB6890689B68984258 +:104DE00094BF002001207047806C0020084B10B554 +:104DF0001C68D868226853601A600122DC60227542 +:104E0000FFF78EFF01462046BDE81040FBF784BC4B +:104E1000806C0020044B1A68DB6892689B689A4299 +:104E200001D9FFF7E3BF7047806C002038B5074C0D +:104E300001230025064907482370656000F0D8FC6F +:104E40000223237085F3118838BD00BFE86E00206F +:104E500044610008806C002008B572B6044B1865E8 +:104E600000F006FB00F0D8FB024B03221A70FEE7AD +:104E7000806C0020E86E002000F046B9EFF311804E +:104E800020B9EFF30583302282F3118870470000C8 +:104E900010B530B9EFF30584C4F3080414B180F3FE +:104EA000118810BDFFF7B6FF84F31188F9E7000001 +:104EB0008B600223086108468B8270478368A3F1E8 +:104EC000840243F8142C026943F8442C426943F8E5 +:104ED000402C094A43F8242CC268A3F1200043F86F +:104EE000182C022203F80C2C002203F80B2C034A86 +:104EF00043F8102C704700BF31070008806C002079 +:104F000008B5FFF7DBFFBDE80840FFF735BF00003D +:104F1000024BDB6898610F20FFF730BF806C0020E8 +:104F2000302383F31188FFF7F3BF000008B5014673 +:104F3000302383F311880820FFF72EFF002383F32B +:104F4000118808BD064BDB6839B1426818605A60A9 +:104F5000136043600420FFF71FBF4FF0FF3070471E +:104F6000806C00200368984206D01A680260506086 +:104F700018469961FFF700BF70470000036810B53D +:104F80009C68A2420CD85C688A600B604C6021600F +:104F9000596099688A1A9A604FF0FF33836010BD98 +:104FA000121B1B68ECE700000A2938BF0A2170B504 +:104FB00004460D460A26601900F012FC00F0FAFBC8 +:104FC000041BA54203D8751C04462E46F3E70A2E9F +:104FD00004D90120BDE8704000F04ABC70BD00005B +:104FE000F8B5144B0D460A2A4FF00A07D96103F1B0 +:104FF0001001826038BF0A224160196914460160BD +:1050000048601861A81800F0DBFB00F0D3FB431BDD +:105010000646A34206D37C1C28192746354600F0D5 +:10502000DFFBF2E70A2F04D90120BDE8F84000F0C9 +:105030001FBCF8BD806C0020F8B506460D4600F098 +:10504000B9FB0F4A134653F8107F9F4206D12A46F8 +:1050500001463046BDE8F840FFF7C2BFD169BB68E2 +:10506000441A2C1928BF2C46A34202D92946FFF71F +:105070009BFF224631460348BDE8F840FFF77EBF5C +:10508000806C0020906C0020C0E90323002310B442 +:105090005DF8044B4361FFF7CFBF000010B5194C1A +:1050A000236998420DD08168D0E9003213605A60BC +:1050B0009A680A449A60002303604FF0FF33A361AB +:1050C00010BD0268234643F8102F5360002202608F +:1050D00022699A4203D1BDE8104000F07BBB93687F +:1050E00081680B44936000F065FB2269E169926876 +:1050F000441AA242E4D91144BDE81040091AFFF74E +:1051000053BF00BF806C00202DE9F047DFF8BC8062 +:1051100008F110072C4ED8F8105000F04BFBD8F8CF +:105120001C40AA68031B9A423ED814444FF0000961 +:10513000D5E90032C8F81C4013605A60C5F80090E9 +:10514000D8F81030B34201D100F044FB89F3118844 +:10515000D5E9033128469847302383F311886B69DA +:10516000002BD8D000F026FB6A69A0EB0409824628 +:105170004A450DD2022000F07BFB0022D8F8103007 +:10518000B34208D151462846BDE8F047FFF728BF93 +:10519000121A2244F2E712EB09092946384638BFB1 +:1051A0004A46FFF7EBFEB5E7D8F81030B34208D017 +:1051B0001444C8F81C00211AA960BDE8F047FFF7A5 +:1051C000F3BEBDE8F08700BF906C0020806C00202B +:1051D00000207047FEE70000704700004FF0FF30EE +:1051E00070470000BFF34F8F044B1A695107FCD181 +:1051F000D3F810215207F8D1704700BF00200052A9 +:1052000008B50D4B1B78ABB9FFF7ECFF0B4BDA6819 +:10521000D10704D50A4A5A6002F188325A60D3F89D +:105220000C21D20706D5064AC3F8042102F18832C0 +:10523000C3F8042108BD00BFF06E0020002000521A +:105240002301674508B5114B1B78F3B9104B1A6958 +:10525000510703D5DA6842F04002DA60D3F8102132 +:10526000520705D5D3F80C2142F04002C3F80C21B7 +:10527000FFF7B8FF064BDA6842F00102DA60D3F8B4 +:105280000C2142F00102C3F80C2108BDF06E002091 +:10529000002000520F289ABF00F5806040040020D3 +:1052A000704700004FF40030704700001020704736 +:1052B0000F2808B50BD8FFF7EDFF00F500330268A3 +:1052C000013204D104308342F9D1012008BD00200D +:1052D000FCE700000F2838B505463FD8FFF782FFEE +:1052E0001F4CFFF78DFF4FF0FF3307286361C4F8B1 +:1052F00014311DD82361FFF775FF030243F0240327 +:10530000E360E36843F08003E36023695A07FCD459 +:105310002846FFF767FFFFF7BDFF4FF4003100F0AD +:1053200057F92846FFF78EFFBDE83840FFF7C0BFAA +:10533000C4F81031FFF756FFA0F108031B0243F039 +:105340002403C4F80C31D4F80C3143F08003C4F8C2 +:105350000C31D4F810315B07FBD4D9E7002038BDFD +:10536000002000522DE9F84F05460C46104645EA4C +:105370000203DE0602D00020BDE8F88F20F01F00F7 +:10538000DFF8BCB0DFF8BCA0FFF73AFF04EB000881 +:10539000444503D10120FFF755FFEDE720222946C0 +:1053A000204600F0F7FD10B920352034F0E72B46F9 +:1053B00005F120021F68791CDDD104339A42F9D12E +:1053C00005F178431B481C4EB3F5801F1B4B38BFBB +:1053D000184603F1F80332BFD946D1461E46FFF7FF +:1053E00001FF0760A5EB040C336804F11C0143F0D6 +:1053F00002033360231FD9F8007017F00507FAD1B4 +:1054000053F8042F8B424CF80320F4D1BFF34F8F95 +:10541000FFF7E8FE4FF0FF332022214603602846C5 +:10542000336823F00203336000F0B4FD0028BBD0E2 +:105430003846B0E7142100520C20005214200052CC +:10544000102000521021005210B5084C237828B1CA +:105450001BB9FFF7D5FE0123237010BD002BFCD034 +:105460002070BDE81040FFF7EDBE00BFF06E0020D9 +:1054700038B5054D00240334696855F80C0B00F06D +:10548000B9F8122CF7D138BD50610008084600F079 +:1054900079BD000070B5104E82B0FFF7EFFC0546F5 +:1054A00000F088F9326803469042336037BF0B4AF8 +:1054B0000A495168146836BF0131D1E90041516091 +:1054C0000419284641F100010191FFF7E1FC204653 +:1054D000019902B070BD00BFF46E0020F86E00208C +:1054E00070B5124E82B0FFF7C9FC054600F062F9B4 +:1054F000326803469042336037BF0D4A0C49516809 +:10550000146836BF0131D1E90041516004192846C1 +:1055100041F100010191FFF7BBFC4FF47A720023C7 +:1055200020460199FAF7E4FE02B070BDF46E002047 +:10553000F86E00200244074BD2B210B5904200D161 +:1055400010BD441C00B253F8200041F8040BE0B237 +:10555000F4E700BF504000580E4B30B51C6F2404D8 +:1055600005D41C6F1C671C6F44F400441C670A4C74 +:1055700002442368D2B243F480732360074B904205 +:1055800000D130BD441C51F8045B00B243F82050F8 +:10559000E0B2F4E700440258004802585040005876 +:1055A00007B5012201A90020FFF7C4FF019803B04D +:1055B0005DF804FB13B50446FFF7F2FFA04205D0E7 +:1055C000012201A900200194FFF7C6FF02B010BD1F +:1055D0000144BFF34F8F064B884204D3BFF34F8F74 +:1055E000BFF36F8F7047C3F85C022030F4E700BF51 +:1055F00000ED00E00144BFF34F8F064B884204D317 +:10560000BFF34F8FBFF36F8F7047C3F87002203026 +:10561000F4E700BF00ED00E070470000074B45F2E3 +:1056200055521A6002225A6040F6FF729A604CF698 +:10563000CC421A600122024B1A7070470048005891 +:10564000046F0020034B1B781BB1034B4AF6AA22C0 +:105650001A607047046F002000480058034B1A6816 +:105660001AB9034AD2F8D0241A607047006F00209C +:1056700000400258024B4FF48032C3F8D0247047E8 +:105680000040025808B5FFF7E9FF024B1868C0F365 +:10569000806008BD006F002070B5BFF34F8FBFF36F +:1056A0006F8F1A4A0021C2F85012BFF34F8FBFF319 +:1056B0006F8F536943F400335361BFF34F8FBFF3D0 +:1056C0006F8FC2F88410BFF34F8FD2F8803043F64B +:1056D000E074C3F3C900C3F34E335B0103EA04066D +:1056E000014646EA81750139C2F86052F9D2203B81 +:1056F00013F1200FF2D1BFF34F8F536943F480337E +:105700005361BFF34F8FBFF36F8F70BD00ED00E0AB +:10571000FEE70000214B2248224A70B5904237D361 +:10572000214BC11EDA1C121A22F003028B4238BF31 +:1057300000220021FCF7D8FA1C4A0023C2F884306A +:10574000BFF34F8FD2F8803043F6E074C3F3C90043 +:10575000C3F34E335B0103EA0406014646EA817552 +:105760000139C2F86C52F9D2203B13F1200FF2D16B +:10577000BFF34F8FBFF36F8FBFF34F8FBFF36F8FA9 +:105780000023C2F85032BFF34F8FBFF36F8F70BD4D +:1057900053F8041B40F8041BC0E700BF80630008F7 +:1057A000086F0020086F0020086F002000ED00E067 +:1057B00000F01AB9014B586A704700BF000C004056 +:1057C000034B002258631A610222DA60704700BF5F +:1057D000000C0040014B0022DA607047000C0040D2 +:1057E000014B5863704700BF000C0040FEE700000B +:1057F00070B51B4B0025044686B058600E46856286 +:105800000163FEF78DFE04F11003A560E562C4E9B3 +:1058100004334FF0FF33C4E90044C4E90635FFF711 +:10582000C9FF2B46024604F134012046C4E908238F +:1058300080230C4A2565FFF73BFB01230A4AE06001 +:1058400000920375684672680192B268CDE902233E +:10585000064BCDE90435FFF753FB06B070BD00BF22 +:10586000E86E0020986100089D610008ED5700086F +:10587000024AD36A1843D062704700BF806C002090 +:105880004B6843608B688360CB68C3600B6943617E +:105890004B6903628B6943620B68036070470000C9 +:1058A00008B53A4B40F2FF713948D3F888200A43D3 +:1058B000C3F88820D3F8882022F4FF6222F0070280 +:1058C000C3F88820D3F88830324B1A6C0A431A6424 +:1058D0009A6E0A439A66304A9B6E1146FFF7D0FFD4 +:1058E00000F5806002F11C01FFF7CAFF00F580603F +:1058F00002F13801FFF7C4FF00F5806002F15401A6 +:10590000FFF7BEFF00F5806002F17001FFF7B8FFFE +:1059100000F5806002F18C01FFF7B2FF00F58060B6 +:1059200002F1A801FFF7ACFF00F5806002F1C401AD +:10593000FFF7A6FF00F5806002F1E001FFF7A0FF8E +:1059400000F5806002F1FC01FFF79AFF02F58C710F +:1059500000F58060FFF794FF00F004F90F4BD3F8D7 +:10596000902242F00102C3F89022D3F8942242F030 +:105970000102C3F894220522C3F898204FF0605228 +:10598000C3F89C20064AC3F8A02008BD0044025872 +:105990000000025800450258A461000800ED00E034 +:1059A0001F00080308B500F0C9FAFFF73FFA0B4BD8 +:1059B000DA6B42F04002DA635A6E22F040025A6615 +:1059C0005B6E074B1A6842F008021A601A6842F0D0 +:1059D00004021A60BDE80840FFF740BE00450258C7 +:1059E00000180248704700000E4B9A6C42F0080203 +:1059F0009A641A6F42F008021A670B4A1B6FD36B46 +:105A000043F00803D363C722084B9A624FF0FF327A +:105A1000DA6200229A615A63DA605A6001225A619E +:105A20001A607047004502580010005C000C0040EE +:105A3000094A08B51169D3680B40D9B29B076FEAD0 +:105A40000101116107D5302383F31188FFF714FAA0 +:105A5000002383F3118808BD000C0040044BDA6B6F +:105A60000243DA635A6E104358665B6E704700BF9C +:105A7000004502583A4B4FF0FF31D3F8802062F0D6 +:105A80000042C3F88020D3F8802002F00042C3F81F +:105A90008020D3F88020D3F88420C3F88410D3F872 +:105AA00084200022C3F88420D3F88400D86F40F00B +:105AB000FF4040F4FF0040F4DF4040F07F00D86733 +:105AC000D86F20F0FF4020F4FF0020F4DF4020F0EA +:105AD0007F00D867D86FD3F888006FEA40506FEA2C +:105AE0005050C3F88800D3F88800C0F30A00C3F808 +:105AF0008800D3F88800D3F89000C3F89010D3F84A +:105B00009000C3F89020D3F89000D3F89400C3F825 +:105B10009410D3F89400C3F89420D3F89400D3F8E9 +:105B20009800C3F89810D3F89800C3F89820D3F8D9 +:105B30009800D3F88C00C3F88C10D3F88C00C3F80D +:105B40008C20D3F88C00D3F89C00C3F89C10D3F8B9 +:105B50009C10C3F89C20D3F89C3000F0BFB900BF64 +:105B60000044025808B50122504BC3F80821504B9D +:105B70005A6D42F002025A65DA6F42F00202DA67A9 +:105B80000422DB6F4B4BDA605A689104FCD54A4A19 +:105B90001A6001229A60494ADA6000221A614FF4C1 +:105BA00040429A61434B9A699204FCD51A6842F4C8 +:105BB00080721A60424B1A6F12F4407F04D04FF487 +:105BC00080321A6700221A671A6842F001021A60CE +:105BD0003B4B1A685007FCD500221A611A6912F073 +:105BE0003802FBD1012119604FF0804159605A679A +:105BF000344ADA62344A1A611A6842F480321A600E +:105C00002F4B1A689103FCD51A6842F480521A602F +:105C10001A689204FCD52D4A2D499A6200225A63D3 +:105C2000196301F57C01DA6301F5E77199635A6440 +:105C3000284A1A64284ADA621A6842F0A8521A609E +:105C40001F4B1A6802F02852B2F1285FF9D148229E +:105C50009A614FF48862DA6140221A621F4ADA645C +:105C60001F4A1A651F4A5A651F4A9A6532231F4AFE +:105C70001360136803F00F03022BFAD1104A136963 +:105C800043F003031361136903F03803182BFAD1AF +:105C90004FF00050FFF7E2FE4FF08040FFF7DEFECE +:105CA0004FF00040BDE80840FFF7D8BE008000512B +:105CB000004502580048025800C000F004000001EE +:105CC000004402580000FF01008890083220600064 +:105CD00063020901470E0508DD0BBF01200000200B +:105CE000000001100910E000000101100020005226 +:105CF0004FF0B04208B5D2F8883003F00103C2F883 +:105D0000883023B1044A13680BB150689847BDE846 +:105D10000840FEF72DBC00BF246300204FF0B042C6 +:105D200008B5D2F8883003F00203C2F8883023B1F6 +:105D3000044A93680BB1D0689847BDE80840FEF765 +:105D400017BC00BF246300204FF0B04208B5D2F862 +:105D5000883003F00403C2F8883023B1044A136981 +:105D60000BB150699847BDE80840FEF701BC00BF81 +:105D7000246300204FF0B04208B5D2F8883003F019 +:105D80000803C2F8883023B1044A93690BB1D06983 +:105D90009847BDE80840FEF7EBBB00BF2463002036 +:105DA0004FF0B04208B5D2F8883003F01003C2F8C3 +:105DB000883023B1044A136A0BB1506A9847BDE892 +:105DC0000840FEF7D5BB00BF246300204FF0B0436E +:105DD00010B5D3F8884004F47872C3F88820A3067D +:105DE00004D5124A936A0BB1D06A9847600604D56D +:105DF0000E4A136B0BB1506B9847210604D50B4A22 +:105E0000936B0BB1D06B9847E20504D5074A136C2E +:105E10000BB1506C9847A30504D5044A936C0BB1A1 +:105E2000D06C9847BDE81040FEF7A2BB2463002069 +:105E30004FF0B04310B5D3F8884004F47C42C3F867 +:105E40008820620504D5164A136D0BB1506D984732 +:105E5000230504D5124A936D0BB1D06D9847E00429 +:105E600004D50F4A136E0BB1506E9847A10404D5A8 +:105E70000B4A936E0BB1D06E9847620404D5084A62 +:105E8000136F0BB1506F9847230404D5044A936FE6 +:105E90000BB1D06F9847BDE81040FEF769BB00BF5B +:105EA0002463002008B50348FEF7CAFCBDE808409B +:105EB000FEF75EBBA463002008B50348FEF7C0FCF4 +:105EC000BDE80840FEF754BB1064002008B5FFF79A +:105ED000AFFDBDE80840FEF74BBB0000062108B54A +:105EE0000846FEF733FD06210720FEF72FFD0621A9 +:105EF0000820FEF72BFD06210920FEF727FD0621CD +:105F00000A20FEF723FD06211720FEF71FFD0621BC +:105F10002820FEF71BFD09217A20FEF717FD072137 +:105F20003220FEF713FD0C212520FEF70FFD0C217A +:105F30005320BDE80840FEF709BD000008B5FFF793 +:105F400099FDFEF73FFBFEF7A9FEFFF74BFDBDE80D +:105F50000840FFF72DBC00000B460146184600F034 +:105F600003B8000000F00EB810B5054C13462CB174 +:105F70000A4601460220AFF3008010BD2046FCE730 +:105F800000000000024B01461868FFF77FBA00BF0F +:105F90003C22002010B501390244904201D100207A +:105FA00005E0037811F8014FA34201D0181B10BD82 +:105FB0000130F2E72DE9F041A3B1C91A1778014485 +:105FC000044603F1FF3C8C42204601D9002009E041 +:105FD0000578BD4204F10104F5D10CEB0405D61897 +:105FE000A54201D1BDE8F08115F8018D16F801ED4B +:105FF000F045F5D0E7E70000034611F8012B03F860 +:10600000012B002AF9D17047636F6D2E63756265AD +:1060100070696C6F742E6865726534666300000089 +:1060200053544D333248373F3F3F0053544D333282 +:10603000483733782F3732780053544D333248374E +:1060400034332F3735332F373530000001105A00E5 +:1060500003105900012058000320560040A2E4F12B +:10606000646891060041A3E5F265699207000000AB +:1060700043414E464449666163653A204D657373FA +:106080006167652052414D204F766572666C6F776F +:10609000210000004261642043414E49666163650E +:1060A00020696E6465782E00000100000001FF0089 +:1060B00000A0004000A4004000000000000000001C +:1060C000912C000869250008CD33000861250008DF +:1060D000D9250008E929000851270008A125000852 +:1060E000A52500087D25000865250008A9290008C8 +:1060F0008925000835350008952500087D29000808 +:1061000000960000000000000000000000000000F9 +:106110000000000000000000000000005D4C0008CE +:10612000494C0008854C0008714C00087D4C000863 +:10613000694C0008554C0008414C0008914C00087F +:106140006330000040610008D86C0020E86E002039 +:1061500000000020000002000200000000010030EA +:1061600000FF0300080000000000002400000800F9 +:10617000040000000004000000FC00000200000019 +:10618000000004300080000008000000000000381B +:1061900000000100010000006D61696E0069646C1F +:1061A000650000000000002800000000AAAAAAAABA +:1061B00000000024FFFF00000000000000000000BD +:1061C00000A0550A00000000AAAA9AAA00500000E8 +:1061D000FFF20000000000770000990000000000BE +:1061E00000000000AAAAAAAA00000000FFFF000009 +:1061F00000000000000000005A0000000000000045 +:106200009AAAAAAA00000000F3FF0000990000006B +:10621000000000000A00000000000000AAAAAAAACC +:1062200005000000FFFF00008800000000000000E3 +:106230000000000000000000AAAAAAAA00000000B6 +:10624000FFFF000000000000000000000000000050 +:1062500000000000AAAAAAAA00000000FFFF000098 +:10626000000000000000000000000000000000002E +:10627000AAAAAAAA00000000FFFF00000000000078 +:10628000000000000000000000000000AAAAAAAA66 +:1062900000000000FFFF0000000000000000000000 +:1062A0000000000000000000AAAAAAAA0000000046 +:1062B000FFFF0000000000000000000000000000E0 +:1062C00000000000AAAAAAAA00000000FFFF000028 +:1062D000000000000000000048A0FF7F0100000057 +:1062E000130400000000000000001800000000007F +:1062F000FE2A0100D2040000FF000000A463002079 +:1063000010640020000000002060000883040000EA +:106310002B60000850040000396000084022002073 +:10632000000000000000000000000000000000006D +:10633000000000000000000000000000000000005D +:10634000000000000000000000000000000000004D +:10635000000000000000000000000000000000003D +:10636000000000000000000000000000000000002D +:10637000000000000000000000000000000000001D +:00000001FF diff --git a/Tools/bootloaders/MatekL431-Serial_bl.bin b/Tools/bootloaders/MatekL431-Serial_bl.bin new file mode 100755 index 00000000000000..80b78370907fa8 Binary files /dev/null and b/Tools/bootloaders/MatekL431-Serial_bl.bin differ diff --git a/Tools/completion/zsh/_ap_bin b/Tools/completion/zsh/_ap_bin index cc5b7417f63657..543bc72c34ecaf 100644 --- a/Tools/completion/zsh/_ap_bin +++ b/Tools/completion/zsh/_ap_bin @@ -17,20 +17,30 @@ _ap_bin() { '(-M --model)'{-M,--model}'[set simulation model]' \ '--config[set additional simulation config string]:CONFIG' \ '(-F --fg)'{-F,--fg}'[set Flight Gear view address, defaults to 127.0.0.1]:ADDRESS' \ - '--disable-fgview[disable Flight Gear view]' \ + '--enable-fgview[disable Flight Gear view]' \ '--gimbal[enable simulated MAVLink gimbal]' \ '--autotest-dir[set directory for additional files]:DIR:' \ '--defaults[set path to defaults file]:PATH:' \ - '--uartA[set device string for UARTA]:DEVICE:' \ - '--uartB[set device string for UARTB]:DEVICE:' \ - '--uartC[set device string for UARTC]:DEVICE:' \ - '--uartD[set device string for UARTD]:DEVICE:' \ - '--uartE[set device string for UARTE]:DEVICE:' \ - '--uartF[set device string for UARTF]:DEVICE:' \ - '--uartG[set device string for UARTG]:DEVICE:' \ - '--uartH[set device string for UARTH]:DEVICE:' \ - '--uartI[set device string for UARTI]:DEVICE:' \ - '--uartJ[set device string for UARTJ]:DEVICE:' \ + '--uartA[(deprecated) set device string for SERIAL0]:DEVICE:' \ + '--uartC[(deprecated) set device string for SERIAL1]:DEVICE:' \ + '--uartD[(deprecated) set device string for SERIAL2]:DEVICE:' \ + '--uartB[(deprecated) set device string for SERIAL3]:DEVICE:' \ + '--uartE[(deprecated) set device string for SERIAL4]:DEVICE:' \ + '--uartF[(deprecated) set device string for SERIAL5]:DEVICE:' \ + '--uartG[(deprecated) set device string for SERIAL6]:DEVICE:' \ + '--uartH[(deprecated) set device string for SERIAL7]:DEVICE:' \ + '--uartI[(deprecated) set device string for SERIAL8]:DEVICE:' \ + '--uartJ[(deprecated) set device string for SERIAL9]:DEVICE:' \ + '--serial0[set device string for SERIAL0]:DEVICE:' \ + '--serial1[set device string for SERIAL1]:DEVICE:' \ + '--serial2[set device string for SERIAL2]:DEVICE:' \ + '--serial3[set device string for SERIAL3]:DEVICE:' \ + '--serial4[set device string for SERIAL4]:DEVICE:' \ + '--serial5[set device string for SERIAL5]:DEVICE:' \ + '--serial6[set device string for SERIAL6]:DEVICE:' \ + '--serial7[set device string for SERIAL7]:DEVICE:' \ + '--serial8[set device string for SERIAL8]:DEVICE:' \ + '--serial9[set device string for SERIAL9]:DEVICE:' \ '--rtscts[enable rtscts on serial ports (default false)]' \ '--base-port[set port num for base port(default 5670) must be before -I option]:PORT:' \ '--rc-in-port[set port num for rc in]:PORT:' \ diff --git a/Tools/debug/gdb-openocd-esp32.init b/Tools/debug/gdb-openocd-esp32.init new file mode 100644 index 00000000000000..c024f050785aff --- /dev/null +++ b/Tools/debug/gdb-openocd-esp32.init @@ -0,0 +1,10 @@ +# copy this file to .gdbinit in your Firmware tree + +# this sets up gdb to use openocd. You must start openocd first +target extended-remote :3333 + +set print pretty +set remote hardware-watchpoint-limit 2 +mon reset halt +maintenance flush register-cache +set confirm off diff --git a/Tools/environment_install/install-prereqs-arch.sh b/Tools/environment_install/install-prereqs-arch.sh index 4a1d69b82f9f8f..52b94eca23cd62 100755 --- a/Tools/environment_install/install-prereqs-arch.sh +++ b/Tools/environment_install/install-prereqs-arch.sh @@ -26,7 +26,7 @@ BASE_PKGS="base-devel ccache git gsfonts tk wget gcc" SITL_PKGS="python-pip python-setuptools python-wheel python-wxpython opencv python-numpy python-scipy" PX4_PKGS="lib32-glibc zip zlib ncurses" -PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect argparse matplotlib pyparsing geocoder pyserial empy dronecan" +PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect argparse matplotlib pyparsing geocoder pyserial empy==3.3.4 dronecan" # GNU Tools for ARM Embedded Processors # (see https://launchpad.net/gcc-arm-embedded/) diff --git a/Tools/environment_install/install-prereqs-mac.sh b/Tools/environment_install/install-prereqs-mac.sh index 5471e6f175e6ef..6486b007d6452c 100755 --- a/Tools/environment_install/install-prereqs-mac.sh +++ b/Tools/environment_install/install-prereqs-mac.sh @@ -78,6 +78,7 @@ function install_arm_none_eabi_toolchain() { ) fi echo "Registering STM32 Toolchain for ccache" + sudo mkdir -p /usr/local/opt/ccache/libexec sudo ln -s -f $CCACHE_PATH /usr/local/opt/ccache/libexec/arm-none-eabi-g++ sudo ln -s -f $CCACHE_PATH /usr/local/opt/ccache/libexec/arm-none-eabi-gcc echo "Done!" @@ -103,8 +104,9 @@ function maybe_prompt_user() { # see https://github.com/orgs/Homebrew/discussions/3895 find /usr/local/bin -lname '*/Library/Frameworks/Python.framework/*' -delete +# brew update randomly failing on CI, so ignore errors: brew update -brew install gawk curl coreutils wget +brew install --force --overwrite gawk curl coreutils wget PIP=pip if maybe_prompt_user "Install python using pyenv [N/y]?" ; then @@ -159,7 +161,7 @@ if [[ $DO_AP_STM_ENV -eq 1 ]]; then install_arm_none_eabi_toolchain fi -PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect geocoder flake8 junitparser empy dronecan" +PYTHON_PKGS="future lxml pymavlink MAVProxy pexpect geocoder flake8 junitparser empy==3.3.4 dronecan" # add some Python packages required for commonly-used MAVProxy modules and hex file generation: if [[ $SKIP_AP_EXT_ENV -ne 1 ]]; then PYTHON_PKGS="$PYTHON_PKGS intelhex gnureadline" diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 6fa833120a0ffe..594aa33708e456 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -152,7 +152,7 @@ fi # Lists of packages to install BASE_PKGS="build-essential ccache g++ gawk git make wget valgrind screen" -PYTHON_PKGS="future lxml pymavlink pyserial MAVProxy pexpect geocoder empy ptyprocess dronecan" +PYTHON_PKGS="future lxml pymavlink pyserial MAVProxy pexpect geocoder empy==3.3.4 ptyprocess dronecan" PYTHON_PKGS="$PYTHON_PKGS flake8 junitparser" # add some Python packages required for commonly-used MAVProxy modules and hex file generation: @@ -323,6 +323,8 @@ if [ -n "$LBTBIN" ]; then SITL_PKGS+=" libtool-bin" fi +SITL_PKGS+=" ppp" + # Install all packages $APT_GET install $BASE_PKGS $SITL_PKGS $PX4_PKGS $ARM_LINUX_PKGS $COVERAGE_PKGS diff --git a/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 b/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 index e1bf9465c4f07e..2076ce1f588d6b 100644 --- a/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 +++ b/Tools/environment_install/install-prereqs-windows-andAPMSource.ps1 @@ -19,7 +19,7 @@ Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i - Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/pip3.7 /usr/bin/pip'" Write-Output "Downloading extra Python packages (5/8)" -Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'pip install empy pyserial pymavlink intelhex dronecan pexpect'" +Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'pip install empy==3.3.4 pyserial pymavlink intelhex dronecan pexpect'" Write-Output "Downloading APM source (6/8)" Copy-Item "APM_install.sh" -Destination "C:\cygwin64\home" diff --git a/Tools/environment_install/install-prereqs-windows.ps1 b/Tools/environment_install/install-prereqs-windows.ps1 index 5529c9f0c11dcb..15bac628949e85 100644 --- a/Tools/environment_install/install-prereqs-windows.ps1 +++ b/Tools/environment_install/install-prereqs-windows.ps1 @@ -19,7 +19,7 @@ Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i - Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'ln -sf /usr/bin/pip3.7 /usr/bin/pip'" Write-Output "Downloading extra Python packages (5/7)" -Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'pip install empy pyserial pymavlink intelhex dronecan pexpect'" +Start-Process -wait -FilePath "C:\cygwin64\bin\bash" -ArgumentList "--login -i -c 'pip install empy==3.3.4 pyserial pymavlink intelhex dronecan pexpect'" Write-Output "Installing ARM GCC Compiler 10-2020-Q4-Major (6/7)" & $PSScriptRoot\gcc-arm-none-eabi-10-2020-q4-major-win32.exe /S /P /R diff --git a/Tools/environment_install/ubuntu-18.04-python3.sh b/Tools/environment_install/ubuntu-18.04-python3.sh index f8e3ca06851594..a82a4138a4c73d 100755 --- a/Tools/environment_install/ubuntu-18.04-python3.sh +++ b/Tools/environment_install/ubuntu-18.04-python3.sh @@ -17,4 +17,4 @@ echo 'export PATH=$HOME/bin:$PATH' >>$HOME/.profile sudo apt install -y python3-wxgtk4.0 python3-opencv python3-matplotlib python3-pip # pip-install python packages (also swiped from install-prereqs-ubuntu.sh): -pip3 install future lxml pymavlink MAVProxy pexpect flake8==3.7.9 requests==2.27.1 monotonic==1.6 geocoder empy configparser==4.0.2 click==7.1.2 decorator==4.4.2 dronecan pygame intelhex empy +pip3 install future lxml pymavlink MAVProxy pexpect flake8==3.7.9 requests==2.27.1 monotonic==1.6 geocoder empy==3.3.4 configparser==4.0.2 click==7.1.2 decorator==4.4.2 dronecan pygame intelhex empy diff --git a/Tools/ros2/README.md b/Tools/ros2/README.md index bdd055e3f13291..9832e412f999d5 100644 --- a/Tools/ros2/README.md +++ b/Tools/ros2/README.md @@ -187,7 +187,7 @@ ros2 run micro_ros_agent micro_ros_agent serial --baudrate 115200 --dev ./dev/tt ``` ```bash -arducopter --synthetic-clock --wipe --model quad --speedup 1 --slave 0 --instance 0 --uartC uart:./dev/ttyROS1 --defaults $(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_serial.parm --sim-address 127.0.0.1 +arducopter --synthetic-clock --wipe --model quad --speedup 1 --slave 0 --instance 0 --serial1 uart:./dev/ttyROS1 --defaults $(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_serial.parm --sim-address 127.0.0.1 ``` ```bash @@ -205,7 +205,7 @@ ros2 launch ardupilot_sitl micro_ros_agent.launch.py transport:=serial refs:=$(r ``` ```bash -ros2 launch ardupilot_sitl sitl.launch.py synthetic_clock:=True wipe:=True model:=quad speedup:=1 slave:=0 instance:=0 uartC:=uart:./dev/ttyROS1 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_serial.parm sim_address:=127.0.0.1 +ros2 launch ardupilot_sitl sitl.launch.py synthetic_clock:=True wipe:=True model:=quad speedup:=1 slave:=0 instance:=0 serial1:=uart:./dev/ttyROS1 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_serial.parm sim_address:=127.0.0.1 ``` ```bash @@ -231,7 +231,7 @@ model:=quad \ speedup:=1 \ slave:=0 \ instance:=0 \ -uartC:=uart:./dev/ttyROS1 \ +serial1:=uart:./dev/ttyROS1 \ defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_serial.parm \ sim_address:=127.0.0.1 \ \ diff --git a/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py new file mode 100755 index 00000000000000..27de6f272f2ed8 --- /dev/null +++ b/Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python3 +# Copyright 2023 ArduPilot.org. +# +# 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 . + +""" +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 02c8230d5c7d9d..6578e537d8fbc7 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/board_list.py b/Tools/scripts/board_list.py index 52235fdb0e4d54..ab92794673821a 100755 --- a/Tools/scripts/board_list.py +++ b/Tools/scripts/board_list.py @@ -70,6 +70,7 @@ def __init__(self): Board("bbbmini"), Board("blue"), Board("pxfmini"), + Board("canzero"), Board("SITL_x86_64_linux_gnu"), Board("SITL_arm_linux_gnueabihf"), ] 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..84bb812a736c95 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -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,28 @@ 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 + 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 6382ae512eb04e..5fcd74bb75b0c3 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -32,6 +32,7 @@ def __init__(self, Feature('AHRS', 'EKF2', 'HAL_NAVEKF2_AVAILABLE', 'Enable EKF2', 0, None), Feature('AHRS', 'AHRS_EXT', 'HAL_EXTERNAL_AHRS_ENABLED', 'Enable External AHRS', 0, None), Feature('AHRS', 'AHRS_EXT_MICROSTRAIN5', 'AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED', 'Enable MICROSTRAIN 5-series External AHRS', 0, "AHRS_EXT"), # noqa: E501 + Feature('AHRS', 'AHRS_EXT_MICROSTRAIN7', 'AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED', 'Enable MICROSTRAIN 7-series External AHRS', 0, "AHRS_EXT"), # noqa: E501 Feature('AHRS', 'AHRS_EXT_VECTORNAV', 'AP_EXTERNAL_AHRS_VECTORNAV_ENABLED', 'Enable VectorNav External AHRS', 0, "AHRS_EXT"), # noqa Feature('AHRS', 'TEMPCAL', 'HAL_INS_TEMPERATURE_CAL_ENABLE', 'Enable IMU Temperature Calibration', 0, None), Feature('AHRS', 'VISUALODOM', 'HAL_VISUALODOM_ENABLED', 'Enable Visual Odometry', 0, 'EKF3_EXTNAV'), @@ -71,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 @@ -202,6 +204,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 @@ -296,6 +299,7 @@ def __init__(self, Feature('Sensors', 'AIRSPEED', 'AP_AIRSPEED_ENABLED', 'Enable Airspeed Sensors', 1, None), # Default to enabled to not annoy Plane users # NOQA: E501 Feature('Sensors', 'BEACON', 'AP_BEACON_ENABLED', 'Enable Beacon', 0, None), Feature('Sensors', 'GPS_MOVING_BASELINE', 'GPS_MOVING_BASELINE', 'Enable GPS Moving Baseline', 0, None), + Feature('Sensors', 'IMU_ON_UART', 'AP_SERIALMANAGER_IMUOUT_ENABLED', 'Enable sending raw IMU data on a serial port', 0, None), # NOQA: E501 Feature('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight Gyro FFT calculations', 0, None), Feature('Other', 'DISPLAY', 'HAL_DISPLAY_ENABLED', 'Enable I2C Displays', 0, None), @@ -371,6 +375,8 @@ 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('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/configure-ci.sh b/Tools/scripts/configure-ci.sh index e7a4c6a60dd807..a547b729fce176 100755 --- a/Tools/scripts/configure-ci.sh +++ b/Tools/scripts/configure-ci.sh @@ -101,5 +101,4 @@ python -m pip install --progress-bar off --user -U argparse pyserial pexpect fut python -m pip install --progress-bar off --user -U intelhex python -m pip install --progress-bar off --user -U numpy python -m pip install --progress-bar off --user -U edn_format -python -m pip install --progress-bar off --user -U empy - +python -m pip install --progress-bar off --user -U empy==3.3.4 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/decode_devid.py b/Tools/scripts/decode_devid.py index 9b8a192e11ed09..04cf8b1738d954 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -101,6 +101,7 @@ def num(s): 0x38 : "DEVTYPE_INS_BMI270", 0x39 : "DEVTYPE_INS_BMI085", 0x3A : "DEVTYPE_INS_ICM42670", + 0x3B : "DEVTYPE_INS_ICM45686", } baro_types = { 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 83a2f0583a18cb..7977fabfa36440 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',), @@ -236,6 +237,8 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('COMPASS_CAL_ENABLED', 'CompassCalibrator::stop'), ('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'), ] def progress(self, msg): diff --git a/Tools/scripts/firmware_version_decoder.py b/Tools/scripts/firmware_version_decoder.py index a0be1928dfc785..cf3321701490d7 100755 --- a/Tools/scripts/firmware_version_decoder.py +++ b/Tools/scripts/firmware_version_decoder.py @@ -68,6 +68,7 @@ class BoardSubType(enum.Enum): LINUX_NAVIGATOR = 1023 LINUX_VNAV = 1024 LINUX_OBAL = 1025 + LINUX_CANZERO = 1026 CHIBIOS_SKYVIPER_F412 = 5000 CHIBIOS_FMUV3 = 5001 CHIBIOS_FMUV4 = 5002 diff --git a/Tools/scripts/imu_uart_decode.py b/Tools/scripts/imu_uart_decode.py new file mode 100755 index 00000000000000..e9954620cf906e --- /dev/null +++ b/Tools/scripts/imu_uart_decode.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python3 +''' +script to decode IMU data on a UART +''' + +import socket, struct, serial, sys + +import argparse + +parser = argparse.ArgumentParser(description='decode IMU data from ArduPilot') + +parser.add_argument('--set-file', type=str, default=None, help='replace parameter defaults from a file') +parser.add_argument('--tcp', default=None, help='TCP endpoint as IP:port') +parser.add_argument('--device', default=None, help='serial port to read from') +parser.add_argument('--baudrate', default=921600, help='baudrate for serial port') + +args = parser.parse_args() + +if args.tcp is None and args.device is None: + print("Must specicy --tcp or --device") + sys.exit(1) + +if args.tcp is not None: + server_address = args.tcp.split(':') + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.connect((server_address[0], int(server_address[1]))) +else: + port = serial.Serial(args.device, baudrate=args.baudrate) + +def crc16_from_bytes(bytes, initial=0): + # CRC-16-CCITT + # Initial value: 0xFFFF + # Poly: 0x1021 + # Reverse: no + # Output xor: 0 + # Check string: '123456789' + # Check value: 0x29B1 + + try: + if isinstance(bytes, basestring): # Python 2.7 compatibility + bytes = map(ord, bytes) + except NameError: + if isinstance(bytes, str): # This branch will be taken on Python 3 + bytes = map(ord, bytes) + + crc = initial + for byte in bytes: + crc ^= byte << 8 + for bit in range(8): + if crc & 0x8000: + crc = ((crc << 1) ^ 0x1021) & 0xFFFF + else: + crc = (crc << 1) & 0xFFFF + return crc & 0xFFFF + +class Packet(object): + def __init__(self, pkt): + '''parse the packet''' + self.pkt = pkt + self.delta_velocity = [0.0]*3 + self.delta_angle = [0.0]*3 + (self.magic, + self.length, + self.timestamp_us, + self.delta_velocity[0], + self.delta_velocity[1], + self.delta_velocity[2], + self.delta_angle[0], + self.delta_angle[1], + self.delta_angle[2], + self.delta_velocity_dt, + self.delta_angle_dt, + self.counter, + self.crc) = struct.unpack(" @@ -757,3 +761,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..a901b50e06b692 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 @@ -327,3 +331,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..0defc67f671503 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 @@ -1941,3 +1945,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..5da4dc658182e9 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 @@ -296,3 +300,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..c8ff8cc4114b3e 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 @@ -1246,3 +1250,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..c199c18ca2dc9b 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 @@ -168,3 +172,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_Avoidance/AP_OABendyRuler.cpp b/libraries/AC_Avoidance/AP_OABendyRuler.cpp index 673dd552766971..de7f42fce20fb4 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) { diff --git a/libraries/AC_Avoidance/AP_OABendyRuler.h b/libraries/AC_Avoidance/AP_OABendyRuler.h index e282e647221942..1b480bdbcfcab5 100644 --- a/libraries/AC_Avoidance/AP_OABendyRuler.h +++ b/libraries/AC_Avoidance/AP_OABendyRuler.h @@ -22,8 +22,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[]; diff --git a/libraries/AC_Avoidance/AP_OADijkstra.cpp b/libraries/AC_Avoidance/AP_OADijkstra.cpp index 749cb140ee24a1..f5150cd6ea32cb 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; } @@ -83,6 +94,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_inclusion_polygon_with_margin_ok) { _inclusion_polygon_with_margin_ok = create_inclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id); if (!_inclusion_polygon_with_margin_ok) { + 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; @@ -93,6 +105,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_exclusion_polygon_with_margin_ok) { _exclusion_polygon_with_margin_ok = create_exclusion_polygon_with_margin(_polyfence_margin * 100.0f, error_id); if (!_exclusion_polygon_with_margin_ok) { + 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; @@ -103,6 +116,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_exclusion_circle_with_margin_ok) { _exclusion_circle_with_margin_ok = create_exclusion_circle_with_margin(_polyfence_margin * 100.0f, error_id); if (!_exclusion_circle_with_margin_ok) { + 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; @@ -114,6 +128,7 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t _polyfence_visgraph_ok = create_fence_visgraph(error_id); if (!_polyfence_visgraph_ok) { _shortest_path_ok = false; + 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; @@ -133,9 +148,10 @@ 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; } @@ -143,17 +159,28 @@ AP_OADijkstra::AP_OADijkstra_State AP_OADijkstra::update(const Location ¤t if (!_shortest_path_ok) { _shortest_path_ok = calc_shortest_path(current_loc, destination, error_id); if (!_shortest_path_ok) { + 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 +199,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 +223,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 +980,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..97c832a78a2515 100644 --- a/libraries/AC_Avoidance/AP_OADijkstra.h +++ b/libraries/AC_Avoidance/AP_OADijkstra.h @@ -30,8 +30,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: @@ -124,7 +132,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 +191,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 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_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index ada094b691e53a..a5df09f11b8e84 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -38,7 +38,7 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { const struct AP_Param::GroupInfo *AC_CustomControl::_backend_var_info[CUSTOMCONTROL_MAX_TYPES]; -AC_CustomControl::AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : +AC_CustomControl::AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : _dt(dt), _ahrs(ahrs), _att_control(att_control), diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index 9cda1f21db6cba..e4405b3c28a779 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include #include @@ -20,7 +20,7 @@ class AC_CustomControl_Backend; class AC_CustomControl { public: - AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& _att_control, AP_MotorsMulticopter*& motors, float dt); + AC_CustomControl(AP_AHRS_View*& ahrs, AC_AttitudeControl*& _att_control, AP_MotorsMulticopter*& motors, float dt); CLASS_NO_COPY(AC_CustomControl); /* Do not allow copies */ @@ -62,7 +62,7 @@ class AC_CustomControl { // References to external libraries AP_AHRS_View*& _ahrs; - AC_AttitudeControl_Multi*& _att_control; + AC_AttitudeControl*& _att_control; AP_MotorsMulticopter*& _motors; AP_Enum _controller_type; diff --git a/libraries/AC_CustomControl/AC_CustomControl_Backend.h b/libraries/AC_CustomControl/AC_CustomControl_Backend.h index c4068a7b1b8f4e..5355ef9cbc18d6 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Backend.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Backend.h @@ -7,7 +7,7 @@ class AC_CustomControl_Backend { public: - AC_CustomControl_Backend(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : + AC_CustomControl_Backend(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : _frontend(frontend), _ahrs(ahrs), _att_control(att_control), @@ -29,7 +29,7 @@ class AC_CustomControl_Backend protected: // References to external libraries AP_AHRS_View*& _ahrs; - AC_AttitudeControl_Multi*& _att_control; + AC_AttitudeControl*& _att_control; AP_MotorsMulticopter*& _motors; AC_CustomControl& _frontend; }; diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp index 222510c4f471d9..5c0550b0898d99 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.cpp @@ -28,7 +28,7 @@ const AP_Param::GroupInfo AC_CustomControl_Empty::var_info[] = { }; // initialize in the constructor -AC_CustomControl_Empty::AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : +AC_CustomControl_Empty::AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AC_CustomControl/AC_CustomControl_Empty.h b/libraries/AC_CustomControl/AC_CustomControl_Empty.h index 692815f1cc5207..e3a6374d216246 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_Empty.h +++ b/libraries/AC_CustomControl/AC_CustomControl_Empty.h @@ -10,7 +10,7 @@ class AC_CustomControl_Empty : public AC_CustomControl_Backend { public: - AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt); + AC_CustomControl_Empty(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt); Vector3f update(void) override; diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp index ddf6a40c6b4e0f..5535d449909403 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.cpp @@ -1,4 +1,5 @@ #include "AC_CustomControl_PID.h" +#include "AC_AttitudeControl/AC_AttitudeControl_Multi.h" #if CUSTOMCONTROL_PID_ENABLED @@ -315,7 +316,7 @@ const AP_Param::GroupInfo AC_CustomControl_PID::var_info[] = { AP_GROUPEND }; -AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt) : +AC_CustomControl_PID::AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt), _p_angle_roll2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), _p_angle_pitch2(AC_ATTITUDE_CONTROL_ANGLE_P * 0.90f), diff --git a/libraries/AC_CustomControl/AC_CustomControl_PID.h b/libraries/AC_CustomControl/AC_CustomControl_PID.h index b3cdf68884475b..75649a5dd92bf8 100644 --- a/libraries/AC_CustomControl/AC_CustomControl_PID.h +++ b/libraries/AC_CustomControl/AC_CustomControl_PID.h @@ -15,7 +15,7 @@ class AC_CustomControl_PID : public AC_CustomControl_Backend { public: - AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl_Multi*& att_control, AP_MotorsMulticopter*& motors, float dt); + AC_CustomControl_PID(AC_CustomControl& frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt); // run lowest level body-frame rate controller and send outputs to the motors Vector3f update() override; diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index aaec44932bd4ed..b537085e42113f 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 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_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_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 1c02fdb0a86bf9..be39a6cbdd9af6 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; 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 9cf414831acf7e..4107082558054b 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -56,7 +56,7 @@ const AP_Param::GroupInfo AR_PosControl::var_info[] = { // @Param: _VEL_I // @DisplayName: Velocity (horizontal) I gain // @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration - // @Range: 0.02 1.00 + // @Range: 0.00 1.00 // @Increment: 0.01 // @User: Advanced @@ -141,9 +141,11 @@ void AR_PosControl::update(float dt) } // calculation velocity error + bool stopping = false; if (_vel_desired_valid) { // add target velocity to desired velocity from position error _vel_target += _vel_desired; + stopping = _vel_desired.is_zero(); } // limit velocity to maximum speed @@ -192,19 +194,21 @@ void AR_PosControl::update(float dt) const Vector2f vel_target_FR = AP::ahrs().earth_to_body2D(_vel_target); // desired speed is normally the forward component (only) of the target velocity - // but we do not let it fall below the minimum turn speed unless the vehicle is slowing down - const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min); - float des_speed; - if (_reversed != backing_up) { - // if reversed or backing up desired speed will be negative - des_speed = MIN(-abs_des_speed_min, vel_target_FR.x); - } else { - des_speed = MAX(abs_des_speed_min, vel_target_FR.x); + float des_speed = vel_target_FR.x; + if (!stopping) { + // do not let target speed fall below the minimum turn speed unless the vehicle is slowing down + const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min); + if (_reversed != backing_up) { + // if reversed or backing up desired speed will be negative + des_speed = MIN(-abs_des_speed_min, vel_target_FR.x); + } else { + des_speed = MAX(abs_des_speed_min, vel_target_FR.x); + } } _desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt); // calculate turn rate from desired lateral acceleration - _desired_lat_accel = accel_target_FR.y; + _desired_lat_accel = stopping ? 0 : accel_target_FR.y; _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed); } diff --git a/libraries/AP_ADSB/AP_ADSB.cpp b/libraries/AP_ADSB/AP_ADSB.cpp index 2acbd81b62189f..53ba4291dc0cec 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); } diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4e061a465ec5df..d0985bd80315bd 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) @@ -178,6 +186,13 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { // index 17 + // @Param: OPTIONS + // @DisplayName: Optional AHRS behaviour + // @Description: This controls optional AHRS behaviour. Setting DisableDCMFallbackFW will change the AHRS behaviour for fixed wing aircraft in fly-forward flight to not fall back to DCM when the EKF stops navigating. Setting DisableDCMFallbackVTOL will change the AHRS behaviour for fixed wing aircraft in non fly-forward (VTOL) flight to not fall back to DCM when the EKF stops navigating. + // @Bitmask: 0:DisableDCMFallbackFW, 1:DisableDCMFallbackVTOL + // @User: Advanced + AP_GROUPINFO("OPTIONS", 18, AP_AHRS, _options, 0), + AP_GROUPEND }; @@ -213,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 @@ -227,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 @@ -805,6 +820,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 */ @@ -1890,21 +1931,11 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const } #if AP_AHRS_DCM_ENABLED - /* - fixed wing and rover will fall back to DCM if the EKF doesn't - have GPS. This is the safest option as DCM is very robust. Note - that we also check the filter status when fly_forward is false - and we are disarmed. This is to ensure that the arming checks do - wait for good GPS position on fixed wing and rover - */ + // 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 (!dcm.yaw_source_available() && !fly_forward) { - // if we don't have a DCM yaw source available and we are - // in a non-fly-forward mode then we are best off using the EKF - return ret; - } + bool should_use_gps = true; nav_filter_status filt_state; #if HAL_NAVEKF2_AVAILABLE @@ -1930,26 +1961,54 @@ AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const should_use_gps = true; } #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; + const bool can_use_ekf = filt_state.flags.attitude && filt_state.flags.vert_vel && filt_state.flags.vert_pos; + if (!can_use_dcm && can_use_ekf) { + // no choice - continue to use EKF + return ret; + } else if (!can_use_ekf) { + // No choice - we have to use DCM + return EKFType::DCM; + } + + const bool disable_dcm_fallback = fly_forward? + option_set(Options::DISABLE_DCM_FALLBACK_FW) : option_set(Options::DISABLE_DCM_FALLBACK_VTOL); + if (disable_dcm_fallback) { + // don't fallback + return ret; + } + + // Handle loss of global position when we still have a GPS fix if (hal.util->get_soft_armed() && - (!filt_state.flags.using_gps || - !filt_state.flags.horiz_pos_abs) && + (_gps_use != GPSUse::Disable) && should_use_gps && - AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D) { - // if the EKF is not fusing GPS or doesn't have a 2D fix - // and we have a 3D lock, then plane and rover would - // prefer to use the GPS position from DCM. This is a - // safety net while some issues with the EKF get sorted - // out + AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D && + (!filt_state.flags.using_gps || !filt_state.flags.horiz_pos_abs)) { + /* + If the EKF is not fusing GPS or doesn't have a 2D fix and we have a 3D GPS lock, + then plane and rover would prefer to use the GPS position from DCM unless the + fallback has been inhibited by the user. + Note: The aircraft could be dead reckoning with acceptable accuracy and rejecting a bad GPS + Note: This is a last resort fallback and makes the navigation highly vulnerable to GPS noise. + Note: When operating in a VTOL flight mode that actively controls height such as QHOVER, + the EKF gives better vertical velocity and position estimates and height control characteristics. + */ return EKFType::DCM; } + + // Handle complete loss of navigation if (hal.util->get_soft_armed() && filt_state.flags.const_pos_mode) { + /* + Provided the EKF has been configured to use GPS, ie should_use_gps is true, then the + key difference to the case handled above is only the absence of a GPS fix which means + that DCM will not be able to navigate either so we are primarily concerned with + providing an attitude, vertical position and vertical velocity estimate. + */ return EKFType::DCM; } - if (!filt_state.flags.attitude || - !filt_state.flags.vert_vel || - !filt_state.flags.vert_pos) { - return EKFType::DCM; - } + if (!filt_state.flags.horiz_vel || (!filt_state.flags.horiz_pos_abs && !filt_state.flags.horiz_pos_rel)) { if ((!AP::compass().use_for_yaw()) && @@ -3493,3 +3552,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 5ea8ae2ed953d8..abb36f2e13692b 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -114,6 +114,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 +430,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 HAL_EXTERNAL_AHRS_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 +548,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 +677,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 +701,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 +716,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 { @@ -991,6 +1007,15 @@ class AP_AHRS { struct AP_AHRS_Backend::Estimates external_estimates; #endif + enum class Options : uint16_t { + DISABLE_DCM_FALLBACK_FW=(1U<<0), + DISABLE_DCM_FALLBACK_VTOL=(1U<<1), + }; + AP_Int16 _options; + + bool option_set(Options option) const { + return (_options & uint16_t(option)) != 0; + } }; namespace AP { 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 df722c5cc52b07..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) @@ -1205,11 +1221,14 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void) bool AP_AHRS_DCM::get_vert_pos_rate_D(float &velocity) const { Vector3f velned; - if (!get_velocity_NED(velned)) { - return false; + if (get_velocity_NED(velned)) { + velocity = velned.z; + return true; + } else if (AP::baro().healthy()) { + velocity = -AP::baro().get_climb_rate(); + return true; } - velocity = velned.z; - return true; + return false; } // returns false if we fail arming checks, in which case the buffer will be populated with a failure message 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_config.h b/libraries/AP_AHRS/AP_AHRS_config.h index 0be9f0cdd828bd..7338a02787a875 100644 --- a/libraries/AP_AHRS/AP_AHRS_config.h +++ b/libraries/AP_AHRS/AP_AHRS_config.h @@ -16,16 +16,16 @@ #endif #ifndef HAL_NAVEKF2_AVAILABLE -// only default to EK2 enabled on boards with over 1M flash -#define HAL_NAVEKF2_AVAILABLE (BOARD_FLASH_SIZE>1024) +// 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_INERTIALSENSOR_ENABLED #endif #ifndef AP_AHRS_SIM_ENABLED -#define AP_AHRS_SIM_ENABLED AP_SIM_ENABLED +#define AP_AHRS_SIM_ENABLED AP_SIM_ENABLED && AP_INERTIALSENSOR_ENABLED #endif #ifndef AP_AHRS_POSITION_RESET_ENABLED 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_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index a13480b531c7e9..65c0b7b2911312 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; } @@ -731,6 +737,24 @@ 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 + // @LoggerMessage: HYGR // @Description: Hygrometer data // @Field: TimeUS: Time since system startup 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..ff6362b68849d4 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) @@ -596,6 +602,7 @@ bool AP_Arming::compass_checks(bool report) return true; } +#if AP_GPS_ENABLED bool AP_Arming::gps_checks(bool report) { const AP_GPS &gps = AP::gps(); @@ -686,7 +693,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 +708,7 @@ bool AP_Arming::battery_checks(bool report) } return true; } +#endif // AP_BATTERY_ENABLED bool AP_Arming::hardware_safety_check(bool report) { @@ -714,6 +724,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 +842,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 +907,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 +915,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 +1131,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 +1140,7 @@ bool AP_Arming::terrain_database_required() const if (mission->contains_terrain_alt_items()) { return true; } +#endif return false; } @@ -1166,10 +1184,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 +1198,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 @@ -1245,14 +1261,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 +1287,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 +1305,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 +1326,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 +1344,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 +1362,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 +1485,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 +1497,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 +1512,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 +1533,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 +1542,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 +1562,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 +1642,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)) { @@ -1646,17 +1702,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 +1745,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 +1765,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 +1814,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 +1830,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 +1859,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 +1869,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 +1877,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 +1895,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 +1976,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..5435ae060c86cc 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -899,7 +899,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_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 45f630ce3bb6f1..bf04aeb514aac4 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -21,6 +21,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 +561,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; @@ -1050,6 +1056,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 +1096,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() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index dbeed2e470ff18..e9e10b7ef06edb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -45,6 +45,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 +71,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 +111,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 +154,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 +276,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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 1aef15a6efda3b..842c6ac907852e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -107,6 +107,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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 85d62127708c82..907d8712c11748 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -58,12 +58,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 +98,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 +121,23 @@ 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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 281831f0d18390..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); @@ -137,13 +143,20 @@ void AP_BattMonitor_DroneCAN::update_interim_state(const float voltage, const fl const uint32_t tnow = AP_HAL::micros(); - if (!_has_battery_info_aux || _mppt.is_detected) { + if (!_has_battery_info_aux || + !use_CAN_SoC()) { const uint32_t dt_us = tnow - _interim_state.last_time_micros; // update total current drawn since startup 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; @@ -184,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 @@ -282,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_Logging.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp index f5595d321aca32..4f3433d87added 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp @@ -16,6 +16,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 +31,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)); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 5302bf77306a66..0eb859d2a4f833 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -17,7 +17,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), 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_CANManager/AP_CANDriver.cpp b/libraries/AP_CANManager/AP_CANDriver.cpp index 1f279c12a690de..0ec7ebbceb2a00 100644 --- a/libraries/AP_CANManager/AP_CANDriver.cpp +++ b/libraries/AP_CANManager/AP_CANDriver.cpp @@ -51,6 +51,14 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { AP_SUBGROUPPTR(_piccolocan, "PC_", 5, AP_CANManager::CANDriver_Params, AP_PiccoloCAN), #endif + // @Param: PROTOCOL2 + // @DisplayName: Secondary protocol with 11 bit CAN addressing + // @Description: Secondary protocol with 11 bit CAN addressing + // @Values: 0:Disabled,7:USD1,10:Scripting,11:Benewake,12:Scripting2,13:TOFSenseP,14:NanoRadar_NRA24 + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("PROTOCOL2", 6, AP_CANManager::CANDriver_Params, _driver_type_11bit, float(AP_CAN::Protocol::None)), + AP_GROUPEND }; #endif diff --git a/libraries/AP_CANManager/AP_CANDriver.h b/libraries/AP_CANManager/AP_CANDriver.h index a950535019f16c..f93e96cec0e7a8 100644 --- a/libraries/AP_CANManager/AP_CANDriver.h +++ b/libraries/AP_CANManager/AP_CANDriver.h @@ -21,6 +21,8 @@ #include class AP_CANManager; +class CANSensor; + class AP_CANDriver { public: @@ -34,4 +36,9 @@ class AP_CANDriver // link protocol drivers with interfaces by adding reference to CANIface virtual bool add_interface(AP_HAL::CANIface* can_iface) = 0; + // add an 11 bit auxillary driver + virtual bool add_11bit_driver(CANSensor *sensor) { return false; } + + // handler for outgoing frames for auxillary drivers + virtual bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) { return false; } }; diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 78be64a97c66c9..20918ede682a67 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -119,6 +119,9 @@ void AP_CANManager::init() { WITH_SEMAPHORE(_sem); + // we need to mutate the HAL to install new CAN interfaces + AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable(); + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (AP::sitl() == nullptr) { AP_HAL::panic("CANManager: SITL not initialised!"); @@ -146,17 +149,17 @@ void AP_CANManager::init() } drv_num--; - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { // So if this interface is not allocated allocate it here, // also pass the index of the CANBus - const_cast (hal).can[i] = new HAL_CANIface(i); + hal_mutable.can[i] = new HAL_CANIface(i); } // Initialise the interface we just allocated - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { continue; } - AP_HAL::CANIface* iface = hal.can[i]; + AP_HAL::CANIface* iface = hal_mutable.can[i]; // Find the driver type that we need to allocate and register this interface with drv_type[drv_num] = (AP_CAN::Protocol) _drv_param[drv_num]._driver_type.get(); @@ -166,13 +169,13 @@ void AP_CANManager::init() #if AP_CAN_SLCAN_ENABLED if (_slcan_interface.init_passthrough(i)) { // we have slcan bridge setup pass that on as can iface - can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); + can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); iface = &_slcan_interface; } else { #else if (true) { #endif - can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); + can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode); } if (!can_initialised) { @@ -245,8 +248,8 @@ void AP_CANManager::init() bool enable_filter = false; for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { if (_interfaces[i]._driver_number == (drv_num+1) && - hal.can[i] != nullptr && - hal.can[i]->get_operating_mode() == AP_HAL::CANIface::FilteredMode) { + hal_mutable.can[i] != nullptr && + hal_mutable.can[i]->get_operating_mode() == AP_HAL::CANIface::FilteredMode) { // Don't worry we don't enable Filters for Normal Ifaces under the driver // this is just to ensure we enable them for the ones we already decided on enable_filter = true; @@ -277,6 +280,7 @@ void AP_CANManager::init() } } #endif + /* register a new CAN driver */ @@ -284,6 +288,9 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver { WITH_SEMAPHORE(_sem); + // we need to mutate the HAL to install new CAN interfaces + AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable(); + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { uint8_t drv_num = _interfaces[i]._driver_number; if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) { @@ -302,17 +309,17 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver continue; } - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { // if this interface is not allocated allocate it here, // also pass the index of the CANBus - const_cast (hal).can[i] = new HAL_CANIface(i); + hal_mutable.can[i] = new HAL_CANIface(i); } // Initialise the interface we just allocated - if (hal.can[i] == nullptr) { + if (hal_mutable.can[i] == nullptr) { continue; } - AP_HAL::CANIface* iface = hal.can[i]; + AP_HAL::CANIface* iface = hal_mutable.can[i]; _drivers[drv_num] = driver; _drivers[drv_num]->add_interface(iface); @@ -328,6 +335,32 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver return false; } +// register a new auxillary sensor driver for 11 bit address frames +bool AP_CANManager::register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index) +{ + WITH_SEMAPHORE(_sem); + + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + uint8_t drv_num = _interfaces[i]._driver_number; + if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) { + continue; + } + // from 1 based to 0 based + drv_num--; + + if (dtype != (AP_CAN::Protocol)_drv_param[drv_num]._driver_type_11bit.get()) { + continue; + } + if (_drivers[drv_num] != nullptr && + _drivers[drv_num]->add_11bit_driver(sensor)) { + driver_index = drv_num; + return true; + } + } + return false; + +} + // Method used by CAN related library methods to report status and debug info // The result of this method can be accessed via ftp get @SYS/can_log.txt void AP_CANManager::log_text(AP_CANManager::LogLevel loglevel, const char *tag, const char *fmt, ...) diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index 85d4aa31ed31f2..5c352ae5b39231 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -34,6 +34,8 @@ #include "AP_CAN.h" +class CANSensor; + class AP_CANManager { public: @@ -63,6 +65,9 @@ class AP_CANManager // register a new driver bool register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver); + // register a new auxillary sensor driver for 11 bit address frames + bool register_11bit_driver(AP_CAN::Protocol dtype, CANSensor *sensor, uint8_t &driver_index); + // returns number of active CAN Drivers uint8_t get_num_drivers(void) const { @@ -141,6 +146,7 @@ class AP_CANManager private: AP_Int8 _driver_type; + AP_Int8 _driver_type_11bit; AP_CANDriver* _uavcan; AP_CANDriver* _piccolocan; }; diff --git a/libraries/AP_CANManager/AP_CANSensor.cpp b/libraries/AP_CANManager/AP_CANSensor.cpp index 881eb39ce93c8f..8e300eba0921bb 100644 --- a/libraries/AP_CANManager/AP_CANSensor.cpp +++ b/libraries/AP_CANManager/AP_CANSensor.cpp @@ -40,7 +40,13 @@ void CANSensor::register_driver(AP_CAN::Protocol dtype) { #if HAL_CANMANAGER_ENABLED if (!AP::can().register_driver(dtype, this)) { - debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name); + if (AP::can().register_11bit_driver(dtype, this, _driver_index)) { + is_aux_11bit_driver = true; + _can_driver = AP::can().get_driver(_driver_index); + _initialized = true; + } else { + debug_can(AP_CANManager::LOG_ERROR, "Failed to register CANSensor %s", _driver_name); + } } else { debug_can(AP_CANManager::LOG_INFO, "%s: constructed", _driver_name); } @@ -121,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; } @@ -135,6 +141,10 @@ bool CANSensor::write_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_ return false; } + if (is_aux_11bit_driver && _can_driver != nullptr) { + return _can_driver->write_aux_frame(out_frame, timeout_us); + } + bool read_select = false; bool write_select = true; bool ret = _can_iface->select(read_select, write_select, &out_frame, AP_HAL::micros64() + timeout_us); diff --git a/libraries/AP_CANManager/AP_CANSensor.h b/libraries/AP_CANManager/AP_CANSensor.h index a1374821e40e4e..2cd775f4c39f78 100644 --- a/libraries/AP_CANManager/AP_CANSensor.h +++ b/libraries/AP_CANManager/AP_CANSensor.h @@ -72,8 +72,12 @@ class CANSensor : public AP_CANDriver { const uint16_t _stack_size; bool _initialized; uint8_t _driver_index; + + // this is true when we are setup as an auxillary driver using CAN_Dn_PROTOCOL2 + 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.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_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_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_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index ed8f338e549be9..d573888c77395f 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -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; 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_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 0e4f309fad0b86..b05c411e5d523c 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #if APM_BUILD_TYPE(APM_BUILD_Replay) #include @@ -46,8 +47,12 @@ void AP_DAL::start_frame(AP_DAL::FrameType frametype) 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); 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 60980e510f8f9a..ebece8962676ac 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -9,6 +9,7 @@ extern const AP_HAL::HAL& hal; #define LOG_TAG "DroneCANIface" #include +#include #define DEBUG_PKTS 0 @@ -346,6 +347,15 @@ void CanardInterface::processRx() { if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) { break; } + + if (!rxmsg.isExtended()) { + // 11 bit frame, see if we have a handler + if (aux_11bit_driver != nullptr) { + aux_11bit_driver->handle_frame(rxmsg); + } + continue; + } + rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); #if HAL_CANFD_SUPPORTED @@ -396,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; } @@ -426,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; } @@ -434,4 +443,29 @@ bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface) num_ifaces++; return true; } + +// add an 11 bit auxillary driver +bool CanardInterface::add_11bit_driver(CANSensor *sensor) +{ + if (aux_11bit_driver != nullptr) { + // only allow one + return false; + } + aux_11bit_driver = sensor; + return true; +} + +// handler for outgoing frames for auxillary drivers +bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +{ + bool ret = false; + for (uint8_t iface = 0; iface < num_ifaces; iface++) { + if (ifaces[iface] == NULL) { + continue; + } + ret |= ifaces[iface]->send(out_frame, timeout_us, 0) > 0; + } + return ret; +} + #endif // #if HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h index aa7533e6913b3f..43d786639fdf05 100644 --- a/libraries/AP_DroneCAN/AP_Canard_iface.h +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -5,6 +5,8 @@ #include class AP_DroneCAN; +class CANSensor; + class CanardInterface : public Canard::Interface { friend class AP_DroneCAN; public: @@ -48,6 +50,12 @@ class CanardInterface : public Canard::Interface { bool add_interface(AP_HAL::CANIface *can_drv); + // add an auxillary driver for 11 bit frames + bool add_11bit_driver(CANSensor *sensor); + + // handler for outgoing frames for auxillary drivers + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us); + #if AP_TEST_DRONECAN_DRIVERS static CanardInterface& get_test_iface() { return test_iface; } static void processTestRx(); @@ -64,11 +72,14 @@ 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; CanardTxTransfer tx_transfer; dronecan_protocol_Stats protocol_stats; + + // auxillary 11 bit CANSensor + CANSensor *aux_11bit_driver; }; -#endif // HAL_ENABLE_DRONECAN_DRIVERS \ No newline at end of file +#endif // HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp index 55d148bf1f90c0..5667f5a54af4f5 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -55,6 +55,10 @@ #include "AP_DroneCAN_serial.h" #endif +#if AP_RELAY_DRONECAN_ENABLED +#include +#endif + extern const AP_HAL::HAL& hal; // setup default pool size @@ -153,6 +157,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 +266,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 }; @@ -434,6 +450,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 +470,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 +550,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 +1234,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 */ @@ -1751,4 +1800,20 @@ void AP_DroneCAN::logging(void) #endif // HAL_LOGGING_ENABLED } +// add an 11 bit auxillary driver +bool AP_DroneCAN::add_11bit_driver(CANSensor *sensor) +{ + return canard_iface.add_11bit_driver(sensor); +} + +// handler for outgoing frames for auxillary drivers +bool AP_DroneCAN::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) +{ + if (out_frame.isExtended()) { + // don't allow extended frames to be sent by auxillary driver + return false; + } + return canard_iface.write_aux_frame(out_frame, timeout_us); +} + #endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h index f502e05a39f276..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 @@ -69,6 +70,7 @@ // fwd-declare callback classes class AP_DroneCAN_DNA_Server; +class CANSensor; class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { friend class AP_DroneCAN_DNA_Server; @@ -85,6 +87,12 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { void init(uint8_t driver_index, bool enable_filters) override; bool add_interface(AP_HAL::CANIface* can_iface) override; + // add an 11 bit auxillary driver + bool add_11bit_driver(CANSensor *sensor) override; + + // handler for outgoing frames for auxillary drivers + bool write_aux_frame(AP_HAL::CANFrame &out_frame, const uint64_t timeout_us) override; + uint8_t get_driver_index() const { return _driver_index; } // define string with length structure @@ -158,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); @@ -265,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_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_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp index 97a7850d41efdd..5745e94b761d04 100644 --- a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp +++ b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp @@ -117,15 +117,18 @@ static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan void DroneCAN_sniffer::init(void) { - const_cast (hal).can[driver_index] = new HAL_CANIface(driver_index); + // we need to mutate the HAL to install new CAN interfaces + AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable(); + + hal_mutable.can[driver_index] = new HAL_CANIface(driver_index); - if (hal.can[driver_index] == nullptr) { + if (hal_mutable.can[driver_index] == nullptr) { AP_HAL::panic("Couldn't allocate CANManager, something is very wrong"); } - hal.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode); + hal_mutable.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode); - if (!hal.can[driver_index]->is_initialized()) { + if (!hal_mutable.can[driver_index]->is_initialized()) { debug_dronecan("Can not initialised\n"); return; } @@ -135,7 +138,7 @@ void DroneCAN_sniffer::init(void) return; } - if (!_uavcan_iface_mgr->add_interface(hal.can[driver_index])) { + if (!_uavcan_iface_mgr->add_interface(hal_mutable.can[driver_index])) { debug_dronecan("Failed to add iface"); return; } 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..7bb347220d0c0b 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; } @@ -498,12 +502,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 +518,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, 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 c1769fe2a00a8e..9b15e6135bfe21 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp @@ -24,8 +24,11 @@ #include "AP_ExternalAHRS_backend.h" #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; @@ -53,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:MicroStrain + // @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), @@ -93,16 +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)); @@ -160,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; } @@ -236,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 89e2331cb86188..557f1e9c699940 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS.h @@ -49,6 +49,17 @@ class AP_ExternalAHRS { #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 + // 6 reserved for Trimble +#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED + MicroStrain7 = 7, +#endif + // 8 reserved for SBG + // 9 reserved for EulerNav }; static AP_ExternalAHRS *get_singleton(void) { @@ -87,6 +98,8 @@ class AP_ExternalAHRS { bool have_origin; bool have_location; bool have_velocity; + + uint32_t last_location_update_us; } state; // accessors for AP_AHRS @@ -145,6 +158,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 { @@ -166,6 +184,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..9019eaef461f01 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp @@ -0,0 +1,716 @@ +/* + 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 (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); + } + + 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 47bffb606a1e29..ed6304429c28d8 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain5.cpp @@ -34,6 +34,8 @@ extern const AP_HAL::HAL &hal; +static constexpr uint8_t gnss_instance = 0; + AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_frontend, AP_ExternalAHRS::state_t &_state): AP_ExternalAHRS_backend(_frontend, _state) { @@ -44,16 +46,16 @@ AP_ExternalAHRS_MicroStrain5::AP_ExternalAHRS_MicroStrain5(AP_ExternalAHRS *_fro port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); if (!uart) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ExternalAHRS no UART"); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MicroStrain5 ExternalAHRS no UART"); return; } if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain5::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { - AP_BoardConfig::allocation_error("Failed to allocate ExternalAHRS update thread"); + AP_BoardConfig::allocation_error("MicroStrain5 failed to allocate ExternalAHRS update thread"); } hal.scheduler->delay(5000); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain ExternalAHRS initialised"); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "MicroStrain5 ExternalAHRS initialised"); } void AP_ExternalAHRS_MicroStrain5::update_thread(void) @@ -92,8 +94,10 @@ void AP_ExternalAHRS_MicroStrain5::build_packet() post_imu(); break; case DescriptorSet::GNSSData: + case DescriptorSet::GNSSRecv1: + case DescriptorSet::GNSSRecv2: break; - case DescriptorSet::EstimationData: + case DescriptorSet::FilterData: post_filter(); break; case DescriptorSet::BaseCommand: @@ -157,26 +161,27 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down}; state.have_velocity = true; - state.location = Location{filter_data.lat, filter_data.lon, gnss_data.msl_altitude, Location::AltFrame::ABSOLUTE}; + 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 { gps_week: filter_data.week, ms_tow: filter_data.tow_ms, - fix_type: (uint8_t) gnss_data.fix_type, - satellites_in_view: gnss_data.satellites, + fix_type: (uint8_t) gnss_data[gnss_instance].fix_type, + satellites_in_view: gnss_data[gnss_instance].satellites, - horizontal_pos_accuracy: gnss_data.horizontal_position_accuracy, - vertical_pos_accuracy: gnss_data.vertical_position_accuracy, - horizontal_vel_accuracy: gnss_data.speed_accuracy, + horizontal_pos_accuracy: gnss_data[gnss_instance].horizontal_position_accuracy, + vertical_pos_accuracy: gnss_data[gnss_instance].vertical_position_accuracy, + horizontal_vel_accuracy: gnss_data[gnss_instance].speed_accuracy, - hdop: gnss_data.hdop, - vdop: gnss_data.vdop, + hdop: gnss_data[gnss_instance].hdop, + vdop: gnss_data[gnss_instance].vdop, longitude: filter_data.lon, latitude: filter_data.lat, - msl_altitude: gnss_data.msl_altitude, + msl_altitude: gnss_data[gnss_instance].msl_altitude, ned_vel_north: filter_data.ned_velocity_north, ned_vel_east: filter_data.ned_velocity_east, @@ -187,14 +192,14 @@ void AP_ExternalAHRS_MicroStrain5::post_filter() const WITH_SEMAPHORE(state.sem); state.origin = Location{int32_t(filter_data.lat), int32_t(filter_data.lon), - int32_t(gnss_data.msl_altitude), + int32_t(gnss_data[gnss_instance].msl_altitude), Location::AltFrame::ABSOLUTE}; state.have_origin = true; } - uint8_t instance; - if (AP::gps().get_first_external_instance(instance)) { - AP::gps().handle_external(gps, instance); + uint8_t gps_instance; + if (AP::gps().get_first_external_instance(gps_instance)) { + AP::gps().handle_external(gps, gps_instance); } } @@ -215,26 +220,26 @@ const char* AP_ExternalAHRS_MicroStrain5::get_name() const bool AP_ExternalAHRS_MicroStrain5::healthy(void) const { uint32_t now = AP_HAL::millis(); - return (now - last_ins_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500); + return (now - last_imu_pkt < 40 && now - last_gps_pkt < 500 && now - last_filter_pkt < 500); } bool AP_ExternalAHRS_MicroStrain5::initialised(void) const { - return last_ins_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0; + return last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0; } bool AP_ExternalAHRS_MicroStrain5::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { if (!healthy()) { - hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain unhealthy"); + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 unhealthy"); return false; } - if (gnss_data.fix_type < 3) { - hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain no GPS lock"); + if (gnss_data[gnss_instance].fix_type < 3) { + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 no GPS lock"); return false; } if (filter_status.state != 0x02) { - hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain filter not running"); + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain5 filter not running"); return false; } @@ -244,15 +249,15 @@ bool AP_ExternalAHRS_MicroStrain5::pre_arm_check(char *failure_msg, uint8_t fail void AP_ExternalAHRS_MicroStrain5::get_filter_status(nav_filter_status &status) const { memset(&status, 0, sizeof(status)); - if (last_ins_pkt != 0 && last_gps_pkt != 0) { + if (last_imu_pkt != 0 && last_gps_pkt != 0) { status.flags.initalized = true; } - if (healthy() && last_ins_pkt != 0) { + if (healthy() && last_imu_pkt != 0) { status.flags.attitude = true; status.flags.vert_vel = true; status.flags.vert_pos = true; - if (gnss_data.fix_type >= 3) { + if (gnss_data[gnss_instance].fix_type >= 3) { status.flags.horiz_vel = true; status.flags.horiz_pos_rel = true; status.flags.horiz_pos_abs = true; @@ -309,7 +314,7 @@ void AP_ExternalAHRS_MicroStrain5::send_status_report(GCS_MAVLINK &link) const const float hgt_gate = 4; // represents hz value data is posted at const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav mavlink_msg_ekf_status_report_send(link.get_chan(), flags, - gnss_data.speed_accuracy/vel_gate, gnss_data.horizontal_position_accuracy/pos_gate, gnss_data.vertical_position_accuracy/hgt_gate, + gnss_data[gnss_instance].speed_accuracy/vel_gate, gnss_data[gnss_instance].horizontal_position_accuracy/pos_gate, gnss_data[gnss_instance].vertical_position_accuracy/hgt_gate, mag_var, 0, 0); } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp new file mode 100644 index 00000000000000..09762f67d6aa88 --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.cpp @@ -0,0 +1,384 @@ +/* + 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 MicroStrain GQ7 serially connected AHRS Systems + Usage in SITL with hardware for debugging: + $ 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 7 + param set GPS_TYPE 21 + param set SERIAL3_BAUD 115 + param set SERIAL3_PROTOCOL 36 + UDEV rules for repeatable USB connection: + $ cat /etc/udev/rules.d/99-usb-serial.rules + SUBSYSTEM=="tty", ATTRS{manufacturer}=="Lord Microstrain", SYMLINK+="3dm-gq7" + Usage with simulated MicroStrain7: + ./Tools/autotest/sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG + */ + +#define ALLOW_DOUBLE_MATH_FUNCTIONS + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED + +#include "AP_ExternalAHRS_MicroStrain7.h" +#include "AP_Compass/AP_Compass_config.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +AP_ExternalAHRS_MicroStrain7::AP_ExternalAHRS_MicroStrain7(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); + + baudrate = sm.find_baudrate(AP_SerialManager::SerialProtocol_AHRS, 0); + port_num = sm.find_portnum(AP_SerialManager::SerialProtocol_AHRS, 0); + + if (!uart) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "MicroStrain7 ExternalAHRS no UART"); + return; + } + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_ExternalAHRS_MicroStrain7::update_thread, void), "AHRS", 2048, AP_HAL::Scheduler::PRIORITY_SPI, 0)) { + 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"); +} + +void AP_ExternalAHRS_MicroStrain7::update_thread(void) +{ + if (!port_open) { + port_open = true; + uart->begin(baudrate); + } + + while (true) { + build_packet(); + hal.scheduler->delay_microseconds(100); + } +} + + + +// Builds packets by looking at each individual byte, once a full packet has been read in it checks the checksum then handles the packet. +void AP_ExternalAHRS_MicroStrain7::build_packet() +{ + if (uart == nullptr) { + return; + } + + WITH_SEMAPHORE(sem); + uint32_t nbytes = MIN(uart->available(), 2048u); + while (nbytes--> 0) { + uint8_t b; + if (!uart->read(b)) { + break; + } + DescriptorSet descriptor; + if (handle_byte(b, descriptor)) { + switch (descriptor) { + case DescriptorSet::IMUData: + post_imu(); + break; + case DescriptorSet::GNSSData: + case DescriptorSet::GNSSRecv1: + case DescriptorSet::GNSSRecv2: + break; + case DescriptorSet::FilterData: + post_filter(); + break; + case DescriptorSet::BaseCommand: + case DescriptorSet::DMCommand: + case DescriptorSet::SystemCommand: + break; + } + } + } +} + + + +// Posts data from an imu packet to `state` and `handle_external` methods +void AP_ExternalAHRS_MicroStrain7::post_imu() const +{ + { + WITH_SEMAPHORE(state.sem); + state.accel = imu_data.accel; + state.gyro = imu_data.gyro; + + state.quat = imu_data.quat; + state.have_quaternion = true; + } + + { + // *INDENT-OFF* + AP_ExternalAHRS::ins_data_message_t ins { + accel: imu_data.accel, + gyro: imu_data.gyro, + temperature: -300 + }; + // *INDENT-ON* + AP::ins().handle_external(ins); + } + +#if AP_COMPASS_EXTERNALAHRS_ENABLED + { + // *INDENT-OFF* + AP_ExternalAHRS::mag_data_message_t mag { + field: imu_data.mag + }; + // *INDENT-ON* + AP::compass().handle_external(mag); + } +#endif + +#if AP_BARO_EXTERNALAHRS_ENABLED + { + // *INDENT-OFF* + const AP_ExternalAHRS::baro_data_message_t baro { + instance: 0, + pressure_pa: imu_data.pressure, + // setting temp to 25 effectively disables barometer temperature calibrations - these are already performed by MicroStrain + temperature: 25, + }; + // *INDENT-ON* + AP::baro().handle_external(baro); + } +#endif +} + +void AP_ExternalAHRS_MicroStrain7::post_filter() const +{ + { + WITH_SEMAPHORE(state.sem); + state.velocity = Vector3f{filter_data.ned_velocity_north, filter_data.ned_velocity_east, filter_data.ned_velocity_down}; + state.have_velocity = true; + + // TODO the filter does not supply MSL altitude. + // The GNSS system has both MSL and WGS-84 ellipsoid height. + // Use GNSS 0 even though it may be bad. + state.location = Location{filter_data.lat, filter_data.lon, gnss_data[0].msl_altitude, Location::AltFrame::ABSOLUTE}; + state.have_location = true; + } + + for (int instance = 0; instance < NUM_GNSS_INSTANCES; instance++) { + // *INDENT-OFF* + AP_ExternalAHRS::gps_data_message_t gps { + gps_week: filter_data.week, + ms_tow: filter_data.tow_ms, + fix_type: (uint8_t) gnss_data[instance].fix_type, + satellites_in_view: gnss_data[instance].satellites, + + horizontal_pos_accuracy: gnss_data[instance].horizontal_position_accuracy, + vertical_pos_accuracy: gnss_data[instance].vertical_position_accuracy, + horizontal_vel_accuracy: gnss_data[instance].speed_accuracy, + + hdop: gnss_data[instance].hdop, + vdop: gnss_data[instance].vdop, + + longitude: filter_data.lon, + latitude: filter_data.lat, + msl_altitude: gnss_data[instance].msl_altitude, + + ned_vel_north: filter_data.ned_velocity_north, + ned_vel_east: filter_data.ned_velocity_east, + ned_vel_down: filter_data.ned_velocity_down, + }; + // *INDENT-ON* + + if (gps.fix_type >= 3 && !state.have_origin) { + WITH_SEMAPHORE(state.sem); + state.origin = Location{int32_t(filter_data.lat), + int32_t(filter_data.lon), + int32_t(gnss_data[instance].msl_altitude), + Location::AltFrame::ABSOLUTE}; + state.have_origin = true; + } + AP::gps().handle_external(gps, instance); + } +} + +int8_t AP_ExternalAHRS_MicroStrain7::get_port(void) const +{ + if (!uart) { + return -1; + } + return port_num; +}; + +// Get model/type name +const char* AP_ExternalAHRS_MicroStrain7::get_name() const +{ + return "MICROSTRAIN7"; +} + +bool AP_ExternalAHRS_MicroStrain7::healthy(void) const +{ + uint32_t now = AP_HAL::millis(); + + // Expect the following rates: + // * Navigation Filter: 25Hz = 40mS + // * GPS: 2Hz = 500mS + // * IMU: 25Hz = 40mS + + // Allow for some slight variance of 10% + constexpr float RateFoS = 1.1; + + constexpr uint32_t expected_filter_time_delta_ms = 40; + constexpr uint32_t expected_gps_time_delta_ms = 500; + constexpr uint32_t expected_imu_time_delta_ms = 40; + + const bool times_healthy = (now - last_imu_pkt < expected_imu_time_delta_ms * RateFoS && \ + now - last_gps_pkt < expected_gps_time_delta_ms * RateFoS && \ + now - last_filter_pkt < expected_filter_time_delta_ms * RateFoS); + const auto filter_state = static_cast(filter_status.state); + const bool filter_healthy = (filter_state == FilterState::GQ7_FULL_NAV || filter_state == FilterState::GQ7_AHRS); + return times_healthy && filter_healthy; +} + +bool AP_ExternalAHRS_MicroStrain7::initialised(void) const +{ + const bool got_packets = last_imu_pkt != 0 && last_gps_pkt != 0 && last_filter_pkt != 0; + const auto filter_state = static_cast(filter_status.state); + const bool filter_healthy = filter_state_healthy(filter_state); + return got_packets && filter_healthy; +} + +bool AP_ExternalAHRS_MicroStrain7::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 unhealthy"); + return false; + } + // TODO is this necessary? hard coding the first instance. + if (gnss_data[0].fix_type < 3) { + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 no GPS lock"); + return false; + } + if (!filter_state_healthy(FilterState(filter_status.state))) { + hal.util->snprintf(failure_msg, failure_msg_len, "MicroStrain7 filter not running"); + return false; + } + + return true; +} + +void AP_ExternalAHRS_MicroStrain7::get_filter_status(nav_filter_status &status) const +{ + memset(&status, 0, sizeof(status)); + if (last_imu_pkt != 0 && last_gps_pkt != 0) { + status.flags.initalized = true; + } + if (healthy() && last_imu_pkt != 0) { + status.flags.attitude = true; + status.flags.vert_vel = true; + status.flags.vert_pos = true; + + const auto filter_state = static_cast(filter_status.state); + if (filter_state_healthy(filter_state)) { + status.flags.horiz_vel = true; + status.flags.horiz_pos_rel = true; + status.flags.horiz_pos_abs = true; + status.flags.pred_horiz_pos_rel = true; + status.flags.pred_horiz_pos_abs = true; + status.flags.using_gps = true; + } + } +} + +void AP_ExternalAHRS_MicroStrain7::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 = 4; // represents hz value data is posted at + const float pos_gate = 4; // represents hz value data is posted at + const float hgt_gate = 4; // represents hz value data is posted at + const float mag_var = 0; //we may need to change this to be like the other gates, set to 0 because mag is ignored by the ins filter in vectornav + + // TODO fix to use NED filter speed accuracy instead of first gnss + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_ned_vel_uncertainty.htm + mavlink_msg_ekf_status_report_send(link.get_chan(), flags, + gnss_data[0].speed_accuracy/vel_gate, gnss_data[0].horizontal_position_accuracy/pos_gate, gnss_data[0].vertical_position_accuracy/hgt_gate, + mag_var, 0, 0); + +} + +bool AP_ExternalAHRS_MicroStrain7::filter_state_healthy(FilterState state) +{ + switch (state) { + case FilterState::GQ7_FULL_NAV: + case FilterState::GQ7_AHRS: + return true; + default: + return false; + } + // return state == FilterState::GQ7_FULL_NAV || state == FilterState::GQ7_AHRS; +} + +#endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h new file mode 100644 index 00000000000000..d9e0832272715d --- /dev/null +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_MicroStrain7.h @@ -0,0 +1,88 @@ +/* + 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 MicroStrain GQ7 serially connected AHRS Systems + */ + +#pragma once + +#include "AP_ExternalAHRS_config.h" + +#if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED + +#include "AP_ExternalAHRS_backend.h" +#include +#include +#include "MicroStrain_common.h" + +class AP_ExternalAHRS_MicroStrain7: public AP_ExternalAHRS_backend, public AP_MicroStrain +{ +public: + + AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state); + + // get serial port number, -1 for not enabled + int8_t get_port(void) const override; + + // Get model/type name + const char* get_name() 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 + { + build_packet(); + }; + +private: + + // GQ7 Filter States + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm + enum class FilterState { + GQ7_INIT = 0x01, + GQ7_VERT_GYRO = 0x02, + GQ7_AHRS = 0x03, + GQ7_FULL_NAV = 0x04 + }; + + uint32_t baudrate; + int8_t port_num; + bool port_open = false; + + + + void build_packet(); + + void post_imu() const; + void post_gnss() const; + void post_filter() const; + + void update_thread(); + + // Only some of the fix types satisfy a healthy filter. + // GQ7_VERT_GYRO is NOT considered healthy for now. + // This may be vehicle-dependent in the future. + static bool filter_state_healthy(FilterState state) WARN_IF_UNUSED; + + AP_HAL::UARTDriver *uart; + HAL_Semaphore sem; + +}; + +#endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp index 8e8515b70aa17d..66c20f05590e22 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; } diff --git a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h index b35d099c2ff1de..02eb1b5319b7d0 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_backend.h @@ -32,21 +32,32 @@ class AP_ExternalAHRS_backend { // Get model/type name virtual const char* get_name() const = 0; - // accessors for AP_AHRS + // Accessors for AP_AHRS + + // If not healthy, none of the other API's can be trusted. + // Example: Serial cable is severed. virtual bool healthy(void) const = 0; + // The communication interface is up and the device has sent valid data. virtual bool initialised(void) const = 0; virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0; virtual void get_filter_status(nav_filter_status &status) const {} virtual void send_status_report(class GCS_MAVLINK &link) const {} - // check for new data + // Check for new data. + // This is used when there's not a separate thread for EAHRS. + // This can also copy interim state protected by locking. virtual void update() = 0; - + protected: AP_ExternalAHRS::state_t &state; 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 ef9ef52158833b..75e8e5228021bb 100644 --- a/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h +++ b/libraries/AP_ExternalAHRS/AP_ExternalAHRS_config.h @@ -14,10 +14,18 @@ #define AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED +#define AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED AP_EXTERNAL_AHRS_BACKEND_DEFAULT_ENABLED +#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 d5b723cb125c6a..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 @@ -23,35 +23,43 @@ #include "MicroStrain_common.h" #include +// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/sensor_data/sensor_data_links.htm enum class INSPacketField { ACCEL = 0x04, GYRO = 0x05, QUAT = 0x0A, MAG = 0x06, - PRESSURE = 0x17 + PRESSURE = 0x17, }; +// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/gnss_recv_1_links.htm enum class GNSSPacketField { LLH_POSITION = 0x03, NED_VELOCITY = 0x05, DOP_DATA = 0x07, - GPS_TIME = 0x09, - FIX_INFO = 0x0B + FIX_INFO = 0x0B, + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm + GPS_TIMESTAMP = 0xD3, }; +// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_fix_info.htm enum class GNSSFixType { FIX_3D = 0x00, FIX_2D = 0x01, TIME_ONLY = 0x02, NONE = 0x03, - INVALID = 0x04 + INVALID = 0x04, + FIX_RTK_FLOAT = 0x05, + FIX_RTK_FIXED = 0x06, }; +// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/filter_data_links.htm enum class FilterPacketField { FILTER_STATUS = 0x10, - GPS_TIME = 0x11, LLH_POSITION = 0x01, - NED_VELOCITY = 0x02 + NED_VELOCITY = 0x02, + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm + GPS_TIMESTAMP = 0xD3, }; bool AP_MicroStrain::handle_byte(const uint8_t b, DescriptorSet& descriptor) @@ -72,17 +80,17 @@ bool AP_MicroStrain::handle_byte(const uint8_t b, DescriptorSet& descriptor) } break; case ParseState::WaitingFor_Descriptor: - message_in.packet.header[2] = b; + message_in.packet.descriptor_set(b); message_in.state = ParseState::WaitingFor_PayloadLength; break; case ParseState::WaitingFor_PayloadLength: - message_in.packet.header[3] = b; + message_in.packet.payload_length(b); message_in.state = ParseState::WaitingFor_Data; message_in.index = 0; break; case ParseState::WaitingFor_Data: message_in.packet.payload[message_in.index++] = b; - if (message_in.index >= message_in.packet.header[3]) { + if (message_in.index >= message_in.packet.payload_length()) { message_in.state = ParseState::WaitingFor_Checksum; message_in.index = 0; } @@ -113,7 +121,7 @@ bool AP_MicroStrain::valid_packet(const MicroStrain_Packet & packet) checksum_two += checksum_one; } - for (int i = 0; i < packet.header[3]; i++) { + for (int i = 0; i < packet.payload_length(); i++) { checksum_one += packet.payload[i]; checksum_two += checksum_one; } @@ -123,21 +131,23 @@ bool AP_MicroStrain::valid_packet(const MicroStrain_Packet & packet) AP_MicroStrain::DescriptorSet AP_MicroStrain::handle_packet(const MicroStrain_Packet& packet) { - const DescriptorSet descriptor = DescriptorSet(packet.header[2]); + const DescriptorSet descriptor = packet.descriptor_set(); switch (descriptor) { case DescriptorSet::IMUData: handle_imu(packet); break; - case DescriptorSet::GNSSData: - handle_gnss(packet); - break; - case DescriptorSet::EstimationData: + case DescriptorSet::FilterData: handle_filter(packet); break; case DescriptorSet::BaseCommand: case DescriptorSet::DMCommand: case DescriptorSet::SystemCommand: break; + case DescriptorSet::GNSSData: + case DescriptorSet::GNSSRecv1: + case DescriptorSet::GNSSRecv2: + handle_gnss(packet); + break; } return descriptor; } @@ -145,10 +155,10 @@ AP_MicroStrain::DescriptorSet AP_MicroStrain::handle_packet(const MicroStrain_Pa void AP_MicroStrain::handle_imu(const MicroStrain_Packet& packet) { - last_ins_pkt = AP_HAL::millis(); + last_imu_pkt = AP_HAL::millis(); // Iterate through fields of varying lengths in INS packet - for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) { + for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) { switch ((INSPacketField) packet.payload[i+1]) { // Scaled Ambient Pressure case INSPacketField::PRESSURE: { @@ -183,63 +193,71 @@ void AP_MicroStrain::handle_imu(const MicroStrain_Packet& packet) void AP_MicroStrain::handle_gnss(const MicroStrain_Packet &packet) { last_gps_pkt = AP_HAL::millis(); + uint8_t gnss_instance; + const DescriptorSet descriptor = DescriptorSet(packet.descriptor_set()); + if (!get_gnss_instance(descriptor, gnss_instance)) { + return; + } // Iterate through fields of varying lengths in GNSS packet - for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) { + for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) { switch ((GNSSPacketField) packet.payload[i+1]) { - // GPS Time - case GNSSPacketField::GPS_TIME: { - gnss_data.tow_ms = double_to_uint32(be64todouble_ptr(packet.payload, i+2) * 1000); // Convert seconds to ms - gnss_data.week = be16toh_ptr(&packet.payload[i+10]); + case GNSSPacketField::GPS_TIMESTAMP: { + gnss_data[gnss_instance].tow_ms = double_to_uint32(be64todouble_ptr(packet.payload, i+2) * 1000); // Convert seconds to ms + gnss_data[gnss_instance].week = be16toh_ptr(&packet.payload[i+10]); break; } - // GNSS Fix Information case GNSSPacketField::FIX_INFO: { switch ((GNSSFixType) packet.payload[i+2]) { + case (GNSSFixType::FIX_RTK_FLOAT): { + gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_RTK_FLOAT; + break; + } + case (GNSSFixType::FIX_RTK_FIXED): { + gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_RTK_FIXED; + break; + } case (GNSSFixType::FIX_3D): { - gnss_data.fix_type = GPS_FIX_TYPE_3D_FIX; + gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_3D_FIX; break; } case (GNSSFixType::FIX_2D): { - gnss_data.fix_type = GPS_FIX_TYPE_2D_FIX; + gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_2D_FIX; break; } case (GNSSFixType::TIME_ONLY): case (GNSSFixType::NONE): { - gnss_data.fix_type = GPS_FIX_TYPE_NO_FIX; + gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_NO_FIX; break; } default: case (GNSSFixType::INVALID): { - gnss_data.fix_type = GPS_FIX_TYPE_NO_GPS; + gnss_data[gnss_instance].fix_type = GPS_FIX_TYPE_NO_GPS; break; } } - gnss_data.satellites = packet.payload[i+3]; + gnss_data[gnss_instance].satellites = packet.payload[i+3]; break; } - // LLH Position case GNSSPacketField::LLH_POSITION: { - gnss_data.lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees - gnss_data.lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7; - gnss_data.msl_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm - gnss_data.horizontal_position_accuracy = be32tofloat_ptr(packet.payload, i+34); - gnss_data.vertical_position_accuracy = be32tofloat_ptr(packet.payload, i+38); + gnss_data[gnss_instance].lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees + gnss_data[gnss_instance].lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7; + gnss_data[gnss_instance].msl_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm + gnss_data[gnss_instance].horizontal_position_accuracy = be32tofloat_ptr(packet.payload, i+34); + gnss_data[gnss_instance].vertical_position_accuracy = be32tofloat_ptr(packet.payload, i+38); break; } - // DOP Data case GNSSPacketField::DOP_DATA: { - gnss_data.hdop = be32tofloat_ptr(packet.payload, i+10); - gnss_data.vdop = be32tofloat_ptr(packet.payload, i+14); + gnss_data[gnss_instance].hdop = be32tofloat_ptr(packet.payload, i+10); + gnss_data[gnss_instance].vdop = be32tofloat_ptr(packet.payload, i+14); break; } - // NED Velocity case GNSSPacketField::NED_VELOCITY: { - gnss_data.ned_velocity_north = be32tofloat_ptr(packet.payload, i+2); - gnss_data.ned_velocity_east = be32tofloat_ptr(packet.payload, i+6); - gnss_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10); - gnss_data.speed_accuracy = be32tofloat_ptr(packet.payload, i+26); + gnss_data[gnss_instance].ned_velocity_north = be32tofloat_ptr(packet.payload, i+2); + gnss_data[gnss_instance].ned_velocity_east = be32tofloat_ptr(packet.payload, i+6); + gnss_data[gnss_instance].ned_velocity_down = be32tofloat_ptr(packet.payload, i+10); + gnss_data[gnss_instance].speed_accuracy = be32tofloat_ptr(packet.payload, i+26); break; } } @@ -251,29 +269,25 @@ void AP_MicroStrain::handle_filter(const MicroStrain_Packet &packet) last_filter_pkt = AP_HAL::millis(); // Iterate through fields of varying lengths in filter packet - for (uint8_t i = 0; i < packet.header[3]; i += packet.payload[i]) { + for (uint8_t i = 0; i < packet.payload_length(); i += packet.payload[i]) { switch ((FilterPacketField) packet.payload[i+1]) { - // GPS Timestamp - case FilterPacketField::GPS_TIME: { + case FilterPacketField::GPS_TIMESTAMP: { filter_data.tow_ms = be64todouble_ptr(packet.payload, i+2) * 1000; // Convert seconds to ms filter_data.week = be16toh_ptr(&packet.payload[i+10]); break; } - // LLH Position case FilterPacketField::LLH_POSITION: { filter_data.lat = be64todouble_ptr(packet.payload, i+2) * 1.0e7; // Decimal degrees to degrees filter_data.lon = be64todouble_ptr(packet.payload, i+10) * 1.0e7; filter_data.hae_altitude = be64todouble_ptr(packet.payload, i+26) * 1.0e2; // Meters to cm break; } - // NED Velocity case FilterPacketField::NED_VELOCITY: { filter_data.ned_velocity_north = be32tofloat_ptr(packet.payload, i+2); filter_data.ned_velocity_east = be32tofloat_ptr(packet.payload, i+6); filter_data.ned_velocity_down = be32tofloat_ptr(packet.payload, i+10); break; } - // Filter Status case FilterPacketField::FILTER_STATUS: { filter_status.state = be16toh_ptr(&packet.payload[i+2]); filter_status.mode = be16toh_ptr(&packet.payload[i+4]); @@ -303,5 +317,25 @@ Quaternion AP_MicroStrain::populate_quaternion(const uint8_t *data, uint8_t offs }; } +bool AP_MicroStrain::get_gnss_instance(const DescriptorSet& descriptor, uint8_t& instance){ + bool success = false; + + switch(descriptor) { + case DescriptorSet::GNSSData: + case DescriptorSet::GNSSRecv1: + instance = 0; + success = true; + break; + case DescriptorSet::GNSSRecv2: + instance = 1; + success = true; + break; + default: + break; + } + return success; +} + + #endif // AP_MICROSTRAIN_ENABLED \ No newline at end of file diff --git a/libraries/AP_ExternalAHRS/MicroStrain_common.h b/libraries/AP_ExternalAHRS/MicroStrain_common.h index 37158d7c70dfa4..b71027720dd9b9 100644 --- a/libraries/AP_ExternalAHRS/MicroStrain_common.h +++ b/libraries/AP_ExternalAHRS/MicroStrain_common.h @@ -40,19 +40,6 @@ class AP_MicroStrain WaitingFor_Checksum }; - // A MicroStrain packet can be a maximum of 261 bytes - struct MicroStrain_Packet { - uint8_t header[4]; - uint8_t payload[255]; - uint8_t checksum[2]; - }; - - struct { - MicroStrain_Packet packet; - AP_MicroStrain::ParseState state; - uint8_t index; - } message_in; - struct { Vector3f accel; Vector3f gyro; @@ -61,6 +48,8 @@ class AP_MicroStrain float pressure; } imu_data; + static constexpr uint8_t NUM_GNSS_INSTANCES = 2; + struct { uint16_t week; uint32_t tow_ms; @@ -77,7 +66,7 @@ class AP_MicroStrain float ned_velocity_east; float ned_velocity_down; float speed_accuracy; - } gnss_data; + } gnss_data[NUM_GNSS_INSTANCES]; struct { uint16_t state; @@ -105,13 +94,47 @@ class AP_MicroStrain SystemCommand = 0x7F, IMUData = 0x80, GNSSData = 0x81, - EstimationData = 0x82 + FilterData = 0x82, + GNSSRecv1 = 0x91, + GNSSRecv2 = 0x92 }; const uint8_t SYNC_ONE = 0x75; const uint8_t SYNC_TWO = 0x65; - uint32_t last_ins_pkt; + struct MicroStrain_Packet { + uint8_t header[4]; + uint8_t payload[255]; + uint8_t checksum[2]; + + // Gets the payload length + uint8_t payload_length() const WARN_IF_UNUSED { + return header[3]; + } + + // Sets the payload length + void payload_length(const uint8_t len) { + header[3] = len; + } + + // Gets the descriptor set + DescriptorSet descriptor_set() const WARN_IF_UNUSED { + return DescriptorSet(header[2]); + } + + // Sets the descriptor set (without validation) + void descriptor_set(const uint8_t descriptor_set) { + header[2] = descriptor_set; + } + }; + + struct { + MicroStrain_Packet packet; + AP_MicroStrain::ParseState state; + uint8_t index; + } message_in; + + uint32_t last_imu_pkt; uint32_t last_gps_pkt; uint32_t last_filter_pkt; @@ -129,6 +152,8 @@ class AP_MicroStrain void handle_filter(const MicroStrain_Packet &packet); static Vector3f populate_vector3f(const uint8_t* data, uint8_t offset); static Quaternion populate_quaternion(const uint8_t* data, uint8_t offset); + // Depending on the descriptor, the data corresponds to a different GNSS instance. + static bool get_gnss_instance(const DescriptorSet& descriptor, uint8_t& instance); }; #endif // AP_MICROSTRAIN_ENABLED 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 258e39f7dddf1e..e62ec5920827c3 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -18,6 +18,7 @@ #include "AP_Filesystem_config.h" #include #include +#include static AP_Filesystem fs; @@ -286,6 +287,37 @@ bool AP_Filesystem::fgets(char *buf, uint8_t buflen, int fd) return true; } +// run crc32 over file with given name, returns true if successful +bool AP_Filesystem::crc32(const char *fname, uint32_t& checksum) +{ + // Open file in readonly mode + int fd = open(fname, O_RDONLY); + if (fd == -1) { + return false; + } + + // Buffer to store data temporarily + const ssize_t buff_len = 64; + uint8_t buf[buff_len]; + + // Read into buffer and run crc + ssize_t read_size; + do { + read_size = read(fd, buf, buff_len); + if (read_size == -1) { + // Read error, note that we have changed the checksum value in this case + close(fd); + return false; + } + checksum = crc_crc32(checksum, buf, MIN(read_size, buff_len)); + } while (read_size > 0); + + close(fd); + + return true; +} + + #if AP_FILESYSTEM_FORMAT_ENABLED // format filesystem bool AP_Filesystem::format(void) @@ -301,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 652f53dcd92f53..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); @@ -108,6 +122,9 @@ class AP_Filesystem { // returns null-terminated string; cr or lf terminates line bool fgets(char *buf, uint8_t buflen, int fd); + // run crc32 over file with given name, returns true if successful + bool crc32(const char *fname, uint32_t& checksum) WARN_IF_UNUSED; + // format filesystem. This is async, monitor get_format_status for progress bool format(void); @@ -118,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_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..274ddf4d935c22 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -539,4 +539,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 0b984bf1df709e..b947175b0bf620 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -129,18 +129,16 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: _TYPE // @DisplayName: 1st GPS type // @Description: GPS type of 1st GPS - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA + // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna // @RebootRequired: True // @User: Advanced AP_GROUPINFO("_TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), #if GPS_MAX_RECEIVERS > 1 // @Param: _TYPE2 + // @CopyFieldsFrom: GPS_TYPE // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd GPS - // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA - // @RebootRequired: True - // @User: Advanced AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0), #endif @@ -645,6 +643,7 @@ void AP_GPS::send_blob_start(uint8_t instance) switch (_type[instance]) { #if AP_GPS_SBF_ENABLED case GPS_TYPE_SBF: + case GPS_TYPE_SBF_DUAL_ANTENNA: #endif //AP_GPS_SBF_ENABLED #if AP_GPS_GSOF_ENABLED case GPS_TYPE_GSOF: @@ -806,6 +805,7 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) #if AP_GPS_SBF_ENABLED // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: + case GPS_TYPE_SBF_DUAL_ANTENNA: return new AP_GPS_SBF(*this, state[instance], _port[instance]); #endif //AP_GPS_SBF_ENABLED #if AP_GPS_NOVA_ENABLED diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 4b184dcde0a827..eedc1a0c5a43a4 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -131,6 +131,7 @@ class AP_GPS GPS_TYPE_UAVCAN_RTK_ROVER = 23, GPS_TYPE_UNICORE_NMEA = 24, GPS_TYPE_UNICORE_MOVINGBASE_NMEA = 25, + GPS_TYPE_SBF_DUAL_ANTENNA = 26, #if HAL_SIM_GPS_ENABLED GPS_TYPE_SITL = 100, #endif diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 9de37cc4b71f48..42616a6af88459 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -22,7 +22,8 @@ // param set GPS_TYPE 11 // GSOF // param set SERIAL3_PROTOCOL 5 // GPS // - +// Pure SITL usage +// param set SIM_GPS_TYPE 11 // GSOF #define ALLOW_DOUBLE_MATH_FUNCTIONS #include "AP_GPS.h" diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 3b1d8d70095c96..569d5b6910ec4f 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -68,7 +68,10 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, // if we ever parse RTK observations it will always be of type NED, so set it once state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED; - if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { + + // yaw available when option bit set or using dual antenna + if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw) || + (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA)) { state.gps_yaw_configured = true; } } @@ -92,9 +95,9 @@ AP_GPS_SBF::read(void) ret |= parse(temp); } + const uint32_t now = AP_HAL::millis(); if (gps._auto_config != AP_GPS::GPS_AUTO_CONFIG_DISABLE) { if (config_step != Config_State::Complete) { - uint32_t now = AP_HAL::millis(); if (now > _init_blob_time) { if (now > _config_last_ack_time + 2000) { const size_t port_enable_len = strlen(_port_enable); @@ -116,9 +119,20 @@ AP_GPS_SBF::read(void) } break; case Config_State::SSO: - if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod,msec100\n", + const char *extra_config; + switch (get_type()) { + case AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA: + extra_config = "+AttCovEuler+AuxAntPositions"; + break; + case AP_GPS::GPS_Type::GPS_TYPE_SBF: + default: + extra_config = ""; + break; + } + if (asprintf(&config_string, "sso,Stream%d,COM%d,PVTGeodetic+DOP+ReceiverStatus+VelCovGeodetic+BaseVectorGeod%s,msec100\n", (int)GPS_SBF_STREAM_NUMBER, - (int)gps._com_port[state.instance]) == -1) { + (int)gps._com_port[state.instance], + extra_config) == -1) { config_string = nullptr; } break; @@ -145,6 +159,17 @@ AP_GPS_SBF::read(void) break; } break; + case Config_State::SGA: + { + const char *targetGA = "none"; + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + targetGA = "MultiAntenna"; + } + if (asprintf(&config_string, "sga, %s\n", targetGA)) { + config_string = nullptr; + } + break; + } case Config_State::Complete: // should never reach here, why search for a config if we have fully configured already INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); @@ -168,7 +193,6 @@ AP_GPS_SBF::read(void) } else if (_has_been_armed && (RxState & SBF_DISK_MOUNTED)) { // since init is done at this point and unmounting should be rate limited, // take over the _init_blob_time variable - uint32_t now = AP_HAL::millis(); if (now > _init_blob_time) { unmount_disk(); _init_blob_time = now + 1000; @@ -177,6 +201,12 @@ AP_GPS_SBF::read(void) } } + // yaw timeout after 300 milliseconds + if ((now - state.gps_yaw_time_ms) > 300) { + state.have_gps_yaw = false; + state.have_gps_yaw_accuracy = false; + } + return ret; } @@ -345,9 +375,12 @@ AP_GPS_SBF::parse(uint8_t temp) _init_blob_index++; if ((gps._sbas_mode == AP_GPS::SBAS_Mode::Disabled) ||_init_blob_index >= ARRAY_SIZE(sbas_on_blob)) { - config_step = Config_State::Complete; + config_step = Config_State::SGA; } break; + case Config_State::SGA: + config_step = Config_State::Complete; + break; case Config_State::Complete: // should never reach here, this implies that we validated a config string when we hadn't sent any INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); @@ -500,6 +533,51 @@ AP_GPS_SBF::process_message(void) } break; } + case AttEulerCov: + { + // yaw accuracy is taken from this message even though we actually calculate the yaw ourself (see AuxAntPositions below) + // this is OK based on the assumption that the calculation methods are similar and that inaccuracy arises from the sensor readings + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + const msg5939 &temp = sbf_msg.data.msg5939u; + + check_new_itow(temp.TOW, sbf_msg.length); + + constexpr double floatDNU = -2e-10f; + constexpr uint8_t errorBits = 0x8F; // Bits 0-1 are aux 1 baseline + // Bits 2-3 are aux 2 baseline + // Bit 7 is attitude not requested +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as it's false positive when testing for DNU values + if (((temp.Error & errorBits) == 0) + && (temp.Cov_HeadHead != floatDNU)) { +#pragma GCC diagnostic pop + state.gps_yaw_accuracy = sqrtf(temp.Cov_HeadHead); + state.have_gps_yaw_accuracy = true; + } else { + state.gps_yaw_accuracy = false; + } + } + break; + } + case AuxAntPositions: + { +#if GPS_MOVING_BASELINE + if (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA) { + // calculate yaw using reported antenna positions in earth-frame + // note that this calculation does not correct for the vehicle's roll and pitch meaning it is inaccurate at very high lean angles + const msg5942 &temp = sbf_msg.data.msg5942u; + check_new_itow(temp.TOW, sbf_msg.length); + if (temp.N > 0 && temp.ant1.Error == 0 && temp.ant1.AmbiguityType == 0) { + // valid RTK integer fix + const float rel_heading_deg = degrees(atan2f(temp.ant1.DeltaEast, temp.ant1.DeltaNorth)); + calculate_moving_base_yaw(rel_heading_deg, + Vector3f(temp.ant1.DeltaNorth, temp.ant1.DeltaEast, temp.ant1.DeltaUp).length(), + -temp.ant1.DeltaUp); + } + } +#endif + break; + } case BaseVectorGeod: { #pragma GCC diagnostic push @@ -542,7 +620,7 @@ AP_GPS_SBF::process_message(void) } #endif // GPS_MOVING_BASELINE - } else { + } else if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { state.rtk_baseline_y_mm = 0; state.rtk_baseline_x_mm = 0; state.rtk_baseline_z_mm = 0; diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index 2d8d47baafc500..e8773cf42cdefc 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -75,6 +75,7 @@ class AP_GPS_SBF : public AP_GPS_Backend SSO, Blob, SBAS, + SGA, Complete }; Config_State config_step; @@ -111,7 +112,9 @@ class AP_GPS_SBF : public AP_GPS_Backend PVTGeodetic = 4007, ReceiverStatus = 4014, BaseVectorGeod = 4028, - VelCovGeodetic = 5908 + VelCovGeodetic = 5908, + AttEulerCov = 5939, + AuxAntPositions = 5942, }; struct PACKED msg4007 // PVTGeodetic @@ -219,12 +222,51 @@ class AP_GPS_SBF : public AP_GPS_Backend float Cov_VuDt; }; + struct PACKED msg5939 // AttEulerCoV + { + uint32_t TOW; // receiver time stamp, 0.001s + uint16_t WNc; // receiver time stamp, 1 week + uint8_t Reserved; // unused + uint8_t Error; // error code. bit 0-1:antenna 1, bit 2-3:antenna2, bit 7: when att not requested + // 00b:no error, 01b:not enough meausurements, 10b:antennas are on one line, 11b:inconsistent with manual anntena pos info + float Cov_HeadHead; // heading estimate variance + float Cov_PitchPitch; // pitch estimate variance + float Cov_RollRoll; // roll estimate variance + float Cov_HeadPitch; // covariance between Euler angle estimates. Always set to Do-No-Use values + float Cov_HeadRoll; + float Cov_PitchRoll; + }; + + struct PACKED AuxAntPositionSubBlock { + uint8_t NrSV; // total number of satellites tracked by the antenna + uint8_t Error; // aux antenna position error code + uint8_t AmbiguityType; // aux antenna positions obtained with 0: fixed ambiguities, 1: float ambiguities + uint8_t AuxAntID; // aux antenna ID: 1 for the first auxiliary antenna, 2 for the second, etc. + double DeltaEast; // position in East direction (relative to main antenna) + double DeltaNorth; // position in North direction (relative to main antenna) + double DeltaUp; // position in Up direction (relative to main antenna) + double EastVel; // velocity in East direction (relative to main antenna) + double NorthVel; // velocity in North direction (relative to main antenna) + double UpVel; // velocity in Up direction (relative to main antenna) + }; + + struct PACKED msg5942 // AuxAntPositions + { + uint32_t TOW; + uint16_t WNc; + uint8_t N; // number of AuxAntPosition sub-blocks in this AuxAntPositions block + uint8_t SBLength; // length of one sub-block in bytes + AuxAntPositionSubBlock ant1; // first aux antennas position + }; + union PACKED msgbuffer { msg4007 msg4007u; msg4001 msg4001u; msg4014 msg4014u; msg4028 msg4028u; msg5908 msg5908u; + msg5939 msg5939u; + msg5942 msg5942u; uint8_t bytes[256]; }; diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.cpp b/libraries/AP_Generator/AP_Generator_IE_2400.cpp index d297261d5ff4fa..85ef02dee5bf20 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_2400.cpp @@ -18,6 +18,7 @@ #if AP_GENERATOR_IE_2400_ENABLED #include +#include extern const AP_HAL::HAL& hal; @@ -32,21 +33,86 @@ void AP_Generator_IE_2400::init() _frontend._has_fuel_remaining = true; } -// Update fuel cell, expected to be called at 20hz +// Assigns the unit specific measurements once a valid sentence is obtained void AP_Generator_IE_2400::assign_measurements(const uint32_t now) { - // Successfully decoded a new valid sentence + + if (_type == PacketType::V2_INFO) { + // Got info packet + if (_had_info) { + // Not expecting the version to change + return; + } + _had_info = true; + + // Info tells us the protocol version, so lock on straight away + if (_version == ProtocolVersion::DETECTING) { + if (strcmp(_info.Protocol_version, "4") == 0) { + _version = ProtocolVersion::V2; + } else { + // Got a valid info packet, but don't know this protocol version + // Give up + _version = ProtocolVersion::UNKNOWN; + } + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "IE Fuel cell detected, PCM: %s, Ver: %s, SN: %s", _info.PCM_number, _info.Software_version, _info.Serial_number); + + return; + } + + // Try and lock onto version + if (_version == ProtocolVersion::DETECTING) { + ProtocolVersion new_version = ProtocolVersion::DETECTING; + switch (_type) { + case PacketType::NONE: + // Should not get a valid packet of type none + _last_version_packet_count = 0; + return; + + case PacketType::LEGACY_DATA: + new_version = ProtocolVersion::LEGACY; + break; + + case PacketType::V2_DATA: + case PacketType::V2_INFO: + new_version = ProtocolVersion::V2; + break; + } + + if (_last_version == new_version) { + _last_version_packet_count++; + } else { + _last_version_packet_count = 0; + } + _last_version = new_version; + + // 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" ); + + } else { + // Don't record any data during version detection + return; + } + } + + if (_type == PacketType::V2_DATA) { + memcpy(&_valid_V2, &_parsed_V2, sizeof(_valid_V2)); + } + // Update internal fuel cell state _pwr_out = _parsed.pwr_out; _spm_pwr = _parsed.spm_pwr; + _battery_pwr = _parsed.battery_pwr; _state = (State)_parsed.state; + _v2_state = (V2_State)_parsed.state; _err_code = _parsed.err_code; + _sub_err_code = _parsed.sub_err_code; - // Scale tank pressure linearly to a value between 0 and 1 - // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. - const float PRESS_GRAD = 0.003389830508f; - _fuel_remaining = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1); + _fuel_remaining = _fuel_rem; // Update battery voltage _voltage = _parsed.battery_volt; @@ -55,7 +121,7 @@ void AP_Generator_IE_2400::assign_measurements(const uint32_t now) battery is charging. This is aligned with normal AP behaviour. This is the opposite of IE's convention hence *-1 */ if (_parsed.battery_volt > 0) { - _current = -1 * _parsed.battery_pwr / _parsed.battery_volt; + _current = -1 * _battery_pwr / _parsed.battery_volt; } else { _current = 0; } @@ -73,13 +139,44 @@ void AP_Generator_IE_2400::decode_latest_term() _term[_term_offset] = 0; _term_offset = 0; _term_number++; + _type = PacketType::NONE; + + if (_start_char == '<') { + decode_data_packet(); + + } else if (_start_char == '[') { + decode_info_packet(); + + } else { + _sentence_valid = false; + + } +} + +void AP_Generator_IE_2400::decode_data_packet() +{ + // Try and decode both protocol versions until locked on + if ((_version == ProtocolVersion::LEGACY) || (_version == ProtocolVersion::DETECTING)) { + decode_legacy_data(); + } + if ((_version == ProtocolVersion::V2) || (_version == ProtocolVersion::DETECTING)) { + decode_v2_data(); + } +} +void AP_Generator_IE_2400::decode_legacy_data() +{ switch (_term_number) { - case 1: + case 1: { // Float _parsed.tank_bar = strtof(_term, NULL); - break; + // Scale tank pressure linearly to a value between 0 and 1 + // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. + const float PRESS_GRAD = 0.003389830508f; + _fuel_rem = constrain_float((_parsed.tank_bar-5)*PRESS_GRAD,0,1); + break; + } case 2: // Float _parsed.battery_volt = strtof(_term, NULL); @@ -110,7 +207,80 @@ void AP_Generator_IE_2400::decode_latest_term() _parsed.err_code = strtoul(_term, nullptr, 10); // Sentence only declared valid when we have the expected number of terms _sentence_valid = true; + _type = PacketType::LEGACY_DATA; + break; + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } +} + +void AP_Generator_IE_2400::decode_v2_data() +{ + switch (_term_number) { + case 1: + _fuel_rem = strtof(_term, NULL) * 0.01; + break; + + case 2: + _parsed_V2.inlet_press = strtof(_term, NULL); + break; + + case 3: + _parsed.battery_volt = strtof(_term, NULL); + break; + + case 4: + _parsed.pwr_out = strtol(_term, nullptr, 10); + break; + + case 5: + _parsed.spm_pwr = strtoul(_term, nullptr, 10); + break; + + case 6: + _parsed_V2.unit_fault = strtoul(_term, nullptr, 10); + break; + + case 7: + _parsed.battery_pwr = strtol(_term, nullptr, 10); + break; + + case 8: + _parsed.state = strtoul(_term, nullptr, 10); + break; + + case 9: + _parsed.err_code = strtoul(_term, nullptr, 10); + break; + + case 10: + _parsed.sub_err_code = strtoul(_term, nullptr, 10); + break; + + case 11: + strncpy(_parsed_V2.info_str, _term, ARRAY_SIZE(_parsed_V2.info_str)); + break; + + case 12: { + // The inverted checksum is sent, un-invert it + uint8_t checksum = ~strtoul(_term, nullptr, 10); + + // Sent checksum only included characters up to the checksum term + // Add on the checksum terms to match our running total + for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) { + if (_term[i] == 0) { + break; + } + checksum += _term[i]; + } + + _sentence_valid = checksum == _checksum; + _type = PacketType::V2_DATA; break; + } default: // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence @@ -119,6 +289,57 @@ void AP_Generator_IE_2400::decode_latest_term() } } + +void AP_Generator_IE_2400::decode_info_packet() +{ + + switch (_term_number) { + case 1: + // PCM software number + strncpy(_info.PCM_number, _term, ARRAY_SIZE(_info.PCM_number)); + break; + + case 2: + // Software version + strncpy(_info.Software_version, _term, ARRAY_SIZE(_info.Software_version)); + break; + + case 3: + // protocol version + strncpy(_info.Protocol_version, _term, ARRAY_SIZE(_info.Protocol_version)); + break; + + case 4: + // Hardware serial number + strncpy(_info.Serial_number, _term, ARRAY_SIZE(_info.Serial_number)); + break; + + case 5: { + // The inverted checksum is sent, un-invert it + uint8_t checksum = ~strtoul(_term, nullptr, 10); + + // Sent checksum only included characters up to the checksum term + // Add on the checksum terms to match our running total + for (uint8_t i = 0; i < ARRAY_SIZE(_term); i++) { + if (_term[i] == 0) { + break; + } + checksum += _term[i]; + } + + _sentence_valid = checksum == _checksum; + _type = PacketType::V2_INFO; + break; + } + + default: + // We have received more terms than, something has gone wrong with telemetry data, mark invalid sentence + _sentence_valid = false; + break; + } + +} + // Check for failsafes AP_BattMonitor::Failsafe AP_Generator_IE_2400::update_failsafes() const { @@ -138,6 +359,12 @@ AP_BattMonitor::Failsafe AP_Generator_IE_2400::update_failsafes() const // Check for error codes that are deemed critical bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const { + // V2 protocol + if (_version == ProtocolVersion::V2) { + return err_in >= 30; + } + + // V1 protocol switch ((ErrorCode)err_in) { // Error codes that lead to critical action battery monitor failsafe case ErrorCode::BATTERY_CRITICAL: @@ -154,6 +381,12 @@ bool AP_Generator_IE_2400::is_critical_error(const uint32_t err_in) const // Check for error codes that are deemed severe and would be cause to trigger a battery monitor low failsafe action bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const { + // V2 protocol + if (_version == ProtocolVersion::V2) { + return (err_in > 20) && (err_in < 30); + } + + // V1 protocol switch ((ErrorCode)err_in) { // Error codes that lead to critical action battery monitor failsafe case ErrorCode::START_DENIED: @@ -161,7 +394,6 @@ bool AP_Generator_IE_2400::is_low_error(const uint32_t err_in) const case ErrorCode::BATTERY_LOW: case ErrorCode::PRESSURE_LOW: case ErrorCode::SPM_LOST: - case ErrorCode::REDUCED_POWER: return true; default: @@ -178,10 +410,47 @@ bool AP_Generator_IE_2400::check_for_err_code(char* msg_txt, uint8_t msg_len) co return false; } + if (_version == ProtocolVersion::V2) { + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str); + return true; + } + hal.util->snprintf(msg_txt, msg_len, "Fuel cell err code <%u>", (unsigned)_err_code); return true; } +bool AP_Generator_IE_2400::check_for_warning_code(char* msg_txt, uint8_t msg_len) const +{ + if (_err_code == 0) { + // No error nothing to do. + return false; + } + if (is_critical_error(_err_code) || is_low_error(_err_code)) { + // Critical or low error are already reported + return false; + } + + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + break; + + case ProtocolVersion::LEGACY: + if ((ErrorCode)_err_code == ErrorCode::REDUCED_POWER) { + hal.util->snprintf(msg_txt, msg_len, "Fuel cell reduced power <%u>", (unsigned)_err_code); + return true; + } + break; + + case ProtocolVersion::V2: + hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning %u.%u: %s", (unsigned)_err_code, (unsigned)_sub_err_code, _valid_V2.info_str); + return true; + } + + hal.util->snprintf(msg_txt, msg_len, "Fuel cell warning code <%u>", (unsigned)_err_code); + return true; +} + #if HAL_LOGGING_ENABLED // log generator status to the onboard log void AP_Generator_IE_2400::log_write() @@ -191,19 +460,106 @@ void AP_Generator_IE_2400::log_write() return; } - AP::logger().WriteStreaming( - "IE24", - "TimeUS,FUEL,SPMPWR,POUT,ERR", - "s%WW-", - "F2---", - "Qfiii", - AP_HAL::micros64(), - _fuel_remaining, - _spm_pwr, - _pwr_out, - _err_code - ); + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + return; + + case ProtocolVersion::LEGACY: + AP::logger().WriteStreaming( + "IE24", + "TimeUS,FUEL,SPMPWR,POUT,ERR", + "s%WW-", + "F2---", + "Qfiii", + AP_HAL::micros64(), + _fuel_remaining, + _spm_pwr, + _pwr_out, + _err_code + ); + break; + + case ProtocolVersion::V2: + AP::logger().WriteStreaming( + "IEFC", + "TimeUS,Tank,Inlet,BattV,OutPwr,SPMPwr,FNo,BPwr,State,F1,F2", + "s%-vWW-W---", + "F----------", + "QfffhHBhBII", + AP_HAL::micros64(), + _fuel_remaining, + _valid_V2.inlet_press, + _voltage, + _pwr_out, + _spm_pwr, + _valid_V2.unit_fault, + _battery_pwr, + uint8_t(_v2_state), + _err_code, + _sub_err_code + ); + break; + } + } #endif // HAL_LOGGING_ENABLED +// Return true is fuel cell is in running state suitable for arming +bool AP_Generator_IE_2400::is_running() const +{ + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + return false; + + case ProtocolVersion::LEGACY: + // Can use the base class method + return AP_Generator_IE_FuelCell::is_running(); + + case ProtocolVersion::V2: + return _v2_state == V2_State::Running; + } + + return false; +} + +// Lookup table for running state. State code is the same for all IE units. +const AP_Generator_IE_2400::Lookup_State_V2 AP_Generator_IE_2400::lookup_state_V2[] = { + { V2_State::FCPM_Off, "FCPM Off"}, + { V2_State::Starting, "Starting"}, + { V2_State::Running, "Running"}, + { V2_State::Stopping, "Stopping"}, + { V2_State::Go_to_Sleep, "Sleep"}, +}; + +// Print msg to user updating on state change +void AP_Generator_IE_2400::update_state_msg() +{ + switch (_version) { + case ProtocolVersion::DETECTING: + case ProtocolVersion::UNKNOWN: + break; + + case ProtocolVersion::LEGACY: + // Can use the base class method + AP_Generator_IE_FuelCell::update_state_msg(); + break; + + case ProtocolVersion::V2: { + // If fuel cell state has changed send gcs message + if (_v2_state != _last_v2_state) { + for (const struct Lookup_State_V2 entry : lookup_state_V2) { + if (_v2_state == entry.option) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); + break; + } + } + _last_v2_state = _v2_state; + } + break; + } + } +} + #endif // AP_GENERATOR_IE_2400_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_2400.h b/libraries/AP_Generator/AP_Generator_IE_2400.h index 65acfa8ef95028..c98238f53f7c53 100644 --- a/libraries/AP_Generator/AP_Generator_IE_2400.h +++ b/libraries/AP_Generator/AP_Generator_IE_2400.h @@ -23,9 +23,20 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell // Process characters received and extract terms for IE 2.4kW void decode_latest_term(void) override; + // Decode a data packet + void decode_data_packet(); + void decode_legacy_data(); + void decode_v2_data(); + + // Decode a info packet + void decode_info_packet(); + // Check if we have received an error code and populate message with error code bool check_for_err_code(char* msg_txt, uint8_t msg_len) const override; + // Check if we have received an warning code and populate message with warning code + bool check_for_warning_code(char* msg_txt, uint8_t msg_len) const override; + // Check for error codes that are deemed critical bool is_critical_error(const uint32_t err_in) const; @@ -36,6 +47,12 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell void log_write(void) override; #endif + // Return true is fuel cell is in running state suitable for arming + bool is_running() const override; + + // Print msg to user updating on state change + void update_state_msg() override; + // IE 2.4kW failsafes enum class ErrorCode { MINOR_INTERNAL = 1, // Minor internal error is to be ignored @@ -53,6 +70,59 @@ class AP_Generator_IE_2400 : public AP_Generator_IE_FuelCell // These measurements are only available on this unit int16_t _pwr_out; // Output power (Watts) uint16_t _spm_pwr; // Stack Power Module (SPM) power draw (Watts) + float _fuel_rem; // fuel remaining 0 to 1 + int16_t _battery_pwr; // Battery charging power + + // Extra data in the V2 packet + struct V2_data { + float inlet_press; + uint8_t unit_fault; // Unit number with issue + char info_str[33]; + }; + V2_data _parsed_V2; + V2_data _valid_V2; + + // Info packet + struct { + char PCM_number[TERM_BUFFER]; + char Software_version[TERM_BUFFER]; + char Protocol_version[TERM_BUFFER]; + char Serial_number[TERM_BUFFER]; + } _info; + bool _had_info; + + enum class ProtocolVersion { + DETECTING = 0, + LEGACY = 1, + V2 = 2, + UNKNOWN = 3, + } _version; + + ProtocolVersion _last_version; + uint8_t _last_version_packet_count; + + enum class PacketType { + NONE = 0, + LEGACY_DATA = 1, + V2_DATA = 2, + V2_INFO = 3, + } _type; + + enum class V2_State { + FCPM_Off = 0, + Starting = 1, + Running = 2, + Stopping = 3, + Go_to_Sleep = 4, + } _v2_state; + V2_State _last_v2_state; + + // State enum to string lookup + struct Lookup_State_V2 { + V2_State option; + const char *msg_txt; + }; + static const Lookup_State_V2 lookup_state_V2[]; }; #endif // AP_GENERATOR_IE_2400_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp index 8c5b770e1511f1..c0e9aae3a2a95b 100644 --- a/libraries/AP_Generator/AP_Generator_IE_650_800.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_650_800.cpp @@ -59,6 +59,11 @@ void AP_Generator_IE_650_800::decode_latest_term() _term_offset = 0; _term_number++; + if (_start_char != '<') { + _sentence_valid = false; + return; + } + switch (_term_number) { case 1: _parsed.tank_pct = strtof(_term, NULL); diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp index b66d59b8ddcd49..1385888435f76a 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.cpp @@ -77,12 +77,14 @@ void AP_Generator_IE_FuelCell::update() bool AP_Generator_IE_FuelCell::decode(char c) { // Start of a string - if (c == '<') { + if ((c == '<') || (c == '[')) { + _start_char = c; _sentence_valid = false; _data_valid = true; _term_number = 0; _term_offset = 0; _in_string = true; + _checksum = c; return false; } if (!_in_string) { @@ -90,7 +92,8 @@ bool AP_Generator_IE_FuelCell::decode(char c) } // End of a string - if (c == '>') { + const char end_char = (_start_char == '[') ? ']' : '>'; + if (c == end_char) { decode_latest_term(); _in_string = false; @@ -100,11 +103,13 @@ bool AP_Generator_IE_FuelCell::decode(char c) // End of a term in the string if (c == ',') { decode_latest_term(); + _checksum += c; return false; } // Otherwise add the char to the current term _term[_term_offset++] = c; + _checksum += c; // We have overrun the expected sentence if (_term_offset >TERM_BUFFER) { @@ -124,7 +129,7 @@ bool AP_Generator_IE_FuelCell::pre_arm_check(char *failmsg, uint8_t failmsg_len) } // Refuse arming if not in running state - if (_state != State::RUNNING) { + if (!is_running()) { strncpy(failmsg, "Status not running", failmsg_len); return false; } @@ -160,36 +165,53 @@ void AP_Generator_IE_FuelCell::check_status(const uint32_t now) } // If fuel cell state has changed send gcs message - if (_state != _last_state) { - for (const struct Lookup_State entry : lookup_state) { - if (_state == entry.option) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); - break; - } - } - _last_state = _state; - } + update_state_msg(); // Check error codes - char msg_txt[32]; - if (check_for_err_code_if_changed(msg_txt, sizeof(msg_txt))) { - GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "%s", msg_txt); - } + check_for_err_code_if_changed(); } // Check error codes and populate message with error code -bool AP_Generator_IE_FuelCell::check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len) +void AP_Generator_IE_FuelCell::check_for_err_code_if_changed() { // Only check if there has been a change in error code - if (_err_code == _last_err_code) { - return false; + if ((_err_code == _last_err_code) && (_sub_err_code == _last_sub_err_code)) { + return; } - if (check_for_err_code(msg_txt, msg_len)) { - _last_err_code = _err_code; - return true; + char msg_txt[64]; + if (check_for_err_code(msg_txt, sizeof(msg_txt)) || check_for_warning_code(msg_txt, sizeof(msg_txt))) { + GCS_SEND_TEXT(MAV_SEVERITY_ALERT, "%s", msg_txt); + + } else if ((_err_code == 0) && (_sub_err_code == 0)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fuel cell error cleared"); + } - return false; + _last_err_code = _err_code; + _last_sub_err_code = _sub_err_code; + } + +// Return true is fuel cell is in running state suitable for arming +bool AP_Generator_IE_FuelCell::is_running() const +{ + return _state == State::RUNNING; +} + +// Print msg to user updating on state change +void AP_Generator_IE_FuelCell::update_state_msg() +{ + // If fuel cell state has changed send gcs message + if (_state != _last_state) { + for (const struct Lookup_State entry : lookup_state) { + if (_state == entry.option) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Generator: %s", entry.msg_txt); + break; + } + } + _last_state = _state; + } +} + #endif // AP_GENERATOR_IE_ENABLED diff --git a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h index 84395f1125c917..8eac8952493019 100644 --- a/libraries/AP_Generator/AP_Generator_IE_FuelCell.h +++ b/libraries/AP_Generator/AP_Generator_IE_FuelCell.h @@ -49,6 +49,8 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend uint32_t _err_code; // The error code from fuel cell uint32_t _last_err_code; // The previous error code from fuel cell + uint32_t _sub_err_code; // The sub error code from fuel cell + uint32_t _last_sub_err_code; // The previous sub error code from fuel cell State _state; // The PSU state State _last_state; // The previous PSU state uint32_t _last_time_ms; // Time we got a reading @@ -66,19 +68,22 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend int16_t battery_pwr; uint8_t state; uint32_t err_code; + uint32_t sub_err_code; } _parsed; // Constants - static const uint8_t TERM_BUFFER = 12; // Max length of term we expect + static const uint8_t TERM_BUFFER = 33; // Max length of term we expect static const uint16_t HEALTHY_TIMEOUT_MS = 5000; // Time for driver to be marked un-healthy // Decoding vars + char _start_char; // inital sentence character giving sentence type char _term[TERM_BUFFER]; // Term buffer bool _sentence_valid; // Is current sentence valid bool _data_valid; // Is data within expected limits uint8_t _term_number; // Term index within the current sentence uint8_t _term_offset; // Offset within the _term buffer where the next character should be placed bool _in_string; // True if we should be decoding + uint8_t _checksum; // Basic checksum used by V2 protocol // Assigns the unit specific measurements once a valid sentence is obtained virtual void assign_measurements(const uint32_t now) = 0; @@ -100,8 +105,17 @@ class AP_Generator_IE_FuelCell : public AP_Generator_Backend // Check error codes and populate message with error code virtual bool check_for_err_code(char* msg_txt, uint8_t msg_len) const = 0; + // Check if we have received an warning code and populate message with warning code + virtual bool check_for_warning_code(char* msg_txt, uint8_t msg_len) const { return false; } + // Only check the error code if it has changed since we last checked - bool check_for_err_code_if_changed(char* msg_txt, uint8_t msg_len); + void check_for_err_code_if_changed(); + + // Return true is fuel cell is in running state suitable for arming + virtual bool is_running() const; + + // Print msg to user updating on state change + virtual void update_state_msg(); }; #endif // AP_GENERATOR_IE_ENABLED 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 7337e3dcbefd12..254895827266d3 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -42,6 +42,7 @@ #define HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR 1023 #define HAL_BOARD_SUBTYPE_LINUX_VNAV 1024 #define HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 1025 +#define HAL_BOARD_SUBTYPE_LINUX_CANZERO 1026 /* HAL CHIBIOS sub-types, starting at 5000 @@ -67,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 @@ -169,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 b3e84314ed9ef0..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; @@ -67,6 +66,7 @@ namespace AP_HAL { class SIMState; - // Must be implemented by the concrete HALs. + // Must be implemented by the concrete HALs and return the same reference. const HAL& get_HAL(); + HAL& get_HAL_mutable(); } 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..47cd3d4e79bbf8 100644 --- a/libraries/AP_HAL/SIMState.cpp +++ b/libraries/AP_HAL/SIMState.cpp @@ -230,6 +230,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 +384,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 +392,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 af62741110f2dc..50589eb85aab24 100644 --- a/libraries/AP_HAL/SIMState.h +++ b/libraries/AP_HAL/SIMState.h @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -43,7 +44,7 @@ #include #include -#include +#include #include @@ -92,7 +93,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; @@ -195,6 +196,12 @@ class AP_HAL::SIMState { // simulated MicroStrain Series 5 system SITL::MicroStrain5 *microstrain5; + // 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; @@ -213,7 +220,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.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..a7e0c61ef2f628 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; 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 5709bbeb0bc074..65ce66d2a10e40 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -286,7 +286,22 @@ #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE_LIST PROBE_MAG_SPI(LIS3MDL, lis3mdl, false, ROTATION_ROLL_180_YAW_90) #define HAL_OPTFLOW_PX4FLOW_I2C_BUS 0 - +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO + #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) + #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") + #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) + #define HAL_PROBE_EXTERNAL_I2C_COMPASSES + #define HAL_NUM_CAN_IFACES 1 + #define HAL_CAN_DRIVER_DEFAULT 1 + #define HAL_GPIO_A_LED_PIN 22 + #define HAL_GPIO_B_LED_PIN 27 + #define HAL_GPIO_C_LED_PIN 6 + #define HAL_GPIO_LED_ON 0 + #define HAL_GPIO_LED_OFF 1 + #define HAL_BOARD_LOG_DIRECTORY "/home/pi/ardupilot/logs" + #define HAL_BOARD_TERRAIN_DIRECTORY "/home/pi/ardupilot/terrain" + #define HAL_BOARD_STORAGE_DIRECTORY "/home/pi/ardupilot" + #define HAL_DEFAULT_INS_FAST_SAMPLE 0 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 //#define HAL_BARO_ALLOW_INIT_NO_BARO @@ -383,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 0052778928aa46..ccb8cb3f394d57 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,16 +96,20 @@ 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)); } /* 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; make_sockaddr(address, port, sockaddr); if (datagram && is_multicast_address(sockaddr)) { @@ -92,7 +121,6 @@ bool SocketAPM::connect(const char *address, uint16_t port) return false; } struct sockaddr_in sockaddr_mc = sockaddr; - int one = 1; struct ip_mreq mreq {}; #ifdef FD_CLOEXEC CALL_PREFIX(fcntl)(fd_in, F_SETFD, FD_CLOEXEC); @@ -109,7 +137,7 @@ bool SocketAPM::connect(const char *address, uint16_t port) ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&sockaddr_mc, sizeof(sockaddr)); if (ret == -1) { - goto fail_mc; + goto fail_multi; } mreq.imr_multiaddr.s_addr = sockaddr.sin_addr.s_addr; @@ -117,10 +145,14 @@ bool SocketAPM::connect(const char *address, uint16_t port) ret = CALL_PREFIX(setsockopt)(fd_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); if (ret == -1) { - goto fail_mc; + goto fail_multi; } - } else if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) { + } + + if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) { + // setup for bi-directional UDP broadcast set_broadcast(); + reuseaddress(); } ret = CALL_PREFIX(connect)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); @@ -128,9 +160,27 @@ bool SocketAPM::connect(const char *address, uint16_t port) return false; } connected = true; + + if (datagram && sockaddr.sin_addr.s_addr == INADDR_BROADCAST) { + // for bi-directional UDP broadcast we need 2 sockets + struct sockaddr_in send_addr; + socklen_t send_len = sizeof(send_addr); + ret = CALL_PREFIX(getsockname)(fd, (struct sockaddr *)&send_addr, &send_len); + fd_in = CALL_PREFIX(socket)(AF_INET, SOCK_DGRAM, 0); + if (fd_in == -1) { + goto fail_multi; + } + CALL_PREFIX(setsockopt)(fd_in, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + // 2nd socket needs to be bound to wildcard + send_addr.sin_addr.s_addr = INADDR_ANY; + ret = CALL_PREFIX(bind)(fd_in, (struct sockaddr *)&send_addr, sizeof(send_addr)); + if (ret == -1) { + goto fail_multi; + } + } return true; -fail_mc: +fail_multi: CALL_PREFIX(close)(fd_in); fd_in = -1; return false; @@ -139,8 +189,11 @@ bool SocketAPM::connect(const char *address, uint16_t port) /* 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); @@ -160,7 +213,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; @@ -170,11 +223,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; } @@ -185,8 +242,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); } @@ -194,8 +254,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); @@ -214,8 +277,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 @@ -226,16 +292,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)); @@ -244,16 +316,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) { @@ -265,9 +342,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; } @@ -278,14 +356,34 @@ 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); + // 16 bytes for aaa.bbb.ccc.ddd with null term + static char buf[16]; + auto *str = last_recv_address(buf, sizeof(buf), port); + ip_addr = str; } -void SocketAPM::set_broadcast(void) const +/* + return the IP address and port of the last received packet, using caller supplied buffer + */ +const char *SOCKET_CLASS_NAME::last_recv_address(char *ip_addr_buf, uint8_t buflen, uint16_t &port) const { + 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(sin.sin_port); + return ret; +} + +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)); } @@ -293,13 +391,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; @@ -315,8 +416,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; @@ -335,8 +439,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; } @@ -344,26 +451,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; } - int newfd = CALL_PREFIX(accept)(fd, nullptr, nullptr); + 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; } - // turn off nagle for lower latency - int one = 1; - CALL_PREFIX(setsockopt)(newfd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); - 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 @@ -371,4 +485,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 76ac0974c508a7..61f5ee7ecf6a3d 100644 --- a/libraries/AP_HAL/utility/Socket.h +++ b/libraries/AP_HAL/utility/Socket.h @@ -1,97 +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 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..ae4ae6cebe331c --- /dev/null +++ b/libraries/AP_HAL/utility/Socket.hpp @@ -0,0 +1,104 @@ +/* + 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 + +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/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp index 2d261f7242084c..08c532e46d1a56 100644 --- a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp @@ -85,7 +85,7 @@ #error "Unsupported MCU for FDCAN" #endif -extern AP_HAL::HAL& hal; +extern const AP_HAL::HAL& hal; #define STR(x) #x #define XSTR(x) STR(x) @@ -568,7 +568,7 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper if (can_ifaces[self_index_] == nullptr) { can_ifaces[self_index_] = this; #if !defined(HAL_BOOTLOADER_BUILD) - hal.can[self_index_] = this; + AP_HAL::get_HAL_mutable().can[self_index_] = this; #endif } @@ -812,11 +812,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 +923,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 +991,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 +1066,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; diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.h b/libraries/AP_HAL_ChibiOS/CANFDIface.h index 9b214441617207..f7ff133392bbdb 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,10 +119,8 @@ 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); @@ -224,10 +221,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 677f484804943e..f6704b6438d09d 100644 --- a/libraries/AP_HAL_ChibiOS/CanIface.cpp +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -84,7 +84,7 @@ #endif -extern AP_HAL::HAL& hal; +extern const AP_HAL::HAL& hal; using namespace ChibiOS; @@ -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; @@ -846,7 +839,7 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode) if (can_ifaces[self_index_] == nullptr) { can_ifaces[self_index_] = this; #if !defined(HAL_BOOTLOADER_BUILD) - hal.can[self_index_] = this; + AP_HAL::get_HAL_mutable().can[self_index_] = this; #endif } 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/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp index 6454f108e85ee3..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 @@ -344,8 +350,13 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const main_loop(); } +static HAL_ChibiOS hal_chibios; + const AP_HAL::HAL& AP_HAL::get_HAL() { - static const HAL_ChibiOS hal_chibios; + return hal_chibios; +} + +AP_HAL::HAL& AP_HAL::get_HAL_mutable() { return hal_chibios; } 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 @@ -78,4 +80,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/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..3f11aa6696eafa 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -76,6 +76,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); } 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. + + + + + + + + + + + + + + + + + +
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/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_Wiring_Diagram.jpg b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_Wiring_Diagram.jpg new file mode 100755 index 00000000000000..550efc89ac4af9 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_Wiring_Diagram.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_bottom.jpg b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_bottom.jpg new file mode 100755 index 00000000000000..e55abd46bec8c7 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_bottom.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_top.jpg b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_top.jpg new file mode 100755 index 00000000000000..673b031aa291c2 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/Aocoda-RC-H743Dual_top.jpg differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/README.md b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/README.md new file mode 100755 index 00000000000000..3597f1ecccb866 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Aocoda-RC-H743Dual/README.md @@ -0,0 +1,113 @@ +# Aocoda-RC-H743Dual Flight Controller + +The Aocoda-RC-H743Dual is a flight controller produced by [Aocoda-RC](https://www.aocoda-rc.com/). + +## Features + +- MCU：STM32H743VIH6 +- Gyro：MPU6000/BIM270x2 +- Baro：DPS310/MS56XX/BMP280 +- Blackbox：128MB +- PWM output：10CH +- Servo：2CH +- UART：8CH +- Power Supply：3-6SLipo +- BEC Output：5V/2.5A, 9V/3A +- USB Connector: Type-C +- Weight：8.8g +- Size：37mm x 37mm +- Mounting Hole：30.5mm x 30.5mm + +## Pinout + + +![Aocoda-RC-H743Dual Top](Aocoda-RC-H743Dual_top.jpg "Aocoda-RC-H743Dual Top") +![Aocoda-RC-H743Dual Bottom](Aocoda-RC-H743Dual_bottom.jpg "Aocoda-RC-H743Dual Bottom") +![Aocoda-RC-H743Dual Wiring](Aocoda-RC-H743Dual_Wiring_Diagram.jpg "Aocoda-RC-H743Dual Wiring") + + +## UART Mapping + +The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the receive pin for UARTn. The Tn pin is the transmit pin for UARTn. + + - SERIAL0 -> USB (primary mavlink, usually USB) + - SERIAL1 -> UART1 (RC input) + - SERIAL2 -> UART2 (GPS) + - SERIAL3 -> UART3 (VTX) + - SERIAL4 -> UART4 + - SERIAL5 -> not available + - SERIAL6 -> UART6 (ESC Telemetry) + - SERIAL7 -> UART7 + - SERIAL8 -> UART8 + +## RC Input + +RC input is configured on SERIAL1 (USART1), which is available on the Rx1, Tx1. PPM receivers are *not* supported as this input does not have a timer resource available. + +*Note* It is recommend to use CRSF/ELRS. + +With recommended option: + +- Set SERIAL1_PROTOCOL" +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
+]] +} + +--[[ + 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 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_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat index 1419ab5aa2e053..a67d7f9ab062f5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Pixhawk6X/hwdef.dat @@ -379,4 +379,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/RADIX2HD/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/hwdef.dat index 356570353543c0..3490c49019837f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/RADIX2HD/hwdef.dat @@ -165,6 +165,7 @@ BARO DPS310 I2C:0:0x76 # probe external compasses (same bus as internal) define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_I2C_INTERNAL_MASK 0 # filesystem setup on sdcard define HAL_OS_FATFS_IO 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SDMODELH7V1/README.md b/libraries/AP_HAL_ChibiOS/hwdef/SDMODELH7V1/README.md index 41e56176f5b75c..87d7a568536d48 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SDMODELH7V1/README.md +++ b/libraries/AP_HAL_ChibiOS/hwdef/SDMODELH7V1/README.md @@ -139,7 +139,9 @@ FrSky Telemetry is supported using the Tx pin of any UART. You need to set the f The SDH7V1 supports OSD using OSD_TYPE 1 (MAX7456 driver). The defaults are also setup to allow DJI Goggle OSD support on UART1. PWM Output¶ -The KakuteH7 supports up to 8 PWM outputs. Outputs are available via two JST-SH connectors. All 8 outputs support DShot and bi-directional DShot, as well as all PWM types. +## PWM Outputs + +The SDH7V1 supports up to 8 PWM outputs. Outputs are available via two JST-SH connectors. All 8 outputs support DShot and bi-directional DShot, as well as all PWM types. The PWM is in 3 groups: diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat index 1ddcd7c9eb8aa9..f6052add8b9772 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat @@ -169,8 +169,6 @@ 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 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/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/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/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/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 5b8b2aa6f55ef4..00000000000000 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/lwipopts.h +++ /dev/null @@ -1,2149 +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 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/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h index 0a56342d256fdb..5a24edd59fe97c 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; 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/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..7f248d2d358efe 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -25,12 +25,13 @@ 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 self.default_gpio = ['INPUT', 'FLOATING'] @@ -111,9 +112,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 +121,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: @@ -735,7 +736,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) @@ -756,7 +757,7 @@ def get_ram_map(self): '''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(): 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) @@ -771,7 +772,7 @@ def get_ram_map(self): 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) @@ -854,7 +855,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 @@ -903,6 +904,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 +974,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 +1029,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 +1047,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 +1056,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 +1074,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 +1085,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( @@ -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(''' @@ -1283,10 +1286,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 +1367,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,7 +1376,7 @@ 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 @@ -1386,7 +1389,7 @@ def write_ldscript(self, fname): ram_reserve_start = self.get_ram_reserve_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 */ \\ @@ -2441,8 +2433,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) @@ -2528,7 +2522,7 @@ def write_all_lines(self, hwdat): 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') @@ -2601,10 +2595,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 +2716,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 +2730,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 +2781,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,13 +2820,13 @@ 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") + self.progress("No default parameter file found") return content = self.get_processed_defaults_file(defaults_abspath) @@ -2860,7 +2858,7 @@ def romfs_add_dir(self, subdirs): '''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 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, '*~'): @@ -3000,7 +2998,7 @@ def process_line(self, line): self.romfs_wildcard(a[1]) 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,7 +3030,7 @@ 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] @@ -3042,6 +3040,17 @@ def process_line(self, line): 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 +3070,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 +3084,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 +3094,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) @@ -3142,7 +3151,7 @@ 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() @@ -3165,7 +3174,7 @@ 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: + if not self.is_bootloader_fw(): 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 +3193,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 +3203,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_betaflight_unified.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/convert_betaflight_unified.py index 0a360657503f84..1e8f9a53f964f9 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/convert_betaflight_unified.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/convert_betaflight_unified.py @@ -192,6 +192,9 @@ def convert_file(fname, board_id): line = lines[i] if line.startswith('resource'): a = line.split() + + if a[3] == 'NONE': + continue # function, number, pin pin = convert_pin(a[3]) resource = [ a[2] , pin, a[1].split('_')[0], a[1] ] 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_iofirmware.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h index 7cd350f397347f..269610f5543523 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_iofirmware.h @@ -41,3 +41,16 @@ #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..e0b0f894a666a9 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h @@ -335,7 +335,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..96a3673148afe9 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py @@ -293,7 +293,7 @@ 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 = [] @@ -331,7 +331,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/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"); + #if CH_CFG_ST_RESOLUTION == 16 static_assert(sizeof(systime_t) == 2, "expected 16 bit systime_t"); #elif CH_CFG_ST_RESOLUTION == 32 @@ -385,7 +388,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 20f07a014758b2..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,13 +52,19 @@ uint64_t micros64() uint64_t millis64() { - return micros64()/1000; + return uint64_div1000(micros64()); } } // namespace AP_HAL +static HAL_ESP32 hal_esp32; + const AP_HAL::HAL& AP_HAL::get_HAL() { - static const HAL_ESP32 hal; - return hal; + return hal_esp32; +} + +AP_HAL::HAL& AP_HAL::get_HAL_mutable() +{ + return hal_esp32; } 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 b2271a7df8eb89..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, @@ -66,9 +67,14 @@ void HAL_Empty::run(int argc, char* const argv[], Callbacks* callbacks) const } } +static HAL_Empty hal_empty; + const AP_HAL::HAL& AP_HAL::get_HAL() { - static const HAL_Empty hal; - return hal; + return hal_empty; +} + +AP_HAL::HAL& AP_HAL::get_HAL_mutable() { + return hal_empty; } #endif 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/GPIO.h b/libraries/AP_HAL_Linux/GPIO.h index 0281c14be6ede4..4b8ec5fa22c1a6 100644 --- a/libraries/AP_HAL_Linux/GPIO.h +++ b/libraries/AP_HAL_Linux/GPIO.h @@ -28,7 +28,8 @@ class DigitalSource : public AP_HAL::DigitalSource { CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO #include "GPIO_RPI.h" #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR #include "GPIO_Navigator.h" diff --git a/libraries/AP_HAL_Linux/GPIO_RPI.cpp b/libraries/AP_HAL_Linux/GPIO_RPI.cpp index 58cf04d0f34041..974e6523e4a61b 100644 --- a/libraries/AP_HAL_Linux/GPIO_RPI.cpp +++ b/libraries/AP_HAL_Linux/GPIO_RPI.cpp @@ -5,7 +5,8 @@ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO #include #include diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index ba68930e89a786..4b2cd90e28fa68 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -57,22 +57,24 @@ using namespace Linux; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO static UtilRPI utilInstance; #else 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; @@ -80,17 +82,31 @@ 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 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR|| \ - ((CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1) && (OBAL_ALLOW_ADC ==1)) + ((CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1) && (OBAL_ALLOW_ADC ==1)) || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO static AnalogIn_ADS1115 analogIn; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD || \ @@ -123,7 +139,8 @@ static GPIO_BBB gpioDriver; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO static GPIO_RPI gpioDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR static GPIO_Navigator gpioDriver; @@ -154,7 +171,8 @@ static RCInput_Multi rcinDriver{2, new RCInput_AioPRU, new RCInput_RCProtocol(NU CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO static RCInput_RPI rcinDriver; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ @@ -219,6 +237,8 @@ static RCOutput_Sysfs rcoutDriver(0, 0, 15); static RCOutput_Sysfs rcoutDriver(0, 0, 8); #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 static RCOutput_PCA9685 rcoutDriver(i2c_mgr_instance.get_device(1, PCA9685_PRIMARY_ADDRESS), 0, 0, RPI_GPIO_<17>()); +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO +static RCOutput_Sysfs rcoutDriver(0, 0, 2); #else static Empty::RCOutput rcoutDriver; #endif @@ -231,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; @@ -241,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, @@ -264,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 @@ -275,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"); @@ -324,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'}, @@ -365,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); @@ -491,3 +530,8 @@ const AP_HAL::HAL &AP_HAL::get_HAL() { return hal_linux; } + +AP_HAL::HAL &AP_HAL::get_HAL_mutable() +{ + return hal_linux; +} diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp index d53ce57ea7d625..c2d24f3c29c738 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -5,7 +5,8 @@ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO #include #include #include 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/SPIDevice.cpp b/libraries/AP_HAL_Linux/SPIDevice.cpp index 4d3e922561e236..a23ab1ca7d174f 100644 --- a/libraries/AP_HAL_Linux/SPIDevice.cpp +++ b/libraries/AP_HAL_Linux/SPIDevice.cpp @@ -97,6 +97,11 @@ SPIDesc SPIDeviceManager::_device[] = { SPIDesc("mpu9250", 0, 1, SPI_MODE_0, 8, SPI_CS_KERNEL, 1*MHZ, 11*MHZ), SPIDesc("ms5611", 0, 0, SPI_MODE_0, 8, SPI_CS_KERNEL, 1*KHZ, 10*MHZ), }; +#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO +SPIDesc SPIDeviceManager::_device[] = { + SPIDesc("mpu9250", 0, 1, SPI_MODE_0, 8, SPI_CS_KERNEL, 1*MHZ, 11*MHZ), + SPIDesc("ms5611", 0, 0, SPI_MODE_0, 8, SPI_CS_KERNEL, 10*MHZ, 10*MHZ), +}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI SPIDesc SPIDeviceManager::_device[] = { SPIDesc("mpu9250", 2, 0, SPI_MODE_3, 8, SPI_CS_KERNEL, 1*MHZ, 11*MHZ), diff --git a/libraries/AP_HAL_Linux/Scheduler.cpp b/libraries/AP_HAL_Linux/Scheduler.cpp index b8a8f48706a549..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 @@ -37,7 +38,8 @@ extern const AP_HAL::HAL& hal; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO #define APM_LINUX_RCIN_RATE 500 #define APM_LINUX_IO_RATE 50 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 @@ -384,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/Util_RPI.cpp b/libraries/AP_HAL_Linux/Util_RPI.cpp index 4575a7dccc828d..9f464374d7c191 100644 --- a/libraries/AP_HAL_Linux/Util_RPI.cpp +++ b/libraries/AP_HAL_Linux/Util_RPI.cpp @@ -8,7 +8,8 @@ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO #include #include 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 8a7537ab9730f7..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" @@ -33,7 +36,7 @@ using namespace HALSITL; -HAL_SITL& hal_sitl = (HAL_SITL&)AP_HAL::get_HAL(); +HAL_SITL& hal_sitl = (HAL_SITL&)AP_HAL::get_HAL_mutable(); static Storage sitlStorage; static SITL_State sitlState; @@ -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 @@ -299,9 +306,14 @@ void HAL_SITL::actually_reboot() AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno)); } +static HAL_SITL hal_sitl_inst; + const AP_HAL::HAL& AP_HAL::get_HAL() { - static const HAL_SITL hal; - return hal; + return hal_sitl_inst; +} + +AP_HAL::HAL& AP_HAL::get_HAL_mutable() { + return hal_sitl_inst; } #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL 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..02810a9ac3b433 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; } } 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 260a9b28dac6f9..ae2603e6dd38ea 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; @@ -269,6 +269,21 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const } microstrain5 = new SITL::MicroStrain5(); return microstrain5; + + } else if (streq(name, "MicroStrain7")) { + if (microstrain7 != nullptr) { + AP_HAL::panic("Only one MicroStrain7 at a time"); + } + 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) { @@ -435,6 +450,13 @@ void SITL_State_Common::sim_update(void) microstrain5->update(); } + if (microstrain7 != nullptr) { + microstrain7->update(); + } + if (inertiallabs != nullptr) { + inertiallabs->update(); + } + #if HAL_SIM_AIS_ENABLED if (ais != nullptr) { ais->update(); diff --git a/libraries/AP_HAL_SITL/SITL_State_common.h b/libraries/AP_HAL_SITL/SITL_State_common.h index e7b82c2c91d4aa..4b59268ef8922b 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 @@ -55,10 +57,6 @@ #include "RCInput.h" #include -#include -#include -#include -#include #include #include @@ -67,7 +65,6 @@ #include #include #include -#include class HAL_SITL; @@ -197,9 +194,15 @@ class HALSITL::SITL_State_Common { // simulated VectorNav system: SITL::VectorNav *vectornav; - // simulated LORD MicroStrain system + // simulated MicroStrain system SITL::MicroStrain5 *microstrain5; + // 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; @@ -217,7 +220,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..93fed42129d0b5 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -377,6 +377,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..1e4a59be2019ec 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; @@ -179,18 +180,7 @@ uint64_t micros64() 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_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..4768ec6e2fb925 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 */ @@ -597,7 +682,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 +691,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 +761,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 +883,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 +932,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 +971,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 +1085,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..2b1e65d145a1ab 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, @@ -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_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 9e0aa38a57af95..436a4d6fbd48e3 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -28,17 +28,21 @@ void AP_InertialNav::update(bool high_vibes) // get the velocity relative to the local earth frame Vector3f velNED; - if (_ahrs_ekf.get_velocity_NED(velNED)) { - // during high vibration events use vertical position change - if (high_vibes) { - float rate_z; - if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) { - velNED.z = rate_z; - } - } + + const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED); + if (velned_ok) { _velocity_cm = velNED * 100; // convert to cm/s _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU } + // During high vibration events, or failure of get_velocity_NED, use the + // fallback vertical velocity estimate. For get_velocity_NED failure, freeze + // the horizontal velocity at the last good value. + if (!velned_ok || high_vibes) { + float rate_z; + if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) { + _velocity_cm.z = -rate_z * 100; // convert from m/s in NED to cm/s in NEU + } + } } /** diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 49081c29e45bd0..4d4480e43f3501 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -986,7 +986,9 @@ AP_InertialSensor::init(uint16_t loop_rate) if (!notch.params.enabled() && !fft_enabled) { continue; } - notch.calculated_notch_freq_hz[0] = notch.params.center_freq_hz(); + for (uint8_t i = 0; i < ARRAY_SIZE(notch.calculated_notch_freq_hz); i++) { + notch.calculated_notch_freq_hz[i] = notch.params.center_freq_hz(); + } notch.num_calculated_notch_frequencies = 1; notch.num_dynamic_notches = 1; #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) @@ -1338,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())) { @@ -1880,6 +1882,11 @@ void AP_InertialSensor::update(void) GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL finished all IMUs"); } #endif +#if AP_SERIALMANAGER_IMUOUT_ENABLED + if (uart.imu_out_uart) { + send_uart_data(); + } +#endif } /* @@ -2612,6 +2619,50 @@ void AP_InertialSensor::kill_imu(uint8_t imu_idx, bool kill_it) } #endif // AP_INERTIALSENSOR_KILL_IMU_ENABLED +#if AP_SERIALMANAGER_IMUOUT_ENABLED +/* + setup a UART for sending external data + */ +void AP_InertialSensor::set_imu_out_uart(AP_HAL::UARTDriver *_uart) +{ + uart.imu_out_uart = _uart; + uart.counter = 0; +} + +/* + send IMU delta-angle and delta-velocity to a UART + */ +void AP_InertialSensor::send_uart_data(void) +{ + struct { + uint16_t magic = 0x29c4; + uint16_t length; + uint32_t timestamp_us; + Vector3f delta_velocity; + Vector3f delta_angle; + float delta_velocity_dt; + float delta_angle_dt; + uint16_t counter; + uint16_t crc; + } data; + + if (uart.imu_out_uart->txspace() < sizeof(data)) { + // not enough space + return; + } + + data.length = sizeof(data); + data.timestamp_us = AP_HAL::micros(); + + get_delta_angle(get_primary_gyro(), data.delta_angle, data.delta_angle_dt); + get_delta_velocity(get_primary_accel(), data.delta_velocity, data.delta_velocity_dt); + + data.counter = uart.counter++; + data.crc = crc_xmodem((const uint8_t *)&data, sizeof(data)-sizeof(uint16_t)); + + uart.imu_out_uart->write((const uint8_t *)&data, sizeof(data)); +} +#endif // AP_SERIALMANAGER_IMUOUT_ENABLED #if HAL_EXTERNAL_AHRS_ENABLED void AP_InertialSensor::handle_external(const AP_ExternalAHRS::ins_data_message_t &pkt) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 6ab9511cffc3b2..fd085cf7d3bb4d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -19,6 +19,7 @@ #include #include #include +#include #include "AP_InertialSensor_Params.h" #include "AP_InertialSensor_tempcal.h" @@ -302,6 +303,17 @@ class AP_InertialSensor : AP_AccelCal_Client // for killing an IMU for testing purposes void kill_imu(uint8_t imu_idx, bool kill_it); +#if AP_SERIALMANAGER_IMUOUT_ENABLED + // optional UART for sending IMU data to an external process + void set_imu_out_uart(AP_HAL::UARTDriver *uart); + void send_uart_data(void); + + struct { + uint16_t counter; + AP_HAL::UARTDriver *imu_out_uart; + } uart; +#endif // AP_SERIALMANAGER_IMUOUT_ENABLED + enum IMU_SENSOR_TYPE { IMU_SENSOR_TYPE_ACCEL = 0, IMU_SENSOR_TYPE_GYRO = 1, 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_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_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index e3947d40f680da..211ecdd46f1da3 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -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 */ diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index b9cfc80e888a9e..5022412d9552df 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -91,8 +91,6 @@ 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 diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index 550fa92ecc6c3d..0a690acbeb631d 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; } @@ -533,17 +533,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 +590,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 +620,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 +644,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 +652,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..4ec0c7cfa7c15d 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 } @@ -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 @@ -368,7 +368,7 @@ 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; diff --git a/libraries/AP_LandingGear/AP_LandingGear.cpp b/libraries/AP_LandingGear/AP_LandingGear.cpp index b7a7bc3ebb45bd..8f77d5e3df7501 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 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_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index ee01834cb19d6f..2f4fe9c63bb188 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -1261,6 +1261,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 +1271,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/benchmarks/benchmark_geodesic_grid.cpp b/libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp index 71da640b0a74af..684a9ec8de4b7e 100644 --- a/libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp +++ b/libraries/AP_Math/benchmarks/benchmark_geodesic_grid.cpp @@ -86,7 +86,7 @@ static bool section_triangle(unsigned int section_index, static void BM_GeodesicGridSections(benchmark::State& state) { Vector3f v, a, b, c; - int section = state.range_x(); + int section = state.range(0); section_triangle(section, a, b, c); v = (a + b + c) / 3.0f; diff --git a/libraries/AP_Math/crc.cpp b/libraries/AP_Math/crc.cpp index 0b283b05153571..de81543b485ccd 100644 --- a/libraries/AP_Math/crc.cpp +++ b/libraries/AP_Math/crc.cpp @@ -481,7 +481,7 @@ uint32_t crc_crc24(const uint8_t *bytes, uint16_t len) } // simple 8 bit checksum used by FPort -uint8_t crc_sum8(const uint8_t *p, uint8_t len) +uint8_t crc_sum8_with_carry(const uint8_t *p, uint8_t len) { uint16_t sum = 0; for (uint8_t i=0; i @@ -64,4 +66,7 @@ uint8_t parity(uint8_t byte); // 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); +uint8_t crc_sum_of_bytes(const 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); diff --git a/libraries/AP_Math/definitions.h b/libraries/AP_Math/definitions.h index 024838a1c62c94..7317a19906e0b4 100644 --- a/libraries/AP_Math/definitions.h +++ b/libraries/AP_Math/definitions.h @@ -95,6 +95,9 @@ static const double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); #define INCH_OF_H2O_TO_PASCAL 248.84f +#define UTESLA_TO_MGAUSS 10.0f // uT to mGauss conversion +#define NTESLA_TO_MGAUSS 0.01f // nT to mGauss conversion + /* use AP_ prefix to prevent conflict with OS headers, such as NuttX clock.h diff --git a/libraries/AP_Math/div1000.h b/libraries/AP_Math/div1000.h new file mode 100644 index 00000000000000..e80a52d542180d --- /dev/null +++ b/libraries/AP_Math/div1000.h @@ -0,0 +1,26 @@ +/* + return 64 bit x / 1000 + faster than the normal gcc implementation using by about 3x + With thanks to https://0x414b.com/2021/04/16/arm-division.html + and https://stackoverflow.com/questions/74765410/multiply-two-uint64-ts-and-store-result-to-uint64-t-doesnt-seem-to-work +*/ +static inline uint64_t uint64_div1000(uint64_t x) +{ + x >>= 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..c8fabcdd8c7db1 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); @@ -825,6 +832,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 +860,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 @@ -923,6 +934,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; @@ -1483,6 +1496,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 @@ -2714,6 +2729,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 +2740,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 +2917,5 @@ AP_Mission *mission() } } + +#endif // AP_MISSION_ENABLED 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_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 1c56e07367f6cc..46672faddb3004 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -50,12 +50,6 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli { // calculate_armed_scalars - recalculates scalars that can change while armed void calculate_armed_scalars() override; - // has_flybar - returns true if we have a mechical flybar - bool has_flybar() const override { return AP_MOTORS_HELI_NOFLYBAR; } - - // supports_yaw_passthrought - returns true if we support yaw passthrough - bool supports_yaw_passthrough() const override { return false; } - // servo_test - move servos through full range of movement void servo_test() override; @@ -71,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 3a3292fa253407..a67cb409116eb5 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.h @@ -37,12 +37,6 @@ class AP_MotorsHeli_Quad : public AP_MotorsHeli { // calculate_armed_scalars - recalculates scalars that can change while armed void calculate_armed_scalars() override; - // has_flybar - returns true if we have a mechanical flybar - bool has_flybar() const override { return AP_MOTORS_HELI_NOFLYBAR; } - - // supports_yaw_passthrought - returns true if we support yaw passthrough - bool supports_yaw_passthrough() const override { return false; } - // servo_test - move servos through full range of movement void servo_test() override; @@ -55,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/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..4bb18ad3608b58 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[] = { diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 8545d75b90bf23..e9760c7ae08e9d 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; } @@ -405,7 +413,7 @@ bool AP_Mount_Backend::handle_global_position_int(uint8_t msg_sysid, const mavli 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; } @@ -570,6 +578,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 +734,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 +750,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..6dbf8cdbd3b260 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 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_NMEA_Output/AP_NMEA_Output.cpp b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp index bc7d0ff2ef898d..6e2ab5c9d20fdc 100644 --- a/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp +++ b/libraries/AP_NMEA_Output/AP_NMEA_Output.cpp @@ -111,8 +111,8 @@ void AP_NMEA_Output::update() struct tm* tm = gmtime(&time_sec); // format time string - char tstring[11]; - snprintf(tstring, sizeof(tstring), "%02u%02u%06.2f", tm->tm_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_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index 7783adf77dac4e..303b50ab9861b6 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -7,15 +7,24 @@ #include "AP_Networking_Backend.h" #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 @@ -31,6 +40,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { // @User: Advanced AP_GROUPINFO_FLAGS("ENABLED", 1, AP_Networking, param.enabled, 0, AP_PARAM_FLAG_ENABLE), +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED // @Group: IPADDR // @Path: AP_Networking_address.cpp AP_SUBGROUPINFO(param.ipaddr, "IPADDR", 2, AP_Networking, AP_Networking_IPV4), @@ -43,6 +53,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { // @User: Advanced AP_GROUPINFO("NETMASK", 3, AP_Networking, param.netmask, AP_NETWORKING_DEFAULT_NETMASK), +#if AP_NETWORKING_DHCP_AVAILABLE // @Param: DHCP // @DisplayName: DHCP client // @Description: Enable/Disable DHCP client @@ -50,6 +61,7 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { // @RebootRequired: True // @User: Advanced AP_GROUPINFO("DHCP", 4, AP_Networking, param.dhcp, AP_NETWORKING_DEFAULT_DHCP_ENABLE), +#endif // @Group: GWADDR // @Path: AP_Networking_address.cpp @@ -58,12 +70,13 @@ const AP_Param::GroupInfo AP_Networking::var_info[] = { // @Group: MACADDR // @Path: AP_Networking_macaddr.cpp AP_SUBGROUPINFO(param.macaddr, "MACADDR", 6, AP_Networking, AP_Networking_MAC), +#endif // AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED #if AP_NETWORKING_TESTS_ENABLED // @Param: TESTS // @DisplayName: Test enable flags // @Description: Enable/Disable networking tests - // @Bitmask: 0:UDP echo test,1:TCP echo test + // @Bitmask: 0:UDP echo test,1:TCP echo test, 2:TCP discard test // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TESTS", 7, AP_Networking, param.tests, 0), @@ -73,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 }; @@ -99,6 +126,7 @@ void AP_Networking::init() return; } +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED // set default MAC Address as lower 3 bytes of the CRC of the UID uint8_t uid[50]; uint8_t uid_len = sizeof(uid); @@ -113,12 +141,34 @@ void AP_Networking::init() param.macaddr.set_default_address_byte(4, crc.bytes[1]); param.macaddr.set_default_address_byte(5, crc.bytes[2]); } +#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) { @@ -133,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"); @@ -150,16 +207,17 @@ void AP_Networking::init() */ void AP_Networking::announce_address_changes() { - auto &as = backend->activeSettings; + const auto &as = backend->activeSettings; if (as.last_change_ms == 0 || as.last_change_ms == announce_ms) { // nothing changed and we've already printed it at least once. Nothing to do. 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()); + 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))); announce_ms = as.last_change_ms; } @@ -197,21 +255,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 */ @@ -256,6 +299,112 @@ uint32_t AP_Networking::get_gateway_active() const return backend?backend->activeSettings.gw:0; } +/* + wait for networking to be active + */ +void AP_Networking::startup_wait(void) const +{ + if (hal.scheduler->in_main_thread()) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + while (!hal.scheduler->is_system_initialized()) { + hal.scheduler->delay(100); + } +#if AP_NETWORKING_BACKEND_CHIBIOS + do { + hal.scheduler->delay(250); + } while (get_ip_active() == 0); +#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 @@ -266,4 +415,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 c931fc26982461..f5db359962fddd 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 { @@ -54,13 +60,22 @@ class AP_Networking // returns true if DHCP is enabled bool get_dhcp_enabled() const { +#if AP_NETWORKING_DHCP_AVAILABLE return param.dhcp; +#else + // DHCP is not available from our scope but could be enabled/controlled + // by the OS which is the case on Linux builds, including SITL + // TODO: ask the OS if DHCP is enabled + return false; +#endif } // Sets DHCP to be enabled or disabled void set_dhcp_enable(const bool enable) { +#if AP_NETWORKING_DHCP_AVAILABLE param.dhcp.set(enable); +#endif } // returns the 32bit value of the active IP address that is currently in use @@ -69,22 +84,20 @@ class AP_Networking // returns the 32bit value of the user-parameter static IP address uint32_t get_ip_param() const { +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED return param.ipaddr.get_uint32(); - } - - /* - 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()); +#else + // TODO: ask the OS for the IP address + return 0; +#endif } // sets the user-parameter static IP address from a 32bit value void set_ip_param(const uint32_t ip) { +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED param.ipaddr.set_uint32(ip); +#endif } // returns the 32bit value of the active Netmask that is currently in use @@ -93,97 +106,103 @@ class AP_Networking // returns the 32bit value of the of the user-parameter static Netmask uint32_t get_netmask_param() const { +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED return convert_netmask_bitcount_to_ip(param.netmask.get()); - } - - // 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) - { - param.netmask.set(convert_netmask_ip_to_bitcount(nm)); +#else + // TODO: ask the OS for the Netmask + return 0; +#endif } uint32_t get_gateway_active() const; uint32_t get_gateway_param() const { +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED return param.gwaddr.get_uint32(); - } - - 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)); +#else + // TODO: ask the OS for the Gateway + return 0; +#endif } void set_gateway_param(const uint32_t gw) { +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED param.gwaddr.set_uint32(gw); +#endif } - // 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); + // wait in a thread for network startup + void startup_wait(void) const; // 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; void announce_address_changes(); struct { +#if AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED AP_Networking_IPV4 ipaddr{AP_NETWORKING_DEFAULT_STATIC_IP_ADDR}; AP_Int8 netmask; // bits to mask. example: (16 == 255.255.0.0) and (24 == 255.255.255.0) AP_Networking_IPV4 gwaddr{AP_NETWORKING_DEFAULT_STATIC_GW_ADDR}; - - AP_Int8 dhcp; AP_Networking_MAC macaddr{AP_NETWORKING_DEFAULT_MAC_ADDR}; +#if AP_NETWORKING_DHCP_AVAILABLE + AP_Int8 dhcp; +#endif +#endif + AP_Int8 enabled; AP_Int32 options; + #if AP_NETWORKING_TESTS_ENABLED 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 { NONE = 0, UDP_CLIENT = 1, + UDP_SERVER = 2, + TCP_CLIENT = 3, + TCP_SERVER = 4, }; // class for NET_Pn_* parameters @@ -199,6 +218,7 @@ class AP_Networking AP_Networking_IPV4 ip {"0.0.0.0"}; AP_Int32 port; SocketAPM *sock; + SocketAPM *listen_sock; bool is_initialized() override { return true; @@ -207,11 +227,21 @@ class AP_Networking return false; } - void udp_client_init(const uint32_t size_rx, const uint32_t size_tx); + void udp_client_init(void); + void udp_server_init(void); + void tcp_server_init(void); + void tcp_client_init(void); + void udp_client_loop(void); + void udp_server_loop(void); + void tcp_client_loop(void); + void tcp_server_loop(void); + + bool send_receive(void); private: bool init_buffers(const uint32_t size_rx, const uint32_t size_tx); + void thread_create(AP_HAL::MemberProc); uint32_t txspace() override; void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; @@ -222,10 +252,22 @@ class AP_Networking void _flush() override {} bool _discard_input() override; + enum flow_control get_flow_control(void) override; + + uint32_t bw_in_bytes_per_second() const override { + return 1000000UL; + } + ByteBuffer *readbuffer; ByteBuffer *writebuffer; + char thread_name[10]; uint32_t last_size_tx; uint32_t last_size_rx; + bool packetise; + bool connected; + bool have_received; + bool close_on_recv_error; + HAL_Semaphore sem; }; @@ -236,15 +278,29 @@ class AP_Networking enum { TEST_UDP_CLIENT = (1U<<0), TEST_TCP_CLIENT = (1U<<1), + TEST_TCP_DISCARD = (1U<<2), }; void start_tests(void); void test_UDP_client(void); void test_TCP_client(void); + void test_TCP_discard(void); #endif // AP_NETWORKING_TESTS_ENABLED // ports for registration with serial manager Port ports[AP_NETWORKING_NUM_PORTS]; + // 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); }; @@ -253,4 +309,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 442efdc454a0ac..3b330425164969 100644 --- a/libraries/AP_Networking/AP_Networking_ChibiOS.cpp +++ b/libraries/AP_Networking/AP_Networking_ChibiOS.cpp @@ -6,9 +6,16 @@ #include "AP_Networking_ChibiOS.h" #include -#include #include #include +#include +#include +#if LWIP_DHCP +#include +#endif +#include +#include "../../modules/ChibiOS/os/various/evtimer.h" +#include extern const AP_HAL::HAL& hal; @@ -18,26 +25,33 @@ 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 = 2; + uint32_t size = 1; uint8_t rasr = 0; // find size closest to power of 2 while (size < total_size) { @@ -84,7 +98,7 @@ bool AP_Networking_ChibiOS::allocate_buffers() /* initialise ChibiOS network backend using LWIP - */ +*/ bool AP_Networking_ChibiOS::init() { #ifdef HAL_GPIO_ETH_ENABLE @@ -102,43 +116,259 @@ bool AP_Networking_ChibiOS::init() return false; } -#if !AP_NETWORKING_DHCP_AVAILABLE - frontend.set_dhcp_enable(false); +#if LWIP_IGMP + if (ETH != nullptr) { + // enbale "permit multicast" so we can receive multicast packets + ETH->MACPFR |= ETH_MACPFR_PM; + } #endif - lwip_options = new lwipthread_opts; + thisif = new netif; + if (thisif == nullptr) { + return false; + } - 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()); + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking_ChibiOS::thread, void), + "network", + 2048, AP_HAL::Scheduler::PRIORITY_NET, 0)) { + return false; } - frontend.param.macaddr.get_address(macaddr); - lwip_options->macaddress = macaddr; + + return true; +} - lwipInit(lwip_options); +void AP_Networking_ChibiOS::link_up_cb(void *p) +{ + auto *driver = (AP_Networking_ChibiOS *)p; +#if LWIP_DHCP + if (driver->frontend.get_dhcp_enabled()) { + dhcp_start(driver->thisif); + } +#endif +} -#if LWIP_IGMP - if (ETH != nullptr) { - // enbale "permit multicast" so we can receive multicast packets - ETH->MACPFR |= ETH_MACPFR_PM; +void AP_Networking_ChibiOS::link_down_cb(void *p) +{ + auto *driver = (AP_Networking_ChibiOS *)p; +#if LWIP_DHCP + if (driver->frontend.get_dhcp_enabled()) { + 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 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..96bd7977ec0538 100644 --- a/libraries/AP_Networking/AP_Networking_ChibiOS.h +++ b/libraries/AP_Networking/AP_Networking_ChibiOS.h @@ -18,10 +18,17 @@ class AP_Networking_ChibiOS : public AP_Networking_Backend private: 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 bfc4af537f226d..d9d8532eeff6eb 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -1,34 +1,67 @@ #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 - // --------------------------- // 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 +#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 // --------------------------- -#if AP_NETWORKING_BACKEND_CHIBIOS -#define AP_NETWORKING_DHCP_AVAILABLE LWIP_DHCP -#else -#define AP_NETWORKING_DHCP_AVAILABLE 1 // for non-ChibiOS, assume it's available +#ifndef AP_NETWORKING_DHCP_AVAILABLE +// AP_NETWORKING_DHCP_AVAILABLE should only be true if, by setting the NET_DHCP parameter, +// we have the ability to turn on/off the DHCP client which effects the assigned IP address. +// Otherwise, param NET_DHCP will be hidden. This does not mean that the system/OS does not +// have DHCP, 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_DHCP_AVAILABLE (AP_NETWORKING_CONTROLS_HOST_IP_SETTINGS_ENABLED || AP_NETWORKING_BACKEND_CHIBIOS) #endif @@ -38,7 +71,7 @@ // Default DHCP #ifndef AP_NETWORKING_DEFAULT_DHCP_ENABLE -#define AP_NETWORKING_DEFAULT_DHCP_ENABLE 1 +#define AP_NETWORKING_DEFAULT_DHCP_ENABLE AP_NETWORKING_DHCP_AVAILABLE #endif // Default Static IP Address: 192.168.13.14 @@ -78,3 +111,25 @@ #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 + 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 5cadbbe6f7a144..93997479f198b0 100644 --- a/libraries/AP_Networking/AP_Networking_address.cpp +++ b/libraries/AP_Networking/AP_Networking_address.cpp @@ -6,7 +6,9 @@ #if AP_NETWORKING_ENABLED +#include #include "AP_Networking.h" +#include const AP_Param::GroupInfo AP_Networking_IPV4::var_info[] = { // @Param: 0 @@ -47,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 @@ -68,9 +70,10 @@ void AP_Networking_IPV4::set_default_uint32(uint32_t v) } } -const char* AP_Networking_IPV4::get_str() const +const char* AP_Networking_IPV4::get_str() { - return AP_Networking::convert_ip_to_str(get_uint32()); + 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_address.h b/libraries/AP_Networking/AP_Networking_address.h index a4a3b6f8b29a8f..f9852948676ec8 100644 --- a/libraries/AP_Networking/AP_Networking_address.h +++ b/libraries/AP_Networking/AP_Networking_address.h @@ -18,12 +18,15 @@ class AP_Networking_IPV4 void set_uint32(uint32_t addr); // return address as a null-terminated string - const char* get_str() const; + const char* get_str(); // set default address from a uint32 void set_default_uint32(uint32_t addr); static const struct AP_Param::GroupInfo var_info[]; + +private: + char strbuf[16]; }; /* diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp index e70224cf4b4b7b..1074d896351c07 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -11,6 +11,8 @@ #include #include #include +#include +#include extern const AP_HAL::HAL& hal; @@ -22,24 +24,30 @@ extern const AP_HAL::HAL& hal; #define AP_NETWORKING_PORT_MIN_RXSIZE 2048 #endif +#ifndef AP_NETWORKING_PORT_STACK_SIZE +#define AP_NETWORKING_PORT_STACK_SIZE 1024 +#endif + const AP_Param::GroupInfo AP_Networking::Port::var_info[] = { // @Param: TYPE // @DisplayName: Port type - // @Description: Port type - // @Values: 0:Disabled, 1:UDP client + // @Description: Port type for network serial port. For the two client types a valid destination IP address must be set. For the two server types either 0.0.0.0 or a local address can be used. The UDP client type will use broadcast if the IP is set to 255.255.255.255 and will use UDP multicast if the IP is in the multicast address range. + // @Values: 0:Disabled, 1:UDP client, 2:UDP server, 3:TCP client, 4:TCP server // @RebootRequired: True // @User: Advanced 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 - // @CopyFieldsFrom: SERIAL0_PROTOCOL + // @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 @@ -66,33 +74,101 @@ void AP_Networking::ports_init(void) case NetworkPortType::NONE: break; case NetworkPortType::UDP_CLIENT: - p.udp_client_init(AP_NETWORKING_PORT_MIN_RXSIZE, AP_NETWORKING_PORT_MIN_TXSIZE); + p.udp_client_init(); + break; + case NetworkPortType::UDP_SERVER: + p.udp_server_init(); + break; + case NetworkPortType::TCP_SERVER: + p.tcp_server_init(); + break; + case NetworkPortType::TCP_CLIENT: + p.tcp_client_init(); break; } - if (p.sock != nullptr) { + if (p.sock != nullptr || p.listen_sock != nullptr) { AP::serialmanager().register_port(&p); } } } /* - initialise a UDP client + wrapper for thread_create for port functions */ -void AP_Networking::Port::udp_client_init(const uint32_t size_rx, const uint32_t size_tx) +void AP_Networking::Port::thread_create(AP_HAL::MemberProc proc) { - WITH_SEMAPHORE(sem); - if (!init_buffers(size_rx, size_tx)) { + const uint8_t idx = state.idx - AP_SERIALMANAGER_NET_PORT_1; + hal.util->snprintf(thread_name, sizeof(thread_name), "NET_P%u", unsigned(idx)); + + if (!init_buffers(AP_NETWORKING_PORT_MIN_RXSIZE, AP_NETWORKING_PORT_MIN_TXSIZE)) { + AP_BoardConfig::allocation_error("Failed to allocate %s buffers", thread_name); return; } - if (sock != nullptr) { + + if (!hal.scheduler->thread_create(proc, thread_name, AP_NETWORKING_PORT_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_UART, 0)) { + AP_BoardConfig::allocation_error("Failed to allocate %s client thread", thread_name); + } +} + +/* + initialise a UDP client + */ +void AP_Networking::Port::udp_client_init(void) +{ + sock = new SocketAPM(true); + if (sock == nullptr) { return; } + sock->set_blocking(false); + + // setup for packet boundaries if this is mavlink + packetise = (state.protocol == AP_SerialManager::SerialProtocol_MAVLink || + state.protocol == AP_SerialManager::SerialProtocol_MAVLink2); + + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_client_loop, void)); +} + +/* + initialise a UDP server + */ +void AP_Networking::Port::udp_server_init(void) +{ sock = new SocketAPM(true); if (sock == nullptr) { return; } - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_client_loop, void), "NET", 2048, AP_HAL::Scheduler::PRIORITY_UART, 0)) { - AP_BoardConfig::allocation_error("Failed to allocate UDP client thread"); + sock->set_blocking(false); + + // setup for packet boundaries if this is mavlink + packetise = (state.protocol == AP_SerialManager::SerialProtocol_MAVLink || + state.protocol == AP_SerialManager::SerialProtocol_MAVLink2); + + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::udp_server_loop, void)); +} + +/* + initialise a TCP server + */ +void AP_Networking::Port::tcp_server_init(void) +{ + listen_sock = new SocketAPM(false); + if (listen_sock == nullptr) { + return; + } + listen_sock->reuseaddress(); + + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::tcp_server_loop, void)); +} + +/* + initialise a TCP client + */ +void AP_Networking::Port::tcp_client_init(void) +{ + sock = new SocketAPM(false); + if (sock != nullptr) { + sock->set_blocking(true); + thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::Port::tcp_client_loop, void)); } } @@ -101,10 +177,7 @@ void AP_Networking::Port::udp_client_init(const uint32_t size_rx, const uint32_t */ void AP_Networking::Port::udp_client_loop(void) { - while (!hal.scheduler->is_system_initialized()) { - hal.scheduler->delay(100); - } - hal.scheduler->delay(1000); + AP::network().startup_wait(); const char *dest = ip.get_str(); if (!sock->connect(dest, port.get())) { @@ -116,31 +189,206 @@ void AP_Networking::Port::udp_client_loop(void) GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: connected to %s:%u", (unsigned)state.idx, dest, unsigned(port.get())); + connected = true; + + bool active = false; + while (true) { + if (!active) { + hal.scheduler->delay_microseconds(100); + } + active = send_receive(); + } +} + +/* + update a UDP server + */ +void AP_Networking::Port::udp_server_loop(void) +{ + AP::network().startup_wait(); + + const char *addr = ip.get_str(); + if (!sock->bind(addr, port.get())) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UDP[%u]: Failed to bind to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + delete sock; + sock = nullptr; + return; + } + sock->reuseaddress(); + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP[%u]: bound to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + + bool active = false; + while (true) { + if (!active) { + hal.scheduler->delay_microseconds(100); + } + active = send_receive(); + } +} + +/* + update a TCP server + */ +void AP_Networking::Port::tcp_server_loop(void) +{ + AP::network().startup_wait(); + + const char *addr = ip.get_str(); + if (!listen_sock->bind(addr, port.get()) || !listen_sock->listen(1)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP[%u]: Failed to bind to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + delete listen_sock; + listen_sock = nullptr; + return; + } + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: bound to %s:%u", (unsigned)state.idx, addr, unsigned(port.get())); + + close_on_recv_error = true; + + bool active = false; while (true) { - hal.scheduler->delay_microseconds(500); + if (!active) { + hal.scheduler->delay_microseconds(100); + } + if (sock == nullptr) { + sock = listen_sock->accept(100); + if (sock != nullptr) { + sock->set_blocking(false); + char buf[16]; + uint16_t last_port; + const char *last_addr = listen_sock->last_recv_address(buf, sizeof(buf), last_port); + if (last_addr != nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: connection from %s:%u", (unsigned)state.idx, last_addr, unsigned(last_port)); + } + connected = true; + sock->reuseaddress(); + } + } + if (sock != nullptr) { + active = send_receive(); + } + } +} + +/* + update a TCP client + */ +void AP_Networking::Port::tcp_client_loop(void) +{ + AP::network().startup_wait(); + + close_on_recv_error = true; + + bool active = false; + while (true) { + if (!active) { + hal.scheduler->delay_microseconds(100); + } + if (sock == nullptr) { + sock = new SocketAPM(false); + if (sock == nullptr) { + continue; + } + sock->set_blocking(true); + connected = false; + } + if (!connected) { + const char *dest = ip.get_str(); + connected = sock->connect(dest, port.get()); + if (connected) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: connected to %s:%u", unsigned(state.idx), dest, unsigned(port.get())); + sock->set_blocking(false); + } else { + delete sock; + sock = nullptr; + // don't try and connect too fast + hal.scheduler->delay(100); + } + } + if (sock != nullptr && connected) { + active = send_receive(); + } + } +} + +/* + run one send/receive loop + */ +bool AP_Networking::Port::send_receive(void) +{ + + bool active = false; + uint32_t space; + + + // handle incoming packets + { WITH_SEMAPHORE(sem); + space = readbuffer->space(); + } + if (space > 0) { + const uint32_t n = MIN(300U, space); + uint8_t buf[n]; + const auto ret = sock->recv(buf, n, 0); + if (close_on_recv_error && ret == 0) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: closed connection", unsigned(state.idx)); + delete sock; + sock = nullptr; + return false; + } + if (ret > 0) { + WITH_SEMAPHORE(sem); + readbuffer->write(buf, ret); + active = true; + have_received = true; + } + } + if (connected) { // handle outgoing packets uint32_t available; - const auto *ptr = writebuffer->readptr(available); - if (ptr != nullptr) { - const auto ret = sock->send(ptr, available); + + { + WITH_SEMAPHORE(sem); + available = writebuffer->available(); + available = MIN(300U, available); +#if HAL_GCS_ENABLED + if (packetise) { + available = mavlink_packetise(*writebuffer, available); + } +#endif + 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; } } - - // handle incoming packets - const auto space = readbuffer->space(); - if (space > 0) { - const uint32_t n = MIN(350U, space); - uint8_t buf[n]; - const auto ret = sock->recv(buf, n, 0); - if (ret > 0) { - readbuffer->write(buf, ret); + } else { + if (type == NetworkPortType::UDP_SERVER && have_received) { + // connect the socket to the last receive address if we have one + char buf[16]; + uint16_t last_port; + const char *last_addr = sock->last_recv_address(buf, sizeof(buf), last_port); + if (last_addr != nullptr && port != 0) { + connected = sock->connect(last_addr, last_port); } } } + + return active; } /* @@ -210,4 +458,22 @@ bool AP_Networking::Port::init_buffers(const uint32_t size_rx, const uint32_t si return readbuffer != nullptr && writebuffer != nullptr; } +/* + return flow control state + */ +enum AP_HAL::UARTDriver::flow_control AP_Networking::Port::get_flow_control(void) +{ + const NetworkPortType ptype = (NetworkPortType)type; + switch (ptype) { + case NetworkPortType::TCP_CLIENT: + case NetworkPortType::TCP_SERVER: + return AP_HAL::UARTDriver::FLOW_CONTROL_ENABLE; + case NetworkPortType::UDP_CLIENT: + case NetworkPortType::UDP_SERVER: + case NetworkPortType::NONE: + break; + } + return AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE; +} + #endif // AP_NETWORKING_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_tests.cpp b/libraries/AP_Networking/AP_Networking_tests.cpp index 9370d93a23b407..839f17f74647ad 100644 --- a/libraries/AP_Networking/AP_Networking_tests.cpp +++ b/libraries/AP_Networking/AP_Networking_tests.cpp @@ -28,6 +28,11 @@ void AP_Networking::start_tests(void) "TCP_client", 8192, AP_HAL::Scheduler::PRIORITY_IO, -1); } + if (param.tests & TEST_TCP_DISCARD) { + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_TCP_discard, void), + "TCP_discard", + 8192, AP_HAL::Scheduler::PRIORITY_UART, -1); + } } /* @@ -35,10 +40,7 @@ void AP_Networking::start_tests(void) */ void AP_Networking::test_UDP_client(void) { - while (!hal.scheduler->is_system_initialized()) { - hal.scheduler->delay(100); - } - hal.scheduler->delay(1000); + startup_wait(); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UDP_client: starting"); const char *dest = param.test_ipaddr.get_str(); auto *sock = new SocketAPM(true); @@ -70,10 +72,7 @@ void AP_Networking::test_UDP_client(void) */ void AP_Networking::test_TCP_client(void) { - while (!hal.scheduler->is_system_initialized()) { - hal.scheduler->delay(100); - } - hal.scheduler->delay(1000); + startup_wait(); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_client: starting"); const char *dest = param.test_ipaddr.get_str(); auto *sock = new SocketAPM(false); @@ -100,4 +99,45 @@ void AP_Networking::test_TCP_client(void) } } +/* + start TCP discard (throughput) test + */ +void AP_Networking::test_TCP_discard(void) +{ + startup_wait(); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP_discard: starting"); + const char *dest = param.test_ipaddr.get_str(); + auto *sock = new SocketAPM(false); + if (sock == nullptr) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "TCP_discard: failed to create socket"); + return; + } + // connect to the discard service, which is port 9 + 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) { + float dt = (now - last_report_ms)*0.001; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Discard throughput %.3f kbyte/sec", (total_sent/dt)*1.0e-3); + total_sent = 0; + last_report_ms = now; + } + } +} + #endif // AP_NETWORKING_ENABLED && AP_NETWORKING_TESTS_ENABLED diff --git a/libraries/AP_Networking/config/lwipopts.h b/libraries/AP_Networking/config/lwipopts.h new file mode 100644 index 00000000000000..78ea48e8270271 --- /dev/null +++ b/libraries/AP_Networking/config/lwipopts.h @@ -0,0 +1,379 @@ +/* + * 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 + +#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) + +#define LWIP_IGMP LWIP_IPV4 +#define LWIP_ICMP LWIP_IPV4 + +#define LWIP_SNMP LWIP_UDP +#ifdef LWIP_HAVE_MBEDTLS +#define LWIP_SNMP_V3 (LWIP_SNMP) +#endif + +#define LWIP_DNS LWIP_UDP +#define LWIP_MDNS_RESPONDER LWIP_UDP + +#define LWIP_NUM_NETIF_CLIENT_DATA (LWIP_MDNS_RESPONDER) + +#define LWIP_HAVE_LOOPIF 1 +#define LWIP_NETIF_LOOPBACK 1 +#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 + +#define LWIP_ALTCP (LWIP_TCP) +#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 +#define ARP_PROXYARP_SUPPORT 1 + + +/* ---------- 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. */ +#define IP_FORWARD 1 + +/* + 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. */ +#define LWIP_DHCP 1 + +/* 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 ---------- */ +#define LWIP_UDP 1 +#define LWIP_UDPLITE LWIP_UDP +#define UDP_TTL 255 + + +/* ---------- RAW options ---------- */ +#define LWIP_RAW 1 + + +/* ---------- Statistics options ---------- */ + +#define LWIP_STATS 0 +#define LWIP_STATS_DISPLAY 0 + +/* ---------- NETBIOS options ---------- */ +#define LWIP_NETBIOS_RESPOND_NAME_QUERY 1 + +/* ---------- PPP options ---------- */ + +#define PPP_SUPPORT 1 /* Set > 0 for PPP */ + +#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 1 + +#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..83053a9ee2629e --- /dev/null +++ b/libraries/AP_Networking/lwip_hal/arch/sys_arch.cpp @@ -0,0 +1,392 @@ +/* + 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 + +#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) { + return hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&ThreadWrapper::run, void), name, MAX(stacksize,2048), AP_HAL::Scheduler::PRIORITY_NET, prio); + } +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) +{ + if (hal.scheduler != nullptr) { + lwprot_mutex.take_blocking(); + } + return 0; +} + +void +sys_arch_unprotect(sys_prot_t pval) +{ + LWIP_UNUSED_ARG(pval); + if (hal.scheduler != nullptr) { + 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 ad0dbe6d955755..bf3b7b1024510a 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -110,7 +110,8 @@ AP_Notify *AP_Notify::_singleton; CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO #define DEFAULT_NTF_LED_TYPES (Notify_LED_Board) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ @@ -436,7 +437,8 @@ void AP_Notify::add_backends(void) CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RST_ZYNQ || \ - CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ + CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO // No noise makers, keep this though to ensure that the final else is safe #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \ 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_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index d4284cc7208981..e88e2363959a48 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -457,11 +457,13 @@ void AP_OSD::update_stats() if (voltage > 0) { _stats.min_voltage_v = fminf(_stats.min_voltage_v, voltage); } +#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 +511,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 +567,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_ParamSetting.cpp b/libraries/AP_OSD/AP_OSD_ParamSetting.cpp index 95476abd8fb440..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" + "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..53cb8ce8e50f1f 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -1410,6 +1410,7 @@ void AP_OSD_Screen::draw_restvolt(uint8_t x, uint8_t y) draw_bat_volt(0,VoltageType::RESTING_VOLTAGE,x,y); } +#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,6 +1432,7 @@ void AP_OSD_Screen::draw_link_quality(uint8_t x, uint8_t y) } } } +#endif // AP_RSSI_ENABLED void AP_OSD_Screen::draw_current(uint8_t instance, uint8_t x, uint8_t y) { @@ -1569,7 +1571,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 +1822,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); diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp index 137a5778b55c8f..be86676d59ac56 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow.cpp @@ -267,6 +267,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 +275,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_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..4cdf7ce692443c 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), @@ -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,19 @@ 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 (!enabled() || (_release_type > AP_PARACHUTE_TRIGGER_TYPE_RELAY_3)) { + 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 9e0ad48b854a40..56d9cbf8ecbf63 100644 --- a/libraries/AP_Param/AP_Param.cpp +++ b/libraries/AP_Param/AP_Param.cpp @@ -2999,6 +2999,11 @@ bool AP_Param::load_int32(uint16_t key, uint32_t group_element, int32_t &value) */ bool AP_Param::add_param(uint8_t _key, uint8_t param_num, const char *pname, float default_value) { + if (_var_info_dynamic == nullptr) { + // No dynamic tables available + return false; + } + // check for valid values if (param_num == 0 || param_num > 63 || strlen(pname) > 16) { return false; diff --git a/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp b/libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp index 1cd68ac08aa0a2..ef07781772cec2 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; } 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_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_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index 9c8ef679a1643b..d2d11cfb304bf4 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 @@ -82,6 +83,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 +301,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 @@ -504,6 +508,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..8889fdce89ee49 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -69,6 +69,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 }; @@ -129,6 +132,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_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp index 08bc5a06fa7912..c16e2c2b4592a8 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp @@ -179,7 +179,7 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame) buf[3] = telem_data.packet.appid & 0xFF; buf[4] = telem_data.packet.appid >> 8; memcpy(&buf[5], &telem_data.packet.data, 4); - buf[9] = crc_sum8(&buf[0], 9); + buf[9] = crc_sum8_with_carry(&buf[0], 9); // perform byte stuffing per FPort spec uint8_t len = 0; @@ -307,7 +307,7 @@ void AP_RCProtocol_FPort::_process_byte(uint32_t timestamp_us, uint8_t b) bool AP_RCProtocol_FPort::check_checksum(void) { const uint8_t len = byte_input.buf[1]+2; - return crc_sum8(&byte_input.buf[1], len) == 0x00; + return crc_sum8_with_carry(&byte_input.buf[1], len) == 0x00; } // support byte input diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp index dc9eceb61f8dd2..2631f51d030254 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp @@ -179,7 +179,7 @@ void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame) // get fresh telem_data in the next call telem_data.available = false; } - buf[9] = crc_sum8(&buf[1], 8); + buf[9] = crc_sum8_with_carry(&buf[1], 8); uart->write(buf, sizeof(buf)); #endif @@ -286,7 +286,7 @@ void AP_RCProtocol_FPort2::_process_byte(uint32_t timestamp_us, uint8_t b) // check checksum byte bool AP_RCProtocol_FPort2::check_checksum(void) { - return crc_sum8(&byte_input.buf[1], byte_input.control_len-1) == 0; + return crc_sum8_with_carry(&byte_input.buf[1], byte_input.control_len-1) == 0; } // support byte input diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp new file mode 100644 index 00000000000000..195e56bfb91641 --- /dev/null +++ b/libraries/AP_RCProtocol/AP_RCProtocol_GHST.cpp @@ -0,0 +1,453 @@ +/* + 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; + } + + // 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 && !APM_BUILD_TYPE(APM_BUILD_iofirmware) + 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 ELRS 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 bootstrapping CRSF + if (uart == nullptr + || baudrate != CRSF_BAUDRATE + || baudrate == GHST_BAUDRATE + || uart->get_baud_rate() == GHST_BAUDRATE + || (get_rc_protocols_mask() & ((1U<<(uint8_t(AP_RCProtocol::GHST)+1))+1)) == 0) { + 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_config.h b/libraries/AP_RCProtocol/AP_RCProtocol_config.h index 5619c5373c0819..d6bef7d61bb01b 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_config.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_config.h @@ -61,3 +61,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..30e16b6d0a8f46 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -931,9 +931,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..72e2d2a09e7e00 --- /dev/null +++ b/libraries/AP_RCTelemetry/AP_GHST_Telem.cpp @@ -0,0 +1,386 @@ +/* + 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; + } + // 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_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..77c763b528e0c0 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; } @@ -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_RangeFinder/AP_RangeFinder_BLPing.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp index 3cd40aa298e734..e7776e0f955745 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_BLPing.cpp @@ -61,10 +61,9 @@ bool AP_RangeFinder_BLPing::get_reading(float &reading_m) } averageStruct; // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - const int16_t b = uart->read(); - if (b < 0) { + for (auto i=0; i<8192; i++) { + uint8_t b; + if (!uart->read(b)) { break; } if (protocol.parse_byte(b) == PingProtocol::MessageId::DISTANCE_SIMPLE) { 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_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index ceea2fd16815fa..f04056588ddb06 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -59,13 +59,11 @@ bool AP_RangeFinder_Benewake::get_reading(float &reading_m) uint16_t count_out_of_range = 0; // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t r = uart->read(); - if (r < 0) { - continue; + for (auto j=0; j<8192; j++) { + uint8_t c; + if (!uart->read(c)) { + break; } - uint8_t c = (uint8_t)r; // if buffer is empty and this byte is 0x59, add to buffer if (linebuf_len == 0) { if (c == BENEWAKE_FRAME_HEADER) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp index d59b030e50e35e..16c37db38f0420 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.cpp @@ -46,10 +46,9 @@ bool AP_RangeFinder_Lanbao::get_reading(float &reading_m) // format is: [ 0xA5 | 0x5A | distance-MSB-mm | distance-LSB-mm | crc16 ] // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t b = uart->read(); - if (b == -1) { + for (auto i=0; i<8192; i++) { + uint8_t b; + if (!uart->read(b)) { break; } if (buf_len == 0 && b != 0xA5) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp index 04e38624e0e428..3d90877d838803 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp @@ -51,13 +51,12 @@ bool AP_RangeFinder_LeddarVu8::get_reading(float &reading_m) bool latest_dist_valid = false; // read any available characters from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t r = uart->read(); - if (r < 0) { - continue; + for (auto i=0; i<8192; i++) { + uint8_t b; + if (!uart->read(b)) { + break; } - if (parse_byte((uint8_t)r, latest_dist_valid, latest_dist_cm)) { + if (parse_byte(b, latest_dist_valid, latest_dist_cm)) { if (latest_dist_valid) { sum_cm += latest_dist_cm; count++; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp index 9636b47bf27003..61c2ed9c2d9751 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.cpp @@ -40,10 +40,11 @@ bool AP_RangeFinder_LightWareSerial::get_reading(float &reading_m) const int16_t distance_cm_max = max_distance_cm(); // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - char c = uart->read(); - + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } // use legacy protocol if (protocol_state == ProtocolState::UNKNOWN || protocol_state == ProtocolState::LEGACY) { if (c == '\r') { 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_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp index 7f0173947c61df..50abcc6bceae77 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.cpp @@ -42,11 +42,13 @@ bool AP_RangeFinder_MaxsonarSerialLV::get_reading(float &reading_m) } int32_t sum = 0; - int16_t nbytes = uart->available(); uint16_t count = 0; - while (nbytes-- > 0) { - char c = uart->read(); + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (c == '\r') { linebuf[linebuf_len] = 0; sum += (int)atoi(linebuf); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index 61945ca7c3a1f1..bc6a3cf3911581 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -32,9 +32,11 @@ bool AP_RangeFinder_NMEA::get_reading(float &reading_m) // read any available lines from the lidar float sum = 0.0f; uint16_t count = 0; - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - char c = uart->read(); + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (decode(c)) { sum += _distance_m; count++; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp index e9a502de79edae..088472b6b8387b 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TeraRanger_Serial.cpp @@ -54,13 +54,11 @@ bool AP_RangeFinder_TeraRanger_Serial::get_reading(float &reading_m) uint16_t bad_read = 0; // read any available lines from the lidar - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - int16_t r = uart->read(); - if (r < 0) { - continue; + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; } - uint8_t c = (uint8_t)r; // if buffer is empty and this byte is 0x57, add to buffer if (linebuf_len == 0) { if (c == FRAME_HEADER) { diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp index 04d8c063298734..26328657d11888 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp @@ -39,11 +39,11 @@ bool AP_RangeFinder_USD1_Serial::detect_version(void) uint8_t count = 0; // read any available data from USD1_Serial - int16_t nbytes = uart->available(); - - while (nbytes-- > 0) { - uint8_t c = uart->read(); - + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (((c == USD1_HDR_V0) || (c == USD1_HDR)) && !hdr_found) { byte1 = c; hdr_found = true; @@ -121,11 +121,11 @@ bool AP_RangeFinder_USD1_Serial::get_reading(float &reading_m) uint16_t count = 0; bool hdr_found = false; - int16_t nbytes = uart->available(); - - while (nbytes-- > 0) { - uint8_t c = uart->read(); - + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if ((c == _header) && !hdr_found) { // located header byte _linebuf_len = 0; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp index cda423c79b11a2..5885b2af146254 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Wasp.cpp @@ -86,9 +86,11 @@ bool AP_RangeFinder_Wasp::get_reading(float &reading_m) { // read any available lines from the lidar float sum = 0; uint16_t count = 0; - int16_t nbytes = uart->available(); - while (nbytes-- > 0) { - char c = uart->read(); + for (auto i=0; i<8192; i++) { + uint8_t c; + if (!uart->read(c)) { + break; + } if (c == '\n') { linebuf[linebuf_len] = 0; linebuf_len = 0; diff --git a/libraries/AP_Relay/AP_Relay.cpp b/libraries/AP_Relay/AP_Relay.cpp index 1c4cb143f7b0fc..47d2b030b8031b 100644 --- a/libraries/AP_Relay/AP_Relay.cpp +++ b/libraries/AP_Relay/AP_Relay.cpp @@ -15,6 +15,24 @@ #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 +78,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 +199,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) { + const int16_t pin = _params[instance].pin; + if (pin == -1) { + // no valid pin to set it on, skip it return; } - if (_pin[instance] == -1) { - 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)); @@ -195,26 +476,182 @@ bool AP_Relay::arming_checks(size_t buflen, char *buffer) const 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 +#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 +45,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 +52,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..c8c9f7ab98b615 --- /dev/null +++ b/libraries/AP_Relay/AP_Relay_Params.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include "AP_Relay_config.h" + +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..93f55a22778646 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; diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index 2c9dd332e7b056..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[] = { @@ -84,7 +88,12 @@ const AP_Param::GroupInfo AP_Scripting::var_info[] = { // @Param: DEBUG_OPTS // @DisplayName: Scripting Debug Level // @Description: Debugging options - // @Bitmask: 0:No Scripts to run message if all scripts have stopped, 1:Runtime messages for memory usage and execution time, 2:Suppress logging scripts to dataflash, 3:log runtime memory usage and execution time, 4:Disable pre-arm check + // @Bitmask: 0: No Scripts to run message if all scripts have stopped + // @Bitmask: 1: Runtime messages for memory usage and execution time + // @Bitmask: 2: Suppress logging scripts to dataflash + // @Bitmask: 3: log runtime memory usage and execution time + // @Bitmask: 4: Disable pre-arm check + // @Bitmask: 5: Save CRC of current scripts to loaded and running checksum parameters enabling pre-arm // @User: Advanced AP_GROUPINFO("DEBUG_OPTS", 4, AP_Scripting, _debug_options, 0), @@ -132,6 +141,26 @@ const AP_Param::GroupInfo AP_Scripting::var_info[] = { // @User: Advanced AP_GROUPINFO("DIR_DISABLE", 9, AP_Scripting, _dir_disable, 0), + // @Param: LD_CHECKSUM + // @DisplayName: Loaded script checksum + // @Description: Required XOR of CRC32 checksum of loaded scripts, vehicle will not arm with incorrect scripts loaded, -1 disables + // @User: Advanced + AP_GROUPINFO("LD_CHECKSUM", 12, AP_Scripting, _required_loaded_checksum, -1), + + // @Param: RUN_CHECKSUM + // @DisplayName: Running script checksum + // @Description: Required XOR of CRC32 checksum of running scripts, vehicle will not arm with incorrect scripts running, -1 disables + // @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 }; @@ -158,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; } @@ -270,6 +320,16 @@ void AP_Scripting::thread(void) { } num_pwm_source = 0; +#if AP_NETWORKING_ENABLED + // clear allocated sockets + for (uint8_t i=0; igive(); + // Use -1 for disabled, this means we don't have to avoid 0 in the CRC, the sign bit is masked off anyway + if (_required_loaded_checksum != -1) { + const uint32_t expected_loaded = (uint32_t)_required_loaded_checksum.get() & checksum_param_mask; + const uint32_t loaded = lua_scripts::get_loaded_checksum() & checksum_param_mask; + if (expected_loaded != loaded) { + hal.util->snprintf(buffer, buflen, "Scripting: loaded CRC incorrect want: 0x%x", (unsigned int)loaded); + return false; + } + } + + if (_required_running_checksum != -1) { + const uint32_t expected_running = (uint32_t)_required_running_checksum.get() & checksum_param_mask; + const uint32_t running = lua_scripts::get_running_checksum() & checksum_param_mask; + if (expected_running != running) { + hal.util->snprintf(buffer, buflen, "Scripting: running CRC incorrect want: 0x%x", (unsigned int)running); + return false; + } + } + return true; } @@ -403,6 +482,34 @@ bool AP_Scripting::is_handling_command(uint16_t cmd_id) } #endif // HAL_GCS_ENABLED +// Update called at 1Hz from AP_Vehicle +void AP_Scripting::update() { + + save_checksum(); + +} + +// Check if DEBUG_OPTS bit has been set to save current checksum values to params +void AP_Scripting::save_checksum() { + + const uint8_t opts = _debug_options.get(); + const uint8_t save_bit = uint8_t(lua_scripts::DebugLevel::SAVE_CHECKSUM); + if ((opts & save_bit) == 0) { + // Bit not set, nothing to do + return; + } + + // Save two checksum parameters to there current values + _required_loaded_checksum.set_and_save(lua_scripts::get_loaded_checksum() & checksum_param_mask); + _required_running_checksum.set_and_save(lua_scripts::get_running_checksum() & checksum_param_mask); + + // Un-set debug option bit + _debug_options.set_and_save(opts & ~save_bit); + + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Scripting: %s", "saved checksums"); + +} + AP_Scripting *AP_Scripting::_singleton = nullptr; namespace AP { diff --git a/libraries/AP_Scripting/AP_Scripting.h b/libraries/AP_Scripting/AP_Scripting.h index bc057eb7335a77..f7139775c02f6b 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/libraries/AP_Scripting/AP_Scripting.h @@ -24,6 +24,7 @@ #include #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: @@ -41,6 +49,8 @@ class AP_Scripting void init(void); + void update(); + bool enabled(void) const { return _enable != 0; }; bool should_run(void) const { return enabled() && !_stop; } @@ -109,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; @@ -134,15 +149,36 @@ class AP_Scripting bool repl_start(void); void repl_stop(void); - void load_script(const char *filename); // load a script from a file - void thread(void); // main script execution thread + // Check if DEBUG_OPTS bit has been set to save current checksum values to params + void save_checksum(); + + // Mask down to 23 bits for comparison with parameters, this the length of the a float mantissa, to deal with the float transport of parameters over MAVLink + // 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; AP_Int8 _debug_options; AP_Int16 _dir_disable; + 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/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/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 " 0 then - -- if we have any FF on an axis then we don't couple I to P, - -- usually we want I = FF for a one sectond time constant for trim - return - end - -- if PI_RATIO is non-zero then update I - local PI_ratio = RTUN_PI_RATIO:get() - if PI_ratio > 0 then - local iname = string.gsub(pname, "_P", "_I") - local I = params[iname] - new_I_gain = value/PI_ratio - I:set(new_I_gain) - param_changed[iname] = true - write_log(iname) - end - end -end - --- return gain multipler for one loop -function get_gain_mul() - return math.exp(math.log(2.0)/(UPDATE_RATE_HZ*RTUN_DOUBLE_TIME:get())) -end - --- setup parameter slewing. slewing some changes over 2 seconds reduces shock to controllers -function setup_slew_gain(pname, gain) - slew_parm = pname - slew_steps = UPDATE_RATE_HZ / 2 - slew_delta = (gain - params[pname]:get()) / slew_steps -end - --- update parameter slewing. slewing some changes over 2 seconds reduces shock to controllers -function update_slew_gain() - if slew_parm ~= nil then - local P = params[slew_parm] - adjust_gain(slew_parm, P:get()+slew_delta) - write_log(slew_parm) - slew_steps = slew_steps - 1 - -- check if slewing is complete - if slew_steps == 0 then - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.3f", slew_parm, P:get())) - slew_parm = nil - end - end + write_log(pname) + gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", pname, old_value, value)) end -- log parameter, current gain and current slew rate @@ -449,25 +353,6 @@ function write_log(pname) logger:write("RTUN","SRate,Gain,Param", "ffN", slew_rate, param_gain, pname) end --- return gain limits on a parameter, or 0 for no limit -function gain_limit(pname) - if pname == "ATC_SPEED_P" then - return RTUN_SPD_P_MAX:get() - elseif pname == "ATC_SPEED_D" then - return RTUN_SPD_D_MAX:get() - end - return 0.0 -end - --- check if parameter's gain has reached its limit -function reached_limit(pname, gain) - local limit = gain_limit(pname) - if limit > 0.0 and gain >= limit then - return true - end - return false -end - -- initialise steering ff tuning function init_steering_ff() ff_steering_sum = 0 @@ -476,7 +361,9 @@ function init_steering_ff() end -- run steering turn rate controller feedforward calibration -function update_steering_ff(pname) +-- ff_pname is the FF parameter being tuned +-- returns true once the tuning has completed +function update_steering_ff(ff_pname) -- get steering, turn rate, throttle and speed local steering_out, _ = vehicle:get_steering_and_throttle() local turn_rate_rads = ahrs:get_gyro():z() @@ -502,7 +389,7 @@ function update_steering_ff(pname) ff_turn_rate_sum = ff_turn_rate_sum + math.abs(turn_rate_rads) ff_turn_rate_count = ff_turn_rate_count + 1 if (update_user) then - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", pname, complete_pct)) + gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", ff_pname, complete_pct)) end else if update_user then @@ -516,28 +403,38 @@ function update_steering_ff(pname) -- check for completion of two rotations of turns data and 10 seconds if complete_pct >= 100 then - local old_gain = params[pname]:get() - local new_gain = (ff_steering_sum / ff_turn_rate_sum) * (1.0-(RTUN_FF_GAINMARG:get()*0.01)) - adjust_gain(pname, new_gain) - write_log(pname) - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", pname, old_gain, new_gain)) - - -- set I gain equal to FF - local iname = string.gsub(pname, "_FF", "_I") - local I = params[iname] - local I_old_gain = I:get() - I:set(new_gain) - param_changed[iname] = true - write_log(iname) - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", iname, I_old_gain, new_gain)) + local FF_new_gain = (ff_steering_sum / ff_turn_rate_sum) * RTUN_STR_FFRATIO:get() + adjust_gain(ff_pname, FF_new_gain) + + -- set P gain + if RTUN_STR_P_RATIO:get() > 0 then + local pname = string.gsub(ff_pname, "_FF", "_P") + adjust_gain(pname, FF_new_gain * RTUN_STR_P_RATIO:get()) + end + + -- set I gain + if RTUN_STR_I_RATIO:get() > 0 then + local iname = string.gsub(ff_pname, "_FF", "_I") + adjust_gain(iname, FF_new_gain * RTUN_STR_I_RATIO:get()) + end + return true end return false end +-- initialise speed ff tuning +function init_speed_ff() + ff_throttle_sum = 0 + ff_speed_sum = 0 + ff_speed_count = 0 +end + -- run speed controller feedforward calibration -function update_speed_ff(pname) +-- ff_pname is the FF parameter being tuned +-- returns true once the tuning has completed +function update_speed_ff(ff_pname) -- get steering, turn rate, throttle and speed local _, throttle_out = vehicle:get_steering_and_throttle() local velocity_ned = ahrs:get_velocity_NED() @@ -564,13 +461,13 @@ function update_speed_ff(pname) ff_speed_sum = ff_speed_sum + speed ff_speed_count = ff_speed_count + 1 if (update_user) then - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", pname, complete_pct)) + gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.0f%% complete", ff_pname, complete_pct)) end else if update_user then if not throttle_ok then gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase throttle (%d < %d)", math.floor(throttle_out * 100), math.floor(SPEED_FF_THROTTLE_MIN * 100))) - elseif not turnrate_ok then + elseif not speed_ok then gcs:send_text(MAV_SEVERITY.WARNING, string.format("RTun: increase speed (%3.1f < %3.1f)", speed, SPEED_FF_SPEED_MIN)) end end @@ -578,26 +475,28 @@ function update_speed_ff(pname) -- check for 10 seconds of data if complete_pct >= 100 then - local cruise_speed_old = params["CRUISE_SPEED"]:get() local cruise_speed_new = ff_speed_sum / ff_speed_count - local cruise_throttle_old = params["CRUISE_THROTTLE"]:get() - local cruise_throttle_new = (ff_throttle_sum / ff_speed_count) * 100 + local cruise_throttle_new = (ff_throttle_sum / ff_speed_count) * 100 * RTUN_SPD_FFRATIO:get() adjust_gain("CRUISE_SPEED", cruise_speed_new) adjust_gain("CRUISE_THROTTLE", cruise_throttle_new) - write_log("CRUISE_SPEED") - write_log("CRUISE_THROTTLE") - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.2f -> %.2f", "CRUISE_SPEED", cruise_speed_old, cruise_speed_new)) - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.1f -> %.1f", "CRUISE_THROTTLE", cruise_throttle_old, cruise_throttle_new)) - - -- set I gain equal to FF - local iname = string.gsub(pname, "_FF", "_I") - local I = params[iname] - local I_old_gain = I:get() - local I_new_gain = ff_throttle_sum / ff_speed_sum - I:set(I_new_gain) - param_changed[iname] = true - write_log(iname) - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusted %s %.3f -> %.3f", iname, I_old_gain, I_new_gain)) + + -- calculate FF equivalent gain (used for setting P and I below) + local speed_ff_equivalent = (ff_throttle_sum / ff_speed_sum) * RTUN_SPD_FFRATIO:get(); + + -- set P gain + if RTUN_SPD_P_RATIO:get() > 0 then + local pname = string.gsub(ff_pname, "_FF", "_P") + local P_new_gain = speed_ff_equivalent * RTUN_SPD_P_RATIO:get() + adjust_gain(pname, P_new_gain) + end + + -- set I gain + if RTUN_SPD_I_RATIO:get() > 0 then + local iname = string.gsub(ff_pname, "_FF", "_I") + local I_new_gain = speed_ff_equivalent * RTUN_SPD_I_RATIO:get() + adjust_gain(iname, I_new_gain) + end + return true end @@ -663,11 +562,8 @@ function update() return end - -- update param gains for params being slewed towards a target - update_slew_gain() - -- return if we have just changed stages to give time for oscillations to subside - if get_time() - last_stage_change < STAGE_DELAY then + if get_time() - last_axis_change < AXIS_CHANGE_DELAY then return end @@ -710,70 +606,21 @@ function update() end -- get parameter currently being tuned - local srate = get_slew_rate(axis) - local pname = axis .. "_" .. stage - local param = params[pname] + local pname = axis .. "_FF" - if stage == "FF" then - -- feedforward tuning - local ff_done - if axis == "ATC_STR_RAT" then - ff_done = update_steering_ff(pname) - elseif axis == "ATC_SPEED" then - ff_done = update_speed_ff(pname) - elseif axis == "PSC_VEL" then - -- position controller feed-forward is not tuned - ff_done = true - else - gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: unsupported FF tuning %s", pname)) - ff_done = true - end - if ff_done then - gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", pname)) - advance_stage(axis) - end + -- feedforward tuning + local ff_done + if axis == "ATC_STR_RAT" then + ff_done = update_steering_ff(pname) + elseif axis == "ATC_SPEED" then + ff_done = update_speed_ff(pname) else - local oscillating = srate > RTUN_OSC_SMAX:get() - local limited = reached_limit(pname, param:get()) - if limited or oscillating then - local reduction = (100.0-RTUN_PD_GAINMARG:get())*0.01 - if not oscillating then - reduction = 1.0 - end - local new_gain = param:get() * reduction - local limit = gain_limit(pname) - if limit > 0.0 and new_gain > limit then - gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s passed limit (>%.3f)", pname, limit)) - new_gain = limit - end - local old_gain = param_saved[pname] - if new_gain < old_gain and string.sub(pname,-2) == '_D' then - -- we are lowering a D gain from the original gain. Also lower the P gain by the same amount - -- so that we don't trigger P oscillation. We don't drop P by more than a factor of 2 - local ratio = math.max(new_gain / old_gain, 0.5) - local P_name = string.gsub(pname, "_D", "_P") - local old_P = params[P_name]:get() - local new_P = old_P * ratio - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: adjusting %s %.3f -> %.3f", P_name, old_P, new_P)) - adjust_gain(P_name, new_P) - write_log(P_name) - end - -- slew gain change over 2 seconds to ease impact on controller - setup_slew_gain(pname, new_gain) - gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done %.3f", pname, new_gain)) - advance_stage(axis) - else - local new_gain = param:get()*get_gain_mul() - if new_gain <= 0.0001 then - new_gain = 0.001 - end - adjust_gain(pname, new_gain) - write_log(pname) - if get_time() - last_gain_report > 3 then - last_gain_report = get_time() - gcs:send_text(MAV_SEVERITY.INFO, string.format("RTun: %s %.3f sr:%.2f", pname, new_gain, srate)) - end - end + gcs:send_text(MAV_SEVERITY.CRITICAL, string.format("RTun: unsupported FF tuning %s", pname)) + ff_done = true + end + if ff_done then + gcs:send_text(MAV_SEVERITY.NOTICE, string.format("RTun: %s tuning done", pname)) + advance_axis(axis) end end diff --git a/libraries/AP_Scripting/applets/rover-quicktune.md b/libraries/AP_Scripting/applets/rover-quicktune.md index 21dc1f647e01da..e80f82a4b65a98 100644 --- a/libraries/AP_Scripting/applets/rover-quicktune.md +++ b/libraries/AP_Scripting/applets/rover-quicktune.md @@ -1,6 +1,6 @@ # Rover QuickTune -Rover QuickTune tunes the steering (aka turn rate), speed and position controller velocity gains for rovers and boats +Rover QuickTune tunes the steering (aka turn rate) and speed controller gains for rovers and boats The script is designed to be used in Circle mode and updates the following parameters @@ -15,14 +15,11 @@ ATC_SPEED_I ATC_SPEED_D CRUISE_SPEED CRUISE_THROTTLE -PSC_VEL_P -PSC_VEL_I -PSC_VEL_D # How To Use Install this script in the autopilot's SD card's APM/scripts directory Set SCR_ENABLE to 1 and reboot the autopilot -Set RTUN_ENABLE to 1. +Set RTUN_ENABLE to 1 (default) Set RCx_OPTION = 300 where "x" refers to the transmitter's 2 or 3 position switch use to start/stop/save tuning. E.g. if channel 6 is used set RC6_OPTION = 300 @@ -30,7 +27,7 @@ use to start/stop/save tuning. E.g. if channel 6 is used set RC6_OPTION = 300 If necessary, the RTUN_RC_FUNC parameter can be set to another number (e.g. 302 for scripting3) to avoid RCx_OPTION conflicts with other scripts. -If only a 2-position switch is available set RTUN_AUTO_SAVE to 1 +By default the tune is saved a few seconds after completion. To control saving of the tune manually set RTUN_AUTO_SAVE to 0 Arm the vehicle and switch to Circle mode Optionally set CIRC_SPEED (or WP_SPEED) to about half the vehicle's max speed @@ -40,23 +37,15 @@ Note the above parmaters only take effect when the vehicle is switched into Circ Move the RC switch to the middle position to begin the tuning process. Text messages should appear on the ground station's messages area showing progress -For P and D gain tuning, the relevant gain is slowly raised until the vehicle begins to oscillate after which the gain is reduced and the script moves onto the next gain. - -For FF tuning, the steering or throttle output are compared to the response for at least 10 seconds. +During tuning the steering or throttle output are compared to the response for at least 10 seconds. A message may appear stating the steering, turn rate, throttle or speed are too low in which case the vehicle speed should be increased (Mission Planner's Action tab "Change Speed" button may be used) -or the radius should be reduced. The velocity FF is not tuned (nor does it need to be). +or the radius should be reduced. By default the gains will be tuned in this order: - - ATC_STR_RAT_D - - ATC_STR_RAT_P and I - - ATC_STR_RAT_FF - - ATC_SPEED_D - - ATC_SPEED_P - - CRUISE_SPEED and CRUISE_THROTTLE - - PSC_VEL_D - - PSC_VEL_P and I +- ATC_STR_RAT_FF, then ATC_STR_RAT_P and I are set to ratios of the FF +- CRUISE_SPEED and CRUISE_THROTTLE, then ATC_SPEED_P and I are set to ratios of the FF The script will also adjust filter settings: @@ -67,7 +56,7 @@ Save the tune by raising the RC switch to the high position If the RC switch is moved high (ie. Save Tune) before the tune is completed the tune will pause, and any parameters completed will be saved and the current value of the one being actively tuned will remain active. You can resume tuning by returning the switch again to the middle position. If the RC switch is moved to the low position, the parameter currently being tuned will be reverted but any previously saved parameters will remain. -If you move the switch to the low position at any time in the tune before using the Tune Save switch position, then all parameters will be reverted to their original values. Parameters will also be reverted if you disarm before saving. +If you move the switch to the low position at any time in the tune before gains are saved, then all parameters will be reverted to their original values. Parameters will also be reverted if you disarm before saving. If the pilot gives steering or throttle input during tuning then tuning is paused for 4 seconds. Tuning restarts once the pilot returns to the input to the neutral position. @@ -86,50 +75,38 @@ By default RCx_OPTIONS of 300 (scripting1) is used ## RTUN_AXES -The axes that will be tuned. The default is 7 meaning steering, speed and velocity -This parameter is a bitmask, so set 1 to tune just steering. 2 for speed. 4 for velocity - -## RTUN_DOUBLE_TIME - -How quickly a gain is raised while tuning. This is the number of seconds -for the gain to double. Most users will want to leave this at the default of 10 seconds. - -## RTUN_PD_GAINMARG - -The percentage P and D gain margin to use. Once the oscillation point is found -the gain is reduced by this percentage. The default of 80% is good for most users. +The axes that will be tuned. The default is 3 meaning steering and speed +This parameter is a bitmask, so set 1 to tune just steering. 2 for just speed -## RTUN_FF_GAINMARG +## RTUN_STR_FFRATIO -The percentage FF gain margin to use. Once the output and response are used to calculate -the ideal feed-forward, the value is reduced by this percentage. The default of 20% is good for most users. +Ratio between measured response and FF gain. Raise this to get a higher FF gain +The default of 0.9 is good for most users. -## RTUN_OSC_SMAX +## RTUN_STR_P_RATIO -The Oscillation threshold in Hertz for detecting oscillation -The default of 5Hz is good for most vehicles but on very large vehicles -you may wish to lower this. For a vehicle of 50kg a value of 3 is likely to be good. -For a vehicle of 100kg a value of 1.5 is likely to be good. +Ratio between steering FF and P gains. Raise this to get a higher P gain, 0 to leave P unchanged +The default of 0.2 is good for most users. -You can tell you have this set too high if you still have visible -oscillations after a parameter has completed tuning. In that case -halve this parameter and try again. +## RTUN_STR_I_RATIO -## RTUN_SPD_P_MAX +Ratio between steering FF and I gains. Raise this to get a higher I gain, 0 to leave I unchanged +The default of 0.2 is good for most users. -The speed controller P gain max (aka ATC_SPEED_P) +## RTUN_SPD_FFRATIO -## RTUN_SPD_D_MAX +Ratio between measured response and CRUISE_THROTTLE value. Raise this to get a higher CRUISE_THROTTLE value +The default of 1.0 is good for most users. -The speed controller D gain max (aka ATC_SPEED_D) +## RTUN_SPD_P_RATIO -## RTUN_PI_RATIO +Ratio between speed FF and P gain. Raise this to get a higher P gain, 0 to leave P unchanged +The default of 1.0 is good for most users. -The ratio for P to I. This should normally be 1, but on some large vehicles a value of up to 3 can be -used if the I term in the PID is causing too much phase lag. +## RTUN_SPD_I_RATIO -If RTUN_RP_PI_RATIO is less than 1 then the I value will not be -changed at all when P is changed. +Ratio between speed FF and I gain. Raise this to get a higher I gain, 0 to leave I unchanged +The default of 1.0 is good for most users. ## RTUN_AUTO_FILTER diff --git a/libraries/AP_Scripting/applets/runcam_on_arm.lua b/libraries/AP_Scripting/applets/runcam_on_arm.lua index 8f99b0a289af8a..6a1924d36b848a 100644 --- a/libraries/AP_Scripting/applets/runcam_on_arm.lua +++ b/libraries/AP_Scripting/applets/runcam_on_arm.lua @@ -19,7 +19,6 @@ -- presses, I want the script to be responsive and start recording as -- soon as the vehicle arms, so there I use a shorter delay. --- luacheck: only 0 -- constants @@ -46,7 +45,6 @@ function update() local is_armed = arming:is_armed() local delay = DELAY_SHORT - if is_armed ~= prev_armed then -- a state transition has occurred if is_armed then @@ -57,8 +55,6 @@ function update() rc:run_aux_function(RC_OPTION.RunCamControl, AuxSwitchPos.LOW) end delay = DELAY_LONG - else - delay = DELAY_SHORT end prev_armed = is_armed diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 40ddb7ac9c35fa..05571a172ec80e 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -452,6 +452,82 @@ function motor_factor_table_ud:roll(index) end ---@param value number function motor_factor_table_ud:roll(index, value) end +-- network socket class +---@class SocketAPM_ud +local SocketAPM_ud = {} + +-- desc +function Socket(param1) end + +-- return true if a socket is connected +---@return boolean +function SocketAPM_ud:is_connected() end + +-- set blocking state of socket +---@param blocking boolean +---@return boolean +function SocketAPM_ud:set_blocking(blocking) end + +-- setup a socket to listen +---@param backlog integer +---@return boolean +function SocketAPM_ud:listen(backlog) end + +-- send a lua string. May contain binary data +---@param str string +---@param len uint32_t_ud +---@return integer +function SocketAPM_ud:send(str, len) end + +-- bind to an address. Use "0.0.0.0" for wildcard bind +---@param IP_address string +---@param port integer +---@return boolean +function SocketAPM_ud:bind(IP_address, port) end + +-- connect a socket to an endpoint +---@param IP_address string +---@param port integer +---@return boolean +function SocketAPM_ud:connect(IP_address, port) end + +--[[ accept new incoming sockets, returning a new socket. + Must be used on a stream socket in listen state +--]] +function SocketAPM_ud:accept(param1) end + +-- receive data from a socket +---@param length +---@return data +function SocketAPM_ud:recv(length) end + +-- check for available input +---@param timeout_ms uint32_t_ud +---@return boolean +function SocketAPM_ud:pollin(timeout_ms) end + +-- check for availability of space to write to socket +---@param timeout_ms uint32_t_ud +---@return boolean +function SocketAPM_ud:pollout(timeout_ms) end + +--[[ + close a socket. Note that there is no automatic garbage + collection of sockets so you must close a socket when you are + finished with it or you will run out of sockets +--]] +function SocketAPM_ud:close() end + +--[[ + setup to send all remaining data from a filehandle to the socket + this also "closes" the socket and the file from the point of view of lua + the underlying socket and file are both closed on end of file +--]] +function SocketAPM_ud:sendfile(filehandle) end + +-- enable SO_REUSEADDR on a socket +---@return boolean +function SocketAPM_ud:reuseaddress() end -- desc ---@class AP_HAL__PWMSource_ud @@ -981,6 +1057,14 @@ local ScriptingCANBuffer_ud = {} ---@return CANFrame_ud|nil function ScriptingCANBuffer_ud:read_frame() end +-- Add a filter to the CAN buffer, mask is bitwise ANDed with the frame id and compared to value if not match frame is not buffered +-- By default no filters are added and all frames are buffered, write is not affected by filters +-- Maximum number of filters is 8 +---@param mask uint32_t_ud +---@param value uint32_t_ud +---@return boolean -- returns true if the filler was added successfully +function ScriptingCANBuffer_ud:add_filter(mask, value) end + -- desc ---@param frame CANFrame_ud ---@param timeout_us uint32_t_ud @@ -1064,6 +1148,14 @@ function AP_HAL__UARTDriver_ud:read() end ---@param baud_rate uint32_t_ud function AP_HAL__UARTDriver_ud:begin(baud_rate) end +--[[ + read count bytes from a uart and return as a lua string. Note + that the returned string can be shorter than the requested length +--]] +---@param count integer +---@return string|nil +function AP_HAL__UARTDriver_ud:readstring(count) end + -- desc ---@class RC_Channel_ud @@ -1707,6 +1799,14 @@ function mission:get_index_of_jump_tag(tag) end function mission:get_last_jump_tag() end +-- Jump the mission to the start of the closest landing sequence. Returns true if one was found +---@return boolean +function mission:jump_to_landing_sequence() end + +-- Jump to the landing abort sequence +-- @return boolean +function mission:jump_to_abort_landing_sequence() end + -- desc ---@class param param = {} @@ -2425,14 +2525,71 @@ function terrain:status() end ---@return boolean function terrain:enabled() end + +-- RangeFinder state structure +---@class RangeFinder_State_ud +local RangeFinder_State_ud = {} + +---@return RangeFinder_State_ud +function RangeFinder_State() end + +-- get system time (ms) of last successful update from sensor +---@return number +function RangeFinder_State_ud:last_reading() end + +-- set system time (ms) of last successful update from sensor +---@param value number +function RangeFinder_State_ud:last_reading(value) end + +-- get sensor status +---@return number +function RangeFinder_State_ud:status() end + +-- set sensor status +---@param value number +function RangeFinder_State_ud:status(value) end + +-- get number of consecutive valid readings (max out at 10) +---@return number +function RangeFinder_State_ud:range_valid_count() end + +-- set number of consecutive valid readings (max out at 10) +---@param value number +function RangeFinder_State_ud:range_valid_count(value) end + +-- get distance in meters +---@return number +function RangeFinder_State_ud:distance() end + +-- set distance in meters +---@param value number +function RangeFinder_State_ud:distance(value) end + +-- get measurement quality in percent 0-100, -1 -> 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 +2607,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 +2660,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 @@ -2699,11 +2870,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 +3140,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 +3302,89 @@ 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 + +-- 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/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/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..909defc96aa622 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,6 +76,19 @@ 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 method num_instances uint8_t @@ -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 @@ -198,18 +214,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 +387,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 +484,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 +571,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 @@ -602,7 +654,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) @@ -814,3 +866,33 @@ 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 + +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 94850cb69d4dd7..b1b60a29c8a711 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/libraries/AP_Scripting/lua_scripts.cpp @@ -36,6 +36,10 @@ HAL_Semaphore lua_scripts::error_msg_buf_sem; uint8_t lua_scripts::print_error_count; uint32_t lua_scripts::last_print_ms; +uint32_t lua_scripts::loaded_checksum; +uint32_t lua_scripts::running_checksum; +HAL_Semaphore lua_scripts::crc_sem; + lua_scripts::lua_scripts(const AP_Int32 &vm_steps, const AP_Int32 &heap_size, const AP_Int8 &debug_options, struct AP_Scripting::terminal_s &_terminal) : _vm_steps(vm_steps), _debug_options(debug_options), @@ -199,6 +203,19 @@ lua_scripts::script_info *lua_scripts::load_script(lua_State *L, char *filename) new_script->lua_ref = luaL_ref(L, LUA_REGISTRYINDEX); // cache the reference new_script->next_run_ms = AP_HAL::millis64() - 1; // force the script to be stale + // Get checksum of file + uint32_t crc = 0; + if (AP::FS().crc32(filename, crc)) { + // Record crc of this script + new_script->crc = crc; + { + // Apply crc to checksum of all scripts + WITH_SEMAPHORE(crc_sem); + loaded_checksum ^= crc; + running_checksum ^= crc; + } + } + return new_script; } @@ -385,6 +402,12 @@ 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); @@ -606,4 +629,17 @@ void lua_scripts::run(void) { error_msg_buf_sem.give(); } +// Return the file checksums of running and loaded scripts +uint32_t lua_scripts::get_loaded_checksum() +{ + WITH_SEMAPHORE(crc_sem); + return loaded_checksum; +} + +uint32_t lua_scripts::get_running_checksum() +{ + WITH_SEMAPHORE(crc_sem); + return running_checksum; +} + #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/lua_scripts.h b/libraries/AP_Scripting/lua_scripts.h index b3201c539490fb..04001f39bfb605 100644 --- a/libraries/AP_Scripting/lua_scripts.h +++ b/libraries/AP_Scripting/lua_scripts.h @@ -54,6 +54,7 @@ class lua_scripts SUPPRESS_SCRIPT_LOG = 1U << 2, LOG_RUNTIME = 1U << 3, DISABLE_PRE_ARM = 1U << 4, + SAVE_CHECKSUM = 1U << 5, }; private: @@ -64,6 +65,7 @@ class lua_scripts typedef struct script_info { int lua_ref; // reference to the loaded script object uint64_t next_run_ms; // time (in milliseconds) the script should next be run at + uint32_t crc; // crc32 checksum char *name; // filename for the script // FIXME: This information should be available from Lua script_info *next; } script_info; @@ -125,6 +127,11 @@ class lua_scripts static uint32_t last_print_ms; int current_ref; + // XOR of crc32 of running scripts + static uint32_t loaded_checksum; + static uint32_t running_checksum; + static HAL_Semaphore crc_sem; + public: // must be static for use in atpanic, public to allow bindings to issue none fatal warnings static void set_and_print_new_error_message(MAV_SEVERITY severity, const char *fmt, ...) FMT_PRINTF(2,3); @@ -135,6 +142,10 @@ class lua_scripts // get semaphore for above error buffer static AP_HAL::Semaphore* get_last_error_semaphore() { return &error_msg_buf_sem; } + // Return the file checksums of running and loaded scripts + static uint32_t get_loaded_checksum(); + static uint32_t get_running_checksum(); + }; #endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/tests/mavlink_test.lua b/libraries/AP_Scripting/tests/mavlink_test.lua index 3d1e769e218172..cc2a062bdd116c 100644 --- a/libraries/AP_Scripting/tests/mavlink_test.lua +++ b/libraries/AP_Scripting/tests/mavlink_test.lua @@ -10,14 +10,6 @@ mavlink.init(1, 10) -- register message id to receive mavlink.register_rx_msgid(heartbeat_msgid) local test_named_value = 0.0 -function str_to_bytes(str) - local str_len = string.len(str) - local bytes = {} - for i = 1, str_len do - bytes[i] = string.byte(str, i) - end - return bytes -end function update() local msg,chan,timestamp_ms = mavlink.receive_chan() if msg then @@ -31,7 +23,7 @@ function update() end test_named_value = test_named_value + 1.0 -- send named value float - mavlink.send_chan(0, mavlink_msgs.encode("NAMED_VALUE_FLOAT", {time_boot_ms = millis():toint(), name = str_to_bytes("test"), value = test_named_value})) + mavlink.send_chan(0, mavlink_msgs.encode("NAMED_VALUE_FLOAT", {time_boot_ms = millis():toint(), name = "test", value = test_named_value})) return update, 1000 end diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 38b8124aaf6fb2..3d2ed06f5a061d 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -24,40 +24,10 @@ #include #include #include +#include #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 @@ -209,7 +179,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Param: 1_PROTOCOL // @DisplayName: Telem1 protocol selection // @Description: Control what protocol to use on the Telem1 port. Note that the Frsky options require external converter hardware. See the wiki for details. - // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:Gimbal, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:EFI Serial, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency, 44:IRC Tramp, 45:DDS XRCE + // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:Gimbal, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out, 15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo, 20:NMEA Output, 21:WindVane, 22:SLCAN, 23:RCIN, 24:EFI Serial, 25:LTM, 26:RunCam, 27:HottTelem, 28:Scripting, 29:Crossfire VTX, 30:Generator, 31:Winch, 32:MSP, 33:DJI FPV, 34:AirSpeed, 35:ADSB, 36:AHRS, 37:SmartAudio, 38:FETtecOneWire, 39:Torqeedo, 40:AIS, 41:CoDevESC, 42:DisplayPort, 43:MAVLink High Latency, 44:IRC Tramp, 45:DDS XRCE, 46:IMUDATA // @User: Standard // @RebootRequired: True AP_GROUPINFO("1_PROTOCOL", 1, AP_SerialManager, state[1].protocol, DEFAULT_SERIAL1_PROTOCOL), @@ -600,6 +570,24 @@ void AP_SerialManager::init() // Note init is handled by AP_MSP break; #endif + +#if AP_SERIALMANAGER_IMUOUT_ENABLED + case SerialProtocol_IMUOUT: + uart->begin(state[i].baudrate(), + AP_SERIALMANAGER_IMUOUT_BUFSIZE_RX, + AP_SERIALMANAGER_IMUOUT_BUFSIZE_TX); + AP::ins().set_imu_out_uart(uart); + 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 01d5d7bbd45640..d569d1f066d43a 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -79,6 +79,9 @@ class AP_SerialManager { SerialProtocol_MAVLinkHL = 43, 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 7dbf0c16a8a0cd..a09a2995a09f2e 100644 --- a/libraries/AP_SerialManager/AP_SerialManager_config.h +++ b/libraries/AP_SerialManager/AP_SerialManager_config.h @@ -21,13 +21,13 @@ #include #include +#include #ifdef HAL_UART_NUM_SERIAL_PORTS #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 @@ -56,6 +56,10 @@ #define AP_SERIALMANAGER_REGISTER_ENABLED BOARD_FLASH_SIZE > 1024 && (AP_NETWORKING_ENABLED || HAL_ENABLE_DRONECAN_DRIVERS) #endif +#ifndef AP_SERIALMANAGER_IMUOUT_ENABLED +#define AP_SERIALMANAGER_IMUOUT_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && AP_INERTIALSENSOR_ENABLED +#endif + // serial ports registered by AP_Networking will use IDs starting at 21 for the first port #define AP_SERIALMANAGER_NET_PORT_1 21 // NET_P1_* @@ -127,3 +131,44 @@ #define AP_SERIALMANAGER_MSP_BUFSIZE_RX 128 #define AP_SERIALMANAGER_MSP_BUFSIZE_TX 256 #define AP_SERIALMANAGER_MSP_BAUD 115200 + +// IMU OUT protocol +#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..0de442dc7faaad 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,7 +866,7 @@ 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); } // logging diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 53e61652848043..47279b5d7b5149 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -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}; diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index a7a59f789e1623..835e32ea71d0a3 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -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; diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index f5892d68585728..7ca36846257b06 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -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 = 0.01f * (float)aparm.airspeed_cruise_cm * _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 @@ -848,7 +856,7 @@ 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); + _pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT); const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(0.01f * (float)aparm.pitch_trim_cd); const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf; @@ -1099,7 +1107,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 +1138,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 +1159,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 +1171,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; } 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_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 0635dc8368cf97..c94a4c2ee1b5a0 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 @@ -22,6 +26,7 @@ #include extern AP_IOMCU iomcu; #endif +#include #define SCHED_TASK(func, rate_hz, max_time_micros, prio) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros, prio) @@ -313,16 +318,16 @@ void AP_Vehicle::setup() gcs().init(); #endif -#if AP_NETWORKING_ENABLED - networking.init(); -#endif - // initialise serial ports serial_manager.init(); #if HAL_GCS_ENABLED gcs().setup_console(); #endif +#if AP_NETWORKING_ENABLED + networking.init(); +#endif + // Register scheduler_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(scheduler_delay_callback, 5); @@ -562,9 +567,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { #if HAL_EFI_ENABLED SCHED_TASK_CLASS(AP_EFI, &vehicle.efi, update, 50, 200, 250), #endif -#if HAL_INS_ACCELCAL_ENABLED SCHED_TASK(one_Hz_update, 1, 100, 252), -#endif #if HAL_WITH_ESC_TELEM && HAL_GYROFFT_ENABLED SCHED_TASK(check_motor_noise, 5, 50, 252), #endif @@ -847,6 +850,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; @@ -869,13 +873,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 @@ -944,6 +949,14 @@ void AP_Vehicle::one_Hz_update(void) } #endif } + +#if AP_SCRIPTING_ENABLED + AP_Scripting *scripting = AP_Scripting::get_singleton(); + if (scripting != nullptr) { + scripting->update(); + } +#endif + } void AP_Vehicle::check_motor_noise() @@ -1022,3 +1035,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..b8d98d2377d519 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 @@ -151,12 +156,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; } @@ -301,7 +309,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 @@ -487,3 +498,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..5519f86d452d8c 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) { @@ -69,6 +70,7 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa position_delta, packet.confidence); } +#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..ce2033ca4ce2f2 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h @@ -27,8 +27,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; diff --git a/libraries/AP_WindVane/AP_WindVane.cpp b/libraries/AP_WindVane/AP_WindVane.cpp index cd541779016eec..44845d4d469e87 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; } @@ -377,7 +395,7 @@ void AP_WindVane::update() 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 +454,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 +482,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 +494,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..f84d0a815d4e9c 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 @@ -29,3 +33,5 @@ class AP_WindVane_SITL : public AP_WindVane_Backend void update_speed() override; #endif }; + +#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 5bb429ef0564f1..90b8ef99157125 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,17 +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(!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() && + !is_omni()) { + if (report) { + 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; } @@ -476,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; } @@ -601,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); } } } @@ -906,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); @@ -918,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 afb6f4956cc685..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 @@ -505,6 +506,10 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) { _cross_track_error = calc_crosstrack_error(current_loc); + // update position controller + _pos_control.set_reversed(_reversed); + _pos_control.update(dt); + // handle pivot turns if (_pivot.active()) { // decelerate to zero @@ -512,14 +517,11 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd(); _desired_turn_rate_rads = is_zero(_desired_speed_limited) ? _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt) : 0; _desired_lat_accel = 0.0f; - return; + } else { + _desired_speed_limited = _pos_control.get_desired_speed(); + _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads(); + _desired_lat_accel = _pos_control.get_desired_lat_accel(); } - - _pos_control.set_reversed(_reversed); - _pos_control.update(dt); - _desired_speed_limited = _pos_control.get_desired_speed(); - _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads(); - _desired_lat_accel = _pos_control.get_desired_lat_accel(); } // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) 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/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp index 82ae789ba5d97a..5a35ea5b7416d2 100644 --- a/libraries/Filter/HarmonicNotchFilter.cpp +++ b/libraries/Filter/HarmonicNotchFilter.cpp @@ -20,9 +20,21 @@ #include "HarmonicNotchFilter.h" #include +#include +#include +#include +#include #define HNF_MAX_FILTERS HAL_HNF_MAX_FILTERS // must be even for double-notch filters +/* + optional logging for SITL only of all notch frequencies + */ +#ifndef NOTCH_DEBUG_LOGGING +#define NOTCH_DEBUG_LOGGING 0 +#endif + + // table of user settable parameters const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = { @@ -177,13 +189,8 @@ void HarmonicNotchFilter::allocate_filters(uint8_t num_notches, uint32_t harm expand the number of filters at runtime, allowing for RPM sources such as lua scripts */ template -void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) +void HarmonicNotchFilter::expand_filter_count(uint16_t total_notches) { - uint8_t num_filters = _num_harmonics * num_notches * _composite_notches; - if (num_filters <= _num_filters) { - // enough already - return; - } if (_alloc_has_failed) { // we've failed to allocate before, don't try again return; @@ -192,7 +199,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) note that we rely on the semaphore in AP_InertialSensor_Backend.cpp to make this thread safe */ - auto filters = new NotchFilter[num_filters]; + auto filters = new NotchFilter[total_notches]; if (filters == nullptr) { _alloc_has_failed = true; return; @@ -200,7 +207,7 @@ void HarmonicNotchFilter::expand_filter_count(uint8_t num_notches) memcpy(filters, _filters, sizeof(filters[0])*_num_filters); auto _old_filters = _filters; _filters = filters; - _num_filters = num_filters; + _num_filters = total_notches; delete[] _old_filters; } @@ -217,7 +224,7 @@ void HarmonicNotchFilter::update(float center_freq_hz) // adjust the fundamental center frequency to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - center_freq_hz = constrain_float(center_freq_hz, 1.0f, nyquist_limit); + center_freq_hz = constrain_float(center_freq_hz, 0.0f, nyquist_limit); _num_enabled_filters = 0; // update all of the filters using the new center frequency and existing A & Q @@ -261,15 +268,16 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq // adjust the frequencies to be in the allowable range const float nyquist_limit = _sample_freq_hz * 0.48f; - if (num_centers > _num_filters) { + uint16_t total_notches = num_centers * _num_harmonics * _composite_notches; + if (total_notches > _num_filters) { // alloc realloc of filters - expand_filter_count(num_centers); + expand_filter_count(total_notches); } _num_enabled_filters = 0; // update all of the filters using the new center frequencies and existing A & Q - for (uint8_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { + for (uint16_t i = 0; i < num_centers * HNF_MAX_HARMONICS && _num_enabled_filters < _num_filters; i++) { const uint8_t harmonic_n = i / num_centers; const uint8_t center_n = i % num_centers; // the filters are ordered by center and then harmonic so @@ -278,7 +286,7 @@ void HarmonicNotchFilter::update(uint8_t num_centers, const float center_freq continue; } - const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 1.0f, nyquist_limit); + const float notch_center = constrain_float(center_freq_hz[center_n] * (harmonic_n+1), 0.0f, nyquist_limit); if (_composite_notches != 2) { // only enable the filter if its center frequency is below the nyquist frequency if (notch_center < nyquist_limit) { @@ -311,10 +319,29 @@ T HarmonicNotchFilter::apply(const T &sample) return sample; } +#if NOTCH_DEBUG_LOGGING + static int dfd = -1; + if (dfd == -1) { + dfd = ::open("notch.txt", O_WRONLY|O_CREAT|O_TRUNC, 0644); + } +#endif + T output = sample; - for (uint8_t i = 0; i < _num_enabled_filters; i++) { + for (uint16_t i = 0; i < _num_enabled_filters; i++) { +#if NOTCH_DEBUG_LOGGING + if (!_filters[i].initialised) { + ::dprintf(dfd, "------- "); + } else { + ::dprintf(dfd, "%.4f ", _filters[i]._center_freq_hz); + } +#endif output = _filters[i].apply(output); } +#if NOTCH_DEBUG_LOGGING + if (_num_enabled_filters > 0) { + ::dprintf(dfd, "\n"); + } +#endif return output; } @@ -328,7 +355,7 @@ void HarmonicNotchFilter::reset() return; } - for (uint8_t i = 0; i < _num_filters; i++) { + for (uint16_t i = 0; i < _num_filters; i++) { _filters[i].reset(); } } diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h index 7de4df799df4e7..898857b82e58ec 100644 --- a/libraries/Filter/HarmonicNotchFilter.h +++ b/libraries/Filter/HarmonicNotchFilter.h @@ -32,7 +32,7 @@ class HarmonicNotchFilter { // allocate a bank of notch filters for this harmonic notch filter void allocate_filters(uint8_t num_notches, uint32_t harmonics, uint8_t composite_notches); // expand filter bank with new filters - void expand_filter_count(uint8_t num_notches); + void expand_filter_count(uint16_t total_notches); // initialize the underlying filters using the provided filter parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); // update the underlying filters' center frequencies using center_freq_hz as the fundamental @@ -60,11 +60,11 @@ class HarmonicNotchFilter { // number of notches that make up a composite notch uint8_t _composite_notches; // number of allocated filters - uint8_t _num_filters; + uint16_t _num_filters; // pre-calculated number of harmonics uint8_t _num_harmonics; // number of enabled filters - uint8_t _num_enabled_filters; + uint16_t _num_enabled_filters; bool _initialised; // have we failed to expand filters? diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index 14ce778efa4cf2..fa2c024721be91 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -71,7 +71,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h _center_freq_hz * NOTCH_MAX_SLEW_UPPER); } - if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { + if (is_positive(new_center_freq) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz; float alpha = sinf(omega) / (2 * Q); b0 = 1.0 + alpha*sq(A); diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h index 476d06d79b3cc2..624929208e1551 100644 --- a/libraries/Filter/NotchFilter.h +++ b/libraries/Filter/NotchFilter.h @@ -26,9 +26,13 @@ #include +template +class HarmonicNotchFilter; + template class NotchFilter { public: + friend class HarmonicNotchFilter; // set parameters void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB); void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q); diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 867ec889031087..d477ea2977365f 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -643,9 +643,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 +658,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 +729,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); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 053e7294aaeeeb..63e2424eb9539d 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 @@ -652,7 +658,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, @@ -1993,7 +2003,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 +2039,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 +2083,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 +2097,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; } @@ -2587,6 +2609,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 +2619,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 @@ -3111,10 +3135,11 @@ float GCS_MAVLINK::vfr_hud_airspeed() const float GCS_MAVLINK::vfr_hud_climbrate() const { Vector3f velned; - if (!AP::ahrs().get_velocity_NED(velned)) { - velned.zero(); + if (AP::ahrs().get_velocity_NED(velned) || + AP::ahrs().get_vert_pos_rate_D(velned.z)) { + return -velned.z; } - return -velned.z; + return 0; } float GCS_MAVLINK::vfr_hud_alt() const @@ -3268,7 +3293,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; } @@ -4469,6 +4498,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 @@ -4527,6 +4557,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) @@ -4729,6 +4760,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. @@ -4773,49 +4847,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) @@ -4881,6 +4912,26 @@ void GCS_MAVLINK::handle_command_long(const mavlink_message_t &msg) 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) @@ -5068,11 +5119,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); @@ -5256,6 +5309,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); @@ -5273,6 +5327,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); @@ -5388,9 +5443,9 @@ void GCS_MAVLINK::send_attitude() const 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); @@ -5612,10 +5667,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; @@ -6636,6 +6693,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(); @@ -6650,6 +6708,7 @@ uint8_t GCS_MAVLINK::receiver_rssi() const // scale across the full valid range return aprssi->read_receiver_rssi() * 254; } +#endif GCS &gcs() { @@ -6669,11 +6728,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; diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 71eef91f427123..53ceca24ea2c07 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -444,26 +444,12 @@ void GCS_MAVLINK::ftp_worker(void) { request.data[sizeof(request.data) - 1] = 0; // ensure the path is null terminated - // actually open the file - int fd = AP::FS().open((char *)request.data, O_RDONLY); - if (fd == -1) { + uint32_t checksum = 0; + if (!AP::FS().crc32((char *)request.data, checksum)) { ftp_error(reply, FTP_ERROR::FailErrno); break; } - uint32_t checksum = 0; - ssize_t read_size; - do { - read_size = AP::FS().read(fd, reply.data, sizeof(reply.data)); - if (read_size == -1) { - ftp_error(reply, FTP_ERROR::FailErrno); - break; - } - checksum = crc_crc32(checksum, reply.data, MIN((size_t)read_size, sizeof(reply.data))); - } while (read_size > 0); - - AP::FS().close(fd); - // reset our scratch area so we don't leak data, and can leverage trimming memset(reply.data, 0, sizeof(reply.data)); reply.size = sizeof(uint32_t); diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index dd4ab4271a5532..36e0cd3fb0851b 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -3,6 +3,7 @@ #pragma once #include +#include // we have separate helpers disabled to make it possible // to select MAVLink 1.0 in the arduino GUI build @@ -14,8 +15,13 @@ #define MAVLINK_START_UART_SEND(chan, size) comm_send_lock(chan, size) #define MAVLINK_END_UART_SEND(chan, size) comm_send_unlock(chan) +#if AP_NETWORKING_ENABLED +// allow 7 telemetry ports with networking +#define MAVLINK_COMM_NUM_BUFFERS 7 +#else // allow five telemetry ports #define MAVLINK_COMM_NUM_BUFFERS 5 +#endif #define MAVLINK_GET_CHANNEL_BUFFER 1 #define MAVLINK_GET_CHANNEL_STATUS 1 @@ -37,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_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/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index fac080b717a4e6..f454515aefab8c 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -171,7 +171,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = { // @Values{Copter}: 69:POSHOLD Mode // @Values{Copter}: 70:ALTHOLD Mode // @Values{Copter}: 71:FLOWHOLD Mode - // @Values{Copter,Plane}: 72:CIRCLE Mode + // @Values{Copter,Rover,Plane}: 72:CIRCLE Mode // @Values{Copter}: 73:DRIFT Mode // @Values{Rover}: 74:Sailboat motoring 3pos // @Values{Copter}: 75:SurfaceTrackingUpDown @@ -207,6 +207,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 +701,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: @@ -1613,6 +1615,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..fcf61047733450 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -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_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_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..58b22308a0e80c 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -490,7 +490,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 +607,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(); + } } /* 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.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.cpp b/libraries/SITL/SIM_GPS.cpp index ac45d68828cdd8..53f858a94634ad 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -12,13 +12,19 @@ #include -#include #include #include #include #include -#include -#include + +#include "SIM_GPS_FILE.h" +#include "SIM_GPS_Trimble.h" +#include "SIM_GPS_MSP.h" +#include "SIM_GPS_NMEA.h" +#include "SIM_GPS_NOVA.h" +#include "SIM_GPS_SBP2.h" +#include "SIM_GPS_SBP.h" +#include "SIM_GPS_UBLOX.h" // the number of GPS leap seconds - copied from AP_GPS.h #define GPS_LEAPSECONDS_MILLIS 18000ULL @@ -27,13 +33,6 @@ extern const AP_HAL::HAL& hal; using namespace SITL; -struct GPS_TOW { - // Number of weeks since midnight 5-6 January 1980 - uint16_t week; - // Time since start of the GPS week [mS] - uint32_t ms; -}; - // ensure the backend we have allocated matches the one that's configured: GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance) : front{_front}, @@ -88,12 +87,12 @@ ssize_t GPS::write_to_autopilot(const char *p, size_t size) const size_t ret = 0; while (size--) { - float r = ((((unsigned)random()) % 1000000)) / 1.0e4; - if (r < byteloss) { - // lose the byte - p++; - continue; - } + float r = ((((unsigned)random()) % 1000000)) / 1.0e4; + if (r < byteloss) { + // lose the byte + p++; + continue; + } const ssize_t pret = SerialDevice::write_to_autopilot(p, 1); if (pret == 0) { @@ -114,7 +113,7 @@ ssize_t GPS::write_to_autopilot(const char *p, size_t size) const /* get timeval using simulation time */ -static void simulation_timeval(struct timeval *tv) +void GPS_Backend::simulation_timeval(struct timeval *tv) { uint64_t now = AP_HAL::micros64(); static uint64_t first_usec; @@ -131,36 +130,74 @@ static void simulation_timeval(struct timeval *tv) } /* - send a UBLOX GPS message + simple simulation of jamming */ -void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) +void GPS::simulate_jamming(struct GPS_Data &d) { - const uint8_t PREAMBLE1 = 0xb5; - const uint8_t PREAMBLE2 = 0x62; - const uint8_t CLASS_NAV = 0x1; - uint8_t hdr[6], chk[2]; - hdr[0] = PREAMBLE1; - hdr[1] = PREAMBLE2; - hdr[2] = CLASS_NAV; - hdr[3] = msgid; - hdr[4] = size & 0xFF; - hdr[5] = size >> 8; - chk[0] = chk[1] = hdr[2]; - chk[1] += (chk[0] += hdr[3]); - chk[1] += (chk[0] += hdr[4]); - chk[1] += (chk[0] += hdr[5]); - for (uint16_t i=0; i 1000) { + jam.jam_start_ms = now_ms; + jam.latitude = d.latitude; + jam.longitude = d.longitude; + } + jam.last_jam_ms = now_ms; + + // how often each of the key state variables change during jamming + const float vz_change_hz = 0.5; + const float vel_change_hz = 0.8; + const float pos_change_hz = 1.1; + const float sats_change_hz = 3; + const float acc_change_hz = 3; + + if (now_ms - jam.jam_start_ms < unsigned(1000U+(get_random16()%5000))) { + // total loss of signal for a period at the start is common + d.num_sats = 0; + d.have_lock = false; + } else { + if ((now_ms - jam.last_sats_change_ms)*0.001 > 2*fabsf(rand_float())/sats_change_hz) { + jam.last_sats_change_ms = now_ms; + d.num_sats = 2 + (get_random16() % 15); + if (d.num_sats >= 4) { + if (get_random16() % 2 == 0) { + d.have_lock = false; + } else { + d.have_lock = true; + } + } else { + d.have_lock = false; + } + } + if ((now_ms - jam.last_vz_change_ms)*0.001 > 2*fabsf(rand_float())/vz_change_hz) { + jam.last_vz_change_ms = now_ms; + d.speedD = rand_float() * 400; + } + if ((now_ms - jam.last_vel_change_ms)*0.001 > 2*fabsf(rand_float())/vel_change_hz) { + jam.last_vel_change_ms = now_ms; + d.speedN = rand_float() * 400; + d.speedE = rand_float() * 400; + } + if ((now_ms - jam.last_pos_change_ms)*0.001 > 2*fabsf(rand_float())/pos_change_hz) { + jam.last_pos_change_ms = now_ms; + jam.latitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7; + jam.longitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7; + } + if ((now_ms - jam.last_acc_change_ms)*0.001 > 2*fabsf(rand_float())/acc_change_hz) { + jam.last_acc_change_ms = now_ms; + d.vertical_acc = fabsf(rand_float())*300; + d.horizontal_acc = fabsf(rand_float())*300; + d.speed_acc = fabsf(rand_float())*50; + } } - write_to_autopilot((char*)hdr, sizeof(hdr)); - write_to_autopilot((char*)buf, size); - write_to_autopilot((char*)chk, sizeof(chk)); + + d.latitude = constrain_float(jam.latitude, -90, 90); + d.longitude = constrain_float(jam.longitude, -180, 180); } /* return GPS time of week */ -static GPS_TOW gps_time() +GPS_Backend::GPS_TOW GPS_Backend::gps_time() { GPS_TOW gps_tow; struct timeval tv; @@ -174,1241 +211,6 @@ static GPS_TOW gps_time() return gps_tow; } -/* - send a new set of GPS UBLOX packets - */ -void GPS_UBlox::publish(const GPS_Data *d) -{ - struct PACKED ubx_nav_posllh { - uint32_t time; // GPS msToW - int32_t longitude; - int32_t latitude; - int32_t altitude_ellipsoid; - int32_t altitude_msl; - uint32_t horizontal_accuracy; - uint32_t vertical_accuracy; - } pos {}; - struct PACKED ubx_nav_status { - uint32_t time; // GPS msToW - uint8_t fix_type; - uint8_t fix_status; - uint8_t differential_status; - uint8_t res; - uint32_t time_to_first_fix; - uint32_t uptime; // milliseconds - } status {}; - struct PACKED ubx_nav_velned { - uint32_t time; // GPS msToW - int32_t ned_north; - int32_t ned_east; - int32_t ned_down; - uint32_t speed_3d; - uint32_t speed_2d; - int32_t heading_2d; - uint32_t speed_accuracy; - uint32_t heading_accuracy; - } velned {}; - struct PACKED ubx_nav_solution { - uint32_t time; - int32_t time_nsec; - int16_t week; - uint8_t fix_type; - uint8_t fix_status; - int32_t ecef_x; - int32_t ecef_y; - int32_t ecef_z; - uint32_t position_accuracy_3d; - int32_t ecef_x_velocity; - int32_t ecef_y_velocity; - int32_t ecef_z_velocity; - uint32_t speed_accuracy; - uint16_t position_DOP; - uint8_t res; - uint8_t satellites; - uint32_t res2; - } sol {}; - struct PACKED ubx_nav_dop { - uint32_t time; // GPS msToW - uint16_t gDOP; - uint16_t pDOP; - uint16_t tDOP; - uint16_t vDOP; - uint16_t hDOP; - uint16_t nDOP; - uint16_t eDOP; - } dop {}; - struct PACKED ubx_nav_pvt { - uint32_t itow; - uint16_t year; - uint8_t month, day, hour, min, sec; - uint8_t valid; - uint32_t t_acc; - int32_t nano; - uint8_t fix_type; - uint8_t flags; - uint8_t flags2; - uint8_t num_sv; - int32_t lon, lat; - int32_t height, h_msl; - uint32_t h_acc, v_acc; - int32_t velN, velE, velD, gspeed; - int32_t head_mot; - uint32_t s_acc; - uint32_t head_acc; - uint16_t p_dop; - uint8_t reserved1[6]; - uint32_t headVeh; - uint8_t reserved2[4]; - } pvt {}; - const uint8_t SV_COUNT = 10; - struct PACKED ubx_nav_svinfo { - uint32_t itow; - uint8_t numCh; - uint8_t globalFlags; - uint8_t reserved1[2]; - // repeated block - struct PACKED svinfo_sv { - uint8_t chn; - uint8_t svid; - uint8_t flags; - uint8_t quality; - uint8_t cno; - int8_t elev; - int16_t azim; - int32_t prRes; - } sv[SV_COUNT]; - } svinfo {}; - enum RELPOSNED { - gnssFixOK = 1U << 0, - diffSoln = 1U << 1, - relPosValid = 1U << 2, - carrSolnFloat = 1U << 3, - - carrSolnFixed = 1U << 4, - isMoving = 1U << 5, - refPosMiss = 1U << 6, - refObsMiss = 1U << 7, - - relPosHeadingValid = 1U << 8, - relPosNormalized = 1U << 9 - }; - struct PACKED ubx_nav_relposned { - uint8_t version; - uint8_t reserved1; - uint16_t refStationId; - uint32_t iTOW; - int32_t relPosN; - int32_t relPosE; - int32_t relPosD; - int32_t relPosLength; - int32_t relPosHeading; - uint8_t reserved2[4]; - int8_t relPosHPN; - int8_t relPosHPE; - int8_t relPosHPD; - int8_t relPosHPLength; - uint32_t accN; - uint32_t accE; - uint32_t accD; - uint32_t accLength; - uint32_t accHeading; - uint8_t reserved3[4]; - uint32_t flags; - } relposned {}; - - const uint8_t MSG_POSLLH = 0x2; - const uint8_t MSG_STATUS = 0x3; - const uint8_t MSG_DOP = 0x4; - const uint8_t MSG_VELNED = 0x12; - const uint8_t MSG_SOL = 0x6; - const uint8_t MSG_PVT = 0x7; - const uint8_t MSG_SVINFO = 0x30; - const uint8_t MSG_RELPOSNED = 0x3c; - - uint32_t _next_nav_sv_info_time = 0; - - const auto gps_tow = gps_time(); - - pos.time = gps_tow.ms; - pos.longitude = d->longitude * 1.0e7; - pos.latitude = d->latitude * 1.0e7; - pos.altitude_ellipsoid = d->altitude * 1000.0f; - pos.altitude_msl = d->altitude * 1000.0f; - pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000; - - status.time = gps_tow.ms; - status.fix_type = d->have_lock?3:0; - status.fix_status = d->have_lock?1:0; - status.differential_status = 0; - status.res = 0; - status.time_to_first_fix = 0; - status.uptime = AP_HAL::millis(); - - velned.time = gps_tow.ms; - velned.ned_north = 100.0f * d->speedN; - velned.ned_east = 100.0f * d->speedE; - velned.ned_down = 100.0f * d->speedD; - velned.speed_2d = norm(d->speedN, d->speedE) * 100; - velned.speed_3d = norm(d->speedN, d->speedE, d->speedD) * 100; - velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f; - if (velned.heading_2d < 0.0f) { - velned.heading_2d += 360.0f * 100000.0f; - } - velned.speed_accuracy = 40; - velned.heading_accuracy = 4; - - memset(&sol, 0, sizeof(sol)); - sol.fix_type = d->have_lock?3:0; - sol.fix_status = 221; - sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3; - sol.time = gps_tow.ms; - sol.week = gps_tow.week; - - dop.time = gps_tow.ms; - dop.gDOP = 65535; - dop.pDOP = 65535; - dop.tDOP = 65535; - dop.vDOP = 200; - dop.hDOP = 121; - dop.nDOP = 65535; - dop.eDOP = 65535; - - pvt.itow = gps_tow.ms; - pvt.year = 0; - pvt.month = 0; - pvt.day = 0; - pvt.hour = 0; - pvt.min = 0; - pvt.sec = 0; - pvt.valid = 0; // invalid utc date - pvt.t_acc = 0; - pvt.nano = 0; - pvt.fix_type = d->have_lock? 0x3 : 0; - pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok - pvt.flags2 =0; - pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3; - pvt.lon = d->longitude * 1.0e7; - pvt.lat = d->latitude * 1.0e7; - pvt.height = d->altitude * 1000.0f; - pvt.h_msl = d->altitude * 1000.0f; - pvt.h_acc = _sitl->gps_accuracy[instance] * 1000; - pvt.v_acc = _sitl->gps_accuracy[instance] * 1000; - pvt.velN = 1000.0f * d->speedN; - pvt.velE = 1000.0f * d->speedE; - pvt.velD = 1000.0f * d->speedD; - pvt.gspeed = norm(d->speedN, d->speedE) * 1000; - pvt.head_mot = ToDeg(atan2f(d->speedE, d->speedN)) * 1.0e5; - pvt.s_acc = 40; - pvt.head_acc = 38 * 1.0e5; - pvt.p_dop = 65535; - memset(pvt.reserved1, '\0', ARRAY_SIZE(pvt.reserved1)); - pvt.headVeh = 0; - memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2)); - - if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { - const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get(); - const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get(); - Vector3f rel_antenna_pos = ant2_pos - ant1_pos; - Matrix3f rot; - // project attitude back using gyros to get antenna orientation at time of GPS sample - Vector3f gyro(radians(_sitl->state.rollRate), - radians(_sitl->state.pitchRate), - radians(_sitl->state.yawRate)); - rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg)); - const float lag = _sitl->gps_delay_ms[instance] * 0.001; - rot.rotate(gyro * (-lag)); - rel_antenna_pos = rot * rel_antenna_pos; - relposned.version = 1; - relposned.iTOW = gps_tow.ms; - relposned.relPosN = rel_antenna_pos.x * 100; - relposned.relPosE = rel_antenna_pos.y * 100; - relposned.relPosD = rel_antenna_pos.z * 100; - relposned.relPosLength = rel_antenna_pos.length() * 100; - relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5; - relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid; - } - - send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); - send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); - send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); - send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); - send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop)); - send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt)); - if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { - send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned)); - } - - if (gps_tow.ms > _next_nav_sv_info_time) { - svinfo.itow = gps_tow.ms; - svinfo.numCh = 32; - svinfo.globalFlags = 4; // u-blox 8/M8 - // fill in the SV's with some data even though firmware does not currently use it - // note that this is not using num_sats as we aren't dynamically creating this to match - for (uint8_t i = 0; i < SV_COUNT; i++) { - svinfo.sv[i].chn = i; - svinfo.sv[i].svid = i; - svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 0x7 : 0x6; // sv used, diff correction data, orbit information - svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized - svinfo.sv[i].cno = MAX(20, 30 - i); - svinfo.sv[i].elev = MAX(30, 90 - i); - svinfo.sv[i].azim = i; - // not bothering to fill in prRes - } - send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo)); - _next_nav_sv_info_time = gps_tow.ms + 10000; // 10 second delay - } -} - -/* - formatted print of NMEA message, with checksum appended - */ -void GPS_NMEA::nmea_printf(const char *fmt, ...) -{ - va_list ap; - - va_start(ap, fmt); - char *s = nmea_vaprintf(fmt, ap); - va_end(ap); - if (s != nullptr) { - write_to_autopilot((const char*)s, strlen(s)); - free(s); - } -} - - -/* - send a new GPS NMEA packet - */ -void GPS_NMEA::publish(const GPS_Data *d) -{ - struct timeval tv; - struct tm *tm; - char tstring[20]; - char dstring[20]; - char lat_string[20]; - char lng_string[20]; - - simulation_timeval(&tv); - - tm = gmtime(&tv.tv_sec); - - // format time string - snprintf(tstring, sizeof(tstring), "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + tv.tv_usec*1.0e-6); - - // format date string - snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); - - // format latitude - double deg = fabs(d->latitude); - snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c", - (unsigned)deg, - (deg - int(deg))*60, - d->latitude<0?'S':'N'); - - // format longitude - deg = fabs(d->longitude); - snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c", - (unsigned)deg, - (deg - int(deg))*60, - d->longitude<0?'W':'E'); - - nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", - tstring, - lat_string, - lng_string, - d->have_lock?1:0, - d->have_lock?_sitl->gps_numsats[instance]:3, - 1.2, - d->altitude); - - const float speed_mps = d->speed_2d(); - const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS; - const auto heading_rad = d->heading(); - - //$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24 - nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A", - tstring, - heading_rad, - heading_rad, - speed_knots, - speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6); - - nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", - tstring, - d->have_lock?'A':'V', - lat_string, - lng_string, - speed_knots, - heading_rad, - dstring); - - if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) { - nmea_printf("$GPHDT,%.2f,T", d->yaw_deg); - } - else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) { - nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V'); - } else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) { - // Unicore support - // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D - nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,", - tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, unsigned(tv.tv_usec*1.e-4), - d->longitude, d->latitude, - d->altitude, - wrap_360(d->yaw_deg), - d->pitch_deg, - heading_rad, - speed_mps, - d->roll_deg, - d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed, - 3, // fixed rtk yaw solution, - d->have_lock?_sitl->gps_numsats[instance]:3, - d->have_lock?_sitl->gps_numsats[instance]:3, - d->speedE * 3.6, - d->speedN * 3.6, - -d->speedD * 3.6); - } -} - -void GPS_SBP_Common::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload) -{ - if (len != 0 && payload == 0) { - return; //SBP_NULL_ERROR; - } - - uint8_t preamble = 0x55; - write_to_autopilot((char*)&preamble, 1); - write_to_autopilot((char*)&msg_type, 2); - write_to_autopilot((char*)&sender_id, 2); - write_to_autopilot((char*)&len, 1); - if (len > 0) { - write_to_autopilot((char*)payload, len); - } - - uint16_t crc; - crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0); - crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc); - crc = crc16_ccitt(&(len), 1, crc); - crc = crc16_ccitt(payload, len, crc); - write_to_autopilot((char*)&crc, 2); -} - -void GPS_SBP::publish(const GPS_Data *d) -{ - struct sbp_heartbeat_t { - bool sys_error : 1; - bool io_error : 1; - bool nap_error : 1; - uint8_t res : 5; - uint8_t protocol_minor : 8; - uint8_t protocol_major : 8; - uint8_t res2 : 7; - bool ext_antenna : 1; - } hb; // 4 bytes - - struct PACKED sbp_gps_time_t { - uint16_t wn; //< GPS week number - uint32_t tow; //< GPS Time of Week rounded to the nearest ms - int32_t ns; //< Nanosecond remainder of rounded tow - uint8_t flags; //< Status flags (reserved) - } t; - struct PACKED sbp_pos_llh_t { - uint32_t tow; //< GPS Time of Week - double lat; //< Latitude - double lon; //< Longitude - double height; //< Height - uint16_t h_accuracy; //< Horizontal position accuracy estimate - uint16_t v_accuracy; //< Vertical position accuracy estimate - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags - } pos; - struct PACKED sbp_vel_ned_t { - uint32_t tow; //< GPS Time of Week - int32_t n; //< Velocity North coordinate - int32_t e; //< Velocity East coordinate - int32_t d; //< Velocity Down coordinate - uint16_t h_accuracy; //< Horizontal velocity accuracy estimate - uint16_t v_accuracy; //< Vertical velocity accuracy estimate - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags (reserved) - } velned; - struct PACKED sbp_dops_t { - uint32_t tow; //< GPS Time of Week - uint16_t gdop; //< Geometric Dilution of Precision - uint16_t pdop; //< Position Dilution of Precision - uint16_t tdop; //< Time Dilution of Precision - uint16_t hdop; //< Horizontal Dilution of Precision - uint16_t vdop; //< Vertical Dilution of Precision - uint8_t flags; //< Status flags (reserved) - } dops; - - static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; - static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100; - static const uint16_t SBP_DOPS_MSGTYPE = 0x0206; - static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201; - static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205; - - const auto gps_tow = gps_time(); - - t.wn = gps_tow.week; - t.tow = gps_tow.ms; - t.ns = 0; - t.flags = 0; - sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t); - - if (!d->have_lock) { - return; - } - - pos.tow = gps_tow.ms; - pos.lon = d->longitude; - pos.lat= d->latitude; - pos.height = d->altitude; - pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; - - // Send single point position solution - pos.flags = 0; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - // Send "pseudo-absolute" RTK position solution - pos.flags = 1; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - - velned.tow = gps_tow.ms; - velned.n = 1e3 * d->speedN; - velned.e = 1e3 * d->speedE; - velned.d = 1e3 * d->speedD; - velned.h_accuracy = 5e3; - velned.v_accuracy = 5e3; - velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; - velned.flags = 0; - sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); - - static uint32_t do_every_count = 0; - if (do_every_count % 5 == 0) { - - dops.tow = gps_tow.ms; - dops.gdop = 1; - dops.pdop = 1; - dops.tdop = 1; - dops.hdop = 100; - dops.vdop = 1; - dops.flags = 1; - sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), - (uint8_t*)&dops); - - hb.protocol_major = 0; //Sends protocol version 0 - sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), - (uint8_t*)&hb); - - } - do_every_count++; -} - - -void GPS_SBP2::publish(const GPS_Data *d) -{ - struct sbp_heartbeat_t { - bool sys_error : 1; - bool io_error : 1; - bool nap_error : 1; - uint8_t res : 5; - uint8_t protocol_minor : 8; - uint8_t protocol_major : 8; - uint8_t res2 : 7; - bool ext_antenna : 1; - } hb; // 4 bytes - - struct PACKED sbp_gps_time_t { - uint16_t wn; //< GPS week number - uint32_t tow; //< GPS Time of Week rounded to the nearest ms - int32_t ns; //< Nanosecond remainder of rounded tow - uint8_t flags; //< Status flags (reserved) - } t; - struct PACKED sbp_pos_llh_t { - uint32_t tow; //< GPS Time of Week - double lat; //< Latitude - double lon; //< Longitude - double height; //< Height - uint16_t h_accuracy; //< Horizontal position accuracy estimate - uint16_t v_accuracy; //< Vertical position accuracy estimate - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags - } pos; - struct PACKED sbp_vel_ned_t { - uint32_t tow; //< GPS Time of Week - int32_t n; //< Velocity North coordinate - int32_t e; //< Velocity East coordinate - int32_t d; //< Velocity Down coordinate - uint16_t h_accuracy; //< Horizontal velocity accuracy estimate - uint16_t v_accuracy; //< Vertical velocity accuracy estimate - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags (reserved) - } velned; - struct PACKED sbp_dops_t { - uint32_t tow; //< GPS Time of Week - uint16_t gdop; //< Geometric Dilution of Precision - uint16_t pdop; //< Position Dilution of Precision - uint16_t tdop; //< Time Dilution of Precision - uint16_t hdop; //< Horizontal Dilution of Precision - uint16_t vdop; //< Vertical Dilution of Precision - uint8_t flags; //< Status flags (reserved) - } dops; - - static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; - static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0102; - static const uint16_t SBP_DOPS_MSGTYPE = 0x0208; - static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A; - static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E; - - const auto gps_tow = gps_time(); - - t.wn = gps_tow.week; - t.tow = gps_tow.ms; - t.ns = 0; - t.flags = 1; - sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t); - - if (!d->have_lock) { - return; - } - - pos.tow = gps_tow.ms; - pos.lon = d->longitude; - pos.lat= d->latitude; - pos.height = d->altitude; - pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; - pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; - - // Send single point position solution - pos.flags = 1; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - // Send "pseudo-absolute" RTK position solution - pos.flags = 4; - sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); - - velned.tow = gps_tow.ms; - velned.n = 1e3 * d->speedN; - velned.e = 1e3 * d->speedE; - velned.d = 1e3 * d->speedD; - velned.h_accuracy = 5e3; - velned.v_accuracy = 5e3; - velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; - velned.flags = 1; - sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); - - static uint32_t do_every_count = 0; - if (do_every_count % 5 == 0) { - - dops.tow = gps_tow.ms; - dops.gdop = 1; - dops.pdop = 1; - dops.tdop = 1; - dops.hdop = 100; - dops.vdop = 1; - dops.flags = 1; - sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), - (uint8_t*)&dops); - - hb.protocol_major = 2; //Sends protocol version 2.0 - sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), - (uint8_t*)&hb); - } - do_every_count++; -} - -void GPS_NOVA::publish(const GPS_Data *d) -{ - static struct PACKED nova_header - { - // 0 - uint8_t preamble[3]; - // 3 - uint8_t headerlength; - // 4 - uint16_t messageid; - // 6 - uint8_t messagetype; - //7 - uint8_t portaddr; - //8 - uint16_t messagelength; - //10 - uint16_t sequence; - //12 - uint8_t idletime; - //13 - uint8_t timestatus; - //14 - uint16_t week; - //16 - uint32_t tow; - //20 - uint32_t recvstatus; - // 24 - uint16_t resv; - //26 - uint16_t recvswver; - } header; - - struct PACKED psrdop - { - float gdop; - float pdop; - float hdop; - float htdop; - float tdop; - float cutoff; - uint32_t svcount; - // extra data for individual prns - } psrdop {}; - - struct PACKED bestpos - { - uint32_t solstat; - uint32_t postype; - double lat; - double lng; - double hgt; - float undulation; - uint32_t datumid; - float latsdev; - float lngsdev; - float hgtsdev; - // 4 bytes - uint8_t stnid[4]; - float diffage; - float sol_age; - uint8_t svstracked; - uint8_t svsused; - uint8_t svsl1; - uint8_t svsmultfreq; - uint8_t resv; - uint8_t extsolstat; - uint8_t galbeisigmask; - uint8_t gpsglosigmask; - } bestpos {}; - - struct PACKED bestvel - { - uint32_t solstat; - uint32_t veltype; - float latency; - float age; - double horspd; - double trkgnd; - // + up - double vertspd; - float resv; - } bestvel {}; - - const auto gps_tow = gps_time(); - - header.preamble[0] = 0xaa; - header.preamble[1] = 0x44; - header.preamble[2] = 0x12; - header.headerlength = sizeof(header); - header.week = gps_tow.week; - header.tow = gps_tow.ms; - - header.messageid = 174; - header.messagelength = sizeof(psrdop); - header.sequence += 1; - - psrdop.hdop = 1.20; - psrdop.htdop = 1.20; - nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop)); - - - header.messageid = 99; - header.messagelength = sizeof(bestvel); - header.sequence += 1; - - bestvel.horspd = norm(d->speedN, d->speedE); - bestvel.trkgnd = ToDeg(atan2f(d->speedE, d->speedN)); - bestvel.vertspd = -d->speedD; - - nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel)); - - - header.messageid = 42; - header.messagelength = sizeof(bestpos); - header.sequence += 1; - - bestpos.lat = d->latitude; - bestpos.lng = d->longitude; - bestpos.hgt = d->altitude; - bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3; - bestpos.latsdev=0.2; - bestpos.lngsdev=0.2; - bestpos.hgtsdev=0.2; - bestpos.solstat=0; - bestpos.postype=32; - - nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos)); -} - -void GPS_NOVA::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen) -{ - write_to_autopilot((char*)header, headerlength); -write_to_autopilot((char*)payload, payloadlen); - - uint32_t crc = CalculateBlockCRC32(headerlength, header, (uint32_t)0); - crc = CalculateBlockCRC32(payloadlen, payload, crc); - - write_to_autopilot((char*)&crc, 4); -} - -#define CRC32_POLYNOMIAL 0xEDB88320L -uint32_t GPS_NOVA::CRC32Value(uint32_t icrc) -{ - int i; - uint32_t crc = icrc; - for ( i = 8 ; i > 0; i-- ) - { - if ( crc & 1 ) - crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL; - else - crc >>= 1; - } - return crc; -} - -uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) -{ - while ( length-- != 0 ) - { - crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff)); - } - return( crc ); -} - -void GPS_GSOF::publish(const GPS_Data *d) -{ - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 - constexpr uint8_t GSOF_POS_TIME_TYPE { 0x01 }; - constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A }; - // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE - const uint8_t bootcount = 17; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 - enum class POS_FLAGS_1 : uint8_t { - NEW_POSITION = 1U << 0, - CLOCK_FIX_CALULATED = 1U << 1, - HORIZ_FROM_THIS_POS = 1U << 2, - HEIGHT_FROM_THIS_POS = 1U << 3, - RESERVED_4 = 1U << 4, - LEAST_SQ_POSITION = 1U << 5, - RESERVED_6 = 1U << 6, - POSITION_L1_PSEUDORANGES = 1U << 7 - }; - const uint8_t pos_flags_1 { - uint8_t(POS_FLAGS_1::NEW_POSITION) | - uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) | - uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) | - uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) | - uint8_t(POS_FLAGS_1::RESERVED_4) | - uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) | - uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES) - }; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 - enum class POS_FLAGS_2 : uint8_t { - DIFFERENTIAL_POS = 1U << 0, - DIFFERENTIAL_POS_PHASE_RTK = 1U << 1, - POSITION_METHOD_FIXED_PHASE = 1U << 2, - OMNISTAR_ACTIVE = 1U << 3, - DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4, - NETWORK_RTK = 1U << 5, - DITHERED_RTK = 1U << 6, - BEACON_DGNSS = 1U << 7, - }; - - // Simulate a GPS without RTK in SIM since there is no RTK SIM params. - // This means these flags are unset: - // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS - uint8_t pos_flags_2 {0}; - if(d->have_lock) { - pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS); - pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK); - pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE); - pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE); - pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT); - } - - const auto gps_tow = gps_time(); - const struct PACKED gsof_pos_time { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - uint32_t time_week_ms; - uint16_t time_week; - uint8_t num_sats; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 - uint8_t pos_flags_1; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 - uint8_t pos_flags_2; - uint8_t initialized_num; - } pos_time { - GSOF_POS_TIME_TYPE, - GSOF_POS_TIME_LEN, - htobe32(gps_tow.ms), - htobe16(gps_tow.week), - d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3), - pos_flags_1, - pos_flags_2, - bootcount - }; - static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN); - - constexpr uint8_t GSOF_POS_TYPE = 0x02; - constexpr uint8_t GSOF_POS_LEN = 0x18; - - const struct PACKED gsof_pos { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - uint64_t lat; - uint64_t lng; - uint64_t alt; - } pos { - GSOF_POS_TYPE, - GSOF_POS_LEN, - gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), - gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), - gsof_pack_double(static_cast(d->altitude)) - }; - static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html - constexpr uint8_t GSOF_VEL_TYPE = 0x08; - // use the smaller packet by ignoring local coordinate system - constexpr uint8_t GSOF_VEL_LEN = 0x0D; - - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags - enum class VEL_FIELDS : uint8_t { - VALID = 1U << 0, - CONSECUTIVE_MEASUREMENTS = 1U << 1, - HEADING_VALID = 1U << 2, - RESERVED_3 = 1U << 3, - RESERVED_4 = 1U << 4, - RESERVED_5 = 1U << 5, - RESERVED_6 = 1U << 6, - RESERVED_7 = 1U << 7, - }; - uint8_t vel_flags {0}; - if(d->have_lock) { - vel_flags |= uint8_t(VEL_FIELDS::VALID); - vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS); - vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID); - } - - const struct PACKED gsof_vel { - const uint8_t OUTPUT_RECORD_TYPE; - const uint8_t RECORD_LEN; - // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags - uint8_t flags; - uint32_t horiz_m_p_s; - uint32_t heading_rad; - uint32_t vertical_m_p_s; - } vel { - GSOF_VEL_TYPE, - GSOF_VEL_LEN, - vel_flags, - gsof_pack_float(d->speed_2d()), - gsof_pack_float(d->heading()), - // Trimble API has ambiguous direction here. - // Intentionally narrow from double. - gsof_pack_float(static_cast(d->speedD)) - }; - static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); - - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 - constexpr uint8_t GSOF_DOP_TYPE = 0x09; - constexpr uint8_t GSOF_DOP_LEN = 0x10; - const struct PACKED gsof_dop { - const uint8_t OUTPUT_RECORD_TYPE { GSOF_DOP_TYPE }; - const uint8_t RECORD_LEN { GSOF_DOP_LEN }; - uint32_t pdop = htobe32(1); - uint32_t hdop = htobe32(1); - uint32_t vdop = htobe32(1); - uint32_t tdop = htobe32(1); - } dop {}; - // Check the payload size calculation in the compiler - constexpr auto dop_size = sizeof(gsof_dop); - static_assert(dop_size == 18); - constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); - static_assert(dop_record_type_size == 1); - constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); - static_assert(len_size == 1); - constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); - static_assert(dop_payload_size == GSOF_DOP_LEN); - - constexpr uint8_t GSOF_POS_SIGMA_TYPE = 0x0C; - constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; - const struct PACKED gsof_pos_sigma { - const uint8_t OUTPUT_RECORD_TYPE { GSOF_POS_SIGMA_TYPE }; - const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN }; - uint32_t pos_rms = htobe32(0); - uint32_t sigma_e = htobe32(0); - uint32_t sigma_n = htobe32(0); - uint32_t cov_en = htobe32(0); - uint32_t sigma_up = htobe32(0); - uint32_t semi_major_axis = htobe32(0); - uint32_t semi_minor_axis = htobe32(0); - uint32_t orientation = htobe32(0); - uint32_t unit_variance = htobe32(0); - uint16_t n_epocs = htobe32(1); // Always 1 for kinematic. - } pos_sigma {}; - static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN); - - // TODO add GSOF49 - const uint8_t payload_sz = sizeof(pos_time) + sizeof(pos) + sizeof(vel) + sizeof(dop) + sizeof(pos_sigma); - uint8_t buf[payload_sz] = {}; - uint8_t offset = 0; - memcpy(&buf[offset], &pos_time, sizeof(pos_time)); - offset += sizeof(pos_time); - memcpy(&buf[offset], &pos, sizeof(pos)); - offset += sizeof(pos); - memcpy(&buf[offset], &vel, sizeof(vel)); - offset += sizeof(vel); - memcpy(&buf[offset], &dop, sizeof(dop)); - offset += sizeof(dop); - memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); - offset += sizeof(pos_sigma); - assert(offset == payload_sz); - send_gsof(buf, sizeof(buf)); -} - - -void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) -{ - // All Trimble "Data Collector" packets, including GSOF, are comprised of three fields: - // * A fixed-length packet header (dcol_header) - // * A variable-length data frame (buf) - // * A fixed-length packet trailer (dcol_trailer) - // Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0 - - const uint8_t STX = 0x02; - // status bitfield - // https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1 - const uint8_t STATUS = 0xa8; - const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT) - - // Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame. - // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2 - - static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number - // Most messages, even GSOF49, only take one page. For SIM, assume it. - assert(size < 0xFA); // GPS SIM doesn't yet support paging - constexpr uint8_t PAGE_INDEX = 0; - constexpr uint8_t MAX_PAGE_INDEX = 0; - const uint8_t gsof_header[3] = { - TRANSMISSION_NUMBER, - PAGE_INDEX, - MAX_PAGE_INDEX, - }; - ++TRANSMISSION_NUMBER; - - // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes. - // A captured GSOF49 packet from BD940 has total bytes of 115 bytes. - // Thus, the following 5 bytes are not counted. - // 1) STX - // 2) STATUS - // 3) PACKET TYPE - // 4) LENGTH - // 5) CHECKSUM - // 6) ETX - // This aligns with manual's idea of data bytes: - // "Each message begins with a 4-byte header, followed by the bytes of data in each packet. The packet ends with a 2-byte trailer." - // Thus, for this implementation with single-page single-record per DCOL packet, - // the length is simply the sum of data packet size, the gsof_header size. - const uint8_t length = size + sizeof(gsof_header); - const uint8_t dcol_header[4] { - STX, - STATUS, - PACKET_TYPE, - length - }; - - - // Sum bytes (status + type + length + data bytes) and modulo 256 the summation - // Because it's a uint8, use natural overflow - uint8_t csum = STATUS + PACKET_TYPE + length; - for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) { - csum += gsof_header[i]; - } - for (size_t i = 0; i < size; i++) { - csum += buf[i]; - } - - constexpr uint8_t ETX = 0x03; - const uint8_t dcol_trailer[2] = { - csum, - ETX - }; - - write_to_autopilot((char*)dcol_header, sizeof(dcol_header)); - write_to_autopilot((char*)gsof_header, sizeof(gsof_header)); - write_to_autopilot((char*)buf, size); - write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer)); - const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer); - // Validate length based on everything but DCOL h - if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) { - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - } -} - -uint64_t GPS_GSOF::gsof_pack_double(const double& src) -{ - uint64_t dst; - static_assert(sizeof(src) == sizeof(dst)); - memcpy(&dst, &src, sizeof(dst)); - dst = htobe64(dst); - return dst; -} - -uint32_t GPS_GSOF::gsof_pack_float(const float& src) -{ - uint32_t dst; - static_assert(sizeof(src) == sizeof(dst)); - memcpy(&dst, &src, sizeof(dst)); - dst = htobe32(dst); - return dst; -} - -/* - send MSP GPS data - */ -void GPS_MSP::publish(const GPS_Data *d) -{ - struct PACKED { - // header - struct PACKED { - uint8_t dollar = '$'; - uint8_t magic = 'X'; - uint8_t code = '<'; - uint8_t flags; - uint16_t cmd = 0x1F03; // GPS - uint16_t size = 52; - } hdr; - uint8_t instance; - uint16_t gps_week; - uint32_t ms_tow; - uint8_t fix_type; - uint8_t satellites_in_view; - uint16_t horizontal_pos_accuracy; // [cm] - uint16_t vertical_pos_accuracy; // [cm] - uint16_t horizontal_vel_accuracy; // [cm/s] - uint16_t hdop; - int32_t longitude; - int32_t latitude; - int32_t msl_altitude; // cm - int32_t ned_vel_north; // cm/s - int32_t ned_vel_east; - int32_t ned_vel_down; - uint16_t ground_course; // deg * 100, 0..36000 - uint16_t true_yaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t hour; - uint8_t min; - uint8_t sec; - - // footer CRC - uint8_t crc; - } msp_gps {}; - - auto t = gps_time(); - struct timeval tv; - simulation_timeval(&tv); - auto *tm = gmtime(&tv.tv_sec); - - msp_gps.gps_week = t.week; - msp_gps.ms_tow = t.ms; - msp_gps.fix_type = d->have_lock?3:0; - msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3; - msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100; - msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100; - msp_gps.horizontal_vel_accuracy = 30; - msp_gps.hdop = 100; - msp_gps.longitude = d->longitude * 1.0e7; - msp_gps.latitude = d->latitude * 1.0e7; - msp_gps.msl_altitude = d->altitude * 100; - msp_gps.ned_vel_north = 100 * d->speedN; - msp_gps.ned_vel_east = 100 * d->speedE; - msp_gps.ned_vel_down = 100 * d->speedD; - msp_gps.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100; - msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw - msp_gps.year = tm->tm_year; - msp_gps.month = tm->tm_mon; - msp_gps.day = tm->tm_mday; - msp_gps.hour = tm->tm_hour; - msp_gps.min = tm->tm_min; - msp_gps.sec = tm->tm_sec; - - // CRC is over packet without first 3 bytes and trailing CRC byte - msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4); - - write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps)); -} - -/* - read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED - */ -#if AP_SIM_GPS_FILE_ENABLED -void GPS_FILE::publish(const GPS_Data *d) -{ - static int fd[2] = {-1,-1}; - static uint32_t base_time[2]; - const uint16_t lognum = uint16_t(_sitl->gps_log_num.get()); - if (instance > 1) { - return; - } - if (fd[instance] == -1) { - char fname[] = "gpsN_NNN.log"; - hal.util->snprintf(fname, 13, "gps%u_%03u.log", instance+1, lognum); - fd[instance] = open(fname, O_RDONLY|O_CLOEXEC); - if (fd[instance] == -1) { - return; - } - } - const uint32_t magic = 0x7fe53b04; - struct { - uint32_t magic; - uint32_t time_ms; - uint32_t n; - } header; - uint8_t *buf = nullptr; - while (true) { - if (::read(fd[instance], (void *)&header, sizeof(header)) != sizeof(header) || - header.magic != magic) { - goto rewind_file; - } - if (header.time_ms+base_time[instance] > AP_HAL::millis()) { - // not ready for this data yet - ::lseek(fd[instance], -sizeof(header), SEEK_CUR); - return; - } - buf = new uint8_t[header.n]; - if (buf != nullptr && ::read(fd[instance], buf, header.n) == ssize_t(header.n)) { - write_to_autopilot((const char *)buf, header.n); - delete[] buf; - buf = nullptr; - continue; - } - goto rewind_file; - } - -rewind_file: - ::printf("GPS[%u] rewind\n", unsigned(instance)); - base_time[instance] = AP_HAL::millis(); - ::lseek(fd[instance], 0, SEEK_SET); - delete[] buf; -} -#endif // AP_SIM_GPS_FILE_ENABLED - void GPS::check_backend_allocation() { const Type configured_type = Type(_sitl->gps_type[instance].get()); @@ -1428,33 +230,47 @@ void GPS::check_backend_allocation() // no GPS attached break; +#if AP_SIM_GPS_UBLOX_ENABLED case Type::UBLOX: backend = new GPS_UBlox(*this, instance); break; +#endif +#if AP_SIM_GPS_NMEA_ENABLED case Type::NMEA: backend = new GPS_NMEA(*this, instance); break; +#endif +#if AP_SIM_GPS_SBP_ENABLED case Type::SBP: backend = new GPS_SBP(*this, instance); break; +#endif +#if AP_SIM_GPS_SBP2_ENABLED case Type::SBP2: backend = new GPS_SBP2(*this, instance); break; +#endif +#if AP_SIM_GPS_NOVA_ENABLED case Type::NOVA: backend = new GPS_NOVA(*this, instance); break; +#endif +#if AP_SIM_GPS_MSP_ENABLED case Type::MSP: backend = new GPS_MSP(*this, instance); break; +#endif - case Type::GSOF: - backend = new GPS_GSOF(*this, instance); +#if AP_SIM_GPS_TRIMBLE_ENABLED + case Type::TRIMBLE: + backend = new GPS_Trimble(*this, instance); break; +#endif #if AP_SIM_GPS_FILE_ENABLED case Type::FILE: @@ -1511,91 +327,98 @@ void GPS::update() const uint8_t idx = instance; // alias to avoid code churn - struct GPS_Data d {}; + struct GPS_Data d {}; - // simulate delayed lock times - bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL); + // simulate delayed lock times + bool have_lock = (!_sitl->gps_disable[idx] && now_ms >= _sitl->gps_lock_time[idx]*1000UL); - // run at configured GPS rate (default 5Hz) - if ((now_ms - last_update) < (uint32_t)(1000/_sitl->gps_hertz[idx])) { - backend->update_read(); - return; - } + // Only let physics run and GPS write at configured GPS rate (default 5Hz). + if ((now_ms - last_write_update_ms) < (uint32_t)(1000/_sitl->gps_hertz[instance])) { + // Reading runs every iteration. + // Beware- physics don't update every iteration with this approach. + // Currently, none of the drivers rely on quickly updated physics. + backend->update_read(); + return; + } - last_update = now_ms; + last_write_update_ms = now_ms; - d.latitude = latitude; - d.longitude = longitude; - d.yaw_deg = _sitl->state.yawDeg; - d.roll_deg = _sitl->state.rollDeg; - d.pitch_deg = _sitl->state.pitchDeg; + d.latitude = latitude; + d.longitude = longitude; + d.yaw_deg = _sitl->state.yawDeg; + d.roll_deg = _sitl->state.rollDeg; + d.pitch_deg = _sitl->state.pitchDeg; - // add an altitude error controlled by a slow sine wave - d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx]; + // add an altitude error controlled by a slow sine wave + d.altitude = altitude + _sitl->gps_noise[idx] * sinf(now_ms * 0.0005f) + _sitl->gps_alt_offset[idx]; - // Add offset to c.g. velocity to get velocity at antenna and add simulated error - Vector3f velErrorNED = _sitl->gps_vel_err[idx]; - d.speedN = speedN + (velErrorNED.x * rand_float()); - d.speedE = speedE + (velErrorNED.y * rand_float()); - d.speedD = speedD + (velErrorNED.z * rand_float()); - d.have_lock = have_lock; + // Add offset to c.g. velocity to get velocity at antenna and add simulated error + Vector3f velErrorNED = _sitl->gps_vel_err[idx]; + d.speedN = speedN + (velErrorNED.x * rand_float()); + d.speedE = speedE + (velErrorNED.y * rand_float()); + d.speedD = speedD + (velErrorNED.z * rand_float()); + d.have_lock = have_lock; - if (_sitl->gps_drift_alt[idx] > 0) { - // add slow altitude drift controlled by a slow sine wave - d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f); - } + if (_sitl->gps_drift_alt[idx] > 0) { + // add slow altitude drift controlled by a slow sine wave + d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f); + } - // correct the latitude, longitude, height and NED velocity for the offset between - // the vehicle c.g. and GPS antenna - Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx]; - if (!posRelOffsetBF.is_zero()) { - // get a rotation matrix following DCM conventions (body to earth) - Matrix3f rotmat; - _sitl->state.quaternion.rotation_matrix(rotmat); - - // rotate the antenna offset into the earth frame - Vector3f posRelOffsetEF = rotmat * posRelOffsetBF; - - // Add the offset to the latitude, longitude and height using a spherical earth approximation - double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius - double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude)); - d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv); - d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor); - d.altitude -= posRelOffsetEF.z; - - // calculate a velocity offset due to the antenna position offset and body rotation rate - // note: % operator is overloaded for cross product - Vector3f gyro(radians(_sitl->state.rollRate), - radians(_sitl->state.pitchRate), - radians(_sitl->state.yawRate)); - Vector3f velRelOffsetBF = gyro % posRelOffsetBF; - - // rotate the velocity offset into earth frame and add to the c.g. velocity - Vector3f velRelOffsetEF = rotmat * velRelOffsetBF; - d.speedN += velRelOffsetEF.x; - d.speedE += velRelOffsetEF.y; - d.speedD += velRelOffsetEF.z; - } + // correct the latitude, longitude, height and NED velocity for the offset between + // the vehicle c.g. and GPS antenna + Vector3f posRelOffsetBF = _sitl->gps_pos_offset[idx]; + if (!posRelOffsetBF.is_zero()) { + // get a rotation matrix following DCM conventions (body to earth) + Matrix3f rotmat; + _sitl->state.quaternion.rotation_matrix(rotmat); - // get delayed data - d.timestamp_ms = now_ms; - d = interpolate_data(d, _sitl->gps_delay_ms[instance]); + // rotate the antenna offset into the earth frame + Vector3f posRelOffsetEF = rotmat * posRelOffsetBF; - // Applying GPS glitch - // Using first gps glitch - Vector3f glitch_offsets = _sitl->gps_glitch[idx]; - d.latitude += glitch_offsets.x; - d.longitude += glitch_offsets.y; - d.altitude += glitch_offsets.z; + // Add the offset to the latitude, longitude and height using a spherical earth approximation + double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius + double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude)); + d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv); + d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor); + d.altitude -= posRelOffsetEF.z; + + // calculate a velocity offset due to the antenna position offset and body rotation rate + // note: % operator is overloaded for cross product + Vector3f gyro(radians(_sitl->state.rollRate), + radians(_sitl->state.pitchRate), + radians(_sitl->state.yawRate)); + Vector3f velRelOffsetBF = gyro % posRelOffsetBF; + + // rotate the velocity offset into earth frame and add to the c.g. velocity + Vector3f velRelOffsetEF = rotmat * velRelOffsetBF; + d.speedN += velRelOffsetEF.x; + d.speedE += velRelOffsetEF.y; + d.speedD += velRelOffsetEF.z; + } + + // get delayed data + d.timestamp_ms = now_ms; + d = interpolate_data(d, _sitl->gps_delay_ms[instance]); + + // Applying GPS glitch + // Using first gps glitch + Vector3f glitch_offsets = _sitl->gps_glitch[idx]; + d.latitude += glitch_offsets.x; + d.longitude += glitch_offsets.y; + d.altitude += glitch_offsets.z; + + if (_sitl->gps_jam[idx] == 1) { + simulate_jamming(d); + } backend->publish(&d); } void GPS_Backend::update_read() { - // swallow any config bytes - char c; - read_from_autopilot(&c, 1); + // swallow any config bytes + char c; + read_from_autopilot(&c, 1); } /* diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 235234fd6ad7a0..87eafb00c43081 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -18,24 +18,15 @@ 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 -#include - -#ifndef HAL_SIM_GPS_ENABLED -#define HAL_SIM_GPS_ENABLED AP_SIM_ENABLED -#endif +#include "SIM_config.h" #if HAL_SIM_GPS_ENABLED -#ifndef AP_SIM_GPS_FILE_ENABLED -// really need to use AP_FileSystem for this. -#define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) -#endif - #include "SIM_SerialDevice.h" namespace SITL { @@ -53,6 +44,10 @@ struct GPS_Data { double roll_deg; double pitch_deg; bool have_lock; + float horizontal_acc; + float vertical_acc; + float speed_acc; + uint8_t num_sats; // Get heading [rad], where 0 = North in WGS-84 coordinate system float heading() const WARN_IF_UNUSED; @@ -79,126 +74,24 @@ 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; - -}; - -class GPS_FILE : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_FILE); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; -}; - -class GPS_GSOF : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_GSOF); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; - -private: - void send_gsof(const uint8_t *buf, const uint16_t size); - - // These packing utilities for GSOF perform a type-safe floating point byteswap. - // They return integer types because returning floating points would involve an extra copy. - uint64_t gsof_pack_double(const double& src) WARN_IF_UNUSED; - uint32_t gsof_pack_float(const float& src) WARN_IF_UNUSED; -}; - -class GPS_NMEA : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_NMEA); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; - -private: - - uint8_t nmea_checksum(const char *s); - void nmea_printf(const char *fmt, ...); - void update_nmea(const GPS_Data *d); - -}; - -class GPS_NOVA : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_NOVA); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; - - uint32_t device_baud() const override { return 19200; } - -private: - void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen); - uint32_t CRC32Value(uint32_t icrc); - uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); -}; - -class GPS_MSP : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_MSP); - - using GPS_Backend::GPS_Backend; - - void publish(const GPS_Data *d) override; -}; - -class GPS_SBP_Common : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_SBP_Common); + struct GPS_TOW { + // Number of weeks since midnight 5-6 January 1980 + uint16_t week; + // Time since start of the GPS week [mS] + uint32_t ms; + }; - using GPS_Backend::GPS_Backend; + static GPS_TOW gps_time(); protected: - void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); - -}; - -class GPS_SBP : public GPS_SBP_Common { -public: - CLASS_NO_COPY(GPS_SBP); - - using GPS_SBP_Common::GPS_SBP_Common; - - void publish(const GPS_Data *d) override; - -}; - -class GPS_SBP2 : public GPS_SBP_Common { -public: - CLASS_NO_COPY(GPS_SBP2); - - using GPS_SBP_Common::GPS_SBP_Common; - - void publish(const GPS_Data *d) override; - -}; - -class GPS_UBlox : public GPS_Backend { -public: - CLASS_NO_COPY(GPS_UBlox); - - using GPS_Backend::GPS_Backend; + uint8_t instance; + GPS &front; - void publish(const GPS_Data *d) override; + class SIM *_sitl; -private: - void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); + static void simulation_timeval(struct timeval *tv); }; class GPS : public SerialDevice { @@ -208,16 +101,30 @@ class GPS : public SerialDevice { enum Type { NONE = 0, +#if AP_SIM_GPS_UBLOX_ENABLED UBLOX = 1, +#endif +#if AP_SIM_GPS_NMEA_ENABLED NMEA = 5, +#endif +#if AP_SIM_GPS_SBP_ENABLED SBP = 6, +#endif #if AP_SIM_GPS_FILE_ENABLED FILE = 7, #endif +#if AP_SIM_GPS_NOVA_ENABLED NOVA = 8, +#endif +#if AP_SIM_GPS_SBP2_ENABLED SBP2 = 9, - GSOF = 11, // matches GPS_TYPE +#endif +#if AP_SIM_GPS_TRIMBLE_ENABLED + TRIMBLE = 11, // matches GPS_TYPE +#endif +#if AP_SIM_GPS_MSP_ENABLED MSP = 19, +#endif }; GPS(uint8_t _instance); @@ -233,16 +140,30 @@ class GPS : public SerialDevice { uint8_t instance; - int ext_fifo_fd; - - uint32_t last_update; // milliseconds + // The last time GPS data was written [mS] + uint32_t last_write_update_ms; // last 20 samples, allowing for up to 20 samples of delay GPS_Data _gps_history[20]; + // state of jamming simulation + struct { + uint32_t last_jam_ms; + uint32_t jam_start_ms; + uint32_t last_sats_change_ms; + uint32_t last_vz_change_ms; + uint32_t last_vel_change_ms; + uint32_t last_pos_change_ms; + uint32_t last_acc_change_ms; + double latitude; + double longitude; + } jamming[2]; + bool _gps_has_basestation_position; GPS_Data _gps_basestation_data; + void simulate_jamming(GPS_Data &d); + // get delayed data GPS_Data interpolate_data(const GPS_Data &d, uint32_t delay_ms); diff --git a/libraries/SITL/SIM_GPS_FILE.cpp b/libraries/SITL/SIM_GPS_FILE.cpp new file mode 100644 index 00000000000000..60868d4355cb6a --- /dev/null +++ b/libraries/SITL/SIM_GPS_FILE.cpp @@ -0,0 +1,70 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_FILE_ENABLED + +#include "SIM_GPS_FILE.h" + +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace SITL; + +/* + read file data logged from AP_GPS_DEBUG_LOGGING_ENABLED + */ +void GPS_FILE::publish(const GPS_Data *d) +{ + static int fd[2] = {-1,-1}; + static uint32_t base_time[2]; + const uint16_t lognum = uint16_t(_sitl->gps_log_num.get()); + if (instance > 1) { + return; + } + if (fd[instance] == -1) { + char fname[] = "gpsN_NNN.log"; + hal.util->snprintf(fname, 13, "gps%u_%03u.log", instance+1, lognum); + fd[instance] = open(fname, O_RDONLY|O_CLOEXEC); + if (fd[instance] == -1) { + return; + } + } + const uint32_t magic = 0x7fe53b04; + struct { + uint32_t magic; + uint32_t time_ms; + uint32_t n; + } header; + uint8_t *buf = nullptr; + while (true) { + if (::read(fd[instance], (void *)&header, sizeof(header)) != sizeof(header) || + header.magic != magic) { + goto rewind_file; + } + if (header.time_ms+base_time[instance] > AP_HAL::millis()) { + // not ready for this data yet + ::lseek(fd[instance], -sizeof(header), SEEK_CUR); + return; + } + buf = new uint8_t[header.n]; + if (buf != nullptr && ::read(fd[instance], buf, header.n) == ssize_t(header.n)) { + write_to_autopilot((const char *)buf, header.n); + delete[] buf; + buf = nullptr; + continue; + } + goto rewind_file; + } + +rewind_file: + ::printf("GPS[%u] rewind\n", unsigned(instance)); + base_time[instance] = AP_HAL::millis(); + ::lseek(fd[instance], 0, SEEK_SET); + delete[] buf; +} + +#endif // AP_SIM_GPS_FILE_ENABLED diff --git a/libraries/SITL/SIM_GPS_FILE.h b/libraries/SITL/SIM_GPS_FILE.h new file mode 100644 index 00000000000000..980f9602adfc94 --- /dev/null +++ b/libraries/SITL/SIM_GPS_FILE.h @@ -0,0 +1,20 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_FILE_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_FILE : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_FILE); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; +}; + +}; + +#endif // AP_SIM_GPS_FILE_ENABLED diff --git a/libraries/SITL/SIM_GPS_MSP.cpp b/libraries/SITL/SIM_GPS_MSP.cpp new file mode 100644 index 00000000000000..125f1b8de2650d --- /dev/null +++ b/libraries/SITL/SIM_GPS_MSP.cpp @@ -0,0 +1,90 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_MSP_ENABLED + +#include "SIM_GPS_MSP.h" + +#include + +#include + +using namespace SITL; + +/* + send MSP GPS data + */ +void GPS_MSP::publish(const GPS_Data *d) +{ + struct PACKED { + // header + struct PACKED { + uint8_t dollar = '$'; + uint8_t magic = 'X'; + uint8_t code = '<'; + uint8_t flags; + uint16_t cmd = 0x1F03; // GPS + uint16_t size = 52; + } hdr; + uint8_t instance; + uint16_t gps_week; + uint32_t ms_tow; + uint8_t fix_type; + uint8_t satellites_in_view; + uint16_t horizontal_pos_accuracy; // [cm] + uint16_t vertical_pos_accuracy; // [cm] + uint16_t horizontal_vel_accuracy; // [cm/s] + uint16_t hdop; + int32_t longitude; + int32_t latitude; + int32_t msl_altitude; // cm + int32_t ned_vel_north; // cm/s + int32_t ned_vel_east; + int32_t ned_vel_down; + uint16_t ground_course; // deg * 100, 0..36000 + uint16_t true_yaw; // deg * 100, values of 0..36000 are valid. 65535 = no data available + uint16_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t min; + uint8_t sec; + + // footer CRC + uint8_t crc; + } msp_gps {}; + + auto t = gps_time(); + struct timeval tv; + simulation_timeval(&tv); + auto *tm = gmtime(&tv.tv_sec); + + msp_gps.gps_week = t.week; + msp_gps.ms_tow = t.ms; + msp_gps.fix_type = d->have_lock?3:0; + msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3; + msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100; + msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100; + msp_gps.horizontal_vel_accuracy = 30; + msp_gps.hdop = 100; + msp_gps.longitude = d->longitude * 1.0e7; + msp_gps.latitude = d->latitude * 1.0e7; + msp_gps.msl_altitude = d->altitude * 100; + msp_gps.ned_vel_north = 100 * d->speedN; + msp_gps.ned_vel_east = 100 * d->speedE; + msp_gps.ned_vel_down = 100 * d->speedD; + msp_gps.ground_course = ToDeg(atan2f(d->speedE, d->speedN)) * 100; + msp_gps.true_yaw = wrap_360(d->yaw_deg)*100U; // can send 65535 for no yaw + msp_gps.year = tm->tm_year; + msp_gps.month = tm->tm_mon; + msp_gps.day = tm->tm_mday; + msp_gps.hour = tm->tm_hour; + msp_gps.min = tm->tm_min; + msp_gps.sec = tm->tm_sec; + + // CRC is over packet without first 3 bytes and trailing CRC byte + msp_gps.crc = crc8_dvb_s2_update(0, (uint8_t *)&msp_gps.hdr.flags, sizeof(msp_gps)-4); + + write_to_autopilot((const char *)&msp_gps, sizeof(msp_gps)); +} + +#endif // AP_SIM_GPS_MSP_ENABLED diff --git a/libraries/SITL/SIM_GPS_MSP.h b/libraries/SITL/SIM_GPS_MSP.h new file mode 100644 index 00000000000000..ae8ca8fa1a5078 --- /dev/null +++ b/libraries/SITL/SIM_GPS_MSP.h @@ -0,0 +1,20 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_MSP_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_MSP : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_MSP); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; +}; + +} + +#endif // AP_SIM_GPS_MSP_ENABLED diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp new file mode 100644 index 00000000000000..161f0ef0f04ff9 --- /dev/null +++ b/libraries/SITL/SIM_GPS_NMEA.cpp @@ -0,0 +1,127 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_NMEA_ENABLED + +#include "SIM_GPS_NMEA.h" + +#include +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +using namespace SITL; + +/* + formatted print of NMEA message, with checksum appended + */ +void GPS_NMEA::nmea_printf(const char *fmt, ...) +{ + va_list ap; + + va_start(ap, fmt); + char *s = nmea_vaprintf(fmt, ap); + va_end(ap); + if (s != nullptr) { + write_to_autopilot((const char*)s, strlen(s)); + free(s); + } +} + + +/* + send a new GPS NMEA packet + */ +void GPS_NMEA::publish(const GPS_Data *d) +{ + struct timeval tv; + struct tm *tm; + char tstring[20]; + char dstring[20]; + char lat_string[20]; + char lng_string[20]; + + simulation_timeval(&tv); + + tm = gmtime(&tv.tv_sec); + + // format time string + hal.util->snprintf(tstring, sizeof(tstring), "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + tv.tv_usec*1.0e-6); + + // format date string + hal.util->snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100); + + // format latitude + double deg = fabs(d->latitude); + hal.util->snprintf(lat_string, sizeof(lat_string), "%02u%08.5f,%c", + (unsigned)deg, + (deg - int(deg))*60, + d->latitude<0?'S':'N'); + + // format longitude + deg = fabs(d->longitude); + hal.util->snprintf(lng_string, sizeof(lng_string), "%03u%08.5f,%c", + (unsigned)deg, + (deg - int(deg))*60, + d->longitude<0?'W':'E'); + + nmea_printf("$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,", + tstring, + lat_string, + lng_string, + d->have_lock?1:0, + d->have_lock?_sitl->gps_numsats[instance]:3, + 1.2, + d->altitude); + + const float speed_mps = d->speed_2d(); + const float speed_knots = speed_mps * M_PER_SEC_TO_KNOTS; + const auto heading_rad = d->heading(); + + //$GPVTG,133.18,T,120.79,M,0.11,N,0.20,K,A*24 + nmea_printf("$GPVTG,%.2f,T,%.2f,M,%.2f,N,%.2f,K,A", + tstring, + heading_rad, + heading_rad, + speed_knots, + speed_knots * KNOTS_TO_METERS_PER_SECOND * 3.6); + + nmea_printf("$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,", + tstring, + d->have_lock?'A':'V', + lat_string, + lng_string, + speed_knots, + heading_rad, + dstring); + + if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_HDT) { + nmea_printf("$GPHDT,%.2f,T", d->yaw_deg); + } + else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_THS) { + nmea_printf("$GPTHS,%.2f,%c,T", d->yaw_deg, d->have_lock ? 'A' : 'V'); + } else if (_sitl->gps_hdg_enabled[instance] == SITL::SIM::GPS_HEADING_KSXT) { + // Unicore support + // $KSXT,20211016083433.00,116.31296102,39.95817066,49.4911,223.57,-11.32,330.19,0.024,,1,3,28,27,,,,-0.012,0.021,0.020,,*2D + nmea_printf("$KSXT,%04u%02u%02u%02u%02u%02u.%02u,%.8f,%.8f,%.4f,%.2f,%.2f,%.2f,%.2f,%.3f,%u,%u,%u,%u,,,,%.3f,%.3f,%.3f,,", + tm->tm_year+1900, tm->tm_mon+1, tm->tm_mday, tm->tm_hour, tm->tm_min, tm->tm_sec, unsigned(tv.tv_usec*1.e-4), + d->longitude, d->latitude, + d->altitude, + wrap_360(d->yaw_deg), + d->pitch_deg, + heading_rad, + speed_mps, + d->roll_deg, + d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed, + 3, // fixed rtk yaw solution, + d->have_lock?_sitl->gps_numsats[instance]:3, + d->have_lock?_sitl->gps_numsats[instance]:3, + d->speedE * 3.6, + d->speedN * 3.6, + -d->speedD * 3.6); + } +} + +#endif // AP_SIM_GPS_NMEA_ENABLED diff --git a/libraries/SITL/SIM_GPS_NMEA.h b/libraries/SITL/SIM_GPS_NMEA.h new file mode 100644 index 00000000000000..471f297857cadd --- /dev/null +++ b/libraries/SITL/SIM_GPS_NMEA.h @@ -0,0 +1,29 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_NMEA_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_NMEA : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_NMEA); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; + +private: + + uint8_t nmea_checksum(const char *s); + void nmea_printf(const char *fmt, ...); + void update_nmea(const GPS_Data *d); + +}; + +}; + +#endif // AP_SIM_GPS_NMEA_ENABLED diff --git a/libraries/SITL/SIM_GPS_NOVA.cpp b/libraries/SITL/SIM_GPS_NOVA.cpp new file mode 100644 index 00000000000000..cd4608b7f0b91d --- /dev/null +++ b/libraries/SITL/SIM_GPS_NOVA.cpp @@ -0,0 +1,177 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_NOVA_ENABLED + +#include "SIM_GPS_NOVA.h" + +#include + +using namespace SITL; + +void GPS_NOVA::publish(const GPS_Data *d) +{ + static struct PACKED nova_header + { + // 0 + uint8_t preamble[3]; + // 3 + uint8_t headerlength; + // 4 + uint16_t messageid; + // 6 + uint8_t messagetype; + //7 + uint8_t portaddr; + //8 + uint16_t messagelength; + //10 + uint16_t sequence; + //12 + uint8_t idletime; + //13 + uint8_t timestatus; + //14 + uint16_t week; + //16 + uint32_t tow; + //20 + uint32_t recvstatus; + // 24 + uint16_t resv; + //26 + uint16_t recvswver; + } header; + + struct PACKED psrdop + { + float gdop; + float pdop; + float hdop; + float htdop; + float tdop; + float cutoff; + uint32_t svcount; + // extra data for individual prns + } psrdop {}; + + struct PACKED bestpos + { + uint32_t solstat; + uint32_t postype; + double lat; + double lng; + double hgt; + float undulation; + uint32_t datumid; + float latsdev; + float lngsdev; + float hgtsdev; + // 4 bytes + uint8_t stnid[4]; + float diffage; + float sol_age; + uint8_t svstracked; + uint8_t svsused; + uint8_t svsl1; + uint8_t svsmultfreq; + uint8_t resv; + uint8_t extsolstat; + uint8_t galbeisigmask; + uint8_t gpsglosigmask; + } bestpos {}; + + struct PACKED bestvel + { + uint32_t solstat; + uint32_t veltype; + float latency; + float age; + double horspd; + double trkgnd; + // + up + double vertspd; + float resv; + } bestvel {}; + + const auto gps_tow = gps_time(); + + header.preamble[0] = 0xaa; + header.preamble[1] = 0x44; + header.preamble[2] = 0x12; + header.headerlength = sizeof(header); + header.week = gps_tow.week; + header.tow = gps_tow.ms; + + header.messageid = 174; + header.messagelength = sizeof(psrdop); + header.sequence += 1; + + psrdop.hdop = 1.20; + psrdop.htdop = 1.20; + nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&psrdop, sizeof(psrdop)); + + + header.messageid = 99; + header.messagelength = sizeof(bestvel); + header.sequence += 1; + + bestvel.horspd = norm(d->speedN, d->speedE); + bestvel.trkgnd = ToDeg(atan2f(d->speedE, d->speedN)); + bestvel.vertspd = -d->speedD; + + nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestvel, sizeof(bestvel)); + + + header.messageid = 42; + header.messagelength = sizeof(bestpos); + header.sequence += 1; + + bestpos.lat = d->latitude; + bestpos.lng = d->longitude; + bestpos.hgt = d->altitude; + bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3; + bestpos.latsdev=0.2; + bestpos.lngsdev=0.2; + bestpos.hgtsdev=0.2; + bestpos.solstat=0; + bestpos.postype=32; + + nova_send_message((uint8_t*)&header,sizeof(header),(uint8_t*)&bestpos, sizeof(bestpos)); +} + +void GPS_NOVA::nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen) +{ + write_to_autopilot((char*)header, headerlength); +write_to_autopilot((char*)payload, payloadlen); + + uint32_t crc = CalculateBlockCRC32(headerlength, header, (uint32_t)0); + crc = CalculateBlockCRC32(payloadlen, payload, crc); + + write_to_autopilot((char*)&crc, 4); +} + +#define CRC32_POLYNOMIAL 0xEDB88320L +uint32_t GPS_NOVA::CRC32Value(uint32_t icrc) +{ + int i; + uint32_t crc = icrc; + for ( i = 8 ; i > 0; i-- ) + { + if ( crc & 1 ) + crc = ( crc >> 1 ) ^ CRC32_POLYNOMIAL; + else + crc >>= 1; + } + return crc; +} + +uint32_t GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) +{ + while ( length-- != 0 ) + { + crc = ((crc >> 8) & 0x00FFFFFFL) ^ (CRC32Value(((uint32_t) crc ^ *buffer++) & 0xff)); + } + return( crc ); +} + +#endif // AP_SIM_GPS_NOVA_ENABLED diff --git a/libraries/SITL/SIM_GPS_NOVA.h b/libraries/SITL/SIM_GPS_NOVA.h new file mode 100644 index 00000000000000..4df22b32d8dc9a --- /dev/null +++ b/libraries/SITL/SIM_GPS_NOVA.h @@ -0,0 +1,30 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_NOVA_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_NOVA : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_NOVA); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; + + uint32_t device_baud() const override { return 19200; } + +private: + + void nova_send_message(uint8_t *header, uint8_t headerlength, uint8_t *payload, uint8_t payloadlen); + uint32_t CRC32Value(uint32_t icrc); + uint32_t CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); +}; + +}; + +#endif // AP_SIM_GPS_NOVA_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP.cpp b/libraries/SITL/SIM_GPS_SBP.cpp new file mode 100644 index 00000000000000..b3b8e77d890711 --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP.cpp @@ -0,0 +1,122 @@ +#include "SIM_GPS_SBP.h" + +#if AP_SIM_GPS_SBP_ENABLED + +#include + +using namespace SITL; + +void GPS_SBP::publish(const GPS_Data *d) +{ + struct sbp_heartbeat_t { + bool sys_error : 1; + bool io_error : 1; + bool nap_error : 1; + uint8_t res : 5; + uint8_t protocol_minor : 8; + uint8_t protocol_major : 8; + uint8_t res2 : 7; + bool ext_antenna : 1; + } hb; // 4 bytes + + struct PACKED sbp_gps_time_t { + uint16_t wn; //< GPS week number + uint32_t tow; //< GPS Time of Week rounded to the nearest ms + int32_t ns; //< Nanosecond remainder of rounded tow + uint8_t flags; //< Status flags (reserved) + } t; + struct PACKED sbp_pos_llh_t { + uint32_t tow; //< GPS Time of Week + double lat; //< Latitude + double lon; //< Longitude + double height; //< Height + uint16_t h_accuracy; //< Horizontal position accuracy estimate + uint16_t v_accuracy; //< Vertical position accuracy estimate + uint8_t n_sats; //< Number of satellites used in solution + uint8_t flags; //< Status flags + } pos; + struct PACKED sbp_vel_ned_t { + uint32_t tow; //< GPS Time of Week + int32_t n; //< Velocity North coordinate + int32_t e; //< Velocity East coordinate + int32_t d; //< Velocity Down coordinate + uint16_t h_accuracy; //< Horizontal velocity accuracy estimate + uint16_t v_accuracy; //< Vertical velocity accuracy estimate + uint8_t n_sats; //< Number of satellites used in solution + uint8_t flags; //< Status flags (reserved) + } velned; + struct PACKED sbp_dops_t { + uint32_t tow; //< GPS Time of Week + uint16_t gdop; //< Geometric Dilution of Precision + uint16_t pdop; //< Position Dilution of Precision + uint16_t tdop; //< Time Dilution of Precision + uint16_t hdop; //< Horizontal Dilution of Precision + uint16_t vdop; //< Vertical Dilution of Precision + uint8_t flags; //< Status flags (reserved) + } dops; + + static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; + static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100; + static const uint16_t SBP_DOPS_MSGTYPE = 0x0206; + static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201; + static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205; + + const auto gps_tow = gps_time(); + + t.wn = gps_tow.week; + t.tow = gps_tow.ms; + t.ns = 0; + t.flags = 0; + sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t); + + if (!d->have_lock) { + return; + } + + pos.tow = gps_tow.ms; + pos.lon = d->longitude; + pos.lat= d->latitude; + pos.height = d->altitude; + pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; + + // Send single point position solution + pos.flags = 0; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + // Send "pseudo-absolute" RTK position solution + pos.flags = 1; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + + velned.tow = gps_tow.ms; + velned.n = 1e3 * d->speedN; + velned.e = 1e3 * d->speedE; + velned.d = 1e3 * d->speedD; + velned.h_accuracy = 5e3; + velned.v_accuracy = 5e3; + velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; + velned.flags = 0; + sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); + + static uint32_t do_every_count = 0; + if (do_every_count % 5 == 0) { + + dops.tow = gps_tow.ms; + dops.gdop = 1; + dops.pdop = 1; + dops.tdop = 1; + dops.hdop = 100; + dops.vdop = 1; + dops.flags = 1; + sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), + (uint8_t*)&dops); + + hb.protocol_major = 0; //Sends protocol version 0 + sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), + (uint8_t*)&hb); + + } + do_every_count++; +} + +#endif // AP_SIM_GPS_SBP_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP.h b/libraries/SITL/SIM_GPS_SBP.h new file mode 100644 index 00000000000000..e0f1f58eab6123 --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP.h @@ -0,0 +1,23 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_SBP_ENABLED + +#include "SIM_GPS_SBP_Common.h" + +namespace SITL { + +class GPS_SBP : public GPS_SBP_Common { +public: + CLASS_NO_COPY(GPS_SBP); + + using GPS_SBP_Common::GPS_SBP_Common; + + void publish(const GPS_Data *d) override; + +}; + +}; + +#endif // AP_SIM_GPS_SBP_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP2.cpp b/libraries/SITL/SIM_GPS_SBP2.cpp new file mode 100644 index 00000000000000..1596d330af6fae --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP2.cpp @@ -0,0 +1,121 @@ +#include "SIM_GPS_SBP2.h" + +#if AP_SIM_GPS_SBP2_ENABLED + +#include + +using namespace SITL; + +void GPS_SBP2::publish(const GPS_Data *d) +{ + struct sbp_heartbeat_t { + bool sys_error : 1; + bool io_error : 1; + bool nap_error : 1; + uint8_t res : 5; + uint8_t protocol_minor : 8; + uint8_t protocol_major : 8; + uint8_t res2 : 7; + bool ext_antenna : 1; + } hb; // 4 bytes + + struct PACKED sbp_gps_time_t { + uint16_t wn; //< GPS week number + uint32_t tow; //< GPS Time of Week rounded to the nearest ms + int32_t ns; //< Nanosecond remainder of rounded tow + uint8_t flags; //< Status flags (reserved) + } t; + struct PACKED sbp_pos_llh_t { + uint32_t tow; //< GPS Time of Week + double lat; //< Latitude + double lon; //< Longitude + double height; //< Height + uint16_t h_accuracy; //< Horizontal position accuracy estimate + uint16_t v_accuracy; //< Vertical position accuracy estimate + uint8_t n_sats; //< Number of satellites used in solution + uint8_t flags; //< Status flags + } pos; + struct PACKED sbp_vel_ned_t { + uint32_t tow; //< GPS Time of Week + int32_t n; //< Velocity North coordinate + int32_t e; //< Velocity East coordinate + int32_t d; //< Velocity Down coordinate + uint16_t h_accuracy; //< Horizontal velocity accuracy estimate + uint16_t v_accuracy; //< Vertical velocity accuracy estimate + uint8_t n_sats; //< Number of satellites used in solution + uint8_t flags; //< Status flags (reserved) + } velned; + struct PACKED sbp_dops_t { + uint32_t tow; //< GPS Time of Week + uint16_t gdop; //< Geometric Dilution of Precision + uint16_t pdop; //< Position Dilution of Precision + uint16_t tdop; //< Time Dilution of Precision + uint16_t hdop; //< Horizontal Dilution of Precision + uint16_t vdop; //< Vertical Dilution of Precision + uint8_t flags; //< Status flags (reserved) + } dops; + + static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; + static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0102; + static const uint16_t SBP_DOPS_MSGTYPE = 0x0208; + static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A; + static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E; + + const auto gps_tow = gps_time(); + + t.wn = gps_tow.week; + t.tow = gps_tow.ms; + t.ns = 0; + t.flags = 1; + sbp_send_message(SBP_GPS_TIME_MSGTYPE, 0x2222, sizeof(t), (uint8_t*)&t); + + if (!d->have_lock) { + return; + } + + pos.tow = gps_tow.ms; + pos.lon = d->longitude; + pos.lat= d->latitude; + pos.height = d->altitude; + pos.h_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.v_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; + + // Send single point position solution + pos.flags = 1; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + // Send "pseudo-absolute" RTK position solution + pos.flags = 4; + sbp_send_message(SBP_POS_LLH_MSGTYPE, 0x2222, sizeof(pos), (uint8_t*)&pos); + + velned.tow = gps_tow.ms; + velned.n = 1e3 * d->speedN; + velned.e = 1e3 * d->speedE; + velned.d = 1e3 * d->speedD; + velned.h_accuracy = 5e3; + velned.v_accuracy = 5e3; + velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3; + velned.flags = 1; + sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned); + + static uint32_t do_every_count = 0; + if (do_every_count % 5 == 0) { + + dops.tow = gps_tow.ms; + dops.gdop = 1; + dops.pdop = 1; + dops.tdop = 1; + dops.hdop = 100; + dops.vdop = 1; + dops.flags = 1; + sbp_send_message(SBP_DOPS_MSGTYPE, 0x2222, sizeof(dops), + (uint8_t*)&dops); + + hb.protocol_major = 2; //Sends protocol version 2.0 + sbp_send_message(SBP_HEARTBEAT_MSGTYPE, 0x2222, sizeof(hb), + (uint8_t*)&hb); + } + do_every_count++; +} + +#endif // AP_SIM_GPS_SBP2_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP2.h b/libraries/SITL/SIM_GPS_SBP2.h new file mode 100644 index 00000000000000..580f430feaa977 --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP2.h @@ -0,0 +1,23 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_SBP2_ENABLED + +#include "SIM_GPS_SBP_Common.h" + +namespace SITL { + +class GPS_SBP2 : public GPS_SBP_Common { +public: + CLASS_NO_COPY(GPS_SBP2); + + using GPS_SBP_Common::GPS_SBP_Common; + + void publish(const GPS_Data *d) override; + +}; + +}; + +#endif // AP_SIM_GPS_SBP2_ENABLED diff --git a/libraries/SITL/SIM_GPS_SBP_Common.cpp b/libraries/SITL/SIM_GPS_SBP_Common.cpp new file mode 100644 index 00000000000000..90116d76da97b5 --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP_Common.cpp @@ -0,0 +1,34 @@ +#include "SIM_config.h" + +#if HAL_SIM_GPS_ENABLED + +#include "SIM_GPS_SBP_Common.h" + +#include + +using namespace SITL; + +void GPS_SBP_Common::sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload) +{ + if (len != 0 && payload == 0) { + return; //SBP_NULL_ERROR; + } + + uint8_t preamble = 0x55; + write_to_autopilot((char*)&preamble, 1); + write_to_autopilot((char*)&msg_type, 2); + write_to_autopilot((char*)&sender_id, 2); + write_to_autopilot((char*)&len, 1); + if (len > 0) { + write_to_autopilot((char*)payload, len); + } + + uint16_t crc; + crc = crc16_ccitt((uint8_t*)&(msg_type), 2, 0); + crc = crc16_ccitt((uint8_t*)&(sender_id), 2, crc); + crc = crc16_ccitt(&(len), 1, crc); + crc = crc16_ccitt(payload, len, crc); + write_to_autopilot((char*)&crc, 2); +} + +#endif diff --git a/libraries/SITL/SIM_GPS_SBP_Common.h b/libraries/SITL/SIM_GPS_SBP_Common.h new file mode 100644 index 00000000000000..5cb13e930f9d28 --- /dev/null +++ b/libraries/SITL/SIM_GPS_SBP_Common.h @@ -0,0 +1,25 @@ +#pragma once + +#include "SIM_config.h" + +#if HAL_SIM_GPS_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_SBP_Common : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_SBP_Common); + + using GPS_Backend::GPS_Backend; + +protected: + + void sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload); + +}; + +}; + +#endif diff --git a/libraries/SITL/SIM_GPS_Trimble.cpp b/libraries/SITL/SIM_GPS_Trimble.cpp new file mode 100644 index 00000000000000..9ddaf64027d2b2 --- /dev/null +++ b/libraries/SITL/SIM_GPS_Trimble.cpp @@ -0,0 +1,570 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_TRIMBLE_ENABLED + +#include "SIM_GPS_Trimble.h" + +#include +#include +#include + +#include + +using namespace SITL; + +void GPS_Trimble::publish(const GPS_Data *d) +{ + // This logic is to populate output buffer only with enabled channels. + // It also only sends each channel at the configured rate. + const uint64_t now = AP_HAL::millis(); + uint8_t buf[MAX_PAYLOAD_SIZE] = {}; + uint8_t payload_sz = 0; + uint8_t offset = 0; + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_TIME)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25 + constexpr uint8_t GSOF_POS_TIME_LEN { 0x0A }; + // TODO magic number until SITL supports GPS bootcount based on GPSN_ENABLE + const uint8_t bootcount = 17; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + enum class POS_FLAGS_1 : uint8_t { + NEW_POSITION = 1U << 0, + CLOCK_FIX_CALULATED = 1U << 1, + HORIZ_FROM_THIS_POS = 1U << 2, + HEIGHT_FROM_THIS_POS = 1U << 3, + RESERVED_4 = 1U << 4, + LEAST_SQ_POSITION = 1U << 5, + RESERVED_6 = 1U << 6, + POSITION_L1_PSEUDORANGES = 1U << 7 + }; + const uint8_t pos_flags_1 { + uint8_t(POS_FLAGS_1::NEW_POSITION) | + uint8_t(POS_FLAGS_1::CLOCK_FIX_CALULATED) | + uint8_t(POS_FLAGS_1::HORIZ_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::HEIGHT_FROM_THIS_POS) | + uint8_t(POS_FLAGS_1::RESERVED_4) | + uint8_t(POS_FLAGS_1::LEAST_SQ_POSITION) | + uint8_t(POS_FLAGS_1::POSITION_L1_PSEUDORANGES) + }; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + enum class POS_FLAGS_2 : uint8_t { + DIFFERENTIAL_POS = 1U << 0, + DIFFERENTIAL_POS_PHASE_RTK = 1U << 1, + POSITION_METHOD_FIXED_PHASE = 1U << 2, + OMNISTAR_ACTIVE = 1U << 3, + DETERMINED_WITH_STATIC_CONSTRAINT = 1U << 4, + NETWORK_RTK = 1U << 5, + DITHERED_RTK = 1U << 6, + BEACON_DGNSS = 1U << 7, + }; + + // Simulate a GPS without RTK in SIM since there is no RTK SIM params. + // This means these flags are unset: + // NETWORK_RTK, DITHERED_RTK, BEACON_DGNSS + uint8_t pos_flags_2 {0}; + if(d->have_lock) { + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DIFFERENTIAL_POS_PHASE_RTK); + pos_flags_2 |= uint8_t(POS_FLAGS_2::POSITION_METHOD_FIXED_PHASE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::OMNISTAR_ACTIVE); + pos_flags_2 |= uint8_t(POS_FLAGS_2::DETERMINED_WITH_STATIC_CONSTRAINT); + } + + const auto gps_tow = gps_time(); + const struct PACKED gsof_pos_time { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint32_t time_week_ms; + uint16_t time_week; + uint8_t num_sats; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%201 + uint8_t pos_flags_1; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Position%20flags%202 + uint8_t pos_flags_2; + uint8_t initialized_num; + } pos_time { + uint8_t(Gsof_Msg_Record_Type::POSITION_TIME), + GSOF_POS_TIME_LEN, + htobe32(gps_tow.ms), + htobe16(gps_tow.week), + d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3), + pos_flags_1, + pos_flags_2, + bootcount + }; + static_assert(sizeof(gsof_pos_time) - (sizeof(gsof_pos_time::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_time::RECORD_LEN)) == GSOF_POS_TIME_LEN); + + payload_sz += sizeof(pos_time); + memcpy(&buf[offset], &pos_time, sizeof(pos_time)); + offset += sizeof(pos_time); + } + } + + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::LLH)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::LLH)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_LLH.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____20 + constexpr uint8_t GSOF_POS_LEN = 0x18; + + const struct PACKED gsof_pos { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + uint64_t lat; + uint64_t lng; + uint64_t alt; + } pos { + uint8_t(Gsof_Msg_Record_Type::LLH), + GSOF_POS_LEN, + gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(static_cast(d->altitude)) + }; + static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); + + payload_sz += sizeof(pos); + memcpy(&buf[offset], &pos, sizeof(pos)); + offset += sizeof(pos); + } + } + + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Velocity.html + // use the smaller packet by ignoring local coordinate system + constexpr uint8_t GSOF_VEL_LEN = 0x0D; + + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + enum class VEL_FIELDS : uint8_t { + VALID = 1U << 0, + CONSECUTIVE_MEASUREMENTS = 1U << 1, + HEADING_VALID = 1U << 2, + RESERVED_3 = 1U << 3, + RESERVED_4 = 1U << 4, + RESERVED_5 = 1U << 5, + RESERVED_6 = 1U << 6, + RESERVED_7 = 1U << 7, + }; + uint8_t vel_flags {0}; + if(d->have_lock) { + vel_flags |= uint8_t(VEL_FIELDS::VALID); + vel_flags |= uint8_t(VEL_FIELDS::CONSECUTIVE_MEASUREMENTS); + vel_flags |= uint8_t(VEL_FIELDS::HEADING_VALID); + } + + const struct PACKED gsof_vel { + const uint8_t OUTPUT_RECORD_TYPE; + const uint8_t RECORD_LEN; + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_Flags.html#Velocity%20flags + uint8_t flags; + uint32_t horiz_m_p_s; + uint32_t heading_rad; + uint32_t vertical_m_p_s; + } vel { + uint8_t(Gsof_Msg_Record_Type::VELOCITY_DATA), + GSOF_VEL_LEN, + vel_flags, + gsof_pack_float(d->speed_2d()), + gsof_pack_float(d->heading()), + // Trimble API has ambiguous direction here. + // Intentionally narrow from double. + gsof_pack_float(static_cast(d->speedD)) + }; + static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); + + payload_sz += sizeof(vel); + memcpy(&buf[offset], &vel, sizeof(vel)); + offset += sizeof(vel); + } + } + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::PDOP_INFO)]; + + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12 + constexpr uint8_t GSOF_DOP_LEN = 0x10; + const struct PACKED gsof_dop { + const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::PDOP_INFO) }; + const uint8_t RECORD_LEN { GSOF_DOP_LEN }; + uint32_t pdop = htobe32(1); + uint32_t hdop = htobe32(1); + uint32_t vdop = htobe32(1); + uint32_t tdop = htobe32(1); + } dop {}; + // Check the payload size calculation in the compiler + constexpr auto dop_size = sizeof(gsof_dop); + static_assert(dop_size == 18); + constexpr auto dop_record_type_size = sizeof(gsof_dop::OUTPUT_RECORD_TYPE); + static_assert(dop_record_type_size == 1); + constexpr auto len_size = sizeof(gsof_dop::RECORD_LEN); + static_assert(len_size == 1); + constexpr auto dop_payload_size = dop_size - (dop_record_type_size + len_size); + static_assert(dop_payload_size == GSOF_DOP_LEN); + + payload_sz += sizeof(dop); + memcpy(&buf[offset], &dop, sizeof(dop)); + offset += sizeof(dop); + } + } + if (channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)] != Output_Rate::OFF){ + const auto last_time = last_publish_ms[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; + const auto desired_rate = channel_rates[uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO)]; + if (now - last_time > RateToPeriodMs(desired_rate)) { + // https://receiverhelp.trimble.com/oem-gnss/GSOFmessages_SIGMA.html + constexpr uint8_t GSOF_POS_SIGMA_LEN = 0x26; + const struct PACKED gsof_pos_sigma { + const uint8_t OUTPUT_RECORD_TYPE { uint8_t(Gsof_Msg_Record_Type::POSITION_SIGMA_INFO) }; + const uint8_t RECORD_LEN { GSOF_POS_SIGMA_LEN }; + uint32_t pos_rms = htobe32(0); + uint32_t sigma_e = htobe32(0); + uint32_t sigma_n = htobe32(0); + uint32_t cov_en = htobe32(0); + uint32_t sigma_up = htobe32(0); + uint32_t semi_major_axis = htobe32(0); + uint32_t semi_minor_axis = htobe32(0); + uint32_t orientation = htobe32(0); + uint32_t unit_variance = htobe32(0); + uint16_t n_epocs = htobe32(1); // Always 1 for kinematic. + } pos_sigma {}; + static_assert(sizeof(gsof_pos_sigma) - (sizeof(gsof_pos_sigma::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos_sigma::RECORD_LEN)) == GSOF_POS_SIGMA_LEN); + payload_sz += sizeof(pos_sigma); + memcpy(&buf[offset], &pos_sigma, sizeof(pos_sigma)); + offset += sizeof(pos_sigma); + } + } + + assert(offset == payload_sz); + + // Don't send empy frames when all channels are disabled; + if (payload_sz > 0) { + send_gsof(buf, payload_sz); + } + +} + +bool DCOL_Parser::dcol_parse(const char data_in) { + bool ret = false; + switch (parse_state) { + case Parse_State::WAITING_ON_STX: + if (data_in == STX) { + reset(); + parse_state = Parse_State::WAITING_ON_STATUS; + } + break; + case Parse_State::WAITING_ON_STATUS: + if (data_in != (uint8_t)Status::OK) { + // Invalid, status should always be OK. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } else { + status = static_cast(data_in); + parse_state = Parse_State::WAITING_ON_PACKET_TYPE; + } + break; + case Parse_State::WAITING_ON_PACKET_TYPE: + if (data_in == (uint8_t)Packet_Type::COMMAND_APPFILE) { + packet_type = Packet_Type::COMMAND_APPFILE; + } else { + // Unsupported command in this simulator. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + parse_state = Parse_State::WAITING_ON_LENGTH; + break; + case Parse_State::WAITING_ON_LENGTH: + expected_payload_length = data_in; + parse_state = Parse_State::WAITING_ON_PACKET_DATA; + break; + case Parse_State::WAITING_ON_PACKET_DATA: + payload[cur_payload_idx] = data_in; + if (++cur_payload_idx == expected_payload_length) { + parse_state = Parse_State::WAITING_ON_CSUM; + } + break; + case Parse_State::WAITING_ON_CSUM: + expected_csum = data_in; + parse_state = Parse_State::WAITING_ON_ETX; + break; + case Parse_State::WAITING_ON_ETX: + if (data_in != ETX) { + reset(); + } + if (!valid_csum()) { + // GSOF driver sent a packet with invalid CSUM. + // In real GSOF driver, the packet is simply ignored with no reply. + // In the SIM, treat this as a coding error. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } else { + ret = parse_payload(); + } + reset(); + break; + } + + return ret; +} + +uint32_t DCOL_Parser::RateToPeriodMs(const Output_Rate rate) { + uint32_t min_period_ms = 0; + switch (rate) { + case Output_Rate::OFF: + min_period_ms = 0; + break; + case Output_Rate::FREQ_10_HZ: + min_period_ms = 100; + break; + case Output_Rate::FREQ_50_HZ: + min_period_ms = 20; + break; + case Output_Rate::FREQ_100_HZ: + min_period_ms = 10; + break; + } + return min_period_ms; +} + + +bool DCOL_Parser::valid_csum() { + uint8_t sum = (uint8_t)status; + sum += (uint8_t)packet_type; + sum += expected_payload_length; + sum += crc_sum_of_bytes(payload, expected_payload_length); + return sum == expected_csum; +} + +bool DCOL_Parser::parse_payload() { + bool success = false; + if (packet_type == Packet_Type::COMMAND_APPFILE) { + success = parse_cmd_appfile(); + } + + return success; +} + +bool DCOL_Parser::parse_cmd_appfile() { + // A file control info block contains: + // * version + // * device type + // * start application file flag + // * factory settings flag + constexpr uint8_t file_control_info_block_sz = 4; + // An appfile header contains: + // * transmisison number + // * page index + // * max page index + constexpr uint8_t appfile_header_sz = 3; + constexpr uint8_t min_cmd_appfile_sz = file_control_info_block_sz + appfile_header_sz; + if (expected_payload_length < min_cmd_appfile_sz) { + return false; + } + + // For now, ignore appfile_trans_num, return success regardless. + // If the driver doesn't send the right value, it's not clear what the behavior is supposed to be. + // Also would need to handle re-sync. + // For now, just store it for debugging. + appfile_trans_num = payload[0]; + + // File control information block parsing: + // https://receiverhelp.trimble.com/oem-gnss/ICD_Subtype_Command64h_FileControlInfo.html + constexpr uint8_t expected_app_file_spec_version = 0x03; + constexpr uint8_t file_ctrl_idx = appfile_header_sz; + if (payload[file_ctrl_idx] != expected_app_file_spec_version) { + // Only version 3 is supported at this time. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t expected_dev_type = 0x00; + if (payload[file_ctrl_idx+1] != expected_dev_type) { + // Only "all" device type is supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t expected_start_flag = 0x01; + if (payload[file_ctrl_idx+2] != expected_start_flag) { + // Only "immediate" start type is supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + + constexpr uint8_t expected_factory_settings_flag = 0x00; + if (payload[file_ctrl_idx+3] != expected_factory_settings_flag) { + // Factory settings restore before appfile is not supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + constexpr uint8_t app_file_records_idx = appfile_header_sz + file_control_info_block_sz; + const uint8_t record_type = payload[app_file_records_idx]; + if (record_type == (uint8_t)Appfile_Record_Type::SERIAL_PORT_BAUD_RATE_FORMAT) { + // Serial port baud/format + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_SerialPort.html + + // Ignore serial port index (COM Port) since there's only one in SITL. + // Ignore baud rate because you can't change baud yet in SITL. + // Ignore parity because it can't be changed in SITL. + // Ignore flow control because it's not yet in SITL. + } else if (record_type == (uint8_t)Appfile_Record_Type::OUTPUT_MESSAGE){ + // Output Message + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html + + + // Ignore record length to save flash. + // Ignore port index since SITL is only one port. + if (payload[app_file_records_idx + 2] == (uint8_t)(Output_Msg_Msg_Type::GSOF)) { + const auto gsof_submessage_type = payload[app_file_records_idx + 6]; + const auto rate = payload[app_file_records_idx + 4]; + if (rate == (uint8_t)Output_Rate::OFF) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_10_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_50_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else if (rate == (uint8_t)Output_Rate::FREQ_100_HZ) { + channel_rates[gsof_submessage_type] = static_cast(rate); + } else { + // Unsupported GSOF rate in SITL. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + } else { + // Only some data output protocols are supported in SITL. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + } else { + // Other application file packets are not yet supported. + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + return true; +} + +void DCOL_Parser::reset() { + cur_payload_idx = 0; + expected_payload_length = 0; + parse_state = Parse_State::WAITING_ON_STX; + // To be pedantic, one could zero the bytes in the payload, + // but this is skipped because it's extra CPU. + + // Note - appfile_trans_number is intended to persist over parser resets. +} + + +void GPS_Trimble::send_gsof(const uint8_t *buf, const uint16_t size) +{ + // All Trimble "Data Collector" packets, including GSOF, are comprised of three fields: + // * A fixed-length packet header (dcol_header) + // * A variable-length data frame (buf) + // * A fixed-length packet trailer (dcol_trailer) + // Reference: // https://receiverhelp.trimble.com/oem-gnss/index.html#API_DataCollectorFormatPacketStructure.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____0 + + // status bitfield + // https://receiverhelp.trimble.com/oem-gnss/index.html#API_ReceiverStatusByte.html?TocPath=API%2520Documentation%257CData%2520collector%2520format%2520packets%257CData%2520collector%2520format%253A%2520packet%2520structure%257C_____1 + const uint8_t STATUS = 0xa8; + const uint8_t PACKET_TYPE = 0x40; // Report Packet 40h (GENOUT) + + // Before writing the GSOF data buffer, the GSOF header needs added between the DCOL header and the payload data frame. + // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_GSOF.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____2 + + static uint8_t TRANSMISSION_NUMBER = 0; // Functionally, this is a sequence number + // Most messages, even GSOF49, only take one page. For SIM, assume it. + assert(size < 0xFA); // GPS SIM doesn't yet support paging + constexpr uint8_t PAGE_INDEX = 0; + constexpr uint8_t MAX_PAGE_INDEX = 0; + const uint8_t gsof_header[3] = { + TRANSMISSION_NUMBER, + PAGE_INDEX, + MAX_PAGE_INDEX, + }; + ++TRANSMISSION_NUMBER; + + // A captured GSOF49 packet from BD940 has LENGTH field set to 0x6d = 109 bytes. + // A captured GSOF49 packet from BD940 has total bytes of 115 bytes. + // Thus, the following 5 bytes are not counted. + // 1) STX + // 2) STATUS + // 3) PACKET TYPE + // 4) LENGTH + // 5) CHECKSUM + // 6) ETX + // This aligns with manual's idea of data bytes: + // "Each message begins with a 4-byte header, followed by the bytes of data in each packet. The packet ends with a 2-byte trailer." + // Thus, for this implementation with single-page single-record per DCOL packet, + // the length is simply the sum of data packet size, the gsof_header size. + const uint8_t length = size + sizeof(gsof_header); + const uint8_t dcol_header[4] { + STX, + STATUS, + PACKET_TYPE, + length + }; + + + // Sum bytes (status + type + length + data bytes) and modulo 256 the summation + // Because it's a uint8, use natural overflow + uint8_t csum = STATUS + PACKET_TYPE + length; + for (size_t i = 0; i < ARRAY_SIZE(gsof_header); i++) { + csum += gsof_header[i]; + } + for (size_t i = 0; i < size; i++) { + csum += buf[i]; + } + + const uint8_t dcol_trailer[2] = { + csum, + ETX + }; + + write_to_autopilot((char*)dcol_header, sizeof(dcol_header)); + write_to_autopilot((char*)gsof_header, sizeof(gsof_header)); + write_to_autopilot((char*)buf, size); + write_to_autopilot((char*)dcol_trailer, sizeof(dcol_trailer)); + const uint8_t total_size = sizeof(dcol_header) + sizeof(gsof_header) + size + sizeof(dcol_trailer); + // Validate length based on everything but DCOL h + if(dcol_header[3] != total_size - (sizeof(dcol_header) + sizeof(dcol_trailer))) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } +} + +uint64_t GPS_Trimble::gsof_pack_double(const double& src) +{ + uint64_t dst; + static_assert(sizeof(src) == sizeof(dst)); + memcpy(&dst, &src, sizeof(dst)); + dst = htobe64(dst); + return dst; +} + +uint32_t GPS_Trimble::gsof_pack_float(const float& src) +{ + uint32_t dst; + static_assert(sizeof(src) == sizeof(dst)); + memcpy(&dst, &src, sizeof(dst)); + dst = htobe32(dst); + return dst; +} + +void GPS_Trimble::update_read() { + // Technically, the max command is slightly larger. + // This will only slightly slow the response for packets that big. + char c[MAX_PAYLOAD_SIZE]; + const auto n_read = read_from_autopilot(c, MAX_PAYLOAD_SIZE); + if (n_read > 0) { + for (uint8_t i = 0; i < n_read; i++) { + if (dcol_parse(c[i])) { + constexpr uint8_t response[1] = {(uint8_t)Command_Response::ACK}; + write_to_autopilot((char*)response, sizeof(response)); + } + // TODO handle NACK for failure + } + } +} + +#endif // AP_SIM_GPS_TRIMBLE_ENABLED diff --git a/libraries/SITL/SIM_GPS_Trimble.h b/libraries/SITL/SIM_GPS_Trimble.h new file mode 100644 index 00000000000000..d1c9a41d7eac90 --- /dev/null +++ b/libraries/SITL/SIM_GPS_Trimble.h @@ -0,0 +1,149 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_GPS_TRIMBLE_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class DCOL_Parser { + // The DCOL parser is used by Trimble GSOF devices. + // It's used for doing configuration. + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html +public: + // Feed data in to the DCOL parser. + // If the data reaches a parse state that needs to write ACK/NACK back out, + // the function returns true with a populated data_out value. + // Otherwise, it returns false waiting for more data. + bool dcol_parse(const char data_in); + + static constexpr uint8_t STX = 0x02; + static constexpr uint8_t ETX = 0x03; + + // Receiver status code + enum class Status : uint8_t { + OK = 0x00, + }; + + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPackets.html + enum class Command_Response : uint8_t { + ACK = 0x06, + NACK = 0x15, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Frequenc + enum class Output_Rate : uint8_t { + OFF = 0, + FREQ_10_HZ = 1, + FREQ_50_HZ = 15, + FREQ_100_HZ = 16, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_ApplicationFilePackets.html?tocpath=API%20Documentation%7CCommand%20and%20report%20packets%7CApplication%20file%20packets%7C_____0 + enum class Packet_Type : uint8_t { + COMMAND_APPFILE = 0x64, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + enum class Appfile_Record_Type : uint8_t { + SERIAL_PORT_BAUD_RATE_FORMAT = 0x02, + OUTPUT_MESSAGE = 0x07, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output + enum class Output_Msg_Msg_Type : uint8_t { + GSOF = 10, + }; + + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + enum class Gsof_Msg_Record_Type : uint8_t { + POSITION_TIME = 1, + LLH = 2, + VELOCITY_DATA = 8, + PDOP_INFO = 9, + POSITION_SIGMA_INFO = 12, + }; + +protected: + // https://receiverhelp.trimble.com/oem-gnss/API_DataCollectorFormatPacketStructure.html + static constexpr uint8_t MAX_PAYLOAD_SIZE = 255; + + // GSOF supports this many different packet types. + // Only a fraction are supported by the simulator. + // Waste some RAM and allocate arrays for the whole set. + // https://receiverhelp.trimble.com/oem-gnss/ICD_Command64h_AppFile_Output.html#Output2 + static constexpr uint8_t MAX_CHANNEL_NUM = 70; + // Rates of dynamically enabled channels. + // Assume factory behavior of no enabled channels. + // Each channel can send data out at its own rate. + Output_Rate channel_rates[MAX_CHANNEL_NUM] = {Output_Rate::OFF}; + + // Last publish time of dynamically enabled channels. + uint32_t last_publish_ms[MAX_CHANNEL_NUM]; + + static uint32_t RateToPeriodMs(const Output_Rate rate); + +private: + + // Internal parser implementation state + enum class Parse_State { + WAITING_ON_STX, + WAITING_ON_STATUS, + WAITING_ON_PACKET_TYPE, + WAITING_ON_LENGTH, + WAITING_ON_PACKET_DATA, + WAITING_ON_CSUM, + WAITING_ON_ETX, + }; + + bool valid_csum(); + bool parse_payload(); + // https://receiverhelp.trimble.com/oem-gnss/ICD_Pkt_Command64h_APPFILE.html + bool parse_cmd_appfile(); + + + // states for currently parsing packet + Status status; + Parse_State parse_state = {Parse_State::WAITING_ON_STX}; + Packet_Type packet_type; + // This is the length in the header. + uint8_t expected_payload_length; + // This is the increasing tally of bytes per packet. + uint8_t cur_payload_idx; + // This is the expected packet checksum in the trailer. + uint8_t expected_csum; + + // The application file record transmission number + uint8_t appfile_trans_num; + + uint8_t payload[MAX_PAYLOAD_SIZE]; + + // Clear all parser state/flags for handling a fresh packet. + void reset(); +}; + +class GPS_Trimble : public GPS_Backend, public DCOL_Parser { +public: + CLASS_NO_COPY(GPS_Trimble); + + using GPS_Backend::GPS_Backend; + + + // GPS_Backend overrides + void publish(const GPS_Data *d) override; + void update_read() override; + +private: + void send_gsof(const uint8_t *buf, const uint16_t size); + + // These packing utilities for GSOF perform a type-safe floating point byteswap. + // They return integer types because returning floating points would involve an extra copy. + uint64_t gsof_pack_double(const double& src) WARN_IF_UNUSED; + uint32_t gsof_pack_float(const float& src) WARN_IF_UNUSED; +}; + +}; + +#endif // AP_SIM_GPS_TRIMBLE_ENABLED diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp new file mode 100644 index 00000000000000..5d743cbfe9b3a3 --- /dev/null +++ b/libraries/SITL/SIM_GPS_UBLOX.cpp @@ -0,0 +1,324 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_UBLOX_ENABLED + +#include "SIM_GPS_UBLOX.h" + +#include + +using namespace SITL; + +/* + send a UBLOX GPS message + */ +void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size) +{ + const uint8_t PREAMBLE1 = 0xb5; + const uint8_t PREAMBLE2 = 0x62; + const uint8_t CLASS_NAV = 0x1; + uint8_t hdr[6], chk[2]; + hdr[0] = PREAMBLE1; + hdr[1] = PREAMBLE2; + hdr[2] = CLASS_NAV; + hdr[3] = msgid; + hdr[4] = size & 0xFF; + hdr[5] = size >> 8; + chk[0] = chk[1] = hdr[2]; + chk[1] += (chk[0] += hdr[3]); + chk[1] += (chk[0] += hdr[4]); + chk[1] += (chk[0] += hdr[5]); + for (uint16_t i=0; ilongitude * 1.0e7; + pos.latitude = d->latitude * 1.0e7; + pos.altitude_ellipsoid = d->altitude * 1000.0f; + pos.altitude_msl = d->altitude * 1000.0f; + pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000; + pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000; + + status.time = gps_tow.ms; + status.fix_type = d->have_lock?3:0; + status.fix_status = d->have_lock?1:0; + status.differential_status = 0; + status.res = 0; + status.time_to_first_fix = 0; + status.uptime = AP_HAL::millis(); + + velned.time = gps_tow.ms; + velned.ned_north = 100.0f * d->speedN; + velned.ned_east = 100.0f * d->speedE; + velned.ned_down = 100.0f * d->speedD; + velned.speed_2d = norm(d->speedN, d->speedE) * 100; + velned.speed_3d = norm(d->speedN, d->speedE, d->speedD) * 100; + velned.heading_2d = ToDeg(atan2f(d->speedE, d->speedN)) * 100000.0f; + if (velned.heading_2d < 0.0f) { + velned.heading_2d += 360.0f * 100000.0f; + } + velned.speed_accuracy = 40; + velned.heading_accuracy = 4; + + memset(&sol, 0, sizeof(sol)); + sol.fix_type = d->have_lock?3:0; + sol.fix_status = 221; + sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3; + sol.time = gps_tow.ms; + sol.week = gps_tow.week; + + dop.time = gps_tow.ms; + dop.gDOP = 65535; + dop.pDOP = 65535; + dop.tDOP = 65535; + dop.vDOP = 200; + dop.hDOP = 121; + dop.nDOP = 65535; + dop.eDOP = 65535; + + pvt.itow = gps_tow.ms; + pvt.year = 0; + pvt.month = 0; + pvt.day = 0; + pvt.hour = 0; + pvt.min = 0; + pvt.sec = 0; + pvt.valid = 0; // invalid utc date + pvt.t_acc = 0; + pvt.nano = 0; + pvt.fix_type = d->have_lock? 0x3 : 0; + pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok + pvt.flags2 =0; + pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3; + pvt.lon = d->longitude * 1.0e7; + pvt.lat = d->latitude * 1.0e7; + pvt.height = d->altitude * 1000.0f; + pvt.h_msl = d->altitude * 1000.0f; + pvt.h_acc = _sitl->gps_accuracy[instance] * 1000; + pvt.v_acc = _sitl->gps_accuracy[instance] * 1000; + pvt.velN = 1000.0f * d->speedN; + pvt.velE = 1000.0f * d->speedE; + pvt.velD = 1000.0f * d->speedD; + pvt.gspeed = norm(d->speedN, d->speedE) * 1000; + pvt.head_mot = ToDeg(atan2f(d->speedE, d->speedN)) * 1.0e5; + pvt.s_acc = 40; + pvt.head_acc = 38 * 1.0e5; + pvt.p_dop = 65535; + memset(pvt.reserved1, '\0', ARRAY_SIZE(pvt.reserved1)); + pvt.headVeh = 0; + memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2)); + + if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { + const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get(); + const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get(); + Vector3f rel_antenna_pos = ant2_pos - ant1_pos; + Matrix3f rot; + // project attitude back using gyros to get antenna orientation at time of GPS sample + Vector3f gyro(radians(_sitl->state.rollRate), + radians(_sitl->state.pitchRate), + radians(_sitl->state.yawRate)); + rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(d->yaw_deg)); + const float lag = _sitl->gps_delay_ms[instance] * 0.001; + rot.rotate(gyro * (-lag)); + rel_antenna_pos = rot * rel_antenna_pos; + relposned.version = 1; + relposned.iTOW = gps_tow.ms; + relposned.relPosN = rel_antenna_pos.x * 100; + relposned.relPosE = rel_antenna_pos.y * 100; + relposned.relPosD = rel_antenna_pos.z * 100; + relposned.relPosLength = rel_antenna_pos.length() * 100; + relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5; + relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid; + } + + send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos)); + send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status)); + send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned)); + send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol)); + send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop)); + send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt)); + if (_sitl->gps_hdg_enabled[instance] > SITL::SIM::GPS_HEADING_NONE) { + send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned)); + } + + if (gps_tow.ms > _next_nav_sv_info_time) { + svinfo.itow = gps_tow.ms; + svinfo.numCh = 32; + svinfo.globalFlags = 4; // u-blox 8/M8 + // fill in the SV's with some data even though firmware does not currently use it + // note that this is not using num_sats as we aren't dynamically creating this to match + for (uint8_t i = 0; i < SV_COUNT; i++) { + svinfo.sv[i].chn = i; + svinfo.sv[i].svid = i; + svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 0x7 : 0x6; // sv used, diff correction data, orbit information + svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized + svinfo.sv[i].cno = MAX(20, 30 - i); + svinfo.sv[i].elev = MAX(30, 90 - i); + svinfo.sv[i].azim = i; + // not bothering to fill in prRes + } + send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo)); + _next_nav_sv_info_time = gps_tow.ms + 10000; // 10 second delay + } +} + +#endif // AP_SIM_GPS_UBLOX_ENABLED diff --git a/libraries/SITL/SIM_GPS_UBLOX.h b/libraries/SITL/SIM_GPS_UBLOX.h new file mode 100644 index 00000000000000..6ab5d01eea76fc --- /dev/null +++ b/libraries/SITL/SIM_GPS_UBLOX.h @@ -0,0 +1,23 @@ +#include "SIM_config.h" + +#if AP_SIM_GPS_UBLOX_ENABLED + +#include "SIM_GPS.h" + +namespace SITL { + +class GPS_UBlox : public GPS_Backend { +public: + CLASS_NO_COPY(GPS_UBlox); + + using GPS_Backend::GPS_Backend; + + void publish(const GPS_Data *d) override; + +private: + void send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size); +}; + +}; + +#endif // AP_SIM_GPS_UBLOX_ENABLED 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_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_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.cpp b/libraries/SITL/SIM_IntelligentEnergy24.cpp index 2445df9a863a95..7b2456ee6b5838 100644 --- a/libraries/SITL/SIM_IntelligentEnergy24.cpp +++ b/libraries/SITL/SIM_IntelligentEnergy24.cpp @@ -29,25 +29,27 @@ extern const AP_HAL::HAL& hal; using namespace SITL; +#define MAX_TANK_PRESSURE 300 //(bar) + // table of user settable parameters const AP_Param::GroupInfo IntelligentEnergy24::var_info[] = { // @Param: ENABLE // @DisplayName: IntelligentEnergy 2.4kWh FuelCell sim enable/disable // @Description: Allows you to enable (1) or disable (0) the FuelCell simulator - // @Values: 0:Disabled,1:Enabled + // @Values: 0:Disabled,1:V1 Protocol,2:V2 Protocol // @User: Advanced AP_GROUPINFO("ENABLE", 1, IntelligentEnergy24, enabled, 0), // @Param: STATE // @DisplayName: Explicitly set state - // @Description: Explicity specify a state for the generator to be in + // @Description: Explicitly specify a state for the generator to be in // @User: Advanced AP_GROUPINFO("STATE", 2, IntelligentEnergy24, set_state, -1), // @Param: ERROR // @DisplayName: Explicitly set error code - // @Description: Explicity specify an error code to send to the generator + // @Description: Explicitly specify an error code to send to the generator // @User: Advanced AP_GROUPINFO("ERROR", 3, IntelligentEnergy24, err_code, 0), @@ -64,15 +66,14 @@ void IntelligentEnergy24::update(const struct sitl_input &input) if (!enabled.get()) { return; } - // gcs().send_text(MAV_SEVERITY_INFO, "fuelcell update"); update_send(); } void IntelligentEnergy24::update_send() { - // just send a chunk of data at 1Hz: + // just send a chunk of data at 2 Hz: const uint32_t now = AP_HAL::millis(); - if (now - last_sent_ms < 500) { + if (now - last_data_sent_ms < 500) { return; } @@ -80,7 +81,7 @@ void IntelligentEnergy24::update_send() float amps = discharge ? -20.0f : 20.0f; // Update pack capacity remaining - bat_capacity_mAh += amps*(now - last_sent_ms)/3600.0f; + bat_capacity_mAh += amps*(now - last_data_sent_ms)/3600.0f; // From capacity remaining approximate voltage by linear interpolation const float min_bat_vol = 42.0f; @@ -90,7 +91,7 @@ void IntelligentEnergy24::update_send() // Simulate tank pressure // Scale tank pressure linearly to a percentage. // Min = 5 bar, max = 300 bar, PRESS_GRAD = 1/295. - const int16_t tank_bar = linear_interpolate(5, 295, bat_capacity_mAh / max_bat_capactiy_mAh, 0, 1); + const int16_t tank_bar = linear_interpolate(5, MAX_TANK_PRESSURE, bat_capacity_mAh / max_bat_capactiy_mAh, 0, 1); battery_voltage = bat_capacity_mAh / max_bat_capactiy_mAh * (max_bat_vol - min_bat_vol) + min_bat_vol; @@ -112,10 +113,13 @@ void IntelligentEnergy24::update_send() state = 2; // Running } - last_sent_ms = now; + last_data_sent_ms = now; char message[128]; - hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.1f,%i,%u,%i,%u,%u>\n", + + if (enabled.get() == 1) { + // V1 Protocol + hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.1f,%i,%u,%i,%u,%u>\n", tank_bar, battery_voltage, (signed)pwr_out, @@ -124,7 +128,69 @@ void IntelligentEnergy24::update_send() (unsigned)state, (unsigned)err_code); + } else { + // V2 Protocol + + // version message sent at 0.2 Hz + if (now - last_ver_sent_ms > 5e3) { + // PCM software part number, software version number, protocol number, hardware serial number, check-sum + hal.util->snprintf(message, ARRAY_SIZE(message), "[10011867,2.132,4,IE12160A8040015,7]\n"); + + if ((unsigned)write_to_autopilot(message, strlen(message)) != strlen(message)) { + AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno)); + } + last_ver_sent_ms = now; + } + + // data message + memset(&message, 0, sizeof(message)); + int8_t tank_remaining_pct = (float)tank_bar / MAX_TANK_PRESSURE * 100.0; + + hal.util->snprintf(message, ARRAY_SIZE(message), "<%i,%.2f,%.1f,%i,%u,%i,%i,%u,%u,%i,%s,", // last blank , is for fuel cell to send info string up to 32 char ASCII + tank_remaining_pct, + 0.67f, // inlet pressure (bar) + battery_voltage, + (signed)pwr_out, + (unsigned)spm_pwr, + 0, // unit at fault (0 = no fault) + (signed)battery_pwr, + (unsigned)state, + (unsigned)err_code, + 0, // fault state 2 (0 = no fault) + get_error_string(err_code)); + + // calculate the checksum + uint8_t checksum = 0; + for (uint8_t i = 0; i < ARRAY_SIZE(message); i++) { + if (message[i] == 0) { + break; + } + checksum += message[i]; + } + // checksum is inverted 8-bit + checksum = ~checksum; + + // add the checksum to the end of the message + char data_end[7]; + hal.util->snprintf(data_end, ARRAY_SIZE(data_end), "%u>\n", checksum); + strncat(message, data_end, ARRAY_SIZE(data_end)); + + } + if ((unsigned)write_to_autopilot(message, strlen(message)) != strlen(message)) { AP_HAL::panic("Failed to write to autopilot: %s", strerror(errno)); } } + +const char * IntelligentEnergy24::get_error_string(const uint32_t code) +{ + switch (code) { + case 20: + return "THERMAL MNGMT"; + + default: + break; + } + + return ""; +} diff --git a/libraries/SITL/SIM_IntelligentEnergy24.h b/libraries/SITL/SIM_IntelligentEnergy24.h index eecd0939e27d52..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 @@ -60,6 +60,8 @@ class IntelligentEnergy24 : public IntelligentEnergy { void update_send(); + const char * get_error_string(const uint32_t code); + AP_Int8 enabled; // enable sim AP_Int8 set_state; AP_Int32 err_code; @@ -67,7 +69,8 @@ class IntelligentEnergy24 : public IntelligentEnergy { float battery_voltage = 50.4f; float bat_capacity_mAh = 3300; bool discharge = true; // used to switch between battery charging and discharging - uint32_t last_sent_ms; + uint32_t last_data_sent_ms; + uint32_t last_ver_sent_ms; }; 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..885e469564c168 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; 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_MicroStrain.cpp b/libraries/SITL/SIM_MicroStrain.cpp index ce126f878bd97a..f4ec6c78f6881a 100644 --- a/libraries/SITL/SIM_MicroStrain.cpp +++ b/libraries/SITL/SIM_MicroStrain.cpp @@ -13,7 +13,7 @@ along with this program. If not, see . */ /* - simulate LORD MicroStrain serial device + simulate MicroStrain GNSS-INS devices */ #include "SIM_MicroStrain.h" #include @@ -24,15 +24,12 @@ using namespace SITL; -MicroStrain5::MicroStrain5() :SerialDevice::SerialDevice() +MicroStrain::MicroStrain() :SerialDevice::SerialDevice() { } -/* - get timeval using simulation time - */ -static void simulation_timeval(struct timeval *tv) +void MicroStrain::simulation_timeval(struct timeval *tv) { uint64_t now = AP_HAL::micros64(); static uint64_t first_usec; @@ -48,7 +45,7 @@ static void simulation_timeval(struct timeval *tv) tv->tv_usec = new_usec % 1000000ULL; } -void MicroStrain5::generate_checksum(MicroStrain_Packet& packet) +void MicroStrain::generate_checksum(MicroStrain_Packet& packet) { uint8_t checksumByte1 = 0; uint8_t checksumByte2 = 0; @@ -67,7 +64,7 @@ void MicroStrain5::generate_checksum(MicroStrain_Packet& packet) packet.checksum[1] = checksumByte2; } -void MicroStrain5::send_packet(MicroStrain_Packet packet) +void MicroStrain::send_packet(MicroStrain_Packet packet) { generate_checksum(packet); @@ -77,7 +74,7 @@ void MicroStrain5::send_packet(MicroStrain_Packet packet) } -void MicroStrain5::send_imu_packet(void) +void MicroStrain::send_imu_packet(void) { const auto &fdm = _sitl->state; MicroStrain_Packet packet; @@ -135,7 +132,6 @@ void MicroStrain5::send_imu_packet(void) send_packet(packet); } - void MicroStrain5::send_gnss_packet(void) { const auto &fdm = _sitl->state; @@ -148,9 +144,9 @@ void MicroStrain5::send_gnss_packet(void) packet.header[1] = 0x65; // Sync Two packet.header[2] = 0x81; // GNSS Descriptor - // Add GPS Time + // Add GPS Timestamp packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size - packet.payload[packet.payload_size++] = 0x09; // Descriptor + packet.payload[packet.payload_size++] = 0xD3; // Descriptor put_double(packet, (double) tv.tv_sec); put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); put_int(packet, 0); @@ -205,6 +201,8 @@ void MicroStrain5::send_gnss_packet(void) send_packet(packet); } + + void MicroStrain5::send_filter_packet(void) { const auto &fdm = _sitl->state; @@ -217,9 +215,9 @@ void MicroStrain5::send_filter_packet(void) packet.header[1] = 0x65; // Sync Two packet.header[2] = 0x82; // Filter Descriptor - // Add Filter Time - packet.payload[packet.payload_size++] = 0x0E; // Filter Time Field Size - packet.payload[packet.payload_size++] = 0x11; // Descriptor + // Add GPS Timestamp Shared Data + packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size + packet.payload[packet.payload_size++] = 0xD3; // Descriptor put_double(packet, (double) tv.tv_usec / 1e6); put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); put_int(packet, 0x0001); @@ -256,34 +254,34 @@ void MicroStrain5::send_filter_packet(void) /* send MicroStrain data */ -void MicroStrain5::update(void) +void MicroStrain::update(void) { if (!init_sitl_pointer()) { return; } - uint32_t us_between_imu_packets = 20000; - uint32_t us_between_gnss_packets = 250000; - uint32_t us_between_filter_packets = 100000; + uint32_t ms_between_imu_packets = 40; + uint32_t ms_between_gnss_packets = 500; + uint32_t ms_between_filter_packets = 40; - uint32_t now = AP_HAL::micros(); - if (now - last_imu_pkt_us >= us_between_imu_packets) { - last_imu_pkt_us = now; + uint32_t now = AP_HAL::millis(); + if (now - last_imu_pkt_ms >= ms_between_imu_packets) { + last_imu_pkt_ms = now; send_imu_packet(); } - if (now - last_gnss_pkt_us >= us_between_gnss_packets) { - last_gnss_pkt_us = now; + if (now - last_gnss_pkt_ms >= ms_between_gnss_packets) { + last_gnss_pkt_ms = now; send_gnss_packet(); } - if (now - last_filter_pkt_us >= us_between_filter_packets) { - last_filter_pkt_us = now; + if (now - last_filter_pkt_ms >= ms_between_filter_packets) { + last_filter_pkt_ms = now; send_filter_packet(); } } -void MicroStrain5::put_float(MicroStrain_Packet &packet, float f) +void MicroStrain::put_float(MicroStrain_Packet &packet, float f) { uint32_t fbits = 0; memcpy(&fbits, &f, sizeof(fbits)); @@ -291,7 +289,7 @@ void MicroStrain5::put_float(MicroStrain_Packet &packet, float f) packet.payload_size += sizeof(float); } -void MicroStrain5::put_double(MicroStrain_Packet &packet, double d) +void MicroStrain::put_double(MicroStrain_Packet &packet, double d) { uint64_t dbits = 0; memcpy(&dbits, &d, sizeof(dbits)); @@ -299,7 +297,7 @@ void MicroStrain5::put_double(MicroStrain_Packet &packet, double d) packet.payload_size += sizeof(double); } -void MicroStrain5::put_int(MicroStrain_Packet &packet, uint16_t t) +void MicroStrain::put_int(MicroStrain_Packet &packet, uint16_t t) { put_be16_ptr(&packet.payload[packet.payload_size], t); packet.payload_size += sizeof(uint16_t); diff --git a/libraries/SITL/SIM_MicroStrain.h b/libraries/SITL/SIM_MicroStrain.h index ebed27a4dae81d..bb2d3ebe73325b 100644 --- a/libraries/SITL/SIM_MicroStrain.h +++ b/libraries/SITL/SIM_MicroStrain.h @@ -4,9 +4,9 @@ //PARAMS: // param set AHRS_EKF_TYPE 11 // param set EAHRS_TYPE 2 -// param set SERIAL4_PROTOCOL 36 -// param set SERIAL4_BAUD 115 -// sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:MicroStrain +// param set SERIAL3_PROTOCOL 36 +// param set SERIAL3_BAUD 115 +// sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG #pragma once #include "SIM_Aircraft.h" @@ -17,16 +17,17 @@ namespace SITL { -class MicroStrain5 : public SerialDevice +class MicroStrain : public SerialDevice { + // This class implements the common MicroStrain driver support. public: - MicroStrain5(); + MicroStrain(); // update state void update(void); -private: +protected: struct MicroStrain_Packet { uint8_t header[4]; uint8_t payload[256]; @@ -35,23 +36,44 @@ class MicroStrain5 : public SerialDevice size_t payload_size = 0; }; - uint32_t last_imu_pkt_us; - uint32_t last_gnss_pkt_us; - uint32_t last_filter_pkt_us; + uint32_t last_imu_pkt_ms; + uint32_t last_gnss_pkt_ms; + uint32_t last_filter_pkt_ms; void generate_checksum(MicroStrain_Packet&); void send_packet(MicroStrain_Packet); void send_imu_packet(); - void send_gnss_packet(); - void send_filter_packet(); + virtual void send_gnss_packet() = 0; + virtual void send_filter_packet() = 0; void put_float(MicroStrain_Packet&, float); void put_double(MicroStrain_Packet&, double); void put_int(MicroStrain_Packet&, uint16_t); + // get timeval using simulation time + static void simulation_timeval(struct timeval *tv); + uint64_t start_us; }; +class MicroStrain5 : public MicroStrain +{ + // This is a specialization for the 3DM-GX5-GNSS/INS +private: + void send_gnss_packet() override; + void send_filter_packet() override; + +}; + +class MicroStrain7 : public MicroStrain +{ + // This is a specialization for the 3DM-GQ7-GNSS/INS +private: + void send_gnss_packet() override; + void send_filter_packet() override; + +}; + } diff --git a/libraries/SITL/SIM_MicroStrain7.cpp b/libraries/SITL/SIM_MicroStrain7.cpp new file mode 100644 index 00000000000000..4664942049e78d --- /dev/null +++ b/libraries/SITL/SIM_MicroStrain7.cpp @@ -0,0 +1,147 @@ +/* + 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 MicroStrain 7-series GNSS-INS devices +*/ + +#include "SIM_MicroStrain.h" + +using namespace SITL; + +void MicroStrain7::send_gnss_packet(void) +{ + const auto &fdm = _sitl->state; + + constexpr uint8_t descriptors[2] = {0x91, 0x92}; + for (uint8_t i = 0; i < ARRAY_SIZE(descriptors); i++) { + MicroStrain_Packet packet; + + struct timeval tv; + simulation_timeval(&tv); + + packet.header[0] = 0x75; // Sync One + packet.header[1] = 0x65; // Sync Two + packet.header[2] = descriptors[i]; // GNSS Descriptor + + // Add GPS Timestamp + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm + packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size + packet.payload[packet.payload_size++] = 0xD3; // Descriptor + put_double(packet, (double) tv.tv_sec); + put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); + put_int(packet, 0); + + // Add GNSS Fix Information + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_fix_info.htm + packet.payload[packet.payload_size++] = 0x08; // GNSS Fix Field Size + packet.payload[packet.payload_size++] = 0x0B; // Descriptor + packet.payload[packet.payload_size++] = 0x00; // Fix type FIX_3D + packet.payload[packet.payload_size++] = 19; // Sat count + put_int(packet, 0); // Fix flags + put_int(packet, 0); // Valid flags + + // Add GNSS LLH position + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_llh_pos.htm + packet.payload[packet.payload_size++] = 0x2C; // GNSS LLH Field Size + packet.payload[packet.payload_size++] = 0x03; // Descriptor + put_double(packet, fdm.latitude); + put_double(packet, fdm.longitude); + put_double(packet, 0); // Height above ellipsoid - unused + put_double(packet, fdm.altitude); + put_float(packet, 0.5f); // Horizontal accuracy + put_float(packet, 0.5f); // Vertical accuracy + put_int(packet, 31); // Valid flags + + // Add DOP Data + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_dop.htm + packet.payload[packet.payload_size++] = 0x20; // DOP Field Size + packet.payload[packet.payload_size++] = 0x07; // Descriptor + put_float(packet, 0); // GDOP + put_float(packet, 0); // PDOP + put_float(packet, 0); // HDOP + put_float(packet, 0); // VDOP + put_float(packet, 0); // TDOP + put_float(packet, 0); // NDOP + put_float(packet, 0); // EDOP + put_int(packet, 127); + + // Add GNSS NED velocity + packet.payload[packet.payload_size++] = 0x24; // GNSS NED Velocity Field Size + packet.payload[packet.payload_size++] = 0x05; // Descriptor + put_float(packet, fdm.speedN); + put_float(packet, fdm.speedE); + put_float(packet, fdm.speedD); + put_float(packet, 0); //speed - unused + put_float(packet, 0); //ground speed - unused + put_float(packet, 0); //heading - unused + put_float(packet, 0.25f); //speed accuracy + put_float(packet, 0); //heading accuracy - unused + put_int(packet, 31); //valid flags + + packet.header[3] = packet.payload_size; + + send_packet(packet); + } + +} + +void MicroStrain7::send_filter_packet(void) +{ + const auto &fdm = _sitl->state; + MicroStrain_Packet packet; + + struct timeval tv; + simulation_timeval(&tv); + + packet.header[0] = 0x75; // Sync One + packet.header[1] = 0x65; // Sync Two + packet.header[2] = 0x82; // Filter Descriptor + + // Add GPS Timestamp Shared Data + packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size + packet.payload[packet.payload_size++] = 0xD3; // Descriptor + put_double(packet, (double) tv.tv_usec / 1e6); + put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL)); + put_int(packet, 0x0001); + + // Add GNSS Filter velocity + packet.payload[packet.payload_size++] = 0x10; // GNSS Velocity Field Size + packet.payload[packet.payload_size++] = 0x02; // Descriptor + put_float(packet, fdm.speedN); + put_float(packet, fdm.speedE); + put_float(packet, fdm.speedD); + put_int(packet, 0x0001); + + // Add Filter LLH position + packet.payload[packet.payload_size++] = 0x1C; // Filter LLH Field Size + packet.payload[packet.payload_size++] = 0x01; // Descriptor + put_double(packet, fdm.latitude); + put_double(packet, fdm.longitude); + put_double(packet, 0); // Height above ellipsoid - unused + put_int(packet, 0x0001); // Valid flags + + // Add Filter State + // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm + packet.payload[packet.payload_size++] = 0x08; // Filter State Field Size + packet.payload[packet.payload_size++] = 0x10; // Descriptor + put_int(packet, 0x04); // Filter state (GQ7_FULL_NAV) + put_int(packet, 0x03); // Dynamics mode (Airborne) + put_int(packet, 0); // Filter flags (None, no warnings) + + packet.header[3] = packet.payload_size; + + + send_packet(packet); +} diff --git a/libraries/SITL/SIM_Morse.cpp b/libraries/SITL/SIM_Morse.cpp index 81ba8ce34972f7..c4c62498697301 100644 --- a/libraries/SITL/SIM_Morse.cpp +++ b/libraries/SITL/SIM_Morse.cpp @@ -261,7 +261,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 +280,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 +602,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_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.h b/libraries/SITL/SIM_RichenPower.h index f0d604c60528e3..1fa763fcf2778c 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 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_SerialDevice.h b/libraries/SITL/SIM_SerialDevice.h index 00f58ab93511b1..d04e573506c8ae 100644 --- a/libraries/SITL/SIM_SerialDevice.h +++ b/libraries/SITL/SIM_SerialDevice.h @@ -46,7 +46,7 @@ class SerialDevice { ByteBuffer *to_autopilot; ByteBuffer *from_autopilot; - bool init_sitl_pointer(); + bool init_sitl_pointer() WARN_IF_UNUSED; private: 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..134fdc62369791 100644 --- a/libraries/SITL/SIM_Webots.cpp +++ b/libraries/SITL/SIM_Webots.cpp @@ -291,7 +291,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 726b9032766205..4eb427c0ae3f10 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -37,3 +37,44 @@ #ifndef AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED #define AP_SIM_SERIALDEVICE_CORRUPTION_ENABLED 0 #endif + +#ifndef HAL_SIM_GPS_ENABLED +#define HAL_SIM_GPS_ENABLED AP_SIM_ENABLED +#endif + +#ifndef AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#define AP_SIM_GPS_BACKEND_DEFAULT_ENABLED AP_SIM_ENABLED +#endif + +#ifndef AP_SIM_GPS_FILE_ENABLED +// really need to use AP_FileSystem for this. +#define AP_SIM_GPS_FILE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) +#endif + +#ifndef AP_SIM_GPS_TRIMBLE_ENABLED +#define AP_SIM_GPS_TRIMBLE_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_MSP_ENABLED +#define AP_SIM_GPS_MSP_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_NMEA_ENABLED +#define AP_SIM_GPS_NMEA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_NOVA_ENABLED +#define AP_SIM_GPS_NOVA_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_SBP2_ENABLED +#define AP_SIM_GPS_SBP2_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_SBP_ENABLED +#define AP_SIM_GPS_SBP_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_SIM_GPS_UBLOX_ENABLED +#define AP_SIM_GPS_UBLOX_ENABLED AP_SIM_GPS_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 9d70ad013aab3e..2ff2220aa7ed7f 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -499,7 +499,7 @@ const AP_Param::GroupInfo SIM::var_gps[] = { // @Param: GPS_TYPE // @DisplayName: GPS 1 type // @Description: Sets the type of simulation used for GPS 1 - // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP, 10:GSOF, 19:MSP + // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP, 10:Trimble, 19:MSP // @User: Advanced AP_GROUPINFO("GPS_TYPE", 3, SIM, gps_type[0], GPS::Type::UBLOX), // @Param: GPS_BYTELOSS @@ -569,6 +569,12 @@ const AP_Param::GroupInfo SIM::var_gps[] = { // @Vector3Parameter: 1 // @User: Advanced AP_GROUPINFO("GPS_VERR", 15, SIM, gps_vel_err[0], 0), + // @Param: GPS_JAM + // @DisplayName: GPS jamming enable + // @Description: Enable simulated GPS jamming + // @User: Advanced + // @Values: 0:Disabled, 1:Enabled + AP_GROUPINFO("GPS_JAM", 16, SIM, gps_jam[0], 0), // @Param: GPS2_DISABLE // @DisplayName: GPS 2 disable // @Description: Disables GPS 2 @@ -672,6 +678,13 @@ const AP_Param::GroupInfo SIM::var_gps[] = { // @Description: Log number for GPS:update_file() AP_GROUPINFO("GPS_LOG_NUM", 48, SIM, gps_log_num, 0), + // @Param: GPS2_JAM + // @DisplayName: GPS jamming enable + // @Description: Enable simulated GPS jamming + // @User: Advanced + // @Values: 0:Disabled, 1:Enabled + AP_GROUPINFO("GPS2_JAM", 49, SIM, gps_jam[1], 0), + AP_GROUPEND }; #endif // HAL_SIM_GPS_ENABLED diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index ade17ae2e854b1..68b30ddeb97a5a 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -92,6 +92,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 +156,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[]; @@ -206,6 +207,7 @@ class SIM { AP_Vector3f gps_pos_offset[2]; // XYZ position of the GPS antenna phase centre relative to the body frame origin (m) AP_Float gps_accuracy[2]; AP_Vector3f gps_vel_err[2]; // Velocity error offsets in NED (x = N, y = E, z = D) + AP_Int8 gps_jam[2]; // jamming simulation enable // initial offset on GPS lat/lon, used to shift origin AP_Float gps_init_lat_ofs; 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/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 3241db7d757922..6a85082c715457 160000 --- a/modules/ChibiOS +++ b/modules/ChibiOS @@ -1 +1 @@ -Subproject commit 3241db7d7579228656bb51435af78d8b7ceda0a4 +Subproject commit 6a85082c715457d1e0cc9627f9939f349de1143e 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 dccd8555cd6014..130a836efbfef0 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit dccd8555cd601467dc793ea9abf85caa63c0a9e8 +Subproject commit 130a836efbfef0eb3287a92cd5e187de7facdce2 diff --git a/wscript b/wscript index a2e8a7347808c6..8d2c2a68b2b299 100644 --- a/wscript +++ b/wscript @@ -180,6 +180,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', @@ -360,10 +365,10 @@ configuration in order to save typing. default=False, help='Use flash storage emulation.') - g.add_option('--disable-ekf2', + g.add_option('--enable-ekf2', action='store_true', default=False, - help='Configure without EKF2.') + help='Configure with EKF2.') g.add_option('--disable-ekf3', action='store_true', @@ -548,6 +553,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')