diff --git a/.github/workflows/test_scripts.yml b/.github/workflows/test_scripts.yml index b26699e8b77bc..1f73fa0567736 100644 --- a/.github/workflows/test_scripts.yml +++ b/.github/workflows/test_scripts.yml @@ -17,6 +17,7 @@ jobs: check_autotest_options, param_parse, python-cleanliness, + astyle-cleanliness, validate_board_list, ] steps: @@ -29,4 +30,6 @@ jobs: CI_BUILD_TARGET: ${{matrix.config}} shell: bash run: | + sudo apt update + sudo apt-get install -y astyle Tools/scripts/build_ci.sh diff --git a/.gitmodules b/.gitmodules index 8c7cc94fcd8e3..ff6918a69b89b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ -[submodule "modules/uavcan"] - path = modules/uavcan - url = https://github.com/DroneCAN/libuavcan.git [submodule "modules/waf"] path = modules/waf url = https://github.com/ArduPilot/waf.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 51091fb001a50..9cd87421bcf78 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -36,6 +36,11 @@ repos: - id: check-xml - id: check-yaml + - repo: https://github.com/lsst-ts/pre-commit-xmllint + rev: v1.0.0 + hooks: + - id: format-xmllint + files: libraries/AP_DDS/dds_xrce_profile.xml # Disable isort and mypy temporarily due to config conflicts # # Use to sort python imports by name and put system import first. # - repo: https://github.com/pycqa/isort diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index db60b9cfc754e..bdd901706b50d 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -9,8 +9,6 @@ //#define AC_OAPATHPLANNER_ENABLED DISABLED // disable path planning around obstacles //#define PARACHUTE DISABLED // disable parachute release to save 1k of flash //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands -//#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor -//#define BEACON_ENABLED DISABLED // disable beacon support //#define STATS_ENABLED DISABLED // disable statistics support //#define MODE_ACRO_ENABLED DISABLED // disable acrobatic mode support //#define MODE_AUTO_ENABLED DISABLED // disable auto mode support diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 91c2db7fd67b2..8f7ae7186612e 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -33,12 +33,6 @@ bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure) return false; } - // if we are using motor Estop switch, it must not be in Estop position - if (SRV_Channels::get_emergency_stop()){ - check_failed(display_failure, "Motor Emergency Stopped"); - return false; - } - if (!disarm_switch_checks(display_failure)) { return false; } diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 293d1d38f5f09..1241b5f9b2336 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -165,7 +165,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 200, 50, 36), #endif -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50, 39), #endif SCHED_TASK(update_altitude, 10, 100, 42), @@ -180,7 +180,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(three_hz_loop, 3, 75, 57), SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75, 60), SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90, 63), -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED SCHED_TASK(update_precland, 400, 50, 69), #endif #if FRAME_CONFIG == HELI_FRAME @@ -550,7 +550,7 @@ void Copter::ten_hz_logging_loop() #if HAL_PROXIMITY_ENABLED g2.proximity.log(); // Write proximity sensor distances #endif -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED g2.beacon.log(); #endif } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index be136776ede47..bde520ce01bef 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -69,6 +69,7 @@ #include // Crop sprayer library #include // ADS-B RF based collision avoidance module library #include // ArduPilot proximity sensor library +#include #include #include @@ -99,8 +100,8 @@ #include "AP_Rally.h" // Rally point library #include "AP_Arming.h" -// libraries which are dependent on #defines in defines.h and/or config.h -#if BEACON_ENABLED == ENABLED +#include +#if AP_BEACON_ENABLED #include #endif @@ -115,7 +116,7 @@ #if AP_GRIPPER_ENABLED # include #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED # include # include #endif @@ -529,7 +530,7 @@ class Copter : public AP_Vehicle { #endif // Precision Landing -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED AC_PrecLand precland; AC_PrecLand_StateMachine precland_statemachine; #endif diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 7721e5e74665d..02599b045d798 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -114,7 +114,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED if (copter.precland.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 28aebf0511093..9250a7928404f 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -640,7 +640,7 @@ void GCS_MAVLINK_Copter::handle_command_ack(const mavlink_message_t &msg) */ void GCS_MAVLINK_Copter::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED copter.precland.handle_msg(packet, timestamp_ms); #endif } diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 4e699f3655e23..579a1baa44650 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -664,7 +664,7 @@ const AP_Param::Info Copter::var_info[] = { GOBJECT(optflow, "FLOW", AP_OpticalFlow), #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // @Group: PLND_ // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp GOBJECT(precland, "PLND_", AC_PrecLand), @@ -776,7 +776,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("DEV_OPTIONS", 7, ParametersG2, dev_options, 0), -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED // @Group: BCN // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), @@ -1225,7 +1225,7 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { */ ParametersG2::ParametersG2(void) : temp_calibration() // this doesn't actually need constructing, but removing it here is problematic syntax-wise -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED , beacon() #endif #if HAL_PROXIMITY_ENABLED @@ -1294,12 +1294,6 @@ ParametersG2::ParametersG2(void) old object. This should be zero for top level parameters. */ const AP_Param::ConversionInfo conversion_table[] = { - // PARAMETER_CONVERSION - Added: Oct-2014 - { Parameters::k_param_log_bitmask_old, 0, AP_PARAM_INT16, "LOG_BITMASK" }, - // PARAMETER_CONVERSION - Added: Jan-2015 - { Parameters::k_param_serial0_baud, 0, AP_PARAM_INT16, "SERIAL0_BAUD" }, - { Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" }, - { Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" }, // PARAMETER_CONVERSION - Added: Jan-2017 { Parameters::k_param_arming_check_old, 0, AP_PARAM_INT8, "ARMING_CHECK" }, // battery @@ -1370,46 +1364,7 @@ void Copter::load_parameters(void) // handle conversion of PID gains void Copter::convert_pid_parameters(void) { - // conversion info - const AP_Param::ConversionInfo pid_conversion_info[] = { - // PARAMETER_CONVERSION - Added: Apr-2016 - { Parameters::k_param_pid_rate_roll, 0, AP_PARAM_FLOAT, "ATC_RAT_RLL_P" }, - { Parameters::k_param_pid_rate_roll, 1, AP_PARAM_FLOAT, "ATC_RAT_RLL_I" }, - { Parameters::k_param_pid_rate_roll, 2, AP_PARAM_FLOAT, "ATC_RAT_RLL_D" }, - { Parameters::k_param_pid_rate_pitch, 0, AP_PARAM_FLOAT, "ATC_RAT_PIT_P" }, - { Parameters::k_param_pid_rate_pitch, 1, AP_PARAM_FLOAT, "ATC_RAT_PIT_I" }, - { Parameters::k_param_pid_rate_pitch, 2, AP_PARAM_FLOAT, "ATC_RAT_PIT_D" }, - { Parameters::k_param_pid_rate_yaw, 0, AP_PARAM_FLOAT, "ATC_RAT_YAW_P" }, - { Parameters::k_param_pid_rate_yaw, 1, AP_PARAM_FLOAT, "ATC_RAT_YAW_I" }, - { Parameters::k_param_pid_rate_yaw, 2, AP_PARAM_FLOAT, "ATC_RAT_YAW_D" }, -#if FRAME_CONFIG == HELI_FRAME - // PARAMETER_CONVERSION - Added: May-2016 - { Parameters::k_param_pid_rate_roll, 4, AP_PARAM_FLOAT, "ATC_RAT_RLL_VFF" }, - { Parameters::k_param_pid_rate_pitch, 4, AP_PARAM_FLOAT, "ATC_RAT_PIT_VFF" }, - { Parameters::k_param_pid_rate_yaw , 4, AP_PARAM_FLOAT, "ATC_RAT_YAW_VFF" }, -#endif - }; - const AP_Param::ConversionInfo imax_conversion_info[] = { - // PARAMETER_CONVERSION - Added: Apr-2016 - { Parameters::k_param_pid_rate_roll, 5, AP_PARAM_FLOAT, "ATC_RAT_RLL_IMAX" }, - { Parameters::k_param_pid_rate_pitch, 5, AP_PARAM_FLOAT, "ATC_RAT_PIT_IMAX" }, - { Parameters::k_param_pid_rate_yaw, 5, AP_PARAM_FLOAT, "ATC_RAT_YAW_IMAX" }, -#if FRAME_CONFIG == HELI_FRAME - // PARAMETER_CONVERSION - Added: May-2016 - { Parameters::k_param_pid_rate_roll, 7, AP_PARAM_FLOAT, "ATC_RAT_RLL_ILMI" }, - { Parameters::k_param_pid_rate_pitch, 7, AP_PARAM_FLOAT, "ATC_RAT_PIT_ILMI" }, - { Parameters::k_param_pid_rate_yaw, 7, AP_PARAM_FLOAT, "ATC_RAT_YAW_ILMI" }, -#endif - }; - // conversion from Copter-3.3 to Copter-3.4 const AP_Param::ConversionInfo angle_and_filt_conversion_info[] = { - // PARAMETER_CONVERSION - Added: May-2016 - { Parameters::k_param_p_stabilize_roll, 0, AP_PARAM_FLOAT, "ATC_ANG_RLL_P" }, - { Parameters::k_param_p_stabilize_pitch, 0, AP_PARAM_FLOAT, "ATC_ANG_PIT_P" }, - { Parameters::k_param_p_stabilize_yaw, 0, AP_PARAM_FLOAT, "ATC_ANG_YAW_P" }, - // PARAMETER_CONVERSION - Added: Apr-2016 - { Parameters::k_param_pid_rate_roll, 6, AP_PARAM_FLOAT, "ATC_RAT_RLL_FILT" }, - { Parameters::k_param_pid_rate_pitch, 6, AP_PARAM_FLOAT, "ATC_RAT_PIT_FILT" }, // PARAMETER_CONVERSION - Added: Jan-2018 { Parameters::k_param_pid_rate_yaw, 6, AP_PARAM_FLOAT, "ATC_RAT_YAW_FILT" }, { Parameters::k_param_pi_vel_xy, 0, AP_PARAM_FLOAT, "PSC_VELXY_P" }, @@ -1429,11 +1384,6 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" }, { Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" }, }; - const AP_Param::ConversionInfo throttle_conversion_info[] = { - // PARAMETER_CONVERSION - Added: Jun-2016 - { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" }, - { Parameters::k_param_throttle_mid, 0, AP_PARAM_FLOAT, "MOT_THST_HOVER" } - }; const AP_Param::ConversionInfo loiter_conversion_info[] = { // PARAMETER_CONVERSION - Added: Apr-2018 { Parameters::k_param_wp_nav, 4, AP_PARAM_FLOAT, "LOIT_SPEED" }, @@ -1442,34 +1392,10 @@ void Copter::convert_pid_parameters(void) { Parameters::k_param_wp_nav, 9, AP_PARAM_FLOAT, "LOIT_BRK_ACCEL" } }; - // PARAMETER_CONVERSION - Added: Apr-2016 - // gains increase by 27% due to attitude controller's switch to use radians instead of centi-degrees - // and motor libraries switch to accept inputs in -1 to +1 range instead of -4500 ~ +4500 - float pid_scaler = 1.27f; - -#if FRAME_CONFIG != HELI_FRAME - // Multicopter x-frame gains are 40% lower because -1 or +1 input to motors now results in maximum rotation - if (g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_X || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_V || g.frame_type == AP_Motors::MOTOR_FRAME_TYPE_H) { - pid_scaler = 0.9f; - } -#endif - - // scale PID gains - for (const auto &info : pid_conversion_info) { - AP_Param::convert_old_parameter(&info, pid_scaler); - } - // reduce IMAX into -1 ~ +1 range - for (const auto &info : imax_conversion_info) { - AP_Param::convert_old_parameter(&info, 1.0f/4500.0f); - } // convert angle controller gain and filter without scaling for (const auto &info : angle_and_filt_conversion_info) { AP_Param::convert_old_parameter(&info, 1.0f); } - // convert throttle parameters (multicopter only) - for (const auto &info : throttle_conversion_info) { - AP_Param::convert_old_parameter(&info, 0.001f); - } // convert RC_FEEL_RP to ATC_INPUT_TC // PARAMETER_CONVERSION - Added: Mar-2018 const AP_Param::ConversionInfo rc_feel_rp_conversion_info = { Parameters::k_param_rc_feel_rp, 0, AP_PARAM_INT8, "ATC_INPUT_TC" }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 22fcee3432c57..15f719224c585 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -525,7 +525,7 @@ class ParametersG2 { // temperature calibration handling AP_TempCalibration temp_calibration; -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED // beacon (non-GPS positioning) library AP_Beacon beacon; #endif diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index afcd65754cad2..03d398c67a3c4 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -391,7 +391,7 @@ bool RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi break; case AUX_FUNC::PRECISION_LOITER: -#if PRECISION_LANDING == ENABLED && MODE_LOITER_ENABLED == ENABLED +#if AC_PRECLAND_ENABLED && MODE_LOITER_ENABLED == ENABLED switch (ch_flag) { case AuxSwitchPos::HIGH: copter.mode_loiter.set_precision_loiter_enabled(true); diff --git a/ArduCopter/ReleaseNotes.txt b/ArduCopter/ReleaseNotes.txt index 5b5ae54095018..9ca8364d6cbc2 100644 --- a/ArduCopter/ReleaseNotes.txt +++ b/ArduCopter/ReleaseNotes.txt @@ -1,5 +1,9 @@ ArduPilot Copter Release Notes: ------------------------------------------------------------------ +Copter 4.3.6 05-Apr-2023 / 4.3.6-beta1, 4.3.6--beta2 27-Mar-2023 +Changes from 4.3.5 +1) Bi-directional DShot fix for possible motor stop approx 72min after startup +------------------------------------------------------------------ Copter Copter 4.3.5 14-Mar-2023 / 4.3.5-rc1 01-Mar-2023 Changes from 4.3.4 1) Bug fixes diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5a4704b14915d..a260b6cba4ec6 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -138,12 +138,6 @@ # define AUTOTUNE_ENABLED ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// Precision Landing with companion computer or IRLock sensor -#ifndef PRECISION_LANDING - # define PRECISION_LANDING ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE @@ -294,13 +288,6 @@ # define MODE_AUTOROTATE_ENABLED DISABLED #endif -////////////////////////////////////////////////////////////////////////////// - -// Beacon support - support for local positioning systems -#ifndef BEACON_ENABLED -# define BEACON_ENABLED !HAL_MINIMIZE_FEATURES -#endif - ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 61b1cbb7561b1..516de29c84ce5 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -594,7 +594,7 @@ void Mode::land_run_vertical_control(bool pause_descent) // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED const bool navigating = pos_control->is_active_xy(); bool doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired() && navigating; @@ -668,7 +668,7 @@ void Mode::land_run_horizontal_control() AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); } copter.ap.land_repo_active = true; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED } else { // no override right now, check if we should allow precland if (copter.precland.allow_precland_after_reposition()) { @@ -681,7 +681,7 @@ void Mode::land_run_horizontal_control() // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle copter.ap.prec_land_active = false; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired(); // run precision landing if (copter.ap.prec_land_active) { @@ -738,7 +738,7 @@ void Mode::land_run_horizontal_control() // pause_descent is true if vehicle should not descend void Mode::land_run_normal_or_precland(bool pause_descent) { -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED if (pause_descent || !copter.precland.enabled()) { // we don't want to start descending immediately or prec land is disabled // in both cases just run simple land controllers @@ -753,7 +753,7 @@ void Mode::land_run_normal_or_precland(bool pause_descent) #endif } -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // Go towards a position commanded by prec land state machine in order to retry landing // The passed in location is expected to be NED and in m void Mode::precland_retry_position(const Vector3f &retry_pos) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 52ed6379802d8..dc417f51c6c83 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -147,7 +147,7 @@ class Mode { // pause_descent is true if vehicle should not descend void land_run_normal_or_precland(bool pause_descent = false); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // Go towards a position commanded by prec land state machine in order to retry landing // The passed in location is expected to be NED and in meters void precland_retry_position(const Vector3f &retry_pos); @@ -1181,7 +1181,7 @@ class ModeLoiter : public Mode { bool has_user_takeoff(bool must_navigate) const override { return true; } bool allows_autotune() const override { return true; } -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED void set_precision_loiter_enabled(bool value) { _precision_loiter_enabled = value; } #endif @@ -1194,14 +1194,14 @@ class ModeLoiter : public Mode { int32_t wp_bearing() const override; float crosstrack_error() const override { return pos_control->crosstrack_error();} -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool do_precision_loiter(); void precision_loiter_xy(); #endif private: -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool _precision_loiter_enabled; bool _precision_loiter_active; // true if user has switched on prec loiter #endif @@ -1794,6 +1794,9 @@ class ModeZigZag : public Mode { const char *name() const override { return "ZIGZAG"; } const char *name4() const override { return "ZIGZ"; } + uint32_t wp_distance() const override; + int32_t wp_bearing() const override; + float crosstrack_error() const override; private: diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b6be6d5cefb16..77be3560749f4 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -53,7 +53,7 @@ bool ModeAuto::init(bool ignore_checks) // reset flag indicating if pilot has applied roll or pitch inputs during landing copter.ap.land_repo_active = false; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 4072e76c8b3a1..377bef5ee9253 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -46,7 +46,7 @@ bool ModeLand::init(bool ignore_checks) copter.fence.auto_disable_fence_for_landing(); #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 0198db443e5b4..602aa52859a15 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -34,14 +34,14 @@ bool ModeLoiter::init(bool ignore_checks) pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED _precision_loiter_active = false; #endif return true; } -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool ModeLoiter::do_precision_loiter() { if (!_precision_loiter_enabled) { @@ -165,7 +165,7 @@ void ModeLoiter::run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED bool precision_loiter_old_state = _precision_loiter_active; if (do_precision_loiter()) { precision_loiter_xy(); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index fc08b39b19e6c..e6ae505e8214a 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -28,7 +28,7 @@ bool ModeRTL::init(bool ignore_checks) // this will be set true if prec land is later active copter.ap.prec_land_active = false; -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precland state machine copter.precland_statemachine.init(); #endif diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index ff186ff65ee77..1bb82aaadba4a 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -583,4 +583,29 @@ void ModeZigZag::spray(bool b) #endif } +uint32_t ModeZigZag::wp_distance() const +{ + if (is_auto) { + return wp_nav->get_wp_distance_to_destination(); + } else { + return 0; + } +} +int32_t ModeZigZag::wp_bearing() const +{ + if (is_auto) { + return wp_nav->get_wp_bearing_to_destination(); + } else { + return 0; + } +} +float ModeZigZag::crosstrack_error() const +{ + if (is_auto) { + return wp_nav->crosstrack_error(); + } else { + return 0; + } +} + #endif // MODE_ZIGZAG_ENABLED == ENABLED diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index 076177667806b..ad98c2fa75552 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -4,7 +4,7 @@ #include "Copter.h" -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED void Copter::init_precland() { diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index b2b8c95e07ed4..4e1527e2fc569 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -132,7 +132,7 @@ void Copter::init_ardupilot() camera.init(); #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precision landing init_precland(); #endif @@ -159,7 +159,7 @@ void Copter::init_ardupilot() g2.proximity.init(); #endif -#if BEACON_ENABLED == ENABLED +#if AP_BEACON_ENABLED // init beacons used for non-gps position estimation g2.beacon.init(); #endif @@ -206,10 +206,7 @@ void Copter::init_ardupilot() ins.set_log_raw_bit(MASK_LOG_IMU_RAW); - // enable output to motors - if (arming.rc_calibration_checks(true)) { - motors->output_min(); // output lowest possible value to motors - } + motors->output_min(); // output lowest possible value to motors // attempt to set the intial_mode, else set to STABILIZE if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) { diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 46e52620876dc..54fdc5592f961 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -106,11 +106,6 @@ bool AP_Arming_Plane::pre_arm_checks(bool display_failure) ret = false; } - if (SRV_Channels::get_emergency_stop()) { - check_failed(display_failure,"Motors Emergency Stopped"); - ret = false; - } - if (plane.g2.flight_options & FlightOptions::CENTER_THROTTLE_TRIM){ int16_t trim = plane.channel_throttle->get_radio_trim(); if (trim < 1250 || trim > 1750) { diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index a724c4af4a0f4..d832d8fed3a34 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -821,12 +821,7 @@ bool Plane::get_target_location(Location& target_loc) */ bool Plane::update_target_location(const Location &old_loc, const Location &new_loc) { - if (!old_loc.same_latlon_as(next_WP_loc)) { - return false; - } - ftype alt_diff; - if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) || - !is_zero(alt_diff)) { + if (!old_loc.same_loc_as(next_WP_loc)) { return false; } next_WP_loc = new_loc; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 83188fe94552b..83c1924e4287b 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -943,9 +943,11 @@ const AP_Param::Info Plane::var_info[] = { // @Path: ../libraries/AP_Mission/AP_Mission.cpp GOBJECT(mission, "MIS_", AP_Mission), +#if HAL_RALLY_ENABLED // @Group: RALLY_ // @Path: ../libraries/AP_Rally/AP_Rally.cpp GOBJECT(rally, "RALLY_", AP_Rally), +#endif #if HAL_NAVEKF2_AVAILABLE // @Group: EK2_ diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 8d5a08a189298..087e86cd414ab 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -244,8 +244,14 @@ class Plane : public AP_Vehicle { AP_OpticalFlow optflow; #endif +#if HAL_RALLY_ENABLED // Rally Ponints AP_Rally rally; +#endif + + // returns a Location for a rally point or home; if + // HAL_RALLY_ENABLED is false, just home. + Location calc_best_rally_or_home_location(const Location ¤t_loc, float rtl_home_alt_amsl_cm) const; #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index e0435c8253af5..1da0108f6ce95 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -154,6 +154,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::RTL: case AUX_FUNC::TAKEOFF: case AUX_FUNC::FBWA: + case AUX_FUNC::AIRBRAKE: #if HAL_QUADPLANE_ENABLED case AUX_FUNC::QRTL: case AUX_FUNC::QSTABILIZE: diff --git a/ArduPlane/ReleaseNotes.txt b/ArduPlane/ReleaseNotes.txt index 7fcb224184bfd..70036d3c84690 100644 --- a/ArduPlane/ReleaseNotes.txt +++ b/ArduPlane/ReleaseNotes.txt @@ -1,5 +1,5 @@ -Release 4.3.5beta1 24th March 2023 ------------------------------------ +Release 4.3.5 26th March 2023 +------------------------------ - fixed 32 bit microsecond wrap in BDShot code diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 299f5e004ccf1..b6d017a53a729 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -344,7 +344,7 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) auto_state.next_wp_crosstrack = false; auto_state.crosstrack = false; prev_WP_loc = current_loc; - next_WP_loc = rally.calc_best_rally_or_home_location(current_loc, 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); @@ -360,6 +360,17 @@ void Plane::do_RTL(int32_t rtl_altitude_AMSL_cm) logger.Write_Mode(control_mode->mode_number(), control_mode_reason); } +Location Plane::calc_best_rally_or_home_location(const Location &_current_loc, float rtl_home_alt_amsl_cm) const +{ +#if HAL_RALLY_ENABLED + return plane.rally.calc_best_rally_or_home_location(_current_loc, rtl_home_alt_amsl_cm); +#else + Location destination = plane.home; + destination.set_alt_cm(rtl_home_alt_amsl_cm, Location::AltFrame::ABSOLUTE); + return destination; +#endif +} + /* start a NAV_TAKEOFF command */ diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp index 28f4abdca677e..640ad65869926 100644 --- a/ArduPlane/fence.cpp +++ b/ArduPlane/fence.cpp @@ -75,7 +75,7 @@ void Plane::fence_check() Location loc; if (fence.get_return_rally() != 0 || fence_act == AC_FENCE_ACTION_RTL_AND_LAND) { - loc = rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); + loc = calc_best_rally_or_home_location(current_loc, get_RTL_altitude_cm()); } else { //return to fence return point, not a rally point if (fence.get_return_altitude() > 0) { diff --git a/ArduPlane/mode_qrtl.cpp b/ArduPlane/mode_qrtl.cpp index 98bfe3a28342b..75d82b48225fa 100644 --- a/ArduPlane/mode_qrtl.cpp +++ b/ArduPlane/mode_qrtl.cpp @@ -17,8 +17,7 @@ bool ModeQRTL::_enter() int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; if (quadplane.motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) { // VTOL motors are active, either in VTOL flight or assisted flight - Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); - + Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); @@ -122,7 +121,7 @@ void ModeQRTL::run() plane.prev_WP_loc = plane.current_loc; int32_t RTL_alt_abs_cm = plane.home.alt + quadplane.qrtl_alt*100UL; - Location destination = plane.rally.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); + Location destination = plane.calc_best_rally_or_home_location(plane.current_loc, RTL_alt_abs_cm); const float dist = plane.current_loc.get_distance(destination); const float radius = get_VTOL_return_radius(); if (dist < radius) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c6c9a08d1f0b1..5d308773ca172 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3044,8 +3044,7 @@ void QuadPlane::waypoint_controller(void) const Location &loc = plane.next_WP_loc; const uint32_t now = AP_HAL::millis(); - if (!loc.same_latlon_as(last_auto_target) || - plane.next_WP_loc.alt != last_auto_target.alt || + if (!loc.same_loc_as(last_auto_target) || now - last_loiter_ms > 500) { wp_nav->set_wp_destination(poscontrol.target_cm.tofloat()); last_auto_target = loc; diff --git a/Blimp/config.h b/Blimp/config.h index 0516a157c12d0..10cac9ea72bf7 100644 --- a/Blimp/config.h +++ b/Blimp/config.h @@ -137,12 +137,6 @@ # define AUTOTUNE_ENABLED ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// Precision Landing with companion computer or IRLock sensor -#ifndef PRECISION_LANDING -# define PRECISION_LANDING ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE diff --git a/Rover/AP_Arming.cpp b/Rover/AP_Arming.cpp index 805f23d720a49..c952f7d9652b5 100644 --- a/Rover/AP_Arming.cpp +++ b/Rover/AP_Arming.cpp @@ -80,10 +80,6 @@ bool AP_Arming_Rover::pre_arm_checks(bool report) if (checks_to_perform == 0) { return true; } - if (SRV_Channels::get_emergency_stop()) { - check_failed(report, "Motors Emergency Stopped"); - return false; - } if (rover.g2.sailboat.sail_enabled() && !rover.g2.windvane.enabled()) { check_failed(report, "Sailing enabled with no WindVane"); @@ -200,6 +196,9 @@ bool AP_Arming_Rover::mode_checks(bool report) if (!rover.control_mode->allows_arming()) { check_failed(report, "Mode not armable"); return false; + } else if (rover.control_mode == &rover.mode_auto && rover.mode_auto.mission.num_commands() <= 1) { + check_failed(report, "AUTO requires mission"); + return false; } return true; } diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index acfa8699b9836..92d17426514e4 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -1063,7 +1063,7 @@ void GCS_MAVLINK_Rover::handle_radio(const mavlink_message_t &msg) */ void GCS_MAVLINK_Rover::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED rover.precland.handle_msg(packet, timestamp_ms); #endif } diff --git a/Rover/Parameters.cpp b/Rover/Parameters.cpp index 1de13e3f6bf3b..792e50153461d 100644 --- a/Rover/Parameters.cpp +++ b/Rover/Parameters.cpp @@ -297,7 +297,7 @@ const AP_Param::Info Rover::var_info[] = { GOBJECT(camera, "CAM", AP_Camera), #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // @Group: PLND_ // @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp GOBJECT(precland, "PLND_", AC_PrecLand), @@ -425,9 +425,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(afs, "AFS_", 5, ParametersG2, AP_AdvancedFailsafe), #endif +#if AP_BEACON_ENABLED // @Group: BCN // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp AP_SUBGROUPINFO(beacon, "BCN", 6, ParametersG2, AP_Beacon), +#endif // 7 was used by AP_VisualOdometry @@ -730,7 +732,9 @@ ParametersG2::ParametersG2(void) #if ADVANCED_FAILSAFE == ENABLED afs(), #endif +#if AP_BEACON_ENABLED beacon(), +#endif motors(rover.ServoRelayEvents, wheel_rate_control), wheel_rate_control(wheel_encoder), attitude_control(), diff --git a/Rover/Parameters.h b/Rover/Parameters.h index 83f718b91e8c3..3c3040ce5304b 100644 --- a/Rover/Parameters.h +++ b/Rover/Parameters.h @@ -312,7 +312,9 @@ class ParametersG2 { AP_AdvancedFailsafe_Rover afs; #endif +#if AP_BEACON_ENABLED AP_Beacon beacon; +#endif // Motor library AP_MotorsUGV motors; diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index bb6f56120a65a..2ef70f1c9e986 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -80,7 +80,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(set_servos, 400, 200, 15), SCHED_TASK_CLASS(AP_GPS, &rover.gps, update, 50, 300, 18), SCHED_TASK_CLASS(AP_Baro, &rover.barometer, update, 10, 200, 21), +#if AP_BEACON_ENABLED SCHED_TASK_CLASS(AP_Beacon, &rover.g2.beacon, update, 50, 200, 24), +#endif #if HAL_PROXIMITY_ENABLED SCHED_TASK_CLASS(AP_Proximity, &rover.g2.proximity, update, 50, 200, 27), #endif @@ -98,7 +100,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { #if AP_GRIPPER_ENABLED SCHED_TASK_CLASS(AP_Gripper, &rover.g2.gripper, update, 10, 75, 69), #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED SCHED_TASK(update_precland, 400, 50, 70), #endif #if AP_RPM_ENABLED @@ -377,7 +379,9 @@ void Rover::update_logging1(void) if (should_log(MASK_LOG_THR)) { Log_Write_Throttle(); +#if AP_BEACON_ENABLED g2.beacon.log(); +#endif } if (should_log(MASK_LOG_NTUN)) { diff --git a/Rover/Rover.h b/Rover/Rover.h index b06a25703329c..f11bae3d600f9 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -42,6 +42,7 @@ #include #include #include +#include // Configuration #include "defines.h" @@ -61,10 +62,10 @@ #include "GCS_Mavlink.h" #include "GCS_Rover.h" #include "AP_Rally.h" -#include "RC_Channel.h" // RC Channel Library -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED #include #endif +#include "RC_Channel.h" // RC Channel Library #include "mode.h" @@ -145,7 +146,7 @@ class Rover : public AP_Vehicle { #if OSD_ENABLED || OSD_PARAM_ENABLED AP_OSD osd; #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED AC_PrecLand precland; #endif // GCS handling diff --git a/Rover/config.h b/Rover/config.h index 6983f9d122eed..77b2981c5fbcc 100644 --- a/Rover/config.h +++ b/Rover/config.h @@ -38,12 +38,6 @@ #define AP_RALLY ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// Precision Landing with companion computer or IRLock sensor -#ifndef PRECISION_LANDING - # define PRECISION_LANDING ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // NAVL1 // @@ -70,7 +64,7 @@ ////////////////////////////////////////////////////////////////////////////// // Dock mode - allows vehicle to dock to a docking target #ifndef MODE_DOCK_ENABLED -# define MODE_DOCK_ENABLED PRECISION_LANDING +# define MODE_DOCK_ENABLED AC_PRECLAND_ENABLED #endif diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp index e9bdd4c6f09e9..b7cf77163cf0b 100644 --- a/Rover/mode_auto.cpp +++ b/Rover/mode_auto.cpp @@ -44,6 +44,12 @@ void ModeAuto::_exit() void ModeAuto::update() { + // check if mission exists (due to being cleared while disarmed in AUTO, + // if no mission, then stop...needs mode change out of AUTO, mission load, + // and change back to AUTO to run a mission at this point + if (!hal.util->get_soft_armed() && mission.num_commands() <= 1) { + start_stop(); + } // start or update mission if (waiting_to_start) { // don't start the mission until we have an origin diff --git a/Rover/precision_landing.cpp b/Rover/precision_landing.cpp index 12b33174ebbe5..c62b499b6a3ec 100644 --- a/Rover/precision_landing.cpp +++ b/Rover/precision_landing.cpp @@ -4,7 +4,7 @@ #include "Rover.h" -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED void Rover::init_precland() { diff --git a/Rover/release-notes.txt b/Rover/release-notes.txt index c2bfdaacef754..4a80f680be59b 100644 --- a/Rover/release-notes.txt +++ b/Rover/release-notes.txt @@ -1,5 +1,9 @@ Rover Release Notes: ------------------------------------------------------------------ +Rover 4.3.0-beta11/beta12 27-Mar-2023 +Changes from 4.3.0-beta10 +1) Bi-directional DShot fix for possible motor stop approx 72min after startup +------------------------------------------------------------------ Rover 4.3.0-beta10 01-Mar-2023 Changes from 4.3.0-beta9 1) Bug fixes diff --git a/Rover/system.cpp b/Rover/system.cpp index c7a17b10d5ac9..bd6ed6b1ccf68 100644 --- a/Rover/system.cpp +++ b/Rover/system.cpp @@ -75,8 +75,10 @@ void Rover::init_ardupilot() g2.proximity.init(); #endif +#if AP_BEACON_ENABLED // init beacons used for non-gps position estimation g2.beacon.init(); +#endif // and baro for EKF barometer.set_log_baro_bit(MASK_LOG_IMU); @@ -117,7 +119,7 @@ void Rover::init_ardupilot() camera.init(); #endif -#if PRECISION_LANDING == ENABLED +#if AC_PRECLAND_ENABLED // initialise precision landing init_precland(); #endif diff --git a/Tools/AP_Bootloader/bl_protocol.cpp b/Tools/AP_Bootloader/bl_protocol.cpp index c0b7171a2043a..c4577cd2312e1 100644 --- a/Tools/AP_Bootloader/bl_protocol.cpp +++ b/Tools/AP_Bootloader/bl_protocol.cpp @@ -163,7 +163,7 @@ extern AP_FlashIface_JEDEC ext_flash; /* 1ms timer tick callback */ -static void sys_tick_handler(void *ctx) +static void sys_tick_handler(virtual_timer_t* vt, void *ctx) { chSysLockFromISR(); chVTSetI(&systick_vt, chTimeMS2I(1), sys_tick_handler, nullptr); diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt index 5a019c4d33eba..af66dca9c15b9 100644 --- a/Tools/AP_Bootloader/board_types.txt +++ b/Tools/AP_Bootloader/board_types.txt @@ -216,10 +216,13 @@ AP_HW_rFCU 1102 AP_HW_rGNSS 1103 AP_HW_AEROFOX_AIRSPEED_DLVR 1104 AP_HW_KakuteH7-Wing 1105 +AP_HW_SpeedyBeeF405WING 1106 AP_HW_PixSurveyA-IND 1107 +AP_HW_JFB110 1110 + AP_HW_ESP32_PERIPH 1205 AP_HW_ESP32S3_PERIPH 1206 diff --git a/Tools/AP_Bootloader/wscript b/Tools/AP_Bootloader/wscript index 22887ea9d30b0..705e269182d38 100644 --- a/Tools/AP_Bootloader/wscript +++ b/Tools/AP_Bootloader/wscript @@ -9,16 +9,6 @@ def build(bld): flashiface_lib = ['AP_HAL', 'AP_FlashIface', 'AP_HAL_Empty'] else: flashiface_lib = [] - - # build external libcanard library - bld.stlib(source='../../modules/DroneCAN/libcanard/canard.c', - use='dronecan', - target='libcanard') - - bld.ap_program( - use=['ap','libcanard','AP_Bootloader_libs', 'dronecan'], - program_groups='bootloader' - ) bld.ap_stlib( name= 'AP_Bootloader_libs', @@ -27,5 +17,17 @@ def build(bld): ap_vehicle='AP_Bootloader', ap_libraries= flashiface_lib + [ 'AP_Math', - 'AP_CheckFirmware' + 'AP_CheckFirmware', + 'AP_HAL', ]) + + # build external libcanard library + bld.stlib(source='../../modules/DroneCAN/libcanard/canard.c', + name='libcanard', + use='dronecan', + target='libcanard') + + bld.ap_program( + use=['AP_Bootloader_libs', 'libcanard', 'dronecan'], + program_groups='bootloader' + ) diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index 613fcd235c82a..8ac1666808099 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -102,7 +102,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN // @User: Advanced // @RebootRequired: True - GARRAY(can_protocol, 0, "CAN_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), + GARRAY(can_protocol, 0, "CAN_PROTOCOL", AP_CANManager::Driver_Type_DroneCAN), // @Param: CAN2_BAUDRATE // @DisplayName: Bitrate of CAN2 interface @@ -118,7 +118,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN // @User: Advanced // @RebootRequired: True - GARRAY(can_protocol, 1, "CAN2_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), + GARRAY(can_protocol, 1, "CAN2_PROTOCOL", AP_CANManager::Driver_Type_DroneCAN), #endif #if HAL_NUM_CAN_IFACES >= 3 @@ -136,7 +136,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Values: 0:Disabled,1:UAVCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN // @User: Advanced // @RebootRequired: True - GARRAY(can_protocol, 2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_UAVCAN), + GARRAY(can_protocol, 2, "CAN3_PROTOCOL", AP_CANManager::Driver_Type_DroneCAN), #endif #if HAL_CANFD_SUPPORTED diff --git a/Tools/AP_Periph/ReleaseNotes.txt b/Tools/AP_Periph/ReleaseNotes.txt index d265eff2d41ca..7666657983979 100644 --- a/Tools/AP_Periph/ReleaseNotes.txt +++ b/Tools/AP_Periph/ReleaseNotes.txt @@ -1,3 +1,16 @@ +Release 1.5.0 27th Mar 2023 +--------------------------- + +This is a major release with the following changes: + +- fixed airspeed bus default +- limit mag to 25Hz by default +- fixed send rate of GPS yaw +- fixed HW ESC telem temp units +- allow set of port for HW telem +- send GNSS heading message if available +- stop sending old GNSS Fix message + Release 1.4.1 27th Sep 2022 --------------------------- diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 85a9519a8797e..1c340a9e12b28 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -440,20 +440,16 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* memset(comms, 0, sizeof(struct app_bootloader_comms)); comms->magic = APP_BOOTLOADER_COMMS_MAGIC; - // manual decoding due to TAO bug in libcanard generated code - if (transfer->payload_len < 1 || transfer->payload_len > sizeof(comms->path)+1) { + uavcan_protocol_file_BeginFirmwareUpdateRequest req; + if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) { return; } - uint32_t offset = 0; - canardDecodeScalar(transfer, 0, 8, false, (void*)&comms->server_node_id); - offset += 8; - for (uint8_t i=0; ipayload_len-1; i++) { - canardDecodeScalar(transfer, offset, 8, false, (void*)&comms->path[i]); - offset += 8; - } + + comms->server_node_id = req.source_node_id; if (comms->server_node_id == 0) { comms->server_node_id = transfer->source_node_id; } + memcpy(comms->path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); comms->my_node_id = canardGetLocalNodeID(ins); uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE] {}; @@ -786,42 +782,19 @@ static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfe static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer) { - // manual decoding due to TAO bug in libcanard generated code - if (transfer->payload_len < 1 || transfer->payload_len > UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE+1) { + uavcan_equipment_actuator_ArrayCommand cmd; + if (uavcan_equipment_actuator_ArrayCommand_decode(transfer, &cmd)) { return; } - const uint8_t data_count = (transfer->payload_len / UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_MAX_SIZE); - uavcan_equipment_actuator_Command data[data_count] {}; - - uint32_t offset = 0; - for (uint8_t i=0; itao = !transfer->canfd; +#endif + /* * Dynamic node ID allocation protocol. * Taking this branch only if we don't have a node ID, ignoring otherwise. @@ -1219,7 +1197,7 @@ static void processTx(void) } #endif #if HAL_NUM_CAN_IFACES >= 2 - if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) { + if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_DroneCAN) { continue; } #endif @@ -1251,7 +1229,7 @@ static void processRx(void) continue; } #if HAL_NUM_CAN_IFACES >= 2 - if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_UAVCAN) { + if (periph.can_protocol_cached[ins.index] != AP_CANManager::Driver_Type_DroneCAN) { continue; } #endif @@ -1477,6 +1455,7 @@ static bool can_do_dna() ,(1U << dronecan.dna_interface) #endif #if HAL_CANFD_SUPPORTED + // always send allocation request as non-FD ,false #endif ); @@ -1506,12 +1485,12 @@ void AP_Periph_FW::can_start() #if AP_PERIPH_ENFORCE_AT_LEAST_ONE_PORT_IS_UAVCAN_1MHz && HAL_NUM_CAN_IFACES >= 2 bool has_uavcan_at_1MHz = false; for (uint8_t i=0; idelay_microseconds(HAL_PERIPH_LOOP_DELAY_US); + +#if HAL_CANFD_SUPPORTED + // allow for user enabling/disabling CANFD at runtime + dronecan.canard.tao_disabled = periph.g.can_fdmode == 1; +#endif + processTx(); processRx(); } diff --git a/Tools/AP_Periph/version.h b/Tools/AP_Periph/version.h index 84aad587c28b2..2cd4fb5bf0ab7 100644 --- a/Tools/AP_Periph/version.h +++ b/Tools/AP_Periph/version.h @@ -2,15 +2,16 @@ #include -#define THISFIRMWARE "AP_Periph V1.5.0-dev" +#define THISFIRMWARE "AP_Periph V1.5.0" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 1,5,0,FIRMWARE_VERSION_TYPE_DEV +#define FIRMWARE_VERSION 1,5,0,FIRMWARE_VERSION_TYPE_OFFICIAL #define FW_MAJOR 1 #define FW_MINOR 5 #define FW_PATCH 0 -#define FW_TYPE FIRMWARE_VERSION_TYPE_DEV +#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL + diff --git a/Tools/CPUInfo/CPUInfo.cpp b/Tools/CPUInfo/CPUInfo.cpp index 73d1c6a45a9cf..9802c9f93a418 100644 --- a/Tools/CPUInfo/CPUInfo.cpp +++ b/Tools/CPUInfo/CPUInfo.cpp @@ -25,7 +25,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); #if CONFIG_HAL_BOARD != HAL_BOARD_LINUX // On H750 we want to measure external flash to ram performance -#if defined(EXT_FLASH_SIZE_MB) && defined(STM32H7) +#if defined(EXT_FLASH_SIZE_MB) && EXT_FLASH_SIZE_MB>0 && defined(STM32H7) #define DISABLE_CACHES #endif diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin index 6e4fbea04f5f5..d148daded7da4 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 97759cc3419c3..b3036b63edd7a 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_highpolh.bin b/Tools/IO_Firmware/iofirmware_highpolh.bin index 414d51ef0e554..3f308f141ffa1 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 114b8101dad94..defcab7926f50 100755 Binary files a/Tools/IO_Firmware/iofirmware_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_lowpolh.bin differ diff --git a/Tools/ardupilotwaf/ap_library.py b/Tools/ardupilotwaf/ap_library.py index d8fdd4c05ca10..9c7b42f774989 100644 --- a/Tools/ardupilotwaf/ap_library.py +++ b/Tools/ardupilotwaf/ap_library.py @@ -239,6 +239,25 @@ def __str__(self): def keyword(self): return 'Checking included headers' +def custom_flags_check(tgen): + ''' + check for tasks marked as having custom cpp or c flags + a library can do this by setting AP_LIB_EXTRA_CXXFLAGS and AP_LIB_EXTRA_CFLAGS + + For example add this is the configure section of the library, using AP_DDS as an example: + + cfg.env.AP_LIB_EXTRA_CXXFLAGS['AP_DDS'] = ['-DSOME_CXX_FLAG'] + cfg.env.AP_LIB_EXTRA_CFLAGS['AP_DDS'] = ['-DSOME_C_FLAG'] + ''' + if not tgen.name.startswith("objs/"): + return + libname = tgen.name[5:] + if libname in tgen.env.AP_LIB_EXTRA_CXXFLAGS: + tgen.env.CXXFLAGS.extend(tgen.env.AP_LIB_EXTRA_CXXFLAGS[libname]) + if libname in tgen.env.AP_LIB_EXTRA_CFLAGS: + tgen.env.CFLAGS.extend(tgen.env.AP_LIB_EXTRA_CFLAGS[libname]) + + def double_precision_check(tasks): '''check for tasks marked as double precision''' @@ -284,6 +303,7 @@ def ap_library_register_for_check(self): if not hasattr(self, 'compiled_tasks'): return + custom_flags_check(self) double_precision_check(self.compiled_tasks) if self.env.ENABLE_ONVIF: gsoap_library_check(self.bld, self.compiled_tasks) @@ -298,4 +318,6 @@ def ap_library_register_for_check(self): def configure(cfg): cfg.env.AP_LIBRARIES_OBJECTS_KW = dict() cfg.env.AP_LIB_EXTRA_SOURCES = dict() + cfg.env.AP_LIB_EXTRA_CXXFLAGS = dict() + cfg.env.AP_LIB_EXTRA_CFLAGS = dict() cfg.env.DOUBLE_PRECISION_SOURCES = dict() diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 78ca6aab34f2f..4ef709a99051c 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -325,6 +325,9 @@ def ap_stlib(bld, **kw): for l in kw['ap_libraries']: bld.ap_library(l, kw['ap_vehicle']) + if 'dynamic_source' not in kw: + kw['dynamic_source'] = 'modules/DroneCAN/libcanard/dsdlc_generated/src/**.c' + kw['features'] = kw.get('features', []) + ['cxx', 'cxxstlib'] kw['target'] = kw['name'] kw['source'] = [] diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index f39c31e3fad34..d8dae5af9f46d 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -430,14 +430,18 @@ def configure_env(self, cfg, env): if self.with_can and not cfg.env.AP_PERIPH: env.AP_LIBRARIES += [ - 'AP_UAVCAN', - 'modules/uavcan/libuavcan/src/**/*.cpp' + 'AP_DroneCAN', + 'modules/DroneCAN/libcanard/*.c', ] + if cfg.options.enable_dronecan_tests: + env.DEFINES.update( + AP_TEST_DRONECAN_DRIVERS = 1 + ) env.DEFINES.update( - UAVCAN_CPP_VERSION = 'UAVCAN_CPP03', - UAVCAN_NO_ASSERTIONS = 1, - UAVCAN_NULLPTR = 'nullptr' + DRONECAN_CXX_WRAPPERS = 1, + USE_USER_HELPERS = 1, + CANARD_ENABLE_DEADLINE = 1, ) @@ -636,8 +640,9 @@ def configure_env(self, cfg, env): if self.with_can: cfg.define('HAL_NUM_CAN_IFACES', 2) - cfg.define('UAVCAN_EXCEPTIONS', 0) - cfg.define('UAVCAN_SUPPORT_CANFD', 1) + env.DEFINES.update(CANARD_MULTI_IFACE=1, + CANARD_IFACE_ALL = 0x3, + CANARD_ENABLE_CANFD = 1) env.CXXFLAGS += [ '-Werror=float-equal' @@ -745,7 +750,7 @@ def configure_env(self, cfg, env): '-fno-slp-vectorize' # compiler bug when trying to use SLP ] - if cfg.options.sitl_32bit: + if cfg.options.force_32bit: # 32bit platform flags env.CXXFLAGS += [ '-m32', @@ -781,6 +786,7 @@ def configure_env(self, cfg, env): AP_MISSION_ENABLED = 0, HAL_RALLY_ENABLED = 0, AP_SCHEDULER_ENABLED = 0, + CANARD_ENABLE_TAO_OPTION = 1, CANARD_ENABLE_CANFD = 1, CANARD_MULTI_IFACE = 1, HAL_CANMANAGER_ENABLED = 0, @@ -794,17 +800,6 @@ def configure_env(self, cfg, env): HAL_SUPPORT_RCOUT_SERIAL = 0, AP_CAN_SLCAN_ENABLED = 0, ) - # libcanard is written for 32bit platforms - env.CXXFLAGS += [ - '-m32', - ] - env.CFLAGS += [ - '-m32', - ] - env.LDFLAGS += [ - '-m32', - ] - class esp32(Board): @@ -1017,7 +1012,7 @@ def configure_env(self, cfg, env): if cfg.env.SAVE_TEMPS: env.CXXFLAGS += [ '-S', '-save-temps=obj' ] - if cfg.options.disable_watchdog: + if cfg.options.disable_watchdog or cfg.env.DEBUG: cfg.msg("Disabling Watchdog", "yes") env.CFLAGS += [ '-DDISABLE_WATCHDOG' ] env.CXXFLAGS += [ '-DDISABLE_WATCHDOG' ] @@ -1069,16 +1064,13 @@ def configure_env(self, cfg, env): ('10','2','1'), ] - if cfg.env.AP_PERIPH: - if cfg.env.HAL_CANFD_SUPPORTED: - env.DEFINES.update(CANARD_ENABLE_CANFD=1) - else: - env.DEFINES.update(CANARD_ENABLE_TAO_OPTION=1) - if not cfg.options.bootloader: - if int(cfg.env.HAL_NUM_CAN_IFACES) > 1: - env.DEFINES.update(CANARD_MULTI_IFACE=1) - else: - env.DEFINES.update(CANARD_MULTI_IFACE=0) + if cfg.env.HAL_CANFD_SUPPORTED: + env.DEFINES.update(CANARD_ENABLE_CANFD=1) + else: + env.DEFINES.update(CANARD_ENABLE_TAO_OPTION=1) + if not cfg.options.bootloader and cfg.env.HAL_NUM_CAN_IFACES: + if int(cfg.env.HAL_NUM_CAN_IFACES) >= 1: + env.DEFINES.update(CANARD_IFACE_ALL=(1< 128 or ret < 0: - Logs.warn('uavcangen crashed with code: {}'.format(ret)) - ret = 0 - else: - Logs.error('uavcangen returned {} error code'.format(ret)) - return ret - - def post_run(self): - super(uavcangen, self).post_run() - for header in self.generator.output_dir.ant_glob("*.hpp **/*.hpp", remove=False): - header.sig = header.cache_sig = self.cache_sig - -def options(opt): - opt.load('python') - -@feature('uavcangen') -@before_method('process_source') -def process_uavcangen(self): - if not hasattr(self, 'output_dir'): - self.bld.fatal('uavcangen: missing option output_dir') - - inputs = self.to_nodes(self.source) - outputs = [] - - self.source = [] - - if not isinstance(self.output_dir, Node.Node): - self.output_dir = self.bld.bldnode.find_or_declare(self.output_dir) - - task = self.create_task('uavcangen', inputs, outputs) - task.env['OUTPUT_DIR'] = self.output_dir.abspath() - - task.env.env = dict(os.environ) - -def configure(cfg): - """ - setup environment for uavcan header generator - """ - cfg.load('python') - cfg.check_python_version(minver=(2,7,0)) - - env = cfg.env - env.DSDL_COMPILER_DIR = cfg.srcnode.make_node('modules/uavcan/libuavcan/dsdl_compiler').abspath() - env.DSDL_COMPILER = env.DSDL_COMPILER_DIR + '/libuavcan_dsdlc' - cfg.msg('DSDL compiler', env.DSDL_COMPILER) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 8cf4ea09b411d..4729468177267 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -2591,8 +2591,8 @@ def CANGPSCopterMission(self): self.reboot_sitl() # Test UAVCAN GPS ordering working - gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) - gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True) gps1_nodeid = int(gps1_det_text.split('-')[1]) gps2_nodeid = int(gps2_det_text.split('-')[1]) if gps1_nodeid is None or gps2_nodeid is None: @@ -2622,11 +2622,11 @@ def CANGPSCopterMission(self): gps1_det_text = None gps2_det_text = None try: - gps1_det_text = self.wait_text("GPS 1: specified as UAVCAN.*", regex=True, check_context=True) + gps1_det_text = self.wait_text("GPS 1: specified as DroneCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass try: - gps2_det_text = self.wait_text("GPS 2: specified as UAVCAN.*", regex=True, check_context=True) + gps2_det_text = self.wait_text("GPS 2: specified as DroneCAN.*", regex=True, check_context=True) except AutoTestTimeoutException: pass diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index e6ece2b37aa74..a19bdf335300d 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -444,10 +444,11 @@ def run_step(step): "postype_single": opts.postype_single, "extra_configure_args": opts.waf_configure_args, "coverage": opts.coverage, - "sitl_32bit" : opts.sitl_32bit, + "force_32bit" : opts.force_32bit, "ubsan" : opts.ubsan, "ubsan_abort" : opts.ubsan_abort, "num_aux_imus" : opts.num_aux_imus, + "dronecan_tests" : opts.dronecan_tests, } if opts.Werror: @@ -958,10 +959,10 @@ def format_epilog(self, formatter): action="store_true", dest="ekf_single", help="force single precision EKF") - group_build.add_option("--sitl-32bit", + group_build.add_option("--force-32bit", default=False, action='store_true', - dest="sitl_32bit", + dest="force_32bit", help="compile sitl using 32-bit") group_build.add_option("", "--ubsan", default=False, @@ -978,6 +979,11 @@ def format_epilog(self, formatter): default=0, type='int', help='number of auxiliary IMUs to simulate') + group_build.add_option("--enable-dronecan-tests", + default=False, + action='store_true', + dest="dronecan_tests", + help="enable dronecan tests") parser.add_option_group(group_build) group_sim = optparse.OptionGroup(parser, "Simulation options") diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 692fc408c27a1..0b75a17b37eca 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1494,10 +1494,11 @@ def __init__(self, replay=False, sup_binaries=[], reset_after_every_test=False, - sitl_32bit=False, + force_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, build_opts={}): self.start_time = time.time() @@ -1521,7 +1522,7 @@ def __init__(self, self.speedup = self.default_speedup() self.sup_binaries = sup_binaries self.reset_after_every_test = reset_after_every_test - self.sitl_32bit = sitl_32bit + self.force_32bit = force_32bit self.ubsan = ubsan self.ubsan_abort = ubsan_abort self.build_opts = build_opts @@ -1578,6 +1579,7 @@ def __init__(self, offline=self.terrain_in_offline_mode ) self.terrain_data_messages_sent = 0 # count of messages back + self.dronecan_tests = dronecan_tests def __del__(self): if self.rc_thread is not None: @@ -2780,7 +2782,7 @@ def all_log_format_ids(self): continue if "#if FRAME_CONFIG == HELI_FRAME" in line: continue - if "#if PRECISION_LANDING == ENABLED" in line: + if "#if AC_PRECLAND_ENABLED" in line: continue if "#end" in line: continue @@ -7856,6 +7858,9 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): passed = False reset_needed = True + # if we haven't already reset ArduPilot because it's dead, + # then ensure the vehicle was disarmed at the end of the test. + # If it wasn't then the test is considered failed: if ardupilot_alive and self.armed() and not self.is_tracker(): if ex is None: ex = ArmedAtEndOfTestException("Still armed at end of test") @@ -7872,6 +7877,10 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): self.progress("Force-rebooting SITL") self.reboot_sitl() # that'll learn it passed = False + elif not passed: # implicit reboot after a failed test: + self.progress("Test failed but ArduPilot process alive; rebooting") + self.progress("Force-rebooting SITL") + self.reboot_sitl() # that'll learn it if self._mavproxy is not None: self.progress("Stopping auto-started mavproxy") @@ -11663,10 +11672,12 @@ def Button(self): 0, want_result=mavutil.mavlink.MAV_RESULT_FAILED ) - self.wait_statustext("PreArm: Motors Emergency Stopped", check_context=True) + self.assert_prearm_failure("Motors Emergency Stopped", + other_prearm_failures_fatal=False) self.reboot_sitl() - self.delay_sim_time(10) - self.assert_prearm_failure("Motors Emergency Stopped") + self.assert_prearm_failure( + "Motors Emergency Stopped", + other_prearm_failures_fatal=False) self.context_pop() self.reboot_sitl() diff --git a/Tools/autotest/param_metadata/param_parse.py b/Tools/autotest/param_metadata/param_parse.py index b5585192bf6eb..fce23c9c10a27 100755 --- a/Tools/autotest/param_metadata/param_parse.py +++ b/Tools/autotest/param_metadata/param_parse.py @@ -12,7 +12,6 @@ import os import re import sys -import glob from argparse import ArgumentParser from param import (Library, Parameter, Vehicle, known_group_fields, @@ -91,16 +90,19 @@ def debug(str_to_print): def lua_applets(): '''return list of Library objects for lua applets and drivers''' lua_lib = Library("", reference="Lua Script", not_rst=True, check_duplicates=True) - patterns = ["libraries/AP_Scripting/applets/*.lua", "libraries/AP_Scripting/drivers/*.lua"] + dirs = ["libraries/AP_Scripting/applets", "libraries/AP_Scripting/drivers"] paths = [] - for p in patterns: - debug("Adding lua paths %s" % p) - luafiles = glob.glob(os.path.join(apm_path, p)) - for f in luafiles: - # the library is expected to have the path as a relative path from within - # a vehicle directory - f = f.replace(apm_path, "../") - paths.append(f) + for d in dirs: + for root, dirs, files in os.walk(os.path.join(apm_path, d)): + for file in files: + if not file.endswith(".lua"): + continue + f = os.path.join(root, file) + debug("Adding lua path %s" % f) + # the library is expected to have the path as a relative path from within + # a vehicle directory + f = f.replace(apm_path, "../") + paths.append(f) setattr(lua_lib, "Path", ','.join(paths)) return lua_lib diff --git a/Tools/autotest/pysim/util.py b/Tools/autotest/pysim/util.py index 68b278138cdc5..2776488c8526a 100644 --- a/Tools/autotest/pysim/util.py +++ b/Tools/autotest/pysim/util.py @@ -112,12 +112,13 @@ def waf_configure(board, coverage=False, ekf_single=False, postype_single=False, - sitl_32bit=False, + force_32bit=False, extra_args=[], extra_hwdef=None, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, extra_defines={}): cmd_configure = [relwaf(), "configure", "--board", board] if debug: @@ -130,14 +131,16 @@ def waf_configure(board, cmd_configure.append('--ekf-single') if postype_single: cmd_configure.append('--postype-single') - if sitl_32bit: - cmd_configure.append('--sitl-32bit') + if force_32bit: + cmd_configure.append('--force-32bit') if ubsan: cmd_configure.append('--ubsan') if ubsan_abort: cmd_configure.append('--ubsan-abort') if num_aux_imus > 0: cmd_configure.append('--num-aux-imus=%u' % num_aux_imus) + if dronecan_tests: + cmd_configure.append('--enable-dronecan-tests') if extra_hwdef is not None: cmd_configure.extend(['--extra-hwdef', extra_hwdef]) for nv in extra_defines.items(): @@ -174,10 +177,11 @@ def build_SITL( j=None, math_check_indexes=False, postype_single=False, - sitl_32bit=False, + force_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, ): # first configure @@ -189,11 +193,12 @@ def build_SITL( ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - sitl_32bit=sitl_32bit, + force_32bit=force_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, extra_defines=extra_defines, num_aux_imus=num_aux_imus, + dronecan_tests=dronecan_tests, extra_args=extra_configure_args,) # then clean @@ -209,7 +214,8 @@ def build_SITL( def build_examples(board, j=None, debug=False, clean=False, configure=True, math_check_indexes=False, coverage=False, - ekf_single=False, postype_single=False, sitl_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + ekf_single=False, postype_single=False, force_32bit=False, ubsan=False, ubsan_abort=False, + num_aux_imus=0, dronecan_tests=False, extra_configure_args=[]): # first configure if configure: @@ -220,10 +226,11 @@ def build_examples(board, j=None, debug=False, clean=False, configure=True, math ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - sitl_32bit=sitl_32bit, + force_32bit=force_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, - extra_args=extra_configure_args) + extra_args=extra_configure_args, + dronecan_tests=dronecan_tests) # then clean if clean: @@ -258,10 +265,11 @@ def build_tests(board, coverage=False, ekf_single=False, postype_single=False, - sitl_32bit=False, + force_32bit=False, ubsan=False, ubsan_abort=False, num_aux_imus=0, + dronecan_tests=False, extra_configure_args=[]): # first configure @@ -273,10 +281,12 @@ def build_tests(board, ekf_single=ekf_single, postype_single=postype_single, coverage=coverage, - sitl_32bit=sitl_32bit, + force_32bit=force_32bit, ubsan=ubsan, ubsan_abort=ubsan_abort, - extra_args=extra_configure_args) + num_aux_imus=num_aux_imus, + dronecan_tests=dronecan_tests, + extra_args=extra_configure_args,) # then clean if clean: diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index 78789d4ff8e78..64e83424f1270 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -1295,17 +1295,21 @@ def Rally(self): self.wait_ready_to_arm() self.arm_vehicle() + # calculate early to avoid round-trips while vehicle is moving: + accuracy = self.get_parameter("WP_RADIUS") + self.reach_heading_manual(10) self.reach_distance_manual(50) self.change_mode("RTL") + # location copied in from rover-test-rally.txt: loc = mavutil.location(40.071553, -105.229401, 0, 0) - accuracy = self.get_parameter("WP_RADIUS") - self.wait_location(loc, accuracy=accuracy, minimum_duration=10) + + self.wait_location(loc, accuracy=accuracy, minimum_duration=10, timeout=45) self.disarm_vehicle() def fence_with_bad_frame(self, target_system=1, target_component=1): @@ -6071,8 +6075,9 @@ def EStopAtBoot(self): }) self.set_rc(9, 2000) self.reboot_sitl() - self.delay_sim_time(10) - self.assert_prearm_failure("Motors Emergency Stopped") + self.assert_prearm_failure( + "Motors Emergency Stopped", + other_prearm_failures_fatal=False) self.context_pop() self.reboot_sitl() diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 58c346a118b93..6a788cb5bfb76 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -393,8 +393,8 @@ def do_build(opts, frame_options): if opts.ekf_single: cmd_configure.append("--ekf-single") - if opts.sitl_32bit: - cmd_configure.append("--sitl-32bit") + if opts.force_32bit: + cmd_configure.append("--force-32bit") if opts.ubsan: cmd_configure.append("--ubsan") @@ -670,8 +670,28 @@ def start_CAN_GPS(opts): options = vinfo.options["sitl_periph_gps"]['frames']['gps'] do_build(opts, options) exe = os.path.join(root_dir, 'build/sitl_periph_gps', 'bin/AP_Periph') - run_in_terminal_window("sitl_periph_gps", - ["nice", exe]) + cmd = ["nice"] + cmd_name = "sitl_periph_gps" + if opts.valgrind: + cmd_name += " (valgrind)" + cmd.append("valgrind") + # adding this option allows valgrind to cope with the overload + # of operator new + cmd.append("--soname-synonyms=somalloc=nouserintercepts") + cmd.append("--track-origins=yes") + if opts.gdb or opts.gdb_stopped: + cmd_name += " (gdb)" + cmd.append("gdb") + gdb_commands_file = tempfile.NamedTemporaryFile(mode='w', delete=False) + atexit.register(os.unlink, gdb_commands_file.name) + gdb_commands_file.write("set pagination off\n") + if not opts.gdb_stopped: + gdb_commands_file.write("r\n") + gdb_commands_file.close() + cmd.extend(["-x", gdb_commands_file.name]) + cmd.append("--args") + cmd.append(exe) + run_in_terminal_window(cmd_name, cmd) def start_vehicle(binary, opts, stuff, spawns=None): @@ -957,6 +977,20 @@ def generate_frame_help(): return ret +# map from some vehicle aliases back to directory names. APMrover2 +# was the old name / directory name for Rover. +vehicle_map = { + "APMrover2": "Rover", + "Copter": "ArduCopter", + "Plane": "ArduPlane", + "Sub": "ArduSub", + "Blimp" : "Blimp", + "Rover": "Rover", +} +# add lower-case equivalents too +for k in list(vehicle_map.keys()): + vehicle_map[k.lower()] = vehicle_map[k] + # define and run parser parser = CompatOptionParser( "sim_vehicle.py", @@ -968,15 +1002,10 @@ def generate_frame_help(): "simulate ArduPlane") vehicle_choices = list(vinfo.options.keys()) -# add an alias for people with too much m -vehicle_choices.append("APMrover2") -vehicle_choices.append("Copter") # should change to ArduCopter at some stage -vehicle_choices.append("Plane") # should change to ArduPlane at some stage -vehicle_choices.append("Sub") # should change to Sub at some stage -vehicle_choices.append("copter") # should change to ArduCopter at some stage -vehicle_choices.append("plane") # should change to ArduPlane at some stage -vehicle_choices.append("sub") # should change to Sub at some stage -vehicle_choices.append("blimp") # should change to Blimp at some stage + +# add vehicle aliases to argument parser options: +for c in vehicle_map.keys(): + vehicle_choices.append(c) parser.add_option("-v", "--vehicle", type='choice', @@ -1029,10 +1058,10 @@ def generate_frame_help(): action="store_true", dest="math_check_indexes", help="enable checking of math indexes") -group_build.add_option("", "--sitl-32bit", +group_build.add_option("", "--force-32bit", default=False, action='store_true', - dest="sitl_32bit", + dest="force_32bit", help="compile sitl using 32-bit") group_build.add_option("", "--configure-define", default=[], @@ -1401,22 +1430,10 @@ def generate_frame_help(): break cwd = os.path.dirname(cwd) -# map from some vehicle aliases back to canonical names. APMrover2 -# was the old name / directory name for Rover. -vehicle_map = { - "APMrover2": "Rover", - "Copter": "ArduCopter", # will switch eventually - "Plane": "ArduPlane", # will switch eventually - "Sub": "ArduSub", # will switch eventually - "copter": "ArduCopter", # will switch eventually - "plane": "ArduPlane", # will switch eventually - "sub": "ArduSub", # will switch eventually - "blimp" : "Blimp", # will switch eventually -} if cmd_opts.vehicle in vehicle_map: - progress("%s is now known as %s" % - (cmd_opts.vehicle, vehicle_map[cmd_opts.vehicle])) cmd_opts.vehicle = vehicle_map[cmd_opts.vehicle] +elif cmd_opts.vehicle.lower() in vehicle_map: + cmd_opts.vehicle = vehicle_map[cmd_opts.vehicle.lower()] # try to validate vehicle if cmd_opts.vehicle not in vinfo.options: diff --git a/Tools/autotest/test_build_options.py b/Tools/autotest/test_build_options.py index 50e9626d0e664..bb665a20135f7 100755 --- a/Tools/autotest/test_build_options.py +++ b/Tools/autotest/test_build_options.py @@ -23,9 +23,13 @@ import fnmatch import optparse import os +import sys from pysim import util +sys.path.insert(1, os.path.join(os.path.dirname(__file__), '..', 'scripts')) +import extract_features # noqa + class TestBuildOptionsResult(object): '''object to return results from a comparison''' @@ -160,6 +164,29 @@ def test_disable_feature(self, feature, options): self.test_compile_with_defines(defines) + # if the feature is truly disabled then extract_features.py + # should say so: + for target in self.build_targets: + path = self.target_to_elf_path(target) + extracter = extract_features.ExtractFeatures(path) + (compiled_in_feature_defines, not_compiled_in_feature_defines) = extracter.extract() + for define in defines: + # the following defines are known not to work on some + # or all vehicles: + feature_define_whitelist = set([ + 'AP_RANGEFINDER_ENABLED', # only at vehicle level ATM + 'AC_AVOID_ENABLED', # Rover doesn't obey this + 'AC_OAPATHPLANNER_ENABLED', # Rover doesn't obey this + 'BEACON_ENABLED', # Rover doesn't obey this (should also be AP_BEACON_ENABLED) + 'WINCH_ENABLED', # Copter doesn't use this; should use AP_WINCH_ENABLED + ]) + if define in compiled_in_feature_defines: + error = f"feature gated by {define} still compiled into ({target}); extract_features.py bug?" + if define in feature_define_whitelist: + print("warn: " + error) + else: + raise ValueError(error) + def test_enable_feature(self, feature, options): defines = self.get_enable_defines(feature, options) @@ -195,9 +222,9 @@ def test_compile_with_defines(self, defines): (t,)) raise - def find_build_sizes(self): - '''returns a hash with size of all build targets''' - ret = {} + def target_to_path(self, target, extension=None): + '''given a build target (e.g. copter), return expected path to .bin + file for that target''' target_to_binpath = { "copter": "arducopter", "plane": "arduplane", @@ -206,8 +233,26 @@ def find_build_sizes(self): "sub": "ardusub", "blimp": "blimp", } + filename = target_to_binpath[target] + if extension is not None: + filename += "." + extension + return os.path.join("build", self.board(), "bin", filename) + + def target_to_bin_path(self, target): + '''given a build target (e.g. copter), return expected path to .bin + file for that target''' + return self.target_to_path(target, 'bin') + + def target_to_elf_path(self, target): + '''given a build target (e.g. copter), return expected path to .elf + file for that target''' + return self.target_to_path(target) + + def find_build_sizes(self): + '''returns a hash with size of all build targets''' + ret = {} for target in self.build_targets: - path = os.path.join("build", self.board(), "bin", "%s.bin" % target_to_binpath[target]) + path = self.target_to_bin_path(target) ret[target] = os.path.getsize(path) return ret @@ -253,9 +298,15 @@ def run_disable_in_turn(self): options = list(filter(lambda x : fnmatch.fnmatch(x.define, self.match_glob), options)) count = 1 for feature in sorted(options, key=lambda x : x.define): + with open("/tmp/run-disable-in-turn-progress", "w") as f: + print(f.write(f"{count}/{len(options)} {feature.define}\n")) + # if feature.define < "WINCH_ENABLED": + # count += 1 + # continue if feature.define in self.must_have_defines_for_board(self._board): self.progress("Feature %s(%s) (%u/%u) is a MUST-HAVE" % (feature.label, feature.define, count, len(options))) + count += 1 continue self.progress("Disabling feature %s(%s) (%u/%u)" % (feature.label, feature.define, count, len(options))) diff --git a/Tools/autotest/web-firmware/index.html b/Tools/autotest/web-firmware/index.html index af07270203cf8..cd23bd958170c 100644 --- a/Tools/autotest/web-firmware/index.html +++ b/Tools/autotest/web-firmware/index.html @@ -97,7 +97,7 @@

Firmwares

CompanionCompanion - Companion Computer example code and Images

AP_PeriphAP_Periph - UAVCAN Peripheral Firmware

+ alt="AP_Periph">AP_Periph - DroneCAN Peripheral Firmware

FilterToolFilterTool - Filter Analysis Tool

diff --git a/Tools/bootloaders/MatekF405-CAN_bl.bin b/Tools/bootloaders/MatekF405-CAN_bl.bin index fb1863ac14b77..8dd1e4a25e805 100755 Binary files a/Tools/bootloaders/MatekF405-CAN_bl.bin and b/Tools/bootloaders/MatekF405-CAN_bl.bin differ diff --git a/Tools/bootloaders/MatekF405-CAN_bl.hex b/Tools/bootloaders/MatekF405-CAN_bl.hex index cbd76558ecc31..3ce4d5cfc5717 100644 --- a/Tools/bootloaders/MatekF405-CAN_bl.hex +++ b/Tools/bootloaders/MatekF405-CAN_bl.hex @@ -1,1024 +1,1016 @@ :020000040800F2 -:1000000000060020E50100080D0F0008110F000890 -:10001000690F0008110F00083D0F0008E7010008F4 -:10002000E7010008E7010008E7010008DD290008F2 -:10003000E7010008E7010008E7010008E701000800 -:10004000E7010008E7010008E7010008E7010008F0 -:10005000E7010008E7010008113A00083D3A0008EE -:10006000693A0008953A0008C13A0008E70100081B -:10007000E7010008E7010008E7010008E7010008C0 -:10008000E7010008E7010008E7010008E7010008B0 -:10009000E7010008E7010008E7010008ED3A000861 -:1000A000E7010008E7010008E7010008E701000890 -:1000B0002D380008E7010008E7010008E701000803 -:1000C000E7010008E7010008E7010008E701000870 -:1000D000E7010008391100084D11000861110008FE -:1000E000553B0008E7010008E7010008E7010008A8 -:1000F000E7010008E7010008E7010008E701000840 -:10010000E7010008E7010008E7010008E70100082F -:100110007511000889110008E7010008E7010008CF -:10012000E7010008E7010008E7010008E70100080F -:10013000E7010008E7010008E7010008E7010008FF -:10014000E7010008E7010008E7010008C9300008DE -:10015000E7010008E7010008E7010008E7010008DF -:10016000E7010008E7010008E7010008E7010008CF -:10017000E7010008E7010008E7010008E7010008BF -:10018000E7010008E7010008E7010008E7010008AF -:10019000E7010008E7010008E7010008E70100089F -:1001A000E7010008E7010008E7010008E70100088F -:1001B000E7010008E7010008E7010008E70100087F -:1001C000E7010008E7010008E7010008E70100086F -:1001D000E7010008E7010008E7010008E70100085F -:1001E000D50E000802E000F000F8FEE772B63948CC -:1001F00080F30888384880F3098838484EF608515B -:10020000CEF20001086040F20000CCF200004EF691 -:100210003471CEF200010860BFF34F8FBFF36F8FD0 -:1002200040F20000C0F2F0004EF68851CEF200011C -:100230000860BFF34F8FBFF36F8F4FF00000E1EE08 -:10024000100A4EF63C71CEF200010860062080F3E1 -:100250001488BFF36F8F02F0FFFB03F07DFA4FF0BD -:1002600055301F491B4A91423CBF41F8040BFAE745 -:100270001C49194A91423CBF41F8040BFAE71A495C -:100280001A4A1B4B9A423EBF51F8040B42F8040B2A -:10029000F8E700201749184A91423CBF41F8040B87 -:1002A000FAE702F0DDFB03F0B3FA144C144DAC4254 -:1002B00003DA54F8041B8847F9E700F03FF8114CC3 -:1002C000114DAC4203DA54F8041B8847F9E702F0F9 -:1002D000C5BB00000006002000220020000000082E -:1002E0000000002000060020B03F0008002200208F -:1002F00028220020282200206C440020E001000871 -:10030000E4010008E4010008E40100082DE9F04FD1 -:100310002DED108AC1F80CD0C3689D46BDEC108A43 -:10032000BDE8F08F002383F311882846A047002002 -:1003300001F0D8FF01F0FAFE00DFFEE71B4B6FF083 -:10034000090270B51A70032200245A700F229C70A3 -:10035000DC701C715C719C71DC711C725C729A7235 -:10036000DC7202F00DFB064602F032FB054650B986 -:100370000F4B9E4217D001339E4241F2883412BF88 -:10038000054600240125002002F004FB0DB100F019 -:1003900059F800F047FD00F0BDFB204600F0B0F832 -:1003A00000F050F8F9E70446EDE700BF28220020EE -:1003B000010007B008B500F02BFBA0F12003584264 -:1003C000584108BD07B5054B02211B88ADF8043024 -:1003D00001A800F089FB03B05DF804FB3C3C000879 -:1003E00010B5202383F311881248C3680BB101F0C4 -:1003F000FFFF0023104A0F484FF47A7101F0BCFF51 -:10040000002383F311880D4C236813B12368013B4B -:100410002360636813B16368013B6360084B1B781A -:1004200033B9636823B9022000F062FC32236360B1 -:1004300010BD00BF34220020E1030008502300203B -:100440004822002010B5254B254953F8042F0132CE -:1004500000D110BD8B42F8D1224C234B22689A4226 -:100460003AD9224B9B6803F1006303F580339A422B -:1004700032D2002000F068FB1D4B0220187000F003 -:1004800029FC1C4B1A6C00221A64196E1A66196E2C -:10049000596C5A64596E5A665A6E5A6942F0800213 -:1004A0005A615A6922F080025A615A691A6942F007 -:1004B00000521A611A6922F000521A611B6972B661 -:1004C0000D4A0E4B13601B682268202181F31188AE -:1004D0009D4683F30888104710BD00BFFCFF00084D -:1004E0001C00010804000108FFFF0008282200206A -:1004F000482200200038024008ED00E0000001081A -:100500002DE9F04F99B0B34C01902022FF2110A8A3 -:10051000A66800F029FCB04A1378A346A3B9AF48F7 -:100520000121C3601170202383F31188C3680BB1CC -:1005300001F05EFF0023AA4AA8484FF47A7101F047 -:100540001BFF002383F31188019B13B1A54B019A74 -:100550001A60A54AA349019F0292002313704B60C1 -:1005600099461C461D469846012000F0B3FB002F1B -:1005700000F012829B4B1B68002B40F00D8219B0DB -:10058000BDE8F08F0220FFF715FF002840F0FB8147 -:10059000019BB9F1000F08BF1F46944B1B88ADF8B3 -:1005A0002C3002210BA800F09FFADDE74FF47A709F -:1005B00000F02EFA031E0393EADB0220FFF7FAFE97 -:1005C00082460028E4D0039B581E042800F2DD81F7 -:1005D000DFE800F0030E1114170018A8052340F8F7 -:1005E000343D042100F080FA54464FF0000856E0F4 -:1005F00004217848F6E704217D48F3E704217D488B -:10060000F0E71C24204600F0A1FA04340B900421EA -:100610000BA800F069FA2C2CF4D1E5E7002DB7D037 -:10062000002CB5D00220FFF7C5FE0546002800F0DB -:10063000AF8101206C4C00F087FA0220207000F09E -:1006400049FB4FF000095FFA89FA504600F08CFA36 -:10065000074658B1504600F097FA09F10109002801 -:10066000F1D12C46A9460027634B97E701230220CE -:10067000237000F021FB3E46DBF808309E4206D294 -:10068000304600F063FA0130EBD10436F4E7029B08 -:1006900000261E70534BA9465E602C46374600F07C -:1006A000B7FB15B1002C18BF0027FFF78BFE5BE7E7 -:1006B000002D3FF46DAF002C3FF46AAF029B022087 -:1006C000187000F007FB322000F0A2F9B0F1000A28 -:1006D000C0F25E811AF0030540F05A81DBF8082071 -:1006E00006EB0A03934200F25381BAF5807F00F2D1 -:1006F0004F8155450DDA4FF47A7000F089F9049076 -:10070000049B002BC0F244813C4A049BAB5401354E -:10071000EFE7C820FFF74EFE0546002800F03881BD -:100720001F2E11D8C6F12004544528BF544610ABE3 -:1007300026F0030022463149184400F0EBFA224625 -:10074000FF212E4800F010FB4FEAAA0A5FFA8AF256 -:100750002A49304600F010FB0446002800F01A81B8 -:1007600006EB8A0605469AE70220FFF723FE0028DB -:100770003FF40EAF00F02EFA00283FF409AF4FF01F -:10078000000A5346DBF8082092451CD2BAF11F0F2D -:1007900012D8109A01320FD02AF0030218A90A4485 -:1007A00052F8202C0B92184604220BA900F0F0FB03 -:1007B0000AF1040A0346E5E75046039300F0C6F940 -:1007C000039B0B90EFE718A8042140F84C3D00F084 -:1007D0008BF964E7282200204C23002034220020DB -:1007E000E103000850230020482200203E3C00087E -:1007F0002C22002030220020403C00084C22002007 -:1008000018A8002340F8343D642100F039F900288D -:100810007FF4BEAE0220FFF7CDFD00283FF4B8AE56 -:100820000B9800F0C3F918AB43F8480D04211846A3 -:10083000CDE718A8002340F8343D642100F020F9EA -:1008400000287FF4A5AE0220FFF7B4FD00283FF496 -:100850009FAE0B9800F0B8F918AB43F8440DE5E7EC -:100860000220FFF7A7FD00283FF492AE00F0C2F986 -:1008700018AB43F8400DD9E70220FFF79BFD002895 -:100880003FF486AE0BA9142000F0BAF9824618A8EE -:10089000042140F83CAD00F027F951460BA896E73B -:1008A000322000F0B5F8B0F1000AFFF671AE1AF090 -:1008B000030F7FF46DAEDBF808200AEB08039342C8 -:1008C0003FF666AE0220FFF775FD00283FF460AEEC -:1008D0002AF0030AC244D0453FF4E1AE404600F09E -:1008E00035F904210A900AA800F0FEF808F104087E -:1008F000F1E74FF47A70FFF75DFD00283FF448AE52 -:1009000000F068F900283FF4AFAE109B01330CD023 -:10091000082210A9002000F02FFA00283FF4A4AE0E -:100920002022FF2110A800F01FFAFFF74BFD3748E7 -:1009300001F0DEFC23E6002D3FF42AAE002C3FF44C -:1009400027AE18A8002340F8343D642100F098F841 -:10095000824600287FF41CAE0220FFF72BFD002802 -:100960003FF416AE0390FFF72DFD41F2883001F001 -:10097000BFFC0B9800F08EFA00F04AFA039B574632 -:100980001C461D46F0E5054689E64FF00008FFE5E8 -:100990002546FDE52C4667E6002000F039F8049076 -:1009A000049B002BFFF6E3AD012000F085F9049BCA -:1009B000213B122B3FF6D8AD01A252F823F000BF25 -:1009C00085050008AD0500081D060008690500083A -:1009D0006905000869050008B1060008A1080008BB -:1009E00069070008010800083308000861080008CA -:1009F000690500087908000869050008F30800087F -:100A00009F0600086905000837090008A086010054 -:100A10002DE9F34702AE00244FF47A7506F8014D34 -:100A2000144FDFF85880454397F900305A1C5FFA9D -:100A300084F901D0A34212D158F82400036801229E -:100A4000D3F820A031462B46D047012807D10A4BC6 -:100A50009DF8070083F8009002B0BDE8F0870134EC -:100A6000022CE1D14FF4FA7001F042FC4FF0FF305C -:100A7000F2E700BF0022002098230020803C0008FD -:100A80002DE9F0474FF47A75144FDFF85880064689 -:100A90004D43002497F900305A1C5FFA84F901D0C5 -:100AA000A34210D158F8240003680422D3F820A0F0 -:100AB00031462B46D047042805D1094B83F80090D6 -:100AC0000020BDE8F0870134022CE3D14FF4FA7026 -:100AD00001F00EFC4FF0FF30BDE8F087002200204F -:100AE00098230020803C000830B4074B1A78074B4D -:100AF00053F822402368DD69054B0A46AC4601469F -:100B0000204630BC604700BF98230020803C00088E -:100B1000A086010070B501F037FE094C094E207027 -:100B2000002530682378834208D901F027FE336816 -:100B300005440133B5F5803F3360F2D370BD00BF8B -:100B4000992300205823002001F0CCBE00F1006062 -:100B500000F580300068704700F10060920000F5F9 -:100B6000803001F057BE0000054B1A68054B1B781A -:100B70009B1A834202D9104401F000BE0020704746 -:100B8000582300209923002038B5074D04462868D3 -:100B9000204401F0FBFD28B928682044BDE8384016 -:100BA00001F00CBE38BD00BF5823002010F0030335 -:100BB00008D1B0F5007F05D800F10050A0F508403D -:100BC0000068704700207047014BC058704700BF55 -:100BD000107AFF1F064991F8243033B10023082210 -:100BE000086A81F82430FFF7B7BF0120704700BFC3 -:100BF0005C230020014B1868704700BF002004E010 -:100C000070B52A4B1C68C4F30B03240C63B140F28B -:100C10001342934231D040F2194293422FD040F216 -:100C2000214293422DD10323214A0C2505FB0323A6 -:100C30005D6893F9082042F201039C4224D0B4F588 -:100C4000805F23D041F201039C4221D041F2030393 -:100C50009C421FD041F207039C4208BF3122441E30 -:100C60000C44013D0B46A3421ED215F9016F581CDE -:100C700096B1034600F8016CF5E70123D4E702239F -:100C8000D2E73F220B4DD6E73322E3E74122E1E7EB -:100C90005A22E4E75922E2E72C2584421D7001D94B -:100CA000981C5A70401A70BD1846FBE7002004E0FB -:100CB000503C0008443C0008022804D14FF0804218 -:100CC000034B9A6170470128FCD14FF00052F7E7BF -:100CD00000000240022804D14FF48042034B9A6185 -:100CE00070470128FCD14FF40052F7E700000240A2 -:100CF000022805D1064A536983F480435361704743 -:100D00000128FCD1024A536983F40053F6E700BF7F -:100D10000000024010B50023934203D0CC5CC454C1 -:100D20000133F9E710BD000030B5441E14F9010F7E -:100D30000B4660B193F90050854201F1010106D1E3 -:100D40001AB993F90020801A30BD013AEEE7002A63 -:100D5000F7D1104630BD000002460346981A13F939 -:100D6000011B0029FAD1704702440346934202D086 -:100D700003F8011BFAE770472DE9F047234C94F87C -:100D8000243005468846174683BB2E46DFF87C9004 -:100D9000C7B394F824302BB92022FF21484626629D -:100DA000FFF7E2FF94F82400C0F10805BD4228BF18 -:100DB0003D465FFA85FAAD0041462A4604EB8000C5 -:100DC000FFF7A8FF94F82430A7EB0A079A445FFACC -:100DD0008AFABAF1080F2E44A844FFB284F824A07E -:100DE000D6D1FFF7F7FE0028D2D108E0266A06EB3D -:100DF0008306B042CAD0FFF7EDFE0028C5D100201F -:100E0000BDE8F0870120BDE8F08700BF5C2300202B -:100E1000024B1A78024B1A70704700BF98230020CB -:100E20000022002038B5154C154D204600F01AFD63 -:100E30002946204600F042FD2D6812486A6DD2F81E -:100E4000043843F00203C2F8043801F051FA0E49A5 -:100E5000284600F03FFE6A6D0C48D2F804380C4971 -:100E600023F00203C2F80438A0424FF4E1330B60D0 -:100E700003D0BDE8384000F053BC38BD983C00209A -:100E8000883D000840420F00B03D00085424002077 -:100E90008423002038B50B4B1A780B4B53F82240B3 -:100EA0000A4B9C4205460CD0094B002118461422DF -:100EB000FFF75AFF056001462046BDE8384000F0C4 -:100EC0002FBC38BD98230020803C0008983C0020AF -:100ED0008423002070B50A4C1C263246002120468F -:100EE000FFF742FF0125074B636065703246002122 -:100EF000A019FFF739FF044B25776577236270BD92 -:100F00009C2300200E0002400D000240FEE700007E -:100F100000B59BB0EFF3098168226846FFF7FAFE3F -:100F2000EFF30583044B9A6BDA6A9A6A9A6A9A6AB3 -:100F30009A6A9A6A9B6AFEE700ED00E000B59BB0F2 -:100F4000EFF3098168226846FFF7E4FEEFF30583BB -:100F5000044B9A6B9A6A9A6A9A6A9A6A9A6A9B6A24 -:100F6000FEE700BF00ED00E000B59BB0EFF30981A4 -:100F700068226846FFF7CEFEEFF30583034B5A6BFA -:100F80009A6A9A6A9A6A9A6A9B6AFEE700ED00E09A -:100F900030B5084D0A4491420BD011F8013B0924A9 -:100FA0005840013CF7D040F300032B4083EA500047 -:100FB000F7E730BD2083B8ED002304491A465A50A4 -:100FC000C8180833802B4260F9D17047D423002021 -:100FD000026843681143016003B11847704700007D -:100FE000024AD36843F0C003D36070470010014049 -:100FF000024AD36843F0C003D36070470044004006 -:10100000024AD36843F0C003D360704700480040F1 -:10101000024AD36843F0C003D3607047004C0040DD -:10102000024AD36843F0C003D360704700500040C9 -:101030002DE9F041D0F85C64F7683368DA050546BD -:101040009CB20DD5202383F311884FF40071043036 -:10105000FFF7BEFF6FF480733360002383F31188C2 -:10106000202383F3118805F1040814F02F0333D1F2 -:1010700083F31188380615D5210613D5202383F371 -:10108000118805F1380000F0A9FA002846DA082195 -:10109000281DFFF79DFF4FF67F733B40F360002351 -:1010A00083F311887A060ED563060CD5202383F3CB -:1010B0001188EA6C2B6D9A4202D12B6C002B2FD138 -:1010C000002383F31188D5F86424D368002B31D032 -:1010D0001069BDE8F0411847230713D014F0080F3A -:1010E0000CBF00218021E00748BF41F02001A2078A -:1010F00048BF41F04001630748BF41F4807140465A -:10110000FFF766FFA406736805D595F860142846B6 -:10111000194000F013FB3468A4B2A6E77060BEE784 -:1011200027F040073F041021281D3F0CFFF750FF18 -:10113000F760C5E7BDE8F08108B50348FFF778FF21 -:10114000BDE8084001F05ABC5424002008B503480B -:10115000FFF76EFFBDE8084001F050BCBC2800203E -:1011600008B50348FFF764FFBDE8084001F046BC3E -:10117000242D002008B50348FFF75AFFBDE80840BA -:1011800001F03CBC8C31002008B50348FFF750FF4C -:10119000BDE8084001F032BCF435002010B5174C12 -:1011A000174A2046002100F099FA164BC4F85C3427 -:1011B000154C164A2046002100F090FA144BC4F852 -:1011C0005C34144C144A2046002100F087FA134B7B -:1011D000C4F85C34124C134A2046002100F07EFA19 -:1011E000114BC4F85C34114C114A0021204600F028 -:1011F00075FA104BC4F85C3410BD00BF54240020B5 -:10120000E10F000800100140BC280020F10F000889 -:1012100000440040242D0020011000080048004038 -:101220008C31002011100008004C0040F4350020E3 -:10123000211000080050004038B5494D037C0029BA -:1012400018BF0D46012B0446C0F8645410D1454B1D -:10125000984242D1444B5A6C42F010025A645A6E82 -:1012600042F010025A665B6E0B21252000F08EF8CA -:101270003E4BD4F85C249A422B6802D03C498A4207 -:101280006BD13C4901EB5301B1FBF3F3A98808048E -:1012900042BF23F0070003F0070343EA40039360D3 -:1012A000EB8843F040039BB213612B8943F00103A9 -:1012B0009BB2536141F4045343F02C03D36001F417 -:1012C000A05100231360B1F5806F136853680CBF01 -:1012D0007F23FF2384F8603438BD274B98420CD11C -:1012E000214B1A6C42F400321A641A6E42F4003236 -:1012F0001A661B6E0B212620B8E7204B98420CD1B2 -:10130000194B1A6C42F480221A641A6E42F480223D -:101310001A661B6E0B212720A8E7194B98420CD1A7 -:10132000114B1A6C42F400221A641A6E42F4002225 -:101330001A661B6E0B21342098E7124B984297D106 -:10134000094B1A6C42F480121A641A6E42F480122D -:101350001A661B6E0B21352088E70B4992E700BF08 -:10136000A83C00085424002000380240001001402E -:101370000014014000BD0105BC280020242D0020E0 -:101380008C310020F435002080DE800200F16043C3 -:1013900000F01F02400903F5614309018000C9B252 -:1013A00000F1604083F8001300F561400123934091 -:1013B000C0F8803103607047F8B515468268066949 -:1013C000AA420B46816938BF8568761AB542044641 -:1013D00007D218462A46FFF79DFCA3692B44A36158 -:1013E0000DE011D932461846FFF794FCAF1B3A4680 -:1013F000E1683044FFF78EFCE2683A44A261A368DA -:101400005B1BA3602846F8BD18462A46FFF782FCFE -:10141000E368E4E783682DE9F0410446934215460A -:10142000266938BF85684069361AB5420F4606D22C -:101430002A46FFF76FFC63692B4463610DE012D904 -:101440003246A5EB0608FFF765FC4246B919E0688D -:10145000FFF760FCE26842446261A3685B1BA36023 -:101460002846BDE8F0812A46FFF754FCE368E4E72C -:1014700010B50A440024C361029B00604060846090 -:10148000C160816141610261036210BD08B582697A -:101490004369934201D1826882B9826801328260D5 -:1014A0005A1C42611970426903699A4201D3C368A8 -:1014B0004361002100F038FF002008BD4FF0FF30ED -:1014C00008BD000070B5202304460E4683F3118842 -:1014D000A568A5B1A368A269013BA360531CA361E1 -:1014E00015782269934224BFE368A361E3690BB1D5 -:1014F00020469847002383F31188284670BD314663 -:10150000204600F001FF0028E2DA85F3118870BD63 -:101510002DE9F74F05460F4690469A46D0F81C90A5 -:10152000202686F311884FF0000B144664B1224642 -:1015300039462846FFF740FF034668B9514628461A -:1015400000F0E2FE0028F1D0002383F31188A8EB1D -:10155000040003B0BDE8F08FB9F1000F03D0019093 -:101560002846C847019B8BF31188E41A1F4486F371 -:101570001188DBE7C16081614161C3611144009B57 -:10158000006040608260016103627047F8B5044604 -:101590000E461746202383F31188A568A5B1A368DA -:1015A000013BA36063695A1C62611E702369626912 -:1015B0009A4224BFE3686361E3690BB12046984710 -:1015C000002080F31188F8BD3946204600F09CFECB -:1015D0000028E2DA85F31188F8BD000083694269CA -:1015E0009A4210B501D182687AB98268013282606C -:1015F0005A1C82611C7803699A4201D3C3688361D3 -:10160000002100F091FE204610BD4FF0FF3010BDCC -:101610002DE9F74F05460F4690469A46D0F81C90A4 -:10162000202686F311884FF0000B144664B1224641 -:1016300039462846FFF7EEFE034668B9514628466C -:1016400000F062FE0028F1D0002383F31188A8EB9C -:10165000040003B0BDE8F08FB9F1000F03D0019092 -:101660002846C847019B8BF31188E41A1F4486F370 -:101670001188DBE7026843681143016003B1184732 -:10168000704700001430FFF743BF00004FF0FF33F6 -:101690001430FFF73DBF00003830FFF7B9BF00003E -:1016A0004FF0FF333830FFF7B3BF00001430FFF7BF -:1016B00009BF00004FF0FF311430FFF703BF0000F7 -:1016C0003830FFF763BF00004FF0FF323830FFF7CC -:1016D0005DBF000000207047FFF760BD37B50F4BBE -:1016E0000360002343608360C360012304460374E6 -:1016F000154600900B464FF4007200F15C01143067 -:10170000FFF7B6FE00942B464FF4007204F51771F4 -:1017100004F13800FFF72EFF03B030BDBC3C0008D9 -:1017200010B52023044683F31188FFF785FD0223BB -:101730002374002383F3118810BD000038B5C369FA -:1017400004460D461BB904210844FFF793FF2946C0 -:1017500004F11400FFF79AFE002806DA201D4FF46A -:101760008061BDE83840FFF785BF38BD0268436837 -:101770001143016003B118477047000013B5446B73 -:10178000D4F894341A681178042915D1217C0229DF -:1017900012D11979128901238B4013420CD101A96E -:1017A00004F14C0001F0E0FED4F89444019B21794F -:1017B0000246206800F0DAF902B010BD143001F0E2 -:1017C00063BE00004FF0FF33143001F05DBE000037 -:1017D0004C3001F035BF00004FF0FF334C3001F0CA -:1017E0002FBF0000143001F031BE00004FF0FF3178 -:1017F000143001F02BBE00004C3001F001BF00009E -:101800004FF0FF324C3001F0FBBE0000D0F89424C2 -:1018100038B5136819780429054601D0012038BD70 -:10182000017C0229FAD112795C89012090400440A0 -:10183000F4D105F1140001F0C3FD02460028EDD0FB -:10184000D5F894544FF480732868697900F07CF9D6 -:10185000204638BD406BFFF7D9BF0000002070471D -:10186000704700007FB5124B036000234360836024 -:10187000C360012502260F4B0574044602900193B4 -:1018800000F18402294600964FF48073143001F071 -:1018900073FD094B0193029400964FF4807304F595 -:1018A0002372294604F14C0001F03AFE04B070BDE9 -:1018B000E43C0008551800087D1700080B6820223A -:1018C00082F311880A7903EB820290614A79093226 -:1018D00043F822008A7912B103EB82039861022354 -:1018E0000374C0F89414002383F311887047000038 -:1018F00038B5037F044613B190F85430ABB9201DBE -:1019000001250221FFF732FF04F1140025776FF063 -:10191000010100F015FD84F8545004F14C006FF003 -:101920000101BDE8384000F00BBD38BD10B5012104 -:1019300004460430FFF71AFF0023237784F854305D -:1019400010BD000038B504460025143001F02CFD10 -:1019500004F14C00257701F0FBFD201D84F8545064 -:101960000121FFF703FF2046BDE83840FFF74EBFD7 -:1019700090F8443003F06003202B19D190F84520F3 -:10198000212A0AD0222A4FF000030ED0202A0FD19C -:10199000084A82630722C26304E0064B836307237D -:1019A000C36300230364012070478363C363F9E7C3 -:1019B0000020704701220020D0F8943437B51A680F -:1019C0001178042904461AD1017C022917D119790A -:1019D000128901238B40134211D100F14C05284696 -:1019E00001F076FE58B101A9284601F0BDFDD4F8FA -:1019F0009444019B21790246206800F0B7F803B0B7 -:101A000030BD000000EB8103F7B59C6905460E462A -:101A1000F4B1202383F3118805EB8607201D0821EC -:101A2000FFF7A4FEFB685B691B684C3413B12046CA -:101A300001F0A8FD01A9204601F096FD024648B13B -:101A4000019B3146284600F091F8002383F311886A -:101A500003B0F0BDFB685A691268002AF5D01B8AF2 -:101A6000013B1340F1D105F14402EAE7093138B5F1 -:101A700050F82140DCB1202383F31188D4F894245A -:101A80001368527903EB8203DB689B695D6845B19B -:101A900004216018FFF76AFE294604F1140001F0E2 -:101AA00099FC2046FFF7B2FE002383F3118838BD6E -:101AB0007047000001F05AB801230370002343600F -:101AC000C36183620362C362436203630381438130 -:101AD0007047000038B50446202383F311880025A1 -:101AE0004160C56005614561856101F04FF80223E1 -:101AF000237085F3118838BD70B500EB810305466E -:101B00005069DA600E46144618B110220021FFF722 -:101B10002BF9A06918B110220021FFF725F93146F1 -:101B20002846BDE8704001F0FBB80000028902F0D1 -:101B300001020281428902F0010242810022026117 -:101B40004261826101F080B9F0B400EB8104478901 -:101B5000E4680125A4698D403D434581236000234D -:101B6000A2606360F0BC01F09BB90000F0B400EB30 -:101B700081040789E468012564698D403D4305813E -:101B800023600023A2606360F0BC01F015BA00007E -:101B900070B50223002504460370A0F84C5080F86D -:101BA0004E5080F84F5005814581C5600561456103 -:101BB000856180F8345001F04FF863681B6823B1E9 -:101BC00029462046BDE87040184770BD436802783A -:101BD0001B6880F85020052202700BB104211847C1 -:101BE00070470000436890F850201B6802700BB1EA -:101BF000052118477047000090F8343070B504464E -:101C000013B1002380F8343004F14402204601F07F -:101C10003DF963689B6863BB94F8445015F0600617 -:101C200015D194F8453005F07F0545EA032540F2CB -:101C300002339D4200F00E815BD8022D00F0DC8063 -:101C40003FD8002D00F08780012D00F0CF800021CB -:101C5000204601F073FB0021204601F065FB63681C -:101C60001B6813B1062120469847062384F83430B8 -:101C700070BD204698470028CED094F84B2094F8A9 -:101C80004A3043EA0223E26B934238BFE36394F99C -:101C90004430E56B002B4FF0200380F2FD80002DD7 -:101CA00000F0EC80092284F8342083F311880021AD -:101CB000E36BA26B2046FFF759FF002383F31188E3 -:101CC00070BDB5F5817F00F0B180B5F5407F49D09A -:101CD000B5F5807FBBD194F84630012BB7D1B4F86D -:101CE0004C3023F00203A4F84C30A663E66326646C -:101CF000C3E740F201639D421ED8B5F5C06F3BD2E9 -:101D0000B5F5A06FA3D1B4F84430B3F5A06F0ED1F0 -:101D100094F8463084F84E30204600F0F5FF6368B2 -:101D20001B6813B101212046984703232370002329 -:101D3000A363E3632364A0E7B5F5106F32D040F6E8 -:101D400002439D4252D0B5F5006F80D104F14F039C -:101D5000A363012324E004F14C03A3630223E363A0 -:101D600025648AE794F84630012B7FF470AFB4F80D -:101D70004C3043F00203B6E794F84920616894F8C8 -:101D800048304D6894F8471043EA0223204694F8FF -:101D90004620A84700283FF45AAF4368A36303686E -:101DA000E363A4E72378042B10D1202383F3118865 -:101DB0002046FFF7BBFE86F31188636884F84F6006 -:101DC0001B68032121700BB12046984794F84630D8 -:101DD000002BACD084F84F300423237063681B6859 -:101DE000002BA4D0022120469847A0E7374BA363DD -:101DF0000223E36300239DE794F8481011F0800F5D -:101E0000204601F00F010ED001F032F8012806D073 -:101E100002287FF41CAF2E4BA363E06367E72D4BD2 -:101E2000A363E56363E701F015F8EFE794F8463044 -:101E3000002B7FF40CAF94F8483013F00F013FF4FF -:101E400076AF1A06204602D501F08CFA6FE701F052 -:101E50007FFA6CE794F84630002B7FF4F8AE94F8E4 -:101E6000483013F00F013FF462AF1B06204602D545 -:101E700001F064FA5BE701F057FA58E7142284F89E -:101E8000342083F311882B462A4629462046FFF743 -:101E90005BFE85F3118870BD5DB1152284F8342096 -:101EA00083F311880021E36BA26B2046FFF74CFE01 -:101EB00003E70B2284F8342083F311882B462A464B -:101EC00029462046FFF752FEE3E700BF143D000815 -:101ED0000C3D0008103D000838B590F83430044639 -:101EE000152B29D8DFE803F03E28282828283E288B -:101EF000280B293928282828282828283E3E90F809 -:101F00004B1090F84A20C36B42EA01229A4214D93E -:101F1000C268128AB3FBF2F502FB15356DB92023B6 -:101F200083F311882B462A462946FFF71FFE85F3C7 -:101F300011880A2384F8343038BD142384F83430EF -:101F4000202383F3118800231A4619462046FFF701 -:101F5000FBFD002383F3118838BD036C03B1984760 -:101F60000023E7E7002101F0E9F90021204601F014 -:101F7000DBF963681B6813B10621204698470623E6 -:101F8000D8E7000090F83420152A38B5044622D846 -:101F90000123934040F6416213421DD113F4801592 -:101FA0000FD19B0217D50B2380F83430202383F305 -:101FB00011882B462A462946FFF7D8FD85F311885C -:101FC00038BDC3689B695B682BB9036C03B1984744 -:101FD000002384F8343038BD002101F0AFF900212E -:101FE000204601F0A1F963681B6813B10621204661 -:101FF00098470623EDE70000024B00221B605B6060 -:102000009A6070475C3A0020002303748268054B95 -:102010001B6899689142FBD25A6803604260106065 -:10202000586070475C3A002008B5202383F311887C -:10203000037C032B05D0042B0DD02BB983F311881F -:1020400008BD436900221A604FF0FF334361FFF778 -:10205000DBFF0023F2E790E80C001A60026853608F -:10206000F2E70000002303748268054B1B6899683F -:102070009142FBD85A680360426010605860704714 -:102080005C3A0020054B19690874186802681A60E8 -:102090005360186101230374FEF738B95C3A0020DD -:1020A00030B54B1C87B005460A4C10D023690A4A4C -:1020B00001A800F061F92846FFF7E4FF049B13B183 -:1020C00001A800F095F92369586907B030BDFFF702 -:1020D000D9FFF8E75C3A00202920000838B50C4DFC -:1020E00041612B6981689A689142044603D8BDE832 -:1020F0003840FFF789BF1846FFF786FF01232C61A0 -:10210000014623742046BDE83840FEF7FFB800BF03 -:102110005C3A0020044B1A681B6990689B689842DF -:1021200094BF0020012070475C3A002010B5084C95 -:10213000236820691A6822605460012223611A749E -:10214000FFF790FF01462069BDE81040FEF7DEB8BA -:102150005C3A0020FEE7000010B5174CFFF74CFF7B -:1021600000F0FCF880221549204600F081F8012398 -:1021700044F8180C0374124B124AD96821F4E06138 -:102180000904090C0A431049DA60CA6842F08072F7 -:10219000CA600E490A6842F001020A601022DA772A -:1021A000202283F82220002383F3118862B6084896 -:1021B000BDE8104000F07EB8843A0020183D0008C9 -:1021C00000ED00E00003FA05F0ED00E0001000E093 -:1021D000203D00082DE9F84F1F4C4FF000086569BD -:1021E00004F11407C1464FF08043266A5B6AAA686F -:1021F0009E1B96421DD34FF0200AAA68236AD5F889 -:102200000CB0134423622B68BB425F60A6EB02064E -:102210006361C5F80C8001D101F084FA89F311885B -:102220002869D8478AF311886569AB689E42E4D271 -:10223000D9E76269BA420CD0916823628E1B96601E -:10224000A86802282CBF1818981CBDE8F84F01F0A8 -:102250006FBABDE8F88F00BF5C3A0020034B5968A5 -:102260005A68521A9042FBD8704700BF001000E035 -:102270008260022202740022427470478368A3F1D4 -:102280007C0243F80C2C026943F83C2C426943F869 -:10229000382C074A43F81C2CC26843F8102C022241 -:1022A00003F8082C002203F8072CA3F1180070474C -:1022B0002503000810B5202383F31188FFF7DEFF04 -:1022C00000210446FFF70AFF002383F3118820460C -:1022D00010BD0000024B1B6958610F20FFF7D2BEF2 -:1022E0005C3A0020202383F31188FFF7F3BF00003E -:1022F00008B50146202383F311880820FFF7D0FE9C -:10230000002383F3118808BD49B1064B42681B695D -:1023100018605A60136043600420FFF7C1BE4FF09D -:10232000FF3070475C3A00200368984206D01A6874 -:102330000260506059611846FFF766BE70470000A2 -:1023400038B504460D462068844200D138BD036884 -:1023500023605C604561FFF757FEF4E7054B03F12E -:1023600014025A619A614FF0FF32DA6100221A6258 -:10237000704700BF5C3A0020F8B503614FF080431E -:10238000C2605C6A194B1A46022952F8145F38BFC2 -:102390000221954206461F460AD1586198611C6287 -:1023A0000560456081600819BDE8F84001F0B2B9E8 -:1023B000186AAB68241A0C1902D3E41A2D6804E0D9 -:1023C0009C4202D2204401F0B3F9AB689C42F4D89D -:1023D0006B68736035601E606E60B460A9684FF012 -:1023E000FF33091BA960FB61F8BD00BF5C3A002008 -:1023F00010B41B4C234653F8141F814210D041687F -:1024000002680A60026851609A424FF00001C160A0 -:1024100003D0936881680B4493605DF8044B704768 -:102420000A68626100209A425360C86003D15DF877 -:10243000044B01F077B993688868034493604FF0C8 -:102440008042206A526A121A9342E6D9991A0129E7 -:1024500098BF931C18445DF8044B01F069B900BFA4 -:102460005C3A002000207047FEE700007047000043 -:102470004FF0FF3070470000022906D0032906D034 -:102480000129064818BF0020704705487047032AF5 -:102490009ABF044800EBC200002070470C3E0008C1 -:1024A000C03D00080822002070B59AB001AD064674 -:1024B00008462946144600F095F82846FEF74CFCDD -:1024C000C0B2431C5B0086E81800237003236370CE -:1024D000002302341946DAB2904204F1020401D812 -:1024E0001AB070BDEA5C04F8022C04F8011C013338 -:1024F000F1E7000008B5202383F311880348FFF7B4 -:102500005BFA002383F3118808BD00BF983C0020CC -:1025100010B50446052916D8DFE801F01615031694 -:10252000161D202383F311880E4A0121FFF7E4FAD8 -:1025300020460D4A0221FFF7DFFA0C48FFF702FAA6 -:10254000002383F3118810BD202383F311880748EB -:10255000FFF7CEF9F4E7202383F311880348FFF750 -:10256000E5F9EDE7403D0008643D0008983C002097 -:1025700038B50C4D0C4C0D492A4604F10800FFF704 -:1025800093FF05F1CA0204F110000949FFF78CFF1F -:1025900005F5CA7204F118000649BDE83840FFF796 -:1025A00083BF00BF6041002008220020903D00084A -:1025B0009A3D0008A53D000870B5044608460D4642 -:1025C000FEF7CAFBC6B22046013403780BB91846A1 -:1025D00070BD32462946FEF7A7FB0028F3D1012043 -:1025E00070BD00002DE9F04704460D46FEF7B4FB30 -:1025F0002C49C6B22046FFF7DFFF08B10636F6B217 -:1026000029492046FFF7D8FF08B11036F6B2632EED -:102610000BD8DFF89080DFF89090244FDFF898A077 -:10262000267846B92E70BDE8F08721462846BDE8D9 -:10263000F04701F0FBBA252E2FD107224146204654 -:10264000FEF772FB70B91A4B2A4603F10C0153F8DE -:10265000040B42F8040B8B42F9D11B78137007343A -:102660000D35DDE7082249462046FEF75DFBA0B99F -:10267000104BAA1C13F8011F09095345C95D02F844 -:10268000021C197801F00F0102F10202C95D02F883 -:10269000031CEFD118350834C2E72E700134013520 -:1026A000BEE700BF2C3E0008A53D0008423E0008E2 -:1026B000343E00080F7AFF1F1B7AFF1FBFF34F8FB6 -:1026C000024AD368DB03FCD4704700BF003C0240E1 -:1026D00008B5094B1B7873B9FFF7F0FF074B1A6970 -:1026E000002ABFBF064A5A6002F188325A601A684F -:1026F00022F480621A6008BDBE430020003C024004 -:102700002301674508B50B4B1B7893B9FFF7D6FF3C -:10271000094B1A6942F000421A611A6842F4805269 -:102720001A601A6822F480521A601A6842F48062B1 -:102730001A6008BDBE430020003C02400B2870B563 -:1027400013D80B4A0B4C137863B90B4E4FF0006152 -:1027500044F8231056F8235001330C2B2944F7D1A9 -:102760000123137054F8200070BD002070BD00BF1D -:10277000F0430020C0430020543E0008014B53F8B2 -:1027800020007047543E00080C2070470B2810B5FD -:10279000044601D9002010BDFFF7D0FF064B53F8C7 -:1027A00024301844C21A0BB9012010BD126801323E -:1027B000F0D1043BF6E700BF543E00080B2810B5EB -:1027C000044621D8FFF77AFFFFF782FF0F4AF32371 -:1027D000D360C300DBB243F4007343F00203136120 -:1027E000136943F480331361FFF768FFFFF7A6FF17 -:1027F000074B53F8241000F0DDF8FFF783FF204665 -:10280000BDE81040FFF7C2BF002010BD003C0240F1 -:10281000543E00082DE9F84312F00103154640D15B -:102820008218B2F1016F3CD22C4B1B6813F00103EC -:1028300037D02B4CFFF74CFFF323E360FFF73EFF4D -:1028400040F20127032D01D9830713D0244C0F46F2 -:10285000401A40F20118EB1B0B44012B00EB07065A -:10286000236924D823F001032361FFF74BFF0120E4 -:10287000BDE8F883236923F44073236123693B4354 -:10288000236151F8046B0660BFF34F8FFFF716FF0B -:1028900003689E4208D0236923F001032361FFF7F8 -:1028A00031FF0020BDE8F883043D0430CAE723F47B -:1028B00040732361236943EA08032361B94637F86B -:1028C000023B3380BFF34F8FFFF7F8FE3688B9F82D -:1028D0000030B6B2B342BED0DDE700BF00380240E0 -:1028E000003C0240084908B50B7828B153B9FFF7FE -:1028F000EFFE01230B7008BD23B1BDE8084008704E -:10290000FFF700BF08BD00BFBE43002010B5024462 -:10291000064B0439D2B2904200D110BD441C00B223 -:1029200053F8200041F8040FE0B2F4E750280040CB -:10293000104B30B51C6F240407D41C6F44F4007492 -:102940001C671C6F44F400441C670B4C236843F461 -:10295000807323600244094B0439D2B2904200D103 -:1029600030BD441C00B251F8045F43F82050E0B27F -:10297000F4E700BF003802400070004050280040DB -:1029800007B5012201A90020FFF7C0FF019803B09D -:102990005DF804FB13B50446FFF7F2FFA04206D032 -:1029A00002A9012241F8044D0020FFF7C1FF02B047 -:1029B00010BD000070470000034B1B689B0042BF26 -:1029C000024B01221A70704774380240F143002014 -:1029D000014B1878704700BFF1430020064A536846 -:1029E00023F001035360EFF30983683383F309880D -:1029F000002383F31188704730EF00E010B52023E7 -:102A000083F31188104B5B6813F4006318D0F1EE68 -:102A1000103AEFF309844FF0807344F84C3C0B4BB1 -:102A2000DB6844F8083CA4F1680383F30988FFF7E6 -:102A300071FB18B1064B44F8503C10BD054BFAE74A -:102A400083F3118810BD00BF00ED00E030EF00E01F -:102A5000350300083803000870470000FEE7000057 -:102A6000084A094B09498B4204D3094A0021934281 -:102A700005D3704752F8040F43F8040BF3E743F80B -:102A8000041BF4E7D43F00086C4400206C44002091 -:102A90006C440020836D30B500229D68446D114662 -:102AA0004FF0FF3004EB421301329542C3F8001996 -:102AB000C3F81019C3F80809C3F8001BC3F8101BAA -:102AC000C3F8080BEED24FF00113C4F81C3830BD28 -:102AD000890141F02001016103699B06FCD41220A9 -:102AE000FFF7BCBB204B03EB80022DE9F047D2F887 -:102AF0000CE05D6DDEF81420461CD2F800C005EB3A -:102B0000063605EB4018516861450BD3D5F83428DB -:102B1000012303FA00F022EA0000C5F83408184641 -:102B2000BDE8F087BEF81040ACEB0103A34228BF1C -:102B30002346D8F81849A4B2B3EB840F10D8946890 -:102B40001F46A4F1040959F804AFC6F800A0042FE9 -:102B500001D9043FF7E71C440B4494605360D2E76B -:102B60000020BDE8F08700BFF443002010B5054CFD -:102B70002046FEF7A1FF4FF0A0436365024BA3651B -:102B800010BD00BFF4430020A83E00080378012BCD -:102B900070B5054650D12A4B446D98421BD1294B44 -:102BA0005A6B42F080025A635A6D42F080025A65B5 -:102BB0005A6D5A6942F080025A615A6922F08002C5 -:102BC0005A610E2143205B69FEF7E0FB1E4BE36078 -:102BD0001E4BC4F800380023C4F8003EC023236015 -:102BE0006E6D4FF40413A3633369002BFCDA0123E9 -:102BF00033610C20FFF732FB3369DB07FCD4122072 -:102C0000FFF72CFB3369002BFCDA0026A660284670 -:102C1000FFF740FF6B68C4F81068DB68C4F81468FD -:102C2000C4F81C684BB90A4BA3614FF0FF336361D2 -:102C3000A36843F00103A36070BD064BF4E700BF37 -:102C4000F4430020003802404014004003002002FA -:102C5000003C30C0083C30C0F8B5446D054600214A -:102C60002046FFF735FFA96D00234FF001128F6852 -:102C7000C4F834384FF00066C4F81C284FF0FF3019 -:102C800004EB431201339F42C2F80069C2F8006BA3 -:102C9000C2F80809C2F8080BF2D20B686A6DEB653E -:102CA000636210231361166916F01006FBD112201F -:102CB000FFF7D4FAD4F8003823F4FE63C4F80038E0 -:102CC000A36943F4402343F01003A3610923C4F82C -:102CD0001038C4F814380A4BEB604FF0C043C4F806 -:102CE000103B084BC4F8003BC4F81069C4F8003925 -:102CF000EB6D03F1100243F48013EA65A362F8BDA3 -:102D0000843E000840800010426D90F84E10D2F8CA -:102D1000003823F4FE6343EA0113C2F80038704719 -:102D20002DE9F0410EB200EB86080D46D8F80C10E4 -:102D30000F6807F00303022B50D0032B50D03D4AFD -:102D40003D4F012B18BF1746446D4FEA451E04EB5B -:102D50000E030022C3F8102B8A6905F1100C002A1B -:102D600040D04A8A05F158035B013A43E2500123FF -:102D7000D4F81C2803FA0CF31343A6444A69C4F898 -:102D80001C380023CEF8103905F13F03002A39D052 -:102D90000A8A898B9208012988BF4A43C16D04EBD6 -:102DA0008303561841EA0242C66529465A60204606 -:102DB000FFF78EFED8F80C301B8A43EA85531F4379 -:102DC00005F148035B01E7500123D4F81C2803FAFE -:102DD00005F51543C4F81C58BDE8F081174FB3E75B -:102DE000174FB1E704EB4613D3F8002B22F400424F -:102DF000C3F8002BD4F81C28012303FA0CF322EAB1 -:102E00000303BAE704EB83030E4A5A6004EB461649 -:102E100029462046FFF75CFED6F8003923F400432C -:102E2000C6F80039D4F81C38012202FA05F523EA65 -:102E30000505CFE700800010008004100080081016 -:102E400000800C1000040002826D1268C265FFF75A -:102E500021BE00005831436D49015B5813F4004016 -:102E600004D013F4001F14BF01200220704700009B -:102E70004831436D49015B5813F4004004D013F40A -:102E8000001F14BF012002207047000000EB8101E9 -:102E9000CB68196A0B6813604B6853607047000079 -:102EA00000EB810330B5DD68AA691368D36019B9F6 -:102EB000402B84BF402313606B8A1468426D1C440E -:102EC000013CB4FBF3F46343033323F0030302EB4D -:102ED000411043EAC44343F0C043C0F8103B2B68A1 -:102EE00003F00303012B09B20ED1D2F8083802EB2C -:102EF000411013F4807FD0F8003B14BF43F080539F -:102F000043F00053C0F8003B02EB4112D2F8003B03 -:102F100043F00443C2F8003B30BD00002DE9F0410E -:102F2000244D6E6D06EB40130446D3F8087BC3F8BE -:102F3000087B38070AD5D6F81438190706D505EBEB -:102F400084032146DB6828465B689847FA072FD53B -:102F5000D6F81438DB072BD505EB8403D968CCB938 -:102F60008B69488A5E68B6FBF0F200FB12628AB990 -:102F70001868DA6890420DD2121A83E814002023F0 -:102F800083F311880B482146FFF78AFF84F31188E9 -:102F9000BDE8F081012303FA04F26B8923EA0203FE -:102FA0006B81CB6823B121460248BDE8F041184748 -:102FB000BDE8F081F443002000EB810370B5DD68CB -:102FC000436D6C692668E6604A0156BB1A444FF4AB -:102FD0000020C2F810092A6802F00302012A0AB28E -:102FE0000ED1D3F8080803EB421410F4807FD4F814 -:102FF000000914BF40F0805040F00050C4F80009B0 -:1030000003EB4212D2F8000940F00440C2F8000974 -:10301000D3F83408012202FA01F10143C3F834184D -:1030200070BD19B9402E84BF4020206020682E8AD0 -:10303000841940F00050013C1A44B4FBF6F440EA15 -:10304000C440C6E7F8B504461E48456D05EB441379 -:10305000D3F80869C3F80869F10717D5D5F810380F -:10306000DA0713D500EB8403D9684B691F68DA6867 -:10307000974218D2D21B00271A605F60202383F387 -:1030800011882146FFF798FF87F31188330617D57B -:10309000D5F834280123A340134211D02046BDE8BF -:1030A000F840FFF71FBD012303FA04F2038923EA66 -:1030B000020303818B68002BE8D021469847E5E79F -:1030C000F8BD00BFF443002096482DE9F84F456D48 -:1030D0006E69AB691E4016F4805F6E61044605D0D0 -:1030E000FEF756FDBDE8F84FFFF788BC002E12DA58 -:1030F000D5F8003E8B489B071EBFD5F8003E23F055 -:103100000303C5F8003ED5F8043823F00103C5F8E1 -:103110000438FEF767FD370502D58248FEF756FDF5 -:10312000B0040CD5D5F8083813F0060FEB6823F47B -:1031300070530CBF43F4105343F4A053EB603107BA -:1031400004D56368DB680BB176489847F2025AD41D -:10315000B3020CD5D4F85480DFF8C8A100274FF093 -:103160000109A36D9B68F9B28B4280F08780F70656 -:1031700019D5616D0A6AC2F30A1702F00F0302F44F -:10318000F012B2F5802F00F0A180B2F5402F0AD1E5 -:1031900004EB830301F58051DB68186A00231A46AB -:1031A0009F4240F087803003D5F8184813D5E103DB -:1031B00002D50020FFF7B2FEA20302D50120FFF7DF -:1031C000ADFE630302D50220FFF7A8FE270302D558 -:1031D0000320FFF7A3FE750384D5E00702D5002086 -:1031E000FFF730FFA10702D50120FFF72BFF620791 -:1031F00002D50220FFF726FF23077FF573AF0320D8 -:10320000FFF720FF6EE7D4F85480DFF818A10027FD -:103210004FF00109A36D9B685FFA87FB5B4597D36D -:1032200008EB4B13D3F8002902F44022B2F5802FAB -:1032300022D1D3F80029002A1EDAD3F8002942F05F -:103240009042C3F80029D3F80029002AFBDB594635 -:10325000606DFFF73DFC228909FA0BF322EA0303B4 -:10326000238104EB8B03DB689B6813B159465046FE -:1032700098475846FFF736FC0137CBE708EB411279 -:10328000D2F8003B03F44023B3F5802F10D1D2F8DD -:10329000003B002B0CDA628909FA01F322EA0303EE -:1032A000638104EB8103DB68DB680BB15046984710 -:1032B000013756E79C0708BF0A68072B98BF0270C2 -:1032C00003F101039CBF120A013069E7023304EBEA -:1032D000830201F58051526890690268D0F808C0F5 -:1032E0004068A2EB000E0022104697420AD104EB80 -:1032F000830463689B699A683A449A605A681744E1 -:103300005F6050E712F0030F08BF0868964588BF5A -:103310008CF8000002F1010284BF000A0CF1010CDC -:10332000E3E700BFF4430020436D03EB4111D1F804 -:10333000003B43F40013C1F8003B7047436D03EBBF -:103340004111D1F8003943F40013C1F80039704736 -:10335000436D03EB4111D1F8003B23F40013C1F896 -:10336000003B7047436D03EB4111D1F8003923F462 -:103370000013C1F80039704730B5039C0172043363 -:1033800004FB0325C361049B03630021059B0060CC -:103390004060C16042610261856104624262816293 -:1033A000C162436330BD00000022416A4161016196 -:1033B000C2608262C2626FF00101FEF7C1BF00000D -:1033C00003694269934203D1C2680AB10020704781 -:1033D000181D704703691960C2680132C260C26972 -:1033E000134482690361934224BF436A036100214D -:1033F000FEF79ABF38B504460D46E3683BB16269F3 -:10340000131D1268A3621344E362002038BD237ABF -:1034100033B929462046FEF777FF0028EDDA38BD9C -:103420006FF00100FBE70000C368C269013BC360A5 -:103430004369134482694361934224BF436A4361F1 -:1034400000238362036B03B11847704770B52023D4 -:10345000044683F31188866A3EB9FFF7CBFF054621 -:1034600018B186F31188284670BDA36AE26A13F882 -:10347000015BA362934202D32046FFF7D5FF0023EE -:1034800083F31188EFE700002DE9F84F04460E465C -:1034900090469946202787F311880025AA46D4F83C -:1034A00028B0BBF1000F09D149462046FFF7A2FF23 -:1034B00020B18BF311882846BDE8F88FA16AE36A32 -:1034C000A8EB050B5B1A9B4528BF9B46BBF1400F41 -:1034D0001BD9334601F1400251F8040B43F8040BA9 -:1034E0009142F9D1A36A40334036A3624035A26AC3 -:1034F000E36A9A4202D32046FFF796FF8AF31188C7 -:103500004545D8D287F31188C9E730465A46FDF7BA -:1035100001FCA36A5B445E44A3625D44E7E70000EC -:1035200010B5029C0172043303FB0421C361002324 -:103530008362C362039B0363049B00604060C460BA -:1035400042610261816104624262436310BD000016 -:10355000026AC260426A4261026100228262C26201 -:103560006FF00101FEF7ECBE436902699A4203D194 -:10357000C2680AB100207047184650F8043B0B603F -:1035800070470000C368C2690133C36043691344D4 -:1035900082694361934224BF436A43610021FEF77D -:1035A000C3BE000038B504460D46E3683BB123694D -:1035B0001A1DA262E2691344E362002038BD237A37 -:1035C00033B929462046FEF79FFE0028EDDA38BDC4 -:1035D0006FF00100FBE7000003691960C268013A5F -:1035E000C260C269134482690361934224BF436A83 -:1035F000036100238362036B03B118477047000027 -:1036000070B5202304460E4683F31188856A35B9C8 -:103610001146FFF7C7FF10B185F3118870BDA36A8B -:103620001E70A36AE26A01339342A36204D3E16984 -:1036300020460439FFF7D0FF002080F3118870BDC9 -:103640002DE9F84F04460D4691469A464FF0200862 -:1036500088F311880026B346A76A4FB95146204621 -:10366000FFF7A0FF20B187F311883046BDE8F88F3F -:10367000A06AE76AA9EB06033F1A9F4228BF1F46CC -:10368000402F1BD905F1400355F8042B40F8042BBB -:103690009D42F9D1A36A4033A3624036A26AE36A2D -:1036A0009A4204D3E16920460439FFF795FF8BF372 -:1036B00011884E45D9D288F31188CDE729463A467C -:1036C000FDF728FBA36A3B443D44A3623E44E5E783 -:1036D000026943699A4203D1C3681BB9184670470F -:1036E0000023FBE7836A002BF8D0043B9B1AF5D03C -:1036F0001360C368013BC360C3691A4483690261F4 -:103700009A4224BF436A0361002383620123E5E7F1 -:1037100000F0A0B84FF08043002258631A610222E3 -:10372000DA6070474FF080430022DA607047000093 -:103730004FF08043586370474B6843608B688360E9 -:10374000CB68C3600B6943614B6903628B69436259 -:103750000B6803607047000008B5224B22481A69C5 -:1037600040F2FF110A431A611A6922F4FF7222F033 -:1037700001021A611A691A6B0A431A631A6D0A4325 -:103780001A651A4A1B6D1146FFF7D6FF184802F159 -:103790001C01FFF7D1FF174802F13801FFF7CCFFFA -:1037A000154802F15401FFF7C7FF144802F17001F8 -:1037B000FFF7C2FF124802F18C01FFF7BDFF11486D -:1037C00002F1A801FFF7B8FF0F4802F1C401FFF7AB -:1037D000B3FF0E4802F1E001FFF7AEFFBDE808407D -:1037E00000F0A2B80038024000000240B43E0008D9 -:1037F0000004024000080240000C02400010024099 -:103800000014024000180240001C02400020024048 -:1038100008B500F0F9F9FEF79FFCFFF7CDF8BDE819 -:103820000840FEF7A5BE0000704700004FF080433F -:1038300010B51A69920708D500241C61202383F370 -:103840001188FEF7C7FC84F31188BDE81040FFF72C -:10385000D5B80000104B1A6C42F001021A641A6EBF -:1038600042F001021A660D4A1B6E936843F0010391 -:1038700093604FF0804353229A624FF0FF32DA6236 -:1038800000229A615A63DA605A6001225A61082163 -:103890001A601C20FDF77ABD00380240002004E0C9 -:1038A0001F4B1A696FEAC2526FEAD2521A611A6943 -:1038B000C2F308021A614FF0FF301A695A69586161 -:1038C00000215A6959615A691A6A62F080521A6273 -:1038D0001A6A02F080521A621A6A5A6A58625A6A5E -:1038E00059625A6A1A6C42F080521A641A6E42F097 -:1038F00080521A661A6E0B4A106840F4807010608D -:10390000186F00F44070B0F5007F1EBF4FF4803098 -:1039100018671967536823F40073536000F054B9B3 -:103920000038024000700040304B4FF080521A6463 -:103930002F4A4FF4404111601A6842F001021A60A8 -:103940001A689207FCD59A6822F003029A60274B06 -:1039500019469A6812F00C02FBD1186800F0F900C1 -:1039600018609A601A6842F480321A600B689B03F0 -:10397000FCD54B6F43F001034B671C4B5A6F90070C -:10398000FCD51C4A5A601A6842F080721A60184AC4 -:1039900053685904FCD5154B1A689201FCD5164A98 -:1039A0009A60164B1A68164B9A42164B1BD1164A50 -:1039B0001168164A914216D140F205121A600B4B5B -:1039C0009A6842F002029A609A6802F00C02082A91 -:1039D000FAD15A6C42F480425A645A6E42F48042E0 -:1039E0005A665B6E704740F20572E7E700380240A6 -:1039F000007000400854400700948838002004E01C -:103A000011640020003C024000ED00E041C20F4183 -:103A1000084A08B5536911680B4003F0010353616C -:103A200023B1054A13680BB150689847BDE80840B8 -:103A3000FEF7E4BF003C0140D4230020084A08B54B -:103A4000536911680B4003F00203536123B1054A27 -:103A500093680BB1D0689847BDE80840FEF7CEBF29 -:103A6000003C0140D4230020084A08B5536911687E -:103A70000B4003F00403536123B1054A13690BB1F2 -:103A800050699847BDE80840FEF7B8BF003C0140C8 -:103A9000D4230020084A08B5536911680B4003F08D -:103AA0000803536123B1054A93690BB1D069984764 -:103AB000BDE80840FEF7A2BF003C0140D42300202F -:103AC000084A08B5536911680B4003F010035361AD -:103AD00023B1054A136A0BB1506A9847BDE8084004 -:103AE000FEF78CBF003C0140D4230020174B10B5DB -:103AF0005C691A68144004F478725A61A30604D50C -:103B0000134A936A0BB1D06A9847600604D5104AED -:103B1000136B0BB1506B9847210604D50C4A936B7D -:103B20000BB1D06B9847E20504D5094A136C0BB171 -:103B3000506C9847A30504D5054A936C0BB1D06C23 -:103B40009847BDE81040FEF759BF00BF003C014058 -:103B5000D42300201A4B10B55C691A68144004F491 -:103B60007C425A61620504D5164A136D0BB1506D43 -:103B70009847230504D5134A936D0BB1D06D984730 -:103B8000E00404D50F4A136E0BB1506E9847A104A0 -:103B900004D50C4A936E0BB1D06E9847620404D5DD -:103BA000084A136F0BB1506F9847230404D5054A98 -:103BB000936F0BB1D06F9847BDE81040FEF71EBF62 -:103BC000003C0140D4230020062108B50846FDF73B -:103BD000DDFB06210720FDF7D9FB06210820FDF7B4 -:103BE000D5FB06210920FDF7D1FB06210A20FDF7B0 -:103BF000CDFB06211720FDF7C9FB06212820BDE8D3 -:103C00000840FDF7C3BB000008B5FFF749FEFDF70C -:103C1000D3F9FDF761FDFDF74DFFFDF721FEFFF73D -:103C200003FEBDE80840FFF773BD0000034611F82E -:103C3000012B03F8012B002AF9D17047121012133F -:103C40001211000053544D3332463F3F3F000000F5 -:103C500000000000443C00083F0000001304000086 -:103C6000883C00083F00000019040000923C000856 -:103C70003F000000210400009C3C00083F000000C1 -:103C8000983C00205424002053544D3332463430A5 -:103C9000780053544D3332463432780053544D3308 -:103CA00032463434365858000096000000000000B8 -:103CB0000000000000000000000000000000000004 -:103CC000A11600088D160008C9160008B5160008D0 -:103CD000C1160008AD1600089916000885160008E0 -:103CE000D516000800000000D9170008C517000805 -:103CF00001180008ED170008F9170008E51700087B -:103D0000D1170008BD1700085D180008000000006A -:103D100001000000000000006D61696E00000000FD -:103D2000383D0008A03A0020983C00200100000027 -:103D3000552100080000000069646C650000000067 -:103D40000200000000000000051A00086D1A0008BB -:103D500040004000304100204041002002000000AF -:103D6000000000000300000000000000B11A00087D -:103D70000000000010000000504100200000000082 -:103D80000100000000000000F443002001010200D7 -:103D90004172647550696C6F740025424F41524402 -:103DA000252D424C002553455249414C2500000029 -:103DB000112500087924000871190008F52400086D -:103DC00043000000C83D000809024300020100C092 -:103DD000320904000001020201000524001001055F -:103DE00024010001042402020524060001070582C3 -:103DF000030800FF09040100020A00000007050192 -:103E0000024000000705810240000000120000008F -:103E1000143E00081201100102000040091241572F -:103E200000020102030100000403090425424F417E -:103E3000524425004D6174656B463430352D434145 -:103E40004E0030313233343536373839414243440D -:103E50004546000000400000004000000040000017 -:103E6000004000000000010000000200000002000D -:103E7000000002000000020000000200000002003A -:103E80000000020000000000F91B0008D91E000815 -:103E9000851F000840004000544400205444002086 -:103EA0000100000064440020800000004001000088 -:103EB00003000000AA01A81600000000AAAAAAAAEE -:103EC00055011400FF9F00008877000070A70A00CA -:103ED0000000000100000000AAAAAAAA0000000138 -:103EE000FFFF000000000000000000000000A01222 -:103EF00000000000AAAAAAAA00005011FFFF0000BB -:103F00000000000000770800200000000000000012 -:103F1000AAAAAAAA10000000FFFF000000080000E3 -:103F2000000000000000000000000000AAAAAAAAE9 -:103F300000000000FFFF0000000000000000000083 -:103F40000000000000000000AAAAAAAA00000000C9 -:103F5000FFFF000000000000000000000000000063 -:103F600000000000AAAAAAAA00000000FFFF0000AB -:103F70000000000000000000000000000000000041 -:103F80000A00000000000000030000000000000024 -:103F90000000000000000000000000000000000021 -:103FA0000000000000000000000000000000000011 -:103FB000FF0096000000000804000000283E0008F2 -:103FC00000000000000000000000000000000000F1 -:083FD0000000000000000000E9 +:1000000000060020E1010008D10E0008890E00085A +:10001000B10E0008890E0008A90E0008E3010008CF +:10002000E3010008E3010008E30100089936000835 +:10003000E3010008E3010008E3010008E301000810 +:10004000E3010008E3010008E3010008E301000800 +:10005000E3010008E301000841390008693900089C +:1000600091390008B9390008E1390008E3010008B6 +:10007000E3010008E3010008E3010008E3010008D0 +:10008000E3010008E3010008E3010008E3010008C0 +:10009000E3010008E3010008E3010008093A000851 +:1000A000E3010008E3010008E3010008E3010008A0 +:1000B000F13A0008E3010008E3010008E301000849 +:1000C000E3010008E3010008E3010008E301000880 +:1000D000E3010008DD3A0008E3010008E30100083D +:1000E0006D3A0008E3010008E3010008E30100089D +:1000F000E3010008E3010008E3010008E301000850 +:10010000E3010008E3010008E3010008E30100083F +:10011000E3010008E3010008E3010008E30100082F +:10012000E3010008E3010008E3010008E30100081F +:10013000E3010008E3010008E3010008E30100080F +:10014000E3010008E3010008E30100084D2E000868 +:10015000E3010008E3010008E3010008E3010008EF +:10016000E3010008E3010008E3010008E3010008DF +:10017000E3010008E3010008E3010008E3010008CF +:10018000E3010008E3010008E3010008E3010008BF +:10019000E3010008E3010008E3010008E3010008AF +:1001A000E3010008E3010008E3010008E30100089F +:1001B000E3010008E3010008E3010008E30100088F +:1001C000E3010008E3010008E3010008E30100087F +:1001D000E3010008E3010008E3010008E30100086F +:1001E00002E000F000F8FEE772B6384880F30888B5 +:1001F000374880F3098837483749086040F20000E3 +:10020000CCF200004EF63471CEF200010860BFF36C +:100210004F8FBFF36F8F40F20000C0F2F0004EF638 +:100220008851CEF200010860BFF34F8FBFF36F8F8C +:100230004FF00000E1EE100A4EF63C71CEF20001E4 +:100240000860062080F31488BFF36F8F02F0E6FA8F +:1002500002F0C2FA03F0BEF94FF055301F491B4AB5 +:1002600091423CBF41F8040BFAE71D49184A9142FC +:100270003CBF41F8040BFAE71A491B4A1B4B9A4250 +:100280003EBF51F8040B42F8040BF8E70020184970 +:10029000184A91423CBF41F8040BFAE702F0A0FA79 +:1002A00003F0ECF9144C154DAC4203DA54F8041B7E +:1002B0008847F9E700F042F8114C124DAC4203DADE +:1002C00054F8041B8847F9E702F088BA00060020BA +:1002D000002200200000000808ED00E000000020DF +:1002E00000060020103F0008002200204C220020C1 +:1002F00050220020F0300020E0010008E00100085A +:10030000E0010008E00100082DE9F04F2DED108A12 +:10031000C1F80CD0C3689D46BDEC108ABDE8F08FD3 +:10032000002383F311882846A047002001F0F0FD48 +:10033000FEE701F059FD00DFFEE7000038B502F0EE +:100340001BFA054602F03EFA0446D8B90F4B9D420F +:100350001AD001339D4218BF044641F2883504BFCC +:1003600001240025002002F011FA0CB100F076F80B +:1003700000F028FD00F0BAFB284600F0C3F800F0BA +:100380006DF8F9E70025EDE70546EBE7010007B05A +:1003900008B500F077FBA0F120035842584108BD92 +:1003A00007B541F21203022101A8ADF8043000F0B4 +:1003B00087FB03B05DF804FB38B5302383F3118865 +:1003C000174803680BB101F06FFE164A144800236A +:1003D0004FF47A7101F05EFE002383F31188124C12 +:1003E000236813B12368013B2360636813B163681A +:1003F000013B63600D4D2B7833B963687BB90220F4 +:1004000000F052FC322363602B78032B07D1636822 +:100410002BB9022000F048FC4FF47A73636038BDBA +:1004200050220020B9030008702300206822002019 +:10043000084B187003280CD8DFE800F00805020804 +:10044000022000F021BC022000F00EBC024B002272 +:100450005A6070476822002070230020244B254AF0 +:1004600010B51C461968013140D004339342F9D1CC +:100470006268214B9A4239D9204B9B6803F1006393 +:1004800003F580339A4231D2002000F049FB02206C +:10049000FFF7CEFF1A4B1A6C00221A64196E1A6607 +:1004A000196E596C5A64596E5A665A6E5A6942F0FE +:1004B00080025A615A6922F080025A615A691A69A7 +:1004C00042F000521A611A6922F000521A611B6947 +:1004D00072B64FF0E0233021C3F8084DD4E9003262 +:1004E00081F311889D4683F30888104710BD00BF33 +:1004F0000000010820000108FFFF00080022002082 +:10050000003802402DE9F04F93B0AA4B0090202212 +:10051000FF210AA89D6800F0FFFBA74A1378A3B942 +:10052000A648012103601170302383F3118803680A +:100530000BB101F0B9FDA24AA04800234FF47A7133 +:1005400001F0A8FD002383F31188009B13B19D4B9C +:10055000009A1A609C4A009C1378032B1EBF00234C +:100560001370984A4FF0000A18BF5360D34656469E +:10057000D146012000F088FB24B1924B1B68002B70 +:1005800000F01182002000F07DFA0390039B002B05 +:10059000F2DB012000F068FB039B213B162BE8D81F +:1005A00001A252F823F000BF050600082D0600083E +:1005B000C1060008730500087305000873050008EC +:1005C0004B0700081B0900083508000897080008B9 +:1005D000BF080008E508000873050008F7080008D0 +:1005E0007305000869090008A506000873050008DE +:1005F000AD09000811060008A506000873050008EB +:10060000970800080220FFF7C3FE002840F0F5819C +:10061000009B0221BAF1000F08BF1C4605A841F259 +:100620001233ADF8143000F04BFAA2E74FF47A70B1 +:1006300000F028FA071EEBDB0220FFF7A9FE0028D6 +:10064000E6D0013F052F00F2DA81DFE807F0030A68 +:100650000D10133605230593042105A800F030FA88 +:1006600017E054480421F9E758480421F6E75848B0 +:100670000421F3E74FF01C08404600F053FA08F15C +:1006800004080590042105A800F01AFAB8F12C0F0F +:10069000F2D1012000FA07F747EA0B0B5FFA8BFB58 +:1006A0004FF0000900F084FB26B10BF00B030B2B7D +:1006B00008BF0024FFF774FE5BE746480421CDE73E +:1006C000002EA5D00BF00B030B2BA1D10220FFF7BE +:1006D0005FFE074600289BD0012000F021FA02208F +:1006E000FFF7A6FE00265FFA86F8404600F028FADB +:1006F000044690B10021404600F032FA013600284D +:10070000F1D1BA46044641F21213022105A8ADF810 +:1007100014303E4600F0D4F92BE70120FFF788FEA5 +:100720002546244B9B68AB4207D9284600F0FAF9CE +:10073000013040F067810435F3E7234B00251D703D +:10074000204BBA465D603E46ACE7002E3FF460AFFA +:100750000BF00B030B2B7FF45BAF0220FFF768FE5F +:10076000322000F08FF9B0F10008FFF651AF18F019 +:1007700003077FF44DAF0F4A926808EB05039342DD +:100780003FF646AFB8F5807F3FF742AF124B01937B +:10079000B84523DD4FF47A7000F074F90390039AA2 +:1007A000002AFFF635AF019B039A03F8012B0137AE +:1007B000EDE700BF002200206C2300205022002023 +:1007C000B9030008702300206822002004220020C2 +:1007D000082200200C2200206C220020C820FFF7F5 +:1007E000D7FD074600283FF413AF1F2D11D8C5F1E0 +:1007F000200242450AAB25F0030028BF4246834948 +:100800000192184400F062FA019A8048FF2100F03A +:1008100083FA4FEAA8037D490193C8F3870228466B +:1008200000F082FA064600283FF46DAF019B05EB0D +:10083000830537E70220FFF7ABFD00283FF4E8AE61 +:1008400000F0BAF900283FF4E3AE0027B846704B39 +:100850009B68BB4218D91F2F11D80A9B01330ED0B9 +:1008600027F0030312AA134453F8203C0593404693 +:10087000042205A900F02EFB04378046E7E738463E +:1008800000F050F90590F2E7CDF81480042105A896 +:1008900000F016F906E70023642104A8049300F091 +:1008A00005F900287FF4B4AE0220FFF771FD00289F +:1008B0003FF4AEAE049800F067F90590E6E7002338 +:1008C000642104A8049300F0F1F800287FF4A0AE9E +:1008D0000220FFF75DFD00283FF49AAE049800F077 +:1008E00063F9EAE70220FFF753FD00283FF490AEDA +:1008F00000F072F9E1E70220FFF74AFD00283FF41B +:1009000087AE05A9142000F06DF904210746049074 +:1009100004A800F0D5F83946B9E7322000F0B2F863 +:10092000071EFFF675AEBB077FF472AE384A9268B9 +:1009300007EB090393423FF66BAE0220FFF728FD59 +:1009400000283FF465AE27F003074F44B9453FF454 +:10095000A9AE484600F0E6F80421059005A800F08D +:10096000AFF809F10409F1E74FF47A70FFF710FDD1 +:1009700000283FF44DAE00F01FF9002844D00A9B38 +:1009800001330BD008220AA9002000F0CDF900287D +:100990003AD02022FF210AA800F0BEF9FFF700FD9F +:1009A0001C4801F0BBFA13B0BDE8F08F002E3FF4F5 +:1009B0002FAE0BF00B030B2B7FF42AAE0023642128 +:1009C00005A8059300F072F8074600287FF420AED2 +:1009D0000220FFF7DDFC804600283FF419AEFFF748 +:1009E000DFFC41F2883001F099FA059800F02AFA0C +:1009F000464600F0DDF93C46BBE5064652E64FF0C0 +:100A0000000905E6BA467EE637467CE66C22002001 +:100A100000220020A08601002DE9F84F4FF47A73E0 +:100A2000DFF85880DFF8589006460D4602FB03F7C2 +:100A3000002498F900305A1C5FFA84FA01D0A342CE +:100A400012D159F8240003682A46D3F820B0314661 +:100A50003B46D847854207D1074B012083F800A0C9 +:100A6000BDE8F88F0124E4E7002CFBD04FF4FA70C6 +:100A700001F054FA0020F3E7B823002010220020F0 +:100A80001422002007B50023024601210DF10700C2 +:100A90008DF80730FFF7C0FF20B19DF8070003B0C5 +:100AA0005DF804FB4FF0FF30F9E700000A4608B597 +:100AB0000421FFF7B1FF80F00100C0B2404208BD41 +:100AC00030B4074B0A461978064B53F82140236887 +:100AD000DD69054B0146AC46204630BC604700BF8F +:100AE000B823002014220020A086010070B501F078 +:100AF00035FD094E094D3080002428683388834233 +:100B000008D901F025FD2B6804440133B4F5803F7A +:100B10002B60F2D370BD00BFBA23002078230020E1 +:100B200001F0DEBD00F1006000F580300068704724 +:100B300000F10060920000F5803001F05DBD000022 +:100B4000054B1A68054B1B889B1A834202D9104437 +:100B500001F0FEBC0020704778230020BA2300205B +:100B600038B5084D044629B128682044BDE838400E +:100B700001F00EBD2868204401F0F2FC0028F3D0FB +:100B800038BD00BF7823002010F003030AD1B0F570 +:100B9000047F05D200F10050A0F51040D0F80038D5 +:100BA000184670470023FBE700F10050A0F5104005 +:100BB000D0F8100A70470000064991F8243033B18C +:100BC0000023086A81F824300822FFF7B1BF012012 +:100BD000704700BF7C230020014B1868704700BF9E +:100BE000002004E0F0B51C4B024618681B4BC0F314 +:100BF0000B050424000C1E88AE421BD15C6893F9DF +:100C00000850174B05261F88874208BF93F90250EA +:100C1000013E03F10403F6D1013A0A44013C0B46BC +:100C200093420ED214F9016F581C66B100F8016CA2 +:100C30000346F5E7013C03F10C03DCD1094C3F25E9 +:100C4000DFE7184605E02C2490421C7001D2981C66 +:100C50005D70401AF0BD00BF002004E0E03B0008DA +:100C6000CC3B0008A03B0008022804D1054B4FF004 +:100C700080429A6170470128FCD1024B4FF000522C +:100C8000F7E700BF00000240022804D1054B4FF4F3 +:100C900080429A6170470128FCD1024B4FF4005208 +:100CA000F7E700BF00000240022805D1064A536959 +:100CB00083F48043536170470128FCD1024A536991 +:100CC00083F40053F6E700BF0000024010B5002394 +:100CD000934203D0CC5CC4540133F9E710BD00004B +:100CE00010B5013810F9013F3BB191F900409C4229 +:100CF00003D11AB10131013AF4E71AB191F9002098 +:100D0000981A10BD1046FCE703460246D01A12F9A5 +:100D1000011B0029FAD1704702440346934202D0D6 +:100D200003F8011BFAE770472DE9F8431F4D1446FD +:100D300095F824200746884652BBDFF870909CB394 +:100D400095F824302BB92022FF2148462F62FFF767 +:100D5000E3FF95F82400C0F10802A24228BF224612 +:100D6000D6B24146920005EB8000FFF7AFFF95F841 +:100D70002430A41B1E44F6B2082E17449044E4B25B +:100D800085F82460DBD1FFF717FF0028D7D108E0F2 +:100D90002B6A03EB82038342CFD0FFF70DFF0028BD +:100DA000CBD10020BDE8F8830120FBE77C230020A5 +:100DB000024B1A78024B1A70704700BFB82300200C +:100DC0001022002038B5194C194D204600F0E6FBE2 +:100DD0002946204600F00EFC2D6816486A6DD2F8B0 +:100DE000043843F00203C2F8043801F097F81249BE +:100DF000284600F005FD6A6D104DD2F804382868C9 +:100E00000F4923F00203C2F80438A0424FF4E13343 +:100E10000B6001D000F022FB6868A04204D0BDE85E +:100E20003840074900F01ABB38BD00BF98280020A1 +:100E3000F83C000840420F00003D0008142200204A +:100E4000A423002070B50C4B0C4D1E780C4B55F8AC +:100E500026209A4204460DD00A4B0021142218463F +:100E6000FFF75AFF0460014655F82600BDE87040C0 +:100E700000F0F4BA70BD00BFB82300201422002097 +:100E800098280020A423002000B59BB0EFF309812F +:100E900068226846FFF71AFFEFF30583014B9B6B4F +:100EA000FEE700BF00ED00E008B5FFF7EDFF000032 +:100EB00000B59BB0EFF3098168226846FFF706FF93 +:100EC000EFF30583014B5B6BFEE700BF00ED00E035 +:100ED000FEE7000030B5094D0A4491420DD011F8EB +:100EE000013B5840082340F30004013B2C4013F021 +:100EF000FF0384EA5000F6D1EFE730BD2083B8ED60 +:100F0000026843681143016003B11847704700004D +:100F1000024AD36843F0C003D36070470010014019 +:100F200010B5084C084A0021204600F075FA074B1E +:100F3000C4F85C3203F1454303F52D43C4F8603235 +:100F400010BD00BFBC230020110F0008001001409D +:100F5000234A037C002918BF0A46012B30B5C0F88C +:100F600068220CD11F4B984209D11F4B596C41F09C +:100F700010015964596E41F0100159665B6EB2F967 +:100F800004501468D0F86032D0F85C12002D03EBE6 +:100F90005403B3FBF4F3BEBF23F0070503F00703CC +:100FA00043EA450394888B60D38843F040030B6188 +:100FB000138943F001034B6144F4045343F02C03C1 +:100FC000CB6004F4A05400230B60B4F5806F0B6871 +:100FD0004B680CBF7F23FF2380F8643230BD00BF15 +:100FE000103C0008BC230020003802402DE9F041ED +:100FF000D0F85C62F7683368DA0504469DB20DD517 +:10100000302383F311884FF480610430FFF778FFB9 +:101010006FF480733360002383F31188302383F3EC +:10102000118804F1040815F02F033AD183F31188D5 +:10103000380615D5290613D5302383F3118804F11A +:10104000380000F065F900284EDA0821201DFFF76E +:1010500057FF4FF67F733B40F360002383F3118803 +:101060007A0616D56B0614D5302383F31188D4E99C +:1010700013239A420AD1236C43B127F040073F045F +:101080001021201D3F0CFFF73BFFF760002383F387 +:101090001188D4F86822D36843B3BDE8F0411069E1 +:1010A00018472B0714D015F0080F0CBF00214FF480 +:1010B0008071E80748BF41F02001AA0748BF41F00E +:1010C00040016B0748BF41F080014046FFF718FF21 +:1010D000AD06736805D594F864122046194000F0F7 +:1010E000CBF93568ADB29EE77060B6E7BDE8F08138 +:1010F000F8B5154682680669AA420B46816938BF71 +:101100008568761AB54204460BD218462A46FFF780 +:10111000DDFDA3692B44A361A3685B1BA360284684 +:10112000F8BD0CD918463246FFF7D0FDAF1BE16879 +:101130003A463044FFF7CAFDE3683B44EBE7184604 +:101140002A46FFF7C3FDE368E5E7000083689342A2 +:10115000F7B51546044638BF8568D0E90460361AED +:10116000B5420BD22A46FFF7B1FD63692B44636198 +:10117000A36828465B1BA36003B0F0BD0DD93246BF +:101180000191FFF7A3FD0199E068AF1B3A46314496 +:10119000FFF79CFDE3683B44E9E72A46FFF796FD2D +:1011A000E368E4E710B50A440024C361029B84604D +:1011B000C0E90000C0E90511C1600261036210BD11 +:1011C00008B5D0E90532934201D1826882B98268BC +:1011D000013282605A1C42611970D0E904329A428D +:1011E00024BFC3684361002100F0B4FE002008BDA5 +:1011F0004FF0FF30FBE7000070B5302304460E4689 +:1012000083F31188A568A5B1A368A269013BA36017 +:10121000531CA36115782269934224BFE368A3613C +:10122000E3690BB120469847002383F311882846D1 +:1012300007E03146204600F07DFE0028E2DA85F323 +:10124000118870BD2DE9F74F04460E4617469846A3 +:10125000D0F81C904FF0300A8AF311884FF0000B41 +:10126000154665B12A4631462046FFF741FF034641 +:1012700060B94146204600F05DFE0028F1D0002311 +:1012800083F31188781B03B0BDE8F08FB9F1000F2C +:1012900003D001902046C847019B8BF31188ED1ABB +:1012A0001E448AF31188DCE7C0E90511C160C361FF +:1012B0001144009B8260C0E9000001610362704735 +:1012C000F8B504460D461646302383F31188A76807 +:1012D000A7B1A368013BA36063695A1C62611D70DA +:1012E000D4E904329A4224BFE3686361E3690BB135 +:1012F00020469847002080F3118807E031462046B9 +:1013000000F018FE0028E2DA87F31188F8BD00002B +:10131000D0E905239A4210B501D182687AB9826872 +:10132000013282605A1C82611C7803699A4224BF90 +:10133000C3688361002100F00DFE204610BD4FF010 +:10134000FF30FBE72DE9F74F04460E461746984657 +:10135000D0F81C904FF0300A8AF311884FF0000B40 +:10136000154665B12A4631462046FFF7EFFE034693 +:1013700060B94146204600F0DDFD0028F1D0002391 +:1013800083F31188781B03B0BDE8F08FB9F1000F2B +:1013900003D001902046C847019B8BF31188ED1ABA +:1013A0001E448AF31188DCE7026843681143016038 +:1013B00003B11847704700001430FFF743BF000027 +:1013C0004FF0FF331430FFF73DBF00003830FFF718 +:1013D000B9BF00004FF0FF333830FFF7B3BF000054 +:1013E0001430FFF709BF00004FF0FF311430FFF752 +:1013F00003BF00003830FFF763BF00004FF0FF323B +:101400003830FFF75DBF0000012914BF6FF01300F3 +:1014100000207047FFF784BD37B515460E4A0260BD +:1014200000224260C0E902220122044602740B46F7 +:10143000009000F15C014FF480721430FFF7B2FEAF +:1014400000942B464FF4807204F5AE7104F138001D +:10145000FFF72AFF03B030BD243C000810B530234D +:10146000044683F31188FFF773FD022323740020E1 +:1014700080F3118810BD000038B5C36904460D46DD +:101480001BB904210844FFF78FFF294604F114001B +:10149000FFF796FE002806DA201D4FF40061BDE834 +:1014A0003840FFF781BF38BD0268436811430160CF +:1014B00003B118477047000013B5446BD4F8943457 +:1014C0001A681178042915D1217C022912D11979C1 +:1014D000128901238B4013420CD101A904F14C0065 +:1014E00001F034FFD4F89444019B2179024620682E +:1014F00000F0D0F902B010BD143001F0B7BE00000A +:101500004FF0FF33143001F0B1BE00004C3001F059 +:1015100089BF00004FF0FF334C3001F083BF000063 +:10152000143001F085BE00004FF0FF31143001F09F +:101530007FBE00004C3001F055BF00004FF0FF327D +:101540004C3001F04FBF00000020704710B5D0F8BC +:1015500094341A6811780429044617D1017C0229B1 +:1015600014D15979528901238B4013420ED1143082 +:1015700001F018FE024648B1D4F894444FF4807349 +:1015800061792068BDE8104000F072B910BD00001C +:10159000406BFFF7DBBF0000704700007FB5124BC8 +:1015A000036000234360C0E90233012502260F4B8C +:1015B000057404460290019300F1840229460096C6 +:1015C0004FF48073143001F0C9FD094B0294CDE94A +:1015D000006304F523724FF48073294604F14C0034 +:1015E00001F090FE04B070BD4C3C0008911500085D +:1015F000B91400080B68302282F311880A7903EBD2 +:10160000820290614A79093243F822008A7912B144 +:1016100003EB820398610223C0F894140374002042 +:1016200080F311887047000038B5037F044613B17A +:1016300090F85430ABB9201D01250221FFF734FF8B +:1016400004F1140025776FF0010100F08FFC84F89D +:10165000545004F14C006FF00101BDE8384000F037 +:1016600085BC38BD10B5012104460430FFF71CFFCE +:101670000023237784F8543010BD000038B50446A9 +:101680000025143001F082FD04F14C00257701F0B3 +:1016900051FE201D84F854500121FFF705FF20461C +:1016A000BDE83840FFF752BF90F8443003F06003C4 +:1016B000202B07D190F84520212A4FF0000303D8B2 +:1016C0001F2A06D800207047222AFBD1C0E90E331A +:1016D00003E0034A82630722C26303640120704768 +:1016E0001C22002037B5D0F894341A6811780429E8 +:1016F00004461AD1017C022917D1197912890123D4 +:101700008B40134211D100F14C05284601F0D2FE66 +:1017100058B101A9284601F019FED4F89444019B60 +:1017200021790246206800F0B5F803B030BD000012 +:10173000F0B500EB810385B09E6904460D46FEB10D +:10174000302383F3118804EB8507301D0821FFF750 +:10175000ABFEFB685B691B6806F14C001BB1019096 +:1017600001F002FE019803A901F0F0FD024648B124 +:10177000039B2946204600F08DF8002383F311884F +:1017800005B0F0BDFB685A691268002AF5D01B8AC3 +:10179000013B1340F1D104F14402EAE7093138B5C5 +:1017A00050F82140DCB1302383F31188D4F894241D +:1017B0001368527903EB8203DB689B695D6845B16E +:1017C00004216018FFF770FE294604F1140001F0AF +:1017D000F3FC2046FFF7BAFE002383F3118838BDDF +:1017E0007047000001F052B8012303700023C0E9E4 +:1017F0000133C36183620362C36243620363704760 +:1018000038B50446302383F311880025C0E9035519 +:10181000C0E90555416001F049F80223237085F3C2 +:101820001188284638BD000070B500EB81030546DD +:101830005069DA600E46144618B110220021FFF7F5 +:101840006BFAA06918B110220021FFF765FA314642 +:101850002846BDE8704001F0F3B80000826802F04D +:10186000011282600022C0E90422826101F074B991 +:10187000F0B400EB81044789E4680125A4698D4038 +:101880003D43458123600023A2606360F0BC01F00A +:101890008FB90000F0B400EB81040789E4680125EA +:1018A00064698D403D43058123600023A26063602D +:1018B000F0BC01F009BA000070B50223002504460F +:1018C0000370C0E90255C0E90455C564856180F81C +:1018D000345001F051F863681B6823B12946204653 +:1018E000BDE87040184770BD0378052B10B504465D +:1018F0000AD080F850300523037043681B680BB191 +:10190000042198470023A36010BD00000178052939 +:1019100006D190F85020436802701B6803B1184745 +:101920007047000070B590F83430044613B10023BE +:1019300080F8343004F14402204601F02FF9636846 +:101940009B68B3B994F8443013F0600535D000219A +:10195000204601F0CFFB0021204601F0C1FB636867 +:101960001B6813B1062120469847062384F83430BB +:1019700070BD204698470028E4D0B4F84A30E26BA6 +:101980009A4288BFE36394F94430E56B002B4FF033 +:10199000300380F20381002D00F0F280092284F8E8 +:1019A000342083F311880021D4E90E232046FFF769 +:1019B00071FF002383F31188DAE794F8452003F0E0 +:1019C0007F0343EA022340F20232934200F0C580D3 +:1019D00021D8B3F5807F48D00DD8012B3FD0022B02 +:1019E00000F09380002BB2D104F14C02A2630222DA +:1019F000E2632364C1E7B3F5817F00F09B80B3F518 +:101A0000407FA4D194F84630012BA0D1B4F84C30DB +:101A100043F0020332E0B3F5006F4DD017D8B3F5B1 +:101A2000A06F31D0A3F5C063012B90D8636894F800 +:101A300046205E6894F84710B4F848302046B04716 +:101A4000002884D04368A3630368E3631AE0B3F516 +:101A5000106F36D040F6024293427FF478AF5C4B71 +:101A6000A3630223E3630023C3E794F84630012B0A +:101A70007FF46DAFB4F84C3023F00203C4E90E5587 +:101A8000A4F84C30256478E7B4F84430B3F5A06F7F +:101A90000ED194F8463084F84E30204600F0C4FF52 +:101AA00063681B6813B10121204698470323237004 +:101AB0000023C4E90E339CE704F14F03A363012321 +:101AC000C3E72378042B10D1302383F311882046F9 +:101AD000FFF7C4FE85F311880321636884F84F5033 +:101AE00021701B680BB12046984794F84630002BB4 +:101AF000DED084F84F300423237063681B68002B0A +:101B0000D6D0022120469847D2E794F848301D06E7 +:101B100003F00F0120460AD501F032F8012804D065 +:101B200002287FF414AF2B4B9AE72B4B98E701F078 +:101B300019F8F3E794F84630002B7FF408AF94F8D7 +:101B4000483013F00F01B3D01A06204602D501F039 +:101B5000E5FAADE701F0D8FAAAE794F84630002B91 +:101B60007FF4F5AE94F8483013F00F01A0D01B06B7 +:101B7000204602D501F0BEFA9AE701F0B1FA97E7E4 +:101B8000142284F8342083F311882B462A462946F0 +:101B90002046FFF76DFE85F31188E9E65DB1152259 +:101BA00084F8342083F311880021D4E90E232046E1 +:101BB000FFF75EFEFDE60B2284F8342083F31188E4 +:101BC0002B462A4629462046FFF764FEE3E700BF7E +:101BD0007C3C0008743C0008783C000838B590F85C +:101BE00034300446002B3ED0063BDAB20F2A34D8FC +:101BF0000F2B32D8DFE803F0373131082232313190 +:101C00003131313131313737C56BB0F84A309D420F +:101C100014D2C3681B8AB5FBF3F203FB12556DB9EE +:101C2000302383F311882B462A462946FFF732FEDC +:101C300085F311880A2384F834300EE0142384F8E5 +:101C40003430302383F3118800231A461946204686 +:101C5000FFF70EFE002383F3118838BD036C03B138 +:101C600098470023E7E70021204601F043FA0021CE +:101C7000204601F035FA63681B6813B1062120463F +:101C800098470623D7E7000010B590F83430142B9E +:101C9000044629D017D8062B05D001D81BB110BD9A +:101CA000093B022BFBD80021204601F023FA00213A +:101CB000204601F015FA63681B6813B1062120461F +:101CC0009847062319E0152BE9D10B2380F834300F +:101CD000302383F3118800231A461946FFF7DAFDF3 +:101CE000002383F31188DAE7C3689B695B68002BE4 +:101CF000D5D1036C03B19847002384F83430CEE784 +:101D000000230375826803691B6899689142FBD2BE +:101D10005A68036042601060586070470023037582 +:101D2000826803691B6899689142FBD85A6803600E +:101D3000426010605860704708B50846302383F34E +:101D400011880B7D032B05D0042B0DD02BB983F309 +:101D5000118808BD8B6900221A604FF0FF33836140 +:101D6000FFF7CEFF0023F2E7D1E9003213605A609B +:101D7000F3E70000FFF7C4BF054BD9680875186882 +:101D800002681A60536001220275D860FEF7BCBA7F +:101D90002826002030B50C4BDD684B1C87B004466C +:101DA0000FD02B46094A684600F074F92046FFF729 +:101DB000E3FF009B13B1684600F076F9A86907B00D +:101DC00030BDFFF7D9FFF9E728260020391D0008AC +:101DD000044B1A68DB6890689B68984294BF0020A7 +:101DE0000120704728260020084B10B51C68D868D1 +:101DF00022681A60536001222275DC60FFF78EFFB3 +:101E000001462046BDE81040FEF77EBA2826002095 +:101E1000044B1A68DB6892689B689A4201D9FFF705 +:101E2000E3BF70472826002038B5074C074908480B +:101E3000012300252370656001F074FB02232370E9 +:101E400085F3118838BD00BF90280020843C00082D +:101E50002826002000F05EB9EFF3118020B9EFF3DF +:101E60000583302282F311887047000010B530B925 +:101E7000EFF30584C4F3080414B180F3118810BD96 +:101E8000FFF7C6FF84F31188F9E70000034A5168A1 +:101E900053685B1A9842FBD8704700BF001000E0FF +:101EA0008B60022308618B82084670478368A3F128 +:101EB000840243F8142C026943F8442C426943F825 +:101EC000402C094A43F8242CC26843F8182C0222FB +:101ED00003F80C2C002203F80B2C044A43F8102CB6 +:101EE000A3F12000704700BF21030008282600202E +:101EF00008B5FFF7DBFFBDE80840FFF73BBF000078 +:101F0000024BDB6898610F20FFF736BF28260020C0 +:101F1000302383F31188FFF7F3BF000008B50146B3 +:101F2000302383F311880820FFF734FF002383F365 +:101F3000118808BD064BDB6839B1426818605A60E9 +:101F4000136043600420FFF725BF4FF0FF30704758 +:101F5000282600200368984206D01A680260506064 +:101F600099611846FFF706BF7047000038B5044670 +:101F70000D462068844200D138BD036823605C6050 +:101F80008561FFF7F7FEF4E710B503689C68A2428D +:101F90000CD85C688A600B604C602160596099685D +:101FA0008A1A9A604FF0FF33836010BD1B68121BC2 +:101FB000ECE700000A2938BF0A2170B504460D4637 +:101FC0000A26601901F0A8FA01F094FA041BA54250 +:101FD00003D8751C2E460446F3E70A2E04D9BDE843 +:101FE0007040012001F0DEBA70BD0000F8B5144B5E +:101FF0000D46D96103F1100141600A2A1969826016 +:1020000038BF0A22016048601861A818144601F020 +:1020100075FA0A2701F06EFA431BA342064606D35F +:102020007C1C281901F078FA27463546F2E70A2F7A +:1020300004D9BDE8F840012001F0B4BAF8BD00BFF2 +:1020400028260020F8B506460D4601F053FA0F4A3F +:10205000134653F8107F9F4206D12A460146304668 +:10206000BDE8F840FFF7C2BFD169BB68441A2C191C +:1020700028BF2C46A34202D92946FFF79BFF2246E0 +:1020800031460348BDE8F840FFF77EBF2826002010 +:102090003826002010B4C0E9032300235DF8044B68 +:1020A0004361FFF7CFBF000010B5194C2369984278 +:1020B0000DD0D0E90032816813605A609A680A44F2 +:1020C0009A60002303604FF0FF33A36110BD2346E5 +:1020D000026843F8102F53600022026022699A427E +:1020E00003D1BDE8104001F011BA936881680B4438 +:1020F000936001F0FFF92269E1699268441AA242F3 +:10210000E4D91144BDE81040091AFFF753BF00BFDE +:10211000282600202DE9F047DFF8BC8008F11007E1 +:102120002C4ED8F8105001F0E5F9D8F81C40AA68F8 +:10213000031B9A423ED81444D5E900324FF00009FF +:10214000C8F81C4013605A60C5F80090D8F81030E9 +:10215000B34201D101F0DAF989F31188D5E90331ED +:1021600028469847302383F311886B69002BD8D019 +:1021700001F0C0F96A69A0EB04094A4582460DD214 +:10218000022001F00FFA0022D8F81030B34208D133 +:1021900051462846BDE8F047FFF728BF121A2244EF +:1021A000F2E712EB090938BF4A4629463846FFF7DD +:1021B000EBFEB5E7D8F81030B34208D01444211A2A +:1021C000C8F81C00A960BDE8F047FFF7F3BEBDE802 +:1021D000F08700BF38260020282600200020704706 +:1021E000FEE70000704700004FF0FF30704700002E +:1021F00002290CD0032904D00129074818BF002068 +:102200007047032A05D8054800EBC2007047044810 +:1022100070470020704700BF5C3D00082C22002062 +:10222000103D000870B59AB00546084601A914464D +:1022300000F0C2F801A8FEF767FD431C5B00C5E98A +:1022400000340022237003236370C6B201AB023452 +:102250001046D1B28E4204F1020401D81AB070BD0A +:1022600013F8011B04F8021C04F8010C0132F0E71A +:1022700008B5302383F311880348FFF733FA0023AE +:1022800083F3118808BD00BF9828002090F84430DF +:1022900003F01F02012A07D190F845200B2A03D131 +:1022A0000023C0E90E3315E003F06003202B08D1B2 +:1022B000B0F848302BB990F84520212A03D81F2ABE +:1022C00004D8FFF7F1B9222AEBD0FAE7034A826378 +:1022D0000722C26303640120704700BF232200204D +:1022E00007B5052917D8DFE801F0191603191920D9 +:1022F000302383F31188104A01900121FFF794FAEB +:1023000001980E4A0221FFF78FFA0D48FFF7B6F940 +:10231000002383F3118803B05DF804FB302383F3BB +:1023200011880748FFF780F9F2E7302383F311881B +:102330000348FFF797F9EBE7B03C0008D43C0008EE +:102340009828002038B50C4D0C4C0D492A4604F154 +:102350000800FFF767FF05F1CA0204F11000094900 +:10236000FFF760FF05F5CA7204F118000649BDE8E1 +:102370003840FFF757BF00BF602D00202C220020FF +:10238000903C00089A3C0008A53C000870B5044643 +:1023900008460D46FEF7B8FCC6B220460134037865 +:1023A0000BB9184670BD32462946FEF799FC002845 +:1023B000F3D10120F6E700002DE9F04705460C4671 +:1023C000FEF7A2FC2B49C6B22846FFF7DFFF08B193 +:1023D0000636F6B228492846FFF7D8FF08B110366E +:1023E000F6B2632E0BD8DFF88C80DFF88C90234F89 +:1023F000DFF894A02E7846B92670BDE8F08729460C +:102400002046BDE8F04701F0C3BB252E2ED10722A0 +:1024100041462846FEF764FC70B9194B224603F189 +:102420000C0153F8040B42F8040B8B42F9D11B78D2 +:10243000137007350D34DDE7082249462846FEF7BC +:102440004FFC98B90F4BA21C197809090232C95DDB +:1024500002F8041C13F8011B01F00F015345C95D7C +:1024600002F8031CF0D118340835C3E704F8016BF7 +:102470000135BFE77C3D0008A53C0008923D0008FF +:10248000843D0008107AFF1F1C7AFF1FBFF34F8F97 +:10249000024AD368DB03FCD4704700BF003C024013 +:1024A00008B5094B1B7873B9FFF7F0FF074B1A69A2 +:1024B000002ABFBF064A5A6002F188325A601A6881 +:1024C00022F480621A6008BDBE2F0020003C02404A +:1024D0002301674508B50B4B1B7893B9FFF7D6FF6F +:1024E000094B1A6942F000421A611A6842F480529C +:1024F0001A601A6822F480521A601A6842F48062E4 +:102500001A6008BDBE2F0020003C02400B28F0B529 +:1025100016D80C4C0C4923787BB90C4D0E460C2375 +:102520004FF0006255F8047B46F8042B013B13F092 +:10253000FF033A44F6D10123237051F82000F0BD87 +:102540000020FCE7F02F0020C02F0020A43D000851 +:10255000014B53F820007047A43D00080C20704741 +:102560000B2810B5044601D9002010BDFFF7CEFF9F +:10257000064B53F824301844C21A0BB90120F4E773 +:1025800012680132F0D1043BF6E700BFA43D000819 +:102590000B2838B5044628D8FFF75EFCFFF776FF16 +:1025A000FFF77EFF124AF323D360E300DBB243F46C +:1025B000007343F002031361136943F48033136122 +:1025C00005462046FFF762FFFFF7A0FF094B53F8CF +:1025D000241000F0E9F82846FFF77CFFFFF746FCDF +:1025E0002046BDE83840FFF7BBBF002038BD00BF24 +:1025F000003C0240A43D000812F001032DE9F04127 +:1026000005460E4614464BD18218B2F1016F61D8CF +:10261000314B1B6813F001035CD0304FFFF71CFCFB +:10262000FFF73EFFF323FB60FFF730FF314640F238 +:102630000128032C18D824F00104284E0C446D1AEC +:1026400040F20118A142336905EB01072AD123F0BA +:1026500001033361FFF73EFFFFF708FC0120BDE8EF +:10266000F081043C0435E4E7AB07E4D13B6923F493 +:1026700040733B613B6943EA08033B6151F8046BDB +:102680002E60BFF34F8FFFF701FF2B689E42E8D00B +:102690003B6923F001033B61FFF71CFFFFF7E6FBFB +:1026A0000020DCE723F440733361336943EA080315 +:1026B00033610B883B80BFF34F8FFFF7E7FE3F8806 +:1026C00031F8023BBFB2BB42BCD0336923F00103F7 +:1026D0003361E1E71846C2E700380240003C02409F +:1026E000084908B50B7828B11BB9FFF7D9FE0123BB +:1026F0000B7008BD002BFCD0BDE808400870FFF748 +:10270000E9BE00BFBE2F002010B50244064BD2B276 +:10271000904200D110BD441C00B253F8200041F893 +:10272000040BE0B2F4E700BF502800400F4B30B577 +:102730001C6F240407D41C6F44F400741C671C6FC6 +:1027400044F400441C670A4C236843F480732360FC +:102750000244084BD2B2904200D130BD441C00B2BA +:1027600051F8045B43F82050E0B2F4E7003802402F +:10277000007000405028004007B5012201A9002048 +:10278000FFF7C2FF019803B05DF804FB13B50446E0 +:10279000FFF7F2FFA04205D0012201A90020019419 +:1027A000FFF7C4FF02B010BD70470000034B1A686A +:1027B0001AB9034AD2F874281A607047F42F00201F +:1027C0000030024008B5FFF7F1FF024B1868C0F374 +:1027D000407008BDF42F002070470000FEE70000A5 +:1027E0000A4B0B480B4A90420BD30B4BDA1C121AC4 +:1027F000C11E22F003028B4238BF00220021FEF7E7 +:102800008BBA53F8041B40F8041BECE75C3F00084C +:10281000F0300020F0300020F03000207047000041 +:1028200070B5D0E915439E6800224FF0FF3504EBE8 +:1028300042135101D3F800090028BEBFD3F80009A4 +:1028400040F08040C3F80009D3F8000B0028BEBF59 +:10285000D3F8000B40F08040C3F8000B013263183E +:102860009642C3F80859C3F8085BE0D24FF0011351 +:10287000C4F81C3870BD0000890141F020010161DD +:1028800003699B06FCD41220FFF700BB10B5054C72 +:102890002046FEF7A9FF4FF0A0436365024BA365F6 +:1028A00010BD00BFF82F0020F83D000870B5037878 +:1028B000012B054650D12A4B446D98421BD1294B20 +:1028C0005A6B42F080025A635A6D42F080025A6598 +:1028D0005A6D5A6942F080025A615A6922F08002A8 +:1028E0005A610E2143205B6900F022FC1E4BE3601D +:1028F0001E4BC4F800380023C4F8003EC0232360F8 +:102900006E6D4FF40413A3633369002BFCDA0123CB +:1029100033610C20FFF7BAFA3369DB07FCD41220CD +:10292000FFF7B4FA3369002BFCDA0026A6602846CC +:10293000FFF776FF6B68C4F81068DB68C4F81468AA +:10294000C4F81C684BB90A4BA3614FF0FF336361B5 +:10295000A36843F00103A36070BD064BF4E700BF1A +:10296000F82F0020003802404014004003002002ED +:10297000003C30C0083C30C0F8B5446D054600212D +:102980002046FFF779FFA96D00234FF001128F68F1 +:10299000C4F834384FF00066C4F81C284FF0FF30FC +:1029A00004EB431201339F42C2F80069C2F8006B86 +:1029B000C2F80809C2F8080BF2D20B686A6DEB6521 +:1029C000636210231361166916F01006FBD1122002 +:1029D000FFF75CFAD4F8003823F4FE63C4F800383B +:1029E000A36943F4402343F01003A3610923C4F80F +:1029F0001038C4F814380A4BEB604FF0C043C4F8E9 +:102A0000103B084BC4F8003BC4F81069C4F8003907 +:102A1000EB6D03F1100243F48013EA65A362F8BD85 +:102A2000D43D000840800010426D90F84E10D2F85E +:102A3000003823F4FE6343EA0113C2F800387047FC +:102A40002DE9F84300EB8103456DDA68166806F05E +:102A50000306731E022B05EB41130C4680460FFA4A +:102A600081F94FEA41104FF00001C3F8101B4FF0FD +:102A7000010104F1100398BFB60401FA03F3916950 +:102A80008EBF334E06F1805606F5004600293AD037 +:102A9000578A04F15801490137436F50D5F81C1883 +:102AA0000B43C5F81C382B180021C3F810195369C3 +:102AB0000127611EA7409BB3138A928B9B08012AB2 +:102AC00088BF5343D8F85C20981842EA034301F1C9 +:102AD000400205EB8202C8F85C002146536028469C +:102AE000FFF7CAFE08EB8900C3681B8A43EA8453D8 +:102AF000483464011E432E51D5F81C381F43C5F8D5 +:102B00001C78BDE8F88305EB4917D7F8001B21F4C2 +:102B10000041C7F8001BD5F81C1821EA0303C0E7E1 +:102B200004F13F0305EB83030A4A5A602846214615 +:102B3000FFF7A2FE05EB4910D0F8003923F400435B +:102B4000C0F80039D5F81C3823EA0707D7E700BFDB +:102B50000080001000040002826D1268C265FFF759 +:102B60005FBE00005831436D49015B5813F40040CB +:102B700004D013F4001F0CBF022001207047000096 +:102B80004831436D49015B5813F4004004D013F4FD +:102B9000001F0CBF022001207047000000EB8101E4 +:102BA000CB68196A0B6813604B685360704700006C +:102BB00000EB810330B5DD68AA691368D36019B9E9 +:102BC000402B84BF402313606B8A1468426D1C4401 +:102BD000013CB4FBF3F46343033323F0030302EB40 +:102BE000411043EAC44343F0C043C0F8103B2B6894 +:102BF00003F00303012B09B20ED1D2F8083802EB1F +:102C0000411013F4807FD0F8003B14BF43F0805391 +:102C100043F00053C0F8003B02EB4112D2F8003BF6 +:102C200043F00443C2F8003B30BD00002DE9F04101 +:102C3000244D6E6D06EB40130446D3F8087BC3F8B1 +:102C4000087B38070AD5D6F81438190706D505EBDE +:102C500084032146DB6828465B689847FA071FD53E +:102C6000D6F81438DB071BD505EB8403D968CCB93B +:102C70008B69488A5A68B2FBF0F600FB16228AB9C3 +:102C80001868DA6890420DD2121AC3E90024302382 +:102C900083F311880B482146FFF78AFF84F31188DC +:102CA000BDE8F081012303FA04F26B8923EA0203F1 +:102CB0006B81CB68002BF3D021460248BDE8F04180 +:102CC000184700BFF82F002000EB810370B5DD68C6 +:102CD000436D6C692668E6604A0156BB1A444FF49E +:102CE0000020C2F810092A6802F00302012A0AB281 +:102CF0000ED1D3F8080803EB421410F4807FD4F807 +:102D0000000914BF40F0805040F00050C4F80009A2 +:102D100003EB4212D2F8000940F00440C2F8000967 +:102D2000D3F83408012202FA01F10143C3F8341840 +:102D300070BD19B9402E84BF4020206020682E8AC3 +:102D40008419013CB4FBF6F440EAC44040F0005062 +:102D50001A44C6E72DE9F8433B4D6E6D06EB401370 +:102D60000446D3F80889C3F8088918F0010F4FEA20 +:102D700040171AD0D6F81038DB0716D505EB8003BC +:102D8000D9684B691868DA68904230D2121A4FF04D +:102D900000091A60C3F80490302383F31188214698 +:102DA0002846FFF791FF89F3118818F0800F1CD097 +:102DB000D6F834380126A640334216D005EB8403FA +:102DC0006D6DD3F80CC0DCF814200134E4B2D2F8F5 +:102DD00000E005EB04342F445168714515D3D5F854 +:102DE000343823EA0606C5F83468BDE8F8830123C1 +:102DF00003FA04F22B8923EA02032B818B68002B50 +:102E0000D3D0214628469847CFE7BCF81000AEEB58 +:102E10000103834228BF0346D7F8180980B2B3EBF9 +:102E2000800FE2D89068A0F1040959F8048FC4F823 +:102E30000080A0EB09089844B8F1040FF5D81844B5 +:102E40000B4490605360C7E7F82F00202DE9F74F3F +:102E5000A24C656D6E69AB691E4016F480586E61B8 +:102E600007D02046FEF728FD03B0BDE8F04F00F084 +:102E700047BC002E12DAD5F8003E98489B071EBFCB +:102E8000D5F8003E23F00303C5F8003ED5F804381A +:102E900023F00103C5F80438FEF738FD370505D5E2 +:102EA0008E48FFF7BDFC8D48FEF71EFDB0040CD523 +:102EB000D5F8083813F0060FEB6823F470530CBFF5 +:102EC00043F4105343F4A053EB6031071BD5636800 +:102ED000DB681BB9AB6923F00803AB612378052BD2 +:102EE0000CD1D5F8003E7D489A071EBFD5F8003EAC +:102EF00023F00303C5F8003EFEF708FD6368DB68B6 +:102F00000BB176489847F30274D4B70227D5D4F8AA +:102F10005490DFF8C8B100274FF0010A09EB4712BF +:102F2000D2F8003B03F44023B3F5802F11D1D2F83F +:102F3000003B002B0DDA62890AFA07F322EA030349 +:102F4000638104EB8703DB68DB6813B139465846BD +:102F50009847A36D01379B68FFB29F42DED9F00608 +:102F600017D5676D3A6AC2F30A1002F00F0302F434 +:102F7000F012B2F5802F00F08580B2F5402F08D115 +:102F800004EB83030022DB681B6A07F5805790423D +:102F90006AD13003D5F8184813D5E10302D50020D3 +:102FA000FFF744FEA20302D50120FFF73FFE6303B3 +:102FB00002D50220FFF73AFE270302D50320FFF7D0 +:102FC00035FE75037FF550AFE00702D50020FFF70F +:102FD000C1FEA10702D50120FFF7BCFE620702D5A2 +:102FE0000220FFF7B7FE23077FF53EAF0320FFF770 +:102FF000B1FE39E7636DDFF8E4A0019300274FF0DD +:103000000109A36D9B685FFA87FB9B453FF67DAF87 +:10301000019B03EB4B13D3F8001901F44021B1F5E8 +:10302000802F1FD1D3F8001900291BDAD3F800191B +:1030300041F09041C3F80019D3F800190029FBDBD7 +:103040005946606DFFF718FC218909FA0BF321EA54 +:103050000303238104EB8B03DB689B6813B15946A0 +:10306000504698470137CCE7910708BFD7F8008052 +:10307000072A98BF03F8018B02F1010298BF4FEABB +:10308000182884E7023304EB830207F5805752685F +:10309000D2F818C0DCF80820DCE9001CA1EB0C0C0D +:1030A000002188420AD104EB830463689B699A6813 +:1030B00002449A605A6802445A606AE711F0030FAA +:1030C00008BFD7F800808C4588BF02F8018B01F15A +:1030D000010188BF4FEA1828E3E700BFF82F00205E +:1030E000436D03EB4111D1F8003B43F40013C1F8E9 +:1030F000003B7047436D03EB4111D1F8003943F4B5 +:103100000013C1F800397047436D03EB4111D1F84A +:10311000003B23F40013C1F8003B7047436D03EB01 +:103120004111D1F8003923F40013C1F80039704778 +:1031300000F1604303F561430901C9B283F800134C +:10314000012200F01F039A4043099B0003F16043F2 +:1031500003F56143C3F880211A60704730B5039CC2 +:103160000172043304FB0325C0E90653049B036387 +:103170000021059BC160C0E90000C0E90422C0E94C +:103180000842C0E90A11436330BD0000416A0022D1 +:10319000C0E90411C0E90A22C2606FF00101FEF724 +:1031A000E5BE0000D0E90432934201D1C2680AB9F9 +:1031B000181D70470020704703691960C26801320A +:1031C000C260C269134482690361934224BF436AA7 +:1031D00003610021FEF7BEBE38B504460D46E36824 +:1031E0003BB16269131D1268A3621344E3620020BD +:1031F00007E0237A33B929462046FEF79BFE0028D4 +:10320000EDDA38BD6FF00100FBE70000C368C2696A +:10321000013BC3604369134482694361934224BF05 +:10322000436A436100238362036B03B1184770470D +:1032300070B53023044683F31188866A3EB9FFF7E0 +:10324000CBFF054618B186F31188284670BDA36AE6 +:10325000E26A13F8015BA362934202D32046FFF7B0 +:10326000D5FF002383F31188EFE700002DE9F84F25 +:1032700004460E46174698464FF0300989F31188E8 +:103280000025AA46D4F828B0BBF1000F09D1414669 +:103290002046FFF7A1FF20B18BF311882846BDE837 +:1032A000F88FD4E90A12A7EB050B521A934528BFF1 +:1032B0009346BBF1400F1BD9334601F1400251F850 +:1032C000040B43F8040B9142F9D1A36A4033403612 +:1032D000A3624035D4E90A239A4202D32046FFF77D +:1032E00095FF8AF31188BD42D8D289F31188C9E7C6 +:1032F00030465A46FDF7EAFCA36A5B445E44A3628B +:103300005D44E7E710B5029C0172043303FB04211E +:10331000C0E906130023C0E90A33039B0363049B3F +:10332000C460C0E90000C0E90422C0E90842436368 +:1033300010BD0000026AC260426AC0E90422002295 +:10334000C0E90A226FF00101FEF710BED0E90423A4 +:103350009A4201D1C26822B9184650F8043B0B606A +:10336000704700231846FAE7C368C2690133C36097 +:103370004369134482694361934224BF436A4361B2 +:103380000021FEF7E7BD000038B504460D46E368AE +:103390003BB123691A1DA262E2691344E362002073 +:1033A00007E0237A33B929462046FEF7C3FD0028FB +:1033B000EDDA38BD6FF00100FBE70000036919602A +:1033C000C268013AC260C2691344826903619342D0 +:1033D00024BF436A036100238362036B03B1184770 +:1033E0007047000070B530230D460446114683F344 +:1033F0001188866A2EB9FFF7C7FF10B186F31188CE +:1034000070BDA36A1D70A36AE26A01339342A3628E +:1034100004D3E16920460439FFF7D0FF002080F390 +:103420001188EDE72DE9F84F04460D469046994680 +:103430004FF0300A8AF311880026B346A76A4FB9C5 +:1034400049462046FFF7A0FF20B187F31188304698 +:10345000BDE8F88FD4E90A073A1AA8EB06079742A5 +:1034600028BF1746402F1BD905F1400355F8042B00 +:1034700040F8042B9D42F9D1A36A4033A362403641 +:10348000D4E90A239A4204D3E16920460439FFF7BC +:1034900095FF8BF311884645D9D28AF31188CDE781 +:1034A00029463A46FDF712FCA36A3B443D44A36219 +:1034B0003E44E5E7D0E904239A4217D1C3689BB1A3 +:1034C000836A8BB1043B9B1A0ED01360C368013B27 +:1034D000C360C3691A44836902619A4224BF436A84 +:1034E0000361002383620123184670470023FBE732 +:1034F00000F030B94FF08043586A70474FF0804376 +:10350000002258631A610222DA6070474FF080434C +:103510000022DA60704700004FF080435863704724 +:10352000FEE7000070B51B4B01630025044686B022 +:10353000586085620E4600F0BFF804F11003C4E93C +:1035400004334FF0FF33C4E90635C4E90044A560F5 +:10355000E562FFF7CFFF2B460246C4E9082304F1DA +:1035600034010D4A256580232046FEF799FC01238E +:10357000E0600A4A0375009272680192B268CDE970 +:103580000223074B6846CDE90435FEF7B1FC06B0CF +:1035900070BD00BF90280020043E0008093E0008CE +:1035A00021350008024AD36A1843D062704700BF31 +:1035B000282600204B6843608B688360CB68C3601B +:1035C0000B6943614B6903628B6943620B6803605B +:1035D0007047000008B5264B26481A6940F2FF11D3 +:1035E0000A431A611A6922F4FF7222F001021A6179 +:1035F0001A691A6B0A431A631A6D0A431A651E4A3E +:103600001B6D1146FFF7D6FF02F11C0100F580602B +:10361000FFF7D0FF02F1380100F58060FFF7CAFF25 +:1036200002F1540100F58060FFF7C4FF02F1700160 +:1036300000F58060FFF7BEFF02F18C0100F58060AD +:10364000FFF7B8FF02F1A80100F58060FFF7B2FFB5 +:1036500002F1C40100F58060FFF7ACFF02F1E00168 +:1036600000F58060FFF7A6FFBDE8084000F0EEB867 +:103670000038024000000240103E000808B500F08B +:1036800067FAFEF7D1FBFFF791F8BDE80840FEF7B7 +:1036900059BE000070470000EFF3098305494A6BEB +:1036A00022F001024A63683383F30988002383F31D +:1036B0001188704700EF00E0302080F3118862B677 +:1036C0000C4B0D4AD96821F4E0610904090C0A4346 +:1036D000DA60D3F8FC20094942F08072C3F8FC207C +:1036E0000A6842F001020A602022DA7783F8220099 +:1036F000704700BF00ED00E00003FA05001000E095 +:1037000010B5302383F311880E4B5B6813F400630C +:1037100014D0F1EE103AEFF30984683C4FF0807357 +:10372000E361094BDB6B236684F30988FEF750FBEA +:1037300010B1064BA36110BD054BFBE783F3118865 +:10374000F9E700BF00ED00E000EF00E03303000800 +:10375000360300080F4B1A6C42F001021A641A6E0D +:1037600042F001021A660C4A1B6E936843F0010393 +:1037700093604FF0804353229A624FF0FF32DA6237 +:1037800000229A615A63DA605A6001225A611A6013 +:10379000704700BF00380240002004E04FF0804234 +:1037A00008B51169D3680B40D9B2C9439B071161B1 +:1037B00007D5302383F31188FEF74CFB002383F3F6 +:1037C000118808BD1F4B1A696FEAC2526FEAD252C4 +:1037D0001A611A69C2F308021A614FF0FF301A69C0 +:1037E0005A69586100215A6959615A691A6A62F026 +:1037F00080521A621A6A02F080521A621A6A5A6A6F +:1038000058625A6A59625A6A1A6C42F080521A64B3 +:103810001A6E42F080521A661A6E0B4A106840F413 +:1038200080701060186F00F44070B0F5007F1EBF0C +:103830004FF4803018671967536823F4007353609E +:1038400000F05EB90038024000700040334B4FF08A +:1038500080521A64324A4FF4404111601A6842F0B3 +:1038600001021A601A689107FCD59A6822F00302D7 +:103870009A602A4B9A6812F00C02FBD1196801F089 +:10388000F90119609A601A6842F480321A601A6865 +:103890009203FCD55A6F42F001025A671F4B5A6FD0 +:1038A0009007FCD51F4A5A601A6842F080721A606D +:1038B0001B4A53685904FCD5184B1A689201FCD571 +:1038C000194A9A60194B1A68194B9A42194B21D11F +:1038D000194A1168194A91421CD140F205121A6026 +:1038E000144A136803F00F03052BFAD10B4B9A68A7 +:1038F00042F002029A609A6802F00C02082AFAD199 +:103900005A6C42F480425A645A6E42F480425A66BB +:103910005B6E704740F20572E1E700BF003802407D +:10392000007000400854400700948838002004E0EC +:1039300011640020003C024000ED00E041C20F4154 +:10394000074A08B5536903F00103536123B1054ADF +:1039500013680BB150689847BDE80840FFF7D0BE28 +:10396000003C014070300020074A08B5536903F05D +:103970000203536123B1054A93680BB1D06898479D +:10398000BDE80840FFF7BCBE003C0140703000209D +:10399000074A08B5536903F00403536123B1054A8C +:1039A00013690BB150699847BDE80840FFF7A8BEFE +:1039B000003C014070300020074A08B5536903F00D +:1039C0000803536123B1054A93690BB1D069984745 +:1039D000BDE80840FFF794BE003C01407030002075 +:1039E000074A08B5536903F01003536123B1054A30 +:1039F000136A0BB1506A9847BDE80840FFF780BED4 +:103A0000003C014070300020164B10B55C6904F496 +:103A100078725A61A30604D5134A936A0BB1D06A2F +:103A20009847600604D5104A136B0BB1506B98474A +:103A3000210604D50C4A936B0BB1D06B9847E20575 +:103A400004D5094A136C0BB1506C9847A30504D5F3 +:103A5000054A936C0BB1D06C9847BDE81040FFF756 +:103A60004FBE00BF003C014070300020194B10B524 +:103A70005C6904F47C425A61620504D5164A136DF0 +:103A80000BB1506D9847230504D5134A936D0BB1C4 +:103A9000D06D9847E00404D50F4A136E0BB1506EF9 +:103AA0009847A10404D50C4A936E0BB1D06E984789 +:103AB000620404D5084A136F0BB1506F9847230472 +:103AC00004D5054A936F0BB1D06F9847BDE81040FD +:103AD000FFF716BE003C01407030002008B50348D7 +:103AE000FDF784FABDE80840FFF70ABEBC230020BA +:103AF00008B5FFF753FEBDE80840FFF701BE000020 +:103B0000062108B50846FFF713FB06210720FFF73B +:103B10000FFB06210820FFF70BFB06210920FFF70A +:103B200007FB06210A20FFF703FB06211720FFF7FA +:103B3000FFFA06212820FFF7FBFA07211C20FFF7D8 +:103B4000F7FABDE808400C212520FFF7F1BA000084 +:103B500008B5FFF737FE00F00DF8FDF75BFCFDF749 +:103B600041FEFDF719FDFFF795FDBDE80840FFF7A1 +:103B7000BFBC00000023054A19460133102BC2E9DF +:103B8000001102F10802F8D1704700BF7030002028 +:103B9000034611F8012B03F8012B002AF9D17047D5 +:103BA00053544D3332463F3F3F00000053544D3392 +:103BB00032463430780053544D3332463432780034 +:103BC00053544D33324634343658580001203300B4 +:103BD0000010410001105A00031059000710310075 +:103BE00000000000A03B00083F000000130400009C +:103BF000AC3B00083F00000019040000B63B000881 +:103C00003F00000021040000C03B00083F0000000E +:103C1000009600000000000000000000000000000E +:103C20000000000000000000D5130008C1130008C8 +:103C3000FD130008E9130008F5130008E11300085C +:103C4000CD130008B9130008091400080000000093 +:103C500015150008011500083D1500082915000874 +:103C600035150008211500080D150008F914000885 +:103C700049150008000000000100000000000000DD +:103C800063300000803C000880260020902800203F +:103C90004172647550696C6F740025424F41524403 +:103CA000252D424C002553455249414C250000002A +:103CB0000200000000000000311700089D170008F6 +:103CC00040004000302D0020402D00200200000068 +:103CD000000000000300000000000000E1170008E1 +:103CE0000000000010000000502D00200000000027 +:103CF0000100000000000000F82F00200101020078 +:103D0000E1220008F12100088D220008712200083C +:103D100043000000183D000809024300020100C0F2 +:103D2000320904000001020201000524001001050F +:103D30002401000104240202052406000107058273 +:103D4000030800FF09040100020A00000007050142 +:103D50000240000007058102400000001200000040 +:103D6000643D000812011001020000400912415791 +:103D700000020102030100000403090425424F412F +:103D8000524425004D6174656B463430352D4341F6 +:103D90004E003031323334353637383941424344BE +:103DA00045460000004000000040000000400000C8 +:103DB00000400000000001000000020000000200BE +:103DC00000000200000002000000020000000200EB +:103DD000000002000000000025190008DD1B00089B +:103DE000891C000840004000583000205830002056 +:103DF0000100000068300020800000004001000049 +:103E0000030000006D61696E0069646C650000006C +:103E10000001A81600000000AAAAAAAA0001140026 +:103E2000FF9F00000000000070A70A0000000001D2 +:103E300000000000AAAAAAAA00000001FFFF0000DB +:103E40000000000000000000000000100000000062 +:103E5000AAAAAAAA00000010FFFF000000000000AC +:103E6000000000000000000000000000AAAAAAAAAA +:103E700000000000FFFF0000000000000000000044 +:103E80000000000000000000AAAAAAAA000000008A +:103E9000FFFF000000000000000000000000000024 +:103EA00000000000AAAAAAAA00000000FFFF00006C +:103EB0000000000000000000000000000000000002 +:103EC000AAAAAAAA00000000FFFF0000000000004C +:103ED0000000000000000000000000000A000000D8 +:103EE00000000000030000000000000000000000CF +:103EF00000000000000000000000000000000000C2 +:103F000000000000000000000000000000000000B1 +:103F1000F60300000000000000000F000000000099 +:103F2000FF00000098280020BC230020009600001D +:103F300000000800960000000008000004000000D7 +:103F4000783D0008000000000000000000000000B4 +:0C3F500000000000000000000000000065 :00000001FF diff --git a/Tools/bootloaders/MatekF405-TE-bdshot_bl.bin b/Tools/bootloaders/MatekF405-TE-bdshot_bl.bin new file mode 100644 index 0000000000000..38f871f537475 Binary files /dev/null and b/Tools/bootloaders/MatekF405-TE-bdshot_bl.bin differ diff --git a/Tools/bootloaders/MatekF405-TE-bdshot_bl.hex b/Tools/bootloaders/MatekF405-TE-bdshot_bl.hex new file mode 100644 index 0000000000000..af75440bf6062 --- /dev/null +++ b/Tools/bootloaders/MatekF405-TE-bdshot_bl.hex @@ -0,0 +1,998 @@ +:020000040800F2 +:1000000000060020E1010008D50E00088D0E000852 +:10001000B50E00088D0E0008AD0E0008E3010008C3 +:10002000E3010008E3010008E30100089527000848 +:10003000E3010008E3010008E3010008E301000810 +:10004000E3010008E3010008E3010008E301000800 +:10005000E3010008E3010008313800085D380008BA +:1000600089380008B5380008E1380008E3010008C5 +:10007000E3010008E3010008E3010008E3010008D0 +:10008000E3010008E3010008E3010008E3010008C0 +:10009000E3010008E3010008E30100080D3900084E +:1000A000E3010008E3010008E3010008E3010008A0 +:1000B000A5360008E3010008E3010008E301000899 +:1000C000E3010008E3010008E3010008E301000880 +:1000D000E30100082D0F0008E3010008E301000818 +:1000E00075390008E3010008E3010008E301000896 +:1000F000E3010008E3010008E3010008E301000850 +:10010000E3010008E3010008E3010008E30100083F +:10011000E3010008E3010008E3010008E30100082F +:10012000E3010008E3010008E3010008E30100081F +:10013000E3010008E3010008E3010008E30100080F +:10014000E3010008E3010008E3010008CD2E0008E8 +:10015000E3010008E3010008E3010008E3010008EF +:10016000E3010008E3010008E3010008E3010008DF +:10017000E3010008E3010008E3010008E3010008CF +:10018000E3010008E3010008E3010008E3010008BF +:10019000E3010008E3010008E3010008E3010008AF +:1001A000E3010008E3010008E3010008E30100089F +:1001B000E3010008E3010008E3010008E30100088F +:1001C000E3010008E3010008E3010008E30100087F +:1001D000E3010008E3010008E3010008E30100086F +:1001E00002E000F000F8FEE772B63A4880F30888B3 +:1001F000394880F3098839484EF60851CEF200019B +:10020000086040F20000CCF200004EF63471CEF2ED +:1002100000010860BFF34F8FBFF36F8F40F2000003 +:10022000C0F2F0004EF68851CEF200010860BFF334 +:100230004F8FBFF36F8F4FF00000E1EE100A4EF6C4 +:100240003C71CEF200010860062080F31488BFF3F1 +:100250006F8F02F01FFB02F0FBFA03F087F94FF0FB +:1002600055301F491B4A91423CBF41F8040BFAE745 +:100270001C49194A91423CBF41F8040BFAE71A495C +:100280001A4A1B4B9A423EBF51F8040B42F8040B2A +:10029000F8E700201749184A91423CBF41F8040B87 +:1002A000FAE702F0D9FA03F0B5F9144C144DAC4258 +:1002B00003DA54F8041B8847F9E700F041F8114CC1 +:1002C000114DAC4203DA54F8041B8847F9E702F0F9 +:1002D000C1BA000000060020002200200000000833 +:1002E0000000002000060020F03D00080022002051 +:1002F0004C2200205022002030300020E001000875 +:10030000E0010008E0010008E00100082DE9F04FDD +:100310002DED108AC1F80CD0C3689D46BDEC108A43 +:10032000BDE8F08F002383F311882846A047002002 +:1003300001F03CFEFEE701F0B7FD00DFFEE7000044 +:1003400038B502F0F7F9054602F01AFA0446D8B9B2 +:100350000F4B9D421AD001339D4218BF044641F213 +:10036000883504BF01240025002002F0EDF90CB10E +:1003700000F076F800F028FD00F0BAFB284600F007 +:10038000C3F800F06DF8F9E70025EDE70546EBE767 +:10039000010007B008B500F077FBA0F12003584238 +:1003A000584108BD07B541F21203022101A8ADF87A +:1003B000043000F087FB03B05DF804FB38B5202360 +:1003C00083F311881748C3680BB101F075FE164A14 +:1003D000144800234FF47A7101F032FE002383F3B6 +:1003E0001188124C236813B12368013B23606368B2 +:1003F00013B16368013B63600D4D2B7833B96368BB +:100400007BB9022000F052FC322363602B78032B6F +:1004100007D163682BB9022000F048FC4FF47A73CF +:10042000636038BD50220020BD0300086C2300200B +:1004300064220020084B187003280CD8DFE800F075 +:1004400008050208022000F021BC022000F00EBCCA +:10045000024B00225A607047642200206C23002067 +:10046000244B254A10B51C461968013140D004338D +:100470009342F9D16268214B9A4239D9204B9B684B +:1004800003F1006303F580339A4231D2002000F07B +:1004900049FB0220FFF7CEFF1A4B1A6C00221A64A8 +:1004A000196E1A66196E596C5A64596E5A665A6EEC +:1004B0005A6942F080025A615A6922F080025A61F8 +:1004C0005A691A6942F000521A611A6922F0005200 +:1004D0001A611B6972B64FF0E0232021C3F8084D62 +:1004E000D4E9003281F311889D4683F308881047D0 +:1004F00010BD00BF0000010820000108FFFF000838 +:1005000000220020003802402DE9F04F93B0AA4BA2 +:1005100000902022FF210AA89D6800F0FFFBA74A57 +:100520001378A3B9A6480121C3601170202383F377 +:100530001188C3680BB101F0BFFDA24AA048002397 +:100540004FF47A7101F07CFD002383F31188009B46 +:1005500013B19D4B009A1A609C4A009C1378032BA0 +:100560001EBF00231370984A4FF0000A18BF536053 +:10057000D3465646D146012000F088FB24B1924B69 +:100580001B68002B00F01182002000F07DFA039020 +:10059000039B002BF2DB012000F068FB039B213B57 +:1005A000162BE8D801A252F823F000BF0906000874 +:1005B00031060008C5060008770500087705000821 +:1005C000770500084F0700081F09000839080008D0 +:1005D0009B080008C3080008E90800087705000820 +:1005E000FB080008770500086D090008A906000847 +:1005F00077050008B109000815060008A9060008DB +:10060000770500089B0800080220FFF7C3FE0028BA +:1006100040F0F581009B0221BAF1000F08BF1C4693 +:1006200005A841F21233ADF8143000F04BFAA2E7FE +:100630004FF47A7000F028FA071EEBDB0220FFF778 +:10064000A9FE0028E6D0013F052F00F2DA81DFE89D +:1006500007F0030A0D10133605230593042105A89E +:1006600000F030FA17E054480421F9E75848042113 +:10067000F6E758480421F3E74FF01C08404600F025 +:1006800053FA08F104080590042105A800F01AFAAD +:10069000B8F12C0FF2D1012000FA07F747EA0B0B53 +:1006A0005FFA8BFB4FF0000900F084FB26B10BF0E2 +:1006B0000B030B2B08BF0024FFF774FE5BE74648D3 +:1006C0000421CDE7002EA5D00BF00B030B2BA1D1FD +:1006D0000220FFF75FFE074600289BD0012000F0B4 +:1006E00021FA0220FFF7A6FE00265FFA86F84046B0 +:1006F00000F028FA044690B10021404600F032FA9A +:1007000001360028F1D1BA46044641F21213022103 +:1007100005A8ADF814303E4600F0D4F92BE70120CF +:10072000FFF788FE2546244B9B68AB4207D9284635 +:1007300000F0FAF9013040F067810435F3E7234B0C +:1007400000251D70204BBA465D603E46ACE7002E8A +:100750003FF460AF0BF00B030B2B7FF45BAF022079 +:10076000FFF768FE322000F08FF9B0F10008FFF6C5 +:1007700051AF18F003077FF44DAF0F4A926808EBB2 +:10078000050393423FF646AFB8F5807F3FF742AF8F +:10079000124B0193B84523DD4FF47A7000F074F9E1 +:1007A0000390039A002AFFF635AF019B039A03F8E2 +:1007B000012B0137EDE700BF002200206823002055 +:1007C00050220020BD0300086C230020642200207A +:1007D00004220020082200200C2200206822002091 +:1007E000C820FFF7D7FD074600283FF413AF1F2DA1 +:1007F00011D8C5F1200242450AAB25F0030028BFFD +:10080000424683490192184400F062FA019A8048F6 +:10081000FF2100F083FA4FEAA8037D490193C8F352 +:100820008702284600F082FA064600283FF46DAFA2 +:10083000019B05EB830537E70220FFF7ABFD00289E +:100840003FF4E8AE00F0BAF900283FF4E3AE002729 +:10085000B846704B9B68BB4218D91F2F11D80A9B12 +:1008600001330ED027F0030312AA134453F8203C9F +:1008700005934046042205A900F02EFB043780466C +:10088000E7E7384600F050F90590F2E7CDF814801C +:10089000042105A800F016F906E70023642104A846 +:1008A000049300F005F900287FF4B4AE0220FFF7AE +:1008B00071FD00283FF4AEAE049800F067F9059092 +:1008C000E6E70023642104A8049300F0F1F800286F +:1008D0007FF4A0AE0220FFF75DFD00283FF49AAE42 +:1008E000049800F063F9EAE70220FFF753FD0028BF +:1008F0003FF490AE00F072F9E1E70220FFF74AFD05 +:1009000000283FF487AE05A9142000F06DF90421FA +:100910000746049004A800F0D5F83946B9E732201C +:1009200000F0B2F8071EFFF675AEBB077FF472AE9B +:10093000384A926807EB090393423FF66BAE0220F8 +:10094000FFF728FD00283FF465AE27F003074F446A +:10095000B9453FF4A9AE484600F0E6F804210590F9 +:1009600005A800F0AFF809F10409F1E74FF47A7037 +:10097000FFF710FD00283FF44DAE00F01FF90028EE +:1009800044D00A9B01330BD008220AA9002000F0B2 +:10099000CDF900283AD02022FF210AA800F0BEF9A4 +:1009A000FFF700FD1C4801F007FB13B0BDE8F08F16 +:1009B000002E3FF42FAE0BF00B030B2B7FF42AAE6F +:1009C0000023642105A8059300F072F8074600286B +:1009D0007FF420AE0220FFF7DDFC804600283FF4C4 +:1009E00019AEFFF7DFFC41F2883001F0E5FA059817 +:1009F00000F02AFA464600F0DDF93C46BBE5064623 +:100A000052E64FF0000905E6BA467EE637467CE638 +:100A10006822002000220020A08601002DE9F84F66 +:100A20004FF47A73DFF85880DFF8589006460D4689 +:100A300002FB03F7002498F900305A1C5FFA84FA8D +:100A400001D0A34212D159F8240003682A46D3F8F2 +:100A500020B031463B46D847854207D1074B01209D +:100A600083F800A0BDE8F88F0124E4E7002CFBD058 +:100A70004FF4FA7001F0A0FA0020F3E7B42300204D +:100A8000102200201422002007B500230246012175 +:100A90000DF107008DF80730FFF7C0FF20B19DF87A +:100AA000070003B05DF804FB4FF0FF30F9E70000EA +:100AB0000A4608B50421FFF7B1FF80F00100C0B27B +:100AC000404208BD30B4074B0A461978064B53F82C +:100AD00021402368DD69054B0146AC46204630BC09 +:100AE000604700BFB423002014220020A08601002C +:100AF00070B501F011FD094E094D308000242868C1 +:100B00003388834208D901F001FD2B680444013386 +:100B1000B4F5803F2B60F2D370BD00BFB623002038 +:100B20007423002001F0BABD00F1006000F58030B0 +:100B30000068704700F10060920000F5803001F01D +:100B400039BD0000054B1A68054B1B889B1A834270 +:100B500002D9104401F0DABC002070477423002051 +:100B6000B623002038B5084D044629B12868204432 +:100B7000BDE8384001F0EABC2868204401F0CEFC12 +:100B80000028F3D038BD00BF7423002010F0030309 +:100B90000AD1B0F5047F05D200F10050A0F5104055 +:100BA000D0F80038184670470023FBE700F10050EA +:100BB000A0F51040D0F8100A70470000064991F8DF +:100BC000243033B10023086A81F824300822FFF76B +:100BD000B1BF0120704700BF78230020014B186887 +:100BE000704700BF002004E0F0B51C4B02461868B7 +:100BF0001B4BC0F30B050424000C1E88AE421BD116 +:100C00005C6893F90850174B05261F88874208BF78 +:100C100093F90250013E03F10403F6D1013A0A446C +:100C2000013C0B4693420ED214F9016F581C66B179 +:100C300000F8016C0346F5E7013C03F10C03DCD13D +:100C4000094C3F25DFE7184605E02C2490421C7034 +:100C500001D2981C5D70401AF0BD00BF002004E076 +:100C6000B83A0008A43A0008783A0008022804D1EB +:100C7000054B4FF080429A6170470128FCD1024B2E +:100C80004FF00052F7E700BF00000240022804D1F5 +:100C9000054B4FF480429A6170470128FCD1024B0A +:100CA0004FF40052F7E700BF00000240022805D1D0 +:100CB000064A536983F48043536170470128FCD18D +:100CC000024A536983F40053F6E700BF0000024074 +:100CD00010B50023934203D0CC5CC4540133F9E730 +:100CE00010BD000010B5013810F9013F3BB191F97A +:100CF00000409C4203D11AB10131013AF4E71AB124 +:100D000091F90020981A10BD1046FCE703460246F0 +:100D1000D01A12F9011B0029FAD170470244034688 +:100D2000934202D003F8011BFAE770472DE9F8431C +:100D30001F4D144695F824200746884652BBDFF81D +:100D400070909CB395F824302BB92022FF2148469F +:100D50002F62FFF7E3FF95F82400C0F10802A242DA +:100D600028BF2246D6B24146920005EB8000FFF72D +:100D7000AFFF95F82430A41B1E44F6B2082E17448A +:100D80009044E4B285F82460DBD1FFF717FF002818 +:100D9000D7D108E02B6A03EB82038342CFD0FFF761 +:100DA0000DFF0028CBD10020BDE8F8830120FBE730 +:100DB00078230020024B1A78024B1A70704700BF4C +:100DC000B42300201022002038B5194C194D2046BC +:100DD00000F008FC2946204600F030FC2D6816483B +:100DE0006A6DD2F8043843F00203C2F8043801F007 +:100DF000E3F82846114900F027FD6A6D104DD2F83E +:100E0000043828680F4923F00203C2F80438A042CE +:100E10004FF4E1330B6001D000F044FB6868A0425E +:100E200004D0BDE83840074900F03CBB38BD00BFE6 +:100E3000D8270020E83B000840420F00F03B0008A4 +:100E400014220020A023002070B50C4B0C4D1E78FE +:100E50000C4B55F826209A4204460DD00A4B00212F +:100E600014221846FFF75AFF0460014655F8260081 +:100E7000BDE8704000F016BB70BD00BFB423002079 +:100E800014220020D8270020A023002000B59BB00A +:100E9000EFF3098168226846FFF71AFFEFF3058335 +:100EA000014B9B6BFEE700BF00ED00E008B5FFF7CC +:100EB000EDFF000000B59BB0EFF3098168226846A2 +:100EC000FFF706FFEFF30583014B5B6BFEE700BF07 +:100ED00000ED00E0FEE7000030B5094D0A44914204 +:100EE0000DD011F8013B5840082340F30004013BAA +:100EF0002C4013F0FF0384EA5000F6D1EFE730BD39 +:100F00002083B8ED024AD36843F0C003D360704732 +:100F100000100140044B5A6810439A6858600AB1A7 +:100F2000181D1047704700BFB82300202DE9F0417D +:100F30003C4ED6F85C52EF682B68DA059CB20CD5B3 +:100F4000202383F311884FF40070FFF7E3FF6FF461 +:100F500080732B60002383F31188202383F311888F +:100F6000DFF8C08014F02F0339D183F311883806DD +:100F700013D5210611D5202383F311882A4800F0C8 +:100F8000EFF900284CDA0820FFF7C4FF4FF67F7313 +:100F90003B40EB60002383F311887A0615D5630686 +:100FA00013D5202383F31188D6E913239A4209D15C +:100FB000336C3BB127F040073F0410203F0CFFF794 +:100FC000A9FFEF60002383F31188D6F86422D36869 +:100FD0000BB110699847BDE8F04101F00FBC230741 +:100FE00012D014F0080F0CBF00208020E10748BF8A +:100FF00040F02000A20748BF40F04000630748BF10 +:1010000040F48070FFF786FFA4066B6805D596F85C +:1010100060124046194000F053FA2C68A4B2A1E7D0 +:101020006860B7E7B8230020F023002010B5054C16 +:10103000054A0021204600F013FA044BC4F85C3244 +:1010400010BD00BFB8230020050F000800100140AC +:1010500038B52A4C037C002918BF0C46012B0546E5 +:10106000C0F8644210D1264B98420DD1254B5A6CE2 +:1010700042F010025A645A6E42F010025A660B2176 +:1010800025205B6E00F046F8D5F85C221E492368E7 +:101090008A424FEA530003D001F580618A422AD187 +:1010A0001A490144B4F90400B1FBF3F30028BEBFB0 +:1010B00023F0070003F0070343EA4003A18893608D +:1010C000E38843F040031361238943F00103536134 +:1010D00041F4045343F02C03D36001F4A0510023E6 +:1010E0001360B1F5806F136853680CBF7F23FF2333 +:1010F00085F8603238BD0649D3E700BFE83A0008FA +:10110000B8230020003802400010014000BD010556 +:1011100080DE800200F1604303F561430901C9B23A +:1011200083F80013012200F01F039A4043099B003B +:1011300003F1604303F56143C3F880211A607047EF +:10114000F8B5154682680669AA420B46816938BF20 +:101150008568761AB54204460BD218462A46FFF730 +:10116000B7FDA3692B44A361A3685B1BA36028465A +:10117000F8BD0CD918463246FFF7AAFDAF1BE1684F +:101180003A463044FFF7A4FDE3683B44EBE71846DA +:101190002A46FFF79DFDE368E5E700008368934278 +:1011A000F7B51546044638BF8568D0E90460361A9D +:1011B000B5420BD22A46FFF78BFD63692B4463616E +:1011C000A36828465B1BA36003B0F0BD0DD932466F +:1011D0000191FFF77DFD0199E068AF1B3A4631446C +:1011E000FFF776FDE3683B44E9E72A46FFF770FD29 +:1011F000E368E4E710B50A440024C361029B8460FD +:10120000C0E90000C0E90511C1600261036210BDC0 +:1012100008B5D0E90532934201D1826882B982686B +:10122000013282605A1C42611970D0E904329A423C +:1012300024BFC3684361002100F0DAFE002008BD2E +:101240004FF0FF30FBE7000070B5202304460E4648 +:1012500083F31188A568A5B1A368A269013BA360C7 +:10126000531CA36115782269934224BFE368A361EC +:10127000E3690BB120469847002383F31188284681 +:1012800007E03146204600F0A3FE0028E2DA85F3AD +:10129000118870BD2DE9F74F04460E461746984653 +:1012A000D0F81C904FF0200A8AF311884FF0000B01 +:1012B000154665B12A4631462046FFF741FF0346F1 +:1012C00060B94146204600F083FE0028F1D000239B +:1012D00083F31188781B03B0BDE8F08FB9F1000FDC +:1012E00003D001902046C847019B8BF31188ED1A6B +:1012F0001E448AF31188DCE7C0E90511C160C361AF +:101300001144009B8260C0E90000016103627047E4 +:10131000F8B504460D461646202383F31188A768C6 +:10132000A7B1A368013BA36063695A1C62611D7089 +:10133000D4E904329A4224BFE3686361E3690BB1E4 +:1013400020469847002080F3118807E03146204668 +:1013500000F03EFE0028E2DA87F31188F8BD0000B5 +:10136000D0E905239A4210B501D182687AB9826822 +:10137000013282605A1C82611C7803699A4224BF40 +:10138000C3688361002100F033FE204610BD4FF09A +:10139000FF30FBE72DE9F74F04460E461746984607 +:1013A000D0F81C904FF0200A8AF311884FF0000B00 +:1013B000154665B12A4631462046FFF7EFFE034643 +:1013C00060B94146204600F003FE0028F1D000231A +:1013D00083F31188781B03B0BDE8F08FB9F1000FDB +:1013E00003D001902046C847019B8BF31188ED1A6A +:1013F0001E448AF31188DCE70268436811430160E8 +:1014000003B11847704700001430FFF743BF0000D6 +:101410004FF0FF331430FFF73DBF00003830FFF7C7 +:10142000B9BF00004FF0FF333830FFF7B3BF000003 +:101430001430FFF709BF00004FF0FF311430FFF701 +:1014400003BF00003830FFF763BF00004FF0FF32EA +:101450003830FFF75DBF000000207047FFF7E6BDA2 +:1014600037B515460E4A026000224260C0E90222EA +:101470000122044602740B46009000F15C014FF417 +:1014800080721430FFF7B6FE00942B464FF4807242 +:1014900004F5AE7104F13800FFF72EFF03B030BD44 +:1014A000FC3A000810B52023044683F31188FFF7A7 +:1014B000CFFD02232374002383F3118810BD0000A5 +:1014C00038B5C36904460D461BB904210844FFF72B +:1014D00093FF294604F11400FFF79AFE002806DA6C +:1014E000201D4FF48061BDE83840FFF785BF38BD4F +:1014F000026843681143016003B118477047000058 +:1015000013B5446BD4F894341A681178042915D1B2 +:10151000217C022912D11979128901238B401342AF +:101520000CD101A904F14C0001F026FFD4F8944439 +:10153000019B21790246206800F0CEF902B010BD6F +:10154000143001F0A9BE00004FF0FF33143001F059 +:10155000A3BE00004C3001F07BBF00004FF0FF3312 +:101560004C3001F075BF0000143001F077BE000070 +:101570004FF0FF31143001F071BE00004C3001F02B +:1015800047BF00004FF0FF324C3001F041BF000078 +:101590000020704710B5D0F894341A6811780429E7 +:1015A000044617D1017C022914D1597952890123AB +:1015B0008B4013420ED1143001F00AFE024648B1AE +:1015C000D4F894444FF4807361792068BDE81040EA +:1015D00000F070B910BD0000406BFFF7DBBF0000EA +:1015E000704700007FB5124B036000234360C0E9E1 +:1015F0000233012502260F4B057404460290019325 +:1016000000F18402294600964FF48073143001F0F3 +:10161000BBFD094B0294CDE9006304F523724FF43E +:101620008073294604F14C0001F082FE04B070BDC5 +:10163000243B0008D9150008011500080B6820227A +:1016400082F311880A7903EB820290614A790932A8 +:1016500043F822008A7912B103EB820398610223D6 +:101660000374C0F89414002383F3118870470000BA +:1016700038B5037F044613B190F85430ABB9201D40 +:1016800001250221FFF734FF04F1140025776FF0E4 +:10169000010100F0B9FC84F8545004F14C006FF0E3 +:1016A0000101BDE8384000F0AFBC38BD10B50121E4 +:1016B00004460430FFF71CFF0023237784F85430DE +:1016C00010BD000038B504460025143001F074FD4B +:1016D00004F14C00257701F043FE201D84F854509E +:1016E0000121FFF705FF2046BDE83840FFF752BF54 +:1016F00090F8443003F06003202B07D190F8452088 +:10170000212A4FF0000303D81F2A06D80020704773 +:10171000222AFBD1C0E90E3303E0034A8263072289 +:10172000C2630364012070471C22002037B5D0F843 +:1017300094341A681178042904461AD1017C0229CC +:1017400017D11979128901238B40134211D100F16D +:101750004C05284601F0C4FE58B101A9284601F005 +:101760000BFED4F89444019B21790246206800F0D6 +:10177000B3F803B030BD0000F0B500EB810385B0D5 +:101780009E6904460D46FEB1202383F3118804EBC5 +:101790008507301D0821FFF7ABFEFB685B691B68FE +:1017A00006F14C001BB1019001F0F4FD019803A972 +:1017B00001F0E2FD024648B1039B2946204600F0B5 +:1017C0008BF8002383F3118805B0F0BDFB685A69DC +:1017D0001268002AF5D01B8A013B1340F1D104F1B5 +:1017E0004402EAE7093138B550F82140DCB1202342 +:1017F00083F31188D4F894241368527903EB82039D +:10180000DB689B695D6845B104216018FFF770FED5 +:10181000294604F1140001F0E5FC2046FFF7BAFE6A +:10182000002383F3118838BD7047000001F0A8B889 +:10183000012303700023C0E90133C36183620362A3 +:10184000C36243620363704738B50446202383F3C1 +:1018500011880025C0E90355C0E90555416001F034 +:101860009FF80223237085F3118838BD70B500EB13 +:10187000810305465069DA600E46144618B11022FD +:101880000021FFF74BFAA06918B110220021FFF7E1 +:1018900045FA31462846BDE8704001F04BB90000DA +:1018A000826802F0011282600022C0E90422826193 +:1018B00001F0CCB9F0B400EB81044789E46801255C +:1018C000A4698D403D43458123600023A26063608D +:1018D000F0BC01F0E7B90000F0B400EB8104078927 +:1018E000E468012564698D403D4305812360002340 +:1018F000A2606360F0BC01F061BA000070B5022321 +:10190000002504460370C0E90255C0E90455C564CA +:10191000856180F8345001F0A9F863681B6823B131 +:1019200029462046BDE87040184770BD037880F80E +:1019300050300523037043681B6810B504460BB193 +:10194000042198470023A36010BD000090F85020A8 +:10195000436802701B680BB10521184770470000EF +:1019600070B590F83430044613B1002380F8343059 +:1019700004F14402204601F08BF963689B68B3B917 +:1019800094F8443013F0600535D00021204601F072 +:10199000DDFB0021204601F0CFFB63681B6813B11B +:1019A000062120469847062384F8343070BD20462F +:1019B00098470028E4D0B4F84A30E26B9A4288BFD6 +:1019C000E36394F94430E56B002B4FF0200380F281 +:1019D0000381002D00F0F280092284F8342083F383 +:1019E00011880021D4E90E232046FFF775FF00235C +:1019F00083F31188DAE794F8452003F07F0343EA84 +:101A0000022340F20232934200F0C58021D8B3F5A0 +:101A1000807F48D00DD8012B3FD0022B00F093805F +:101A2000002BB2D104F14C02A2630222E2632364D0 +:101A3000C1E7B3F5817F00F09B80B3F5407FA4D16F +:101A400094F84630012BA0D1B4F84C3043F0020397 +:101A500032E0B3F5006F4DD017D8B3F5A06F31D099 +:101A6000A3F5C063012B90D8636894F846205E68A4 +:101A700094F84710B4F848302046B047002884D086 +:101A80004368A3630368E3631AE0B3F5106F36D0CD +:101A900040F6024293427FF478AF5C4BA36302238B +:101AA000E3630023C3E794F84630012B7FF46DAF66 +:101AB000B4F84C3023F00203C4E90E55A4F84C30BE +:101AC000256478E7B4F84430B3F5A06F0ED194F8EC +:101AD000463084F84E30204601F020F863681B68D9 +:101AE00013B1012120469847032323700023C4E942 +:101AF0000E339CE704F14F03A3630123C3E723786C +:101B0000042B10D1202383F311882046FFF7C8FE51 +:101B100085F311880321636884F84F5021701B6896 +:101B20000BB12046984794F84630002BDED084F85D +:101B30004F300423237063681B68002BD6D002212A +:101B400020469847D2E794F848301D0603F00F016D +:101B500020460AD501F08EF8012804D002287FF42F +:101B600014AF2B4B9AE72B4B98E701F075F8F3E78E +:101B700094F84630002B7FF408AF94F8483013F007 +:101B80000F01B3D01A06204602D501F0F3FAADE7F3 +:101B900001F0E6FAAAE794F84630002B7FF4F5AEA0 +:101BA00094F8483013F00F01A0D01B06204602D550 +:101BB00001F0CCFA9AE701F0BFFA97E7142284F813 +:101BC000342083F311882B462A4629462046FFF706 +:101BD00071FE85F31188E9E65DB1152284F83420A1 +:101BE00083F311880021D4E90E232046FFF762FE1B +:101BF000FDE60B2284F8342083F311882B462A4615 +:101C000029462046FFF768FEE3E700BF543B000883 +:101C10004C3B0008503B000838B590F8343004467F +:101C2000002B3ED0063BDAB20F2A34D80F2B32D825 +:101C3000DFE803F0373131082232313131313131CF +:101C400031313737C56BB0F84A309D4214D2C36882 +:101C50001B8AB5FBF3F203FB12556DB9202383F306 +:101C600011882B462A462946FFF736FE85F3118850 +:101C70000A2384F834300EE0142384F8343020230F +:101C800083F3118800231A4619462046FFF712FEF7 +:101C9000002383F3118838BD036C03B198470023F8 +:101CA000E7E70021204601F051FA0021204601F02B +:101CB00043FA63681B6813B1062120469847062340 +:101CC000D7E7000010B590F83430142B044629D023 +:101CD00017D8062B05D001D81BB110BD093B022B2C +:101CE000FBD80021204601F031FA0021204601F006 +:101CF00023FA63681B6813B1062120469847062320 +:101D000019E0152BE9D10B2380F83430202383F31D +:101D1000118800231A461946FFF7DEFD002383F3DE +:101D20001188DAE7C3689B695B68002BD5D1036C27 +:101D300003B19847002384F83430CEE7024B0022E9 +:101D4000C3E900339A607047202600200023037403 +:101D50008268054B1B6899689142FBD25A68036000 +:101D600042601060586070472026002008B520238C +:101D700083F31188037C032B05D0042B0DD02BB9E2 +:101D800083F3118808BD436900221A604FF0FF33C6 +:101D90004361FFF7DBFF0023F2E7D0E90032136075 +:101DA0005A60F3E7002303748268054B1B68996847 +:101DB0009142FBD85A6803604260106058607047D7 +:101DC00020260020054B19690874186802681A60FB +:101DD0005360186101230374FEF798BA202600208F +:101DE00030B54B1C0B4D87B0044610D02B690A4A06 +:101DF00001A800F025F92046FFF7E4FF049B13B18A +:101E000001A800F059F92B69586907B030BDFFF7F8 +:101E1000D9FFF8E7202600206D1D000838B50C4DCD +:101E200041612B6981689A689142044603D8BDE8F4 +:101E30003840FFF78BBF1846FFF7B4FF01232C6132 +:101E4000014623742046BDE83840FEF75FBA00BF64 +:101E500020260020044B1A681B6990689B689842F2 +:101E600094BF0020012070472026002010B5084CA8 +:101E7000236820691A6822605460012223611A7461 +:101E8000FFF790FF01462069BDE81040FEF73EBA1B +:101E90002026002008B5FFF7DDFF18B1BDE8084097 +:101EA000FFF7E4BF08BD0000FFF7E0BFFEE700005A +:101EB00010B50C4CFFF742FF00F0B4F80A4980223D +:101EC000204600F03BF8012344F8180C037400F09E +:101ED00071FC002383F3118862B60448BDE810400A +:101EE00000F04CB848260020583B0008683B00082A +:101EF00000F01CB9EFF3118020B9EFF30583202225 +:101F000082F311887047000010B530B9EFF30584F3 +:101F1000C4F3080414B180F3118810BDFFF7BAFFB1 +:101F200084F31188F9E70000034A516853685B1A8B +:101F30009842FBD8704700BF001000E08260022288 +:101F4000028270478368A3F17C0243F80C2C02697B +:101F500043F83C2C426943F8382C074A43F81C2CC0 +:101F6000C26843F8102C022203F8082C002203F860 +:101F7000072CA3F1180070472503000810B5202393 +:101F800083F31188FFF7DEFF00210446FFF746FFC9 +:101F9000002383F31188204610BD0000024B1B690B +:101FA00058610F20FFF70EBF20260020202383F367 +:101FB0001188FFF7F3BF000008B50146202383F323 +:101FC00011880820FFF70CFF002383F3118808BD58 +:101FD00049B1064B42681B6918605A601360436040 +:101FE0000420FFF7FDBE4FF0FF3070472026002091 +:101FF0000368984206D01A6802605060596118461A +:10200000FFF7A4BE7047000038B504460D462068AF +:10201000844200D138BD036823605C604561FFF7EE +:1020200095FEF4E7054B03F11402C3E905224FF0D6 +:10203000FF310022C3E90712704700BF20260020AD +:1020400070B51C4EC0E9032305460C4601F07AFA30 +:10205000334653F8142F9A420DD13062C5E901245A +:102060002A600A2C2CBF00190A30C6E90555BDE8C4 +:10207000704001F055BA316A431AE31838BF1C4664 +:102080009368A34202D9081901F058FA73699A6853 +:1020900094420CD85A68AC602B606A6015609A68EC +:1020A0005D60121B9A604FF0FF33F36170BD1B68D7 +:1020B000A41AECE72026002038B51B4C636998422F +:1020C0000DD0D0E9003213605A6000228168C260EE +:1020D0009A680A449A604FF0FF33E36138BD2246A4 +:1020E000036842F8143F002193425A60C16003D153 +:1020F000BDE8384001F01CBA9A688168256A0A4434 +:102100009A6001F01FFA63699A68411B8A42E5D917 +:10211000AB181D1A092D206A98BF01F10A02BDE80B +:102120003840104401F00ABA202600202DE9F04181 +:10213000184C002704F11406656901F003FA236ABC +:10214000AA68C11A8A4215D813442362D5E900321D +:1021500013605A606369D5F80C80EF60B34201D117 +:1021600001F0E6F987F311882869C047202383F33B +:102170001188E1E76169B14209D013441B1ABDE837 +:10218000F0410A2B2CBFC0180A3001F0D7B9BDE8C6 +:10219000F08100BF2026002000207047FEE70000ED +:1021A000704700004FF0FF307047000002290CD04C +:1021B000032904D00129074818BF00207047032ACB +:1021C00005D8054800EBC20070470448704700205E +:1021D000704700BF4C3C00082C220020003C000847 +:1021E00070B59AB00546084601A9144600F0C2F839 +:1021F00001A8FEF78BFD431C5B00C5E900340022FB +:10220000237003236370C6B201AB02341046D1B20F +:102210008E4204F1020401D81AB070BD13F8011BFC +:1022200004F8021C04F8010C0132F0E708B5202381 +:1022300083F311880348FFF779FA002383F31188A9 +:1022400008BD00BFD827002090F8443003F01F02DB +:10225000012A07D190F845200B2A03D10023C0E9B9 +:102260000E3315E003F06003202B08D1B0F848309E +:102270002BB990F84520212A03D81F2A04D8FFF74C +:1022800037BA222AEBD0FAE7034A82630722C263F5 +:1022900003640120704700BF2322002007B50529F1 +:1022A00017D8DFE801F0191603191920202383F34A +:1022B0001188104A01900121FFF7D8FA01980E4ABF +:1022C0000221FFF7D3FA0D48FFF7FCF9002383F34F +:1022D000118803B05DF804FB202383F311880748BD +:1022E000FFF7C6F9F2E7202383F311880348FFF7CD +:1022F000DDF9EBE7A03B0008C43B0008D82700202D +:1023000038B50C4D0C4C0D492A4604F10800FFF776 +:1023100067FF05F1CA0204F110000949FFF760FFE9 +:1023200005F5CA7204F118000649BDE83840FFF708 +:1023300057BF00BFA02C00202C220020803B0008AB +:102340008A3B0008953B000870B5044608460D46D8 +:10235000FEF7DCFCC6B22046013403780BB9184600 +:1023600070BD32462946FEF7BDFC0028F3D101209E +:10237000F6E700002DE9F04705460C46FEF7C6FCDF +:102380002B49C6B22846FFF7DFFF08B10736F6B281 +:1023900028492846FFF7D8FF08B11036F6B2632E59 +:1023A0000BD8DFF88C80DFF88C90234FDFF894A0F7 +:1023B0002E7846B92670BDE8F08729462046BDE84C +:1023C000F04701F051BB252E2ED107224146284669 +:1023D000FEF788FC70B9194B224603F10C0153F843 +:1023E000040B42F8040B8B42F9D11B88138007358C +:1023F0000E34DDE7082249462846FEF773FC98B9FB +:102400000F4BA21C197809090232C95D02F8041C9D +:1024100013F8011B01F00F015345C95D02F8031CBD +:10242000F0D118340835C3E704F8016B0135BFE774 +:102430006C3C0008953B0008833C0008743C000895 +:10244000107AFF1F1C7AFF1FBFF34F8F024AD36819 +:10245000DB03FCD4704700BF003C024008B5094BC9 +:102460001B7873B9FFF7F0FF074B1A69002ABFBF4B +:10247000064A5A6002F188325A601A6822F4806271 +:102480001A6008BDFE2E0020003C02402301674573 +:1024900008B50B4B1B7893B9FFF7D6FF094B1A69A8 +:1024A00042F000421A611A6842F480521A601A68B7 +:1024B00022F480521A601A6842F480621A6008BDE1 +:1024C000FE2E0020003C02400B28F0B516D80C4C24 +:1024D0000C4923787BB90C4D0E460C234FF000625B +:1024E00055F8047B46F8042B013B13F0FF033A44F4 +:1024F000F6D10123237051F82000F0BD0020FCE745 +:10250000302F0020002F0020943C0008014B53F88E +:1025100020007047943C00080C2070470B2810B531 +:10252000044601D9002010BDFFF7CEFF064B53F83B +:1025300024301844C21A0BB90120F4E712680132A2 +:10254000F0D1043BF6E700BF943C00080B2838B5F7 +:10255000044628D8FFF7CEFCFFF776FFFFF77EFF93 +:10256000124AF323D360E300DBB243F4007343F079 +:1025700002031361136943F4803313610546204657 +:10258000FFF762FFFFF7A0FF094B53F8241000F09C +:10259000E9F82846FFF77CFFFFF7B6FC2046BDE8C8 +:1025A0003840FFF7BBBF002038BD00BF003C0240F1 +:1025B000943C000812F001032DE9F04105460E4657 +:1025C00014464BD18218B2F1016F61D8314B1B68B0 +:1025D00013F001035CD0304FFFF78CFCFFF73EFF98 +:1025E000F323FB60FFF730FF314640F20128032C54 +:1025F00018D824F00104284E0C446D1A40F201183A +:10260000A142336905EB01072AD123F001033361AD +:10261000FFF73EFFFFF778FC0120BDE8F081043CA6 +:102620000435E4E7AB07E4D13B6923F440733B6135 +:102630003B6943EA08033B6151F8046B2E60BFF32A +:102640004F8FFFF701FF2B689E42E8D03B6923F0D4 +:1026500001033B61FFF71CFFFFF756FC0020DCE79E +:1026600023F440733361336943EA080333610B8811 +:102670003B80BFF34F8FFFF7E7FE3F8831F8023B07 +:10268000BFB2BB42BCD0336923F001033361E1E741 +:102690001846C2E700380240003C0240084908B52D +:1026A0000B7828B11BB9FFF7D9FE01230B7008BDC9 +:1026B000002BFCD0BDE808400870FFF7E9BE00BF62 +:1026C000FE2E002010B50244064BD2B2904200D13B +:1026D00010BD441C00B253F8200041F8040BE0B2D6 +:1026E000F4E700BF502800400F4B30B51C6F2404A6 +:1026F00007D41C6F44F400741C671C6F44F400443E +:102700001C670A4C236843F4807323600244084B1F +:10271000D2B2904200D130BD441C00B251F8045BEB +:1027200043F82050E0B2F4E7003802400070004067 +:102730005028004007B5012201A90020FFF7C2FF81 +:10274000019803B05DF804FB13B50446FFF7F2FFF0 +:10275000A04205D0012201A900200194FFF7C4FF87 +:1027600002B010BD70470000034B1A681AB9034A43 +:10277000D2F874281A607047342F002000300240CD +:1027800008B5FFF7F1FF024B1868C0F3407008BDB1 +:10279000342F0020EFF3098305494A6B22F0010230 +:1027A0004A63683383F30988002383F311887047F1 +:1027B00000EF00E0202080F3118862B60C4B0D4A38 +:1027C000D96821F4E0610904090C0A43DA60D3F8FE +:1027D000FC20094942F08072C3F8FC200A6842F0EC +:1027E00001020A601022DA7783F82200704700BFE6 +:1027F00000ED00E00003FA05001000E010B5202312 +:1028000083F311880E4B5B6813F4006314D0F1EE70 +:10281000103AEFF30984683C4FF08073E361094B91 +:10282000DB6B236684F30988FFF714FB10B1064BBA +:10283000A36110BD054BFBE783F31188F9E700BFE7 +:1028400000ED00E000EF00E0370300083A03000865 +:1028500070470000FEE700000A4B0B480B4A90420D +:102860000BD30B4BDA1C121AC11E22F003028B424F +:1028700038BF00220021FEF751BA53F8041B40F87C +:10288000041BECE73C3E00083030002030300020D4 +:10289000303000207047000070B5D0E915439E68C5 +:1028A00000224FF0FF3504EB42135101D3F8000929 +:1028B0000028BEBFD3F8000940F08040C3F80009EB +:1028C000D3F8000B0028BEBFD3F8000B40F08040C7 +:1028D000C3F8000B013263189642C3F80859C3F8D5 +:1028E000085BE0D24FF00113C4F81C3870BD000043 +:1028F0001D4B03EB80022DE9F043D2F80CC05D6D57 +:10290000DCF81420461CD2F800E005EB063605EB97 +:102910004018516871450AD3D5F83438012202FABB +:1029200000F023EA0000C5F83408BDE8F083BCF8E5 +:102930001040AEEB0103A34228BF2346D8F8184944 +:10294000A4B2B3EB840FF0D89468A4F1040959F849 +:10295000047F3760A4EB09071F44042FF7D81C44F9 +:102960000B4494605360D4E7382F0020890141F074 +:102970002001016103699B06FCD41220FFF7D4BA41 +:1029800010B5054C2046FEF753FF4FF0A04363659A +:10299000024BA36510BD00BF382F0020E83C0008A3 +:1029A00070B50378012B054650D12A4B446D9842EF +:1029B0001BD1294B5A6B42F080025A635A6D42F088 +:1029C00080025A655A6D5A6942F080025A615A690A +:1029D00022F080025A610E2143205B69FEF79AFBC8 +:1029E0001E4BE3601E4BC4F800380023C4F8003EC1 +:1029F000C02323606E6D4FF40413A3633369002B6F +:102A0000FCDA012333610C20FFF78EFA3369DB0710 +:102A1000FCD41220FFF788FA3369002BFCDA002679 +:102A2000A6602846FFF738FF6B68C4F81068DB68BB +:102A3000C4F81468C4F81C684BB90A4BA3614FF082 +:102A4000FF336361A36843F00103A36070BD064BCD +:102A5000F4E700BF382F0020003802404014004047 +:102A600003002002003C30C0083C30C0F8B5446D83 +:102A7000054600212046FFF779FFA96D00234FF09E +:102A800001128F68C4F834384FF00066C4F81C286F +:102A90004FF0FF3004EB431201339F42C2F800694C +:102AA000C2F8006BC2F80809C2F8080BF2D20B6832 +:102AB0006A6DEB65636210231361166916F01006E8 +:102AC000FBD11220FFF730FAD4F8003823F4FE636C +:102AD000C4F80038A36943F4402343F01003A36112 +:102AE0000923C4F81038C4F814380A4BEB604FF0CF +:102AF000C043C4F8103B084BC4F8003BC4F810694D +:102B0000C4F80039EB6D03F1100243F48013EA6559 +:102B1000A362F8BDC43C000840800010426D90F8EC +:102B20004E10D2F8003823F4FE6343EA0113C2F8D2 +:102B3000003870472DE9F84300EB8103456DDA68F2 +:102B4000166806F00306731E022B05EB41130C46B4 +:102B500080460FFA81F94FEA41104FF00001C3F8A7 +:102B6000101B4FF0010104F1100398BFB60401FAE5 +:102B700003F391698EBF334E06F1805606F5004689 +:102B800000293AD0578A04F15801490137436F5060 +:102B9000D5F81C180B43C5F81C382B180021C3F8B6 +:102BA000101953690127611EA7409BB3138A928BAA +:102BB0009B08012A88BF5343D8F85C20981842EA42 +:102BC000034301F1400205EB8202C8F85C00214694 +:102BD00053602846FFF7CAFE08EB8900C3681B8ACA +:102BE00043EA8453483464011E432E51D5F81C38FF +:102BF0001F43C5F81C78BDE8F88305EB4917D7F8E3 +:102C0000001B21F40041C7F8001BD5F81C1821EA6D +:102C10000303C0E704F13F0305EB83030A4A5A604C +:102C200028462146FFF7A2FE05EB4910D0F80039EF +:102C300023F40043C0F80039D5F81C3823EA07070D +:102C4000D7E700BF0080001000040002826D126808 +:102C5000C265FFF721BE00005831436D49015B5842 +:102C600013F4004004D013F4001F0CBF0220012015 +:102C7000704700004831436D49015B5813F4004030 +:102C800004D013F4001F0CBF022001207047000085 +:102C900000EB8101CB68196A0B6813604B685360C5 +:102CA0007047000000EB810330B5DD68AA69136846 +:102CB000D36019B9402B84BF402313606B8A14681A +:102CC000426D1C44013CB4FBF3F46343033323F033 +:102CD000030302EB411043EAC44343F0C043C0F88E +:102CE000103B2B6803F00303012B09B20ED1D2F87D +:102CF000083802EB411013F4807FD0F8003B14BF7A +:102D000043F0805343F00053C0F8003B02EB411204 +:102D1000D2F8003B43F00443C2F8003B30BD000052 +:102D20002DE9F041244D6E6D06EB40130446D3F8B7 +:102D3000087BC3F8087B38070AD5D6F8143819077A +:102D400006D505EB84032146DB6828465B68984777 +:102D5000FA071FD5D6F81438DB071BD505EB84031B +:102D6000D968CCB98B69488A5A68B2FBF0F600FB87 +:102D700016228AB91868DA6890420DD2121AC3E98D +:102D80000024202383F311880B482146FFF78AFF94 +:102D900084F31188BDE8F081012303FA04F26B8902 +:102DA00023EA02036B81CB68002BF3D02146024853 +:102DB000BDE8F041184700BF382F002000EB810329 +:102DC00070B5DD68436D6C692668E6604A0156BBE4 +:102DD0001A444FF40020C2F810092A6802F00302D6 +:102DE000012A0AB20ED1D3F8080803EB421410F4FA +:102DF000807FD4F8000914BF40F0805040F00050AC +:102E0000C4F8000903EB4212D2F8000940F0044074 +:102E1000C2F80009D3F83408012202FA01F1014393 +:102E2000C3F8341870BD19B9402E84BF402020600B +:102E300020682E8A8419013CB4FBF6F440EAC440B1 +:102E400040F000501A44C6E7F8B504461E48456DE8 +:102E500005EB4413D3F80869C3F80869F10717D5DF +:102E6000D5F81038DA0713D500EB8403D9684B691D +:102E70001F68DA68974218D2D21B00271A605F6079 +:102E8000202383F311882146FFF798FF87F31188E9 +:102E9000330617D5D5F834280123A340134211D0A7 +:102EA0002046BDE8F840FFF723BD012303FA04F2F2 +:102EB000038923EA020303818B68002BE8D02146B3 +:102EC0009847E5E7F8BD00BF382F00202DE9F74F00 +:102ED000984C666D7569B3691D4015F4805875612D +:102EE00007D02046FEF70AFD03B0BDE8F04FFFF71C +:102EF00085BC002D12DAD6F8003E8E489F071EBF13 +:102F0000D6F8003E23F00303C6F8003ED6F8043896 +:102F100023F00103C6F80438FEF718FD280505D58F +:102F20008448FFF7B9FC8348FEF700FDA9040CD5DF +:102F3000D6F8083813F0060FF36823F470530CBF6B +:102F400043F4105343F4A053F3602A0704D5636895 +:102F5000DB680BB177489847EB0274D4AF0227D5F2 +:102F6000D4F85490DFF8CCB100274FF0010A09EBF8 +:102F70004712D2F8003B03F44023B3F5802F11D160 +:102F8000D2F8003B002B0DDA62890AFA07F322EA35 +:102F90000303638104EB8703DB68DB6813B1394605 +:102FA00058469847A36D01379B68FFB29F42DED910 +:102FB000E80617D5676D3A6AC2F30A1002F00F03EC +:102FC00002F4F012B2F5802F00F08880B2F5402FA5 +:102FD00008D104EB83030022DB681B6A07F58057E6 +:102FE00090426DD12903D6F8184813D5E20302D5D3 +:102FF0000020FFF795FEA30302D50120FFF790FE06 +:10300000670302D50220FFF78BFE260302D50320BB +:10301000FFF786FE6D037FF567AFE00702D500205E +:10302000FFF712FFA10702D50120FFF70DFF62078E +:1030300002D50220FFF708FF23077FF555AF0320D5 +:10304000FFF702FF50E7636DDFF8E8B00193002758 +:103050004FF0010AA36D9B685FFA87F999453FF627 +:103060007DAF019B03EB4913D3F8002902F4402202 +:10307000B2F5802F22D1D3F80029002A1EDAD3F826 +:10308000002942F09042C3F80029D3F80029002A11 +:10309000FBDB606D4946FFF769FC22890AFA09F3F8 +:1030A00022EA0303238104EB8903DB689B6813B1E5 +:1030B0004946584698474846FFF71AFC0137C9E782 +:1030C000910708BFD7F80080072A98BF03F8018B43 +:1030D00002F1010298BF4FEA182881E7023304EB9E +:1030E000830207F580575268D2F818C0DCF8082030 +:1030F000DCE9001CA1EB0C0C002188420AD104EB96 +:10310000830463689B699A6802449A605A6802441F +:103110005A6067E711F0030F08BFD7F800808C45AD +:1031200088BF02F8018B01F1010188BF4FEA18281E +:10313000E3E700BF382F0020436D03EB4111D1F8C6 +:10314000003B43F40013C1F8003B7047436D03EBB1 +:103150004111D1F8003943F40013C1F80039704728 +:10316000436D03EB4111D1F8003B23F40013C1F888 +:10317000003B7047436D03EB4111D1F8003923F454 +:103180000013C1F80039704730B5039C0172043355 +:1031900004FB0325C0E90653049B03630021059B40 +:1031A000C160C0E90000C0E90422C0E90842C0E9EA +:1031B0000A11436330BD0000416A0022C0E90411D6 +:1031C000C0E90A22C2606FF00101FEF71DBF0000D6 +:1031D000D0E90432934201D1C2680AB9181D704780 +:1031E0000020704703691960C2680132C260C26979 +:1031F000134482690361934224BF436A036100213F +:10320000FEF7F6BE38B504460D46E3683BB1626989 +:10321000131D1268A3621344E362002007E0237ABF +:1032200033B929462046FEF7D3FE0028EDDA38BD33 +:103230006FF00100FBE70000C368C269013BC36097 +:103240004369134482694361934224BF436A4361E3 +:1032500000238362036B03B11847704770B52023C6 +:10326000044683F31188866A3EB9FFF7CBFF054613 +:1032700018B186F31188284670BDA36AE26A13F874 +:10328000015BA362934202D32046FFF7D5FF0023E0 +:1032900083F31188EFE700002DE9F84F04460E464E +:1032A000174698464FF0200989F311880025AA4651 +:1032B000D4F828B0BBF1000F09D141462046FFF7F2 +:1032C000A1FF20B18BF311882846BDE8F88FD4E91F +:1032D0000A12A7EB050B521A934528BF9346BBF180 +:1032E000400F1BD9334601F1400251F8040B43F85B +:1032F000040B9142F9D1A36A40334036A3624035B2 +:10330000D4E90A239A4202D32046FFF795FF8AF3B5 +:103310001188BD42D8D289F31188C9E730465A4690 +:10332000FDF7D6FCA36A5B445E44A3625D44E7E715 +:1033300010B5029C0172043304FB0321C0E906139B +:103340000023C0E90A33039B0363049BC460C0E904 +:103350000000C0E90422C0E90842436310BD000038 +:10336000026AC260426AC0E904220022C0E90A225D +:103370006FF00101FEF748BED0E904239A4201D163 +:10338000C26822B9184650F8043B0B607047002011 +:1033900070470000C368C2690133C36043691344C6 +:1033A00082694361934224BF436A43610021FEF76F +:1033B0001FBE000038B504460D46E3683BB12369E3 +:1033C0001A1DA262E2691344E362002007E0237A37 +:1033D00033B929462046FEF7FBFD0028EDDA38BD5B +:1033E0006FF00100FBE7000003691960C268013A51 +:1033F000C260C269134482690361934224BF436A75 +:10340000036100238362036B03B118477047000018 +:1034100070B520230D460446114683F31188866A51 +:103420002EB9FFF7C7FF10B186F3118870BDA36AEC +:103430001D70A36AE26A01339342A36204D3E16977 +:1034400020460439FFF7D0FF002080F31188EDE714 +:103450002DE9F84F04460D46904699464FF0200A54 +:103460008AF311880026B346A76A4FB94946204619 +:10347000FFF7A0FF20B187F311883046BDE8F88F31 +:10348000D4E90A073A1AA8EB0607974228BF17465D +:10349000402F1BD905F1400355F8042B40F8042BAD +:1034A0009D42F9D1A36A4033A3624036D4E90A238E +:1034B0009A4204D3E16920460439FFF795FF8BF364 +:1034C00011884645D9D28AF31188CDE729463A4674 +:1034D000FDF7FEFBA36A3B443D44A3623E44E5E79F +:1034E000D0E904239A4217D1C3689BB1836A8BB198 +:1034F000043B9B1A0ED01360C368013BC360C369D1 +:103500001A44836902619A4224BF436A036100231B +:1035100083620123184670470023FBE700F088B858 +:103520004FF08043002258631A610222DA6070472C +:103530004FF080430022DA60704700004FF0804374 +:10354000586370474FF08043586A70474B68436038 +:103550008B688360CB68C3600B6943614B6903620E +:103560008B6943620B6803607047000008B5264B07 +:1035700026481A6940F2FF110A431A611A6922F4B7 +:10358000FF7222F001021A611A691A6B0A431A6368 +:103590001A6D0A431A651E4A1B6D1146FFF7D6FFC6 +:1035A00002F11C0100F58060FFF7D0FF02F1380145 +:1035B00000F58060FFF7CAFF02F1540100F580605A +:1035C000FFF7C4FF02F1700100F58060FFF7BEFF56 +:1035D00002F18C0100F58060FFF7B8FF02F1A8014D +:1035E00000F58060FFF7B2FF02F1C40100F58060D2 +:1035F000FFF7ACFF02F1E00100F58060FFF7A6FFE6 +:10360000BDE8084000F09AB80038024000000240CF +:10361000F43C000808B500F007FAFEF749FCFFF794 +:10362000A3F8BDE80840FEF76BBE0000704700003D +:10363000104B1A6C42F001021A641A6E42F0010239 +:103640001A660D4A1B6E936843F0010393604FF0B6 +:10365000804353229A624FF0FF32DA6200229A616D +:103660005A63DA605A6001225A6108211A601C20EC +:10367000FDF750BD00380240002004E04FF08042CA +:1036800008B51169D3680B40D9B2C9439B071161D2 +:1036900007D5202383F31188FEF72AFC002383F348 +:1036A000118808BD08B5FFF7E9FFBDE80840FFF73E +:1036B000A5B800001F4B1A696FEAC2526FEAD252D6 +:1036C0001A611A69C2F308021A614FF0FF301A69D1 +:1036D0005A69586100215A6959615A691A6A62F037 +:1036E00080521A621A6A02F080521A621A6A5A6A80 +:1036F00058625A6A59625A6A1A6C42F080521A64C5 +:103700001A6E42F080521A661A6E0B4A106840F424 +:1037100080701060186F00F44070B0F5007F1EBF1D +:103720004FF4803018671967536823F400735360AF +:1037300000F05AB90038024000700040334B4FF09F +:1037400080521A64324A4FF4404111601A6842F0C4 +:1037500001021A601A689107FCD59A6822F00302E8 +:103760009A602A4B9A6812F00C02FBD1196801F09A +:10377000F90119609A601A6842F480321A601A6876 +:103780009203FCD55A6F42F001025A671F4B5A6FE1 +:103790009007FCD51F4A5A601A6842F080721A607E +:1037A0001B4A53685904FCD5184B1A689201FCD582 +:1037B000194A9A60194B1A68194B9A42194B21D130 +:1037C000194A1168194A91421CD140F205121A6037 +:1037D000144A136803F00F03052BFAD10B4B9A68B8 +:1037E00042F002029A609A6802F00C02082AFAD1AA +:1037F0005A6C42F480425A645A6E42F480425A66CD +:103800005B6E704740F20572E1E700BF003802408E +:10381000007000400854400700948838002004E0FD +:1038200011640020003C024000ED00E041C20F4165 +:10383000084A08B5516913680B4003F0010353614E +:1038400023B1054A13680BB150689847BDE808409A +:10385000FEF7D4BF003C0140B02F0020084A08B555 +:10386000516913680B4003F00203536123B1054A09 +:1038700093680BB1D0689847BDE80840FEF7BEBF1B +:10388000003C0140B02F0020084A08B55169136878 +:103890000B4003F00403536123B1054A13690BB1D4 +:1038A00050699847BDE80840FEF7A8BF003C0140BA +:1038B000B02F0020084A08B5516913680B4003F087 +:1038C0000803536123B1054A93690BB1D069984746 +:1038D000BDE80840FEF792BF003C0140B02F002039 +:1038E000084A08B5516913680B4003F0100353618F +:1038F00023B1054A136A0BB1506A9847BDE80840E6 +:10390000FEF77CBF003C0140B02F0020174B10B5E4 +:103910005A691C68144004F478725A61A30604D5ED +:10392000134A936A0BB1D06A9847600604D5104ACF +:10393000136B0BB1506B9847210604D50C4A936B5F +:103940000BB1D06B9847E20504D5094A136C0BB153 +:10395000506C9847A30504D5054A936C0BB1D06C05 +:103960009847BDE81040FEF749BF00BF003C01404A +:10397000B02F00201A4B10B55A691C68144004F48B +:103980007C425A61620504D5164A136D0BB1506D25 +:103990009847230504D5134A936D0BB1D06D984712 +:1039A000E00404D50F4A136E0BB1506E9847A10482 +:1039B00004D50C4A936E0BB1D06E9847620404D5BF +:1039C000084A136F0BB1506F9847230404D5054A7A +:1039D000936F0BB1D06F9847BDE81040FEF70EBF54 +:1039E000003C0140B02F0020062108B50846FDF735 +:1039F00091FB06210720FDF78DFB06210820FDF72E +:103A000089FB06210920FDF785FB06210A20FDF729 +:103A100081FB06211720FDF77DFBBDE8084006214C +:103A20002820FDF777BB000008B5FFF743FE00F044 +:103A30000DF8FDF713FDFDF7F9FEFDF7D1FDFFF7DA +:103A4000F5FDBDE80840FFF769BD00000023054A09 +:103A500019460133102BC2E9001102F10802F8D116 +:103A6000704700BFB02F0020034611F8012B03F868 +:103A7000012B002AF9D1704753544D3332463F3F52 +:103A80003F00000053544D333246343078005354D5 +:103A90004D3332463432780053544D333246343449 +:103AA00036585800012033000010410001105A0020 +:103AB000031059000710310000000000783A000898 +:103AC0003F00000013040000843A00083F0000009B +:103AD000190400008E3A00083F0000002104000095 +:103AE000983A00083F000000009600000000000027 +:103AF00000000000000000000000000000000000C6 +:103B000025140008111400084D1400083914000889 +:103B100045140008311400081D1400080914000899 +:103B200059140008000000005D1500084915000840 +:103B300085150008711500087D1500086915000835 +:103B400055150008411500089115000800000000F7 +:103B500001000000000000006D61696E00000000BF +:103B600069646C6500000000603B0008602600206E +:103B7000D827002001000000AD1E00080000000052 +:103B80004172647550696C6F740025424F41524414 +:103B9000252D424C002553455249414C250000003B +:103BA000020000000000000079170008E517000877 +:103BB00040004000702C0020802C002002000000FB +:103BC00000000000030000000000000029180008A9 +:103BD0000000000010000000902C002000000000F9 +:103BE0000100000000000000382F00200101020049 +:103BF0009D220008AD210008492200082D2200085E +:103C000043000000083C000809024300020100C014 +:103C10003209040000010202010005240010010520 +:103C20002401000104240202052406000107058284 +:103C3000030800FF09040100020A00000007050153 +:103C40000240000007058102400000001200000051 +:103C5000543C0008120110010200004009124157B3 +:103C600000020102030100000403090425424F4140 +:103C7000524425004D6174656B463430352D5654E1 +:103C80004F4C0030313233343536373839414243C6 +:103C90004445460000400000004000000040000095 +:103CA00000400000000001000000020000000200CF +:103CB00000000200000002000000020000000200FC +:103CC000000002000000000061190008191C000833 +:103CD000C51C000840004000982F0020982F0020AD +:103CE00001000000A82F002080000000400100001B +:103CF000030000000001A81600000000AAAAAAAA5A +:103D000000001400EF9F00000000000070A70A00F0 +:103D10000004000100000000AAAAAAAA00000001F5 +:103D2000DFFF00000000000000000000040000149D +:103D300000000000AAAAAAAA04000014FFFF0000C5 +:103D40000000000000000000000000000000000073 +:103D5000AAAAAAAA00000000FFFF000000000000BD +:103D6000000000000000000000000000AAAAAAAAAB +:103D700000000000FFFF0000000000000000000045 +:103D80000000000000000000AAAAAAAA000000008B +:103D9000FFFF000000000000000000000000000025 +:103DA00000000000AAAAAAAA00000000FFFF00006D +:103DB0000000000000000000000000000000000003 +:103DC0000A000000000000000300000000000000E6 +:103DD00000000000000000000000000000000000E3 +:103DE00000000000000000000000000000000000D3 +:103DF0001E0400000000000000000F000000000092 +:103E0000FF000000D8270020B82300200096000003 +:103E100000000800960000000008000004000000F8 +:103E2000683C0008000000000000000000000000E6 +:0C3E300000000000000000000000000086 +:00000001FF diff --git a/Tools/bootloaders/SPRacingH7_bl.bin b/Tools/bootloaders/SPRacingH7_bl.bin index 07d603851818f..842e1e3c4d6dc 100644 Binary files a/Tools/bootloaders/SPRacingH7_bl.bin and b/Tools/bootloaders/SPRacingH7_bl.bin differ diff --git a/Tools/bootloaders/SPRacingH7_bl.hex b/Tools/bootloaders/SPRacingH7_bl.hex index e7a1420986b59..620282252b3fb 100644 --- a/Tools/bootloaders/SPRacingH7_bl.hex +++ b/Tools/bootloaders/SPRacingH7_bl.hex @@ -1,1656 +1,1693 @@ :020000040800F2 -:1000000000060020B5050008F91900087519000858 -:10001000CD19000875190008A1190008B7050008D6 -:10002000B7050008B7050008B705000855460008E1 -:10003000B7050008B7050008B7050008B7050008B0 -:10004000B7050008B7050008B7050008B7050008A0 -:10005000B7050008B7050008B95C0008E55C0008B2 -:10006000115D00083D5D0008695D0008B7050008E6 -:10007000B7050008B7050008B7050008B705000870 -:10008000B7050008B7050008B7050008B705000860 -:10009000B7050008B7050008B7050008955D00081A -:1000A000B7050008B7050008B7050008B705000840 -:1000B000B7050008B7050008B7050008B705000830 -:1000C000B7050008B7050008B7050008B705000820 -:1000D000B7050008815E0008B7050008B7050008ED -:1000E000F95D0008B7050008B7050008B705000866 -:1000F000B7050008B7050008B7050008B7050008F0 -:10010000B7050008B7050008955E0008B7050008A8 -:10011000B7050008B7050008B7050008B7050008CF -:10012000B7050008B7050008B7050008B7050008BF -:10013000B7050008B7050008B7050008B7050008AF -:10014000B7050008B7050008B7050008B70500089F -:10015000B7050008B7050008B7050008B70500088F -:10016000B7050008B7050008B7050008B70500087F -:10017000B705000875530008B7050008B705000863 -:10018000B7050008B7050008B7050008B70500085F -:10019000B7050008B7050008B7050008B70500084F -:1001A000B7050008B7050008B7050008B70500083F -:1001B0006D5E0008B7050008B7050008B705000820 -:1001C000B7050008B7050008B7050008B70500081F -:1001D000B705000861530008B7050008B705000817 -:1001E000B7050008B7050008B7050008B7050008FF -:1001F000B7050008B7050008B7050008B7050008EF -:10020000B7050008B7050008B7050008B7050008DE -:10021000B7050008B7050008B7050008B7050008CE -:10022000B7050008B7050008E5470008B70500084E -:10023000B7050008B7050008B7050008B7050008AE -:10024000B7050008B7050008B7050008B70500089E -:10025000B7050008B7050008B7050008B70500088E -:10026000B7050008B7050008B7050008B70500087E -:10027000B7050008B7050008B7050008B70500086E -:10028000B7050008B7050008B7050008B70500085E -:10029000B7050008B7050008B7050008B70500084E -:1002A0009D0700084D1900085D24000800000000AB -:1002B00053B94AB9002908BF00281CBF4FF0FF31CD -:1002C0004FF0FF3000F074B9ADF1080C6DE904CEC9 -:1002D00000F006F8DDF804E0DDE9022304B0704721 -:1002E0002DE9F047089D04468E46002B4DD18A42E9 -:1002F000944669D9B2FA82F252B101FA02F3C2F11C -:10030000200120FA01F10CFA02FC41EA030E9440AC -:100310004FEA1C48210CBEFBF8F61FFA8CF708FBCD -:1003200016E341EA034306FB07F199420AD91CEBA5 -:10033000030306F1FF3080F01F81994240F21C81D7 -:10034000023E63445B1AA4B2B3FBF8F008FB10331F -:1003500044EA034400FB07F7A7420AD91CEB040454 -:1003600000F1FF3380F00A81A74240F20781644424 -:10037000023840EA0640E41B00261DB1D4400023A9 -:10038000C5E900433146BDE8F0878B4209D9002D0D -:1003900000F0EF800026C5E9000130463146BDE897 -:1003A000F087B3FA83F6002E4AD18B4202D3824201 -:1003B00000F2F980841A61EB030301209E46002DB0 -:1003C000E0D0C5E9004EDDE702B9FFDEB2FA82F205 -:1003D000002A40F09280A1EB0C014FEA1C471FFA63 -:1003E0008CFE0126200CB1FBF7F307FB131140EA4A -:1003F00001410EFB03F0884208D91CEB010103F117 -:10040000FF3802D2884200F2CB804346091AA4B2D8 -:10041000B1FBF7F007FB101144EA01440EFB00FEAC -:10042000A64508D91CEB040400F1FF3102D2A64511 -:1004300000F2BB800846A4EB0E0440EA03409CE7B0 -:10044000C6F12007B34022FA07FC4CEA030C20FA5D -:1004500007F401FA06F31C43F9404FEA1C4900FA7D -:1004600006F3B1FBF9F8200C1FFA8CFE09FB1811FA -:1004700040EA014108FB0EF0884202FA06F20BD96D -:100480001CEB010108F1FF3A80F08880884240F2BD -:100490008580A8F102086144091AA4B2B1FBF9F001 -:1004A00009FB101144EA014100FB0EFE8E4508D9FC -:1004B0001CEB010100F1FF346CD28E456AD9023881 -:1004C000614440EA0840A0FB0294A1EB0E01A14266 -:1004D000C846A64656D353D05DB1B3EB080261EBD4 -:1004E0000E0101FA07F722FA06F3F1401F43C5E9AE -:1004F000007100263146BDE8F087C2F12003D840E4 -:100500000CFA02FC21FA03F3914001434FEA1C4725 -:100510001FFA8CFEB3FBF7F007FB10360B0C43EA17 -:10052000064300FB0EF69E4204FA02F408D91CEBC7 -:10053000030300F1FF382FD29E422DD902386344C5 -:100540009B1B89B2B3FBF7F607FB163341EA034165 -:1005500006FB0EF38B4208D91CEB010106F1FF38B4 -:1005600016D28B4214D9023E6144C91A46EA0046AB -:1005700038E72E46284605E70646E3E61846F8E63D -:100580004B45A9D2B9EB020864EB0C0E0138A3E786 -:100590004646EAE7204694E74046D1E7D0467BE767 -:1005A000023B614432E7304609E76444023842E7DF -:1005B000704700BF02E000F000F8FEE772B63A486C -:1005C00080F30888394880F3098839484EF6085185 -:1005D000CEF20001086040F20000CCF200004EF6BE -:1005E0003471CEF200010860BFF34F8FBFF36F8FFD -:1005F00040F20000C0F2F0004EF68851CEF2000149 -:100600000860BFF34F8FBFF36F8F4FF00000E1EE34 -:10061000100A4EF63C71CEF200010860062080F30D -:100620001488BFF36F8F04F0D3F804F075F805F069 -:1006300011F94FF055301F491B4A91423CBF41F818 -:10064000040BFAE71C49194A91423CBF41F8040BDC -:10065000FAE71A491A4A1B4B9A423EBF51F8040B5B -:1006600042F8040BF8E700201749184A91423CBFB2 -:1006700041F8040BFAE704F08DF805F055F9144C35 -:10068000144DAC4203DA54F8041B8847F9E700F034 -:100690004FF8114C114DAC4203DA54F8041B884753 -:1006A000F9E704F075B800000006002000220020E1 -:1006B000000000080000002000060020A0660008DE -:1006C00000220020C0220020C0220020D0490020AB -:1006D000A0020008AC020008AC020008AC0200084E -:1006E0002DE9F04F2DED108AC1F80CD0D0F80CD0C8 -:1006F000BDEC108ABDE8F08F002383F311882846F3 -:10070000A047002003F0ECF9FEE703F01BF900DF3F -:10071000FEE70000054B10B504460360806810B189 -:1007200003685B689847204610BD00BF84620008DC -:1007300038B503F0B5FE054603F082FF0446F0B974 -:10074000144B9D421DD001339D4241F2883512BFAA -:10075000044600250124002003F0ACFE0CB100F09B -:100760007FF80D4C00F000FF00F09CFD204601F0EA -:10077000B1FC40B94FF47A7003F0B8F9F6E7002500 -:10078000E9E70546E7E7284600F02AF900F068F8AF -:10079000F9E700BF010007B0C02200200448054B64 -:1007A000054A036000230549836005F0CDBB00BF07 -:1007B000C022002084620008D02400201507000811 -:1007C00008B500F03BFDA0F120035842584108BD98 -:1007D00007B5042101900DEB010000F04DFD03B0C1 -:1007E0005DF804FB07B541F21203022101A8ADF840 -:1007F000043000F041FD03B05DF804FB202310B588 -:1008000083F311881248C3680BB103F007FA002381 -:10081000104A4FF47A710E4803F0C4F9002383F3B1 -:1008200011880D4C236813B12368013B2360636872 -:1008300013B16368013B6360084B1B7833B963688D -:1008400023B9022000F0FAFD3223636010BD00BF1F -:1008500050230020FD0700086C24002064230020A2 -:10086000F7B5574801A901F0ADFD002800F0A580BB -:10087000544B554A1C461968013100F09B800433E3 -:100880009342F8D16268514B9A4240F29380504BA8 -:10089000DB6803F1104303F580139A4280F08A80ED -:1008A000002000F019FD02204A4B187000F0BEFD38 -:1008B000494B0021D3F8E820C3F8E810D3F8102101 -:1008C000C3F81011D3F81021D3F8EC20C3F8EC10C2 -:1008D000D3F81421C3F81411D3F81421D3F8F0205D -:1008E000C3F8F010D3F81821C3F81811D3F8182161 -:1008F000D3F8802042F00062C3F88020D3F8802033 -:1009000022F00062C3F88020D3F88020D3F8802042 -:1009100042F00072C3F88020D3F8802022F00072E9 -:10092000C3F88020D3F8803072B64FF0E023C3F8CC -:10093000084DD4E90004BFF34F8FBFF36F8F274AF0 -:10094000C2F88410BFF34F8F536923F4803353618F -:10095000BFF34F8FD2F8803043F6E076C3F3C9057A -:10096000C3F34E335B0103EA060C29464CEA817758 -:100970000139C2F87472F9D2203B13F1200FF2D181 -:10098000BFF34F8FBFF36F8FBFF34F8FBFF36F8FE7 -:10099000536923F4003353610023C2F85032BFF38C -:1009A0004F8FBFF36F8F202383F31188854680F329 -:1009B00008882047024801F065F803B0F0BD00BF89 -:1009C000C02200200000109020001090FFFF0F9028 -:1009D00000220020642300200044025800ED00E0C3 -:1009E0002DE9F04FA84B9FB02022FF21089016A8B8 -:1009F0009E68DB68079300F061FDA44A1378A3B9F1 -:100A00000121A3481170C360202383F31188C368B8 -:100A10000BB103F003F900239E4A4FF47A719C480E -:100A200003F0C0F8002383F31188089B9A4A03B1AE -:100A3000136000239949089D98469B46984F0B7078 -:100A40005360CDE90533012000F0F0FC25B1924B55 -:100A50001B68002B00F07D83002000F0EFFB0E9060 -:100A60000E9B002BF2DB012000F0D6FC0E9B213BFD -:100A7000162BE8D801A252F823F000BFD90A0008CB -:100A8000010B00089B0B0008470A0008470A0008F2 -:100A9000470A0008910E0008951000082B0F000867 -:100AA00013100008391000085F100008470A0008FA -:100AB00071100008470A0008DD100008790B0008D3 -:100AC000470A0008591100083D0C0008770D00087E -:100AD000470A0008890F00080220FFF771FE00286E -:100AE00040F06383089B0221069A11A8002A08BFE0 -:100AF0001D4641F21233ADF8443000F0BDFBA2E7D1 -:100B00004FF47A7000F09AFB041EEBDB0220FFF733 -:100B100057FE0028E6D0013C052C00F24883DFE8B0 -:100B200004F0030A0D1013390523042111A81193B1 -:100B300000F0A2FB17E004215348F9E704215948CB -:100B4000F6E704215848F3E74FF01C09484609F13D -:100B5000040900F0C3FB0421119011A800F08CFBE4 -:100B6000B9F12C0FF2D10120059B4FF0000B00FAD8 -:100B700004F41C43E3B2059300F0ECFCB8F1000F61 -:100B800005D0059B03F00B030B2B08BF0025FFF7D7 -:100B900029FE58E704214548CAE7B8F1000FA1D063 -:100BA000059B03F00B030B2B9CD10220FFF708FEE3 -:100BB0000446002896D00120DFF8E08000F08CFB8E -:100BC0004FF00009022088F8000000F02FFC5FFAC7 -:100BD00089FA504600F090FB054690B1504609F165 -:100BE000010900F099FB0028F1D10546069441F275 -:100BF0001213022111A8A046ADF8443000F03CFBCE -:100C000021E701232E46DFF88090022088F800308B -:100C100000F002FCD9F80830B34207D9304600F0A2 -:100C20005DFB013040F0C6820436F3E700261A4B24 -:100C3000069488F80060A0465E609DE7B8F1000F5A -:100C40003FF450AF059B03F00B030B2B7FF44AAF2F -:100C500064210FA800F006FB814600287FF442AF14 -:100C60000220FFF7ADFD044600283FF43BAF3A6A8F -:100C70007B6A0F9953438DF837908B421AD20C48F8 -:100C800000F0E6FB2EE700BF0022002068240020D1 -:100C900050230020FD0700086C240020642300205E -:100CA000C022002004220020082200200C22002064 -:100CB0008C6000084FF4801AAE48C846BAFBF2FABE -:100CC00000F0C6FBFFF78EFD01210DF1370000F0AB -:100CD000D3FA0F9B434542D9002310AA5146384608 -:100CE000CDE9103311AB00F0D1FE00283FF47FAF07 -:100CF00000F08AFE109B00EB030901210DF1370083 -:100D000000F0BAFA00F080FE484520D9384601F0DC -:100D1000F3F880B964210F9B97480AF1010A01FB9F -:100D200008F1B1FBF3F18DF83710C9B200F090FB78 -:100D30003B6A9844CDE700F067FE119B1844484594 -:100D40003FF655AF00F060FE109B00EB0309109BCF -:100D50004FF47A70A3FB000302F0C8FECDE76423D2 -:100D600001210DF13700A0468DF8373000F084FAEC -:100D70000023079300E7B8F1000F3FF4B3AE059BE3 -:100D800003F00B030B2B7FF4ADAE02207B4B1870EE -:100D900000F04CFB322000F051FAB0F1000AFFF6EF -:100DA000A1AE079BD14653440993754B099ADB6862 -:100DB0009A423FF697AEBAF5807F3FF793AE714AFD -:100DC0000024A2450A920EDD4FF47A7000F036FA44 -:100DD0000E900E9B002BFFF685AE0E9B01340A9AF7 -:100DE00002F8013BEDE7C820FFF7EAFC04460028C3 -:100DF0003FF478AE079B1F2B11D8C3F1200223F0DC -:100E0000030016AB5F495245184428BF52460A9268 -:100E100000F022FB0A9AFF215A4800F04FFB002302 -:100E20009846CDE91033079B03F580130B93B9F176 -:100E3000000F02D1A046099B9BE711AB514A0B99C9 -:100E40003846029310AB4244414401930FAB0093E8 -:100E50004B4601F023FA00283FF4C9AE00F0D6FD5E -:100E60000F9B82460A919844A9EB030900F0CEFD3E -:100E7000109B0A9A13EB0A0342F10002834272EBC1 -:100E80000103F3D2384601F037F80028EED1CEE75F -:100E9000B8F1000F3FF426AE059B03F00B030B2BBC -:100EA0007FF420AE0220354B187000F0BFFA3220DC -:100EB00000F0C4F9B0F1000AFFF614AE1AF0030412 -:100EC0007FF410AE2E4A0AEB0603926893423FF677 -:100ED00009AEBAF5807F3FF705AEDFF8A890A245CE -:100EE0000DDD4FF47A7000F0A9F90E900E9B002BE7 -:100EF000FFF6F8AD0E9B013409F8013BEFE7C8207F -:100F0000FFF75EFC044600283FF4ECADCAF387020D -:100F10001C4930464FEAAA0900F0D8FA804600285A -:100F20003FF465AE06EB890626E60220FFF748FC93 -:100F300000283FF4D7AD00F00BFA00283FF4D2AD03 -:100F40004FF00009DFF838A04C46DAF808304B457E -:100F50000CD9484609F1040900F0C0F904221190A7 -:100F600011A9204601F0A0FA0446EEE72046FFF75B -:100F70002FFC01E6B1600008C9600008642300206E -:100F80000022002068230020642110A800F06AF9E4 -:100F9000044600287FF4A6AD0220FFF711FC0028CC -:100FA0003FF4A0AD109BA14603F0030A1099A1EBFA -:100FB0000A01A1421BD91F2C11D8169B01330ED058 -:100FC00024F003031EAA134453F8203C119348460F -:100FD000042211A9043401F067FA8146E6E70423EC -:100FE00011AA04F58011384601F0A4F9EFE7BAF12F -:100FF000000F0CD0534611AA01F58011384601F0BC -:1010000099F94846524611A901F04EFA81464846E0 -:10101000ADE70023642111A8119300F023F9002803 -:101020007FF460AD0220FFF7CBFB00283FF45AAD00 -:10103000119800F083F99AE70023642111A8119315 -:1010400000F010F900287FF44DAD0220FFF7B8FB47 -:1010500000283FF447AD119800F072F987E70220AD -:10106000FFF7AEFB00283FF43DAD00F081F97EE7CD -:101070000220FFF7A5FB00283FF434AD11A914208E -:1010800000F07CF90990FFF7A3FB099911A800F083 -:10109000F3F871E5322000F0D1F8041EFFF622AD1E -:1010A000A3077FF41FAD444A04EB0B0392689342FD -:1010B0003FF618AD0220FFF783FB00283FF412AD86 -:1010C00024F003045C44A3453FF456AD58460BF1AD -:1010D000040B00F003F9FFF77BFBF4E74FF47A70A1 -:1010E000FFF76EFB00283FF4FDAC00F031F980B152 -:1010F000169B013328D011AB16AA4FF48011384645 -:10110000029310AB01930FAB0093202301F0C6F8BC -:1011100008B944466BE500F079FC284E04460D46BC -:1011200000F074FC109B1B1945F10002834272EB26 -:101130000103F5D2304600F0DFFE0028F0D1202276 -:10114000FF2116A800F0BAF9FFF74CFB1C4802F08B -:10115000CDFC1FB0BDE8F08FB8F1000F3FF4C2AC7A -:10116000059B03F00B030B2B7FF4BCAC0023642125 -:1011700011A8119300F076F8044600287FF4B2AC71 -:101180000220FFF71DFB814600283FF4ABACFFF7C0 -:1011900029FB41F2883002F0A9FC119800F01EFAF8 -:1011A000C846254600F0D6F94DE48046E4E44FF009 -:1011B000000B97E406941AE500220020C0220020CC -:1011C000A0860100F7B50C46184E4FF47A7105461B -:1011D00002FB01F396F90020501C0BD1144829465C -:1011E00001930268176A2246B8478442019B03D1E3 -:1011F000002310E0002AF1D096F90020511C01D004 -:10120000012A0DD10B4829460268166A2246B047CA -:10121000844205D10123084A0120137003B0F0BDB8 -:101220004FF4FA7002F062FC0020F7E71022002071 -:10123000382B0020FC240020B8240020002307B510 -:10124000024601210DF107008DF80730FFF7BAFFC4 -:1012500020B19DF8070003B05DF804FB4FF0FF30AC -:10126000F9E700000A46042108B5FFF7ABFF80F05C -:101270000100C0B2404208BD074B0A4630B419789D -:10128000064B53F82140014623682046DD69044B94 -:10129000AC4630BC604700BFB8240020F4600008B2 -:1012A000A086010070B50A4E00240A4D02F0ECFE43 -:1012B000308028683388834208D902F0E1FE2B6829 -:1012C00004440133B4F5003F2B60F2D370BD00BF7E -:1012D000BA2400207424002002F07CBF00F10060DA -:1012E00000F500300068704700F10060920000F5E2 -:1012F000003002F0FBBE0000054B1A68054B1B884E -:101300009B1A834202D9104402F0BABE00207047F3 -:1013100074240020BA24002038B5074D04462868FC -:10132000204402F0B3FE28B928682044BDE83840C4 -:1013300002F0B2BE38BD00BF742400200020704708 -:1013400000F1FF5000F58F10D0F800087047000042 -:10135000064991F8243033B100230822086A81F845 -:101360002430FFF7C1BF0120704700BF7824002060 -:10137000014B1868704700BF0010005C244BF0B5AB -:101380001A680446234BC2F30B06120C1F885868D8 -:10139000BE4293F9085028D09F89BE4206D1012051 -:1013A0000C2505FB0033586893F9085041F20103FE -:1013B0009A421CD041F203039A421AD042F201032E -:1013C0009A4218D042F203039A4208BF5625621E81 -:1013D0000B46441E0A4493420FD214F9016F581C65 -:1013E0006EB1034600F8016CF5E70020D8E75A25F6 -:1013F000EDE75925EBE75825E9E7184605E02C24E9 -:1014000082421C7001D9981C5D70401AF0BD00BF6B -:101410000010005C14220020022803D1024B4FF47C -:1014200000229A61704700BF00100258022802D1C2 -:10143000014B08229A61704700100258022804D11B -:10144000024A536983F00803536170470010025841 -:101450000FB404B070470000002310B5934203D0CE -:10146000CC5CC4540133F9E710BD0000013810B55D -:1014700010F9013F3BB191F900409C4203D11AB1F0 -:101480000131013AF4E71AB191F90020981A10BD20 -:101490001046FCE70138013910F9013F11F9012F1D -:1014A0000BB19342F8D0981A7047000003460246E9 -:1014B000D01A12F9011B0029FAD1704702440346E1 -:1014C000934202D003F8011BFAE770472DE9F84375 -:1014D0001F4D14460746884695F8242052BBDFF876 -:1014E00070909CB395F824302BB92022FF214846F8 -:1014F0002F62FFF7E3FF95F824004146C0F1080290 -:1015000005EB8000A24228BF2246D6B29200FFF728 -:10151000A3FF95F82430A41B17441E449044E4B262 -:10152000F6B2082E85F82460DBD1FFF711FF002802 -:10153000D7D108E02B6A03EB82038342CFD0FFF7B9 -:1015400007FF0028CBD10020BDE8F8830120FBE78E -:1015500078240020024B1A78024B1A70704700BFA3 -:10156000B82400201022002038B5164C164D204615 -:1015700001F018FC2946204601F040FC2D68134874 -:10158000D5F89020D2F8043843F00203C2F80438AA -:1015900002F0ACFA0E49284601F03EFDD5F8902045 -:1015A0000C48D2F804380C49A04223F00203C2F8D8 -:1015B00004384FF4E1330B6003D0BDE8384001F04C -:1015C0004FBB38BD382B0020F863000840420F00A5 -:1015D00000640008FC240020A024002038B50B4B38 -:1015E00004461A780A4B53F822500A4B9D420CD0FD -:1015F000094B002118221846FFF760FF04600146DE -:101600002846BDE8384001F02BBB38BDB824002087 -:10161000F4600008382B0020A024002038B5084BC7 -:1016200004461546017543600A21243002F076FF16 -:1016300004F128002A460A2102F070FF204638BD36 -:10164000CC620008F8B51E461B6805460C461746D6 -:101650001BB9236863B9012408E0BDF8182031469E -:10166000806A02F073FF0028F3D100242046F8BD01 -:1016700021463A46686A02F09FFF04460028EAD1F4 -:101680003368002BF1D0A86A02F0CAFFEEE7000031 -:1016900038B50D46114604461A4611B1806A02F06B -:1016A0007DFF2DB12946606ABDE8384002F0B0BF29 -:1016B00038BD0000704700007047000000207047F0 -:1016C000002070470020704713B56C4600F10C03F2 -:1016D00084E8060094E8030083E8050002B010BD2A -:1016E0000023C0E90333704782B002AB03E9060070 -:1016F00002B070477047000000207047704700003C -:1017000000207047002070470120704782B0002001 -:1017100002A901E90C0002B07047000000207047E8 -:10172000002070470B6803634B68C3628B68436398 -:10173000CB6883630B69C3637047000038B5836A65 -:101740000446154D1B7955F8330001F061FFD4E9CB -:1017500009231B7955F8331002F130034968994287 -:1017600018D000231363626A5363D4E90912537CCF -:10177000581E137C92681B0443EA002313438B63B7 -:10178000D4E909131B79303155F83300BDE83840EE -:1017900001F0D2BE38BD00BF58610008F8B51A4F3D -:1017A00005460E4611463868FFF774FE40BB746864 -:1017B0003B792CB194F82C209A420FD02468F8E79A -:1017C000402004F0CBFB0122B5210446FFF726FFA1 -:1017D000736823603B79746084F82C30402004F0F7 -:1017E000BDFB002328600371C0E90333037A43F093 -:1017F00007030372054BC0E9094703602846F8BD9B -:1018000000232B60FAE700BFBC24002068610008B9 -:10181000044B10B504460360C06908B104F09CFB9A -:10182000204610BD1061000810B50446FFF7F0FF18 -:10183000204604F08FFB204610BD000008B519B10A -:10184000FFF77CFF012008BD836A1A79024B53F829 -:10185000320001F0E1FEF5E7586100082DE9FF4193 -:10186000DDF828800546174603911FFA88F6029393 -:10187000FFF764FF02ABBAB203A9686A0096FFF7EC -:10188000E1FE044638B900212846FFF7D7FF20467D -:1018900004B0BDE8F081AFB9AB6A05F12C011A794B -:1018A000134B53F83200B8F1000F19D101F052FE7A -:1018B00080F00104E4B23346686ADDE90221FFF7F3 -:1018C000E7FEE0E7029B83B9AB6A3A46084805F1B8 -:1018D0002C011C79039B50F8340001F051FEE7E71E -:1018E000029B424601F062FEE2E70024E3E700BF0C -:1018F0005861000813B504460191FFF71FFF019AD4 -:10190000A36A04F12C011879054B53F8300001F05B -:1019100063FE00212046FFF791FF012002B010BDB9 -:101920005861000810B50446FFF708FFA36A1A794A -:10193000054B53F8320001F05DFE00212046FFF711 -:101940007DFF012010BD00BF58610008064B01213A -:10195000064A1A6000221A71054AC3E902124FF4BE -:1019600006721A82704700BFBC240020FC60000889 -:10197000000E270700B59BB0EFF309816822684687 -:10198000FFF76AFDEFF30583044B9A6BDA6A9A6AF4 -:101990009A6A9A6A9A6A9A6A9B6AFEE700ED00E080 -:1019A00000B59BB0EFF3098168226846FFF754FD4C -:1019B000EFF30583044B9A6B9A6A9A6A9A6A9A6A59 -:1019C0009A6A9B6AFEE700BF00ED00E000B59BB09D -:1019D000EFF3098168226846FFF73EFDEFF30583C8 -:1019E000034B5A6B9A6A9A6A9A6A9A6A9B6AFEE7EA -:1019F00000ED00E0FEE70000FEE700000FB408B5D0 -:101A0000029802F003F8FEE702F0EABC02F0C2BC62 -:101A100080697047C0697047006A7047406A7047C4 -:101A2000806A7047F0B5ADF2044D0E4605460021C0 -:101A30004FF47F7201A80091FFF740FD2C6A7443B8 -:101A400001362B6A7343A34216D92B686A46214696 -:101A500028465F684FF48063B84770B16A464FF418 -:101A6000806312F8011BFF290AD1013B9BB2002BB6 -:101A7000F7D104F58064E4E701200DF2044DF0BDD8 -:101A80000020FAE780680368DB69184730B5456ACB -:101A900085B004468D420FD30568CDE90023026A64 -:101AA00003ABAD6A5143A84720B1206A039BC31A18 -:101AB0005842584105B030BD0020FBE7F0B5C5697C -:101AC00087B004468D4212D30C9F86690568714326 -:101AD000CDE9013705AB00936D69036AA84720B1D2 -:101AE000A069059BC31A5842584107B0F0BD0020B9 -:101AF000FBE70000F0B5662389B0002405464FF4EB -:101B000080768068079403A9CDE90336CDE90544C2 -:101B100003685B699847A8682246214603680094D9 -:101B20001F692346B847A8689923079403A9CDE9FC -:101B30000336CDE9054403685B699847A8682246E7 -:101B40002146036800941D692346A84709B0F0BDEB -:101B5000254B2DE9F04F1B789FB0002504468DF8EA -:101B600028304FF09F0E80684FF00123ADF82950C8 -:101B700005A90995CDE905E3CDE9075503685B693A -:101B80009847A06803222946036800922A461E69E6 -:101B90000AABB04728B394F88C20144B9DF828105A -:101BA00003EBC2031A7991421BD19DF829205B797E -:101BB0009A4216D1A0684FF05A0C0D4B082605A981 -:101BC0000996CDE905C3CDE9075503685B699847D8 -:101BD000A0682A462946036800961F690AABB847E1 -:101BE00040B9002048E100BF74600008646200084A -:101BF000002500010422B1490AA804F0DFF90546D6 -:101C00000028EED1BDF82C306FF48277A3813B44DD -:101C1000012BE6D8079605A9CDE90A00A06803685C -:101C20005B699847A0682A462946036800961E69A2 -:101C30000AABB0470028D4D0DDE90A361A0202F414 -:101C4000706242EA1662FF2ACBD11B0E26F07F4655 -:101C500050222946142B26610AA828BF1423A373F7 -:101C6000FFF72CFCA068079605A903685B699847F5 -:101C7000A068A37B294602689B0000930AAB166903 -:101C80002A46B0470028ACD00B990029A9DBC90827 -:101C90006FF07E43149D4FF0010E994228BF194604 -:101CA000C5F303130EFA03F221FA03F30391616102 -:101CB000A261E361002B94D02646A44600274FF092 -:101CC00002081EAAC7F3460302EB830353F834ACA1 -:101CD00007F001031B01DA1D0EFA03F908FA02F2FC -:101CE000A2EB090202EA0A02DA401AD003F108095B -:101CF0000F330EFA02F2216A08FA03F30EFA09FB17 -:101D0000CCF830208A42A3EB0B0B0BEA0A0A2AFA22 -:101D100009F98CF82C90A36A88BF2262934288BF8D -:101D2000A26201370CF1100C042FCAD1236A4FF0C4 -:101D3000020C039A4FF0010E139F9A4224BFB2FB8C -:101D4000F3F1616207F00F0138BF01234FF0090280 -:101D500001F1010138BF63624900336B33B3531C97 -:101D60000EFA02F8A2F105090CFA03F30EFA09FAC9 -:101D7000A3EB080302F1FF380CFA08F83B40A8EB8C -:101D80000A08D34008EA070828FA09F877D0012B97 -:101D900077D0022B77D0032B08BF4FF47A735FFA0A -:101DA00088F808FB033373634B43B3630732103681 -:101DB000252AD2D115F0C04FC5F3417365D0012B50 -:101DC00065D0022B0CBF4FF47A634FF47A43C5F30E -:101DD000046202FB0333E3664B432367199B9E07B0 -:101DE00058D0062384F8743015F4005F05F00F0313 -:101DF000C5F3042514BF40220822013305FB022548 -:101E00005B00A5675D430A9B9F02E5677FF5E9AE2E -:101E10000C9B1A0A84F88020C3F3421203F01F03BC -:101E200084F8882084F88130189BC3F3025213F49D -:101E3000401F84F883207FF4D4AEDE0602D5042A46 -:101E40007FF4CFAE03F40072002A14BF01220022F7 -:101E500084F8872004D0DD0324D5002384F889305A -:101E6000179B1A0722D5002384F88630702384F844 -:101E70008530012384F88D301FB0BDE8F08F012339 -:101E80008DE710238BE7802389E710239FE74FF42A -:101E900080739CE7590701D55023A3E713F0780F0F -:101EA000A2D09EE69903DBD50123D7E75B07E0D5F7 -:101EB000012384F886300523D9E700BFC4610008F8 -:101EC00030B58BB000240546806803924FF00122A4 -:101ED000059105A9CDE90624CDE9084402685269B7 -:101EE0009047A8680121039B02680091214615696B -:101EF0002246A8470BB030BD13B504460DF10702CA -:101F000090F88510FFF7DCFF94F886309DF8070005 -:101F10001BB100F0010002B010BDC043C0F3C010FF -:101F2000F9E7000030B54FF001238BB000240546DF -:101F300080688DF80F20059105A9CDE90634CDE91B -:101F4000084403685B699847A86801220DF10F01F6 -:101F5000036800941D692346A8470BB030BD0000FC -:101F6000F7B517460DF1070206461D46FFF7A8FF15 -:101F700068B19DF8074039469DF82020304624EA94 -:101F800005042A4022438DF80720FFF7CBFF03B05A -:101F9000F0BD000030B54FF4807389B000240546D1 -:101FA0008068039103A9CDE90434CDE906440368B0 -:101FB0005B699847A86822462146036800941D691A -:101FC0002346A84709B030BD10B504462368204613 -:101FD000DB6A98470028F9D110BD000010B590F8D1 -:101FE000743004465BB1FFF7EFFF012394F87410DF -:101FF000204684F88E30BDE81040FFF7CBBF0120AB -:1020000010BD000010B590F87430044653B1FFF7CE -:10201000DBFF01232046042184F88E30BDE8104008 -:10202000FFF7B8BF012010BD70B590F883308AB0BB -:102030000446042B48D10DF10E020521FFF740FFA5 -:1020400000282FD00DF10F0235212046FFF738FF71 -:1020500040B320461E4EFFF7C1FF2046FFF7B4FFF6 -:1020600005AD0FCE0FC5336805A99DF80E202B6076 -:102070009DF80F30A06843F002038DF810208DF812 -:102080000F308DF8113003685B699847A068002312 -:1020900004A90268009315690222A84728B92046BE -:1020A000FFF7B0FF00200AB070BD2046FFF7AAFF7F -:1020B0000DF10F0235212046FFF702FF0028F1D075 -:1020C0009DF80F309B07EDD501202071EBE700BF95 -:1020D000786000082DE9F341002304468068A3607E -:1020E00010B103685B6898470025244E244F56F8CA -:1020F000352001A839465FFA85F8FFF74FFB019BB1 -:10210000A0680022A360019210B103685B68984741 -:10211000019810B103685B689847A368DBB141F28E -:10212000883084F88C8001F0E1FC2046FFF7E2FC67 -:102130001E2001F0DBFC2046FFF70AFD054680B9B2 -:1021400094F88C300F4856F83310FFF781F9284681 -:1021500002B0BDE8F0810135032DC8D10A48FFF770 -:102160004DFC2046FFF760FF94F88C30054656F88A -:10217000331008B90548E8E70548E6E76462000857 -:10218000D4240020CC61000842620008F6610008F7 -:10219000206200082DE9F041044688B00F4616463B -:1021A000FFF71CFF2046FFF70FFF0025C722A0689E -:1021B0004FF48073079503A9CDE90323CDE90555B5 -:1021C00003685B699847A0682A462946036800951A -:1021D000D3F810802B46C047054630B92046FFF79C -:1021E00011FF284608B0BDE8F081E36E3B60236F25 -:1021F0003360F3E72DE9F041002588B01F4604461F -:102200000E460E9B00F1400CA8463D60016B81B16B -:10221000A9420ED391420CD8B6FBF1FE01FB1E6120 -:1022200039B9416B90F82C800F9D1960816B296042 -:10223000154610306045E9D16DB3AB196269934220 -:1022400029D82046FFF7CAFE2046FFF7BDFE4FF40F -:102250001453A06803A9CDF80C80CDE904360026FC -:10226000CDE9066603685B699847A0683246314647 -:1022700003680096D3F810803346C04706462046D0 -:102280002EB9FFF7BFFE304608B0BDE8F081FFF77A -:10229000B9FE3D60F7E70026F5E700002DE9F041C3 -:1022A0008669054688B00F46B3429046B1FBF6FCFE -:1022B00006FB1C1C28BF3346A6EB0C0C00269C45D5 -:1022C000644628BF1C46FFF789FE2846FFF77CFEC0 -:1022D0000222A86803A9164B0597CDE90323CDE98F -:1022E000066603685B699847A8682246414603680A -:1022F00000961F693346B847064628462EB9FFF7B1 -:1023000081FE304608B0BDE8F081FFF77BFE0E9BF2 -:102310001C60AB6FAA696343B3FBF2F30F9A1360BF -:10232000EA6F109B5443AA69B4FBF2F41C60E8E71F -:10233000002500012DE9F04FCE18436989B004460D -:102340009E420F46904634D83D464FF00009DFF8D4 -:1023500070A0FFF739FEAE422FD9A6EB050B94F81B -:10236000803094F8882003A9BBF5806F039394F81C -:10237000813028BF4FF4806B012AA06808BF03F1A9 -:10238000FF33CDF810A0CDE90559079303685B69C9 -:102390009847A068A8EB07030268CDF800B02B446B -:1023A000D2F810B000221146D84718B105F5806563 -:1023B000D1E7002009B0BDE8F08F0120FAE700BFA7 -:1023C00000ED0003F0B5044689B00E46FFF7FCFDB2 -:1023D00094F887502DB194F889302BB1012B1AD085 -:1023E0000025284609B0F0BD94F880200392A5226C -:1023F0000692184ACDE9042394F88130A06803A915 -:10240000079303685B699847A068314603689B6936 -:1024100098470546E5E720460027FFF7DFFD204601 -:10242000FFF7D2FD08238122852120460097FFF780 -:1024300097FD054618B92046FFF7E4FDD1E794F86B -:10244000803006970393044BCDE9043794F881302C -:10245000013BD3E700ED000300ED0013014B024AFE -:102460001A607047D4240020B86100080368002176 -:102470001B68184703790BB1002202711846704798 -:1024800003685B681847000003791BB901230371D7 -:1024900018467047002070477047000010B504468A -:1024A00003F058FD204610BD30B50A44084D914256 -:1024B0000DD011F8013B5840082340F30004013BC4 -:1024C0002C4013F0FF0384EA5000F6D1EFE730BD53 -:1024D0002083B8ED38B5836904460D465B695A011F -:1024E00018D543681B682BB10522027098472378E2 -:1024F000052B01D102232370202383F311880021AF -:1025000004F1080001F00CFB002383F31188E0695B -:1025100002F02CFAEB0704D50248BDE8384001F080 -:1025200075BA38BDE462000810B5044C204600F0CE -:10253000FBFF034A0023C4E9062310BDDC2400206E -:102540000050005210B503780446012B17D1104BF0 -:10255000984214D102460F49102002F0D3F90E4BD5 -:10256000E061D3F8D42042F48042C3F8D420D3F8F9 -:10257000FC2042F48042C3F8FC20D3F8FC306268AF -:10258000A36992685A60054A1A601B22DA6010BD7E -:10259000DC240020D5240008004402581500020164 -:1025A00083691B2210B5DA60D1E9002012F44C4493 -:1025B00006D192011C61D86102F440425A6110BDFB -:1025C0000024104312F4406F1C61CC68DC61586138 -:1025D00001D08A689A619A689206FCD4EFE70000FD -:1025E00030B584691B25C069E5600068836104F12A -:1025F0002003C36102234261013A03614623C360A1 -:1026000016238362CB682261E361D1E9002313437F -:1026100012F4406F636101D08B68A361C36843F01B -:102620000103C36030BD000030B5D0E9064000684A -:1026300004F120058561C36108234261013A036109 -:1026400046230D69C36016238362CB682261E36170 -:10265000D1E90023134312F4406F43EA854343F06A -:102660008063636101D08B68A361C36843F0010399 -:10267000C36030BD8069036823F0040310B50360B4 -:1026800000230361C3618361D1E9004309692343E6 -:1026900043EA814343F04063436112B14FF010437A -:1026A000136010BD83691A6842F002021A601A684A -:1026B0009207FCD41A6842F004021A6070470000C6 -:1026C00083691B2210B5DA6004465B695B011AD48A -:1026D00043681B682BB10522027098472378052BAD -:1026E00001D102232370202383F31188002104F1F8 -:1026F000080001F015FA002383F31188E069BDE8B2 -:10270000104002F033B910BD026843681143016004 -:1027100003B1184770470000024A136843F0C00332 -:10272000136070470010014013B50E4C204600F0B6 -:102730008BFA04F1140000234FF400720A4900944C -:1027400000F04CF9094B4FF40072094904F13800CC -:10275000009400F0C5F9074A074BC4E9172302B0FB -:1027600010BD00BFFC2400206825002019270008A8 -:10277000682700200010014000E1F505037C30B51A -:10278000244C002918BF0C46012B11D1224B984232 -:102790000ED1224BD3F8F02042F01002C3F8F02003 -:1027A000D3F8182142F01002C3F81821D3F81831D9 -:1027B0002268036EC16D03EB52038466B3FBF2F330 -:1027C0006268150442BF23F0070503F0070343EADC -:1027D0004503CB60A36843F040034B60E36843F0DC -:1027E00001038B6042F4967343F001030B604FF0DA -:1027F000FF330B62510505D512F0102205D0B2F15E -:10280000805F04D080F8643030BD7F23FAE73F2337 -:10281000F8E700BFF4620008FC24002000440258DE -:102820002DE9F047C66D05463768F4692107346223 -:1028300019D014F0080118BF8021E20748BF41F009 -:102840002001A3074FF0200348BF41F0400160077B -:1028500048BF41F4807183F31188281DFFF754FFAE -:10286000002383F31188E2050AD5202383F311881E -:102870004FF40071281DFFF747FF002383F31188F1 -:102880004FF020094FF0000A14F0200838D13B0621 -:1028900016D54FF0200905F1380A200610D589F326 -:1028A0001188504600F050F9002836DA0821281D1A -:1028B000FFF72AFF27F080033360002383F311889A -:1028C000790614D5620612D5202383F31188D5E941 -:1028D00013239A4208D12B6C33B127F04007102103 -:1028E000281DFFF711FF3760002383F31188E306EB -:1028F00018D5AA6E1369ABB15069BDE8F047184707 -:1029000089F31188736A284695F86410194000F01D -:10291000B5F98AF31188F469B6E7B06288F31188D3 -:10292000F469BAE7BDE8F087F8B515468268044651 -:102930000B46AA4200D28568A1692669761AB5427B -:102940000BD218462A46FEF787FDA3692B44A361E4 -:102950002846A3685B1BA360F8BD0CD9AF1B1846C3 -:102960003246FEF779FD3A46E1683044FEF774FDE1 -:10297000E3683B44EBE718462A46FEF76DFDE36843 -:10298000E5E7000083689342F7B50446154600D298 -:102990008568D4E90460361AB5420BD22A46FEF7A0 -:1029A0005BFD63692B4463612846A3685B1BA360DE -:1029B00003B0F0BD0DD93246AF1B0191FEF74CFDBF -:1029C00001993A46E0683144FEF746FDE3683B442E -:1029D000E9E72A46FEF740FDE368E4E710B50A445C -:1029E0000024C361029B8460C16002610362C0E98C -:1029F0000000C0E9051110BD08B5D0E905329342C9 -:102A000001D1826882B98268013282605A1C4261B7 -:102A100019700021D0E904329A4224BFC36843618F -:102A200001F098F8002008BD4FF0FF30FBE70000F0 -:102A300070B5202304460E4683F31188A568A5B11E -:102A4000A368A269013BA360531CA36115782269A6 -:102A5000934224BFE368A361E3690BB12046984722 -:102A6000002383F31188284607E03146204601F011 -:102A700061F80028E2DA85F3118870BD2DE9F74F7F -:102A800004460E4617469846D0F81C904FF0200A90 -:102A90008AF311884FF0000B154665B12A4631467E -:102AA0002046FFF741FF034660B94146204601F04A -:102AB00041F80028F1D0002383F31188781B03B07C -:102AC000BDE8F08FB9F1000F03D001902046C84750 -:102AD000019B8BF31188ED1A1E448AF31188DCE701 -:102AE000C160C361009B82600362C0E905111144AB -:102AF000C0E9000001617047F8B504460D4616466E -:102B0000202383F31188A768A7B1A368013BA360C2 -:102B100063695A1C62611D70D4E904329A4224BF71 -:102B2000E3686361E3690BB120469847002080F3B6 -:102B3000118807E03146204600F0FCFF0028E2DA69 -:102B400087F31188F8BD0000D0E9052310B59A423B -:102B500001D182687AB982680021013282605A1CF0 -:102B600082611C7803699A4224BFC368836100F0C4 -:102B7000F1FF204610BD4FF0FF30FBE72DE9F74F86 -:102B800004460E4617469846D0F81C904FF0200A8F -:102B90008AF311884FF0000B154665B12A4631467D -:102BA0002046FFF7EFFE034660B94146204600F09D -:102BB000C1FF0028F1D0002383F31188781B03B0F4 -:102BC000BDE8F08FB9F1000F03D001902046C8474F -:102BD000019B8BF31188ED1A1E448AF31188DCE700 -:102BE000026843681143016003B118477047000051 -:102BF0001430FFF743BF00004FF0FF331430FFF7EE -:102C00003DBF00003830FFF7B9BF00004FF0FF3381 -:102C10003830FFF7B3BF00001430FFF709BF0000E2 -:102C20004FF0FF311430FFF703BF00003830FFF7DB -:102C300063BF00004FF0FF323830FFF75DBF000088 -:102C400000207047FFF770BD044B03600023436012 -:102C5000C0E9023301230374704700BF0C6300080E -:102C600010B52023044683F31188FFF787FD022364 -:102C70002374002383F3118810BD000038B5C369A5 -:102C800004460D461BB904210844FFF7A9FF294655 -:102C900004F11400FFF7B0FE002806DA201D4FF4FF -:102CA0008061BDE83840FFF79BBF38BD02684368CC -:102CB0001143016003B118477047000013B5406B22 -:102CC00000F58054D4F8A4381A681178042914D176 -:102CD000017C022911D11979012312898B401342F9 -:102CE0000BD101A94C3002F08DFCD4F8A448024667 -:102CF000019B2179206800F0DFF902B010BD0000CF -:102D0000143002F00FBC00004FF0FF33143002F01B -:102D100009BC00004C3002F0E1BC00004FF0FF3372 -:102D20004C3002F0DBBC0000143002F0DDBB0000D0 -:102D30004FF0FF31143002F0D7BB00004C3002F0EE -:102D4000ADBC00004FF0FF324C3002F0A7BC0000D9 -:102D50000020704710B500F58054D4F8A4381A68E4 -:102D60001178042917D1017C022914D15979012342 -:102D700052898B4013420ED1143002F06FFB024691 -:102D800048B1D4F8A4484FF4407361792068BDE895 -:102D9000104000F07FB910BD406BFFF7DBBF0000B3 -:102DA000704700007FB5124B0125042604460360DE -:102DB0000023057400F1840243602946C0E9023310 -:102DC0000C4B0290143001934FF44073009602F0C4 -:102DD00021FB094B04F69442294604F14C0002946D -:102DE000CDE900634FF4407302F0E8FB04B070BD1E -:102DF00034630008992D0008BD2C00080A682023C0 -:102E000083F311880B790B3342F823004B7913338A -:102E100042F823008B7913B10B3342F8230000F5FD -:102E20008053C3F8A41802230374002383F311888A -:102E30007047000038B5037F044613B190F8543052 -:102E4000ABB90125201D0221FFF730FF04F114006A -:102E50006FF00101257700F089FE04F14C0084F841 -:102E600054506FF00101BDE8384000F07FBE38BD1E -:102E700010B5012104460430FFF718FF0023237723 -:102E800084F8543010BD000038B5044600251430D5 -:102E900002F0D8FA04F14C00257702F0A7FB201DC0 -:102EA00084F854500121FFF701FF2046BDE8384067 -:102EB000FFF750BF90F8803003F06003202B06D15D -:102EC00090F881200023212A03D81F2A06D8002049 -:102ED0007047222AFBD1C0E91D3303E0034A426751 -:102EE00007228267C3670120704700BF2C220020A1 -:102EF00037B500F58055D5F8A4381A68117804293B -:102F00001AD1017C022917D11979012312898B402A -:102F1000134211D100F14C04204602F027FC58B1B5 -:102F200001A9204602F06EFBD5F8A4480246019B99 -:102F30002179206800F0C0F803B030BD01F10B0327 -:102F4000F0B550F8236085B004460D46FEB120234D -:102F500083F3118804EB8507301D0821FFF7A6FED7 -:102F6000FB6806F14C005B691B681BB1019002F025 -:102F700057FB019803A902F045FB024648B1039BA9 -:102F80002946204600F098F8002383F3118805B005 -:102F9000F0BDFB685A691268002AF5D01B8A013B14 -:102FA0001340F1D104F18002EAE70000133138B593 -:102FB00050F82140ECB1202383F3118804F58053AD -:102FC000D3F8A4281368527903EB8203DB689B696A -:102FD0005D6845B104216018FFF768FE294604F1D9 -:102FE000140002F045FA2046FFF7B4FE002383F3F5 -:102FF000118838BD7047000001F038BD0123402220 -:10300000002110B5044600F8303BFEF757FA0023C4 -:10301000C4E9013310BD000010B52023044683F33A -:1030200011882422416000210C30FEF747FA204627 -:1030300001F03EFD02232370002383F3118810BDAD -:1030400070B500EB8103054650690E461446DA6000 -:1030500018B110220021FEF731FAA06918B1102230 -:103060000021FEF72BFA31462846BDE8704001F0FA -:1030700031BE000083682022002103F0011310B547 -:10308000044683601030FEF719FA2046BDE8104070 -:1030900001F0ACBEF0B4012500EB810447898D40FE -:1030A000E4683D43A469458123600023A260636016 -:1030B000F0BC01F0C9BE0000F0B4012500EB8104B2 -:1030C00007898D40E4683D436469058123600023DE -:1030D000A2606360F0BC01F03FBF000070B5022346 -:1030E00000250446242203702946C0F888500C307D -:1030F00040F8045CFEF7E2F9204684F8705001F0D5 -:103100007DFD63681B6823B129462046BDE87040F9 -:10311000184770BD037880F88C300523037043682E -:103120001B6810B504460BB1042198470023A36027 -:1031300010BD000090F88C20436802701B680BB132 -:10314000052118477047000070B590F870300446AC -:1031500013B1002380F8703004F18002204601F0A2 -:1031600069FE63689B68B3B994F8803013F060051A -:1031700035D00021204602F013F90021204602F04C -:1031800003F963681B6813B106212046984706239C -:1031900084F8703070BD204698470028E4D0B4F819 -:1031A0008630A26F9A4288BFA36794F98030A56FDA -:1031B000002B4FF0200380F20381002D00F0F280FD -:1031C000092284F8702083F3118800212046D4E975 -:1031D0001D23FFF771FF002383F31188DAE794F8CA -:1031E000812003F07F0343EA022340F2023293423C -:1031F00000F0C58021D8B3F5807F48D00DD8012BD1 -:103200003FD0022B00F09380002BB2D104F1880252 -:1032100062670222A267E367C1E7B3F5817F00F02E -:103220009B80B3F5407FA4D194F88230012BA0D1CC -:10323000B4F8883043F0020332E0B3F5006F4DD0AC -:1032400017D8B3F5A06F31D0A3F5C063012B90D888 -:103250006368204694F882205E6894F88310B4F87E -:103260008430B047002884D0436863670368A3674D -:103270001AE0B3F5106F36D040F6024293427FF465 -:1032800078AF5C4B63670223A3670023C3E794F81E -:103290008230012B7FF46DAFB4F8883023F0020345 -:1032A000A4F88830C4E91D55E56778E7B4F88030A4 -:1032B000B3F5A06F0ED194F88230204684F88A309E -:1032C00001F0FAFC63681B6813B10121204698479E -:1032D000032323700023C4E91D339CE704F18B030F -:1032E00063670123C3E72378042B10D1202383F3E2 -:1032F00011882046FFF7BEFE85F31188032163681D -:1033000084F88B5021701B680BB12046984794F8C5 -:103310008230002BDED084F88B3004232370636866 -:103320001B68002BD6D0022120469847D2E794F89C -:10333000843020461D0603F00F010AD501F06CFD14 -:10334000012804D002287FF414AF2B4B9AE72B4BB3 -:1033500098E701F053FDF3E794F88230002B7FF4F7 -:1033600008AF94F8843013F00F01B3D01A0620464A -:1033700002D502F02DF8ADE702F01EF8AAE794F8A6 -:103380008230002B7FF4F5AE94F8843013F00F01F7 -:10339000A0D01B06204602D502F002F89AE701F001 -:1033A000F3FF97E7142284F8702083F311882B46EB -:1033B0002A4629462046FFF76DFE85F31188E9E687 -:1033C0005DB1152284F8702083F311880021204616 -:1033D000D4E91D23FFF75EFEFDE60B2284F8702082 -:1033E00083F311882B462A4629462046FFF764FEC0 -:1033F000E3E700BF646300085C63000860630008E3 -:1034000038B590F870300446002B3ED0063BDAB257 -:103410000F2A34D80F2B32D8DFE803F037313108C8 -:10342000223231313131313131313737856FB0F8B6 -:1034300086309D4214D2C3681B8AB5FBF3F203FBAE -:1034400012556DB9202383F311882B462A4629464D -:10345000FFF732FE85F311880A2384F870300EE0FE -:10346000142384F87030202383F31188002320462E -:103470001A461946FFF70EFE002383F3118838BD64 -:10348000C36F03B198470023E7E70021204601F00E -:1034900087FF0021204601F077FF63681B6813B1A6 -:1034A0000621204698470623D7E7000010B590F87C -:1034B00070300446142B29D017D8062B05D001D81C -:1034C0001BB110BD093B022BFBD80021204601F0A7 -:1034D00067FF0021204601F057FF63681B6813B1A6 -:1034E000062120469847062319E0152BE9D10B2326 -:1034F00080F87030202383F3118800231A46194680 -:10350000FFF7DAFD002383F31188DAE7C3689B69CC -:103510005B68002BD5D1C36F03B19847002384F8B3 -:103520007030CEE7FFF700B8012100230170C0E939 -:1035300001330C3000F008BA10B52023044683F3A1 -:1035400011884160FEF7FEFF02232370002383F3FE -:10355000118810BD10B52023044683F3118803237E -:1035600004F8083BFFF71CF84FF0FF31204600F04D -:10357000C9FA002383F31188C01A18BF012010BDB7 -:1035800038B50446202585F31188032504F8085B27 -:10359000FFF726F84FF0FF31204600F0B3FA002382 -:1035A00083F31188C01A18BF012038BD38B504460E -:1035B000202585F31188042504F8085BFFF734F80B -:1035C0004FF0FF31204600F09DFA002383F311886D -:1035D000C01A18BF012038BD10B52023044683F35C -:1035E0001188FFF747F806232370002383F311881F -:1035F00010BD000010B52023044683F31188FFF7A7 -:1036000051F802232370002383F3118810BD0000BA -:103610000C3000F0B5B900000C3000F0BBB9000070 -:1036200004480121044B03600023C0E901330C303E -:1036300000F08AB96829002021580008CB1D083AFB -:1036400023F00703591A521A012110B4D20800249A -:10365000C0E9004384600C301C605A605DF8044B84 -:1036600000F072B92DE9F84F364ECD1D0F460028F7 -:1036700018BF0646082A4FEAD50538BF082206F1CA -:103680000C08341D9146404600F07AF909F1070113 -:10369000C9F1000E224624686CB9404600F07AF960 -:1036A0003368CBB308224946E8009847044698B3EC -:1036B00040E9026730E004EB010CD4F804A00CEA06 -:1036C0000E0C0AF10100ACF1080304EBC0009842B3 -:1036D000E0D9A0EB0C0CB5EBEC0F4FEAEC0BD9D812 -:1036E0009C421CD204F10802AB45A3EB02024FEA54 -:1036F000E202626009D9691CED43206803EBC10254 -:103700005D44556043F8310022601C465F604046CE -:1037100044F8086B00F03EF92046BDE8F88FAA4552 -:10372000216802D111602346EFE7013504EBC503A0 -:1037300044F8351003F10801401AC01058601360B6 -:10374000F1E700BF68290020F8B550F8043C0446B2 -:1037500050F8085CA0F1080607332F1D0C35DB0874 -:1037600040F8043C284600F00BF93B46BB421A687F -:1037700001D09E4228D90AB1964225D244F8082C9D -:1037800054F8042C1E60013254F8081C06EBC200E9 -:10379000814206D14868024444F8042C0A6844F87F -:1037A000082C5868411C03EBC1018E4207D154F824 -:1037B000042C013202445A6054F8082C1A6028463E -:1037C000BDE8F84000F0E6B81346CFE7024B002210 -:1037D000C3E900339A607047802900200023826883 -:1037E0000374054B1B6899689142FBD25A680360C9 -:1037F00042601060586070478029002008B520237F -:1038000083F31188027C0023052A06D8DFE802F042 -:103810000B050503120E426913604FF0FF3343613D -:10382000FFF7DCFF002383F3118808BD426993682A -:1038300001339360D0E9003213605A60EDE7000075 -:10384000002382680374054B1B6899689142FBD87A -:103850005A680360426010605860704780290020F9 -:10386000054B196908741868026853601A6018617A -:1038700001230374FCF734BF802900204B1C30B5B2 -:10388000044687B00A4D10D02B6901A8094A00F000 -:1038900089F92046FFF7E4FF049B13B101A800F06B -:1038A000BDF92B69586907B030BDFFF7D9FFF8E7BC -:1038B00080290020FD37000838B50C4D04464161D1 -:1038C0002B6981689A68914203D8BDE83840FFF7B8 -:1038D00085BF1846FFF7B4FF01232C61014623740E -:1038E0002046BDE83840FCF7FBBE00BF8029002021 -:1038F000044B1A681B6990689B68984294BF00202B -:10390000012070478029002010B5084C23682069E9 -:103910001A6854602260012223611A74FFF790FF35 -:1039200001462069BDE81040FCF7DABE802900207E -:1039300008B5FFF7DDFF18B1BDE80840FFF7E4BFA9 -:1039400008BD0000FFF7E0BFC0E9000081607047DC -:103950008368013B002B10B583600DDA074C426889 -:103960002369586118605A60136043600520FFF7AF -:1039700077FF2369586910BD0020FCE780290020EB -:1039800008B5202383F31188FFF7E2FF002383F3B8 -:10399000118808BD08B5202383F31188836801339B -:1039A000002B836007DC036800211A680260506006 -:1039B0001846FFF781FF002383F3118808BD00003C -:1039C000FEE7000010B50E4CFFF700FF00F0DCF83A -:1039D00001F008FFFFF724FE80220A49204600F08C -:1039E00047F8012344F8180C037400F043FE002349 -:1039F00083F3118862B60448BDE8104000F058B85F -:103A0000A8290020686300087863000808B572B62A -:103A1000034B586200F0B0FC00F07EFDFEE700BFF3 -:103A20008029002000F034B9EFF3118020B9EFF3C2 -:103A30000583202282F311887047000010B530B949 -:103A4000EFF30584C4F3080414B180F3118810BDAA -:103A5000FFF76EFF84F31188F9E70000034A51680D -:103A600053685B1A9842FBD8704700BF001000E013 -:103A700082600222028270478368A3F17C0243F8CD -:103A80000C2C026943F83C2C426943F8382C074A55 -:103A900043F81C2CC268A3F1180043F8102C022232 -:103AA00003F8082C002203F8072C7047F9060008D9 -:103AB00010B5202383F31188FFF7DEFF00210446B1 -:103AC000FFF7FAFE002383F31188204610BD0000A3 -:103AD000024B1B6958610F20FFF7C2BE80290020EE -:103AE000202383F31188FFF7F3BF000008B50146D8 -:103AF000202383F311880820FFF7C0FE002383F3FF -:103B0000118808BD054B1B6921B103605861032072 -:103B1000FFF7B4BE4FF0FF30704700BF8029002090 -:103B200003682BB10022026018465961FFF756BEA8 -:103B30007047000049B1064B42681B6918605A6023 -:103B4000136043600420FFF799BE4FF0FF307047C9 -:103B5000802900200368984206D01A6802605060ED -:103B600018465961FFF73ABE7047000038B5044661 -:103B70000D462068844200D138BD036823605C6034 -:103B80004561FFF72BFEF4E7054B4FF0FF3103F1E2 -:103B90001402C3E905220022C3E90712704700BFDF -:103BA0008029002070B51C4E05460C46C0E9032351 -:103BB00001F012FE334653F8142F9A420DD13062B1 -:103BC0000A2C2CBF00190A302A60C5E90124C6E975 -:103BD0000555BDE8704001F0E9BD316A431AE318AC -:103BE00038BF1C469368A34202D9081901F0EEFDC4 -:103BF00073699A6894420CD85A68AC602B606A600A -:103C000015609A685D60121B9A604FF0FF33F36194 -:103C100070BDA41A1B68ECE78029002038B51B4C46 -:103C2000636998420DD08168D0E9003213605A6010 -:103C30000022C2609A680A449A604FF0FF33E36141 -:103C400038BD03682246002142F8143F93425A606F -:103C5000C16003D1BDE8384001F0B2BD9A68816807 -:103C6000256A0A449A6001F0B7FD6369411B9A68AE -:103C70008A42E5D9AB181D1A206A092D98BF01F1B7 -:103C80000A02BDE83840104401F0A0BD80290020A0 -:103C90002DE9F041184C002704F11406656901F084 -:103CA0009BFD236AAA68C11A8A4215D81344D5F825 -:103CB0000C802362D5E9003213605A606369EF60BB -:103CC000B34201D101F07CFD87F311882869C04718 -:103CD000202383F31188E1E76169B14209D01344DD -:103CE0001B1ABDE8F0410A2B2CBFC0180A3001F0A6 -:103CF0006DBDBDE8F08100BF802900200020704725 -:103D0000FEE70000704700004FF0FF3070470000F2 -:103D100002290CD0032904D00129074818BF00202C -:103D20007047032A05D8054800EBC20070470448D5 -:103D300070470020704700BF5C6400083C220020F0 -:103D40001064000870B59AB005460846144601A9EB -:103D500000F0C2F801A8FDF7A9FB431C0022C6B27F -:103D60005B001046C5E9003423700323023404F8D5 -:103D7000013C01ABD1B202348E4201D81AB070BD01 -:103D800013F8011B013204F8010C04F8021CF1E7DE -:103D900008B5202383F311880348FFF7A9F800230F -:103DA00083F3118808BD00BF382B002090F88030C5 -:103DB00003F01F02012A07D190F881200B2A03D1BA -:103DC0000023C0E91D3315E003F06003202B08D168 -:103DD000B0F884302BB990F88120212A03D81F2A0B -:103DE00004D8FFF767B8222AEBD0FAE7034A426704 -:103DF00007228267C3670120704700BF332200207B -:103E000007B5052917D8DFE801F01916031919209D -:103E1000202383F31188104A01210190FFF710F944 -:103E2000019802210D4AFFF70BF90D48FFF72CF816 -:103E3000002383F3118803B05DF804FB202383F390 -:103E400011880748FEF7F6FFF2E7202383F3118875 -:103E50000348FFF70DF8EBE7B0630008D4630008F0 -:103E6000382B002038B50C4D0C4C2A460C4904F177 -:103E70000800FFF767FF05F1CA0204F110000949C5 -:103E8000FFF760FF05F5CA7204F118000649BDE8A6 -:103E90003840FFF757BF00BF104400203C220020ED -:103EA000906300089A630008A563000870B5044693 -:103EB00008460D46FDF7FAFAC6B2204601340378EB -:103EC0000BB9184670BD32462946FDF7CFFA0028D7 -:103ED000F3D10120F6E700002DE9F84F05460C4626 -:103EE000FDF7E4FA2C49C6B22846FFF7DFFF08B118 -:103EF0000336F6B229492846FFF7D8FF08B1103635 -:103F0000F6B2632E0DD8DFF89080DFF89090244F42 -:103F1000DFF894A0DFF894B02E7846B92670BDE89B -:103F2000F88F29462046BDE8F84F02F09BB8252EB1 -:103F30002ED1072241462846FDF798FA70B9DBF8E2 -:103F4000003007350A3444F80A3CDBF8043044F802 -:103F5000063CBBF8083024F8023CDDE7082249465D -:103F60002846FDF783FA98B9A21C0E4B1978023245 -:103F70000909C95D02F8041C13F8011B01F00F01C7 -:103F80005345C95D02F8031CF0D118340835C3E766 -:103F9000013504F8016BBFE77C640008A5630008E5 -:103FA0008F64000800E8F11F0CE8F11F846400082A -:103FB000BFF34F8F044B1A695107FCD1D3F810217E -:103FC0005207F8D1704700BF0020005208B50D4BD2 -:103FD0001B78ABB9FFF7ECFF0B4BDA68D10704D5C0 -:103FE0000A4A5A6002F188325A60D3F80C21D2078B -:103FF00006D5064AC3F8042102F18832C3F8042129 -:1040000008BD00BF6E460020002000522301674516 -:104010004FF00063054A1968013104D1043393421B -:10402000F9D1012070470020704700BF000002084E -:1040300008B5114B1B78F3B9104B1A69510703D51A -:10404000DA6842F04002DA60D3F81021520705D551 -:10405000D3F80C2142F04002C3F80C21FFF7A8FF6F -:10406000064BDA6842F00102DA60D3F80C2142F024 -:104070000102C3F80C2108BD6E460020002000524A -:104080004FF40030704700000120704708B9FFF777 -:10409000BFBF00207047000008B518BB1248FFF7EB -:1040A00087FFFFF793FF4FF0FF334361C0F81431F0 -:1040B0000361FFF77DFF2423C360C36843F08003DF -:1040C000C36003695B07FCD4FFF772FF4FF0006029 -:1040D0004FF4003100F0FCF9FFF7AAFFBDE80840FB -:1040E000FFF796BF002008BD002000522DE9F84FD1 -:1040F00005460C46104645EA0203DB065DD122F078 -:1041000003022B462A44934209D120F01F00DFF816 -:10411000BCB0DFF8BCA0FFF759FF271844E01968CE -:1041200001314AD10433EEE705F1784324492548AB -:10413000B3F5801F244B38BF184603F1F80339BF8D -:104140008846D9469846D146FFF732FF4FF0FF33F5 -:10415000A5EB040C04F11C010360D8F8003043F017 -:104160000203C8F80030231FD9F8006016F00506D6 -:10417000FAD153F8042F99424CF80320F4D1BFF33D -:104180004F8FFFF715FF4FF0FF33202221460360CA -:104190002846D8F8003023F00203C8F8003001F0B8 -:1041A0000DFF40B920352034BC42BDD10120FFF7BE -:1041B0003FFFBDE8F88F3046F9E70020F9E700BF80 -:1041C0000C200052142100521420005210200052E2 -:1041D0001021005210B5084C237828B11BB9FFF705 -:1041E000F5FE0123237010BD002BFCD02070BDE82C -:1041F0001040FFF71DBF00BF6E4600202DE9F74FAE -:104200000E460546002864D011F0050763D139B980 -:10421000082229463046FFF725FA0446002848D1EF -:1042200008224FF00109DFF8C08006F00403DFF830 -:10423000BCA006EA090BBBF1000F27D0D8F8141078 -:10424000C80723D409F1010908F10C08B9F1060FD8 -:10425000F1D18FB9224E2946F0190092FFF702FAE8 -:10426000044630BB1837009A782FF4D12946FFF75F -:10427000F9F90446E8B9009A29461A48FFF7F2F915 -:10428000044608BB204603B0BDE8F08FABB1D8F8B8 -:10429000141011F0040FD5D01820294600FB09A0F6 -:1042A000CDE90023FFF7DEF90446DDE9002300280D -:1042B000C8D02A460021204608E0B107ECD5D8F83E -:1042C000141011F0020FE6E72A460021FDF7F6F878 -:1042D000D8E70446D6E71F35202225F01F05A0E7C2 -:1042E000A046002070460020A064000888460020F8 -:1042F0000021FFF783BF00000121FFF77FBF00000F -:10430000F8B5144D01241827134E40F2FF32002156 -:104310000120FDF7D3F807FB046001342A6955F842 -:104320000C1FFFF78BF9062CF5D137254FF4C0543D -:104330002046FFF7E1FF014628B122460748BDE8C5 -:10434000F840FFF77BB9C4EBC404013D4FEAD40445 -:10435000EED1F8BDA0640008884600207046002019 -:104360000421FFF74BBF00004843FFF7C1BF000027 -:1043700008B1FFF7E9B9704738B5054D002403349B -:10438000696855F80C0B00F0B5F8122CF7D138BD60 -:10439000A064000870B5104E82B0FFF745FB0546DB -:1043A00001F01AFA326803469042336037BF0B4A75 -:1043B0000A495168146836BF0131D1E900415160A2 -:1043C0000419284641F100010191FFF737FB20460F -:1043D000019902B070BD00BF18470020204700209F -:1043E00070B5124E82B0FFF71FFB054601F0F4F9DD -:1043F000326803469042336037BF0D4A0C4951681A -:10440000146836BF0131D1E90041516004192846D2 -:1044100041F100010191FFF711FB4FF47A72002383 -:1044200020460199FBF744FF02B070BD18470020F9 -:10443000204700200244074BD2B210B5904200D171 -:1044400010BD441C00B253F8200041F8040BE0B248 -:10445000F4E700BF504000580E4B30B51C6F2404E9 -:1044600005D41C6F1C671C6F44F400441C670A4C85 -:1044700002442368D2B243F480732360074B904216 -:1044800000D130BD441C51F8045B00B243F8205009 -:10449000E0B2F4E700440258004802585040005887 -:1044A00007B5012201A90020FFF7C4FF019803B05E -:1044B0005DF804FB13B50446FFF7F2FFA04205D0F8 -:1044C000012201A900200194FFF7C6FF02B010BD30 -:1044D0000144BFF34F8F064B884204D3BFF34F8F85 -:1044E000BFF36F8F7047C3F85C022030F4E700BF62 -:1044F00000ED00E00144BFF34F8F064B884204D328 -:10450000BFF34F8FBFF36F8F7047C3F87002203037 -:10451000F4E700BF00ED00E07047000070B505460D -:1045200016460C4601201021FFF71EFF2860467337 -:104530003CB1204636B1FFF713FF2B68186000B17D -:104540009C6070BDFFF7D8FEF7E7000070B50E461F -:104550001546044600B30B6843608368934210D24B -:1045600013B10068FFF704FF637B28462BB1FFF708 -:10457000F7FE206020B9A06070BDFFF7BDFEF8E730 -:10458000A560206805F11F01306021F01F01FFF7D1 -:104590009FFF01202073EFE70120EDE710B50446EF -:1045A00040B10068884205D1606808B1FCF754FF4B -:1045B0000023237310BD000070B50E461546044657 -:1045C00020B38368934210D213B10068FFF7D0FE86 -:1045D000637B28462BB1FFF7C3FE206020B9A060A3 -:1045E00070BDFFF789FEF8E7A560316819B12A466A -:1045F0002068FCF731FF206805F11F01306021F0D1 -:104600001F01FFF777FF01202073E9E70120E7E7AB -:1046100020B103688B4204BF00230373704700007E -:1046200008B1002303737047034B1A681AB9034A91 -:10463000D2F8D0241A607047284700200040025862 -:1046400008B5FFF7F1FF024B1868C0F3806008BDA2 -:1046500028470020EFF30983054968334A6B22F0AD -:1046600001024A6383F30988002383F311887047AA -:1046700000EF00E0202080F3118862B60D4B0E4A57 -:10468000D96821F4E0610904090C0A430B49DA6096 -:10469000D3F8FC2042F08072C3F8FC20084AC2F82C -:1046A000B01F116841F0010111601022DA7783F820 -:1046B0002200704700ED00E00003FA0555CEACC5BE -:1046C000001000E0202310B583F311880E4B5B68C7 -:1046D00013F4006314D0F1EE103AEFF309844FF0B5 -:1046E0008073683CE361094BDB6B236684F30988C4 -:1046F000FFF7FEF810B1064BA36110BD054BFBE7B9 -:1047000083F31188F9E700BF00ED00E000EF00E05F -:104710000B0700080E07000870B5BFF34F8FBFF3FB -:104720006F8F1A4A0021C2F85012BFF34F8FBFF3A8 -:104730006F8F536943F400335361BFF34F8FBFF35F -:104740006F8FC2F88410BFF34F8FD2F8803043F6DA -:10475000E074C3F3C900C3F34E335B0103EA0406FC -:10476000014646EA81750139C2F86052F9D2203B10 -:1047700013F1200FF2D1BFF34F8F536943F480330D -:104780005361BFF34F8FBFF36F8F70BD00ED00E03B -:10479000FEE700000A4B0B480B4A90420BD30B4B31 -:1047A000C11EDA1C121A22F003028B4238BF00220B -:1047B0000021FCF783BE53F8041B40F8041BECE710 -:1047C00060670008D0490020D0490020D04900206F -:1047D0007047000003681968596043680BB180682E -:1047E000184770474FF0A44310B51C68E00702D586 -:1047F0002848FFF7EFFFA10702D52748FFF7EAFF98 -:10480000620702D52548FFF7E5FF230702D52448B4 -:10481000FFF7E0FFE00602D52248FFF7DBFFA10625 -:1048200002D52148FFF7D6FF620602D51F48FFF7E1 -:10483000D1FF230602D51E48FFF7CCFFE00502D5C5 -:104840001C48FFF7C7FFA10502D51B48FFF7C2FFB1 -:10485000620502D51948FFF7BDFF230502D51848A8 -:10486000FFF7B8FFE00402D51648FFF7B3FFA10435 -:1048700002D51548FFF7AEFF620402D51348FFF7D3 -:10488000A9FF230402D51248FFF7A4FFBDE810409A -:10489000FFF718BF304700203C4700204847002062 -:1048A00054470020604700206C47002078470020D4 -:1048B00084470020904700209C470020A847002004 -:1048C000B4470020C0470020CC470020D847002034 -:1048D000E447002030B5094B0021094C1020072582 -:1048E000196054F8042B0138C3E90121D16003F1A8 -:1048F0000C0311615560F4D130BD00BF2C4700207E -:10490000E86400080F28F0B50AD9102806D10F2353 -:104910000020144C01272568984203D900201DE08F -:104920000346F6E707FA00F6354218D10C23354363 -:1049300043432560181D2344C3E902120A4B204457 -:10494000D3F8D42042F00102C3F8D420D3F8FC20DD -:1049500042F00102C3F8FC20D3F8FC30F0BD013076 -:10496000DAE700BF2C470020004402580368D968EA -:10497000DA68C90722F03F02DA6002D51A689207A6 -:10498000FCD507225A60704770B5D0E92443002255 -:104990004FF0FF359E6804EB42135101D3F8000934 -:1049A000002805DAD3F8000940F08040C3F8000978 -:1049B000D3F8000B002805DAD3F8000B40F0804054 -:1049C000C3F8000B013263189642C3F80859C3F8C4 -:1049D000085BE0D24FF00113C4F81C3870BD000032 -:1049E00000EB8103D3F80CC02DE9F043DCF8142070 -:1049F0004E1CD0F89050D2F800E005EB063605EBDF -:104A00004118506870450AD30122D5F8343802FAAB -:104A100001F123EA0101C5F83418BDE8F083AEEBDB -:104A20000003BCF81040A34228BF2346D8F8184919 -:104A3000A4B2B3EB840FF0D89468A4F1040959F838 -:104A4000047F3760A4EB09071F44042FF7D81C44E8 -:104A5000034494605360D4E7890141F0200101616F -:104A600003699B06FCD41220FEF7F8BF10B50A4C70 -:104A70002046FEF7C3FA094BC4F89030084BC4F83F -:104A80009430084C2046FEF7B9FA074BC4F8903032 -:104A9000064BC4F8943010BDF047002000000840D9 -:104AA0004C6500088C480020000004405865000850 -:104AB00070B503780546012B5DD1494BD0F8904085 -:104AC000984259D1474B0E216520D3F8D82042F0A7 -:104AD0000062C3F8D820D3F8002142F00062C3F886 -:104AE0000021D3F80021D3F8802042F00062C3F8FF -:104AF0008020D3F8802022F00062C3F88020D3F811 -:104B0000803000F071FC384BE360384BC4F800385B -:104B10000023D5F89060C4F8003EC02323604FF412 -:104B20000413A3633369002BFCDA01230C203361E7 -:104B3000FEF794FF3369DB07FCD41220FEF78EFFEB -:104B40003369002BFCDA00262846A660FFF71CFF1D -:104B50006B68C4F81068DB68C4F81468C4F81C6893 -:104B6000002B3AD1224BA3614FF0FF336361A3685E -:104B700043F00103A36070BD1E4B9842C8D1194B8E -:104B80000E214D20D3F8D82042F00072C3F8D8206F -:104B9000D3F8002142F00072C3F80021D3F80021BD -:104BA000D3F8802042F00072C3F88020D3F8802030 -:104BB00022F00072C3F88020D3F88020D3F8D820E8 -:104BC00022F08062C3F8D820D3F8002122F080625E -:104BD000C3F80021D3F8003193E7074BC3E700BFC8 -:104BE000F047002000440258401400400300200217 -:104BF000003C30C08C480020083C30C0F8B5D0F8EC -:104C00009040054600214FF000662046FFF724FF44 -:104C1000D5F8941000234FF001128F684FF0FF3049 -:104C2000C4F83438C4F81C2804EB431201339F4203 -:104C3000C2F80069C2F8006BC2F80809C2F8080B94 -:104C4000F2D20B68D5F89020C5F898306362102333 -:104C50001361166916F01006FBD11220FEF7FEFE56 -:104C6000D4F8003823F4FE63C4F80038A36943F491 -:104C7000402343F01003A3610923C4F81038C4F89B -:104C800014380B4BEB604FF0C043C4F8103B094B9A -:104C9000C4F8003BC4F81069C4F80039D5F898305E -:104CA00003F1100243F48013C5F89820A362F8BD05 -:104CB0002865000840800010D0F8902090F88A10F5 -:104CC000D2F8003823F4FE6343EA0113C2F8003837 -:104CD000704700002DE9F84300EB8103D0F89050B5 -:104CE0000C468046DA680FFA81F94801166806F02A -:104CF0000306731E022B05EB41134FF0000194BF16 -:104D0000B604384EC3F8101B4FF0010104F1100334 -:104D100098BF06F1805601FA03F3916998BF06F532 -:104D2000004600293AD0578A04F158013743490117 -:104D30006F50D5F81C180B430021C5F81C382B18F0 -:104D40000127C3F81019A7405369611E9BB3138A4A -:104D5000928B9B08012A88BF5343D8F89820981853 -:104D600042EA034301F140022146C8F89800284670 -:104D700005EB82025360FFF76FFE08EB8900C36802 -:104D80001B8A43EA845348341E4364012E51D5F8EC -:104D90001C381F43C5F81C78BDE8F88305EB49179C -:104DA000D7F8001B21F40041C7F8001BD5F81C18E8 -:104DB00021EA0303C0E704F13F030B4A28462146DA -:104DC00005EB83035A60FFF747FE05EB4910D0F867 -:104DD000003923F40043C0F80039D5F81C3823EA21 -:104DE0000707D7E70080001000040002D0F89420E5 -:104DF0001268C0F89820FFF7C7BD00005831D0F8FE -:104E0000903049015B5813F4004004D013F4001FA4 -:104E10000CBF0220012070474831D0F89030490182 -:104E20005B5813F4004004D013F4001F0CBF0220A1 -:104E30000120704700EB8101CB68196A0B68136091 -:104E40004B6853607047000000EB810330B5DD68AC -:104E5000AA691368D36019B9402B84BF402313603B -:104E60006B8A1468D0F890201C4402EB4110013C7E -:104E700009B2B4FBF3F46343033323F0030343EABF -:104E8000C44343F0C043C0F8103B2B6803F0030356 -:104E9000012B0ED1D2F8083802EB411013F4807FB9 -:104EA000D0F8003B14BF43F0805343F00053C0F8E8 -:104EB000003B02EB4112D2F8003B43F00443C2F83E -:104EC000003B30BD2DE9F041D0F8906005460C461E -:104ED00006EB4113D3F8087B3A07C3F8087B08D5E3 -:104EE000D6F814381B0704D500EB8103DB685B6838 -:104EF0009847FA071FD5D6F81438DB071BD505EB02 -:104F00008403D968CCB98B69488A5A68B2FBF0F639 -:104F100000FB16228AB91868DA6890420DD2121A7C -:104F2000C3E90024202383F3118821462846FFF794 -:104F30008BFF84F31188BDE8F081012303FA04F2AA -:104F40006B8923EA02036B81CB68002BF3D02146E7 -:104F50002846BDE8F041184700EB81034A0170B5CF -:104F6000DD68D0F890306C692668E66056BB1A445C -:104F70004FF40020C2F810092A6802F00302012A47 -:104F80000AB20ED1D3F8080803EB421410F4807F64 -:104F9000D4F8000914BF40F0805040F00050C4F82D -:104FA000000903EB4212D2F8000940F00440C2F8B5 -:104FB00000090122D3F8340802FA01F10143C3F8D1 -:104FC000341870BD19B9402E84BF4020206020687D -:104FD0001A442E8A8419013CB4FBF6F440EAC4401A -:104FE00040F00050C6E700002DE9F041D0F8906095 -:104FF00004460D4606EB4113D3F80879C3F8087947 -:10500000FB071CD5D6F81038DA0718D500EB81035A -:10501000D3F80CC0DCF81430D3F800E0DA68964519 -:105020001BD2A2EB0E024FF000081A60C3F80480F6 -:10503000202383F31188FFF78FFF88F311883B0645 -:1050400018D50123D6F83428AB40134212D0294694 -:105050002046BDE8F041FFF7C3BC012303FA01F28B -:10506000038923EA02030381DCF80830002BE6D031 -:105070009847E4E7BDE8F0812DE9F84FD0F890506B -:1050800004466E69AB691E4016F480586E6103D009 -:10509000BDE8F84FFEF722B8002E12DAD5F8003E30 -:1050A0009F0705D0D5F8003E23F00303C5F8003E66 -:1050B000D5F80438204623F00103C5F80438FEF77C -:1050C00039F8300505D52046FFF75EFC2046FEF78F -:1050D00021F8B1040CD5D5F8083813F0060FEB68A9 -:1050E00023F470530CBF43F4105343F4A053EB600C -:1050F000320704D56368DB680BB120469847F3029A -:1051000000F1BA80B70226D5D4F8909000274FF06E -:10511000010A09EB4712D2F8003B03F44023B3F530 -:10512000802F11D1D2F8003B002B0DDA62890AFAE8 -:1051300007F322EA0303638104EB8703DB68DB6880 -:1051400013B13946204698470137D4F89430FFB25E -:105150009B689F42DDD9F00619D5D4F89000026A09 -:10516000C2F30A1702F00F0302F4F012B2F5802F17 -:1051700000F0CC80B2F5402F09D104EB830300226C -:1051800000F58050DB681B6A974240F0B280300324 -:10519000D5F8185835D5E90303D500212046FFF787 -:1051A00091FEAA0303D501212046FFF78BFE6B0376 -:1051B00003D502212046FFF785FE2F0303D50321E7 -:1051C0002046FFF77FFEE80203D504212046FFF7C3 -:1051D00079FEA90203D505212046FFF773FE6A0276 -:1051E00003D506212046FFF76DFE2B0203D50721CC -:1051F0002046FFF767FEEF0103D508212046FFF7A1 -:1052000061FE700340F1A980E90703D50021204623 -:10521000FFF7EAFEAA0703D501212046FFF7E4FEC7 -:105220006B0703D502212046FFF7DEFE2F0703D5CB -:1052300003212046FFF7D8FEEE0603D504212046C1 -:10524000FFF7D2FEA80603D505212046FFF7CCFEC6 -:10525000690603D506212046FFF7C6FE2A0603D5B8 -:1052600007212046FFF7C0FEEB0576D52046082132 -:10527000BDE8F84FFFF7B8BED4F8909000274FF084 -:10528000010AD4F894305FFA87FB9B689B453FF690 -:1052900039AF09EB4B13D3F8002902F44022B2F5E1 -:1052A000802F24D1D3F80029002A20DAD3F800294E -:1052B00042F09042C3F80029D3F80029002AFBDB12 -:1052C0005946D4F89000FFF7C7FB22890AFA0BF37E -:1052D00022EA0303238104EB8B03DB689B6813B191 -:1052E00059462046984759462046FFF779FB013733 -:1052F000C7E7910701D1D0F80080072A02F1010227 -:105300009CBF03F8018B4FEA18283DE704EB8303A9 -:1053100000F58050DA68D2F818C0DCF80820DCE923 -:10532000001CA1EB0C0C00218F4208D1DB689B69AB -:105330009A683A449A605A683A445A6027E711F0EA -:10534000030F01D1D0F800808C4501F1010184BF29 -:1053500002F8018B4FEA1828E6E7BDE8F88F000055 -:1053600008B50348FFF788FEBDE80840FFF7AAB973 -:10537000F047002008B50348FFF77EFEBDE808406F -:10538000FFF7A0B98C480020D0F8903003EB411112 -:10539000D1F8003B43F40013C1F8003B7047000014 -:1053A000D0F8903003EB4111D1F8003943F40013E9 -:1053B000C1F8003970470000D0F8903003EB41117C -:1053C000D1F8003B23F40013C1F8003B7047000004 -:1053D000D0F8903003EB4111D1F8003923F40013D9 -:1053E000C1F8003970470000090100F16043012253 -:1053F00003F56143C9B283F8001300F01F039A401C -:1054000043099B0003F1604303F56143C3F8802126 -:105410001A60704730B50433039C0172002104FB0D -:105420000325C160C0E90653049B0363059BC0E9E3 -:105430000000C0E90422C0E90842C0E90A11436340 -:1054400030BD00000022416AC260C0E90411C0E919 -:105450000A226FF00101FEF789BB0000D0E9043297 -:10546000934201D1C2680AB9181D704700207047E5 -:10547000036919600021C2680132C260C269134425 -:1054800082699342036124BF436A0361FEF762BBF2 -:1054900038B504460D46E3683BB162690020131D30 -:1054A0001268A3621344E36207E0237A33B9294602 -:1054B0002046FEF73FFB0028EDDA38BD6FF0010013 -:1054C000FBE70000C368C269013BC3604369134442 -:1054D00082699342436124BF436A4361002383622C -:1054E000036B03B11847704770B52023044683F35C -:1054F0001188866A3EB9FFF7CBFF054618B186F3DF -:105500001188284670BDA36AE26A13F8015B9342D2 -:10551000A36202D32046FFF7D5FF002383F311884F -:10552000EFE700002DE9F84F04460E46174698466F -:105530004FF0200989F311880025AA46D4F828B035 -:10554000BBF1000F09D141462046FFF7A1FF20B172 -:105550008BF311882846BDE8F88FD4E90A12A7EB2F -:10556000050B521A934528BF9346BBF1400F1BD938 -:10557000334601F1400251F8040B914243F8040B09 -:10558000F9D1A36A403640354033A362D4E90A23F7 -:105590009A4202D32046FFF795FF8AF31188BD4255 -:1055A000D8D289F31188C9E730465A46FBF754FF31 -:1055B000A36A5E445D445B44A362E7E710B5029CC6 -:1055C0000433017204FB0321C460C0E90613002305 -:1055D000C0E90A33039B0363049BC0E90000C0E9F0 -:1055E0000422C0E90842436310BD0000026A6FF064 -:1055F0000101C260426AC0E904220022C0E90A2215 -:10560000FEF7B4BAD0E904239A4201D1C26822B9A4 -:10561000184650F8043B0B607047002070470000AC -:10562000C3680021C2690133C360436913448269BE -:105630009342436124BF436A4361FEF78BBA000083 -:1056400038B504460D46E3683BB1236900201A1DB6 -:10565000A262E2691344E36207E0237A33B9294680 -:105660002046FEF767FA0028EDDA38BD6FF001003A -:10567000FBE7000003691960C268013AC260C269B1 -:10568000134482699342036124BF436A0361002388 -:105690008362036B03B118477047000070B5202385 -:1056A0000D460446114683F31188866A2EB9FFF72A -:1056B000C7FF10B186F3118870BDA36A1D70A36A7D -:1056C000E26A01339342A36204D3E16920460439BC -:1056D000FFF7D0FF002080F31188EDE72DE9F84FA8 -:1056E00004460D46904699464FF0200A8AF31188E9 -:1056F0000026B346A76A4FB949462046FFF7A0FFE8 -:1057000020B187F311883046BDE8F88FD4E90A0745 -:105710003A1AA8EB0607974228BF1746402F1BD915 -:1057200005F1400355F8042B9D4240F8042BF9D1B4 -:10573000A36A40364033A362D4E90A239A4204D3D1 -:10574000E16920460439FFF795FF8BF31188464540 -:10575000D9D28AF31188CDE729463A46FBF77CFE79 -:10576000A36A3D443E443B44A362E5E7D0E90423F9 -:105770009A4217D1C3689BB1836A8BB1043B9B1AD1 -:105780000ED01360C368013BC360C3691A448369C8 -:105790009A42026124BF436A0361002383620123AA -:1057A000184670470023FBE700F0EAB8034B0022DD -:1057B00058631A610222DA60704700BF000C004093 -:1057C000014B0022DA607047000C0040014B586327 -:1057D000704700BF000C0040014B586A704700BF83 -:1057E000000C0040024B034A1A60034A5A6070479B -:1057F00040490020D049002000000220074B4942C8 -:1058000010B55C68201A08401968821A914203D8C2 -:10581000944201D35A6010BD0020FCE740490020AB -:1058200008B5202383F31188FFF7E8FF002383F3F3 -:10583000118808BD4B6843608B688360CB68C36088 -:105840000B6943614B6903628B6943620B680360B8 -:105850007047000008B52F4B40F2FF70D3F8E010FE -:105860000143C3F8E010D3F8082102432A48C3F8E3 -:1058700008212A4AD3F808311146FFF7DBFF00F56B -:10588000806002F11C01FFF7D5FF00F5806002F196 -:105890003801FFF7CFFF00F5806002F15401FFF7F8 -:1058A000C9FF00F5806002F17001FFF7C3FF00F54A -:1058B000806002F18C01FFF7BDFF00F5806002F10E -:1058C000A801FFF7B7FF00F5806002F1C401FFF700 -:1058D000B1FF00F5806002F1E001FFF7ABFF00F5DA -:1058E000806002F1FC01FFF7A5FF02F58C7100F565 -:1058F0008060FFF79FFF00F0FBF8094B0522C3F81B -:1059000098204FF06052C3F89C20064AC3F8A020AC -:1059100008BD00BF0044025800000258646500083A -:1059200000ED00E01F00080308B500F0EBFAFEF7F9 -:1059300049F8104BD3F8DC2042F04002C3F8DC20D9 -:10594000D3F8042122F04002C3F80421D3F8043133 -:10595000094B1A6842F008021A601A6842F0040201 -:105960001A60FEF761FEFEF7CBFCBDE80840FEF7CB -:1059700079BA00BF0044025800180248704700007E -:10598000114BD3F8E82042F00802C3F8E820D3F81E -:10599000102142F00802C3F810210C4AD3F810314C -:1059A000D36B43F00803D363C722094B9A624FF0CD -:1059B000FF32DA6200229A615A63DA605A60012289 -:1059C0005A611A60704700BF004402580010005C22 -:1059D000000C0040094A08B51169D3680B40D9B2E0 -:1059E0009B076FEA0101116107D5202383F311881A -:1059F000FEF718F8002383F3118808BD000C00405F -:105A000008B54FF0FF31394BD3F88020C3F8801030 -:105A1000D3F880200022C3F88020D3F88000D3F888 -:105A20008400C3F88410D3F88400C3F88420D3F82A -:105A30008400D86F40F0FF4040F4FF0040F43F5036 -:105A400040F03F00D867D86F20F0FF4020F4FF00FF -:105A500020F43F5020F03F00D867D86FD3F888007B -:105A60006FEA40506FEA5050C3F88800D3F88800BE -:105A7000C0F30A00C3F88800D3F88800D3F8900078 -:105A8000C3F89010D3F89000C3F89020D3F890009A -:105A9000D3F89400C3F89410D3F89400C3F894207A -:105AA000D3F89400D3F89800C3F89810D3F898006E -:105AB000C3F89820D3F89800D3F88C00C3F88C1062 -:105AC000D3F88C00C3F88C20D3F88C00D3F89C005A -:105AD000C3F89C10D3F89C10C3F89C20D3F89C30DA -:105AE000FEF7F8FEBDE8084000F0DCB900440258BB -:105AF000614B0122C3F80821604BD3F8F42042F037 -:105B00000202C3F8F420D3F81C2142F00202C3F8C9 -:105B10001C210222D3F81C31594BDA605A689104D7 -:105B2000FCD5584A1A6001229A60574ADA6000226E -:105B30001A614FF440429A61514B9A699204FCD524 -:105B40001A6842F480721A604C4B1A6F12F4407F4C -:105B500004D04FF480321A6700221A671A6842F0A4 -:105B600001021A60454B1A685007FCD500221A61E1 -:105B70001A6912F03802FBD1012119604FF08041FF -:105B800059605A67414ADA62414A1A611A6842F416 -:105B900080321A60394B1A689103FCD51A6842F4B6 -:105BA00080521A601A689204FCD53A4A3A499A62BD -:105BB00000225A6319633949DA6399635A64384A8F -:105BC0001A64384ADA621A6842F0A8521A602B4BFB -:105BD0001A6802F02852B2F1285FF9D148229A617E -:105BE0004FF48862DA6140221A622F4ADA644FF079 -:105BF00080521A652D4A5A652D4A9A6532232D4ADC -:105C00001360136803F00F03022BFAD11B4B1A69C0 -:105C100042F003021A611A6902F03802182AFAD116 -:105C2000D3F8DC2042F00052C3F8DC20D3F8042182 -:105C300042F00052C3F80421D3F80421D3F8DC2049 -:105C400042F08042C3F8DC20D3F8042142F08042C5 -:105C5000C3F80421D3F80421D3F8DC2042F0004239 -:105C6000C3F8DC20D3F8042142F00042C3F8042139 -:105C7000D3F80431704700BF00800051004402583F -:105C80000048025800C000F0020000010000FF01BF -:105C90000088900812102000630209012C02040001 -:105CA00047040508FD0BFF01200000200010E00064 -:105CB00000010100002000524FF0B04208B5D2F8B8 -:105CC000883003F00103C2F8883023B1044A136816 -:105CD0000BB150689847BDE80840FEF7F3BC00BF21 -:105CE000484900204FF0B04208B5D2F8883003F0A0 -:105CF0000203C2F8883023B1044A93680BB1D0681C -:105D00009847BDE80840FEF7DDBC00BF48490020C9 -:105D10004FF0B04208B5D2F8883003F00403C2F85F -:105D2000883023B1044A13690BB150699847BDE824 -:105D30000840FEF7C7BC00BF484900204FF0B04202 -:105D400008B5D2F8883003F00803C2F8883023B1D0 -:105D5000044A93690BB1D0699847BDE80840FEF743 -:105D6000B1BC00BF484900204FF0B04208B5D2F89E -:105D7000883003F01003C2F8883023B1044A136A54 -:105D80000BB1506A9847BDE80840FEF79BBC00BFC6 -:105D9000484900204FF0B04310B5D3F8884004F4D0 -:105DA0007872C3F88820A30604D5124A936A0BB10F -:105DB000D06A9847600604D50E4A136B0BB1506B3E -:105DC0009847210604D50B4A936B0BB1D06B9847CB -:105DD000E20504D5074A136C0BB1506C9847A30534 -:105DE00004D5044A936C0BB1D06C9847BDE81040C1 -:105DF000FEF768BC484900204FF0B04310B5D3F817 -:105E0000884004F47C42C3F88820620504D5164A11 -:105E1000136D0BB1506D9847230504D5124A936D4D -:105E20000BB1D06D9847E00404D50F4A136E0BB147 -:105E3000506E9847A10404D50B4A936E0BB1D06EF7 -:105E40009847620404D5084A136F0BB1506F984706 -:105E5000230404D5044A936F0BB1D06F9847BDE873 -:105E60001040FEF72FBC00BF4849002008B503488A -:105E7000FCF726FCBDE80840FEF724BCDC2400202B -:105E800008B50348FCF7CCFCBDE80840FEF71ABC97 -:105E9000FC24002008B5FFF79DFDBDE80840FEF793 -:105EA00011BC0000062108B50846FFF79DFA06213F -:105EB0000720FFF799FA06210820FFF795FA062137 -:105EC0000920FFF791FA06210A20FFF78DFA062133 -:105ED0001720FFF789FA06212820FFF785FA092104 -:105EE0007A20FFF781FA0A215C20FFF77DFA07216B -:105EF0003220FFF779FA0C212520BDE80840FFF792 -:105F000073BA000008B5FFF77BFD00F00FF8FCF74F -:105F100099FEFDF771F8FCF743FFFDF703FBFFF770 -:105F20002DFDBDE80840FFF73FBC00000023054AF7 -:105F300019460133102BC2E9001102F10802F8D111 -:105F4000704700BF484900200B460146184600F044 -:105F500027B80000FEF70CBAFFF7FCBF012838BFD6 -:105F6000012010B504462046FEF7C2F930B900F012 -:105F700007F808B900F00CF88047F4E710BD0000FE -:105F8000024B1868BFF35B8F704700BFC849002001 -:105F900008B5062000F04AF80120FDF7B1FE000028 -:105FA00010B5054C13462CB10A4601460220AFF34A -:105FB000008010BD2046FCE70000000010B501394C -:105FC0000244904201D1002005E0037811F8014F0E -:105FD000A34201D0181B10BD0130F2E71F2938B5CC -:105FE00004460D4604D9162303604FF0FF3038BD38 -:105FF000426C12B152F821304BB9204600F030F813 -:106000002A4601462046BDE8384000F017B8012B6B -:106010000AD0591C03D1162303600120E7E70024AE -:1060200042F82540284698470020E0E7024B014609 -:106030001868FFF7D3BF00BF5C22002038B5074DBA -:1060400000230446084611462B60FDF75DFE431C05 -:1060500002D12B6803B1236038BD00BFCC490020BA -:10606000FDF74CBE034611F8012B03F8012B002A63 -:10607000F9D170470000000001000000000100019C -:106080000000000000000000000000005265717573 -:10609000657374656420746F206572617365206D2B -:1060A0006F7265207468616E2077652063616E0A87 -:1060B00000457261736520436F6D6D616E6420529F -:1060C000656365697665640A0050435420444F4E09 -:1060D000453A2025640A000053544D333248373F77 -:1060E0003F3F0053544D3332483734332F37353325 -:1060F00000000000382B0020FC240020773235718E -:106100002D64747200000000000000000000000018 -:106110000000000000000000B5160008F519000896 -:10612000F5190008B9160008BD160008C1160008C8 -:10613000F5190008F5190008F5190008C51600083A -:10614000C9160008E1160008E9160008F51600084F -:10615000F9160008FD160008DC24002001000000EC -:1061600000000000000000001118000829180008B5 -:10617000B5160008091700085D1800082517000863 -:10618000F518000825190008211700080D17000848 -:106190001D170008C5160008C9160008E1160008FA -:1061A000E9160008F5160008F9160008FD160008A3 -:1061B00000000000000000009D1700080117000803 -:1061C0000517000853464450000000004A45444566 -:1061D000433A204661696C656420746F206465747D -:1061E00065637420666C61736820646576696365B5 -:1061F0003A2025730A004A454445433A20466169DE -:106200006C656420746F20636F6E66696720666CCE -:10621000617368206465766963653A2025730A00B6 -:106220004A454445433A2044657465637465642077 -:10623000466C617368204465766963653A2025730E -:106240000A0045787420466C617368204E6F742094 -:10625000466F756E642100006D74323571007732BF -:10626000357100005862000820BA00005E62000824 -:10627000EF400000FC600008EF700000000000002C -:1062800000000000D520000835230008111A00087E -:10629000151A0008BD1A00089D220008191A0008E6 -:1062A0001D1A0008952100088D1A0008F521000824 -:1062B000F91E0008251A0008211A0008C523000845 -:1062C000851A00080000000000000000812400087A -:1062D000892400086D24000875240008992400080A -:1062E0009D2400084D444D41206661696C757265BE -:1062F0000000000000960000000000000000000008 -:10630000000000000000000000000000000000008D -:106310000D2C0008F92B0008352C0008212C000852 -:106320002D2C0008192C0008052C0008F12B000862 -:10633000412C0008000000001D2D0008092D000858 -:10634000452D0008312D00083D2D0008292D00089D -:10635000152D0008012D0008512D00080000000037 -:1063600001000000000000006D61696E0000000087 -:1063700069646C650000000070630008C02900209B -:10638000382B002001000000C13900080000000087 -:106390004172647550696C6F740025424F415244DC -:1063A000252D424C002553455249414C2500000003 -:1063B00002000000000000003D2F0008AD2F000883 -:1063C00040004000E0430020F043002002000000B5 -:1063D000000000000300000000000000F52F00088E -:1063E0000000000010000000004400200000000039 -:1063F0000100000000000000F04700200101020041 -:10640000013E0008113D0008AD3D0008913D000827 -:10641000430000001864000809024300020100C0A4 -:1064200032090400000102020100052400100105E8 -:10643000240100010424020205240600010705824C -:10644000030800FF09040100020A0000000705011B -:106450000240000007058102400000001200000019 -:106460006464000812011001020000400912415743 -:1064700000020102030100000403090425424F4108 -:10648000524425005350526163696E6748370030AB -:10649000313233343536373839414243444546008A -:1064A0000000002000000200020000000000003098 -:1064B00000000400000000000000002400000800AC -:1064C000040000000004000000FC000002000000C6 -:1064D00000000430008000000000000000000038D0 -:1064E0000000010001000000400000528000005246 -:1064F000C0000052000100524001005280010052D1 -:10650000C0010052000200524002005280020052BC -:10651000C0020052000300524003005280030052A8 -:10652000C00300520004005200000000493100087E -:1065300001340008AD340008400040002849002024 -:106540002849002001000000384900208000000098 -:1065500040010000080000000001000000040000ED -:10656000080000000000806A00000000AAAAAAAA91 -:1065700000000064FFFF00000000000000A00A000F -:10658000240020A100000000AAAAAAAA000000512D -:10659000FFFF0000000900000009004400000000A7 -:1065A00000000000AAAAAAAA00000000FFFF000045 -:1065B00000000000000000000000800A0000000051 -:1065C000AAAAAAAA00000000FFFF00000000000025 -:1065D000009099006000400000000000AAAAAAAA4A -:1065E00000004000F7FF000000090000000000006C -:1065F0000000000000000000AAAAAAAA00000000F3 -:10660000FFFF00000000000000000000000000008C -:1066100000000000AAAAAAAA00000000FFFF0000D4 -:10662000000000000000000000000000000000006A -:10663000AAAAAAAA00000000FFFF000000000000B4 -:10664000000000000000000000000000AAAAAAAAA2 -:1066500000000000FFFF000000000000000000003C -:106660000000000000000000AAAAAAAA0000000082 -:10667000FFFF00000000000000000000000000001C -:1066800000000000AAAAAAAA00000000FFFF000064 -:106690000000000000000000489CFF7F0100000097 -:1066A0002404000000000000000000000000E900D9 -:1066B000FF00000000000000D86000083F0000005C -:1066C00050040000E36000083F0000000096000056 -:1066D0000000080096000000000800000400000010 -:1066E00078640008000000000000000000000000C6 -:1066F00000000000000000000000000060220020F8 -:106700000000000000000000000000000000000089 -:106710000000000000000000000000000000000079 -:106720000000000000000000000000000000000069 -:106730000000000000000000000000000000000059 -:106740000000000000000000000000000000000049 -:106750000000000000000000000000000000000039 +:1000000000060020F5050008B51A0008311A00089E +:10001000891A0008311A00085D1A0008F70500085F +:10002000F7050008F7050008F7050008155B00084C +:10003000F7050008F7050008F7050008F7050008B0 +:10004000F7050008F7050008F7050008F7050008A0 +:10005000F7050008F70500080D5F0008395F000884 +:10006000655F0008915F0008BD5F0008F7050008A4 +:10007000F7050008F7050008F7050008F705000870 +:10008000F7050008F7050008F7050008F705000860 +:10009000F7050008F7050008F7050008E95F000804 +:1000A000F7050008F7050008F7050008F705000840 +:1000B000F7050008F7050008F7050008F705000830 +:1000C000F7050008F7050008F7050008F705000820 +:1000D000F7050008D5600008F7050008F7050008D7 +:1000E0004D600008F7050008F7050008F70500084F +:1000F000F7050008F7050008F7050008F7050008F0 +:10010000F7050008F7050008E9600008F705000892 +:10011000F7050008F7050008F7050008F7050008CF +:10012000F7050008F7050008F7050008F7050008BF +:10013000F7050008F7050008F7050008F7050008AF +:10014000F7050008F7050008F7050008F70500089F +:10015000F7050008F7050008F7050008F70500088F +:10016000F7050008F7050008F7050008F70500087F +:10017000F70500085D540008F7050008F7050008BA +:10018000F7050008F7050008F7050008F70500085F +:10019000F7050008F7050008F7050008F70500084F +:1001A000F7050008F7050008F7050008F70500083F +:1001B000C1600008F7050008F7050008F70500080A +:1001C000F7050008F7050008F7050008F70500081F +:1001D000F705000849540008F7050008F70500086E +:1001E000F7050008F7050008F7050008F7050008FF +:1001F000F7050008F7050008F7050008F7050008EF +:10020000F7050008F7050008F7050008F7050008DE +:10021000F7050008F7050008F7050008F7050008CE +:10022000F7050008F7050008D5480008F70500089D +:10023000F7050008F7050008F7050008F7050008AE +:10024000F7050008F7050008F7050008F70500089E +:10025000F7050008F7050008F7050008F70500088E +:10026000F7050008F7050008F7050008F70500087E +:10027000F7050008F7050008F7050008F70500086E +:10028000F7050008F7050008F7050008F70500085E +:10029000F7050008F7050008F7050008F70500084E +:1002A000F7050008F7050008F7050008F70500083E +:1002B000F7050008F7050008F7050008F70500082E +:1002C000F7050008F7050008F7050008F70500081E +:1002D000F7050008F7050008F7050008F70500080E +:1002E000D9070008091A00082125000800000000AD +: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 +:1005F000704700BF02E000F000F8FEE772B638482E +:1006000080F30888374880F30988374837490860FD +:1006100040F20000CCF200004EF63471CEF2000140 +:100620000860BFF34F8FBFF36F8F40F20000C0F23E +:10063000F0004EF68851CEF200010860BFF34F8FF4 +:10064000BFF36F8F4FF00000E1EE100A4EF63C71E1 +:10065000CEF200010860062080F31488BFF36F8F8C +:1006600004F02AF904F0CCF805F0B2F94FF0553057 +:100670001F491B4A91423CBF41F8040BFAE71D4950 +:10068000184A91423CBF41F8040BFAE71A491B4A49 +:100690001B4B9A423EBF51F8040B42F8040BF8E79B +:1006A00000201849184A91423CBF41F8040BFAE770 +:1006B00004F0E4F805F002FA144C154DAC4203DAEC +:1006C00054F8041B8847F9E700F050F8114C124D1C +:1006D000AC4203DA54F8041B8847F9E704F0CCB8BD +:1006E00000060020002200200000000808ED00E0C5 +:1006F0000000002000060020F06800080022002012 +:10070000C0220020C0220020904A0020E002000801 +:10071000EC020008EC020008EC0200082DE9F04FA2 +:100720002DED108AC1F80CD0D0F80CD0BDEC108A99 +:10073000BDE8F08F002383F311882846A0470020EE +:1007400003F026FAFEE703F03FF900DFFEE70000C2 +:10075000054B10B504460360806810B103685B6800 +:100760009847204610BD00BFE064000838B503F08C +:100770006DFF054604F03AF80446F0B9144B9D426B +:100780001DD001339D4241F2883512BF0446002539 +:100790000124002003F064FF0CB100F09FF80D4C21 +:1007A00000F02EFF00F0C8FD204601F0F1FC40B93A +:1007B00044F6206003F0F2F9F6E70025E9E7054684 +:1007C000E7E7284600F046F900F088F8F9E700BFAF +:1007D000010007B0C02200200448054B054A036011 +:1007E00000230549836005F0D9BC00BFC02200206A +:1007F000E0640008D42400205107000808B500F088 +:1008000067FDA0F120035842584108BD07B50421F7 +:1008100001900DEB010000F079FD03B05DF804FBE1 +:1008200007B541F21203022101A8ADF8043000F02F +:100830006DFD03B05DF804FB38B5302383F31188F8 +:10084000174803680BB103F087FA0023154A4FF4E9 +:100850007A71134803F076FA002383F31188124C5F +:10086000236813B12368013B2360636813B1636895 +:10087000013B63600D4D2B7833B963687BB902206F +:1008800000F028FE322363602B78032B07D16368C6 +:100890002BB9022000F01EFE4FF47A73636038BD5E +:1008A000502300203908000870240020682300200D +:1008B000084B187003280CD8DFE800F00805020880 +:1008C000022000F0FFBD022000F0F2BD024B00222A +:1008D0005A6070476823002070240020F7B55648FE +:1008E00001A901F0CDFD002800F0A380534B544A2C +:1008F0001C461968013100F0998004339342F8D105 +:100900006268504B9A4240F291804F4BDB6803F192 +:10091000104303F580139A4280F08880002000F095 +:1009200025FD0220FFF7C4FF484B0021D3F8E82043 +:10093000C3F8E810D3F81021C3F81011D3F8102130 +:10094000D3F8EC20C3F8EC10D3F81421C3F8141139 +:10095000D3F81421D3F8F020C3F8F010D3F81821FD +:10096000C3F81811D3F81821D3F8802042F00062A0 +:10097000C3F88020D3F8802022F00062C3F88020E2 +:10098000D3F88020D3F8802042F00072C3F8802092 +:10099000D3F8802022F00072C3F88020D3F8803092 +:1009A00072B64FF0E023C3F8084DD4E90004BFF35A +:1009B0004F8FBFF36F8F264AC2F88410BFF34F8F5B +:1009C000536923F480335361BFF34F8FD2F88030E3 +:1009D00043F6E076C3F3C905C3F34E335B0103EA84 +:1009E000060C29464CEA81770139C2F87472F9D2B3 +:1009F000203B13F1200FF2D1BFF34F8FBFF36F8F66 +:100A0000BFF34F8FBFF36F8F536923F400335361EC +:100A10000023C2F85032BFF34F8FBFF36F8F3023E4 +:100A200083F31188854680F308882047024801F047 +:100A300087F803B0F0BD00BFC02200200000109076 +:100A400020001090FFFF0F90002200200044025869 +:100A500000ED00E02DE9F04FB34B9FB02022FF21C5 +:100A6000089016A89E68DB68079300F073FDAF4AF4 +:100A70001378A3B90121AE4811700360302383F3CA +:100A8000118803680BB103F067F90023A94A4FF4FA +:100A90007A71A74803F056F9002383F31188089B65 +:100AA00013B1A54B089A1A60A44A1378032B03D0FC +:100AB00000231370A04A53600023089DA04F98465E +:100AC0009B46CDE90533012000F0FCFC25B19A4B93 +:100AD0001B68002B00F08783002000F0F9FB0E90CC +:100AE0000E9B002BF2DB012000F0E2FC0E9B213B71 +:100AF0001F2BE8D801A252F823F000BF7D0B00089D +:100B0000A50B00083F0C0008C70A0008C70A000828 +:100B1000C70A00082B0F000829110008C10F0008A0 +:100B2000A7100008CD100008F3100008C70A00083D +:100B300005110008C70A0008711100081D0C000803 +:100B4000C70A0008ED110008DD0C0008150E0008AA +:100B5000C70A00081D100008C70A0008C70A0008D5 +:100B6000C70A0008C70A0008C70A0008C70A000821 +:100B7000C70A0008C70A00083F0C00080220FFF758 +:100B80003DFE002840F05B83089B0221069A11A8D5 +:100B9000002A08BF1D4641F21233ADF8443000F080 +:100BA000B5FB90E74FF47A7000F092FB041EEBDB8C +:100BB0000220FFF723FE0028E6D0013C052C00F2BE +:100BC0004083DFE804F0030A0D10133905230421E4 +:100BD00011A8119300F09AFB17E004215248F9E79D +:100BE00004215848F6E704215748F3E74FF01C0961 +:100BF000484609F1040900F0BBFB0421119011A83B +:100C000000F084FBB9F12C0FF2D10120059B4FF0CD +:100C1000000B00FA04F41C43E3B2059300F0E6FC79 +:100C2000B8F1000F05D0059B03F00B030B2B08BF99 +:100C30000025FFF7F5FD46E704214448CAE7B8F16F +:100C4000000FA1D0059B03F00B030B2B9CD10220BE +:100C5000FFF7D4FD0446002896D001204FF000088D +:100C600000F084FB0220FFF723FE5FFA88F9484674 +:100C700000F08CFB0546B8B10E99484608F1010812 +:100C8000A1F140025142514100F090FB0028ECD10B +:100C90000546069441F21213022111A8A046ADF8B0 +:100CA000443000F033FB0EE72E46DFF87C80012055 +:100CB000FFF7FEFDD8F80830B34207D9304600F000 +:100CC00057FB013040F0C0820436F3E700261B4B8F +:100CD000A04606941E70184B5E609FE7B8F1000FA7 +:100CE0003FF452AF059B03F00B030B2B7FF44CAF8B +:100CF00064210FA800F000FB814600287FF444AF78 +:100D00000220FFF77BFD044600283FF43DAF3A6A1E +:100D10007B6A0F9953438DF837908B421AD20C4857 +:100D200000F0E2FB30E700BF002200206C2400202E +:100D30005023002039080008702400206823002078 +:100D4000C022002004220020082200200C220020C3 +:100D5000E06200084FF4801AAC48C846BAFBF2FAC9 +:100D600000F0C2FBFFF75CFD01210DF1370000F040 +:100D7000CDFA0F9B434541D9002310AA514638466E +:100D8000CDE9103311AB00F0DFFE002882D000F077 +:100D900099FE109B00EB030901210DF1370000F0D3 +:100DA000B5FA00F08FFE484520D9384601F002F927 +:100DB00080B964210F9B96480AF1010A01FB08F1F2 +:100DC000B1FBF3F18DF83710C9B200F08DFB3B6A2F +:100DD0009844CEE700F076FE119B184448453FF654 +:100DE00059AF00F06FFE109B00EB0309109B4FF40E +:100DF0007A70A3FB000302F0D1FECDE7642301214A +:100E00000DF13700A0468DF8373000F07FFA00234F +:100E1000079303E7B8F1000F3FF4B6AE059B03F06C +:100E20000B030B2B7FF4B0AE0220FFF741FD322005 +:100E300000F04EFAB0F1000AFFF6A6AE079BD146CD +:100E400053440993734B099ADB689A423FF69CAE70 +:100E5000BAF5807F3FF798AE6F4A0024A2450A9208 +:100E60000EDD4FF47A7000F033FA0E900E9B002BDB +:100E7000FFF68AAE0E9B01340A9A02F8013BEDE7B9 +:100E8000C820FFF7BBFC044600283FF47DAE079B5B +:100E90001F2B11D8C3F1200223F0030016AB5E49CB +:100EA0005245184428BF52460A9200F021FB0A9A84 +:100EB000FF21594800F04EFB00239846CDE910333E +:100EC000079B03F580130B93B9F1000F02D1A046E5 +:100ED000099B9DE711AB504A0B993846029310AB22 +:100EE0004244414401930FAB00934B4601F034FA66 +:100EF00000283FF4CFAE00F0E7FD0F9B82460A9139 +:100F00009844A9EB030900F0DFFD109B0A9A13EB4C +:100F10000A0342F10002834272EB0103F3D2384626 +:100F200001F048F80028EED1CEE7B8F1000F3FF409 +:100F30002BAE059B03F00B030B2B7FF425AE022099 +:100F4000FFF7B6FC322000F0C3F9B0F1000AFFF65B +:100F50001BAE1AF003047FF417AE2E4A0AEB060309 +:100F6000926893423FF610AEBAF5807F3FF70CAE21 +:100F7000DFF8A490A2450DDD4FF47A7000F0A8F9D7 +:100F80000E900E9B002BFFF6FFAD0E9B013409F86F +:100F9000013BEFE7C820FFF731FC044600283FF48F +:100FA000F3ADCAF387021C4930464FEAAA0900F0A4 +:100FB000D9FA804600283FF46DAE06EB89062DE68F +:100FC0000220FFF71BFC00283FF4DEAD00F00CFA16 +:100FD00000283FF4D9AD4FF00009DFF838A04C46A7 +:100FE000DAF808304B450CD9484609F1040900F0FD +:100FF000BFF90422119011A9204601F0B7FA044666 +:10100000EEE72046FFF702FC08E600BF0563000894 +:101010001D630008002200206C230020642110A81A +:1010200000F06AF9044600287FF4AEAD0220FFF715 +:10103000E5FB00283FF4A8AD109BA14603F0030A8E +:101040001099A1EB0A01A1421BD91F2C11D8169BA4 +:1010500001330ED024F003031EAA134453F8203C9E +:1010600011934846042211A9043401F07FFA814605 +:10107000E6E7042311AA04F58011384601F0B8F917 +:10108000EFE7BAF1000F0CD0534611AA01F5801119 +:10109000384601F0ADF94846524611A901F066FA0A +:1010A00081464846AEE70023642111A8119300F061 +:1010B00023F900287FF468AD0220FFF79FFB00288A +:1010C0003FF462AD119800F085F99BE7002364219D +:1010D00011A8119300F010F900287FF455AD0220FB +:1010E000FFF78CFB00283FF44FAD119800F074F926 +:1010F00088E70220FFF782FB00283FF445AD00F0AF +:1011000083F97FE70220FFF779FB00283FF43CAD2D +:1011100011A9142000F07EF90990FFF777FB0999D7 +:1011200011A800F0F3F879E5322000F0D1F8041EA0 +:10113000FFF62AADA3077FF427AD444A04EB0B0367 +:10114000926893423FF620AD0220FFF757FB00283C +:101150003FF41AAD24F003045C44A3453FF45EADB4 +:1011600058460BF1040B00F003F9FFF74FFBF4E7CF +:101170004FF47A70FFF742FB00283FF405AD00F012 +:1011800033F980B1169B013328D011AB16AA4FF466 +:1011900080113846029310AB01930FAB00932023CC +:1011A00001F0DAF808B9444674E500F08DFC284EE9 +:1011B00004460D4600F088FC109B1B1945F1000207 +:1011C000834272EB0103F5D2304600F0F3FE0028B3 +:1011D000F0D12022FF2116A800F0BCF9FFF720FB78 +:1011E0001C4802F0DBFC1FB0BDE8F08FB8F1000F27 +:1011F0003FF4CAAC059B03F00B030B2B7FF4C4AC8C +:101200000023642111A8119300F076F80446002809 +:101210007FF4BAAC0220FFF7F1FA814600283FF4D0 +:10122000B3ACFFF7FDFA41F2883002F0B7FC119839 +:1012300000F020FAC846254600F0D8F943E480467D +:10124000ECE44FF0000B9FE4069423E5002200201D +:10125000C0220020A0860100F7B50C46184E4FF4BE +:101260007A71054602FB01F396F90020501C0BD160 +:101270001448294601930268176A2246B8478442F7 +:10128000019B03D1002310E0002AF1D096F9002041 +:10129000511C01D0012A0DD10B4829460268166A5B +:1012A0002246B047844205D10123084A0120137029 +:1012B00003B0F0BD4FF4FA7002F070FC0020F7E7C5 +:1012C00010220020F82B002000250020BC24002044 +:1012D000002307B5024601210DF107008DF8073004 +:1012E000FFF7BAFF20B19DF8070003B05DF804FBDB +:1012F0004FF0FF30F9E700000A46042108B5FFF778 +:10130000ABFF80F00100C0B2404208BD074B0A4667 +:1013100030B41978064B53F8214001462368204623 +:10132000DD69044BAC4630BC604700BFBC240020E4 +:1013300048630008A086010070B50A4E00240A4DDB +:1013400002F06AFF308028683388834208D902F0AF +:101350005FFF2B6804440133B4F5003F2B60F2D3E8 +:1013600070BD00BFBE2400207824002003F008B820 +:1013700000F1006000F500300068704700F1006087 +:10138000920000F5003002F07FBF0000054B1A68A4 +:10139000054B1B889B1A834202D9104402F038BFC8 +:1013A0000020704778240020BE24002038B5044671 +:1013B000074D29B128682044BDE8384002F034BF09 +:1013C0002868204402F02AFF0028F3D038BD00BF6F +:1013D000782400200020704700F1FF5000F58F10A6 +:1013E000D0F8000870470000064991F8243033B166 +:1013F00000230822086A81F82430FFF7BFBF0120CC +:10140000704700BF7C240020014B1868704700BF64 +:101410000010005C244BF0B51A680446234BC2F35D +:101420000B06120C1F885868BE4293F9085028D04A +:101430009F89BE4206D101200C2505FB0033586868 +:1014400093F9085041F201039A421CD041F2030380 +:101450009A421AD042F201039A4218D042F2030390 +:101460009A4208BF5625621E0B46441E0A44934208 +:101470000FD214F9016F581C6EB1034600F8016CCD +:10148000F5E70020D8E75A25EDE75925EBE7582581 +:10149000E9E7184605E02C2482421C7001D9981C0B +:1014A0005D70401AF0BD00BF0010005C14220020E7 +:1014B000022803D1024B4FF400229A61704700BF0B +:1014C00000100258022802D1014B08229A6170478D +:1014D00000100258022804D1024A536983F008031D +:1014E00053617047001002580FB404B070470000F9 +:1014F000002310B5934203D0CC5CC4540133F9E708 +:1015000010BD0000013810B510F9013F3BB191F951 +:1015100000409C4203D11AB10131013AF4E71AB1FB +:1015200091F90020981A10BD1046FCE701380139E6 +:1015300010F9013F11F9012F0BB19342F8D0981A1D +:101540007047000003460246D01A12F9011B002919 +:10155000FAD1704702440346934202D003F8011BBC +:10156000FAE770472DE9F8431F4D144607468846B1 +:1015700095F8242052BBDFF870909CB395F8243086 +:101580002BB92022FF2148462F62FFF7E3FF95F891 +:1015900024004146C0F1080205EB8000A24228BFAA +:1015A0002246D6B29200FFF7A3FF95F82430A41B81 +:1015B00017441E449044E4B2F6B2082E85F8246025 +:1015C000DBD1FFF711FF0028D7D108E02B6A03EB2E +:1015D00082038342CFD0FFF707FF0028CBD1002042 +:1015E000BDE8F8830120FBE77C240020024B1A7839 +:1015F000024B1A70704700BFBC240020102200204C +:1016000038B5164C164D204601F036FC29462046CA +:1016100001F05EFC2D681348D5F89020D2F804380C +:1016200043F00203C2F8043802F0B8FA0E49284623 +:1016300001F05CFDD5F890200C48D2F804380C4934 +:10164000A04223F00203C2F804384FF4E1330B60E8 +:1016500003D0BDE8384001F06DBB38BDF82B002049 +:101660003C66000840420F00446600080025002048 +:10167000A424002038B50B4B04461A780A4B53F8C3 +:1016800022500A4B9D420CD0094B002118221846CB +:10169000FFF760FF046001462846BDE8384001F0CE +:1016A00049BB38BDBC24002048630008F82B00204B +:1016B000A424002038B5084B044615460175436044 +:1016C0000A21243003F000F804F128002A460A21F8 +:1016D00002F0FAFF204638BD28650008F8B51E461E +:1016E0001B6805460C4617461BB9236863B90124DD +:1016F00008E0BDF818203146806A02F0FDFF00289E +:10170000F3D100242046F8BD21463A46686A03F02A +:1017100029F804460028EAD13368002BF1D0A86AE2 +:1017200003F054F8EEE7000038B50D4611460446C4 +:101730001A4611B1806A03F007F82DB12946606A94 +:10174000BDE8384003F03AB838BD000070470000EB +:1017500070470000002070470020704713B56C46AA +:1017600000F1140384E8060094E8030083E8050010 +:1017700002B010BD0020704713B56C4600F10C0399 +:1017800084E8060094E8030083E8050002B010BD79 +:101790000023C0E90333704782B002AB03E90600BF +:1017A00002B070477047000000207047704700008B +:1017B00000207047002070470120704782B0002051 +:1017C00002A901E90C0002B0704700000020704738 +:1017D000002070470B6883634B6843638B68C36367 +:1017E000CB6803640B6943647047000038B5036B32 +:1017F0000446154D1B7955F8330001F073FFD4E909 +:101800000B231B7955F8331002F1300349689942D4 +:1018100018D000231363E26A5363D4E90B12537C9C +:10182000581E137C92681B0443EA002313438B6306 +:10183000D4E90B131B79303155F83300BDE838403B +:1018400001F0E4BE38BD00BFB0630008F8B51D4F1D +:1018500005460E4611463868FFF768FE78BB746887 +:101860003B7924B394F82C209A420BD02468F8E7F3 +:101870000122B521FFF71EFF736823603B79746076 +:1018800084F82C30482004F093FC68B100220271E7 +:10189000C0E90322C0E90522027A42F0070202727F +:1018A000094AC0E90B47026028602846F8BD40207D +:1018B00004F07EFC04460028DAD12C60F5E7002312 +:1018C0002B60F2E7C0240020C0630008044B10B571 +:1018D00004460360406A08B104F068FC204610BD6D +:1018E0006463000810B50446FFF7F0FF204604F0DB +:1018F0005BFC204610BD000008B519B1FFF776FF6C +:10190000012008BD036B1A79024B53F8320001F035 +:10191000EDFEF5E7B06300082DE9FF41DDF8288012 +:101920000546174603911FFA88F60293FFF75EFFFC +:1019300002ABBAB203A9E86A0096FFF7CFFE0446ED +:1019400038B900212846FFF7D7FF204604B0BDE88C +:10195000F081AFB92B6B05F134011A79134B53F8B1 +:101960003200B8F1000F19D101F05EFE80F00104E1 +:10197000E4B23346E86ADDE90221FFF7D5FEE0E78D +:10198000029B83B92B6B3A46084805F134011C7958 +:10199000039B50F8340001F05DFEE7E7029B4246EE +:1019A00001F06EFEE2E70024E3E700BFB063000849 +:1019B00013B504460191FFF719FF019A236B04F157 +:1019C00034011879054B53F8300001F06FFE002107 +:1019D0002046FFF791FF012002B010BDB063000860 +:1019E00010B50446FFF702FF236B1A79054B53F835 +:1019F000320001F069FE00212046FFF77DFF012043 +:101A000010BD00BFB0630008064B0121064A1A60F2 +:101A100000221A71054AC3E902124FF48C721A822D +:101A2000704700BFC02400205063000800E1F505A6 +:101A300000B59BB0EFF3098168226846FFF758FDB7 +:101A4000EFF30583044B9A6BDA6A9A6A9A6A9A6A88 +:101A50009A6A9A6A9B6AFEE700ED00E000B59BB0C7 +:101A6000EFF3098168226846FFF742FDEFF3058333 +:101A7000044B9A6B9A6A9A6A9A6A9A6A9A6A9B6AF9 +:101A8000FEE700BF00ED00E000B59BB0EFF3098179 +:101A900068226846FFF72CFDEFF30583034B5A6B72 +:101AA0009A6A9A6A9A6A9A6A9B6AFEE700ED00E06F +:101AB000FEE70000FEE700000FB408B5029801F051 +:101AC000F9FFFEE702F062BD02F03ABD806970479F +:101AD000C0697047006A7047406A7047806A704703 +:101AE000F0B5ADF2044D0E46054600214FF47F726D +:101AF00001A80091FFF72EFD2C6A744301362B6A72 +:101B00007343A34216D92B686A46214628465F686C +:101B10004FF48063B84770B16A464FF4806312F89F +:101B2000011BFF290AD1013B9BB2002BF7D104F521 +:101B30008064E4E701200DF2044DF0BD0020FAE7D7 +:101B400080680368DB69184730B5456A85B004468C +:101B50008D420FD30568CDE90023026A03ABAD6A5D +:101B60005143A84720B1206A039BC31A58425841E9 +:101B700005B030BD0020FBE7F0B5C56987B004466D +:101B80008D4212D30C9F866905687143CDE90137F8 +:101B900005AB00936D69036AA84720B1A069059B56 +:101BA000C31A5842584107B0F0BD0020FBE70000BF +:101BB000F0B5662389B0002405464FF4807680682E +:101BC000079403A9CDE90336CDE9054403685B69B1 +:101BD0009847A86822462146036800941F69234657 +:101BE000B847A8689923079403A9CDE90336CDE93E +:101BF000054403685B699847A86822462146036844 +:101C000000941D692346A84709B0F0BD254B2DE976 +:101C1000F04F1B789FB0002504468DF828304FF018 +:101C20009F0E80684FF00123ADF8295005A9099552 +:101C3000CDE905E3CDE9075503685B699847A068DE +:101C400003222946036800922A461E690AABB04760 +:101C500028B394F88C20144B9DF8281003EBC20392 +:101C60001A7991421BD19DF829205B799A4216D1AD +:101C7000A0684FF05A0C0D4B082605A90996CDE92E +:101C800005C3CDE9075503685B699847A0682A46F4 +:101C90002946036800961F690AABB84740B900207F +:101CA00048E100BFC8620008C064000800250001C8 +:101CB0000422B1490AA804F0ABFA05460028EED187 +:101CC000BDF82C306FF48277A3813B44012BE6D81A +:101CD000079605A9CDE90A00A06803685B699847E3 +:101CE000A0682A462946036800961E690AABB047D9 +:101CF0000028D4D0DDE90A361A0202F4706242EA02 +:101D00001662FF2ACBD11B0E26F07F4650222946B1 +:101D1000142B26610AA828BF1423A373FFF71AFC0B +:101D2000A068079605A903685B699847A068A37B2C +:101D3000294602689B0000930AAB16692A46B04701 +:101D40000028ACD00B990029A9DBC9086FF07E43AD +:101D5000149D4FF0010E994228BF1946C5F3031395 +:101D60000EFA03F221FA03F303916161A261E361C8 +:101D7000002B94D02646A44600274FF002081EAA46 +:101D8000C7F3460302EB830353F834AC07F00103B7 +:101D90001B01DA1D0EFA03F908FA02F2A2EB09029E +:101DA00002EA0A02DA401AD003F108090F330EFAE8 +:101DB00002F2216A08FA03F30EFA09FBCCF830208C +:101DC0008A42A3EB0B0B0BEA0A0A2AFA09F98CF8F0 +:101DD0002C90A36A88BF2262934288BFA262013717 +:101DE0000CF1100C042FCAD1236A4FF0020C039A95 +:101DF0004FF0010E139F9A4224BFB2FBF3F16162D0 +:101E000007F00F0138BF01234FF0090201F1010172 +:101E100038BF63624900336B33B3531C0EFA02F8C8 +:101E2000A2F105090CFA03F30EFA09FAA3EB080371 +:101E300002F1FF380CFA08F83B40A8EB0A08D3403F +:101E400008EA070828FA09F877D0012B77D0022B87 +:101E500077D0032B08BF4FF47A735FFA88F808FB3A +:101E6000033373634B43B36307321036252AD2D151 +:101E700015F0C04FC5F3417365D0012B65D0022B1F +:101E80000CBF4FF47A634FF47A43C5F3046202FB4C +:101E90000333E3664B432367199B9E0758D0062301 +:101EA00084F8743015F4005F05F00F03C5F30425C2 +:101EB00014BF40220822013305FB02255B00A56701 +:101EC0005D430A9B9F02E5677FF5E9AE0C9B1A0A0A +:101ED00084F88020C3F3421203F01F0384F88820A3 +:101EE00084F88130189BC3F3025213F4401F84F826 +:101EF00083207FF4D4AEDE0602D5042A7FF4CFAE71 +:101F000003F40072002A14BF0122002284F8872003 +:101F100004D0DD0324D5002384F88930179B1A07E9 +:101F200022D5002384F88630702384F8853001237D +:101F300084F88D301FB0BDE8F08F01238DE71023AA +:101F40008BE7802389E710239FE74FF480739CE79A +:101F5000590701D55023A3E713F0780FA2D09EE6CE +:101F60009903DBD50123D7E75B07E0D5012384F88C +:101F700086300523D9E700BF2064000830B58BB058 +:101F800000240546806803924FF00122059105A9BF +:101F9000CDE90624CDE90844026852699047A86853 +:101FA0000121039B02680091214615692246A8473A +:101FB0000BB030BD13B504460DF1070290F8851043 +:101FC000FFF7DCFF94F886309DF807001BB100F0A6 +:101FD000010002B010BDC043C0F3C010F9E700001B +:101FE00030B54FF001238BB00024054680688DF892 +:101FF0000F20059105A9CDE90634CDE90844036811 +:102000005B699847A86801220DF10F0103680094ED +:102010001D692346A8470BB030BD0000F7B5174631 +:102020000DF1070206461D46FFF7A8FF68B19DF8AF +:10203000074039469DF82020304624EA05042A400E +:1020400022438DF80720FFF7CBFF03B0F0BD00005F +:1020500030B54FF4807389B0002405468068039141 +:1020600003A9CDE90434CDE9064403685B699847C8 +:10207000A86822462146036800941D692346A847A4 +:1020800009B030BD10B5044623682046DB6A984786 +:102090000028F9D110BD000010B590F87430044646 +:1020A0005BB1FFF7EFFF012394F87410204684F82A +:1020B0008E30BDE81040FFF7CBBF012010BD0000FF +:1020C00010B590F87430044653B1FFF7DBFF0123DD +:1020D0002046042184F88E30BDE81040FFF7B8BFD9 +:1020E000012010BD70B590F883308AB00446042BEF +:1020F00048D10DF10E020521FFF740FF00282FD037 +:102100000DF10F0235212046FFF738FF40B320467E +:102110001E4EFFF7C1FF2046FFF7B4FF05AD0FCEFF +:102120000FC5336805A99DF80E202B609DF80F3070 +:10213000A06843F002038DF810208DF80F308DF861 +:10214000113003685B699847A068002304A90268FE +:10215000009315690222A84728B92046FFF7B0FF6F +:1021600000200AB070BD2046FFF7AAFF0DF10F0254 +:1021700035212046FFF702FF0028F1D09DF80F30EF +:102180009B07EDD501202071EBE700BFCC62000872 +:102190002DE9F341002304468068A36010B1036871 +:1021A0005B6898470025244E244F56F8352001A837 +:1021B00039465FFA85F8FFF749FB019BA0680022CA +:1021C000A360019210B103685B689847019810B151 +:1021D00003685B689847A368DBB141F2883084F8F4 +:1021E0008C8001F0DBFC2046FFF7E2FC1E2001F0B2 +:1021F000D5FC2046FFF70AFD054680B994F88C30DF +:102200000F4856F83310FFF76FF9284602B0BDE8C3 +:10221000F0810135032DC8D10A48FFF74DFC204657 +:10222000FFF760FF94F88C30054656F8331008B974 +:102230000548E8E70548E6E7C0640008D824002020 +:10224000286400089E640008526400087C6400084A +:102250002DE9F041044688B00F461646FFF71CFFF3 +:102260002046FFF70FFF0025C722A0684FF48073B8 +:10227000079503A9CDE90323CDE9055503685B69FB +:102280009847A0682A46294603680095D3F810802D +:102290002B46C047054630B92046FFF711FF2846B8 +:1022A00008B0BDE8F081E36E3B60236F3360F3E775 +:1022B0002DE9F041002588B01F4604460E460E9BCE +:1022C00000F1400CA8463D60016B81B1A9420ED3DC +:1022D00091420CD8B6FBF1FE01FB1E6139B9416B8E +:1022E00090F82C800F9D1960816B29601546103085 +:1022F0006045E9D16DB3AB196269934229D8204694 +:10230000FFF7CAFE2046FFF7BDFE4FF41453A06846 +:1023100003A9CDF80C80CDE904360026CDE9066688 +:1023200003685B699847A0683246314603680096A7 +:10233000D3F810803346C047064620462EB9FFF733 +:10234000BFFE304608B0BDE8F081FFF7B9FE3D6042 +:10235000F7E70026F5E700002DE9F041866905461C +:1023600088B00F46B3429046B1FBF6FC06FB1C1C3E +:1023700028BF3346A6EB0C0C00269C45644628BFBC +:102380001C46FFF789FE2846FFF77CFE0222A8685C +:1023900003A9164B0597CDE90323CDE9066603682B +:1023A0005B699847A86822464146036800961F6902 +:1023B0003346B847064628462EB9FFF781FE304619 +:1023C00008B0BDE8F081FFF77BFE0E9B1C60AB6F91 +:1023D000AA696343B3FBF2F30F9A1360EA6F109B91 +:1023E0005443AA69B4FBF2F41C60E8E7002500013D +:1023F0002DE9F04FCE18436989B004469E420F463E +:10240000904634D83D464FF00009DFF870A0FFF742 +:1024100039FEAE422FD9A6EB050B94F8803094F824 +:10242000882003A9BBF5806F039394F8813028BFFF +:102430004FF4806B012AA06808BF03F1FF33CDF889 +:1024400010A0CDE90559079303685B699847A06818 +:10245000A8EB07030268CDF800B02B44D2F810B007 +:1024600000221146D84718B105F58065D1E7002054 +:1024700009B0BDE8F08F0120FAE700BF00ED0003CE +:10248000F0B5044689B00E46FFF7FCFD94F887507E +:102490002DB194F889302BB1012B1DD00025284691 +:1024A00009B0F0BD94F880200392F02206921A4AF7 +:1024B000CDE9042394F88130A06803A90793036849 +:1024C0005B699847A068314603689B699847054651 +:1024D000642001F063FBE2E720460027FFF7DCFD04 +:1024E0002046FFF7CFFD0823812285212046009753 +:1024F000FFF794FD054618B92046FFF7E1FDCEE74A +:1025000094F8803006970393044BCDE9043794F890 +:102510008130013BD0E700BF00ED000300ED001368 +:10252000014B024A1A607047D82400201464000846 +:10253000036800211B68184703790BB10022027160 +:102540001846704703685B681847000003791BB999 +:102550000123037118467047002070477047000040 +:1025600010B5044603F020FE204610BD30B50A44E5 +:10257000084D91420DD011F8013B5840082340F31B +:102580000004013B2C4013F0FF0384EA5000F6D115 +:10259000EFE730BD2083B8ED8A0710B504461DD59E +:1025A0000378042B1AD143681B682BB105220270F3 +:1025B00098472378052B01D102232370302383F31E +:1025C0001188002104F1080001F002FB002383F3CD +:1025D0001188E069BDE8104002F040BACB0704D58D +:1025E0000248BDE8104001F065BA10BD4065000822 +:1025F00010B5044C204601F001F8034A0023C4E959 +:10260000062310BDE02400200050005210B50378CE +:102610000446012B17D1104B984214D102460F49A2 +:10262000102002F0E7F90E4BE061D3F8D42042F419 +:102630008042C3F8D420D3F8FC2042F48042C3F88F +:10264000FC20D3F8FC306268A36992685A60054A9E +:102650001A601B22DA6010BDE024002099250008D2 +:10266000004402581500020182691B2310B5D36093 +:10267000D1E9003013F44C4406D19B011461D061C0 +:1026800003F44043536110BD0024184313F4406F1A +:102690001461CC68D4615061F5D08B689361F2E726 +:1026A00030B584691B25C069E5600068836104F169 +:1026B0002003C36102234261013A03614223C360E4 +:1026C00016238362CB682261E361D1E900231343BF +:1026D00012F4406F636101D08B68A361C36843F05B +:1026E0000103C36030BD000030B5D0E9064000688A +:1026F00004F120058561C36108234261013A036149 +:1027000046230D69C36016238362CB682261E361AF +:10271000D1E90023134312F4406F43EA854343F0A9 +:102720008063636101D08B68A361C36843F00103D8 +:10273000C36030BD8069036823F0040310B5CC6822 +:10274000036000230361C4618361D1E90043096927 +:10275000234343EA814343F04063436112B14FF0A6 +:102760001043136010BD000083691A6842F0020232 +:102770001A601A689207FCD41A6842F004021A60C0 +:10278000704700008269936810B5D3609B070446C8 +:102790001DD50378032B1AD143681B682BB1052282 +:1027A000027098472378052B01D102232370302330 +:1027B00083F31188002104F1080001F009FA0023D5 +:1027C00083F31188E069BDE8104002F047B910BDFD +:1027D000026843681143016003B118477047000065 +:1027E000024A136843F0C0031360704700100140B1 +:1027F00013B50E4C204600F091FA04F114000023AA +:102800004FF400720A49009400F04EF9094B4FF45E +:102810000072094904F13800009400F0C7F9074A32 +:10282000074BC4E9172302B010BD00BF00250020EC +:102830006C250020E12700086C27002000100140D3 +:1028400000E1F505037C30B5244C002918BF0C4687 +:10285000012B11D1224B98420ED1224BD3F8F020FC +:1028600042F01002C3F8F020D3F8182142F0100211 +:10287000C3F81821D3F818312268036EC16D03EB39 +:1028800052038466B3FBF2F36268150442BF23F07F +:10289000070503F0070343EA4503CB60A36843F051 +:1028A00040034B60E36843F001038B6042F496738E +:1028B00043F001030B604FF0FF330B62510505D568 +:1028C00012F0102205D0B2F1805F04D080F864309D +:1028D00030BD7F23FAE73F23F8E700BF50650008CB +:1028E00000250020004402582DE9F047C66D05463A +:1028F0003768F469210734621AD014F0080118BF50 +:102900004FF48071E20748BF41F02001A3074FF068 +:10291000300348BF41F04001600748BF41F08001EB +:1029200083F31188281DFFF753FF002383F31188D9 +:10293000E2050AD5302383F311884FF48061281D06 +:10294000FFF746FF002383F311884FF030094FF063 +:10295000000A14F0200838D13B0616D54FF0300994 +:1029600005F1380A200610D589F31188504600F089 +:1029700051F9002836DA0821281DFFF729FF27F032 +:1029800080033360002383F31188790614D562062F +:1029900012D5302383F31188D5E913239A4208D145 +:1029A0002B6C33B127F040071021281DFFF710FFD3 +:1029B0003760002383F31188E30618D5AA6E1369E4 +:1029C000ABB15069BDE8F047184789F31188736AC5 +:1029D000284695F86410194000F0BAF98AF3118876 +:1029E000F469B6E7B06288F31188F469BAE7BDE824 +:1029F000F0870000F8B51546826804460B46AA42E7 +:102A000000D28568A1692669761AB5420BD21846AC +:102A10002A46FEF76DFDA3692B44A3612846A368EF +:102A20005B1BA360F8BD0CD9AF1B18463246FEF7FE +:102A30005FFD3A46E1683044FEF75AFDE3683B44E7 +:102A4000EBE718462A46FEF753FDE368E5E700008A +:102A500083689342F7B50446154600D28568D4E9E9 +:102A60000460361AB5420BD22A46FEF741FD63696F +:102A70002B4463612846A3685B1BA36003B0F0BDD1 +:102A80000DD93246AF1B0191FEF732FD01993A464E +:102A9000E0683144FEF72CFDE3683B44E9E72A4651 +:102AA000FEF726FDE368E4E710B50A440024C3619D +:102AB000029B8460C16002610362C0E90000C0E95A +:102AC000051110BD08B5D0E90532934201D18268E5 +:102AD00082B98268013282605A1C426119700021F9 +:102AE000D0E904329A4224BFC368436101F08AF8F6 +:102AF000002008BD4FF0FF30FBE7000070B5302329 +:102B000004460E4683F31188A568A5B1A368A2699F +:102B1000013BA360531CA36115782269934224BF33 +:102B2000E368A361E3690BB120469847002383F370 +:102B30001188284607E03146204601F053F8002866 +:102B4000E2DA85F3118870BD2DE9F74F04460E4691 +:102B500017469846D0F81C904FF0300A8AF3118837 +:102B60004FF0000B154665B12A4631462046FFF767 +:102B700041FF034660B94146204601F033F8002882 +:102B8000F1D0002383F31188781B03B0BDE8F08FE8 +:102B9000B9F1000F03D001902046C847019B8BF389 +:102BA0001188ED1A1E448AF31188DCE7C160C36105 +:102BB000009B82600362C0E905111144C0E9000076 +:102BC00001617047F8B504460D461646302383F37D +:102BD0001188A768A7B1A368013BA36063695A1C69 +:102BE00062611D70D4E904329A4224BFE3686361D4 +:102BF000E3690BB120469847002080F3118807E075 +:102C00003146204600F0EEFF0028E2DA87F3118813 +:102C1000F8BD0000D0E9052310B59A4201D18268C1 +:102C20007AB982680021013282605A1C82611C7864 +:102C300003699A4224BFC368836100F0E3FF204622 +:102C400010BD4FF0FF30FBE72DE9F74F04460E466D +:102C500017469846D0F81C904FF0300A8AF3118836 +:102C60004FF0000B154665B12A4631462046FFF766 +:102C7000EFFE034660B94146204600F0B3FF00284E +:102C8000F1D0002383F31188781B03B0BDE8F08FE7 +:102C9000B9F1000F03D001902046C847019B8BF388 +:102CA0001188ED1A1E448AF31188DCE70268436834 +:102CB0001143016003B11847704700001430FFF75B +:102CC00043BF00004FF0FF331430FFF73DBF00005B +:102CD0003830FFF7B9BF00004FF0FF333830FFF74F +:102CE000B3BF00001430FFF709BF00004FF0FF3101 +:102CF0001430FFF703BF00003830FFF763BF000058 +:102D00004FF0FF323830FFF75DBF0000012914BFDC +:102D10006FF0130000207047FFF76ABD044B03609B +:102D200000234360C0E9023301230374704700BFEE +:102D30006865000810B53023044683F31188FFF757 +:102D400081FD02230020237480F3118810BD000050 +:102D500038B5C36904460D461BB904210844FFF782 +:102D6000A5FF294604F11400FFF7ACFE002806DA9F +:102D7000201D4FF40061BDE83840FFF797BF38BD14 +:102D8000026843681143016003B1184770470000AF +:102D900013B5406B00F58054D4F8A4381A68117844 +:102DA000042914D1017C022911D119790123128936 +:102DB0008B4013420BD101A94C3002F097FCD4F8A0 +:102DC000A4480246019B2179206800F0DFF902B097 +:102DD00010BD0000143002F019BC00004FF0FF33AA +:102DE000143002F013BC00004C3002F0EBBC0000C9 +:102DF0004FF0FF334C3002F0E5BC0000143002F01D +:102E0000E7BB00004FF0FF31143002F0E1BB0000DF +:102E10004C3002F0B7BC00004FF0FF324C3002F0F3 +:102E2000B1BC00000020704710B500F58054D4F804 +:102E3000A4381A681178042917D1017C022914D109 +:102E40005979012352898B4013420ED1143002F07C +:102E500079FB024648B1D4F8A4484FF44073617935 +:102E60002068BDE8104000F07FB910BD406BFFF74F +:102E7000DBBF0000704700007FB5124B0125042620 +:102E8000044603600023057400F184024360294670 +:102E9000C0E902330C4B0290143001934FF440739D +:102EA000009602F02BFB094B04F69442294604F1EC +:102EB0004C000294CDE900634FF4407302F0F2FB42 +:102EC00004B070BD906500086D2E0008912D0008BB +:102ED0000A68302383F311880B790B3342F82300FF +:102EE0004B79133342F823008B7913B10B3342F83B +:102EF000230000F58053C3F8A418022303740020B4 +:102F000080F311887047000038B5037F044613B181 +:102F100090F85430ABB90125201D0221FFF730FF96 +:102F200004F114006FF00101257700F077FE04F141 +:102F30004C0084F854506FF00101BDE8384000F0B7 +:102F40006DBE38BD10B5012104460430FFF718FFEF +:102F50000023237784F8543010BD000038B50446B0 +:102F60000025143002F0E2FA04F14C00257702F05B +:102F7000B1FB201D84F854500121FFF701FF2046CA +:102F8000BDE83840FFF750BF90F8803003F0600391 +:102F9000202B06D190F881200023212A03D81F2A54 +:102FA00006D800207047222AFBD1C0E91D3303E078 +:102FB000034A426707228267C3670120704700BF48 +:102FC0002C22002037B500F58055D5F8A4381A68B2 +:102FD000117804291AD1017C022917D1197901230A +:102FE00012898B40134211D100F14C04204602F0AB +:102FF00031FC58B101A9204602F078FBD5F8A4486D +:103000000246019B2179206800F0C0F803B030BD72 +:1030100001F10B03F0B550F8236085B004460D466E +:10302000FEB1302383F3118804EB8507301D08219E +:10303000FFF7A6FEFB6806F14C005B691B681BB13D +:10304000019002F061FB019803A902F04FFB0246D8 +:1030500048B1039B2946204600F098F8002383F3EB +:10306000118805B0F0BDFB685A691268002AF5D0D6 +:103070001B8A013B1340F1D104F18002EAE7000012 +:10308000133138B550F82140ECB1302383F3118867 +:1030900004F58053D3F8A4281368527903EB820314 +:1030A000DB689B695D6845B104216018FFF768FE25 +:1030B000294604F1140002F04FFA2046FFF7B4FE4F +:1030C000002383F3118838BD7047000001F01CBD58 +:1030D00001234022002110B5044600F8303BFEF7E2 +:1030E00039FA0023C4E9013310BD000010B53023C4 +:1030F000044683F311882422416000210C30FEF73E +:1031000029FA204601F022FD02230020237080F3DB +:10311000118810BD70B500EB8103054650690E465D +:103120001446DA6018B110220021FEF713FAA069E4 +:1031300018B110220021FEF70DFA31462846BDE8ED +:10314000704001F009BE000083682022002103F0D6 +:10315000011310B5044683601030FEF7FBF92046DA +:10316000BDE8104001F084BEF0B4012500EB8104FD +:1031700047898D40E4683D43A4694581236000236D +:10318000A2606360F0BC01F0A1BE0000F0B40125B4 +:1031900000EB810407898D40E4683D436469058143 +:1031A00023600023A2606360F0BC01F017BF000041 +:1031B00070B5022300250446242203702946C0F876 +:1031C00088500C3040F8045CFEF7C4F9204684F8BF +:1031D000705001F055FD63681B6823B129462046F5 +:1031E000BDE87040184770BD037880F88C30052327 +:1031F000037043681B6810B504460BB1042198475F +:103200000023A36010BD000090F88C20436802707A +:103210001B680BB1052118477047000070B590F886 +:103220007030044613B1002380F8703004F180023E +:10323000204601F041FE63689B68B3B994F8803082 +:1032400013F0600535D00021204602F01DF9002161 +:10325000204602F00DF963681B6813B10621204671 +:103260009847062384F8703070BD204698470028A0 +:10327000E4D0B4F88630A26F9A4288BFA36794F96D +:103280008030A56F002B4FF0300380F20381002DBA +:1032900000F0F280092284F8702083F31188002165 +:1032A0002046D4E91D23FFF771FF002383F3118823 +:1032B000DAE794F8812003F07F0343EA022340F227 +:1032C0000232934200F0C58021D8B3F5807F48D008 +:1032D0000DD8012B3FD0022B00F09380002BB2D1F0 +:1032E00004F1880262670222A267E367C1E7B3F5CF +:1032F000817F00F09B80B3F5407FA4D194F88230A9 +:10330000012BA0D1B4F8883043F0020332E0B3F5CA +:10331000006F4DD017D8B3F5A06F31D0A3F5C063BF +:10332000012B90D86368204694F882205E6894F858 +:103330008310B4F88430B047002884D043686367B2 +:103340000368A3671AE0B3F5106F36D040F6024267 +:1033500093427FF478AF5C4B63670223A36700233B +:10336000C3E794F88230012B7FF46DAFB4F8883056 +:1033700023F00203A4F88830C4E91D55E56778E717 +:10338000B4F88030B3F5A06F0ED194F882302046A7 +:1033900084F88A3001F0D2FC63681B6813B1012104 +:1033A00020469847032323700023C4E91D339CE77C +:1033B00004F18B0363670123C3E72378042B10D147 +:1033C000302383F311882046FFF7BEFE85F3118872 +:1033D0000321636884F88B5021701B680BB1204671 +:1033E000984794F88230002BDED084F88B30042389 +:1033F000237063681B68002BD6D0022120469847B3 +:10340000D2E794F8843020461D0603F00F010AD558 +:1034100001F044FD012804D002287FF414AF2B4BA7 +:103420009AE72B4B98E701F02BFDF3E794F88230F5 +:10343000002B7FF408AF94F8843013F00F01B3D061 +:103440001A06204602D502F037F8ADE702F028F858 +:10345000AAE794F88230002B7FF4F5AE94F884301C +:1034600013F00F01A0D01B06204602D502F00CF885 +:103470009AE701F0FDFF97E7142284F8702083F3A8 +:1034800011882B462A4629462046FFF76DFE85F314 +:103490001188E9E65DB1152284F8702083F3118864 +:1034A00000212046D4E91D23FFF75EFEFDE60B2236 +:1034B00084F8702083F311882B462A46294620463B +:1034C000FFF764FEE3E700BFC0650008B8650008C9 +:1034D000BC65000838B590F870300446002B3ED02B +:1034E000063BDAB20F2A34D80F2B32D8DFE803F0CC +:1034F00037313108223231313131313131313737E1 +:10350000856FB0F886309D4214D2C3681B8AB5FB24 +:10351000F3F203FB12556DB9302383F311882B4668 +:103520002A462946FFF732FE85F311880A2384F8DC +:1035300070300EE0142384F87030302383F3118848 +:10354000002320461A461946FFF70EFE002383F398 +:10355000118838BDC36F03B198470023E7E7002106 +:10356000204601F091FF0021204601F081FF6368B1 +:103570001B6813B10621204698470623D7E70000B1 +:1035800010B590F870300446142B29D017D8062BAC +:1035900005D001D81BB110BD093B022BFBD800217F +:1035A000204601F071FF0021204601F061FF6368B1 +:1035B0001B6813B1062120469847062319E0152BF6 +:1035C000E9D10B2380F87030302383F31188002376 +:1035D0001A461946FFF7DAFD002383F31188DAE76C +:1035E000C3689B695B68002BD5D1C36F03B1984753 +:1035F000002384F87030CEE7FEF7FABF01210023E4 +:103600000170C0E901330C3000F0FEB910B5302371 +:10361000044683F311884160FEF7F8FF022300207F +:10362000237080F3118810BD10B53023044683F356 +:103630001188032304F8083BFFF716F84FF0FF3119 +:10364000204600F0B7FA002383F31188C01A18BF90 +:10365000012010BD38B50446302585F311880325B7 +:1036600004F8085BFFF71CF84FF0FF31204600F02C +:10367000A1FA002383F31188C01A18BF012038BDB6 +:1036800038B50446302585F31188042504F8085B15 +:10369000FFF72AF84FF0FF31204600F08BFA0023A5 +:1036A00083F31188C01A18BF012038BD10B530232C +:1036B000044683F31188FFF73DF8062323700023A7 +:1036C00083F3118810BD000010B53023044683F346 +:1036D0001188FFF749F802232370002383F3118830 +:1036E00010BD00000C3000F0ABB900000C3000F051 +:1036F000B1B9000004480121044B03600023C0E974 +:1037000001330C3000F080B96C2900200959000801 +:10371000CB1D083A23F00703591A521A012110B49D +:10372000D2080024C0E9004384600C301C605A6059 +:103730005DF8044B00F068B92DE9F84F364ECD1D09 +:103740000F46002818BF0646082A4FEAD50538BF9D +:10375000082206F10C08341D9146404600F070F92D +:1037600009F10701C9F1000E224624686CB94046F0 +:1037700000F070F93368CBB308224946E800984757 +:10378000044698B340E9026730E004EB010CD4F83A +:1037900004A00CEA0E0C0AF10100ACF1080304EBE2 +:1037A000C0009842E0D9A0EB0C0CB5EBEC0F4FEA4F +:1037B000EC0BD9D89C421CD204F10802AB45A3EB18 +:1037C00002024FEAE202626009D9691CED432068F7 +:1037D00003EBC1025D44556043F8310022601C4692 +:1037E0005F60404644F8086B00F034F92046BDE8BD +:1037F000F88FAA45216802D111602346EFE7013511 +:1038000004EBC50344F8351003F10801401AC01059 +:1038100058601360F1E700BF6C290020F8B550F83C +:10382000043C044650F8085CA0F1080607332F1D3D +:103830000C35DB0840F8043C284600F001F93B4613 +:103840009F421A6801D0B34228D20AB1964225D2CB +:1038500044F8082C54F8042C1E60013254F8081C5B +:1038600006EBC200814206D14868024444F8042CA9 +:103870000A6844F8082C5868411C03EBC1018E42C9 +:1038800007D154F8042C013202445A6054F8082C31 +:103890001A602846BDE8F84000F0DCB81346CFE7D0 +:1038A00000238268037503691B6899689142FBD203 +:1038B0005A68036042601060586070470023826855 +:1038C000037503691B6899689142FBD85A680360C5 +:1038D000426010605860704708B50846302383F393 +:1038E00011880A7D0023052A06D8DFE802F00B05BF +:1038F0000503120E826913604FF0FF338361FFF7F7 +:10390000CFFF002383F3118808BD826993680133D8 +:103910009360D0E9003213605A60EDE7FFF7C0BF53 +:10392000054BD96808751868026853601A6001224F +:10393000D8600275FCF7F2BE882900200C4B30B528 +:10394000DD684B1C87B004460FD02B46094A6846F9 +:1039500000F0F8F92046FFF7E3FF009B13B168463B +:1039600000F0FAF9A86907B030BDFFF7D9FFF9E711 +:1039700088290020D938000838B50C4D04468161EB +:10398000EB6881689A68914203D8BDE83840FFF738 +:1039900087BF1846FFF792FF01230146EC602046DF +:1039A0002375BDE83840FCF7B9BE00BF8829002068 +:1039B000044B1A68DB6890689B68984294BF0020AB +:1039C0000120704788290020084B10B51C68D86872 +:1039D000226853601A600122DC602275FFF76EFFD7 +:1039E00001462046BDE81040FCF798BE882900201B +:1039F000044B1A68DB6892689B689A4201D9FFF70A +:103A0000E3BF704788290020C0E90000816070474B +:103A10008368013B002B10B583600CDA074BDC6830 +:103A20004368A061206063601C6044600520FFF76C +:103A300077FFA06910BD0020FCE700BF88290020A7 +:103A400008B5302383F31188FFF7E2FF002383F3E7 +:103A5000118808BD08B5302383F3118883680133CA +:103A6000002B836007DC036800211A680260506045 +:103A70001846FFF781FF002383F3118808BD00007B +:103A800038B50123084C00252370656001F01EFF46 +:103A9000FFF730FE0549064801F042FF022323707C +:103AA00085F3118838BD00BFF02B0020C8650008E1 +:103AB0008829002008B572B6044B186500F032FD65 +:103AC00000F000FE024B03221A70FEE78829002056 +:103AD000F02B002000F076B9EFF3118020B9EFF35E +:103AE0000583302282F311887047000010B530B989 +:103AF000EFF30584C4F3080414B180F3118810BDFA +:103B0000FFF776FF84F31188F9E70000034A516854 +:103B100053685B1A9842FBD8704700BF001000E062 +:103B20008B600223086108468B8270478368A3F18B +:103B3000840243F8142C026943F8442C426943F888 +:103B4000402C094A43F8242CC268A3F1200043F812 +:103B5000182C022203F80C2C002203F80B2C034A29 +:103B600043F8102C704700BF350700088829002053 +:103B700008B5FFF7DBFFBDE80840FFF7CFBE000048 +:103B8000024BDB6898610F20FFF7CABE882900202E +:103B9000302383F31188FFF7F3BF000008B5014617 +:103BA000302383F311880820FFF7C8FE002383F336 +:103BB000118808BD054BDB6821B1036098610320C3 +:103BC000FFF7BCBE4FF0FF30704700BF88290020D0 +:103BD00003682BB10022026018469961FFF79EBE70 +:103BE00070470000064BDB6839B1426818605A60C4 +:103BF000136043600420FFF7A1BE4FF0FF30704711 +:103C0000882900200368984206D01A680260506034 +:103C100018469961FFF782BE7047000038B5044628 +:103C20000D462068844200D138BD036823605C6083 +:103C30008561FFF773FEF4E7036810B59C68A24244 +:103C40000CD85C688A600B604C6021605960996890 +:103C50008A1A9A604FF0FF33836010BD121B1B68F5 +:103C6000ECE700000A2938BF0A2170B504460D466A +:103C70000A26601901F024FE01F00CFE041BA54287 +:103C800003D8751C04462E46F3E70A2E04D90120FA +:103C9000BDE8704001F084BE70BD0000F8B5144B63 +:103CA0000D460A2A4FF00A07D96103F1100182601C +:103CB00038BF0A22416019691446016048601861E2 +:103CC000A81801F0EDFD01F0E5FD431B0646A342F7 +:103CD00006D37C1C28192746354601F0F1FDF2E792 +:103CE0000A2F04D90120BDE8F84001F059BEF8BD03 +:103CF00088290020F8B506460D4601F0CBFD0F4A95 +:103D0000134653F8107F9F4206D12A46014630469B +:103D1000BDE8F840FFF7C2BFD169BB68441A2C194F +:103D200028BF2C46A34202D92946FFF79BFF224613 +:103D300031460348BDE8F840FFF77EBF88290020E0 +:103D400098290020C0E90323002310B45DF8044B38 +:103D50004361FFF7CFBF000010B5194C23699842AB +:103D60000DD08168D0E9003213605A609A680A4425 +:103D70009A60002303604FF0FF33A36110BD026817 +:103D8000234643F8102F53600022026022699A42B2 +:103D900003D1BDE8104001F08DBD936881680B44EC +:103DA000936001F077FD2269E1699268441AA242AA +:103DB000E4D91144BDE81040091AFFF753BF00BF12 +:103DC000882900202DE9F047DFF8BC8008F11007B2 +:103DD0002C4ED8F8105001F05DFDD8F81C40AA68B0 +:103DE000031B9A423ED814444FF00009D5E9003233 +:103DF000C8F81C4013605A60C5F80090D8F810301D +:103E0000B34201D101F056FD89F31188D5E90331A0 +:103E100028469847302383F311886B69002BD8D04C +:103E200001F038FD6A69A0EB040982464A450DD2CB +:103E3000022001F0B5FD0022D8F81030B34208D1BD +:103E400051462846BDE8F047FFF728BF121A224422 +:103E5000F2E712EB09092946384638BF4A46FFF710 +:103E6000EBFEB5E7D8F81030B34208D01444C8F8D8 +:103E70001C00211AA960BDE8F047FFF7F3BEBDE8BA +:103E8000F08700BF98290020882900200020704773 +:103E9000FEE70000704700004FF0FF307047000061 +:103EA00002290CD0032904D00129074818BF00209B +:103EB0007047032A05D8054800EBC2007047044844 +:103EC00070470020704700BFA06600083C22002019 +:103ED0005466000870B59AB005460846144601A914 +:103EE00000F0C2F801A8FDF72DFB431C0022C6B26A +:103EF0005B001046C5E9003423700323023404F844 +:103F0000013C01ABD1B202348E4201D81AB070BD6F +:103F100013F8011B013204F8010C04F8021CF1E74C +:103F200008B5302383F311880348FFF74BF80023CB +:103F300083F3118808BD00BFF82B002090F8803073 +:103F400003F01F02012A07D190F881200B2A03D128 +:103F50000023C0E91D3315E003F06003202B08D1D6 +:103F6000B0F884302BB990F88120212A03D81F2A79 +:103F700004D8FFF709B8222AEBD0FAE7034A4267D0 +:103F800007228267C3670120704700BF33220020E9 +:103F900007B5052917D8DFE801F01916031919200C +:103FA000302383F31188104A01210190FFF7B2F802 +:103FB000019802210D4AFFF7ADF80D48FEF7CEFF3C +:103FC000002383F3118803B05DF804FB302383F3EF +:103FD00011880748FEF798FFF2E7302383F3118832 +:103FE0000348FEF7AFFFEBE7F4650008186600082A +:103FF000F82B002038B50C4D0C4C2A460C4904F126 +:104000000800FFF767FF05F1CA0204F11000094933 +:10401000FFF760FF05F5CA7204F118000649BDE814 +:104020003840FFF757BF00BFD04400203C2200209B +:10403000D4650008DE650008E965000870B504462F +:1040400008460D46FDF77EFAC6B2204601340378D5 +:104050000BB9184670BD32462946FDF753FA0028C1 +:10406000F3D10120F6E700002DE9F84F05460C4694 +:10407000FDF768FA2C49C6B22846FFF7DFFF08B102 +:104080000336F6B229492846FFF7D8FF08B11036A3 +:10409000F6B2632E0DD8DFF89080DFF89090244FB1 +:1040A000DFF894A0DFF894B02E7846B92670BDE80A +:1040B000F88F29462046BDE8F84F02F0FDB8252EBE +:1040C0002ED1072241462846FDF71CFA70B9DBF8CD +:1040D000003007350A3444F80A3CDBF8043044F871 +:1040E000063CBBF8083024F8023CDDE708224946CC +:1040F0002846FDF707FA98B9A21C0E4B1978023230 +:104100000909C95D02F8041C13F8011B01F00F0135 +:104110005345C95D02F8031CF0D118340835C3E7D4 +:10412000013504F8016BBFE7C0660008E9650008C7 +:10413000D366000800E8F11F0CE8F11FC86600080C +:10414000BFF34F8F044B1A695107FCD1D3F81021EC +:104150005207F8D1704700BF0020005208B50D4B40 +:104160001B78ABB9FFF7ECFF0B4BDA68D10704D52E +:104170000A4A5A6002F188325A60D3F80C21D207F9 +:1041800006D5064AC3F8042102F18832C3F8042197 +:1041900008BD00BF2E4700200020005223016745C4 +:1041A0004FF00063054A1968013104D1043393428A +:1041B000F9D1012070470020704700BF00000208BD +:1041C00008B5114B1B78F3B9104B1A69510703D589 +:1041D000DA6842F04002DA60D3F81021520705D5C0 +:1041E000D3F80C2142F04002C3F80C21FFF7A8FFDE +:1041F000064BDA6842F00102DA60D3F80C2142F093 +:104200000102C3F80C2108BD2E47002000200052F7 +:104210004FF40030704700000120704708B9FFF7E5 +:10422000BFBF00207047000038B548BB154CFFF7F2 +:1042300053FCFFF785FF0546FFF790FF4FF0FF3374 +:104240006361C4F814312361FFF77AFF2423E3602C +:10425000E36843F08003E36023695B07FCD4FFF766 +:104260006FFF4FF000604FF4003100F007FA28466E +:10427000FFF7A6FFFFF73AFCBDE83840FFF790BF15 +:10428000002038BD002000522DE9F84F40EA02031B +:1042900005460C461746D80602D00020BDE8F88F28 +:1042A00027F01F07DFF8D4B0FFF758FF2744BC42C0 +:1042B00003D10120FFF784FFF0E7202229462046A2 +:1042C00001F0A6FF10B920352034F0E72B4605F1A8 +:1042D00020021E68711CE0D104339A42F9D1FFF725 +:1042E000FBFB05F17843234AB3F5801F224B28BF1F +:1042F0009A4603F1040338BF9046A2F1080228BF92 +:104300009846A3F108033ABF9146DA469946FFF76B +:1043100017FFC8F80060A5EB040CD9F8002004F1E1 +:104320001C0142F00202C9F80020221FDAF80060E6 +:1043300016F00506FAD152F8043F8A424CF80230D2 +:10434000F4D1BFF34F8FFFF7FBFE4FF0FF32C8F8F9 +:104350000020D9F8002022F00202C9F80020FFF75F +:10436000C5FB20222146284601F052FF0028AAD092 +:1043700030469FE7142000521021005210200052B6 +:1043800010B5084C237828B11BB9FFF7E7FE0123CD +:10439000237010BD002BFCD02070BDE81040FFF74B +:1043A0000FBF00BF2E4700202DE9F74F0E460546F0 +:1043B000002864D011F0050763D139B908222946D5 +:1043C0003046FFF7B9F90446002848D108224FF0DB +:1043D0000109DFF8C08006F00403DFF8BCA006EA9C +:1043E000090BBBF1000F27D0D8F81410C80723D44D +:1043F00009F1010908F10C08B9F1060FF1D18FB9E3 +:10440000224E2946F0190092FFF796F9044630BB78 +:104410001837009A782FF4D12946FFF78DF9044612 +:10442000E8B9009A29461A48FFF786F9044608BBFE +:10443000204603B0BDE8F08FABB1D8F8141011F0EE +:10444000040FD5D01820294600FB09A0CDE9002390 +:10445000FFF772F90446DDE900230028C8D02A4698 +:104460000021204608E0B107ECD5D8F8141011F06F +:10447000020FE6E72A460021FDF76CF8D8E704466C +:10448000D6E71F35202225F01F05A0E76047002052 +:1044900030470020E4660008484700200021FFF76D +:1044A00083BF00000121FFF77FBF0000F8B5144D66 +:1044B00001241827134E40F2FF3200210120FDF79E +:1044C00049F807FB046001342A6955F80C1FFFF70F +:1044D0001FF9062CF5D137254FF4C0542046FFF7BD +:1044E000E1FF014628B122460748BDE8F840FFF742 +:1044F0000FB9C4EBC404013D4FEAD404EED1F8BDBA +:10450000E466000848470020304700200421FFF7F8 +:104510004BBF00004843FFF7C1BF000008B1FFF7E1 +:104520007DB9704738B5054D00240334696855F8E6 +:104530000C0B00F0B5F8122CF7D138BDE46600087A +:1045400070B5104E82B0FFF7C7FA054601F0A2F928 +:10455000326803469042336037BF0B4A0A495168BC +:10456000146836BF0131D1E9004151600419284671 +:1045700041F100010191FFF7B9FA2046019902B01B +:1045800070BD00BFD8470020E047002070B5124E34 +:1045900082B0FFF7A1FA054601F07CF932680346C4 +:1045A0009042336037BF0D4A0C495168146836BFDA +:1045B0000131D1E9004151600419284641F100015F +:1045C0000191FFF793FA4FF47A7200232046019984 +:1045D000FBF78EFE02B070BDD8470020E0470020F8 +:1045E0000244074BD2B210B5904200D110BD441C1A +:1045F00000B253F8200041F8040BE0B2F4E700BF2A +:10460000504000580E4B30B51C6F240405D41C6F6D +:104610001C671C6F44F400441C670A4C0244236866 +:10462000D2B243F480732360074B904200D130BD77 +:10463000441C51F8045B00B243F82050E0B2F4E7A8 +:1046400000440258004802585040005807B5012263 +:1046500001A90020FFF7C4FF019803B05DF804FB37 +:1046600013B50446FFF7F2FFA04205D0012201A9CD +:1046700000200194FFF7C6FF02B010BD0144BFF354 +:104680004F8F064B884204D3BFF34F8FBFF36F8F1A +:104690007047C3F85C022030F4E700BF00ED00E093 +:1046A0000144BFF34F8F064B884204D3BFF34F8FB3 +:1046B000BFF36F8F7047C3F870022030F4E700BF7C +:1046C00000ED00E07047000070B5054616460C4648 +:1046D00001201021FFF71EFF286046733CB12046E1 +:1046E00036B1FFF713FF2B68186000B19C6070BDF6 +:1046F000FFF7D8FEF7E7000070B50E4615460446F2 +:1047000000B30B6843608368934210D213B1006812 +:10471000FFF704FF637B28462BB1FFF7F7FE20600D +:1047200020B9A06070BDFFF7BDFEF8E7A560206866 +:1047300005F11F01306021F01F01FFF79FFF0120ED +:104740002073EFE70120EDE710B5044640B10068A3 +:10475000884205D1606808B1FCF7CAFE00232373C4 +:1047600010BD000070B50E461546044620B38368A0 +:104770009A4210D913B10068FFF7D0FE637B284638 +:104780002BB1FFF7C3FE206020B9A06070BDFFF71A +:1047900089FEF8E7A560316819B12A462068FCF760 +:1047A000A7FE206805F11F01306021F01F01FFF70F +:1047B00077FF01202073E9E70120E7E720B10368D4 +:1047C0008B4204BF002303737047000008B100232D +:1047D00003737047034B1A681AB9034AD2F8D024FE +:1047E0001A607047E84700200040025808B5FFF7FC +:1047F000F1FF024B1868C0F3806008BDE847002055 +:1048000070B5BFF34F8FBFF36F8F1A4A0021C2F804 +:104810005012BFF34F8FBFF36F8F536943F40033D0 +:104820005361BFF34F8FBFF36F8FC2F88410BFF394 +:104830004F8FD2F8803043F6E074C3F3C900C3F35E +:104840004E335B0103EA0406014646EA81750139ED +:10485000C2F86052F9D2203B13F1200FF2D1BFF31E +:104860004F8F536943F480335361BFF34F8FBFF3CE +:104870006F8F70BD00ED00E0FEE700000A4B0B48B3 +:104880000B4A90420BD30B4BC11EDA1C121A22F0BA +:1048900003028B4238BF00220021FCF75BBE53F8B5 +:1048A000041B40F8041BECE7B0690008904A0020A4 +:1048B000904A0020904A00207047000003681A6860 +:1048C00099685A6043681BB142EA01418068184701 +:1048D000704700004FF0A44310B51C68E00702D5F4 +:1048E0002848FFF7EBFFA10702D52748FFF7E6FFAF +:1048F000620702D52548FFF7E1FF230702D52448C8 +:10490000FFF7DCFFE00602D52248FFF7D7FFA1063C +:1049100002D52148FFF7D2FF620602D51F48FFF7F4 +:10492000CDFF230602D51E48FFF7C8FFE00502D5DC +:104930001C48FFF7C3FFA10502D51B48FFF7BEFFC8 +:10494000620502D51948FFF7B9FF230502D51848BB +:10495000FFF7B4FFE00402D51648FFF7AFFFA1044C +:1049600002D51548FFF7AAFF620402D51348FFF7E6 +:10497000A5FF230402D51248FFF7A0FFBDE81040B1 +:1049800001F000B9F0470020FC4700200848002053 +:1049900014480020204800202C48002038480020DF +:1049A00044480020504800205C480020684800200F +:1049B00074480020804800208C480020984800203F +:1049C000A448002030B5094B0021094C10200725D0 +:1049D000196054F8042B0138C3E90121D16003F1B7 +:1049E0000C0311615560F4D130BD00BFEC470020CD +:1049F0002C6700080F28F0B50AD9102806D10F231C +:104A00000020144C01272568984203D900201DE09E +:104A10000346F6E707FA00F6354218D10C23354372 +:104A200043432560181D2344C3E902120A4B204466 +:104A3000D3F8D42042F00102C3F8D420D3F8FC20EC +:104A400042F00102C3F8FC20D3F8FC30F0BD013085 +:104A5000DAE700BFEC470020004402580368D96839 +:104A6000DA68C90722F03F02DA6002D51A689207B5 +:104A7000FCD507225A607047074BD3F8D81021EABB +:104A80000001C3F8D810D3F8002122EA0002C3F8CD +:104A90000021D3F8003170470044025870B5D0E9C6 +:104AA000244300224FF0FF359E6804EB421351016E +:104AB000D3F80009002805DAD3F8000940F0804057 +:104AC000C3F80009D3F8000B002805DAD3F8000B6F +:104AD00040F08040C3F8000B013263189642C3F8DF +:104AE0000859C3F8085BE0D24FF00113C4F81C3832 +:104AF00070BD0000890141F02001016103699B063E +:104B0000FCD41220FFF702B810B50A4C2046FEF77D +:104B1000DFFA094BC4F89030084BC4F89430084CC5 +:104B20002046FEF7D5FA074BC4F89030064BC4F880 +:104B3000943010BDB0480020000008409067000885 +:104B40004C490020000004409C67000870B50378C1 +:104B50000546012B5CD1434BD0F89040984258D188 +:104B6000414B0E216520D3F8D82042F00062C3F8F3 +:104B7000D820D3F8002142F00062C3F80021D3F816 +:104B80000021D3F8802042F00062C3F88020D3F8DF +:104B9000802022F00062C3F88020D3F8803000F03B +:104BA00097FC324BE360324BC4F800380023D5F851 +:104BB0009060C4F8003EC02323604FF40413A36345 +:104BC0003369002BFCDA01230C203361FEF79EFFD2 +:104BD0003369DB07FCD41220FEF798FF3369002B02 +:104BE000FCDA00262846A660FFF758FF6B68C4F879 +:104BF0001068DB68C4F81468C4F81C6883BB1D4BDC +:104C0000A3614FF0FF336361A36843F00103A36026 +:104C100070BD194B9842C9D1134B4FF08060D3F847 +:104C2000D82042F00072C3F8D820D3F8002142F017 +:104C30000072C3F80021D3F80021D3F8802042F09D +:104C40000072C3F88020D3F8802022F00072C3F8ED +:104C50008020D3F88030FFF70FFF0E214D209EE714 +:104C6000064BCDE7B04800200044025840140040F5 +:104C700003002002003C30C04C490020083C30C0FA +:104C8000F8B5D0F89040054600214FF00066204668 +:104C9000FFF730FFD5F8941000234FF001128F6812 +:104CA0004FF0FF30C4F83438C4F81C2804EB43122A +:104CB00001339F42C2F80069C2F8006BC2F80809CC +:104CC000C2F8080BF2D20B68D5F89020C5F89830DE +:104CD000636210231361166916F01006FBD11220CF +:104CE000FEF714FFD4F8003823F4FE63C4F800384C +:104CF000A36943F4402343F01003A3610923C4F8DC +:104D00001038C4F814380B4BEB604FF0C043C4F8B4 +:104D1000103B094BC4F8003BC4F81069C4F80039D3 +:104D2000D5F8983003F1100243F48013C5F89820A9 +:104D3000A362F8BD6C67000840800010D0F8902096 +:104D400090F88A10D2F8003823F4FE6343EA011386 +:104D5000C2F80038704700002DE9F84300EB8103EA +:104D6000D0F890500C468046DA680FFA81F9480175 +:104D7000166806F00306731E022B05EB41134FF075 +:104D8000000194BFB604384EC3F8101B4FF0010168 +:104D900004F1100398BF06F1805601FA03F39169FC +:104DA00098BF06F5004600293AD0578A04F1580109 +:104DB000374349016F50D5F81C180B430021C5F843 +:104DC0001C382B180127C3F81019A7405369611E1E +:104DD0009BB3138A928B9B08012A88BF5343D8F850 +:104DE0009820981842EA034301F140022146C8F88E +:104DF0009800284605EB82025360FFF77BFE08EB24 +:104E00008900C3681B8A43EA845348341E43640103 +:104E10002E51D5F81C381F43C5F81C78BDE8F8831F +:104E200005EB4917D7F8001B21F40041C7F8001B18 +:104E3000D5F81C1821EA0303C0E704F13F030B4A2D +:104E40002846214605EB83035A60FFF753FE05EB26 +:104E50004910D0F8003923F40043C0F80039D5F8E0 +:104E60001C3823EA0707D7E700800010000400027F +:104E7000D0F894201268C0F89820FFF70FBE000009 +:104E80005831D0F8903049015B5813F4004004D0F9 +:104E900013F4001F0CBF0220012070474831D0F8E6 +:104EA000903049015B5813F4004004D013F4001F04 +:104EB0000CBF02200120704700EB8101CB68196A0A +:104EC0000B6813604B6853607047000000EB810370 +:104ED00030B5DD68AA691368D36019B9402B84BF67 +:104EE000402313606B8A1468D0F890201C4402EBB6 +:104EF0004110013C09B2B4FBF3F46343033323F0E4 +:104F0000030343EAC44343F0C043C0F8103B2B689B +:104F100003F00303012B0ED1D2F8083802EB411045 +:104F200013F4807FD0F8003B14BF43F0805343F06C +:104F30000053C0F8003B02EB4112D2F8003B43F0B3 +:104F40000443C2F8003B30BD2DE9F041D0F8906039 +:104F500005460C4606EB4113D3F8087B3A07C3F825 +:104F6000087B08D5D6F814381B0704D500EB81035D +:104F7000DB685B689847FA071FD5D6F81438DB075B +:104F80001BD505EB8403D968CCB98B69488A5A686C +:104F9000B2FBF0F600FB16228AB91868DA68904274 +:104FA0000DD2121AC3E90024302383F3118821465D +:104FB0002846FFF78BFF84F31188BDE8F0810123B9 +:104FC00003FA04F26B8923EA02036B81CB68002B9E +:104FD000F3D021462846BDE8F041184700EB810395 +:104FE0004A0170B5DD68D0F890306C692668E660DB +:104FF00056BB1A444FF40020C2F810092A6802F088 +:105000000302012A0AB20ED1D3F8080803EB4214B6 +:1050100010F4807FD4F8000914BF40F0805040F0B5 +:105020000050C4F8000903EB4212D2F8000940F026 +:105030000440C2F800090122D3F8340802FA01F151 +:105040000143C3F8341870BD19B9402E84BF402005 +:10505000206020681A442E8A8419013CB4FBF6F4BF +:1050600040EAC44040F00050C6E700002DE9F84394 +:10507000D0F8906005460C464F0106EB4113D3F87B +:10508000088918F0010FC3F808891CD0D6F8103829 +:10509000DB0718D500EB8103D3F80CC0DCF8143023 +:1050A000D3F800E0DA68964530D2A2EB0E024FF05A +:1050B00000091A60C3F80490302383F31188FFF7C6 +:1050C0008DFF89F3118818F0800F1DD0D6F8343881 +:1050D0000126A640334217D005EB84030134D5F8EE +:1050E0009050D3F80CC0E4B22F44DCF8142005EB48 +:1050F0000434D2F800E05168714514D3D5F834383F +:1051000023EA0606C5F83468BDE8F883012303FAEC +:1051100001F2038923EA02030381DCF80830002B43 +:10512000D1D09847CFE7AEEB0103BCF81000834223 +:1051300028BF0346D7F8180980B2B3EB800FE3D835 +:105140009068A0F1040959F8048FC4F80080A0EB1E +:1051500009089844B8F1040FF5D818440B4490603E +:105160005360C8E72DE9F84FD0F8905004466E69B7 +:10517000AB691E4016F480586E6103D0BDE8F84F4D +:10518000FEF716B8002E12DAD5F8003E9F0705D0BC +:10519000D5F8003E23F00303C5F8003ED5F80438E7 +:1051A000204623F00103C5F80438FEF72DF830053A +:1051B00005D52046FFF772FC2046FEF715F8B1042E +:1051C0000CD5D5F8083813F0060FEB6823F47053AC +:1051D0000CBF43F4105343F4A053EB60320704D5E3 +:1051E0006368DB680BB120469847F30200F1BA8090 +:1051F000B70226D5D4F8909000274FF0010A09EBAA +:105200004712D2F8003B03F44023B3F5802F11D1AD +:10521000D2F8003B002B0DDA62890AFA07F322EA82 +:105220000303638104EB8703DB68DB6813B1394652 +:10523000204698470137D4F89430FFB29B689F42CC +:10524000DDD9F00619D5D4F89000026AC2F30A1726 +:1052500002F00F0302F4F012B2F5802F00F0CA80C2 +:10526000B2F5402F09D104EB8303002200F58050F2 +:10527000DB681B6A974240F0B0803003D5F81858BD +:1052800035D5E90303D500212046FFF75DFEAA03CB +:1052900003D501212046FFF757FE6B0303D50221FA +:1052A0002046FFF751FE2F0303D503212046FFF7C9 +:1052B0004BFEE80203D504212046FFF745FEA90274 +:1052C00003D505212046FFF73FFE6A0203D50621DC +:1052D0002046FFF739FE2B0203D507212046FFF7B2 +:1052E00033FEEF0103D508212046FFF72DFE7003A2 +:1052F00040F1A780E90703D500212046FFF7B6FE5D +:10530000AA0703D501212046FFF7B0FE6B0703D59E +:1053100002212046FFF7AAFE2F0703D503212046CE +:10532000FFF7A4FEEE0603D504212046FFF79EFEFC +:10533000A80603D505212046FFF798FE690603D588 +:1053400006212046FFF792FE2A0603D507212046B4 +:10535000FFF78CFEEB0574D520460821BDE8F84F19 +:10536000FFF784BED4F890904FF0000B4FF0010A85 +:10537000D4F894305FFA8BF79B689F423FF638AFC2 +:1053800009EB4713D3F8002902F44022B2F5802F2D +:1053900020D1D3F80029002A1CDAD3F8002942F0E2 +:1053A0009042C3F80029D3F80029002AFBDB3946D4 +:1053B000D4F89000FFF79EFB22890AFA07F322EA4D +:1053C0000303238104EB8703DB689B6813B1394631 +:1053D000204698470BF1010BCAE7910701D1D0F89D +:1053E0000080072A02F101029CBF03F8018B4FEAFB +:1053F00018283FE704EB830300F58050DA68D2F801 +:1054000018C0DCF80820DCE9001CA1EB0C0C002122 +:105410008F4208D1DB689B699A683A449A605A685F +:105420003A445A6029E711F0030F01D1D0F8008007 +:105430008C4501F1010184BF02F8018B4FEA182865 +:10544000E6E7BDE8F88F000008B50348FFF78AFEDD +:10545000BDE8084000F096BBB048002008B50348FE +:10546000FFF780FEBDE8084000F08CBB4C490020EF +:10547000D0F8903003EB4111D1F8003B43F4001316 +:10548000C1F8003B70470000D0F8903003EB4111A9 +:10549000D1F8003943F40013C1F800397047000017 +:1054A000D0F8903003EB4111D1F8003B23F4001306 +:1054B000C1F8003B70470000D0F8903003EB411179 +:1054C000D1F8003923F40013C1F800397047000007 +:1054D000090100F16043012203F56143C9B283F879 +:1054E000001300F01F039A4043099B0003F160433F +:1054F00003F56143C3F880211A60704730B5043367 +:10550000039C0172002104FB0325C160C0E906531E +:10551000049B0363059BC0E90000C0E90422C0E9C5 +:105520000842C0E90A11436330BD00000022416A0D +:10553000C260C0E90411C0E90A226FF00101FEF760 +:105540006DBB0000D0E90432934201D1C2680AB9B0 +:10555000181D704700207047036919600021C26858 +:105560000132C260C269134482699342036124BF5D +:10557000436A0361FEF746BB38B504460D46E3684F +:105580003BB162690020131D1268A3621344E362F9 +:1055900007E0237A33B929462046FEF723FB00288B +:1055A000EDDA38BD6FF00100FBE70000C368C269A7 +:1055B000013BC3604369134482699342436124BF42 +:1055C000436A436100238362036B03B1184770474A +:1055D00070B53023044683F31188866A3EB9FFF71D +:1055E000CBFF054618B186F31188284670BDA36A23 +:1055F000E26A13F8015B9342A36202D32046FFF7ED +:10560000D5FF002383F31188EFE700002DE9F84F61 +:1056100004460E46174698464FF0300989F3118824 +:105620000025AA46D4F828B0BBF1000F09D14146A5 +:105630002046FFF7A1FF20B18BF311882846BDE873 +:10564000F88FD4E90A12A7EB050B521A934528BF2D +:105650009346BBF1400F1BD9334601F1400251F88C +:10566000040B914243F8040BF9D1A36A403640354C +:105670004033A362D4E90A239A4202D32046FFF7BB +:1056800095FF8AF31188BD42D8D289F31188C9E702 +:1056900030465A46FBF72CFFA36A5E445D445B44E8 +:1056A000A362E7E710B5029C0433017203FB0421F7 +:1056B000C460C0E906130023C0E90A33039B0363F7 +:1056C000049BC0E90000C0E90422C0E9084243632A +:1056D00010BD0000026A6FF00101C260426AC0E9B9 +:1056E00004220022C0E90A22FEF798BAD0E9042376 +:1056F0009A4201D1C26822B9184650F8043B0B60A7 +:105700007047002070470000C3680021C269013360 +:10571000C3604369134482699342436124BF436A6F +:105720004361FEF76FBA000038B504460D46E368E2 +:105730003BB1236900201A1DA262E2691344E362AF +:1057400007E0237A33B929462046FEF74BFA0028B2 +:10575000EDDA38BD6FF00100FBE700000369196066 +:10576000C268013AC260C26913448269934203610C +:1057700024BF436A036100238362036B03B11847AC +:105780007047000070B530230D460446114683F380 +:105790001188866A2EB9FFF7C7FF10B186F311880A +:1057A00070BDA36A1D70A36AE26A01339342A362CB +:1057B00004D3E16920460439FFF7D0FF002080F3CD +:1057C0001188EDE72DE9F84F04460D4690469946BD +:1057D0004FF0300A8AF311880026B346A76A4FB902 +:1057E00049462046FFF7A0FF20B187F311883046D5 +:1057F000BDE8F88FD4E90A073A1AA8EB06079742E2 +:1058000028BF1746402F1BD905F1400355F8042B3C +:105810009D4240F8042BF9D1A36A40364033A3627D +:10582000D4E90A239A4204D3E16920460439FFF7F8 +:1058300095FF8BF311884645D9D28AF31188CDE7BD +:1058400029463A46FBF754FEA36A3D443E443B4496 +:10585000A362E5E7D0E904239A4217D1C3689BB15C +:10586000836A8BB1043B9B1A0ED01360C368013B63 +:10587000C360C3691A4483699A42026124BF436AC0 +:105880000361002383620123184670470023FBE76E +:1058900000F0A2B9014B586A704700BF000C0040ED +:1058A000034B002258631A610222DA60704700BF7E +:1058B000000C0040014B0022DA607047000C0040F1 +:1058C000014B5863704700BF000C0040024B034A75 +:1058D0001A60034A5A607047004A0020904A00202C +:1058E00000000220074B494210B55C68201A0840AE +:1058F0001968821A8A4203D3A24201D85A6010BDA5 +:105900000020FCE7004A002008B5302383F311880B +:10591000FFF7E8FF002383F3118808BDFEE70000CE +:1059200070B51B4B0025044686B058600E46856254 +:10593000016300F0FFF804F11003A560E562C4E91B +:1059400004334FF0FF33C4E90044C4E90635FFF7E0 +:10595000A1FF2B46024604F134012046C4E9082386 +:1059600080230C4A2565FEF7DBF801230A4AE06034 +:1059700000920375684672680192B268CDE902230D +:10598000064BCDE90435FEF7F3F806B070BD00BF55 +:10599000F02B0020A8670008AD6700081D5900081B +:1059A000024AD36A1843D062704700BF882900209A +:1059B0004B6843608B688360CB68C3600B6943614D +:1059C0004B6903628B6943620B6803607047000098 +:1059D00008B5354B40F2FF70D3F8E0100143C3F82F +:1059E000E010D3F8082102433048C3F80821304AB8 +:1059F000D3F808311146FFF7DBFF00F5806002F1B4 +:105A00001C01FFF7D5FF00F5806002F13801FFF7B8 +:105A1000CFFF00F5806002F15401FFF7C9FF00F5E8 +:105A2000806002F17001FFF7C3FF00F5806002F1B2 +:105A30008C01FFF7BDFF00F5806002F1A801FFF7C0 +:105A4000B7FF00F5806002F1C401FFF7B1FF00F578 +:105A5000806002F1E001FFF7ABFF00F5806002F12A +:105A6000FC01FFF7A5FF02F58C7100F58060FFF7E0 +:105A70009FFF00F079F90F4BD3F8902242F001021A +:105A8000C3F89022D3F8942242F00102C3F8942282 +:105A90000522C3F898204FF06052C3F89C20064AB4 +:105AA000C3F8A02008BD00BF0044025800000258FF +:105AB000B467000800ED00E01F00080308B500F01F +:105AC0004BFBFDF7DDFF104BD3F8DC2042F040022A +:105AD000C3F8DC20D3F8042122F04002C3F80421EB +:105AE000D3F80431094B1A6842F008021A601A68A8 +:105AF00042F004021A60FEF76DFEFEF7D7FCBDE827 +:105B00000840FEF777BA00BF004402580018024868 +:105B100070470000EFF30983054968334A6B22F0B0 +:105B200001024A6383F30988002383F311887047D5 +:105B300000EF00E0302080F3118862B60D4B0E4A72 +:105B4000D96821F4E0610904090C0A430B49DA60C1 +:105B5000D3F8FC2042F08072C3F8FC20084AC2F857 +:105B6000B01F116841F0010111602022DA7783F83B +:105B70002200704700ED00E00003FA0555CEACC5E9 +:105B8000001000E0302310B583F311880E4B5B68E2 +:105B900013F4006314D0F1EE103AEFF309844FF0E0 +:105BA0008073683CE361094BDB6B236684F30988EF +:105BB000FDF7FEFE10B1064BA36110BD054BFBE7E0 +:105BC00083F31188F9E700BF00ED00E000EF00E08B +:105BD000470700084A070008114BD3F8E82042F0B5 +:105BE0000802C3F8E820D3F8102142F00802C3F8F5 +:105BF00010210C4AD3F81031D36B43F00803D36360 +:105C0000EF22094B9A624FF0FF32DA6200229A616A +:105C10005A63DA605A6001225A611A60704700BF05 +:105C2000004402580010005C000C0040094A08B50E +:105C30001169D3680B40D9B29B076FEA010111616A +:105C400007D5302383F31188FDF744FF002383F346 +:105C5000118808BD000C0040064BD3F8DC2002433D +:105C6000C3F8DC20D3F804211043C3F80401D3F8AF +:105C7000043170470044025808B54FF0FF31394BEA +:105C8000D3F88020C3F88010D3F880200022C3F816 +:105C90008020D3F88000D3F88400C3F88410D3F8B0 +:105CA0008400C3F88420D3F88400D86F40F0FF400C +:105CB00040F4FF0040F4DF4040F07F00D867D86F29 +:105CC00020F0FF4020F4FF0020F4DF4020F07F00B0 +:105CD000D867D86FD3F888006FEA40506FEA505009 +:105CE000C3F88800D3F88800C0F30A00C3F888001E +:105CF000D3F88800D3F89000C3F89010D3F8900040 +:105D0000C3F89020D3F89000D3F89400C3F894100F +:105D1000D3F89400C3F89420D3F89400D3F89800F3 +:105D2000C3F89810D3F89800C3F89820D3F89800D7 +:105D3000D3F88C00C3F88C10D3F88C00C3F88C20F7 +:105D4000D3F88C00D3F89C00C3F89C10D3F89C10B7 +:105D5000C3F89C20D3F89C30FEF734FEBDE8084021 +:105D600000F0CAB90044025808B50122564BC3F8E6 +:105D70000821564BD3F8F42042F00202C3F8F42075 +:105D8000D3F81C2142F00202C3F81C210222D3F8EE +:105D90001C314F4BDA605A689104FCD54D4A4E498C +:105DA0001A6001229A60D960002119614FF44041C4 +:105DB00099614A4BDA6203F511339A699204FCD572 +:105DC0001A6842F480721A60404B1A6F12F4407FD6 +:105DD00004D04FF480321A6700221A671A6842F022 +:105DE00001021A60394B1A685007FCD500221A616B +:105DF0001A6912F03802FBD1012119604FF080417D +:105E000059604FF00051D9605A67354ADA62354A15 +:105E10001A611A6842F480321A602C4B1A68910396 +:105E2000FCD51A6842F480521A601A689204FCD5B4 +:105E30002D4A2E499A6200225A63196301F57A01AC +:105E4000DA6301F2D31199635A64294A1A64294A20 +:105E5000DA621A6842F0A8521A601C4B1A6802F003 +:105E60002852B2F1285FF9D148229A614FF4886232 +:105E7000DA6140221A62204ADA64204A1A65204A0E +:105E80005A65204A9A6534231F4A1360136803F049 +:105E90000F03042BFAD10D4A136943F00303136176 +:105EA000136903F03803182BFAD14FF00050FFF7B5 +:105EB000D3FE4FF08040FFF7CFFE4FF00040BDE82B +:105EC0000840FFF7C9BE00BF0080005100440258DF +:105ED0000048025800C000F0020000010004005811 +:105EE0000000FF01008890081210200077020B01CB +:105EF000470E0508DD0BBF01200000200000011047 +:105F00000910E00000010110002000524FF0B042E3 +:105F100008B5D2F8883003F00103C2F8883023B105 +:105F2000044A13680BB150689847BDE80840FFF772 +:105F300029BE00BF084A00204FF0B04208B5D2F891 +:105F4000883003F00203C2F8883023B1044A936812 +:105F50000BB1D0689847BDE80840FFF713BE00BFFB +:105F6000084A00204FF0B04208B5D2F8883003F05C +:105F70000403C2F8883023B1044A13690BB1506995 +:105F80009847BDE80840FFF7FDBD00BF084A002064 +:105F90004FF0B04208B5D2F8883003F00803C2F8D9 +:105FA000883023B1044A93690BB1D0699847BDE8A2 +:105FB0000840FFF7E7BD00BF084A00204FF0B0429D +:105FC00008B5D2F8883003F01003C2F8883023B146 +:105FD000044A136A0BB1506A9847BDE80840FFF7BE +:105FE000D1BD00BF084A00204FF0B04310B5D3F830 +:105FF000884004F47872C3F88820A30604D5124AB6 +:10600000936A0BB1D06A9847600604D50E4A136BA9 +:106010000BB1506B9847210604D50B4A936B0BB11B +:10602000D06B9847E20504D5074A136C0BB1506C4E +:106030009847A30504D5044A936C0BB1D06C9847DC +:10604000BDE81040FFF79EBD084A00204FF0B04366 +:1060500010B5D3F8884004F47C42C3F88820620568 +:1060600004D5164A136D0BB1506D9847230504D51E +:10607000124A936D0BB1D06D9847E00404D50F4AD6 +:10608000136E0BB1506E9847A10404D50B4A936E62 +:106090000BB1D06E9847620404D5084A136F0BB158 +:1060A000506F9847230404D5044A936F0BB1D06F07 +:1060B0009847BDE81040FFF765BD00BF084A0020C3 +:1060C00008B50348FCF75EFBBDE80840FFF75ABD82 +:1060D000E024002008B50348FCF706FCBDE80840B2 +:1060E000FFF750BD0025002008B5FFF79FFDBDE874 +:1060F0000840FFF747BD0000062108B50846FFF736 +:10610000E7F906210720FFF7E3F906210820FFF74A +:10611000DFF906210920FFF7DBF906210A20FFF746 +:10612000D7F906211720FFF7D3F906212820FFF71A +:10613000CFF909217A20FFF7CBF90A215C20FFF77C +:10614000C7F907213220FFF7C3F90C212520BDE84C +:106150000840FFF7BDB9000008B5FFF78DFD00F05E +:106160000FF8FCF7D9FDFCF7B1FFFCF783FEFDF754 +:1061700043FAFFF7CDFCBDE80840FFF789BB0000FC +:106180000023054A19460133102BC2E9001102F120 +:106190000802F8D1704700BF084A00200B460146AC +:1061A000184600F027B80000FEF7B8B9FFF7FCBFAB +:1061B000012838BF012010B504462046FEF76EF9CD +:1061C00030B900F007F808B900F00CF88047F4E7A0 +:1061D00010BD0000024B1868BFF35B8F704700BF13 +:1061E000884A002008B5062000F04AF80120FDF793 +:1061F0004FFE000010B5054C13462CB10A4601466F +:106200000220AFF3008010BD2046FCE70000000034 +:1062100010B501390244904201D1002005E0037815 +:1062200011F8014FA34201D0181B10BD0130F2E755 +:106230001F2938B504460D4604D9162303604FF0D4 +:10624000FF3038BD426C12B152F821304BB92046B4 +:1062500000F030F82A4601462046BDE8384000F0FC +:1062600017B8012B0AD0591C03D116230360012053 +:10627000E7E7002442F82540284698470020E0E759 +:10628000024B01461868FFF7D3BF00BF5C22002015 +:1062900038B5074D00230446084611462B60FDF72C +:1062A000FBFD431C02D12B6803B1236038BD00BF46 +:1062B0008C4A0020FDF7EABD034611F8012B03F8D4 +:1062C000012B002AF9D170470000000001000000F6 +:1062D00000010001000000000000000000000000BC +:1062E00052657175657374656420746F20657261A1 +:1062F0007365206D6F7265207468616E207765200C +:1063000063616E0A00457261736520436F6D6D6154 +:106310006E642052656365697665640A0050435473 +:1063200020444F4E453A2025640A000053544D3313 +:106330003248373F3F3F0053544D333248373433B0 +:106340002F37353300000000F82B002000250020F7 +:10635000773235712D647472000000000000000077 +:106360000000000000000000000000004D170008C1 +:10637000B11A0008B11A0008511700085517000893 +:10638000591700085D170008B11A0008B11A000873 +:10639000B11A00087517000879170008911700084E +:1063A00099170008A5170008A9170008AD170008DD +:1063B000E0240020010000000000000000000000B8 +:1063C000CD180008E51800084D170008B917000897 +:1063D00019190008D5170008B1190008E1190008BB +:1063E0005D170008D1170008BD170008CD17000879 +:1063F0007517000879170008911700089917000809 +:10640000A5170008A9170008AD1700080000000034 +:10641000000000004D180008B1170008B51700086B +:1064200053464450000000004A454445433A204644 +:1064300061696C656420746F2064657465637420A1 +:10644000666C617368206465766963653A202573BC +:106450000A004A454445433A204661696C65642018 +:10646000746F20636F6E66696720666C6173682065 +:106470006465766963653A2025730A004A45444598 +:10648000433A20446574656374656420466C6173A7 +:1064900068204465766963653A2025730A0045786B +:1064A0007420466C617368204E6F7420466F756E61 +:1064B000642100006D74323571007732357100004F +:1064C000B464000820BA0000BA640008EF4000007D +:1064D00050630008EF7000000000000000000000A2 +:1064E00091210008F1230008CD1A0008D11A0008F4 +:1064F000791B000859230008D51A0008D91A00088A +:1065000051220008491B0008B1220008B51F0008ED +:10651000E11A0008DD1A000881240008411B000868 +:106520000000000000000000452500084D2500087F +:1065300031250008392500085D250008612500087F +:106540004D444D41206661696C7572650000000024 +:1065500000960000000000000000000000000000A5 +:10656000000000000000000000000000D92C00081E +:10657000C52C0008012D0008ED2C0008F92C00089E +:10658000E52C0008D12C0008BD2C00080D2D0008BA +:1065900000000000F12D0008DD2D0008192E000874 +:1065A000052E0008112E0008FD2D0008E92D000819 +:1065B000D52D0008252E0008000000000100000075 +:1065C0000000000063300000C4650008E0290020DE +:1065D000F02B00204172647550696C6F7400254285 +:1065E0004F415244252D424C002553455249414CC0 +:1065F000250000000200000000000000113000082B +:106600008130000840004000A0440020B044002039 +:106610000200000000000000030000000000000075 +:10662000C93000080000000010000000C044002035 +:10663000000000000100000000000000B048002041 +:1066400001010200913F0008A13E00083D3F000803 +:10665000213F0008430000005C6600080902430077 +:10666000020100C0320904000001020201000524F9 +:106670000010010524010001042402020524060083 +:1066800001070582030800FF09040100020A000057 +:1066900000070501024000000705810240000000DC +:1066A00012000000A866000812011001020000405C +:1066B000091241570002010203010000040309040A +:1066C00025424F41524425005350526163696E6721 +:1066D0004837003031323334353637383941424368 +:1066E00044454600000000200000020002000000B7 +:1066F0000000003000000400000000000000002442 +:1067000000000800040000000004000000FC00007D +:1067100002000000000004300080000000000000C3 +:10672000000000380000010001000000400000529D +:1067300080000052C000005200010052400100528F +:1067400080010052C001005200020052400200527B +:1067500080020052C0020052000300524003005267 +:1067600080030052C00300520004005200000000E9 +:106770001D320008D5340008813500084000400073 +:10678000E8490020E849002001000000F849002005 +:10679000800000004001000008000000000100002F +:1067A00000040000080000006D61696E0069646CFF +:1067B000650000000000806A00000000AAAAAAAAE2 +:1067C00000000064FFFF00000000000000A00A00BD +:1067D000240020A100000000BAAABAAA00000051BB +:1067E000FFFF000000090000000900440000000055 +:1067F00000000000AAAAAAAA00000000FFFF0000F3 +:1068000000000000000000000000800A00000000FE +:10681000AAAAEAAF00000000FFFF0000000000008D +:10682000009099006000400000000000BAAAAAAAE7 +:1068300000004000F7FF0000000900000000000019 +:106840000000000000000000AAAAAAAA00000000A0 +:10685000FFFF00000000000000000000000000003A +:1068600000000000AAAAAAAA00000000FFFF000082 +:106870000000000000000000000000000000000018 +:10688000AAAAAAAA00000000FFFF00000000000062 +:10689000000000000000000000000000AAAAAAAA50 +:1068A00000000000FFFF00000000000000000000EA +:1068B0000000000000000000AAAAAAAA0000000030 +:1068C000FFFF0000000000000000000000000000CA +:1068D00000000000AAAAAAAA00000000FFFF000012 +:1068E0000000000000000000389AFF7F0100000057 +:1068F0002404000000000000000000000000E90087 +:10690000FF000000000000002C6300083F000000B2 +:1069100050040000376300083F00000000960000AC +:1069200000000800960000000008000004000000BD +:10693000BC6600080000000000000000000000002D +:1069400000000000000000000000000060220020A5 +:106950000000000000000000000000000000000037 +:106960000000000000000000000000000000000027 +:106970000000000000000000000000000000000017 +:106980000000000000000000000000000000000007 +:1069900000000000000000000000000000000000F7 +:1069A00000000000000000000000000000000000E7 :00000001FF diff --git a/Tools/bootloaders/SpeedyBeeF405WING_bl.bin b/Tools/bootloaders/SpeedyBeeF405WING_bl.bin new file mode 100755 index 0000000000000..a52f96f9c8212 Binary files /dev/null and b/Tools/bootloaders/SpeedyBeeF405WING_bl.bin differ diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index a0cae745d78d7..c4fa17d6c934d 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -130,7 +130,7 @@ for t in $CI_BUILD_TARGET; do fi if [ "$t" == "sitltest-can" ]; then echo "Building SITL Periph GPS" - $waf configure --board sitl + $waf configure --board sitl --force-32bit $waf copter run_autotest "Copter" "build.SITLPeriphGPS" "test.CAN" continue @@ -285,6 +285,8 @@ for t in $CI_BUILD_TARGET; do $waf configure --board Durandal $waf clean $waf copter + echo "Building CPUInfo" + $waf --target=tool/CPUInfo # test external flash build echo "Building SPRacingH7" @@ -401,6 +403,12 @@ for t in $CI_BUILD_TARGET; do continue fi + if [ "$t" == "astyle-cleanliness" ]; then + echo "Checking AStyle code cleanliness" + ./Tools/scripts/run_astyle.py + continue + fi + if [ "$t" == "configure-all" ]; then echo "Checking configure of all boards" ./Tools/scripts/configure_all.py diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index d5fca1208e6cc..b2ed18e10c3d5 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -113,6 +113,7 @@ def __init__(self, Feature('Camera', 'Camera', 'AP_CAMERA_ENABLED', 'Enable Camera Trigger support', 0, None), Feature('Camera', 'Camera_MAVLink', 'AP_CAMERA_MAVLINK_ENABLED', 'Enable MAVLink Camera support', 0, 'Camera'), + Feature('Camera', 'Camera_MAVLinkCamV2', 'AP_CAMERA_MAVLINKCAMV2_ENABLED', 'Enable MAVLink CameraV2 support', 0, 'Camera'), Feature('Camera', 'Camera_Mount', 'AP_CAMERA_MOUNT_ENABLED', 'Enable Camera-in-Mount support', 0, 'Camera,MOUNT'), Feature('Camera', 'Camera_Relay', 'AP_CAMERA_RELAY_ENABLED', 'Enable Camera Trigger via Relay support', 0, 'Camera'), Feature('Camera', 'Camera_Servo', 'AP_CAMERA_SERVO_ENABLED', 'Enable Camera Trigger via Servo support', 0, 'Camera'), @@ -144,7 +145,7 @@ def __init__(self, Feature('Compass', 'MMC5XX3', 'AP_COMPASS_MMC5XX3_ENABLED', 'Enable MMC5XX3 compasses', 1, None), Feature('Compass', 'QMC5883L', 'AP_COMPASS_QMC5883L_ENABLED', 'Enable QMC5883L compasses', 1, None), Feature('Compass', 'RM3100', 'AP_COMPASS_RM3100_ENABLED', 'Enable RM3100 compasses', 1, None), - Feature('Compass', 'UAVCAN_COMPASS', 'AP_COMPASS_UAVCAN_ENABLED', 'Enable UAVCAN compasses', 0, None), + Feature('Compass', 'DRONECAN_COMPASS', 'AP_COMPASS_DRONECAN_ENABLED', 'Enable DroneCAN compasses', 0, None), Feature('Gimbal', 'MOUNT', 'HAL_MOUNT_ENABLED', 'Enable Mount', 0, None), Feature('Gimbal', 'ALEXMOS', 'HAL_MOUNT_ALEXMOS_ENABLED', 'Enable Alexmos Gimbal', 0, "MOUNT"), @@ -166,7 +167,7 @@ def __init__(self, Feature('Payload', 'GRIPPER', 'AP_GRIPPER_ENABLED', 'Enable Gripper', 0, None), Feature('Payload', 'SPRAYER', 'HAL_SPRAYER_ENABLED', 'Enable Sprayer', 0, None), Feature('Payload', 'LANDING_GEAR', 'AP_LANDINGGEAR_ENABLED', 'Enable Landing Gear', 0, None), - Feature('Payload', 'WINCH', 'WINCH_ENABLED', 'Enable Winch', 0, None), + Feature('Payload', 'WINCH', 'AP_WINCH_ENABLED', 'Enable Winch', 0, None), Feature('Plane', 'QUADPLANE', 'HAL_QUADPLANE_ENABLED', 'Enable QuadPlane support', 0, None), Feature('Plane', 'SOARING', 'HAL_SOARING_ENABLED', 'Enable Soaring', 0, None), @@ -201,7 +202,7 @@ def __init__(self, # Feature('Rangefinder', 'RANGEFINDER_SIM', 'AP_RANGEFINDER_SIM_ENABLED', "Enable Rangefinder - SIM", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_TRI2C', 'AP_RANGEFINDER_TRI2C_ENABLED', "Enable Rangefinder - TeraRangerI2C", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_TR_SERIAL', 'AP_RANGEFINDER_TERARANGER_SERIAL_ENABLED', "Enable Rangefinder - TeraRanger Serial", 0, "RANGEFINDER"), # NOQA: E501 - Feature('Rangefinder', 'RANGEFINDER_UAVCAN', 'AP_RANGEFINDER_UAVCAN_ENABLED', "Enable Rangefinder - UAVCAN", 0, "RANGEFINDER"), # NOQA: E501 + Feature('Rangefinder', 'RANGEFINDER_DRONECAN', 'AP_RANGEFINDER_DRONECAN_ENABLED', "Enable Rangefinder - DroneCAN", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_USD1_CAN', 'AP_RANGEFINDER_USD1_CAN_ENABLED', "Enable Rangefinder - USD1 (CAN)", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_USD1_SERIAL', 'AP_RANGEFINDER_USD1_SERIAL_ENABLED', "Enable Rangefinder - USD1 (SERIAL)", 0, "RANGEFINDER"), # NOQA: E501 Feature('Rangefinder', 'RANGEFINDER_VL53L0X', 'AP_RANGEFINDER_VL53L0X_ENABLED', "Enable Rangefinder - VL53L0X", 0, "RANGEFINDER"), # NOQA: E501 @@ -230,7 +231,7 @@ def __init__(self, Feature('Sensors', 'MS56XX', 'AP_BARO_MS56XX_ENABLED', 'Enable MS56XX Barometric Sensor', 1, None), Feature('Sensors', 'MSP_BARO', 'AP_BARO_MSP_ENABLED', 'Enable MSP Barometric Sensor', 0, 'MSP'), Feature('Sensors', 'SPL06', 'AP_BARO_SPL06_ENABLED', 'Enable SPL06 Barometric Sensor', 1, None), - Feature('Sensors', 'UAVCAN_BARO', 'AP_BARO_UAVCAN_ENABLED', 'Enable UAVCAN Barometric Sensor', 0, None), + Feature('Sensors', 'DRONECAN_BARO', 'AP_BARO_DRONECAN_ENABLED', 'Enable DroneCAN Barometric Sensor', 0, None), Feature('Sensors', 'ICP101XX', 'AP_BARO_ICP101XX_ENABLED', 'Enable ICP101XX Barometric Sensor', 0, None), Feature('Sensors', 'ICP201XX', 'AP_BARO_ICP201XX_ENABLED', 'Enable ICP201XX Barometric Sensor', 0, None), @@ -246,7 +247,7 @@ def __init__(self, Feature('Sensors', 'TEMP_MCP9600', 'AP_TEMPERATURE_SENSOR_MCP9600_ENABLED', 'Enable Temp Sensor - MCP9600', 0, "TEMP"), Feature('Sensors', 'AIRSPEED', 'AP_AIRSPEED_ENABLED', 'Enable Airspeed Sensors', 1, None), # Default to enabled to not annoy Plane users # NOQA: E501 - Feature('Sensors', 'BEACON', 'BEACON_ENABLED', 'Enable Beacon', 0, None), + 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('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight Gyro FFT calculations', 0, None), @@ -277,13 +278,16 @@ def __init__(self, Feature('Airspeed Drivers', 'MSP_AIRSPEED', 'AP_AIRSPEED_MSP_ENABLED', 'ENABLE MSP AIRSPEED', 0, 'AIRSPEED,MSP,OSD'), Feature('Airspeed Drivers', 'NMEA_AIRSPEED', 'AP_AIRSPEED_NMEA_ENABLED', 'ENABLE NMEA AIRSPEED', 0, 'AIRSPEED'), Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'ENABLE SDP3X AIRSPEED', 0, 'AIRSPEED'), - Feature('Airspeed Drivers', 'UAVCAN_ASPD', 'AP_AIRSPEED_UAVCAN_ENABLED', 'ENABLE UAVCAN AIRSPEED', 0, 'AIRSPEED'), # NOQA: E501 + Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'ENABLE DroneCAN AIRSPEED', 0, 'AIRSPEED'), # NOQA: E501 Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None), Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, None), Feature('Actuators', 'RobotisServo', 'AP_ROBOTISSERVO_ENABLED', 'Enable RobotisServo Protocol', 0, None), Feature('Actuators', 'FETTecOneWire', 'AP_FETTEC_ONEWIRE_ENABLED', 'Enable FETTec OneWire ESCs', 0, None), + Feature('Precision Landing', 'PrecLand', 'AC_PRECLAND_ENABLED', 'Enable Precision Landing support', 0, None), + Feature('Precision Landing', 'PrecLand - Companion', 'AC_PRECLAND_COMPANION_ENABLED', 'Enable Companion-Supported Precision Landing support', 0, "PrecLand"), # noqa + Feature('Precision Landing', 'PrecLand - IRLock', 'AC_PRECLAND_IRLOCK_ENABLED', 'Enable IRLock-Supported Precision Landing support', 0, "PrecLand"), # noqa ] BUILD_OPTIONS.sort(key=lambda x: (x.category + x.label)) diff --git a/Tools/scripts/configure_all.py b/Tools/scripts/configure_all.py index 2c28c8fc7ba3c..45629955f5982 100755 --- a/Tools/scripts/configure_all.py +++ b/Tools/scripts/configure_all.py @@ -68,7 +68,7 @@ def is_ap_periph(board): hwdef = os.path.join('libraries/AP_HAL_ChibiOS/hwdef/%s/hwdef.dat' % board) try: r = open(hwdef, 'r').read() - if r.find('periph/hwdef.dat') != -1 or r.find('AP_PERIPH') != -1: + if r.find('periph/hwdef.dat') != -1 or r.find('periph/hwdef.inc') != -1 or r.find('AP_PERIPH') != -1: print("%s is AP_Periph" % board) return True except Exception as ex: diff --git a/Tools/scripts/decode_devid.py b/Tools/scripts/decode_devid.py index 15b574306bf65..4d79b9d063d36 100755 --- a/Tools/scripts/decode_devid.py +++ b/Tools/scripts/decode_devid.py @@ -38,7 +38,7 @@ def num(s): bustypes = { 1: "I2C", 2: "SPI", - 3: "UAVCAN", + 3: "DRONECAN", 4: "SITL", 5: "MSP", 6: "SERIAL", @@ -114,7 +114,7 @@ def num(s): 0x0A : "DEVTYPE_BARO_LPS2XH", 0x0B : "DEVTYPE_BARO_MS5611", 0x0C : "DEVTYPE_BARO_SPL06", - 0x0D : "DEVTYPE_BARO_UAVCAN", + 0x0D : "DEVTYPE_BARO_DRONECAN", 0x0E : "DEVTYPE_BARO_MSP", 0x0F : "DEVTYPE_BARO_ICP101XX", 0x10 : "DEVTYPE_BARO_ICP201XX", @@ -131,7 +131,7 @@ def num(s): 0x04 : "DEVTYPE_AIRSPEED_DLVR", 0x05 : "DEVTYPE_AIRSPEED_MSP", 0x06 : "DEVTYPE_AIRSPEED_SDP3X", - 0x07 : "DEVTYPE_AIRSPEED_UAVCAN", + 0x07 : "DEVTYPE_AIRSPEED_DRONECAN", 0x08 : "DEVTYPE_AIRSPEED_ANALOG", 0x09 : "DEVTYPE_AIRSPEED_NMEA", 0x0A : "DEVTYPE_AIRSPEED_ASP5033", @@ -152,7 +152,7 @@ def num(s): decoded_devname = airspeed_types.get(devtype, "UNKNOWN") if bus_type == 3: - #uavcan devtype represents sensor_id + #dronecan devtype represents sensor_id print("bus_type:%s(%u) bus:%u address:%u(0x%x) sensor_id:%u(0x%x) %s" % ( bustypes.get(bus_type,"UNKNOWN"), bus_type, bus, address, address, devtype-1, devtype-1, decoded_devname)) diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index cb261eb1597f4..cf6e64cc764a7 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -39,6 +39,10 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('AP_AIRSPEED_ENABLED', 'AP_Airspeed::AP_Airspeed',), ('AP_AIRSPEED_{type}_ENABLED', r'AP_Airspeed_(?P.*)::init',), + ('AC_PRECLAND_ENABLED', 'AC_PrecLand::AC_PrecLand',), + ('AC_PRECLAND_ENABLED', 'AC_PrecLand::AC_PrecLand',), + ('AC_PRECLAND_{type}_ENABLED', 'AC_PrecLand_(?P.*)::update',), + ('HAL_ADSB_ENABLED', 'AP_ADSB::AP_ADSB',), ('HAL_ADSB_{type}_ENABLED', r'AP_ADSB_(?P.*)::update',), ('HAL_ADSB_UCP_ENABLED', 'AP_ADSB_uAvionix_UCP::update',), @@ -54,7 +58,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('AP_TEMPERATURE_SENSOR_ENABLED', 'AP_TemperatureSensor::AP_TemperatureSensor',), ('AP_TEMPERATURE_SENSOR_{type}_ENABLED', 'AP_TemperatureSensor_(?P.*)::update',), - ('BEACON_ENABLED', 'AP_Beacon::AP_Beacon',), + ('AP_BEACON_ENABLED', 'AP_Beacon::AP_Beacon',), ('HAL_TORQEEDO_ENABLED', 'AP_Torqeedo::AP_Torqeedo'), ('HAL_NAVEKF3_AVAILABLE', 'NavEKF3::NavEKF3',), @@ -125,7 +129,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('HAL_GENERATOR_ENABLED', 'AP_Generator::AP_Generator',), ('AP_GENERATOR_{type}_ENABLED', r'AP_Generator_(?P.*)::update',), - ('OSD_ENABLED', 'AP_OSD::AP_OSD',), + ('OSD_ENABLED', 'AP_OSD::update_osd',), ('HAL_PLUSCODE_ENABLE', 'AP_OSD_Screen::draw_pluscode',), ('OSD_PARAM_ENABLED', 'AP_OSD_ParamScreen::AP_OSD_ParamScreen',), ('HAL_OSD_SIDEBAR_ENABLE', 'AP_OSD_Screen::draw_sidebars',), @@ -142,13 +146,13 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('AP_GRIPPER_ENABLED', r'AP_Gripper::init\b',), ('HAL_SPRAYER_ENABLED', 'AC_Sprayer::AC_Sprayer',), ('AP_LANDINGGEAR_ENABLED', r'AP_LandingGear::init\b',), - ('WINCH_ENABLED', 'AP_Winch::AP_Winch',), + ('AP_WINCH_ENABLED', 'AP_Winch::AP_Winch',), ('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P.*)::_process_byte\b',), ('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P.*)::_process_pulse\b',), ('AP_VOLZ_ENABLED', r'AP_Volz_Protocol::init\b',), - ('AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', r'AP_UAVCAN::handle_actuator_status_Volz\b',), + ('AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', r'AP_DroneCAN::handle_actuator_status_Volz\b',), ('AP_ROBOTISSERVO_ENABLED', r'AP_RobotisServo::init\b',), ('AP_FETTEC_ONEWIRE_ENABLED', r'AP_FETtecOneWire::init\b',), @@ -156,7 +160,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('AP_RPM_{type}_ENABLED', r'AP_RPM_(?P.*)::update',), ('GPS_MOVING_BASELINE', r'AP_GPS_Backend::calculate_moving_base_yaw\b',), - ('AP_DRONECAN_SEND_GPS', r'AP_GPS_UAVCAN::instance_exists\b',), + ('AP_DRONECAN_SEND_GPS', r'AP_GPS_DroneCAN::instance_exists\b',), ('HAL_WITH_DSP', r'AP_HAL::DSP::find_peaks\b',), ('HAL_GYROFFT_ENABLED', r'AP_GyroFFT::AP_GyroFFT\b',), @@ -315,9 +319,8 @@ def extract_symbols_from_elf(self, filename): return ret - def create_string(self): - - ret = "" + def extract(self): + '''returns two sets - compiled_in and not_compiled_in''' build_options_defines = set([x.define for x in build_options.BUILD_OPTIONS]) @@ -346,10 +349,18 @@ def create_string(self): continue compiled_in_feature_defines.append(some_define) remaining_build_options_defines.discard(some_define) + return (compiled_in_feature_defines, remaining_build_options_defines) + + def create_string(self): + '''returns a string with compiled in and not compiled-in features''' + + (compiled_in_feature_defines, not_compiled_in_feature_defines) = self.extract() + + ret = "" for compiled_in_feature_define in sorted(compiled_in_feature_defines): ret += compiled_in_feature_define + "\n" - for remaining in sorted(remaining_build_options_defines): + for remaining in sorted(not_compiled_in_feature_defines): ret += "!" + remaining + "\n" return ret diff --git a/Tools/scripts/install-apt-ci.sh b/Tools/scripts/install-apt-ci.sh deleted file mode 100755 index 82a0d98bfc019..0000000000000 --- a/Tools/scripts/install-apt-ci.sh +++ /dev/null @@ -1,25 +0,0 @@ -#!/bin/bash -# Install APT packages for CI build testing - -set -ex - -PKGS=" \ - build-essential \ - gawk \ - libc6-i386 \ - libxml2-dev \ - libxslt1-dev \ - python-pip \ - python-dev \ - zlib1g-dev \ - " - -read -r UBUNTU_CODENAME <<<$(lsb_release -c -s) - -sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y - -#wget -q -O - http://llvm.org/apt/llvm-snapshot.gpg.key | sudo apt-key add - -#sudo add-apt-repository "deb http://llvm.org/apt/${UBUNTU_CODENAME}/ llvm-toolchain-${UBUNTU_CODENAME}-3.7 main" -y -sudo apt-get -qq -y --force-yes update -sudo apt-get -qq -y --force-yes remove clang llvm -sudo apt-get -y --force-yes install $PKGS diff --git a/Tools/scripts/magfit_flashlog.py b/Tools/scripts/magfit_flashlog.py deleted file mode 100755 index 3ec6aaf33b41d..0000000000000 --- a/Tools/scripts/magfit_flashlog.py +++ /dev/null @@ -1,149 +0,0 @@ -#!/usr/bin/env python - -''' fit best estimate of magnetometer offsets from ArduCopter flashlog -using the algorithm from Bill Premerlani -''' - -import sys - -# command line option handling -from optparse import OptionParser -parser = OptionParser("magfit_flashlog.py [options]") -parser.add_option("--verbose", action='store_true', default=False, help="verbose offset output") -parser.add_option("--gain", type='float', default=0.01, help="algorithm gain") -parser.add_option("--noise", type='float', default=0, help="noise to add") -parser.add_option("--max-change", type='float', default=10, help="max step change") -parser.add_option("--min-diff", type='float', default=50, help="min mag vector delta") -parser.add_option("--history", type='int', default=20, help="how many points to keep") -parser.add_option("--repeat", type='int', default=1, help="number of repeats through the data") - -(opts, args) = parser.parse_args() - -from pymavlink.rotmat import Vector3 - -if len(args) < 1: - print("Usage: magfit_flashlog.py [options] ") - sys.exit(1) - -def noise(): - '''a noise vector''' - from random import gauss - v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1)) - v.normalize() - return v * opts.noise - -def find_offsets(data, ofs): - '''find mag offsets by applying Bills "offsets revisited" algorithm - on the data - - This is an implementation of the algorithm from: - http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf - ''' - - # a limit on the maximum change in each step - max_change = opts.max_change - - # the gain factor for the algorithm - gain = opts.gain - - data2 = [] - for d in data: - d = d.copy() + noise() - d.x = float(int(d.x + 0.5)) - d.y = float(int(d.y + 0.5)) - d.z = float(int(d.z + 0.5)) - data2.append(d) - data = data2 - - history_idx = 0 - mag_history = data[0:opts.history] - - for i in range(opts.history, len(data)): - B1 = mag_history[history_idx] + ofs - B2 = data[i] + ofs - - diff = B2 - B1 - diff_length = diff.length() - if diff_length <= opts.min_diff: - # the mag vector hasn't changed enough - we don't get any - # information from this - history_idx = (history_idx+1) % opts.history - continue - - mag_history[history_idx] = data[i] - history_idx = (history_idx+1) % opts.history - - # equation 6 of Bills paper - delta = diff * (gain * (B2.length() - B1.length()) / diff_length) - - # limit the change from any one reading. This is to prevent - # single crazy readings from throwing off the offsets for a long - # time - delta_length = delta.length() - if max_change != 0 and delta_length > max_change: - delta *= max_change / delta_length - - # set the new offsets - ofs = ofs - delta - - if opts.verbose: - print(ofs) - return ofs - - -def plot_corrected_field(filename, data, offsets): - f = open(filename, mode='w') - for d in data: - corrected = d + offsets - f.write("%.1f\n" % corrected.length()) - f.close() - -def magfit(logfile): - '''find best magnetometer offset fit to a log file''' - - print("Processing log %s" % filename) - - # open the log file - flog = open(filename, mode='r') - - data = [] - data_no_motors = [] - mag = None - offsets = None - - # now gather all the data - for line in flog: - if not line.startswith('COMPASS,'): - continue - line = line.rstrip() - line = line.replace(' ', '') - a = line.split(',') - ofs = Vector3(float(a[4]), float(a[5]), float(a[6])) - if offsets is None: - initial_offsets = ofs - offsets = ofs - motor_ofs = Vector3(float(a[7]), float(a[8]), float(a[9])) - mag = Vector3(float(a[1]), float(a[2]), float(a[3])) - mag = mag - offsets - data.append(mag) - data_no_motors.append(mag - motor_ofs) - - print("Extracted %u data points" % len(data)) - print("Current offsets: %s" % initial_offsets) - - # run the fitting algorithm - ofs = initial_offsets - for r in range(opts.repeat): - ofs = find_offsets(data, ofs) - plot_corrected_field('plot.dat', data, ofs) - plot_corrected_field('initial.dat', data, initial_offsets) - plot_corrected_field('zero.dat', data, Vector3(0,0,0)) - plot_corrected_field('hand.dat', data, Vector3(-25,-8,-2)) - plot_corrected_field('zero-no-motors.dat', data_no_motors, Vector3(0,0,0)) - print('Loop %u offsets %s' % (r, ofs)) - sys.stdout.flush() - print("New offsets: %s" % ofs) - -total = 0.0 -for filename in args: - magfit(filename) diff --git a/Tools/scripts/make_abin.sh b/Tools/scripts/make_abin.sh index 96d6756281b90..8e903558bba00 100755 --- a/Tools/scripts/make_abin.sh +++ b/Tools/scripts/make_abin.sh @@ -12,7 +12,7 @@ BINFILE="$1" ABINFILE="$2" [ -f "$BINFILE" ] || { - echo "Can't find bin file" + echo "Can't find bin file $BINFILE for abin" exit 1 } diff --git a/Tools/scripts/run_astyle.py b/Tools/scripts/run_astyle.py new file mode 100755 index 0000000000000..2a9ac336b74c1 --- /dev/null +++ b/Tools/scripts/run_astyle.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 + +""" +Runs astyle over directory sub-trees known to be "astyle-clean" + + AP_FLAKE8_CLEAN +""" + +import os +import pathlib +import subprocess +import sys + +import argparse + +os.environ['PYTHONUNBUFFERED'] = '1' + + +class AStyleChecker(object): + def __init__(self): + self.retcode = 0 + self.directories_to_check = [ + 'libraries/AP_DDS', + ] + self.files_to_check = [] + + def progress(self, string): + print("****** %s" % (string,)) + + def check(self): + '''run astyle on all files in self.files_to_check''' + # for path in self.files_to_check: + # self.progress("Checking (%s)" % path) + astyle_command = ["astyle", "--dry-run"] + astyle_command.extend(self.files_to_check) + ret = subprocess.run( + astyle_command, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True + ) + if ret.returncode != 0: + self.progress("astyle check failed: (%s)" % (ret.stdout)) + self.retcode = 1 + if "Formatted" in ret.stdout: + self.progress("Files needing formatting found") + print(ret.stdout) + self.retcode = 1 + + def run(self): + for d in self.directories_to_check: + self.files_to_check.extend(list(pathlib.Path(d).glob("*"))) + self.files_to_check = list(filter(lambda x : x.suffix in [".c", ".h", ".cpp"], self.files_to_check)) + self.check() + return self.retcode + + +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Check all Python files for astyle cleanliness') + # parser.add_argument('--build', action='store_true', default=False, help='build as well as configure') + args = parser.parse_args() + + checker = AStyleChecker() + sys.exit(checker.run()) diff --git a/Tools/scripts/run_coverage.py b/Tools/scripts/run_coverage.py index d21a0dec63531..3c254e205eec5 100755 --- a/Tools/scripts/run_coverage.py +++ b/Tools/scripts/run_coverage.py @@ -82,7 +82,6 @@ def init_coverage(self, use_example=False): "--no-external", "--initial", "--capture", - "--exclude", root_dir + "/modules/uavcan/*", "--exclude", root_dir + "/build/sitl/modules/*", "--directory", root_dir, "-o", self.INFO_FILE_BASE, @@ -227,7 +226,6 @@ def update_stats(self): "--remove", self.INFO_FILE, ".waf*", root_dir + "/modules/gtest/*", - root_dir + "/modules/uavcan/*", root_dir + "/modules/DroneCAN/libcanard/*", root_dir + "/build/linux/libraries/*", root_dir + "/build/sitl/libraries/*", diff --git a/Tools/scripts/size_compare_branches.py b/Tools/scripts/size_compare_branches.py index 9e719910c1712..092e718c797dc 100755 --- a/Tools/scripts/size_compare_branches.py +++ b/Tools/scripts/size_compare_branches.py @@ -120,6 +120,7 @@ def __init__(self, # TODO: find a way to get this information from board_list: self.bootloader_blacklist = set([ 'CubeOrange-SimOnHardWare', + 'CubeOrangePlus-SimOnHardWare', 'fmuv2', 'fmuv3-bdshot', 'iomcu', @@ -297,6 +298,11 @@ def build_branch_into_dir(self, board, branch, vehicle, outdir, extra_hwdef=None # need special configuration directive bootloader_waf_configure_args = copy.copy(waf_configure_args) bootloader_waf_configure_args.append('--bootloader') + # hopefully temporary hack so you can build bootloader + # after building other vehicles without a clean: + dsdl_generated_path = os.path.join('build', board, "modules", "DroneCAN", "libcanard", "dsdlc_generated") + self.progress("HACK: Removing (%s)" % dsdl_generated_path) + shutil.rmtree(dsdl_generated_path, ignore_errors=True) self.run_waf(bootloader_waf_configure_args) self.run_waf([v]) self.run_program("rsync", ["rsync", "-aP", "build/", outdir]) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index a9b4d0926e487..5284f3f36fabc 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -293,7 +293,7 @@ class AC_PosControl /// set_vel_desired_z_cms - sets desired velocity in cm/s in z axis void set_vel_desired_z_cms(float vel_z_cms) {_vel_desired.z = vel_z_cms;} - /// get_vel_target_z_cms - returns current vertical speed in cm/s + /// get_vel_target_z_cms - returns target vertical speed in cm/s float get_vel_target_z_cms() const { return _vel_target.z; } diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 0e402fc543e0a..7b2e5fdf20f49 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -126,9 +126,11 @@ void AC_Avoid::adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desir { // Only horizontal component needed for most fences, since fences are 2D Vector2f desired_velocity_xy_cms{desired_vel_cms.x, desired_vel_cms.y}; - + +#if AP_FENCE_ENABLED || AP_BEACON_ENABLED // limit acceleration const float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX); +#endif // maximum component of desired backup velocity in each quadrant Vector2f quad_1_back_vel, quad_2_back_vel, quad_3_back_vel, quad_4_back_vel; diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index cf23c855496d8..398286b878aca 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -1,7 +1,12 @@ +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_ENABLED + +#include "AC_PrecLand.h" #include #include #include -#include "AC_PrecLand.h" + #include "AC_PrecLand_Backend.h" #include "AC_PrecLand_Companion.h" #include "AC_PrecLand_IRLock.h" @@ -236,17 +241,23 @@ void AC_PrecLand::init(uint16_t update_rate_hz) default: return; // companion computer +#if AC_PRECLAND_COMPANION_ENABLED case Type::COMPANION: _backend = new AC_PrecLand_Companion(*this, _backend_state); break; // IR Lock +#endif +#if AC_PRECLAND_IRLOCK_ENABLED case Type::IRLOCK: _backend = new AC_PrecLand_IRLock(*this, _backend_state); break; -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#endif +#if AC_PRECLAND_SITL_GAZEBO_ENABLED case Type::SITL_GAZEBO: _backend = new AC_PrecLand_SITL_Gazebo(*this, _backend_state); break; +#endif +#if AC_PRECLAND_SITL_ENABLED case Type::SITL: _backend = new AC_PrecLand_SITL(*this, _backend_state); break; @@ -774,3 +785,5 @@ AC_PrecLand *ac_precland() } } + +#endif // AC_PRECLAND_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 2f804f59c933d..6826bded51fda 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -1,7 +1,11 @@ #pragma once -#include +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_ENABLED + #include +#include #include #include "PosVelEKF.h" #include @@ -123,10 +127,18 @@ class AC_PrecLand // types of precision landing (used for PRECLAND_TYPE parameter) enum class Type : uint8_t { NONE = 0, +#if AC_PRECLAND_COMPANION_ENABLED COMPANION = 1, +#endif +#if AC_PRECLAND_IRLOCK_ENABLED IRLOCK = 2, +#endif +#if AC_PRECLAND_SITL_GAZEBO_ENABLED SITL_GAZEBO = 3, +#endif +#if AC_PRECLAND_SITL_ENABLED SITL = 4, +#endif }; enum PLndOptions { @@ -231,3 +243,5 @@ class AC_PrecLand namespace AP { AC_PrecLand *ac_precland(); }; + +#endif // AC_PRECLAND_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index d816a85d8ddbb..41facd0deb4a0 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -1,8 +1,13 @@ #pragma once +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_ENABLED + +#include "AC_PrecLand.h" #include #include -#include "AC_PrecLand.h" + class AC_PrecLand_Backend { @@ -44,3 +49,5 @@ class AC_PrecLand_Backend const AC_PrecLand& _frontend; // reference to precision landing front end AC_PrecLand::precland_state &_state; // reference to this instances state }; + +#endif // AC_PRECLAND_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index e65ff1d6861d3..51d46feb777fe 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -1,6 +1,10 @@ +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_COMPANION_ENABLED + +#include "AC_PrecLand_Companion.h" #include #include -#include "AC_PrecLand_Companion.h" // perform any required initialisation of backend void AC_PrecLand_Companion::init() @@ -73,3 +77,5 @@ void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, u _los_meas_time_ms = timestamp_ms; _have_los_meas = true; } + +#endif // AC_PRECLAND_COMPANION_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 67af707a9bd0b..892a5aa1f8265 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -1,7 +1,11 @@ #pragma once -#include +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_COMPANION_ENABLED + #include "AC_PrecLand_Backend.h" +#include /* * AC_PrecLand_Companion - implements precision landing using target vectors provided @@ -46,3 +50,6 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured bool _wrong_frame_msg_sent; }; + + +#endif // AC_PRECLAND_COMPANION_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp index 04b1126621300..e58bb52e82965 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -1,5 +1,9 @@ -#include +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_IRLOCK_ENABLED + #include "AC_PrecLand_IRLock.h" +#include // Constructor AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) @@ -50,3 +54,5 @@ uint32_t AC_PrecLand_IRLock::los_meas_time_ms() { bool AC_PrecLand_IRLock::have_los_meas() { return _have_los_meas; } + +#endif // AC_PRECLAND_IRLOCK_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index da2923b1265a7..08831bc362595 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -1,7 +1,12 @@ #pragma once -#include +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_IRLOCK_ENABLED + #include +#include + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include #else @@ -46,3 +51,5 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend bool _have_los_meas; // true if there is a valid measurement from the camera uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured }; + +#endif // AC_PRECLAND_IRLOCK_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp index b71a4d1c26b2d..1d920818fb789 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp @@ -1,7 +1,7 @@ #include #include "AC_PrecLand_SITL.h" -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AC_PRECLAND_SITL_ENABLED #include "AP_AHRS/AP_AHRS.h" // init - perform initialisation of this backend @@ -54,4 +54,4 @@ bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) { return true; } -#endif +#endif // AC_PRECLAND_SITL_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.h b/libraries/AC_PrecLand/AC_PrecLand_SITL.h index ee6fd526edf55..53b0c23aa6974 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.h @@ -1,8 +1,11 @@ #pragma once -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_SITL_ENABLED + #include +#include #include /* @@ -43,4 +46,4 @@ class AC_PrecLand_SITL : public AC_PrecLand_Backend float _distance_to_target; // distance to target in meters }; -#endif +#endif // AC_PRECLAND_SITL_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp index 2bf94759dc8a3..95d331d75ba78 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp @@ -3,7 +3,7 @@ extern const AP_HAL::HAL& hal; -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if AC_PRECLAND_SITL_GAZEBO_ENABLED // Constructor AC_PrecLand_SITL_Gazebo::AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) @@ -55,4 +55,4 @@ bool AC_PrecLand_SITL_Gazebo::have_los_meas() { return _have_los_meas; } -#endif +#endif // AC_PRECLAND_SITL_GAZEBO_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h index e7f1ab06ed1ca..dcddec958ebb2 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h @@ -1,8 +1,11 @@ #pragma once -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_SITL_GAZEBO_ENABLED + #include +#include #include /* @@ -41,4 +44,4 @@ class AC_PrecLand_SITL_Gazebo : public AC_PrecLand_Backend uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured }; -#endif +#endif // AC_PRECLAND_SITL_GAZEBO_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp index e2d3615651323..c456288ca2d36 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp @@ -1,5 +1,9 @@ -#include "AC_PrecLand_StateMachine.h" +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_ENABLED + #include +#include "AC_PrecLand_StateMachine.h" #include #include @@ -274,3 +278,5 @@ AC_PrecLand_StateMachine::FailSafeAction AC_PrecLand_StateMachine::get_failsafe_ // should never reach here return FailSafeAction::DESCEND; } + +#endif // AC_PRECLAND_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h index b72b0e8d72e26..8479be3c6abf7 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h +++ b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.h @@ -1,5 +1,10 @@ #pragma once +#include "AC_PrecLand_config.h" + +#if AC_PRECLAND_ENABLED + +#include #include // This class constantly monitors what the status of the landing target is @@ -101,3 +106,5 @@ class AC_PrecLand_StateMachine { uint32_t failsafe_start_ms; // timestamp of when failsafe was triggered }; + +#endif // AC_PRECLAND_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_config.h b/libraries/AC_PrecLand/AC_PrecLand_config.h new file mode 100644 index 0000000000000..74a5043955d0b --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_config.h @@ -0,0 +1,27 @@ +#pragma once + +#include + +#ifndef AC_PRECLAND_ENABLED +#define AC_PRECLAND_ENABLED 1 +#endif + +#ifndef AC_PRECLAND_BACKEND_DEFAULT_ENABLED +#define AC_PRECLAND_BACKEND_DEFAULT_ENABLED AC_PRECLAND_ENABLED +#endif + +#ifndef AC_PRECLAND_COMPANION_ENABLED +#define AC_PRECLAND_COMPANION_ENABLED AC_PRECLAND_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AC_PRECLAND_IRLOCK_ENABLED +#define AC_PRECLAND_IRLOCK_ENABLED AC_PRECLAND_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AC_PRECLAND_SITL_ENABLED +#define AC_PRECLAND_SITL_ENABLED (AC_PRECLAND_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AC_PRECLAND_SITL_GAZEBO_ENABLED +#define AC_PRECLAND_SITL_GAZEBO_ENABLED (AC_PRECLAND_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 715a5261ca694..a3d97e30c0538 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -45,7 +45,7 @@ #include "AP_Airspeed_analog.h" #include "AP_Airspeed_ASP5033.h" #include "AP_Airspeed_Backend.h" -#include "AP_Airspeed_UAVCAN.h" +#include "AP_Airspeed_DroneCAN.h" #include "AP_Airspeed_NMEA.h" #include "AP_Airspeed_MSP.h" #include "AP_Airspeed_SITL.h" @@ -415,8 +415,8 @@ void AP_Airspeed::allocate() #endif break; case TYPE_UAVCAN: -#if AP_AIRSPEED_UAVCAN_ENABLED - sensor[i] = AP_Airspeed_UAVCAN::probe(*this, i, uint32_t(param[i].bus_id.get())); +#if AP_AIRSPEED_DRONECAN_ENABLED + sensor[i] = AP_Airspeed_DroneCAN::probe(*this, i, uint32_t(param[i].bus_id.get())); #endif break; case TYPE_NMEA_WATER: @@ -442,18 +442,18 @@ void AP_Airspeed::allocate() } } -#if AP_AIRSPEED_UAVCAN_ENABLED +#if AP_AIRSPEED_DRONECAN_ENABLED // we need a 2nd pass for DroneCAN sensors so we can match order by DEVID // the 2nd pass accepts any devid for (uint8_t i=0; i +#include +#include + +extern const AP_HAL::HAL& hal; + +#define LOG_TAG "AirSpeed" + +AP_Airspeed_DroneCAN::DetectedModules AP_Airspeed_DroneCAN::_detected_modules[]; +HAL_Semaphore AP_Airspeed_DroneCAN::_sem_registry; + +void AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +{ + if (ap_dronecan == nullptr) { + return; + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("airspeed_sub"); + } + +#if AP_AIRSPEED_HYGROMETER_ENABLE + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("hygrometer_sub"); + } +#endif +} + +AP_Airspeed_Backend* AP_Airspeed_DroneCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_Airspeed_DroneCAN* backend = nullptr; + + for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { + if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) { + const auto bus_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, + _detected_modules[i].ap_dronecan->get_driver_index(), + _detected_modules[i].node_id, 0); + if (previous_devid != 0 && previous_devid != bus_id) { + // match with previous ID only + continue; + } + backend = new AP_Airspeed_DroneCAN(_frontend, _instance); + if (backend == nullptr) { + AP::can().log_text(AP_CANManager::LOG_INFO, + LOG_TAG, + "Failed register DroneCAN Airspeed Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_dronecan->get_driver_index()); + } else { + _detected_modules[i].driver = backend; + AP::can().log_text(AP_CANManager::LOG_INFO, + LOG_TAG, + "Registered DroneCAN Airspeed Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_dronecan->get_driver_index()); + backend->set_bus_id(bus_id); + } + break; + } + } + + return backend; +} + +AP_Airspeed_DroneCAN* AP_Airspeed_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id) +{ + if (ap_dronecan == nullptr) { + return nullptr; + } + + for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { + if (_detected_modules[i].driver != nullptr && + _detected_modules[i].ap_dronecan == ap_dronecan && + _detected_modules[i].node_id == node_id ) { + return _detected_modules[i].driver; + } + } + + bool detected = false; + for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { + if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) { + // detected + detected = true; + break; + } + } + + if (!detected) { + for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { + if (_detected_modules[i].ap_dronecan == nullptr) { + _detected_modules[i].ap_dronecan = ap_dronecan; + _detected_modules[i].node_id = node_id; + break; + } + } + } + + return nullptr; +} + +void AP_Airspeed_DroneCAN::handle_airspeed(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_RawAirData &msg) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_Airspeed_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); + + if (driver != nullptr) { + WITH_SEMAPHORE(driver->_sem_airspeed); + driver->_pressure = msg.differential_pressure; + if (!isnan(msg.static_air_temperature) && + msg.static_air_temperature > 0) { + driver->_temperature = KELVIN_TO_C(msg.static_air_temperature); + driver->_have_temperature = true; + } + driver->_last_sample_time_ms = AP_HAL::millis(); + } +} + +#if AP_AIRSPEED_HYGROMETER_ENABLE +void AP_Airspeed_DroneCAN::handle_hygrometer(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_hygrometer_Hygrometer &msg) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_Airspeed_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); + + if (driver != nullptr) { + WITH_SEMAPHORE(driver->_sem_airspeed); + driver->_hygrometer.temperature = KELVIN_TO_C(msg.temperature); + driver->_hygrometer.humidity = msg.humidity; + driver->_hygrometer.last_sample_ms = AP_HAL::millis(); + } +} +#endif // AP_AIRSPEED_HYGROMETER_ENABLE + +bool AP_Airspeed_DroneCAN::init() +{ + // always returns true + return true; +} + +bool AP_Airspeed_DroneCAN::get_differential_pressure(float &pressure) +{ + WITH_SEMAPHORE(_sem_airspeed); + + if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { + return false; + } + + pressure = _pressure; + + return true; +} + +bool AP_Airspeed_DroneCAN::get_temperature(float &temperature) +{ + if (!_have_temperature) { + return false; + } + WITH_SEMAPHORE(_sem_airspeed); + + if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { + return false; + } + + temperature = _temperature; + + return true; +} + +#if AP_AIRSPEED_HYGROMETER_ENABLE +/* + return hygrometer data if available + */ +bool AP_Airspeed_DroneCAN::get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) +{ + if (_hygrometer.last_sample_ms == 0) { + return false; + } + WITH_SEMAPHORE(_sem_airspeed); + last_sample_ms = _hygrometer.last_sample_ms; + temperature = _hygrometer.temperature; + humidity = _hygrometer.humidity; + return true; +} +#endif // AP_AIRSPEED_HYGROMETER_ENABLE +#endif // AP_AIRSPEED_DRONECAN_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h similarity index 57% rename from libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h rename to libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h index ae67d77c61cb5..cc2484d27d828 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h +++ b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h @@ -2,22 +2,20 @@ #include -#ifndef AP_AIRSPEED_UAVCAN_ENABLED -#define AP_AIRSPEED_UAVCAN_ENABLED HAL_ENABLE_LIBUAVCAN_DRIVERS +#ifndef AP_AIRSPEED_DRONECAN_ENABLED +#define AP_AIRSPEED_DRONECAN_ENABLED HAL_ENABLE_DRONECAN_DRIVERS #endif -#if AP_AIRSPEED_UAVCAN_ENABLED +#if AP_AIRSPEED_DRONECAN_ENABLED #include "AP_Airspeed_Backend.h" -#include +#include -class AirspeedCb; -class HygrometerCb; - -class AP_Airspeed_UAVCAN : public AP_Airspeed_Backend { +class AP_Airspeed_DroneCAN : public AP_Airspeed_Backend { public: - AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance); + + using AP_Airspeed_Backend::AP_Airspeed_Backend; bool init(void) override; @@ -32,16 +30,16 @@ class AP_Airspeed_UAVCAN : public AP_Airspeed_Backend { bool get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) override; #endif - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance, uint32_t previous_devid); private: - static void handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AirspeedCb &cb); - static void handle_hygrometer(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HygrometerCb &cb); + static void handle_airspeed(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_RawAirData &msg); + static void handle_hygrometer(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_sensors_hygrometer_Hygrometer &msg); - static AP_Airspeed_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id); + static AP_Airspeed_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id); float _pressure; // Pascal float _temperature; // Celcius @@ -58,9 +56,9 @@ class AP_Airspeed_UAVCAN : public AP_Airspeed_Backend { // Module Detection Registry static struct DetectedModules { - AP_UAVCAN* ap_uavcan; + AP_DroneCAN* ap_dronecan; uint8_t node_id; - AP_Airspeed_UAVCAN *driver; + AP_Airspeed_DroneCAN *driver; } _detected_modules[AIRSPEED_MAX_SENSORS]; static HAL_Semaphore _sem_registry; @@ -68,4 +66,4 @@ class AP_Airspeed_UAVCAN : public AP_Airspeed_Backend { }; -#endif // AP_AIRSPEED_UAVCAN_ENABLED +#endif // AP_AIRSPEED_DRONECAN_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h index 64ea04fb76913..70e30b127e086 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS4525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS4525.h @@ -27,7 +27,6 @@ */ #include -#include #include #include #include diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h index afa7d448c422c..0bcb74a2d85e1 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h @@ -27,7 +27,6 @@ #if AP_AIRSPEED_MS5525_ENABLED #include -#include #include #include #include diff --git a/libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp b/libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp index d085e5bd99e72..3da9c968e2f82 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_NMEA.cpp @@ -30,13 +30,6 @@ #define TIMEOUT_MS 2000 -extern const AP_HAL::HAL &hal; - -AP_Airspeed_NMEA::AP_Airspeed_NMEA(AP_Airspeed &_frontend, uint8_t _instance) : - AP_Airspeed_Backend(_frontend, _instance) -{ -} - bool AP_Airspeed_NMEA::init() { const AP_SerialManager& serial_manager = AP::serialmanager(); diff --git a/libraries/AP_Airspeed/AP_Airspeed_NMEA.h b/libraries/AP_Airspeed/AP_Airspeed_NMEA.h index 480637799f63b..264f1c651dca0 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_NMEA.h +++ b/libraries/AP_Airspeed/AP_Airspeed_NMEA.h @@ -15,7 +15,8 @@ class AP_Airspeed_NMEA : public AP_Airspeed_Backend { public: - AP_Airspeed_NMEA(AP_Airspeed &frontend, uint8_t _instance); + + using AP_Airspeed_Backend::AP_Airspeed_Backend; // probe and initialise the sensor bool init(void) override; diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp index 616e634c8aac2..e68fc3cd210f0 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.cpp @@ -42,11 +42,6 @@ extern const AP_HAL::HAL &hal; -AP_Airspeed_SDP3X::AP_Airspeed_SDP3X(AP_Airspeed &_frontend, uint8_t _instance) : - AP_Airspeed_Backend(_frontend, _instance) -{ -} - /* send a 16 bit command code */ diff --git a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h index 6669cd1751c42..01b652f17fc3e 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h +++ b/libraries/AP_Airspeed/AP_Airspeed_SDP3X.h @@ -27,7 +27,6 @@ */ #include -#include #include #include #include @@ -42,8 +41,8 @@ class AP_Airspeed_SDP3X : public AP_Airspeed_Backend { public: - AP_Airspeed_SDP3X(AP_Airspeed &frontend, uint8_t _instance); - ~AP_Airspeed_SDP3X(void) {} + + using AP_Airspeed_Backend::AP_Airspeed_Backend; // probe and initialise the sensor bool init() override; diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp deleted file mode 100644 index deea83c2aa58a..0000000000000 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp +++ /dev/null @@ -1,217 +0,0 @@ -#include "AP_Airspeed_UAVCAN.h" - -#if AP_AIRSPEED_UAVCAN_ENABLED - -#include -#include - -#include -#if AP_AIRSPEED_HYGROMETER_ENABLE -#include -#endif -extern const AP_HAL::HAL& hal; - -#define LOG_TAG "AirSpeed" - -// Frontend Registry Binders -UC_REGISTRY_BINDER(AirspeedCb, uavcan::equipment::air_data::RawAirData); - -#if AP_AIRSPEED_HYGROMETER_ENABLE -UC_REGISTRY_BINDER(HygrometerCb, dronecan::sensors::hygrometer::Hygrometer); -#endif - -AP_Airspeed_UAVCAN::DetectedModules AP_Airspeed_UAVCAN::_detected_modules[]; -HAL_Semaphore AP_Airspeed_UAVCAN::_sem_registry; - -// constructor -AP_Airspeed_UAVCAN::AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance) : - AP_Airspeed_Backend(_frontend, _instance) -{} - -void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) -{ - if (ap_uavcan == nullptr) { - return; - } - - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *airspeed_listener; - airspeed_listener = new uavcan::Subscriber(*node); - - const int airspeed_listener_res = airspeed_listener->start(AirspeedCb(ap_uavcan, &handle_airspeed)); - if (airspeed_listener_res < 0) { - AP_HAL::panic("DroneCAN Airspeed subscriber error \n"); - } - -#if AP_AIRSPEED_HYGROMETER_ENABLE - uavcan::Subscriber *hygrometer_listener; - hygrometer_listener = new uavcan::Subscriber(*node); - const int hygrometer_listener_res = hygrometer_listener->start(HygrometerCb(ap_uavcan, &handle_hygrometer)); - if (hygrometer_listener_res < 0) { - AP_HAL::panic("DroneCAN Hygrometer subscriber error\n"); - } -#endif -} - -AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid) -{ - WITH_SEMAPHORE(_sem_registry); - - AP_Airspeed_UAVCAN* backend = nullptr; - - for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { - if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) { - const auto bus_id = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, - _detected_modules[i].ap_uavcan->get_driver_index(), - _detected_modules[i].node_id, 0); - if (previous_devid != 0 && previous_devid != bus_id) { - // match with previous ID only - continue; - } - backend = new AP_Airspeed_UAVCAN(_frontend, _instance); - if (backend == nullptr) { - AP::can().log_text(AP_CANManager::LOG_INFO, - LOG_TAG, - "Failed register UAVCAN Airspeed Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); - } else { - _detected_modules[i].driver = backend; - AP::can().log_text(AP_CANManager::LOG_INFO, - LOG_TAG, - "Registered UAVCAN Airspeed Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); - backend->set_bus_id(bus_id); - } - break; - } - } - - return backend; -} - -AP_Airspeed_UAVCAN* AP_Airspeed_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id) -{ - if (ap_uavcan == nullptr) { - return nullptr; - } - - for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { - if (_detected_modules[i].driver != nullptr && - _detected_modules[i].ap_uavcan == ap_uavcan && - _detected_modules[i].node_id == node_id ) { - return _detected_modules[i].driver; - } - } - - bool detected = false; - for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { - if (_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].node_id == node_id) { - // detected - detected = true; - break; - } - } - - if (!detected) { - for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) { - if (_detected_modules[i].ap_uavcan == nullptr) { - _detected_modules[i].ap_uavcan = ap_uavcan; - _detected_modules[i].node_id = node_id; - break; - } - } - } - - return nullptr; -} - -void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AirspeedCb &cb) -{ - WITH_SEMAPHORE(_sem_registry); - - AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); - - if (driver != nullptr) { - WITH_SEMAPHORE(driver->_sem_airspeed); - driver->_pressure = cb.msg->differential_pressure; - if (!isnan(cb.msg->static_air_temperature) && - cb.msg->static_air_temperature > 0) { - driver->_temperature = KELVIN_TO_C(cb.msg->static_air_temperature); - driver->_have_temperature = true; - } - driver->_last_sample_time_ms = AP_HAL::millis(); - } -} - -#if AP_AIRSPEED_HYGROMETER_ENABLE -void AP_Airspeed_UAVCAN::handle_hygrometer(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HygrometerCb &cb) -{ - WITH_SEMAPHORE(_sem_registry); - - AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); - - if (driver != nullptr) { - WITH_SEMAPHORE(driver->_sem_airspeed); - driver->_hygrometer.temperature = KELVIN_TO_C(cb.msg->temperature); - driver->_hygrometer.humidity = cb.msg->humidity; - driver->_hygrometer.last_sample_ms = AP_HAL::millis(); - } -} -#endif // AP_AIRSPEED_HYGROMETER_ENABLE - -bool AP_Airspeed_UAVCAN::init() -{ - // always returns true - return true; -} - -bool AP_Airspeed_UAVCAN::get_differential_pressure(float &pressure) -{ - WITH_SEMAPHORE(_sem_airspeed); - - if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { - return false; - } - - pressure = _pressure; - - return true; -} - -bool AP_Airspeed_UAVCAN::get_temperature(float &temperature) -{ - if (!_have_temperature) { - return false; - } - WITH_SEMAPHORE(_sem_airspeed); - - if ((AP_HAL::millis() - _last_sample_time_ms) > 100) { - return false; - } - - temperature = _temperature; - - return true; -} - -#if AP_AIRSPEED_HYGROMETER_ENABLE -/* - return hygrometer data if available - */ -bool AP_Airspeed_UAVCAN::get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) -{ - if (_hygrometer.last_sample_ms == 0) { - return false; - } - WITH_SEMAPHORE(_sem_airspeed); - last_sample_ms = _hygrometer.last_sample_ms; - temperature = _hygrometer.temperature; - humidity = _hygrometer.humidity; - return true; -} -#endif // AP_AIRSPEED_HYGROMETER_ENABLE - -#endif // AP_AIRSPEED_UAVCAN_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed_analog.h b/libraries/AP_Airspeed/AP_Airspeed_analog.h index 3ee890db48198..9490843692550 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_analog.h +++ b/libraries/AP_Airspeed/AP_Airspeed_analog.h @@ -9,7 +9,6 @@ #if AP_AIRSPEED_ANALOG_ENABLED #include -#include #include "AP_Airspeed_Backend.h" diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index d2434bbfb79f2..55079c3ba44e8 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -65,7 +65,7 @@ #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub) #include #endif - #include + #include #endif #include @@ -1173,12 +1173,12 @@ bool AP_Arming::can_checks(bool report) #endif break; } - case AP_CANManager::Driver_Type_UAVCAN: + case AP_CANManager::Driver_Type_DroneCAN: { -#if HAL_ENABLE_LIBUAVCAN_DRIVERS - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); - if (ap_uavcan != nullptr && !ap_uavcan->prearm_check(fail_msg, ARRAY_SIZE(fail_msg))) { - check_failed(ARMING_CHECK_SYSTEM, report, "UAVCAN: %s", fail_msg); +#if HAL_ENABLE_DRONECAN_DRIVERS + AP_DroneCAN *ap_dronecan = AP_DroneCAN::get_dronecan(i); + if (ap_dronecan != nullptr && !ap_dronecan->prearm_check(fail_msg, ARRAY_SIZE(fail_msg))) { + check_failed(ARMING_CHECK_SYSTEM, report, "DroneCAN: %s", fail_msg); return false; } #endif @@ -1466,6 +1466,26 @@ bool AP_Arming::serial_protocol_checks(bool display_failure) return true; } +//Check for estop +bool AP_Arming::estop_checks(bool display_failure) +{ + if (!SRV_Channels::get_emergency_stop()) { + // not emergency-stopped, so no prearm failure: + return true; + } + // 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) { + // an RC channel is configured for arm_emergency_stop option, so estop maybe activated via this switch + if (chan->get_aux_switch_pos() == RC_Channel::AuxSwitchPos::LOW) { + // switch is configured and is in estop position, so likely the reason we are estopped, so no prearm failure + return true; // no prearm failure + } + } + check_failed(display_failure,"Motors Emergency Stopped"); + return false; +} + bool AP_Arming::pre_arm_checks(bool report) { #if !APM_BUILD_COPTER_OR_HELI @@ -1507,7 +1527,8 @@ bool AP_Arming::pre_arm_checks(bool report) & disarm_switch_checks(report) & fence_checks(report) & opendroneid_checks(report) - & serial_protocol_checks(report); + & serial_protocol_checks(report) + & estop_checks(report); } bool AP_Arming::arm_checks(AP_Arming::Method method) diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 98e531e127ce2..8cb1c5f060d3a 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -213,6 +213,8 @@ class AP_Arming { bool opendroneid_checks(bool display_failure); bool serial_protocol_checks(bool display_failure); + + bool estop_checks(bool display_failure); virtual bool system_checks(bool report); diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 2d7371ae43c86..ef39507cc3c6e 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -43,7 +43,7 @@ #include "AP_Baro_DPS280.h" #include "AP_Baro_BMP388.h" #include "AP_Baro_Dummy.h" -#include "AP_Baro_UAVCAN.h" +#include "AP_Baro_DroneCAN.h" #include "AP_Baro_MSP.h" #include "AP_Baro_ExternalAHRS.h" #include "AP_Baro_ICP101XX.h" @@ -605,15 +605,18 @@ void AP_Baro::init(void) if (sitl == nullptr) { AP_HAL::panic("No SITL pointer"); } +#if !AP_TEST_DRONECAN_DRIVERS + // use dronecan instances instead of SITL instances for(uint8_t i = 0; i < sitl->baro_count; i++) { ADD_BACKEND(new AP_Baro_SITL(*this)); } #endif +#endif -#if AP_BARO_UAVCAN_ENABLED +#if AP_BARO_DRONECAN_ENABLED // Detect UAVCAN Modules, try as many times as there are driver slots for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { - ADD_BACKEND(AP_Baro_UAVCAN::probe(*this)); + ADD_BACKEND(AP_Baro_DroneCAN::probe(*this)); } #endif diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 3d423164cd1ec..ba76b23419c94 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -28,6 +28,7 @@ class AP_Baro { friend class AP_Baro_Backend; friend class AP_Baro_SITL; // for access to sensors[] + friend class AP_Baro_DroneCAN; // for access to sensors[] public: AP_Baro(); diff --git a/libraries/AP_Baro/AP_Baro_DroneCAN.cpp b/libraries/AP_Baro/AP_Baro_DroneCAN.cpp new file mode 100644 index 0000000000000..ee1ee9e427ba9 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_DroneCAN.cpp @@ -0,0 +1,177 @@ +#include "AP_Baro_DroneCAN.h" + +#if AP_BARO_DRONECAN_ENABLED + +#include +#include +#include "AP_Baro_SITL.h" +#include + +extern const AP_HAL::HAL& hal; + +#define LOG_TAG "Baro" + +AP_Baro_DroneCAN::DetectedModules AP_Baro_DroneCAN::_detected_modules[]; +HAL_Semaphore AP_Baro_DroneCAN::_sem_registry; + +/* + constructor - registers instance at top Baro driver + */ +AP_Baro_DroneCAN::AP_Baro_DroneCAN(AP_Baro &baro) : + AP_Baro_Backend(baro) +{} + +void AP_Baro_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +{ + if (ap_dronecan == nullptr) { + return; + } + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_pressure, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("pressure_sub"); + } + + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_temperature, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("temperature_sub"); + } +} + +AP_Baro_Backend* AP_Baro_DroneCAN::probe(AP_Baro &baro) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_Baro_DroneCAN* backend = nullptr; + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) { + backend = new AP_Baro_DroneCAN(baro); + if (backend == nullptr) { + AP::can().log_text(AP_CANManager::LOG_ERROR, + LOG_TAG, + "Failed register DroneCAN Baro Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_dronecan->get_driver_index()); + } else { + _detected_modules[i].driver = backend; + backend->_pressure = 0; + backend->_pressure_count = 0; + backend->_ap_dronecan = _detected_modules[i].ap_dronecan; + backend->_node_id = _detected_modules[i].node_id; + + backend->_instance = backend->_frontend.register_sensor(); + backend->set_bus_id(backend->_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, + _detected_modules[i].ap_dronecan->get_driver_index(), + backend->_node_id, 0)); + + AP::can().log_text(AP_CANManager::LOG_INFO, + LOG_TAG, + "Registered DroneCAN Baro Node %d on Bus %d\n", + _detected_modules[i].node_id, + _detected_modules[i].ap_dronecan->get_driver_index()); + } + break; + } + } + return backend; +} + +AP_Baro_DroneCAN* AP_Baro_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, bool create_new) +{ + if (ap_dronecan == nullptr) { + return nullptr; + } + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].driver != nullptr && + _detected_modules[i].ap_dronecan == ap_dronecan && + _detected_modules[i].node_id == node_id) { + return _detected_modules[i].driver; + } + } + + if (create_new) { + bool already_detected = false; + //Check if there's an empty spot for possible registeration + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) { + //Already Detected + already_detected = true; + break; + } + } + if (!already_detected) { + for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { + if (_detected_modules[i].ap_dronecan == nullptr) { + _detected_modules[i].ap_dronecan = ap_dronecan; + _detected_modules[i].node_id = node_id; + break; + } + } + } + } + + return nullptr; +} + + +void AP_Baro_DroneCAN::_update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count) +{ + *accum += val; + *count += 1; + if (*count == max_count) { + *count = max_count / 2; + *accum = *accum / 2; + } +} + +void AP_Baro_DroneCAN::handle_pressure(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticPressure &msg) +{ + AP_Baro_DroneCAN* driver; + { + WITH_SEMAPHORE(_sem_registry); + driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, true); + if (driver == nullptr) { + return; + } + } + { + WITH_SEMAPHORE(driver->_sem_baro); + _update_and_wrap_accumulator(&driver->_pressure, msg.static_pressure, &driver->_pressure_count, 32); + driver->new_pressure = true; + } +} + +void AP_Baro_DroneCAN::handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticTemperature &msg) +{ + AP_Baro_DroneCAN* driver; + { + WITH_SEMAPHORE(_sem_registry); + driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, false); + if (driver == nullptr) { + return; + } + } + { + WITH_SEMAPHORE(driver->_sem_baro); + driver->_temperature = KELVIN_TO_C(msg.static_temperature); + } +} + +// Read the sensor +void AP_Baro_DroneCAN::update(void) +{ + float pressure = 0; + + WITH_SEMAPHORE(_sem_baro); + if (new_pressure) { + if (_pressure_count != 0) { + pressure = _pressure / _pressure_count; + _pressure_count = 0; + _pressure = 0; + } + _copy_to_frontend(_instance, pressure, _temperature); + + + _frontend.set_external_temperature(_temperature); + new_pressure = false; + } +} + +#endif // AP_BARO_DRONECAN_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_DroneCAN.h b/libraries/AP_Baro/AP_Baro_DroneCAN.h new file mode 100644 index 0000000000000..3040b97ab6b36 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_DroneCAN.h @@ -0,0 +1,52 @@ +#pragma once + +#include "AP_Baro_Backend.h" + +#if AP_BARO_DRONECAN_ENABLED + +#include +#if AP_TEST_DRONECAN_DRIVERS +#include +#endif + +class AP_Baro_DroneCAN : public AP_Baro_Backend { +public: + AP_Baro_DroneCAN(AP_Baro &baro); + + void update() override; + + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static AP_Baro_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, bool create_new); + static AP_Baro_Backend* probe(AP_Baro &baro); + + static void handle_pressure(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticPressure &msg); + static void handle_temperature(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_StaticTemperature &msg); +#if AP_TEST_DRONECAN_DRIVERS + void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = !AP::sitl()->baro[instance].disable; }; +#endif +private: + + static void _update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count); + + uint8_t _instance; + + bool new_pressure; + float _pressure; + float _temperature; + uint8_t _pressure_count; + HAL_Semaphore _sem_baro; + + AP_DroneCAN* _ap_dronecan; + uint8_t _node_id; + + // Module Detection Registry + static struct DetectedModules { + AP_DroneCAN* ap_dronecan; + uint8_t node_id; + AP_Baro_DroneCAN* driver; + } _detected_modules[BARO_MAX_DRIVERS]; + + static HAL_Semaphore _sem_registry; +}; + +#endif // AP_BARO_DRONECAN_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_SITL.cpp b/libraries/AP_Baro/AP_Baro_SITL.cpp index 1dd4e0e15be7a..9b41e336459d7 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.cpp +++ b/libraries/AP_Baro/AP_Baro_SITL.cpp @@ -29,17 +29,17 @@ AP_Baro_SITL::AP_Baro_SITL(AP_Baro &baro) : void AP_Baro_SITL::temperature_adjustment(float &p, float &T) { const float tsec = AP_HAL::millis() * 0.001f; - const float T_sensor = T + _sitl->temp_board_offset; - const float tconst = _sitl->temp_tconst; + const float T_sensor = T + AP::sitl()->temp_board_offset; + const float tconst = AP::sitl()->temp_tconst; if (tsec < 23 * tconst) { // time which past the equation below equals T_sensor within approx. 1E-9 - const float T0 = _sitl->temp_start; + const float T0 = AP::sitl()->temp_start; T = T_sensor - (T_sensor - T0) * expf(-tsec / tconst); } else { T = T_sensor; } - const float baro_factor = _sitl->temp_baro_factor; + const float baro_factor = AP::sitl()->temp_baro_factor; const float Tzero = 30.0f; // start baro adjustment at 30C if (is_positive(baro_factor)) { // this produces a pressure change with temperature that @@ -129,7 +129,7 @@ void AP_Baro_SITL::_timer() #endif // add in correction for wind effects - p += wind_pressure_correction(); + p += wind_pressure_correction(_instance); _recent_press = p; _recent_temp = T; @@ -157,12 +157,12 @@ void AP_Baro_SITL::update(void) /* return pressure correction for wind based on SIM_BARO_WCF parameters */ -float AP_Baro_SITL::wind_pressure_correction(void) +float AP_Baro_SITL::wind_pressure_correction(uint8_t instance) { - const auto &bp = _sitl->baro[_instance]; + const auto &bp = AP::sitl()->baro[instance]; // correct for static pressure position errors - const Vector3f &airspeed_vec_bf = _sitl->state.velocity_air_bf; + const Vector3f &airspeed_vec_bf = AP::sitl()->state.velocity_air_bf; float error = 0.0; const float sqx = sq(airspeed_vec_bf.x); diff --git a/libraries/AP_Baro/AP_Baro_SITL.h b/libraries/AP_Baro/AP_Baro_SITL.h index 5fe90a985cc72..68ba04d016ac0 100644 --- a/libraries/AP_Baro/AP_Baro_SITL.h +++ b/libraries/AP_Baro/AP_Baro_SITL.h @@ -14,6 +14,12 @@ class AP_Baro_SITL : public AP_Baro_Backend { void update() override; + // adjust for simulated board temperature + static void temperature_adjustment(float &p, float &T); + + // adjust for wind effects + static float wind_pressure_correction(uint8_t instance); + protected: void update_healthy_flag(uint8_t instance) override { _frontend.sensors[instance].healthy = healthy(instance); }; @@ -32,12 +38,6 @@ class AP_Baro_SITL : public AP_Baro_Backend { static const uint8_t _buffer_length = 50; VectorN _buffer; - // adjust for simulated board temperature - void temperature_adjustment(float &p, float &T); - - // adjust for wind effects - float wind_pressure_correction(void); - // is the barometer usable for flight bool healthy(uint8_t instance); diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp deleted file mode 100644 index 0c7c46e5cc8a9..0000000000000 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include "AP_Baro_UAVCAN.h" - -#if AP_BARO_UAVCAN_ENABLED - -#include -#include - -#include -#include - -extern const AP_HAL::HAL& hal; - -#define LOG_TAG "Baro" - -//UAVCAN Frontend Registry Binder -UC_REGISTRY_BINDER(PressureCb, uavcan::equipment::air_data::StaticPressure); -UC_REGISTRY_BINDER(TemperatureCb, uavcan::equipment::air_data::StaticTemperature); - -AP_Baro_UAVCAN::DetectedModules AP_Baro_UAVCAN::_detected_modules[]; -HAL_Semaphore AP_Baro_UAVCAN::_sem_registry; - -/* - constructor - registers instance at top Baro driver - */ -AP_Baro_UAVCAN::AP_Baro_UAVCAN(AP_Baro &baro) : - AP_Baro_Backend(baro) -{} - -void AP_Baro_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) -{ - if (ap_uavcan == nullptr) { - return; - } - - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *pressure_listener; - pressure_listener = new uavcan::Subscriber(*node); - // Msg Handler - const int pressure_listener_res = pressure_listener->start(PressureCb(ap_uavcan, &handle_pressure)); - if (pressure_listener_res < 0) { - AP_HAL::panic("UAVCAN Baro subscriber start problem\n\r"); - } - - uavcan::Subscriber *temperature_listener; - temperature_listener = new uavcan::Subscriber(*node); - // Msg Handler - const int temperature_listener_res = temperature_listener->start(TemperatureCb(ap_uavcan, &handle_temperature)); - if (temperature_listener_res < 0) { - AP_HAL::panic("UAVCAN Baro subscriber start problem\n\r"); - } -} - -AP_Baro_Backend* AP_Baro_UAVCAN::probe(AP_Baro &baro) -{ - WITH_SEMAPHORE(_sem_registry); - - AP_Baro_UAVCAN* backend = nullptr; - for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { - if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) { - backend = new AP_Baro_UAVCAN(baro); - if (backend == nullptr) { - AP::can().log_text(AP_CANManager::LOG_ERROR, - LOG_TAG, - "Failed register UAVCAN Baro Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); - } else { - _detected_modules[i].driver = backend; - backend->_pressure = 0; - backend->_pressure_count = 0; - backend->_ap_uavcan = _detected_modules[i].ap_uavcan; - backend->_node_id = _detected_modules[i].node_id; - - backend->_instance = backend->_frontend.register_sensor(); - backend->set_bus_id(backend->_instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, - _detected_modules[i].ap_uavcan->get_driver_index(), - backend->_node_id, 0)); - - AP::can().log_text(AP_CANManager::LOG_INFO, - LOG_TAG, - "Registered UAVCAN Baro Node %d on Bus %d\n", - _detected_modules[i].node_id, - _detected_modules[i].ap_uavcan->get_driver_index()); - } - break; - } - } - return backend; -} - -AP_Baro_UAVCAN* AP_Baro_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, bool create_new) -{ - if (ap_uavcan == nullptr) { - return nullptr; - } - for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { - if (_detected_modules[i].driver != nullptr && - _detected_modules[i].ap_uavcan == ap_uavcan && - _detected_modules[i].node_id == node_id) { - return _detected_modules[i].driver; - } - } - - if (create_new) { - bool already_detected = false; - //Check if there's an empty spot for possible registeration - for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { - if (_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].node_id == node_id) { - //Already Detected - already_detected = true; - break; - } - } - if (!already_detected) { - for (uint8_t i = 0; i < BARO_MAX_DRIVERS; i++) { - if (_detected_modules[i].ap_uavcan == nullptr) { - _detected_modules[i].ap_uavcan = ap_uavcan; - _detected_modules[i].node_id = node_id; - break; - } - } - } - } - - return nullptr; -} - - -void AP_Baro_UAVCAN::_update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count) -{ - *accum += val; - *count += 1; - if (*count == max_count) { - *count = max_count / 2; - *accum = *accum / 2; - } -} - -void AP_Baro_UAVCAN::handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PressureCb &cb) -{ - AP_Baro_UAVCAN* driver; - { - WITH_SEMAPHORE(_sem_registry); - driver = get_uavcan_backend(ap_uavcan, node_id, true); - if (driver == nullptr) { - return; - } - } - { - WITH_SEMAPHORE(driver->_sem_baro); - _update_and_wrap_accumulator(&driver->_pressure, cb.msg->static_pressure, &driver->_pressure_count, 32); - driver->new_pressure = true; - } -} - -void AP_Baro_UAVCAN::handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb) -{ - AP_Baro_UAVCAN* driver; - { - WITH_SEMAPHORE(_sem_registry); - driver = get_uavcan_backend(ap_uavcan, node_id, false); - if (driver == nullptr) { - return; - } - } - { - WITH_SEMAPHORE(driver->_sem_baro); - driver->_temperature = KELVIN_TO_C(cb.msg->static_temperature); - } -} - -// Read the sensor -void AP_Baro_UAVCAN::update(void) -{ - float pressure = 0; - - WITH_SEMAPHORE(_sem_baro); - if (new_pressure) { - if (_pressure_count != 0) { - pressure = _pressure / _pressure_count; - _pressure_count = 0; - _pressure = 0; - } - _copy_to_frontend(_instance, pressure, _temperature); - - - _frontend.set_external_temperature(_temperature); - new_pressure = false; - } -} - -#endif // AP_BARO_UAVCAN_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.h b/libraries/AP_Baro/AP_Baro_UAVCAN.h deleted file mode 100644 index 2edcc2e871cb5..0000000000000 --- a/libraries/AP_Baro/AP_Baro_UAVCAN.h +++ /dev/null @@ -1,50 +0,0 @@ -#pragma once - -#include "AP_Baro_Backend.h" - -#if AP_BARO_UAVCAN_ENABLED - -#include - -class PressureCb; -class TemperatureCb; - -class AP_Baro_UAVCAN : public AP_Baro_Backend { -public: - AP_Baro_UAVCAN(AP_Baro &baro); - - void update() override; - - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static AP_Baro_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, bool create_new); - static AP_Baro_Backend* probe(AP_Baro &baro); - - static void handle_pressure(AP_UAVCAN* ap_uavcan, uint8_t node_id, const PressureCb &cb); - static void handle_temperature(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TemperatureCb &cb); - -private: - - static void _update_and_wrap_accumulator(float *accum, float val, uint8_t *count, const uint8_t max_count); - - uint8_t _instance; - - bool new_pressure; - float _pressure; - float _temperature; - uint8_t _pressure_count; - HAL_Semaphore _sem_baro; - - AP_UAVCAN* _ap_uavcan; - uint8_t _node_id; - - // Module Detection Registry - static struct DetectedModules { - AP_UAVCAN* ap_uavcan; - uint8_t node_id; - AP_Baro_UAVCAN* driver; - } _detected_modules[BARO_MAX_DRIVERS]; - - static HAL_Semaphore _sem_registry; -}; - -#endif // AP_BARO_UAVCAN_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_config.h b/libraries/AP_Baro/AP_Baro_config.h index 87df55266b0fc..981baf6684816 100644 --- a/libraries/AP_Baro/AP_Baro_config.h +++ b/libraries/AP_Baro/AP_Baro_config.h @@ -77,6 +77,6 @@ #define AP_BARO_SPL06_ENABLED AP_BARO_BACKEND_DEFAULT_ENABLED #endif -#ifndef AP_BARO_UAVCAN_ENABLED -#define AP_BARO_UAVCAN_ENABLED (AP_BARO_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_LIBUAVCAN_DRIVERS) +#ifndef AP_BARO_DRONECAN_ENABLED +#define AP_BARO_DRONECAN_ENABLED (AP_BARO_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS) #endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 89dea3872ecd6..c2880d24e6eb7 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -23,8 +23,8 @@ #include -#if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include "AP_BattMonitor_UAVCAN.h" +#if HAL_ENABLE_DRONECAN_DRIVERS +#include "AP_BattMonitor_DroneCAN.h" #endif #include @@ -52,7 +52,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: _ // @Path: AP_BattMonitor_Sum.cpp // @Group: _ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: _ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: _ @@ -71,7 +71,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 2_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 2_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 2_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 2_ @@ -91,7 +91,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 3_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 3_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 3_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 3_ @@ -111,7 +111,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 4_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 4_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 4_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 4_ @@ -131,7 +131,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 5_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 5_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 5_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 5_ @@ -151,7 +151,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 6_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 6_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 6_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 6_ @@ -171,7 +171,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 7_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 7_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 7_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 7_ @@ -191,7 +191,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 8_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 8_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 8_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 8_ @@ -211,7 +211,7 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Group: 9_ // @Path: AP_BattMonitor_Sum.cpp // @Group: 9_ - // @Path: AP_BattMonitor_UAVCAN.cpp + // @Path: AP_BattMonitor_DroneCAN.cpp // @Group: 9_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp // @Group: 9_ @@ -266,10 +266,12 @@ AP_BattMonitor::init() state[instance].instance = instance; switch (get_type(instance)) { +#if AP_BATTERY_ANALOG_ENABLED case Type::ANALOG_VOLTAGE_ONLY: case Type::ANALOG_VOLTAGE_AND_CURRENT: drivers[instance] = new AP_BattMonitor_Analog(*this, state[instance], _params[instance]); break; +#endif #if AP_BATTERY_SMBUS_SOLO_ENABLED case Type::SOLO: drivers[instance] = new AP_BattMonitor_SMBus_Solo(*this, state[instance], _params[instance]); @@ -310,7 +312,7 @@ AP_BattMonitor::init() break; case Type::UAVCAN_BatteryInfo: #if AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED - drivers[instance] = new AP_BattMonitor_UAVCAN(*this, state[instance], AP_BattMonitor_UAVCAN::UAVCAN_BATTERY_INFO, _params[instance]); + drivers[instance] = new AP_BattMonitor_DroneCAN(*this, state[instance], AP_BattMonitor_DroneCAN::UAVCAN_BATTERY_INFO, _params[instance]); #endif break; case Type::BLHeliESC: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index 6b04bd9fef62c..93987634cdc84 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -37,7 +37,7 @@ class AP_BattMonitor_SMBus_Solo; class AP_BattMonitor_SMBus_Generic; class AP_BattMonitor_SMBus_Maxell; class AP_BattMonitor_SMBus_Rotoye; -class AP_BattMonitor_UAVCAN; +class AP_BattMonitor_DroneCAN; class AP_BattMonitor_Generator; class AP_BattMonitor_INA2XX; class AP_BattMonitor_INA239; @@ -56,7 +56,7 @@ class AP_BattMonitor friend class AP_BattMonitor_SMBus_Generic; friend class AP_BattMonitor_SMBus_Maxell; friend class AP_BattMonitor_SMBus_Rotoye; - friend class AP_BattMonitor_UAVCAN; + friend class AP_BattMonitor_DroneCAN; friend class AP_BattMonitor_Sum; friend class AP_BattMonitor_FuelFlow; friend class AP_BattMonitor_FuelLevel_PWM; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp similarity index 57% rename from libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp rename to libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 2c1c7fbc8bfd3..6b30aee470d2d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -4,47 +4,35 @@ #include #include "AP_BattMonitor.h" -#include "AP_BattMonitor_UAVCAN.h" +#include "AP_BattMonitor_DroneCAN.h" #include #include #include #include -#include +#include #include -#include -#include -#include -#include - #define LOG_TAG "BattMon" extern const AP_HAL::HAL& hal; -const AP_Param::GroupInfo AP_BattMonitor_UAVCAN::var_info[] = { +const AP_Param::GroupInfo AP_BattMonitor_DroneCAN::var_info[] = { // @Param: CURR_MULT // @DisplayName: Scales reported power monitor current // @Description: Multiplier applied to all current related reports to allow for adjustment if no UAVCAN param access or current splitting applications // @Range: .1 10 // @User: Advanced - AP_GROUPINFO("CURR_MULT", 30, AP_BattMonitor_UAVCAN, _curr_mult, 1.0), + AP_GROUPINFO("CURR_MULT", 30, AP_BattMonitor_DroneCAN, _curr_mult, 1.0), // Param indexes must be between 30 and 39 to avoid conflict with other battery monitor param tables loaded by pointer AP_GROUPEND }; -UC_REGISTRY_BINDER(BattInfoCb, uavcan::equipment::power::BatteryInfo); -UC_REGISTRY_BINDER(BattInfoAuxCb, ardupilot::equipment::power::BatteryInfoAux); -UC_REGISTRY_BINDER(MpptStreamCb, mppt::Stream); - -static void trampoline_handleOutputEnable(const uavcan::ServiceCallResult& resp); -static uavcan::ServiceClient* outputEnable_client[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - /// Constructor -AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms) : +AP_BattMonitor_DroneCAN::AP_BattMonitor_DroneCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_DroneCAN_Type type, AP_BattMonitor_Params ¶ms) : AP_BattMonitor_Backend(mon, mon_state, params), _type(type) { @@ -55,54 +43,28 @@ AP_BattMonitor_UAVCAN::AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor _state.healthy = false; } -void AP_BattMonitor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return; } - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *battinfo_listener; - battinfo_listener = new uavcan::Subscriber(*node); - // Backend Msg Handler - const int battinfo_listener_res = battinfo_listener->start(BattInfoCb(ap_uavcan, &handle_battery_info_trampoline)); - if (battinfo_listener_res < 0) { - AP_BoardConfig::allocation_error("UAVCAN BatteryInfo subscriber start problem\n\r"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("battinfo_sub"); } - uavcan::Subscriber *battinfo_aux_listener; - battinfo_aux_listener = new uavcan::Subscriber(*node); - // Backend Msg Handler - const int battinfo_aux_listener_res = battinfo_aux_listener->start(BattInfoAuxCb(ap_uavcan, &handle_battery_info_aux_trampoline)); - if (battinfo_aux_listener_res < 0) { - AP_BoardConfig::allocation_error("UAVCAN BatteryInfoAux subscriber start problem"); - return; - } - - uavcan::Subscriber *mppt_stream_listener; - mppt_stream_listener = new uavcan::Subscriber(*node); - // Backend Msg Handler - const int mppt_stream_listener_res = mppt_stream_listener->start(MpptStreamCb(ap_uavcan, &handle_mppt_stream_trampoline)); - if (mppt_stream_listener_res < 0) { - AP_BoardConfig::allocation_error("UAVCAN Mppt::Stream subscriber start problem"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("battinfo_aux_sub"); } - // set up callback to verify mppt::OutputEnable - const uint8_t driver_index = ap_uavcan->get_driver_index(); - outputEnable_client[driver_index] = new uavcan::ServiceClient(*node); - if (outputEnable_client[driver_index] == nullptr || outputEnable_client[driver_index]->init() < 0) { - AP_BoardConfig::allocation_error("UAVCAN Mppt::outputEnable client start problem"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("mppt_stream_sub"); } - outputEnable_client[driver_index]->setCallback(trampoline_handleOutputEnable); } -AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t battery_id) +AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return nullptr; } for (uint8_t i = 0; i < AP::battery()._num_instances; i++) { @@ -110,8 +72,8 @@ AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u AP::battery().get_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) { continue; } - AP_BattMonitor_UAVCAN* driver = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i]; - if (driver->_ap_uavcan == ap_uavcan && driver->_node_id == node_id && match_battery_id(i, battery_id)) { + AP_BattMonitor_DroneCAN* driver = (AP_BattMonitor_DroneCAN*)AP::battery().drivers[i]; + if (driver->_ap_dronecan == ap_dronecan && driver->_node_id == node_id && match_battery_id(i, battery_id)) { return driver; } } @@ -121,36 +83,35 @@ AP_BattMonitor_UAVCAN* AP_BattMonitor_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u AP::battery().get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo && match_battery_id(i, battery_id)) { - AP_BattMonitor_UAVCAN* batmon = (AP_BattMonitor_UAVCAN*)AP::battery().drivers[i]; - if(batmon->_ap_uavcan != nullptr || batmon->_node_id != 0) { + AP_BattMonitor_DroneCAN* batmon = (AP_BattMonitor_DroneCAN*)AP::battery().drivers[i]; + if(batmon->_ap_dronecan != nullptr || batmon->_node_id != 0) { continue; } - batmon->_ap_uavcan = ap_uavcan; + batmon->_ap_dronecan = ap_dronecan; batmon->_node_id = node_id; batmon->_instance = i; - batmon->_node = ap_uavcan->get_node(); batmon->init(); AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "Registered BattMonitor Node %d on Bus %d\n", node_id, - ap_uavcan->get_driver_index()); + ap_dronecan->get_driver_index()); return batmon; } } return nullptr; } -void AP_BattMonitor_UAVCAN::handle_battery_info(const BattInfoCb &cb) +void AP_BattMonitor_DroneCAN::handle_battery_info(const uavcan_equipment_power_BatteryInfo &msg) { - update_interim_state(cb.msg->voltage, cb.msg->current, cb.msg->temperature, cb.msg->state_of_charge_pct); + update_interim_state(msg.voltage, msg.current, msg.temperature, msg.state_of_charge_pct); WITH_SEMAPHORE(_sem_battmon); - _remaining_capacity_wh = cb.msg->remaining_capacity_wh; - _full_charge_capacity_wh = cb.msg->full_charge_capacity_wh; + _remaining_capacity_wh = msg.remaining_capacity_wh; + _full_charge_capacity_wh = msg.full_charge_capacity_wh; } -void AP_BattMonitor_UAVCAN::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) { WITH_SEMAPHORE(_sem_battmon); @@ -178,18 +139,18 @@ void AP_BattMonitor_UAVCAN::update_interim_state(const float voltage, const floa _interim_state.healthy = true; } -void AP_BattMonitor_UAVCAN::handle_battery_info_aux(const BattInfoAuxCb &cb) +void AP_BattMonitor_DroneCAN::handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg) { WITH_SEMAPHORE(_sem_battmon); - uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), cb.msg->voltage_cell.size()); - float remaining_capacity_ah = _remaining_capacity_wh / cb.msg->nominal_voltage; - float full_charge_capacity_ah = _full_charge_capacity_wh / cb.msg->nominal_voltage; + uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), msg.voltage_cell.len); + float remaining_capacity_ah = _remaining_capacity_wh / msg.nominal_voltage; + float full_charge_capacity_ah = _full_charge_capacity_wh / msg.nominal_voltage; - _cycle_count = cb.msg->cycle_count; + _cycle_count = msg.cycle_count; for (uint8_t i = 0; i < cell_count; i++) { - _interim_state.cell_voltages.cells[i] = cb.msg->voltage_cell[i] * 1000; + _interim_state.cell_voltages.cells[i] = msg.voltage_cell.data[i] * 1000; } - _interim_state.is_powering_off = cb.msg->is_powering_off; + _interim_state.is_powering_off = msg.is_powering_off; _interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000; _interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh; _interim_state.time_remaining = is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600); @@ -201,17 +162,17 @@ void AP_BattMonitor_UAVCAN::handle_battery_info_aux(const BattInfoAuxCb &cb) _has_battery_info_aux = true; } -void AP_BattMonitor_UAVCAN::handle_mppt_stream(const MpptStreamCb &cb) +void AP_BattMonitor_DroneCAN::handle_mppt_stream(const mppt_Stream &msg) { const bool use_input_value = option_is_set(AP_BattMonitor_Params::Options::MPPT_Use_Input_Value); - const float voltage = use_input_value ? cb.msg->input_voltage : cb.msg->output_voltage; - const float current = use_input_value ? cb.msg->input_current : cb.msg->output_current; + const float voltage = use_input_value ? msg.input_voltage : msg.output_voltage; + const float current = use_input_value ? msg.input_current : msg.output_current; // use an invalid soc so we use the library calculated one const uint8_t soc = 127; // convert C to Kelvin - const float temperature_K = isnan(cb.msg->temperature) ? 0 : C_TO_KELVIN(cb.msg->temperature); + const float temperature_K = isnan(msg.temperature) ? 0 : C_TO_KELVIN(msg.temperature); update_interim_state(voltage, current, temperature_K, soc); @@ -229,42 +190,42 @@ void AP_BattMonitor_UAVCAN::handle_mppt_stream(const MpptStreamCb &cb) } #if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG - if (_mppt.fault_flags != cb.msg->fault_flags) { - mppt_report_faults(_instance, cb.msg->fault_flags); + if (_mppt.fault_flags != msg.fault_flags) { + mppt_report_faults(_instance, msg.fault_flags); } #endif - _mppt.fault_flags = cb.msg->fault_flags; + _mppt.fault_flags = msg.fault_flags; } -void AP_BattMonitor_UAVCAN::handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb) +void AP_BattMonitor_DroneCAN::handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg) { - AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->battery_id); + AP_BattMonitor_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id); if (driver == nullptr) { return; } - driver->handle_battery_info(cb); + driver->handle_battery_info(msg); } -void AP_BattMonitor_UAVCAN::handle_battery_info_aux_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoAuxCb &cb) +void AP_BattMonitor_DroneCAN::handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg) { - AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->battery_id); + AP_BattMonitor_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id); if (driver == nullptr) { return; } - driver->handle_battery_info_aux(cb); + driver->handle_battery_info_aux(msg); } -void AP_BattMonitor_UAVCAN::handle_mppt_stream_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MpptStreamCb &cb) +void AP_BattMonitor_DroneCAN::handle_mppt_stream_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const mppt_Stream &msg) { - AP_BattMonitor_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, node_id); + AP_BattMonitor_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, transfer.source_node_id); if (driver == nullptr) { return; } - driver->handle_mppt_stream(cb); + driver->handle_mppt_stream(msg); } // read - read the voltage and current -void AP_BattMonitor_UAVCAN::read() +void AP_BattMonitor_DroneCAN::read() { uint32_t tnow = AP_HAL::micros(); @@ -296,7 +257,7 @@ void AP_BattMonitor_UAVCAN::read() } /// capacity_remaining_pct - returns true if the percentage is valid and writes to percentage argument -bool AP_BattMonitor_UAVCAN::capacity_remaining_pct(uint8_t &percentage) const +bool AP_BattMonitor_DroneCAN::capacity_remaining_pct(uint8_t &percentage) const { if ((uint32_t(_params._options.get()) & uint32_t(AP_BattMonitor_Params::Options::Ignore_UAVCAN_SoC)) || _mppt.is_detected || @@ -317,7 +278,7 @@ bool AP_BattMonitor_UAVCAN::capacity_remaining_pct(uint8_t &percentage) const } /// get_cycle_count - return true if cycle count can be provided and fills in cycles argument -bool AP_BattMonitor_UAVCAN::get_cycle_count(uint16_t &cycles) const +bool AP_BattMonitor_DroneCAN::get_cycle_count(uint16_t &cycles) const { if (_has_battery_info_aux) { cycles = _cycle_count; @@ -327,7 +288,7 @@ bool AP_BattMonitor_UAVCAN::get_cycle_count(uint16_t &cycles) const } // request MPPT board to power on/off depending upon vehicle arming state as specified by BATT_OPTIONS -void AP_BattMonitor_UAVCAN::mppt_check_powered_state() +void AP_BattMonitor_DroneCAN::mppt_check_powered_state() { if ((_mppt.powered_state_remote_ms != 0) && (AP_HAL::millis() - _mppt.powered_state_remote_ms >= 1000)) { // there's already a set attempt that didnt' respond. Retry at 1Hz @@ -349,9 +310,9 @@ void AP_BattMonitor_UAVCAN::mppt_check_powered_state() // request MPPT board to power on or off // power_on should be true to power on the MPPT, false to power off // force should be true to force sending the state change request to the MPPT -void AP_BattMonitor_UAVCAN::mppt_set_powered_state(bool power_on) +void AP_BattMonitor_DroneCAN::mppt_set_powered_state(bool power_on) { - if (_ap_uavcan == nullptr || _node == nullptr || !_mppt.is_detected) { + if (_ap_dronecan == nullptr || !_mppt.is_detected) { return; } @@ -360,48 +321,28 @@ void AP_BattMonitor_UAVCAN::mppt_set_powered_state(bool power_on) GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Battery %u: powering %s%s", (unsigned)_instance+1, _mppt.powered_state ? "ON" : "OFF", (_mppt.powered_state_remote_ms == 0) ? "" : " Retry"); - // set up a request /w a status callback - mppt::OutputEnable::Request request; + mppt_OutputEnableRequest request; request.enable = _mppt.powered_state; request.disable = !request.enable; - _mppt.powered_state_remote_ms = AP_HAL::millis(); - const uint8_t driver_index = _ap_uavcan->get_driver_index(); - outputEnable_client[driver_index]->call(_node_id, request); -} - -// callback trampoline of outputEnable response to verify it is enabled or disabled -void trampoline_handleOutputEnable(const uavcan::ServiceCallResult& resp) -{ - uint8_t can_num_drivers = AP::can().get_num_drivers(); - for (uint8_t i = 0; i < can_num_drivers; i++) { - AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); - if (uavcan == nullptr) { - continue; + if (mppt_outputenable_client == nullptr) { + mppt_outputenable_client = new Canard::Client{_ap_dronecan->get_canard_iface(), mppt_outputenable_res_cb}; + if (mppt_outputenable_client == nullptr) { + return; } - - const uint8_t node_id = resp.getResponse().getSrcNodeID().get(); - AP_BattMonitor_UAVCAN* driver = AP_BattMonitor_UAVCAN::get_uavcan_backend(uavcan, node_id, node_id); - if (driver == nullptr) { - continue; - } - - const auto &response = resp.getResponse(); - const uint8_t nodeId = response.getSrcNodeID().get(); - const bool enabled = response.enabled; - driver->handle_outputEnable_response(nodeId, enabled); } + mppt_outputenable_client->request(_node_id, request); } // callback from outputEnable to verify it is enabled or disabled -void AP_BattMonitor_UAVCAN::handle_outputEnable_response(const uint8_t nodeId, const bool enabled) +void AP_BattMonitor_DroneCAN::handle_outputEnable_response(const CanardRxTransfer& transfer, const mppt_OutputEnableResponse& response) { - if (nodeId != _node_id) { + if (transfer.source_node_id != _node_id) { // this response is not from the node we are looking for return; } - if (enabled == _mppt.powered_state) { + if (response.enabled == _mppt.powered_state) { // we got back what we expected it to be. We set it on, it now says it on (or vice versa). // Clear the timer so we don't re-request _mppt.powered_state_remote_ms = 0; @@ -410,7 +351,7 @@ void AP_BattMonitor_UAVCAN::handle_outputEnable_response(const uint8_t nodeId, c #if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG // report changes in MPPT faults -void AP_BattMonitor_UAVCAN::mppt_report_faults(const uint8_t instance, const uint8_t fault_flags) +void AP_BattMonitor_DroneCAN::mppt_report_faults(const uint8_t instance, const uint8_t fault_flags) { // handle recovery if (fault_flags == 0) { @@ -429,7 +370,7 @@ void AP_BattMonitor_UAVCAN::mppt_report_faults(const uint8_t instance, const uin } // returns string description of MPPT fault bit. Only handles single bit faults -const char* AP_BattMonitor_UAVCAN::mppt_fault_string(const MPPT_FaultFlags fault) +const char* AP_BattMonitor_DroneCAN::mppt_fault_string(const MPPT_FaultFlags fault) { switch (fault) { case MPPT_FaultFlags::OVER_VOLTAGE: @@ -446,7 +387,7 @@ const char* AP_BattMonitor_UAVCAN::mppt_fault_string(const MPPT_FaultFlags fault #endif // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum) -uint32_t AP_BattMonitor_UAVCAN::get_mavlink_fault_bitmask() const +uint32_t AP_BattMonitor_DroneCAN::get_mavlink_fault_bitmask() const { // return immediately if not mppt or no faults if (!_mppt.is_detected || (_mppt.fault_flags == 0)) { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h similarity index 67% rename from libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h rename to libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index ec1c517d4a961..9c5c56bb73b52 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -2,9 +2,8 @@ #include "AP_BattMonitor.h" #include "AP_BattMonitor_Backend.h" - -#include -#include +#if HAL_ENABLE_DRONECAN_DRIVERS +#include #define AP_BATTMONITOR_UAVCAN_TIMEOUT_MICROS 5000000 // sensor becomes unhealthy if no successful readings for 5 seconds @@ -12,19 +11,15 @@ #define AP_BATTMONITOR_UAVCAN_MPPT_DEBUG 0 #endif -class BattInfoCb; -class BattInfoAuxCb; -class MpptStreamCb; - -class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend +class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend { public: - enum BattMonitor_UAVCAN_Type { + enum BattMonitor_DroneCAN_Type { UAVCAN_BATTERY_INFO = 0 }; /// Constructor - AP_BattMonitor_UAVCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_UAVCAN_Type type, AP_BattMonitor_Params ¶ms); + AP_BattMonitor_DroneCAN(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, BattMonitor_DroneCAN_Type type, AP_BattMonitor_Params ¶ms); static const struct AP_Param::GroupInfo var_info[]; @@ -51,18 +46,17 @@ class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum) uint32_t get_mavlink_fault_bitmask() const override; - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static AP_BattMonitor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t battery_id); - static void handle_battery_info_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoCb &cb); - static void handle_battery_info_aux_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const BattInfoAuxCb &cb); - static void handle_mppt_stream_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MpptStreamCb &cb); - void handle_outputEnable_response(const uint8_t nodeId, const bool enabled); + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static AP_BattMonitor_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id); + static void handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg); + static void handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg); + static void handle_mppt_stream_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const mppt_Stream &msg); void mppt_set_powered_state(bool power_on) override; private: - void handle_battery_info(const BattInfoCb &cb); - void handle_battery_info_aux(const BattInfoAuxCb &cb); + 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); static bool match_battery_id(uint8_t instance, uint8_t battery_id) { @@ -77,7 +71,7 @@ class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend OVER_CURRENT = (1U<<2), OVER_TEMPERATURE = (1U<<3), }; - void handle_mppt_stream(const MpptStreamCb &cb); + void handle_mppt_stream(const mppt_Stream &msg); void mppt_check_powered_state(); #if AP_BATTMONITOR_UAVCAN_MPPT_DEBUG @@ -86,11 +80,11 @@ class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend #endif AP_BattMonitor::BattMonitor_State _interim_state; - BattMonitor_UAVCAN_Type _type; + BattMonitor_DroneCAN_Type _type; HAL_Semaphore _sem_battmon; - AP_UAVCAN* _ap_uavcan; + AP_DroneCAN* _ap_dronecan; uint8_t _soc; uint8_t _node_id; uint16_t _cycle_count; @@ -102,7 +96,7 @@ class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend bool _has_consumed_energy; bool _has_battery_info_aux; uint8_t _instance; // instance of this battery monitor - uavcan::Node<0> *_node; // UAVCAN node id + AP_Float _curr_mult; // scaling multiplier applied to current reports for adjustment // MPPT variables struct { @@ -112,4 +106,10 @@ class AP_BattMonitor_UAVCAN : public AP_BattMonitor_Backend uint8_t fault_flags; // bits holding fault flags uint32_t powered_state_remote_ms; // timestamp of when request was sent, zeroed on response. Used to retry } _mppt; + + void handle_outputEnable_response(const CanardRxTransfer&, const mppt_OutputEnableResponse&); + + Canard::ObjCallback mppt_outputenable_res_cb{this, &AP_BattMonitor_DroneCAN::handle_outputEnable_response}; + Canard::Client *mppt_outputenable_client; }; +#endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_config.h b/libraries/AP_BattMonitor/AP_BattMonitor_config.h index 4856281e0193b..1a020bee26fe2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_config.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_config.h @@ -31,7 +31,7 @@ #endif #ifndef AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED -#define AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_LIBUAVCAN_DRIVERS +#define AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS #endif #ifndef AP_BATTERY_FUELFLOW_ENABLED diff --git a/libraries/AP_BoardConfig/board_drivers.cpp b/libraries/AP_BoardConfig/board_drivers.cpp index 40b2a30647075..6d59ea32d61d3 100644 --- a/libraries/AP_BoardConfig/board_drivers.cpp +++ b/libraries/AP_BoardConfig/board_drivers.cpp @@ -54,7 +54,7 @@ void AP_BoardConfig::board_init_safety() */ void AP_BoardConfig::board_init_debug() { -#ifndef HAL_BUILD_AP_PERIPH +#if !defined(HAL_BUILD_AP_PERIPH) && !defined(HAL_DEBUG_BUILD) if ((_options & BOARD_OPTION_DEBUG_ENABLE) == 0) { #ifdef HAL_GPIO_PIN_JTCK_SWCLK palSetLineMode(HAL_GPIO_PIN_JTCK_SWCLK, PAL_MODE_INPUT); @@ -63,7 +63,7 @@ void AP_BoardConfig::board_init_debug() palSetLineMode(HAL_GPIO_PIN_JTMS_SWDIO, PAL_MODE_INPUT); #endif } -#endif // HAL_BUILD_AP_PERIPH +#endif // HAL_BUILD_AP_PERIPH && HAL_DEBUG_BUILD } diff --git a/libraries/AP_CANManager/AP_CANDriver.cpp b/libraries/AP_CANManager/AP_CANDriver.cpp index 5e602b1a789c6..150c66f3c718d 100644 --- a/libraries/AP_CANManager/AP_CANDriver.cpp +++ b/libraries/AP_CANManager/AP_CANDriver.cpp @@ -19,7 +19,7 @@ #include "AP_CANManager.h" #include -#include +#include #include #include "AP_CANTester.h" #include @@ -34,11 +34,13 @@ const AP_Param::GroupInfo AP_CANManager::CANDriver_Params::var_info[] = { // @Values: 0:Disabled,1:DroneCAN,4:PiccoloCAN,5:CANTester,6:EFI_NWPMU,7:USD1,8:KDECAN,10:Scripting,11:Benewake,12:Scripting2 // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, AP_CANManager::Driver_Type_UAVCAN), + AP_GROUPINFO("PROTOCOL", 1, AP_CANManager::CANDriver_Params, _driver_type, AP_CANManager::Driver_Type_DroneCAN), +#if HAL_ENABLE_DRONECAN_DRIVERS // @Group: UC_ - // @Path: ../AP_UAVCAN/AP_UAVCAN.cpp - AP_SUBGROUPPTR(_uavcan, "UC_", 2, AP_CANManager::CANDriver_Params, AP_UAVCAN), + // @Path: ../AP_DroneCAN/AP_DroneCAN.cpp + AP_SUBGROUPPTR(_uavcan, "UC_", 2, AP_CANManager::CANDriver_Params, AP_DroneCAN), +#endif #if (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)) // @Group: KDE_ diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index b84a749c053df..00aeaa4da8cff 100644 --- a/libraries/AP_CANManager/AP_CANManager.cpp +++ b/libraries/AP_CANManager/AP_CANManager.cpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include @@ -115,6 +115,7 @@ AP_CANManager::AP_CANManager() _singleton = this; } +#if !AP_TEST_DRONECAN_DRIVERS void AP_CANManager::init() { WITH_SEMAPHORE(_sem); @@ -202,16 +203,19 @@ void AP_CANManager::init() } // Allocate the set type of Driver - if (drv_type[drv_num] == Driver_Type_UAVCAN) { - _drivers[drv_num] = _drv_param[drv_num]._uavcan = new AP_UAVCAN; +#if HAL_ENABLE_DRONECAN_DRIVERS + if (drv_type[drv_num] == Driver_Type_DroneCAN) { + _drivers[drv_num] = _drv_param[drv_num]._uavcan = new AP_DroneCAN(drv_num); if (_drivers[drv_num] == nullptr) { AP_BoardConfig::allocation_error("uavcan %d", i + 1); continue; } - AP_Param::load_object_from_eeprom((AP_UAVCAN*)_drivers[drv_num], AP_UAVCAN::var_info); - } else if (drv_type[drv_num] == Driver_Type_KDECAN) { + AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[drv_num], AP_DroneCAN::var_info); + } else +#endif + if (drv_type[drv_num] == Driver_Type_KDECAN) { #if (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)) // To be replaced with macro saying if KDECAN library is included _drivers[drv_num] = _drv_param[drv_num]._kdecan = new AP_KDECAN; @@ -279,7 +283,26 @@ void AP_CANManager::init() _driver_type_cache[drv_num] = drv_type[drv_num]; } } +#else +void AP_CANManager::init() +{ + WITH_SEMAPHORE(_sem); + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { + if ((Driver_Type) _drv_param[i]._driver_type.get() == Driver_Type_DroneCAN) { + _drivers[i] = _drv_param[i]._uavcan = new AP_DroneCAN(i); + + if (_drivers[i] == nullptr) { + AP_BoardConfig::allocation_error("uavcan %d", i + 1); + continue; + } + AP_Param::load_object_from_eeprom((AP_DroneCAN*)_drivers[i], AP_DroneCAN::var_info); + _drivers[i]->init(i, true); + _driver_type_cache[i] = (Driver_Type) _drv_param[i]._driver_type.get(); + } + } +} +#endif /* register a new CAN driver */ @@ -498,7 +521,7 @@ void AP_CANManager::process_frame_buffer(void) } const int16_t retcode = hal.can[frame.bus]->send(frame.frame, AP_HAL::native_micros64() + timeout_us, - frame.frame.isCanFDFrame()?AP_HAL::CANIface::IsMAVCAN:0); + AP_HAL::CANIface::IsMAVCAN); if (retcode == 0) { // no space in the CAN output slots, try again later break; diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index 5b04384a8a71c..8c350261415ee 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -58,7 +58,7 @@ class AP_CANManager enum Driver_Type : uint8_t { Driver_Type_None = 0, - Driver_Type_UAVCAN = 1, + Driver_Type_DroneCAN = 1, // 2 was KDECAN -- do not re-use // 3 was ToshibaCAN -- do not re-use Driver_Type_PiccoloCAN = 4, diff --git a/libraries/AP_CANManager/AP_CANTester.cpp b/libraries/AP_CANManager/AP_CANTester.cpp index 806713f8939a9..15abf98de86db 100644 --- a/libraries/AP_CANManager/AP_CANTester.cpp +++ b/libraries/AP_CANManager/AP_CANTester.cpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include #include @@ -182,14 +182,14 @@ void CANTester::main_thread() break; case CANTester::TEST_UAVCAN_DNA: if (_can_ifaces[1] == nullptr) { - gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN DNA Test********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********Running DroneCAN DNA Test********"); if (test_uavcan_dna()) { - gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Pass********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN DNA Test Pass********"); } else { - gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Fail********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN DNA Test Fail********"); } } else { - gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_DNA_TEST"); + gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for DroneCAN_DNA_TEST"); } break; case CANTester::TEST_KDE_CAN: @@ -206,23 +206,23 @@ void CANTester::main_thread() break; case CANTester::TEST_UAVCAN_ESC: if (_can_ifaces[1] == nullptr) { - gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN ESC Test********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********Running DroneCAN ESC Test********"); if (test_uavcan_esc(false)) { - gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Pass********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN ESC Test Pass********"); } else { - gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Fail********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN ESC Test Fail********"); } } else { - gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_ESC_TEST"); + gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for DroneCAN_ESC_TEST"); } break; case CANTester::TEST_UAVCAN_FD_ESC: if (_can_ifaces[1] == nullptr) { - gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN FD ESC Test********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********Running DroneCAN FD ESC Test********"); if (test_uavcan_esc(true)) { - gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Pass********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN FD ESC Test Pass********"); } else { - gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Fail********"); + gcs().send_text(MAV_SEVERITY_ALERT, "********DroneCAN FD ESC Test Fail********"); } } else { gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_FD_ESC_TEST"); @@ -417,7 +417,7 @@ bool CANTester::test_busoff_recovery() } /***************************************** - * UAVCAN DNA Test * + * DroneCAN DNA Test * * ***************************************/ bool CANTester::test_uavcan_dna() @@ -542,7 +542,7 @@ bool CANTester::run_kdecan_enumeration(bool start_stop) } /*********************************************** - * UAVCAN ESC * + * DroneCAN ESC * * *********************************************/ #define NUM_ESCS 4 diff --git a/libraries/AP_CANManager/AP_CANTester.h b/libraries/AP_CANManager/AP_CANTester.h index 45fffa26b4950..6fb07c761847c 100644 --- a/libraries/AP_CANManager/AP_CANTester.h +++ b/libraries/AP_CANManager/AP_CANTester.h @@ -17,7 +17,7 @@ #include "AP_CANDriver.h" #include -#include +#include #ifndef HAL_ENABLE_CANTESTER #define HAL_ENABLE_CANTESTER 0 #endif @@ -96,7 +96,7 @@ class CANTester : public AP_CANDriver // Classes required for UAVCAN Test class RaiiSynchronizer {}; - uavcan::PoolAllocator _node_allocator; + uavcan::PoolAllocator _node_allocator; HAL_EventHandle _event_handle; AP_Int8 _test_driver_index; diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index e9dad0de90608..d5db4eda2132e 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -13,9 +13,10 @@ #include "AP_Camera_Backend.h" #include "AP_Camera_Servo.h" #include "AP_Camera_Relay.h" +#include "AP_Camera_SoloGimbal.h" #include "AP_Camera_Mount.h" #include "AP_Camera_MAVLink.h" -#include "AP_Camera_SoloGimbal.h" +#include "AP_Camera_MAVLinkCamV2.h" const AP_Param::GroupInfo AP_Camera::var_info[] = { @@ -163,6 +164,12 @@ void AP_Camera::init() case CameraType::MAVLINK: _backends[instance] = new AP_Camera_MAVLink(*this, _params[instance], instance); break; +#endif +#if AP_CAMERA_MAVLINKCAMV2_ENABLED + // check for MAVLink Camv2 driver + case CameraType::MAVLINK_CAMV2: + _backends[instance] = new AP_Camera_MAVLinkCamV2(*this, _params[instance], instance); + break; #endif case CameraType::NONE: break; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 274e9f869600d..84df4681338c2 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -21,6 +21,8 @@ class AP_Camera_Servo; class AP_Camera_Relay; class AP_Camera_SoloGimbal; class AP_Camera_Mount; +class AP_Camera_MAVLink; +class AP_Camera_MAVLinkCamV2; /// @class Camera /// @brief Object managing a Photo or video camera @@ -33,6 +35,7 @@ class AP_Camera { friend class AP_Camera_SoloGimbal; friend class AP_Camera_Mount; friend class AP_Camera_MAVLink; + friend class AP_Camera_MAVLinkCamV2; public: @@ -62,6 +65,9 @@ class AP_Camera { #endif #if AP_CAMERA_MAVLINK_ENABLED MAVLINK = 5, // MAVLink enabled camera +#endif +#if AP_CAMERA_MAVLINKCAMV2_ENABLED + MAVLINK_CAMV2 = 6, // MAVLink camera v2 #endif }; diff --git a/libraries/AP_Camera/AP_Camera_MAVLink.h b/libraries/AP_Camera/AP_Camera_MAVLink.h index 21a7308a6ce63..0be92f09c0729 100644 --- a/libraries/AP_Camera/AP_Camera_MAVLink.h +++ b/libraries/AP_Camera/AP_Camera_MAVLink.h @@ -14,7 +14,7 @@ */ /* - Camera driver for cameras included in Mount + Camera driver for cameras that implement the older MAVLink camera protocol */ #pragma once diff --git a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp new file mode 100644 index 0000000000000..3eb6eb67bef43 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.cpp @@ -0,0 +1,222 @@ +#include "AP_Camera_MAVLinkCamV2.h" + +#if AP_CAMERA_MAVLINKCAMV2_ENABLED +#include + +extern const AP_HAL::HAL& hal; + +#define AP_CAMERA_MAVLINKCAMV2_SEARCH_MS 60000 // search for camera for 60 seconds after startup + +// update - should be called at 50hz +void AP_Camera_MAVLinkCamV2::update() +{ + // exit immediately if not initialised + if (!_initialised) { + find_camera(); + } + + // call parent update + AP_Camera_Backend::update(); +} + +// entry point to actually take a picture. returns true on success +bool AP_Camera_MAVLinkCamV2::trigger_pic() +{ + // exit immediately if have not found camera or does not support taking pictures + if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_CAPTURE_IMAGE)) { + return false; + } + + // prepare and send message + mavlink_command_long_t pkt {}; + pkt.command = MAV_CMD_IMAGE_START_CAPTURE; + pkt.param3 = 1; // number of images to take + pkt.param4 = image_index+1; // starting sequence number + + _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt); + + return true; +} + +// start or stop video recording. returns true on success +// set start_recording = true to start record, false to stop recording +bool AP_Camera_MAVLinkCamV2::record_video(bool start_recording) +{ + // exit immediately if have not found camera or does not support recording video + if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_CAPTURE_VIDEO)) { + return false; + } + + // prepare and send message + mavlink_command_long_t pkt {}; + + if (start_recording) { + pkt.command = MAV_CMD_VIDEO_START_CAPTURE; + // param1 = 0, video stream id. 0 for all streams + // param2 = 0, status frequency. frequency that CAMERA_CAPTURE_STATUS messages should be sent while recording. 0 for no messages + } else { + pkt.command = MAV_CMD_VIDEO_STOP_CAPTURE; + // param1 = 0, video stream id. 0 for all streams + } + + _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt); + + return true; +} + +// set camera zoom step. returns true on success +// zoom out = -1, hold = 0, zoom in = 1 +bool AP_Camera_MAVLinkCamV2::set_zoom_step(int8_t zoom_step) +{ + // exit immediately if have not found camera or does not support zoom + if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_ZOOM)) { + return false; + } + + // prepare and send message + mavlink_command_long_t pkt {}; + pkt.command = MAV_CMD_SET_CAMERA_ZOOM; + pkt.param1 = ZOOM_TYPE_CONTINUOUS; // Zoom Type, 0:ZOOM_TYPE_STEP, 1:ZOOM_TYPE_CONTINUOUS, 2:ZOOM_TYPE_RANGE, 3:ZOOM_TYPE_FOCAL_LENGTH + pkt.param2 = zoom_step; // Zoom Value + + _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt); + + return true; +} + +// set focus in, out or hold. returns true on success +// focus in = -1, focus hold = 0, focus out = 1 +bool AP_Camera_MAVLinkCamV2::set_manual_focus_step(int8_t focus_step) +{ + // exit immediately if have not found camera or does not support focus + if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) { + return false; + } + + // prepare and send message + mavlink_command_long_t pkt {}; + pkt.command = MAV_CMD_SET_CAMERA_FOCUS; + pkt.param1 = FOCUS_TYPE_CONTINUOUS; // Focus Type, 0:FOCUS_TYPE_STEP, 1:FOCUS_TYPE_CONTINUOUS, 2:FOCUS_TYPE_RANGE, 3:FOCUS_TYPE_METERS, 4:FOCUS_TYPE_AUTO, 5:FOCUS_TYPE_AUTO_SINGLE, 5:FOCUS_TYPE_AUTO_CONTINUOUS + pkt.param2 = focus_step; // Focus Value + + _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt); + + return true; +} + +// auto focus. returns true on success +bool AP_Camera_MAVLinkCamV2::set_auto_focus() +{ + // exit immediately if have not found camera or does not support focus + if (_link == nullptr || !(_cap_flags & CAMERA_CAP_FLAGS_HAS_BASIC_FOCUS)) { + return false; + } + + // prepare and send message + mavlink_command_long_t pkt {}; + pkt.command = MAV_CMD_SET_CAMERA_FOCUS; + pkt.param1 = FOCUS_TYPE_AUTO; // Focus Type, 0:FOCUS_TYPE_STEP, 1:FOCUS_TYPE_CONTINUOUS, 2:FOCUS_TYPE_RANGE, 3:FOCUS_TYPE_METERS, 4:FOCUS_TYPE_AUTO, 5:FOCUS_TYPE_AUTO_SINGLE, 5:FOCUS_TYPE_AUTO_CONTINUOUS + pkt.param2 = 0; // Focus Value + + _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt); + + return true; +} + +// handle incoming mavlink message including CAMERA_INFORMATION +void AP_Camera_MAVLinkCamV2::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) +{ + // exit immediately if this is not our message + if (msg.sysid != _sysid || msg.compid != _compid) { + return; + } + + // handle CAMERA_INFORMATION + if (msg.msgid == MAVLINK_MSG_ID_CAMERA_INFORMATION) { + mavlink_camera_information_t cam_info; + mavlink_msg_camera_information_decode(&msg, &cam_info); + + const uint8_t fw_ver_major = cam_info.firmware_version & 0x000000FF; + const uint8_t fw_ver_minor = (cam_info.firmware_version & 0x0000FF00) >> 8; + const uint8_t fw_ver_revision = (cam_info.firmware_version & 0x00FF0000) >> 16; + const uint8_t fw_ver_build = (cam_info.firmware_version & 0xFF000000) >> 24; + + // display camera info to user + gcs().send_text(MAV_SEVERITY_INFO, "Camera: %s.32 %s.32 fw:%u.%u.%u.%u", + cam_info.vendor_name, + cam_info.model_name, + (unsigned)fw_ver_major, + (unsigned)fw_ver_minor, + (unsigned)fw_ver_revision, + (unsigned)fw_ver_build); + + // capability flags + _cap_flags = cam_info.flags; + + _got_camera_info = true; + } +} + +// search for camera in GCS_MAVLink routing table +void AP_Camera_MAVLinkCamV2::find_camera() +{ + // do not look for camera for first 10 seconds so user may see banner + uint32_t now_ms = AP_HAL::millis(); + if (now_ms < 10000) { + return; + } + + // search for camera for 60 seconds or until armed + if ((now_ms > AP_CAMERA_MAVLINKCAMV2_SEARCH_MS) && hal.util->get_soft_armed()) { + return; + } + + // search for a mavlink enabled camera + if (_link == nullptr) { + // we expect that instance 0 has compid = MAV_COMP_ID_CAMERA, instance 1 has compid = MAV_COMP_ID_CAMERA2, etc + uint8_t compid = MIN(MAV_COMP_ID_CAMERA + _instance, MAV_COMP_ID_CAMERA6); + _link = GCS_MAVLINK::find_by_mavtype_and_compid(MAV_TYPE_CAMERA, compid, _sysid); + if (_link == nullptr) { + // have not yet found a camera so return + return; + } + _compid = compid; + } + + // request CAMERA_INFORMATION + if (!_got_camera_info) { + if (now_ms - _last_caminfo_req_ms > 1000) { + _last_caminfo_req_ms = now_ms; + request_camera_information(); + } + return; + } + + _initialised = true; +} + +// request CAMERA_INFORMATION (holds vendor and model name) +void AP_Camera_MAVLinkCamV2::request_camera_information() const +{ + if (_link == nullptr) { + return; + } + + const mavlink_command_long_t pkt { + MAVLINK_MSG_ID_CAMERA_INFORMATION, // param1 + 0, // param2 + 0, // param3 + 0, // param4 + 0, // param5 + 0, // param6 + 0, // param7 + MAV_CMD_REQUEST_MESSAGE, + _sysid, + _compid, + 0 // confirmation + }; + + _link->send_message(MAVLINK_MSG_ID_COMMAND_LONG, (const char*)&pkt); +} + +#endif // AP_CAMERA_MAVLINKCAMV2_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h new file mode 100644 index 0000000000000..57c991ee8abe2 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_MAVLinkCamV2.h @@ -0,0 +1,78 @@ +/* + 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 . + */ + +/* + Camera driver for cameras that implement the newer MAVLink camera v2 protocol + see https://mavlink.io/en/services/camera.html + */ +#pragma once + +#include "AP_Camera_Backend.h" + +#if AP_CAMERA_MAVLINKCAMV2_ENABLED + +class AP_Camera_MAVLinkCamV2 : public AP_Camera_Backend +{ +public: + + // Constructor + using AP_Camera_Backend::AP_Camera_Backend; + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Camera_MAVLinkCamV2); + + // update - should be called at 50hz + void update() override; + + // entry point to actually take a picture + bool trigger_pic() override; + + // start or stop video recording. returns true on success + // set start_recording = true to start record, false to stop recording + bool record_video(bool start_recording) override; + + // set camera zoom step. returns true on success + // zoom out = -1, hold = 0, zoom in = 1 + bool set_zoom_step(int8_t zoom_step) override; + + // set focus in, out or hold. returns true on success + // focus in = -1, focus hold = 0, focus out = 1 + bool set_manual_focus_step(int8_t focus_step) override; + + // auto focus. returns true on success + bool set_auto_focus() override; + + // handle incoming mavlink message + void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) override; + +private: + + // search for camera in GCS_MAVLink routing table + void find_camera(); + + // request CAMERA_INFORMATION (holds vendor and model name) + void request_camera_information() const; + + // internal members + bool _initialised; // true once the camera has provided a CAMERA_INFORMATION + bool _got_camera_info; // true once camera has provided CAMERA_INFORMATION + uint32_t _last_caminfo_req_ms; // system time that CAMERA_INFORMATION was last requested (used to throttle requests) + class GCS_MAVLINK *_link; // link we have found the camera on. nullptr if not seen yet + uint8_t _sysid; // sysid of camera + uint8_t _compid; // component id of gimbal + uint32_t _cap_flags; // capability flags from CAMERA_INFORMATION msg, see MAVLink CAMERA_CAP_FLAGS enum +}; + +#endif // AP_CAMERA_MAVLINKCAMV2_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 623aac18ddc6a..0ecec96b124ef 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink + // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2 // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index 9dc0123a503a4..f814a1524c998 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -15,6 +15,10 @@ #define AP_CAMERA_MAVLINK_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_CAMERA_MAVLINKCAMV2_ENABLED +#define AP_CAMERA_MAVLINKCAMV2_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024 +#endif + #ifndef AP_CAMERA_MOUNT_ENABLED #define AP_CAMERA_MOUNT_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && HAL_MOUNT_ENABLED #endif diff --git a/libraries/AP_Common/AP_Common.cpp b/libraries/AP_Common/AP_Common.cpp index cfc2e5379afa3..19c403c353bf0 100644 --- a/libraries/AP_Common/AP_Common.cpp +++ b/libraries/AP_Common/AP_Common.cpp @@ -74,14 +74,16 @@ bool hex_to_uint8(uint8_t a, uint8_t &res) /* strncpy without the warning for not leaving room for nul termination */ -void strncpy_noterm(char *dest, const char *src, size_t n) +size_t strncpy_noterm(char *dest, const char *src, size_t n) { size_t len = strnlen(src, n); + size_t ret = len; // return value is length of src if (len < n) { // include nul term if it fits len++; } memcpy(dest, src, len); + return ret; } /** diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index f9632eec01a17..fb2fdfc9ceb03 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -44,7 +44,9 @@ #define OPTIMIZE(level) __attribute__((optimize(level))) // sometimes we need to prevent inlining to prevent large stack usage +#ifndef NOINLINE #define NOINLINE __attribute__((noinline)) +#endif // used to ignore results for functions marked as warn unused #define IGNORE_RETURN(x) do {if (x) {}} while(0) @@ -158,7 +160,7 @@ bool hex_to_uint8(uint8_t a, uint8_t &res); // return the uint8 value of an asc /* strncpy without the warning for not leaving room for nul termination */ -void strncpy_noterm(char *dest, const char *src, size_t n); +size_t strncpy_noterm(char *dest, const char *src, size_t n); // return the numeric value of an ascii hex character int16_t char_to_hex(char a); diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index e7c31636f0da5..fb5225caaaa1c 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -393,6 +393,20 @@ bool Location::same_latlon_as(const Location &loc2) const return (lat == loc2.lat) && (lng == loc2.lng); } +bool Location::same_alt_as(const Location &loc2) const +{ + // fast path if the altitude frame is the same + if (this->get_alt_frame() == loc2.get_alt_frame()) { + return this->alt == loc2.alt; + } + + ftype alt_diff; + bool have_diff = this->get_alt_distance(loc2, alt_diff); + + const ftype tolerance = FLT_EPSILON; + return have_diff && (fabsF(alt_diff) < tolerance); +} + // return true when lat and lng are within range bool Location::check_latlng() const { diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index e294cef2fbc9f..122bead689b6c 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -107,6 +107,14 @@ class Location // check if lat and lng match. Ignore altitude and options bool same_latlon_as(const Location &loc2) const; + // check if altitude matches. + bool same_alt_as(const Location &loc2) const; + + // check if lat, lng, and alt match. + bool same_loc_as(const Location &loc2) const { + return same_latlon_as(loc2) && same_alt_as(loc2); + } + /* * convert invalid waypoint with useful data. return true if location changed */ diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index c3f8625533c49..f2c9e48c5045c 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -24,8 +24,8 @@ #include "AP_Compass_LIS3MDL.h" #include "AP_Compass_AK09916.h" #include "AP_Compass_QMC5883L.h" -#if AP_COMPASS_UAVCAN_ENABLED -#include "AP_Compass_UAVCAN.h" +#if AP_COMPASS_DRONECAN_ENABLED +#include "AP_Compass_DroneCAN.h" #endif #include "AP_Compass_MMC3416.h" #include "AP_Compass_MMC5xx3.h" @@ -1286,7 +1286,7 @@ void Compass::_detect_backends(void) } #endif -#if AP_COMPASS_SITL_ENABLED +#if AP_COMPASS_SITL_ENABLED && !AP_TEST_DRONECAN_DRIVERS ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL()); #endif @@ -1449,10 +1449,10 @@ void Compass::_detect_backends(void) #endif -#if AP_COMPASS_UAVCAN_ENABLED +#if AP_COMPASS_DRONECAN_ENABLED if (_driver_enabled(DRIVER_UAVCAN)) { for (uint8_t i=0; i 0 } -#endif // AP_COMPASS_UAVCAN_ENABLED +#endif // AP_COMPASS_DRONECAN_ENABLED if (_backend_count == 0 || _compass_count == 0) { @@ -1608,7 +1608,7 @@ void Compass::_reset_compass_id() void Compass::_detect_runtime(void) { -#if AP_COMPASS_UAVCAN_ENABLED +#if AP_COMPASS_DRONECAN_ENABLED if (!available()) { return; } @@ -1625,14 +1625,14 @@ Compass::_detect_runtime(void) last_try = AP_HAL::millis(); if (_driver_enabled(DRIVER_UAVCAN)) { for (uint8_t i=0; i. */ -#include "AP_Compass_UAVCAN.h" +#include "AP_Compass_DroneCAN.h" -#if AP_COMPASS_UAVCAN_ENABLED +#if AP_COMPASS_DRONECAN_ENABLED #include #include -#include - -#include -#include +#include +#include +#include extern const AP_HAL::HAL& hal; #define LOG_TAG "COMPASS" -// Frontend Registry Binders -UC_REGISTRY_BINDER(MagCb, uavcan::equipment::ahrs::MagneticFieldStrength); -UC_REGISTRY_BINDER(Mag2Cb, uavcan::equipment::ahrs::MagneticFieldStrength2); -AP_Compass_UAVCAN::DetectedModules AP_Compass_UAVCAN::_detected_modules[]; -HAL_Semaphore AP_Compass_UAVCAN::_sem_registry; +AP_Compass_DroneCAN::DetectedModules AP_Compass_DroneCAN::_detected_modules[]; +HAL_Semaphore AP_Compass_DroneCAN::_sem_registry; -AP_Compass_UAVCAN::AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id, uint32_t devid) - : _ap_uavcan(ap_uavcan) +AP_Compass_DroneCAN::AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id, uint32_t devid) + : _ap_dronecan(ap_dronecan) , _node_id(node_id) , _sensor_id(sensor_id) , _devid(devid) { } -void AP_Compass_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +void AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return; } - - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *mag_listener; - mag_listener = new uavcan::Subscriber(*node); - const int mag_listener_res = mag_listener->start(MagCb(ap_uavcan, &handle_magnetic_field)); - if (mag_listener_res < 0) { - AP_HAL::panic("UAVCAN Mag subscriber start problem\n\r"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("mag_sub"); } - uavcan::Subscriber *mag2_listener; - mag2_listener = new uavcan::Subscriber(*node); - const int mag2_listener_res = mag2_listener->start(Mag2Cb(ap_uavcan, &handle_magnetic_field_2)); - if (mag2_listener_res < 0) { - AP_HAL::panic("UAVCAN Mag subscriber start problem\n\r"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("mag2_sub"); } } -AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index) +AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index) { - AP_Compass_UAVCAN* driver = nullptr; - if (!_detected_modules[index].driver && _detected_modules[index].ap_uavcan) { + AP_Compass_DroneCAN* driver = nullptr; + if (!_detected_modules[index].driver && _detected_modules[index].ap_dronecan) { WITH_SEMAPHORE(_sem_registry); // Register new Compass mode to a backend - driver = new AP_Compass_UAVCAN(_detected_modules[index].ap_uavcan, _detected_modules[index].node_id, _detected_modules[index].sensor_id, _detected_modules[index].devid); + driver = new AP_Compass_DroneCAN(_detected_modules[index].ap_dronecan, _detected_modules[index].node_id, _detected_modules[index].sensor_id, _detected_modules[index].devid); if (driver) { if (!driver->init()) { delete driver; @@ -85,14 +70,31 @@ AP_Compass_Backend* AP_Compass_UAVCAN::probe(uint8_t index) LOG_TAG, "Found Mag Node %d on Bus %d Sensor ID %d\n", _detected_modules[index].node_id, - _detected_modules[index].ap_uavcan->get_driver_index(), + _detected_modules[index].ap_dronecan->get_driver_index(), _detected_modules[index].sensor_id); +#if AP_TEST_DRONECAN_DRIVERS + // Scroll through the registered compasses, and set the offsets + if (driver->_compass.get_offsets(index).is_zero()) { + driver->_compass.set_offsets(index, AP::sitl()->mag_ofs[index]); + } + + // we want to simulate a calibrated compass by default, so set + // scale to 1 + AP_Param::set_default_by_name("COMPASS_SCALE", 1); + AP_Param::set_default_by_name("COMPASS_SCALE2", 1); + AP_Param::set_default_by_name("COMPASS_SCALE3", 1); + driver->save_dev_id(index); + driver->set_rotation(index, ROTATION_NONE); + + // make first compass external + driver->set_external(index, true); +#endif } } return driver; } -bool AP_Compass_UAVCAN::init() +bool AP_Compass_DroneCAN::init() { // Adding 1 is necessary to allow backward compatibilty, where this field was set as 1 by default if (!register_compass(_devid, _instance)) { @@ -102,18 +104,18 @@ bool AP_Compass_UAVCAN::init() set_dev_id(_instance, _devid); set_external(_instance, true); - AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "AP_Compass_UAVCAN loaded\n\r"); + AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "AP_Compass_DroneCAN loaded\n\r"); return true; } -AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id) +AP_Compass_DroneCAN* AP_Compass_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return nullptr; } for (uint8_t i=0; iget_driver_index(), + ap_dronecan->get_driver_index(), node_id, sensor_id + 1); // we use sensor_id as devtype break; @@ -163,45 +165,44 @@ AP_Compass_UAVCAN* AP_Compass_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, u return nullptr; } -void AP_Compass_UAVCAN::handle_mag_msg(const Vector3f &mag) +void AP_Compass_DroneCAN::handle_mag_msg(const Vector3f &mag) { Vector3f raw_field = mag * 1000.0; accumulate_sample(raw_field, _instance); } -void AP_Compass_UAVCAN::handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb) +void AP_Compass_DroneCAN::handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg) { WITH_SEMAPHORE(_sem_registry); Vector3f mag_vector; - AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, 0); + AP_Compass_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, 0); if (driver != nullptr) { - mag_vector[0] = cb.msg->magnetic_field_ga[0]; - mag_vector[1] = cb.msg->magnetic_field_ga[1]; - mag_vector[2] = cb.msg->magnetic_field_ga[2]; + mag_vector[0] = msg.magnetic_field_ga[0]; + mag_vector[1] = msg.magnetic_field_ga[1]; + mag_vector[2] = msg.magnetic_field_ga[2]; driver->handle_mag_msg(mag_vector); } } -void AP_Compass_UAVCAN::handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb) +void AP_Compass_DroneCAN::handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg) { WITH_SEMAPHORE(_sem_registry); Vector3f mag_vector; - uint8_t sensor_id = cb.msg->sensor_id; - AP_Compass_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, sensor_id); + uint8_t sensor_id = msg.sensor_id; + AP_Compass_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, sensor_id); if (driver != nullptr) { - mag_vector[0] = cb.msg->magnetic_field_ga[0]; - mag_vector[1] = cb.msg->magnetic_field_ga[1]; - mag_vector[2] = cb.msg->magnetic_field_ga[2]; + mag_vector[0] = msg.magnetic_field_ga[0]; + mag_vector[1] = msg.magnetic_field_ga[1]; + mag_vector[2] = msg.magnetic_field_ga[2]; driver->handle_mag_msg(mag_vector); } } -void AP_Compass_UAVCAN::read(void) +void AP_Compass_DroneCAN::read(void) { drain_accumulated_samples(_instance); } - -#endif // AP_COMPASS_UAVCAN_ENABLED +#endif // AP_COMPASS_DRONECAN_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.h b/libraries/AP_Compass/AP_Compass_DroneCAN.h new file mode 100644 index 0000000000000..61f677454ed68 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_DroneCAN.h @@ -0,0 +1,50 @@ +#pragma once + +#include "AP_Compass.h" + +#if AP_COMPASS_DRONECAN_ENABLED + +#include "AP_Compass_Backend.h" + +#include + +class AP_Compass_DroneCAN : public AP_Compass_Backend { +public: + AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id, uint32_t devid); + + void read(void) override; + + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static AP_Compass_Backend* probe(uint8_t index); + static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; } + static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg); + static void handle_magnetic_field_2(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength2 &msg); + +private: + bool init(); + + // callback for DroneCAN messages + void handle_mag_msg(const Vector3f &mag); + + static AP_Compass_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t sensor_id); + + uint8_t _instance; + + AP_DroneCAN* _ap_dronecan; + uint8_t _node_id; + uint8_t _sensor_id; + uint32_t _devid; + + // Module Detection Registry + static struct DetectedModules { + AP_DroneCAN* ap_dronecan; + uint8_t node_id; + uint8_t sensor_id; + AP_Compass_DroneCAN *driver; + uint32_t devid; + } _detected_modules[COMPASS_MAX_BACKEND]; + + static HAL_Semaphore _sem_registry; +}; + +#endif // AP_COMPASS_DRONECAN_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_UAVCAN.h b/libraries/AP_Compass/AP_Compass_UAVCAN.h deleted file mode 100644 index 8eb57d9884198..0000000000000 --- a/libraries/AP_Compass/AP_Compass_UAVCAN.h +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once - -#include "AP_Compass.h" - -#if AP_COMPASS_UAVCAN_ENABLED - -#include "AP_Compass_Backend.h" - -#include - -class MagCb; -class Mag2Cb; - -class AP_Compass_UAVCAN : public AP_Compass_Backend { -public: - AP_Compass_UAVCAN(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id, uint32_t devid); - - void read(void) override; - - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static AP_Compass_Backend* probe(uint8_t index); - static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; } - static void handle_magnetic_field(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MagCb &cb); - static void handle_magnetic_field_2(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Mag2Cb &cb); - -private: - bool init(); - - // callback for UAVCAN messages - void handle_mag_msg(const Vector3f &mag); - - static AP_Compass_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t sensor_id); - - uint8_t _instance; - - AP_UAVCAN* _ap_uavcan; - uint8_t _node_id; - uint8_t _sensor_id; - uint32_t _devid; - - // Module Detection Registry - static struct DetectedModules { - AP_UAVCAN* ap_uavcan; - uint8_t node_id; - uint8_t sensor_id; - AP_Compass_UAVCAN *driver; - uint32_t devid; - } _detected_modules[COMPASS_MAX_BACKEND]; - - static HAL_Semaphore _sem_registry; -}; - -#endif // AP_COMPASS_UAVCAN_ENABLED diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h index 58d1bf95e46c3..72ae8a411c9c9 100644 --- a/libraries/AP_Compass/AP_Compass_config.h +++ b/libraries/AP_Compass/AP_Compass_config.h @@ -25,8 +25,8 @@ #define AP_COMPASS_SITL_ENABLED (AP_COMPASS_BACKEND_DEFAULT_ENABLED && AP_SIM_ENABLED) #endif -#ifndef AP_COMPASS_UAVCAN_ENABLED -#define AP_COMPASS_UAVCAN_ENABLED (AP_COMPASS_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_LIBUAVCAN_DRIVERS) +#ifndef AP_COMPASS_DRONECAN_ENABLED +#define AP_COMPASS_DRONECAN_ENABLED (AP_COMPASS_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS) #endif // i2c-based compasses: diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 2df813a0f8221..8dacb4825397f 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -2,6 +2,7 @@ #if AP_DDS_ENABLED +#include #include #include #include @@ -9,6 +10,10 @@ #include "AP_DDS_Client.h" #include "generated/Time.h" +static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; +static constexpr uint16_t DELAY_NAV_SAT_FIX_TOPIC_MS = 1000; +static char WGS_84_FRAME_ID[] = "WGS-84"; + AP_HAL::UARTDriver *dds_port; @@ -31,6 +36,90 @@ void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) } +void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) +{ + // Add a lambda that takes in navsatfix msg and populates the cov + // Make it constexpr if possible + // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ + // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; + + // assert(instance >= GPS_MAX_RECEIVERS); + if (instance >= GPS_MAX_RECEIVERS) { + return; + } + + update_topic(msg.header.stamp); + strcpy(msg.header.frame_id, WGS_84_FRAME_ID); + msg.status.service = 0; // SERVICE_GPS + msg.status.status = -1; // STATUS_NO_FIX + + auto &gps = AP::gps(); + WITH_SEMAPHORE(gps.get_semaphore()); + + if (!gps.is_healthy(instance)) { + msg.status.status = -1; // STATUS_NO_FIX + msg.status.service = 0; // No services supported + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return; + } + + + //! @todo What about glonass, compass, galileo? + //! This will be properly designed and implemented to spec in #23277 + msg.status.service = 1; // SERVICE_GPS + + const auto status = gps.status(instance); + switch (status) { + case AP_GPS::NO_GPS: + case AP_GPS::NO_FIX: + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return; + case AP_GPS::GPS_OK_FIX_2D: + case AP_GPS::GPS_OK_FIX_3D: + msg.status.status = 0; // STATUS_FIX + msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED + break; + case AP_GPS::GPS_OK_FIX_3D_DGPS: + msg.status.status = 1; // STATUS_SBAS_FIX + msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED + break; + case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: + case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: + msg.status.status = 2; // STATUS_SBAS_FIX + msg.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED + // RTK provides "rtk_accuracy" member, should it be used for the covariance? + break; + default: + //! @todo Can we not just use an enum class and not worry about this condition? + break; + } + const auto loc = gps.location(instance); + msg.latitude = loc.lat * 1E-7; + msg.longitude = loc.lng * 1E-7; + + int32_t alt_cm; + if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { + // With absolute frame, this condition is unlikely + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return; + } + msg.altitude = alt_cm / 100.0; + + // Calculate covariance: https://answers.ros.org/question/10310/calculate-navsatfix-covariance/ + // https://github.com/ros-drivers/nmea_navsat_driver/blob/indigo-devel/src/libnmea_navsat_driver/driver.py#L110-L114 + //! @todo This calculation will be moved to AP::gps and fixed in #23259 + //! It is a placeholder for now matching the ROS1 nmea_navsat_driver behavior + const auto hdop = gps.get_hdop(instance); + const auto hdopSq = hdop * hdop; + const auto vdop = gps.get_vdop(instance); + const auto vdopSq = vdop * vdop; + msg.position_covariance[0] = hdopSq; + msg.position_covariance[4] = hdopSq; + msg.position_covariance[8] = vdopSq; +} + /* class constructor @@ -107,64 +196,109 @@ bool AP_DDS_Client::create() const char* participant_ref = "participant_profile"; const auto participant_req_id = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id,0,participant_ref,UXR_REPLACE); - // Topic - const uxrObjectId topic_id = { - .id = topics[0].topic_id, - .type = UXR_TOPIC_ID - }; - const char* topic_ref = topics[0].topic_profile_label; - const auto topic_req_id = uxr_buffer_create_topic_ref(&session,reliable_out,topic_id,participant_id,topic_ref,UXR_REPLACE); - - // Publisher - const uxrObjectId pub_id = { - .id = topics[0].pub_id, - .type = UXR_PUBLISHER_ID - }; - const char* pub_xml = ""; - const auto pub_req_id = uxr_buffer_create_publisher_xml(&session,reliable_out,pub_id,participant_id,pub_xml,UXR_REPLACE); - - // Data Writer - const char* data_writer_ref = topics[0].dw_profile_label; - const auto dwriter_req_id = uxr_buffer_create_datawriter_ref(&session,reliable_out,dwriter_id,pub_id,data_writer_ref,UXR_REPLACE); - - //Status requests - constexpr uint8_t nRequests = 4; - const uint16_t requests[nRequests] = {participant_req_id, topic_req_id, pub_req_id, dwriter_req_id}; + //Participant requests + constexpr uint8_t nRequestsParticipant = 1; + const uint16_t requestsParticipant[nRequestsParticipant] = {participant_req_id}; constexpr int maxTimeMsPerRequestMs = 250; - constexpr int requestTimeoutMs = nRequests * maxTimeMsPerRequestMs; - uint8_t status[nRequests]; - if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) { + constexpr int requestTimeoutParticipantMs = nRequestsParticipant * maxTimeMsPerRequestMs; + uint8_t statusParticipant[nRequestsParticipant]; + if (!uxr_run_session_until_all_status(&session, requestTimeoutParticipantMs, requestsParticipant, statusParticipant, nRequestsParticipant)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Participant session request failure"); // TODO add a failure log message sharing the status results return false; } + for (size_t i = 0 ; i < ARRAY_SIZE(topics); i++) { + // Topic + const uxrObjectId topic_id = { + .id = topics[i].topic_id, + .type = UXR_TOPIC_ID + }; + const char* topic_ref = topics[i].topic_profile_label; + const auto topic_req_id = uxr_buffer_create_topic_ref(&session,reliable_out,topic_id,participant_id,topic_ref,UXR_REPLACE); + + // Publisher + const uxrObjectId pub_id = { + .id = topics[i].pub_id, + .type = UXR_PUBLISHER_ID + }; + const char* pub_xml = ""; + const auto pub_req_id = uxr_buffer_create_publisher_xml(&session,reliable_out,pub_id,participant_id,pub_xml,UXR_REPLACE); + + // Data Writer + const char* data_writer_ref = topics[i].dw_profile_label; + const auto dwriter_req_id = uxr_buffer_create_datawriter_ref(&session,reliable_out,topics[i].dw_id,pub_id,data_writer_ref,UXR_REPLACE); + + // Status requests + constexpr uint8_t nRequests = 3; + const uint16_t requests[nRequests] = {topic_req_id, pub_req_id, dwriter_req_id}; + constexpr int requestTimeoutMs = nRequests * maxTimeMsPerRequestMs; + uint8_t status[nRequests]; + if (!uxr_run_session_until_all_status(&session, requestTimeoutMs, requests, status, nRequests)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Topic/Pub/Writer session request failure for index 'TODO'"); + for (int s = 0 ; s < nRequests; s++) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR,"XRCE Client: Status '%d' result '%u'", s, status[s]); + } + // TODO add a failure log message sharing the status results + return false; + } else { + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"XRCE Client: Topic/Pub/Writer session pass for index 'TOOO'"); + } + } + return true; } -void AP_DDS_Client::write() +void AP_DDS_Client::write_time_topic() { WITH_SEMAPHORE(csem); if (connected) { ucdrBuffer ub; - uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0); - uxr_prepare_output_stream(&session,reliable_out,dwriter_id,&ub,topic_size); + const uint32_t topic_size = builtin_interfaces_msg_Time_size_of_topic(&time_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[0].dw_id,&ub,topic_size); const bool success = builtin_interfaces_msg_Time_serialize_topic(&ub, &time_topic); if (!success) { // TODO sometimes serialization fails on bootup. Determine why. - // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + // AP_HAL::panic("FATAL: XRCE_Client failed to serialize\n"); } - } +} +void AP_DDS_Client::write_nav_sat_fix_topic() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub; + const uint32_t topic_size = sensor_msgs_msg_NavSatFix_size_of_topic(&nav_sat_fix_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[1].dw_id,&ub,topic_size); + const bool success = sensor_msgs_msg_NavSatFix_serialize_topic(&ub, &nav_sat_fix_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } } void AP_DDS_Client::update() { WITH_SEMAPHORE(csem); - update_topic(time_topic); - write(); + const auto cur_time_ms = AP_HAL::millis64(); + + if (cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) { + update_topic(time_topic); + last_time_time_ms = cur_time_ms; + write_time_topic(); + } + + if (cur_time_ms - last_nav_sat_fix_time_ms > DELAY_NAV_SAT_FIX_TOPIC_MS) { + constexpr uint8_t instance = 0; + update_topic(nav_sat_fix_topic, instance); + last_nav_sat_fix_time_ms = cur_time_ms; + write_nav_sat_fix_topic(); + } + connected = uxr_run_session_time(&session, 1); } diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 706ccd758e6a8..3037cceb0f889 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -6,6 +6,7 @@ #include "ucdr/microcdr.h" #include "generated/Time.h" #include "AP_DDS_Generic_Fn_T.h" +#include "generated/NavSatFix.h" #include #include @@ -39,12 +40,7 @@ class AP_DDS_Client // Topic builtin_interfaces_msg_Time time_topic; - - // Data Writer - const uxrObjectId dwriter_id = { - .id = 0x01, - .type = UXR_DATAWRITER_ID - }; + sensor_msgs_msg_NavSatFix nav_sat_fix_topic; HAL_Semaphore csem; @@ -52,6 +48,13 @@ class AP_DDS_Client bool connected = true; static void update_topic(builtin_interfaces_msg_Time& msg); + static void update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance); + + // The last ms timestamp AP_DDS wrote a Time message + uint64_t last_time_time_ms; + // The last ms timestamp AP_DDS wrote a NavSatFix message + uint64_t last_nav_sat_fix_time_ms; + public: // Constructor @@ -61,15 +64,17 @@ class AP_DDS_Client //! @brief Initialize the client's transport, uxr session, and IO stream(s) //! @return True on successful initialization, false on failure - bool init() WARN_IF_UNUSED; + bool init() WARN_IF_UNUSED; //! @brief Set up the client's participants, data read/writes, // publishers, subscribers //! @return True on successful creation, false on failure bool create() WARN_IF_UNUSED; - //! @brief Serialize the current data state and publish to to the IO stream(s) - void write(); + //! @brief Serialize the current time state and publish to to the IO stream(s) + void write_time_topic(); + //! @brief Serialize the current nav_sat_fix state and publish to to the IO stream(s) + void write_nav_sat_fix_topic(); //! @brief Update the internally stored DDS messages with latest data void update(); @@ -80,6 +85,7 @@ class AP_DDS_Client struct Topic_table { const uint8_t topic_id; const uint8_t pub_id; + const uxrObjectId dw_id; const char* topic_profile_label; const char* dw_profile_label; Generic_serialize_topic_fn_t serialize; diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 8f8134d086fe8..1b8b12badba49 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -1,6 +1,9 @@ -// #include "Generated/Time.h" // might change to brackets for include path +#include "generated/Time.h" +#include "generated/NavSatFix.h" + #include "AP_DDS_Generic_Fn_T.h" +#include "uxr/client/client.h" // Code generated table based on the enabled topics. // Mavgen is using python, loops are not readable. @@ -11,10 +14,21 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { { .topic_id = 0x01, .pub_id = 0x01, + .dw_id = uxrObjectId{.id=0x01, .type=UXR_DATAWRITER_ID}, .topic_profile_label = "time__t", .dw_profile_label = "time__dw", .serialize = Generic_serialize_topic_fn_t(&builtin_interfaces_msg_Time_serialize_topic), .deserialize = Generic_deserialize_topic_fn_t(&builtin_interfaces_msg_Time_deserialize_topic), .size_of = Generic_size_of_topic_fn_t(&builtin_interfaces_msg_Time_size_of_topic), }, + { + .topic_id = 0x02, + .pub_id = 0x02, + .dw_id = uxrObjectId{.id=0x02, .type=UXR_DATAWRITER_ID}, + .topic_profile_label = "navsatfix0__t", + .dw_profile_label = "navsatfix0__dw", + .serialize = Generic_serialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_serialize_topic), + .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_deserialize_topic), + .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_NavSatFix_size_of_topic), + }, }; diff --git a/libraries/AP_DDS/Idl/NavSatFix.idl b/libraries/AP_DDS/Idl/NavSatFix.idl index 57de832cb9e5f..cfe9024ed18a2 100644 --- a/libraries/AP_DDS/Idl/NavSatFix.idl +++ b/libraries/AP_DDS/Idl/NavSatFix.idl @@ -1,4 +1,4 @@ -// generated from rosidl_adapter/resource/msg.idl.em +/// generated from rosidl_adapter/resource/msg.idl.em // with input from sensor_msgs/msg/NavSatFix.msg // generated code does not contain a copyright notice @@ -11,10 +11,10 @@ module sensor_msgs { module NavSatFix_Constants { @verbatim (language="comment", text= "If the covariance of the fix is known, fill it in completely. If the" "\n" "GPS receiver provides the variance of each measurement, put them" "\n" "along the diagonal. If only Dilution of Precision is available," "\n" "estimate an approximate covariance from that.") - const octet COVARIANCE_TYPE_UNKNOWN = 0; - const octet COVARIANCE_TYPE_APPROXIMATED = 1; - const octet COVARIANCE_TYPE_DIAGONAL_KNOWN = 2; - const octet COVARIANCE_TYPE_KNOWN = 3; + const uint8 COVARIANCE_TYPE_UNKNOWN = 0; + const uint8 COVARIANCE_TYPE_APPROXIMATED = 1; + const uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2; + const uint8 COVARIANCE_TYPE_KNOWN = 3; }; @verbatim (language="comment", text= "Navigation Satellite fix for any Global Navigation Satellite System" "\n" @@ -59,9 +59,9 @@ module sensor_msgs { "" "\n" "Beware: this coordinate system exhibits singularities at the poles.") @unit (value="m^2") - double position_covariance[9]; + double__9 position_covariance; - octet position_covariance_type; + uint8 position_covariance_type; }; }; }; diff --git a/libraries/AP_DDS/Idl/NavSatStatus.idl b/libraries/AP_DDS/Idl/NavSatStatus.idl index f320c9d9d604f..a19fa90235325 100644 --- a/libraries/AP_DDS/Idl/NavSatStatus.idl +++ b/libraries/AP_DDS/Idl/NavSatStatus.idl @@ -8,16 +8,16 @@ module sensor_msgs { module NavSatStatus_Constants { @verbatim (language="comment", text= "unable to fix position") - const char STATUS_NO_FIX = -1; + const int8 STATUS_NO_FIX = -1; @verbatim (language="comment", text= "unaugmented fix") - const char STATUS_FIX = 0; + const int8 STATUS_FIX = 0; @verbatim (language="comment", text= "with satellite-based augmentation") - const char STATUS_SBAS_FIX = 1; + const int8 STATUS_SBAS_FIX = 1; @verbatim (language="comment", text= "with ground-based augmentation") - const char STATUS_GBAS_FIX = 2; + const int8 STATUS_GBAS_FIX = 2; @verbatim (language="comment", text= "Bits defining which Global Navigation Satellite System signals were" "\n" "used by the receiver.") const uint16 SERVICE_GPS = 1; @@ -34,7 +34,7 @@ module sensor_msgs { "type and the last time differential corrections were received. A" "\n" "fix is valid when status >= STATUS_FIX.") struct NavSatStatus { - char status; + int8 status; uint16 service; }; diff --git a/libraries/AP_DDS/Idl/PoseStamped.idl b/libraries/AP_DDS/Idl/PoseStamped.idl index 9af9d0a30bb08..e69efef37ab4a 100644 --- a/libraries/AP_DDS/Idl/PoseStamped.idl +++ b/libraries/AP_DDS/Idl/PoseStamped.idl @@ -2,14 +2,9 @@ // with input from geometry_msgs/msg/PoseStamped.msg // generated code does not contain a copyright notice -#include "Point.idl" -#include "Quaternion.idl" +#include "Pose.idl" #include "Header.idl" -struct Pose { - geometry_msgs::msg::Point position; - geometry_msgs::msg::Quaternion orientation; -}; module geometry_msgs { module msg { @verbatim (language="comment", text= @@ -17,7 +12,7 @@ module geometry_msgs { struct PoseStamped { std_msgs::msg::Header header; - Pose pose; + geometry_msgs::msg::Pose pose; }; }; }; diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 446b2df762b5d..1dbb8bf712e11 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -33,20 +33,25 @@ Currently, serial is the only supported transport, but there are plans to add IP While DDS support in Ardupilot is mostly through git submodules, another tool needs to be available on your system: Micro XRCE DDS Gen. -1. Go to a directory on your system to clone the repo (perhaps next to `ardupilot`) -1. Install java +- Go to a directory on your system to clone the repo (perhaps next to `ardupilot`) +- Install java ```console - sudo apt install java + sudo apt install default-jre ```` -1. Follow instructions [here](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html#installing-the-micro-xrce-dds-gen-tool) to install the generator, but use `develop` branch instead of `master` (for now). +- Follow instructions [here](https://micro-xrce-dds.docs.eprosima.com/en/latest/installation.html#installing-the-micro-xrce-dds-gen-tool) to install the generator, but use `develop` branch instead of `master` (for now). ```console git clone -b develop --recurse-submodules https://github.com/eProsima/Micro-XRCE-DDS-Gen.git cd Micro-XRCE-DDS-Gen ./gradlew assemble ``` -1. Add the generator directory to $PATH, like [so](https://github.com/eProsima/Micro-XRCE-DDS-docs/issues/83). -1. Test it +- Add the generator directory to $PATH. + ```console + # Add this to ~/.bashrc + + export PATH=$PATH:/your/path/to/Micro-XRCE-DDS-Gen/scripts + ``` +- Test it ```console cd /path/to/ardupilot microxrceddsgen -version @@ -63,34 +68,24 @@ For now, avoid having simultaneous local and global installs. If you followed the [global install](https://fast-dds.docs.eprosima.com/en/latest/installation/sources/sources_linux.html#global-installation) section, you should remove it and switch to local install. -## Parameters for DDS +## Setup serial for SITL with DDS -| Name | Description | -| - | - | -| SERIAL1_BAUD | The serial baud rate for DDS | -| SERIAL1_PROTOCOL | Set this to 45 to use DDS on the serial port | - - -## Testing with a UART - -On Linux, first create a virtual serial port for use with SITL like [this](https://stackoverflow.com/questions/52187/virtual-serial-port-for-linux) +On Linux, creating a virtual serial port will be necessary to use serial in SITL, because of that install socat. ``` sudo apt-get update sudo apt-get install socat ``` -Then, start a virtual serial port with socat. Take note of the two `/dev/pts/*` ports. If yours are different, substitute as needed. -``` -socat -d -d pty,raw,echo=0 pty,raw,echo=0 ->>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/1 ->>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/2 ->>> 2023/02/21 05:26:06 socat[334] N starting data transfer loop with FDs [5,5] and [7,7] -``` +## Setup ardupilot for SITL with DDS Set up your [SITL](https://ardupilot.org/dev/docs/setting-up-sitl-on-linux.html). -Run the simulator with the following command (assuming we are using /dev/pts/1 for Ardupilot SITL). Take note how two parameters need adjusting from default to use DDS. -``` +Run the simulator with the following command. Take note how two parameters need adjusting from default to use DDS. +| Name | Description | +| - | - | +| SERIAL1_BAUD | The serial baud rate for DDS | +| SERIAL1_PROTOCOL | Set this to 45 to use DDS on the serial port | +```bash # Wipe params till you see "AP: ArduPilot Ready" # Select your favorite vehicle type sim_vehicle.py -w -v ArduPlane @@ -100,13 +95,7 @@ param set SERIAL1_BAUD 115 # See libraries/AP_SerialManager/AP_SerialManager.h AP_SerialManager SerialProtocol_DDS_XRCE param set SERIAL1_PROTOCOL 45 ``` - -# Start the sim now with the new params -``` -sim_vehicle.py -v ArduPlane -D --console --enable-dds -A "--uartC=uart:/dev/pts/1" -``` - -## Starting with microROS Agent +## Setup ROS 2 and micro-ROS Follow the steps to use the microROS Agent @@ -115,60 +104,63 @@ Follow the steps to use the microROS Agent - https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html - Install and run the microROS agent (as descibed here). Make sure to use the `humble` branch. + - Follow [the instructions](https://micro.ros.org/docs/tutorials/core/first_application_linux/) for the following: -- https://micro.ros.org/docs/tutorials/core/first_application_linux/ + - Do "Installing ROS 2 and the micro-ROS build system" + - Skip the docker run command, build it locally instead + - Skip "Creating a new firmware workspace" + - Skip "Building the firmware" + - Do "Creating the micro-ROS agent" + - Source your ROS workspace -Until this [PR](https://github.com/micro-ROS/micro-ROS.github.io) is merged, ignore the notes about `foxy`. It works on `humble`. +Until this [PR](https://github.com/micro-ROS/micro-ROS.github.io/pull/401) is merged, ignore the notes about `foxy`. It works on `humble`. -Follow the instructions for the following: +## Using the ROS2 CLI to Read Ardupilot Data -* Do "Installing ROS 2 and the micro-ROS build system" - * Skip the docker run command, build it locally instead -* Skip "Creating a new firmware workspace" -* Skip "Building the firmware" -* Do "Creating the micro-ROS agent" -* Source your ROS workspace - -- Run microROS agent with the following command - -```bash -cd ardupilot/libraries/AP_DDS -ros2 run micro_ros_agent micro_ros_agent serial -b 115200 -D /dev/pts/2 -r dds_xrce_profile.xml # (assuming we are using tty/pts/2 for Ardupilot) -``` - -## Tutorial - -### Using the ROS2 CLI to Read Ardupilot Data - -If you have installed the microROS agent and ROS-2 Humble +After your setups are complete, do the following: - Source the ros2 installation - - ```source /opt/ros/humble/setup.bash``` - -- If SITL is running alongise MicroROS Agent, you should be able to see the agent here and view the data output. - - -``` -$ ros2 node list -/Ardupilot_DDS_XRCE_Client - -$ ros2 topic list -v -Published topics: - * /ROS2_Time [builtin_interfaces/msg/Time] 1 publisher - * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher - * /rosout [rcl_interfaces/msg/Log] 1 publisher - -Subscribed topics: - -$ ros2 topic hz /ROS2_Time -average rate: 50.115 - min: 0.012s max: 0.024s std dev: 0.00328s window: 52 - -$ ros2 topic echo /ROS2_Time -sec: 1678668735 -nanosec: 729410000 ---- -``` + ```bash + source /opt/ros/humble/setup.bash + ``` +- Start a virtual serial port with socat. Take note of the two `/dev/pts/*` ports. If yours are different, substitute as needed. + ```bash + socat -d -d pty,raw,echo=0 pty,raw,echo=0 + >>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/1 + >>> 2023/02/21 05:26:06 socat[334] N PTY is /dev/pts/2 + >>> 2023/02/21 05:26:06 socat[334] N starting data transfer loop with FDs [5,5] and [7,7] + ``` +- Run the microROS agent + ```bash + cd ardupilot/libraries/AP_DDS + ros2 run micro_ros_agent micro_ros_agent serial -b 115200 -D /dev/pts/2 -r dds_xrce_profile.xml # (assuming we are using tty/pts/2 for DDS Application) + ``` +- Run SITL (remember to kill any terminals running ardupilot SITL beforehand) + ```bash + sim_vehicle.py -v ArduPlane -D --console --enable-dds -A "--uartC=uart:/dev/pts/1" # (assuming we are using /dev/pts/1 for Ardupilot SITL) + ``` +- You should be able to see the agent here and view the data output. + ```bash + $ ros2 node list + /Ardupilot_DDS_XRCE_Client + + $ ros2 topic list -v + Published topics: + * /ROS2_Time [builtin_interfaces/msg/Time] 1 publisher + * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher + * /rosout [rcl_interfaces/msg/Log] 1 publisher + + Subscribed topics: + + $ ros2 topic hz /ROS2_Time + average rate: 50.115 + min: 0.012s max: 0.024s std dev: 0.00328s window: 52 + + $ ros2 topic echo /ROS2_Time + sec: 1678668735 + nanosec: 729410000 + --- + ``` ## Adding DDS messages to Ardupilot diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index ae091652b3ebb..a8089b76af9df 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -30,4 +30,32 @@ + + rt/ROS2_NavSatFix0 + sensor_msgs::msg::dds_::NavSatFix_ + + KEEP_LAST + 5 + + + + PREALLOCATED_WITH_REALLOC + + + BEST_EFFORT + + + VOLATILE + + + + NO_KEY + rt/ROS2_NavSatFix0 + sensor_msgs::msg::dds_::NavSatFix_ + + KEEP_LAST + 5 + + + diff --git a/libraries/AP_DDS/wscript b/libraries/AP_DDS/wscript index f74060c0e4f72..0efd551a62e03 100644 --- a/libraries/AP_DDS/wscript +++ b/libraries/AP_DDS/wscript @@ -103,7 +103,7 @@ def build(bld): # build IDL file source=idl, target=gen, - rule=f"{bld.env.MICROXRCEDDSGEN[0]} -replace -d {os.path.join(gen_path, 'generated')} {idl}", + rule=f"{bld.env.MICROXRCEDDSGEN[0]} -cs -replace -d {os.path.join(gen_path, 'generated')} {idl}", group='dynamic_sources', ) bld.env.AP_LIB_EXTRA_SOURCES['AP_DDS'] += [os.path.join('generated', gen_c)] diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp new file mode 100644 index 0000000000000..8f302e1c0fb4d --- /dev/null +++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp @@ -0,0 +1,312 @@ +#include "AP_Canard_iface.h" +#include +#include +#if HAL_ENABLE_DRONECAN_DRIVERS +#include +#include +extern const AP_HAL::HAL& hal; +#define LOG_TAG "DroneCANIface" + +#define DEBUG_PKTS 0 + +DEFINE_HANDLER_LIST_HEADS(); +DEFINE_HANDLER_LIST_SEMAPHORES(); + +DEFINE_TRANSFER_OBJECT_HEADS(); +DEFINE_TRANSFER_OBJECT_SEMAPHORES(); + +#if AP_TEST_DRONECAN_DRIVERS +CanardInterface* CanardInterface::canard_ifaces[] = {nullptr, nullptr, nullptr}; +CanardInterface CanardInterface::test_iface{2}; +uint8_t test_node_mem_area[1024]; +HAL_Semaphore test_iface_sem; +#endif + +CanardInterface::CanardInterface(uint8_t iface_index) : +Interface(iface_index) { +#if AP_TEST_DRONECAN_DRIVERS + if (iface_index < 3) { + canard_ifaces[iface_index] = this; + } + if (iface_index == 0) { + test_iface.init(test_node_mem_area, sizeof(test_node_mem_area), 125); + } +#endif +} + +void CanardInterface::init(void* mem_arena, size_t mem_arena_size, uint8_t node_id) { + canardInit(&canard, mem_arena, mem_arena_size, onTransferReception, shouldAcceptTransfer, this); + canardSetLocalNodeID(&canard, node_id); + initialized = true; +} + +bool CanardInterface::broadcast(const Canard::Transfer &bcast_transfer) { + if (!initialized) { + return false; + } + WITH_SEMAPHORE(_sem); +#if AP_TEST_DRONECAN_DRIVERS + if (this == &test_iface) { + test_iface_sem.take_blocking(); + } +#endif + + // do canard broadcast + bool success = canardBroadcast(&canard, + bcast_transfer.data_type_signature, + bcast_transfer.data_type_id, + bcast_transfer.inout_transfer_id, + bcast_transfer.priority, + bcast_transfer.payload, + bcast_transfer.payload_len, + AP_HAL::native_micros64() + (bcast_transfer.timeout_ms * 1000) +#if CANARD_MULTI_IFACE + , ((1< 0; +#if AP_TEST_DRONECAN_DRIVERS + if (this == &test_iface) { + test_iface_sem.give(); + } +#endif + return success; +} + +bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) { + if (!initialized) { + return false; + } + WITH_SEMAPHORE(_sem); + // do canard request + return canardRequestOrRespond(&canard, + destination_node_id, + req_transfer.data_type_signature, + req_transfer.data_type_id, + req_transfer.inout_transfer_id, + req_transfer.priority, + CanardRequest, + req_transfer.payload, + req_transfer.payload_len, + AP_HAL::native_micros64() + (req_transfer.timeout_ms * 1000) +#if CANARD_MULTI_IFACE + , ((1< 0; +} + +bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) { + if (!initialized) { + return false; + } + WITH_SEMAPHORE(_sem); + // do canard respond + return canardRequestOrRespond(&canard, + destination_node_id, + res_transfer.data_type_signature, + res_transfer.data_type_id, + res_transfer.inout_transfer_id, + res_transfer.priority, + CanardResponse, + res_transfer.payload, + res_transfer.payload_len, + AP_HAL::native_micros64() + (res_transfer.timeout_ms * 1000) +#if CANARD_MULTI_IFACE + , ((1< 0; +} + +void CanardInterface::onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer) { + CanardInterface* iface = (CanardInterface*) ins->user_reference; + iface->handle_message(*transfer); +} + +bool CanardInterface::shouldAcceptTransfer(const CanardInstance* ins, + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id) { + CanardInterface* iface = (CanardInterface*) ins->user_reference; + return iface->accept_message(data_type_id, *out_data_type_signature); +} + +#if AP_TEST_DRONECAN_DRIVERS +void CanardInterface::processTestRx() { + if (!test_iface.initialized) { + return; + } + WITH_SEMAPHORE(test_iface_sem); + for (const CanardCANFrame* txf = canardPeekTxQueue(&test_iface.canard); txf != NULL; txf = canardPeekTxQueue(&test_iface.canard)) { + if (canard_ifaces[0]) { + canardHandleRxFrame(&canard_ifaces[0]->canard, txf, AP_HAL::native_micros64()); + } + canardPopTxQueue(&test_iface.canard); + } +} +#endif + +void CanardInterface::processTx() { + WITH_SEMAPHORE(_sem); + + for (uint8_t iface = 0; iface < num_ifaces; iface++) { + if (ifaces[iface] == NULL) { + continue; + } + auto txq = canard.tx_queue; + if (txq == nullptr) { + return; + } + AP_HAL::CANFrame txmsg {}; + // scan through list of pending transfers + while (true) { + auto txf = &txq->frame; + txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len); + memcpy(txmsg.data, txf->data, txf->data_len); + txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF); +#if HAL_CANFD_SUPPORTED + txmsg.canfd = txf->canfd; +#endif + bool write = true; + bool read = false; + ifaces[iface]->select(read, write, &txmsg, 0); + if ((AP_HAL::native_micros64() < txf->deadline_usec) && (txf->iface_mask & (1U<send(txmsg, txf->deadline_usec, 0) > 0) { + txf->iface_mask &= ~(1U<next; + if (txq == nullptr) { + break; + } + } + } + + // purge expired transfers + for (const CanardCANFrame* txf = canardPeekTxQueue(&canard); txf != NULL; txf = canardPeekTxQueue(&canard)) { + if ((AP_HAL::native_micros64() >= txf->deadline_usec) || (txf->iface_mask == 0)) { + canardPopTxQueue(&canard); + } else { + break; + } + } +} + +void CanardInterface::processRx() { + AP_HAL::CANFrame rxmsg; + for (uint8_t i=0; iselect(read_select, write_select, nullptr, 0); + if (!read_select) { // No data pending + break; + } + CanardCANFrame rx_frame {}; + + //palToggleLine(HAL_GPIO_PIN_LED); + uint64_t timestamp; + AP_HAL::CANIface::CanIOFlags flags; + if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) { + break; + } + rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc); + memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len); +#if HAL_CANFD_SUPPORTED + rx_frame.canfd = rxmsg.canfd; +#endif + rx_frame.id = rxmsg.id; +#if CANARD_MULTI_IFACE + rx_frame.iface_id = i; +#endif + { + WITH_SEMAPHORE(_sem); + +#if DEBUG_PKTS + const int16_t res = +#endif + canardHandleRxFrame(&canard, &rx_frame, timestamp); +#if DEBUG_PKTS + // hal.console->printf("DTID: %u\n", extractDataType(rx_frame.id)); + // hal.console->printf("Rx %d, IF%d %lx: ", res, i, rx_frame.id); + if (res < 0 && + res != -CANARD_ERROR_RX_NOT_WANTED && + res != -CANARD_ERROR_RX_WRONG_ADDRESS) { + hal.console->printf("Rx error %d, IF%d %lx: \n", res, i, rx_frame.id); + // for (uint8_t index = 0; index < rx_frame.data_len; index++) { + // hal.console->printf("%02x", rx_frame.data[index]); + // } + // hal.console->printf("\n"); + } +#endif + } + } + } +} + +void CanardInterface::process(uint32_t duration_ms) { +#if AP_TEST_DRONECAN_DRIVERS + const uint64_t deadline = AP_HAL::micros64() + duration_ms*1000; + while (AP_HAL::micros64() < deadline) { + processTestRx(); + hal.scheduler->delay_microseconds(1000); + } +#else + const uint64_t deadline = AP_HAL::native_micros64() + duration_ms*1000; + while (true) { + processRx(); + processTx(); + uint64_t now = AP_HAL::native_micros64(); + if (now < deadline) { + _event_handle.wait(deadline - now); + } else { + break; + } + } +#endif +} + +bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface) +{ + if (num_ifaces > HAL_NUM_CAN_IFACES) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Num Ifaces Exceeded\n"); + return false; + } + if (can_iface == nullptr) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface Null\n"); + return false; + } + if (ifaces[num_ifaces] != nullptr) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Iface already added\n"); + return false; + } + ifaces[num_ifaces] = can_iface; + if (ifaces[num_ifaces] == nullptr) { + 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)) { + AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "DroneCANIfaceMgr: Setting event handle failed\n"); + return false; + } + AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "DroneCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces)); + num_ifaces++; + return true; +} +#endif // #if HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.h b/libraries/AP_DroneCAN/AP_Canard_iface.h new file mode 100644 index 0000000000000..21a9b8eea4881 --- /dev/null +++ b/libraries/AP_DroneCAN/AP_Canard_iface.h @@ -0,0 +1,68 @@ +#pragma once +#include +#if HAL_ENABLE_DRONECAN_DRIVERS +#include + +class AP_DroneCAN; +class CanardInterface : public Canard::Interface { + friend class AP_DroneCAN; +public: + + /// @brief delete copy constructor and assignment operator + CanardInterface(const CanardInterface&) = delete; + CanardInterface& operator=(const CanardInterface&) = delete; + + CanardInterface(uint8_t driver_index); + + void init(void* mem_arena, size_t mem_arena_size, uint8_t node_id); + + /// @brief broadcast message to all listeners on Interface + /// @param bc_transfer + /// @return true if message was added to the queue + bool broadcast(const Canard::Transfer &bcast_transfer) override; + + /// @brief request message from + /// @param destination_node_id + /// @param req_transfer + /// @return true if request was added to the queue + bool request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) override; + + /// @brief respond to a request + /// @param destination_node_id + /// @param res_transfer + /// @return true if response was added to the queue + bool respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) override; + + void processTx(); + void processRx(); + + void process(uint32_t duration); + + static void onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer); + static bool shouldAcceptTransfer(const CanardInstance* ins, + uint64_t* out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id); + + bool add_interface(AP_HAL::CANIface *can_drv); + +#if AP_TEST_DRONECAN_DRIVERS + static CanardInterface& get_test_iface() { return test_iface; } + static void processTestRx(); +#endif + + uint8_t get_node_id() const override { return canard.node_id; } +private: + CanardInstance canard; + AP_HAL::CANIface* ifaces[HAL_NUM_CAN_IFACES]; +#if AP_TEST_DRONECAN_DRIVERS + static CanardInterface* canard_ifaces[3]; + static CanardInterface test_iface; +#endif + uint8_t num_ifaces; + HAL_EventHandle _event_handle; + bool initialized; + HAL_Semaphore _sem; +}; +#endif // HAL_ENABLE_DRONECAN_DRIVERS \ No newline at end of file diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp new file mode 100644 index 0000000000000..74d5e29c1052d --- /dev/null +++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp @@ -0,0 +1,1355 @@ +/* + * 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 . + * + * Author: Eugene Shamaev, Siddharth Bharat Purohit + */ + +#include +#include + +#if HAL_ENABLE_DRONECAN_DRIVERS +#include "AP_DroneCAN.h" +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "AP_DroneCAN_DNA_Server.h" +#include +#include +#include +#include + +#define LED_DELAY_US 50000 + +extern const AP_HAL::HAL& hal; + +// setup default pool size +#ifndef DRONECAN_NODE_POOL_SIZE +#if HAL_CANFD_SUPPORTED +#define DRONECAN_NODE_POOL_SIZE 16384 +#else +#define DRONECAN_NODE_POOL_SIZE 8192 +#endif +#endif + +#if HAL_CANFD_SUPPORTED +#define DRONECAN_STACK_SIZE 8192 +#else +#define DRONECAN_STACK_SIZE 4096 +#endif + +#ifndef AP_DRONECAN_VOLZ_FEEDBACK_ENABLED +#define AP_DRONECAN_VOLZ_FEEDBACK_ENABLED 0 +#endif + +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED +#include +#endif + +#define debug_dronecan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "DroneCAN", fmt, ##args); } while (0) + +// Translation of all messages from DroneCAN structures into AP structures is done +// in AP_DroneCAN and not in corresponding drivers. +// The overhead of including definitions of DSDL is very high and it is best to +// concentrate in one place. + +// table of user settable CAN bus parameters +const AP_Param::GroupInfo AP_DroneCAN::var_info[] = { + // @Param: NODE + // @DisplayName: DroneCAN node that is used for this network + // @Description: DroneCAN node should be set implicitly + // @Range: 1 250 + // @User: Advanced + AP_GROUPINFO("NODE", 1, AP_DroneCAN, _dronecan_node, 10), + + // @Param: SRV_BM + // @DisplayName: Output channels to be transmitted as servo over DroneCAN + // @Description: Bitmask with one set for channel to be transmitted as a servo command over DroneCAN + // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32 + + // @User: Advanced + AP_GROUPINFO("SRV_BM", 2, AP_DroneCAN, _servo_bm, 0), + + // @Param: ESC_BM + // @DisplayName: Output channels to be transmitted as ESC over DroneCAN + // @Description: Bitmask with one set for channel to be transmitted as a ESC command over DroneCAN + // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32 + // @User: Advanced + AP_GROUPINFO("ESC_BM", 3, AP_DroneCAN, _esc_bm, 0), + + // @Param: SRV_RT + // @DisplayName: Servo output rate + // @Description: Maximum transmit rate for servo outputs + // @Range: 1 200 + // @Units: Hz + // @User: Advanced + AP_GROUPINFO("SRV_RT", 4, AP_DroneCAN, _servo_rate_hz, 50), + + // @Param: OPTION + // @DisplayName: DroneCAN options + // @Description: Option flags + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS + // @User: Advanced + AP_GROUPINFO("OPTION", 5, AP_DroneCAN, _options, 0), + + // @Param: NTF_RT + // @DisplayName: Notify State rate + // @Description: Maximum transmit rate for Notify State Message + // @Range: 1 200 + // @Units: Hz + // @User: Advanced + AP_GROUPINFO("NTF_RT", 6, AP_DroneCAN, _notify_state_hz, 20), + + // @Param: ESC_OF + // @DisplayName: ESC Output channels offset + // @Description: Offset for ESC numbering in DroneCAN ESC RawCommand messages. This allows for more efficient packing of ESC command messages. If your ESCs are on servo functions 5 to 8 and you set this parameter to 4 then the ESC RawCommand will be sent with the first 4 slots filled. This can be used for more efficint usage of CAN bandwidth + // @Range: 0 18 + // @User: Advanced + AP_GROUPINFO("ESC_OF", 7, AP_DroneCAN, _esc_offset, 0), + + // @Param: POOL + // @DisplayName: CAN pool size + // @Description: Amount of memory in bytes to allocate for the DroneCAN memory pool. More memory is needed for higher CAN bus loads + // @Range: 1024 16384 + // @User: Advanced + AP_GROUPINFO("POOL", 8, AP_DroneCAN, _pool_size, DRONECAN_NODE_POOL_SIZE), + + AP_GROUPEND +}; + +// this is the timeout in milliseconds for periodic message types. We +// set this to 1 to minimise resend of stale msgs +#define CAN_PERIODIC_TX_TIMEOUT_MS 2 + +AP_DroneCAN::AP_DroneCAN(const int driver_index) : +_driver_index(driver_index), +canard_iface(driver_index), +_dna_server(*this) +{ + AP_Param::setup_object_defaults(this, var_info); + + for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) { + _SRV_conf[i].esc_pending = false; + _SRV_conf[i].servo_pending = false; + } + + debug_dronecan(AP_CANManager::LOG_INFO, "AP_DroneCAN constructed\n\r"); +} + +AP_DroneCAN::~AP_DroneCAN() +{ +} + +AP_DroneCAN *AP_DroneCAN::get_dronecan(uint8_t driver_index) +{ + if (driver_index >= AP::can().get_num_drivers() || + AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_DroneCAN) { + return nullptr; + } + return static_cast(AP::can().get_driver(driver_index)); +} + +bool AP_DroneCAN::add_interface(AP_HAL::CANIface* can_iface) +{ + if (!canard_iface.add_interface(can_iface)) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: can't add DroneCAN interface\n\r"); + return false; + } + return true; +} + +void AP_DroneCAN::init(uint8_t driver_index, bool enable_filters) +{ + if (driver_index != _driver_index) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: init called with wrong driver_index"); + return; + } + if (_initialized) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: init called more than once\n\r"); + return; + } + + node_info_rsp.name.len = snprintf((char*)node_info_rsp.name.data, sizeof(node_info_rsp.name.data), "org.ardupilot:%u", driver_index); + + node_info_rsp.software_version.major = AP_DRONECAN_SW_VERS_MAJOR; + node_info_rsp.software_version.minor = AP_DRONECAN_SW_VERS_MINOR; + node_info_rsp.hardware_version.major = AP_DRONECAN_HW_VERS_MAJOR; + node_info_rsp.hardware_version.minor = AP_DRONECAN_HW_VERS_MINOR; + +#if HAL_CANFD_SUPPORTED + if (option_is_set(Options::CANFD_ENABLED)) { + canard_iface.set_canfd(true); + } +#endif + + uint8_t uid_len = sizeof(uavcan_protocol_HardwareVersion::unique_id); + uint8_t unique_id[sizeof(uavcan_protocol_HardwareVersion::unique_id)]; + + mem_pool = new uint32_t[_pool_size/sizeof(uint32_t)]; + if (mem_pool == nullptr) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: Failed to allocate memory pool\n\r"); + return; + } + canard_iface.init(mem_pool, (_pool_size/sizeof(uint32_t))*sizeof(uint32_t), _dronecan_node); + + if (!hal.util->get_system_id_unformatted(unique_id, uid_len)) { + return; + } + unique_id[uid_len - 1] += _dronecan_node; + memcpy(node_info_rsp.hardware_version.unique_id, unique_id, uid_len); + + //Start Servers + if (!_dna_server.init(unique_id, uid_len, _dronecan_node)) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: Failed to start DNA Server\n\r"); + return; + } + + // Roundup all subscribers from supported drivers + AP_GPS_DroneCAN::subscribe_msgs(this); +#if AP_COMPASS_DRONECAN_ENABLED + AP_Compass_DroneCAN::subscribe_msgs(this); +#endif +#if AP_BARO_DRONECAN_ENABLED + AP_Baro_DroneCAN::subscribe_msgs(this); +#endif + AP_BattMonitor_DroneCAN::subscribe_msgs(this); +#if AP_AIRSPEED_DRONECAN_ENABLED + AP_Airspeed_DroneCAN::subscribe_msgs(this); +#endif +#if AP_OPTICALFLOW_HEREFLOW_ENABLED + AP_OpticalFlow_HereFlow::subscribe_msgs(this); +#endif +#if AP_RANGEFINDER_DRONECAN_ENABLED + AP_RangeFinder_DroneCAN::subscribe_msgs(this); +#endif +#if AP_EFI_DRONECAN_ENABLED + AP_EFI_DroneCAN::subscribe_msgs(this); +#endif + +#if AP_PROXIMITY_DRONECAN_ENABLED + AP_Proximity_DroneCAN::subscribe_msgs(this); +#endif + + act_out_array.set_timeout_ms(2); + act_out_array.set_priority(CANARD_TRANSFER_PRIORITY_HIGH); + + esc_raw.set_timeout_ms(2); + esc_raw.set_priority(CANARD_TRANSFER_PRIORITY_HIGH); + + rgb_led.set_timeout_ms(20); + rgb_led.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + buzzer.set_timeout_ms(20); + buzzer.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + safety_state.set_timeout_ms(20); + safety_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + arming_status.set_timeout_ms(20); + arming_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + +#if AP_DRONECAN_SEND_GPS + gnss_fix2.set_timeout_ms(20); + gnss_fix2.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + gnss_auxiliary.set_timeout_ms(20); + gnss_auxiliary.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + gnss_heading.set_timeout_ms(20); + gnss_heading.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + gnss_status.set_timeout_ms(20); + gnss_status.set_priority(CANARD_TRANSFER_PRIORITY_LOW); +#endif + + rtcm_stream.set_timeout_ms(20); + rtcm_stream.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + notify_state.set_timeout_ms(20); + notify_state.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + param_save_client.set_timeout_ms(20); + param_save_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + param_get_set_client.set_timeout_ms(20); + param_get_set_client.set_priority(CANARD_TRANSFER_PRIORITY_LOW); + + node_status.set_priority(CANARD_TRANSFER_PRIORITY_LOWEST); + node_status.set_timeout_ms(1000); + + node_info_server.set_timeout_ms(20); + + _led_conf.devices_count = 0; + + // setup node status + node_status_msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + node_status_msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + node_status_msg.sub_mode = 0; + + // Spin node for device discovery + for (uint8_t i = 0; i < 5; i++) { + send_node_status(); + canard_iface.process(1000); + } + + snprintf(_thread_name, sizeof(_thread_name), "dronecan_%u", driver_index); + + if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_DroneCAN::loop, void), _thread_name, DRONECAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { + debug_dronecan(AP_CANManager::LOG_ERROR, "DroneCAN: couldn't create thread\n\r"); + return; + } + + _initialized = true; + debug_dronecan(AP_CANManager::LOG_INFO, "DroneCAN: init done\n\r"); +} + +void AP_DroneCAN::loop(void) +{ + while (true) { + if (!_initialized) { + hal.scheduler->delay_microseconds(1000); + continue; + } + + canard_iface.process(1); + + if (_SRV_armed) { + bool sent_servos = false; + + if (_servo_bm > 0) { + // if we have any Servos in bitmask + uint32_t now = AP_HAL::native_micros(); + const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); + if (now - _SRV_last_send_us >= servo_period_us) { + _SRV_last_send_us = now; + SRV_send_actuator(); + sent_servos = true; + for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) { + _SRV_conf[i].servo_pending = false; + } + } + } + + // if we have any ESC's in bitmask + if (_esc_bm > 0 && !sent_servos) { + SRV_send_esc(); + } + + for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) { + _SRV_conf[i].esc_pending = false; + } + } + + led_out_send(); + buzzer_send(); + rtcm_stream_send(); + safety_state_send(); + notify_state_send(); + send_parameter_request(); + send_parameter_save_request(); + send_node_status(); + _dna_server.verify_nodes(); +#if AP_OPENDRONEID_ENABLED + AP::opendroneid().dronecan_send(this); +#endif + +#if AP_DRONECAN_SEND_GPS + if (option_is_set(AP_DroneCAN::Options::SEND_GNSS) && !AP_GPS_DroneCAN::instance_exists(this)) { + // send if enabled and this interface/driver is not used by the AP_GPS driver + gnss_send_fix(); + gnss_send_yaw(); + } +#endif + + logging(); + } +} + + +void AP_DroneCAN::send_node_status(void) +{ + const uint32_t now = AP_HAL::native_millis(); + if (now - _node_status_last_send_ms < 1000) { + return; + } + _node_status_last_send_ms = now; + node_status_msg.uptime_sec = now / 1000; + node_status.broadcast(node_status_msg); +} + +void AP_DroneCAN::handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req) +{ + node_info_rsp.status = node_status_msg; + node_info_rsp.status.uptime_sec = AP_HAL::native_millis() / 1000; + + node_info_server.respond(transfer, node_info_rsp); +} + + +///// SRV output ///// + +void AP_DroneCAN::SRV_send_actuator(void) +{ + uint8_t starting_servo = 0; + bool repeat_send; + + WITH_SEMAPHORE(SRV_sem); + + do { + repeat_send = false; + uavcan_equipment_actuator_ArrayCommand msg; + + uint8_t i; + // DroneCAN can hold maximum of 15 commands in one frame + for (i = 0; starting_servo < DRONECAN_SRV_NUMBER && i < 15; starting_servo++) { + uavcan_equipment_actuator_Command cmd; + + /* + * Servo output uses a range of 1000-2000 PWM for scaling. + * This converts output PWM from [1000:2000] range to [-1:1] range that + * is passed to servo as unitless type via DroneCAN. + * This approach allows for MIN/TRIM/MAX values to be used fully on + * autopilot side and for servo it should have the setup to provide maximum + * physically possible throws at [-1:1] limits. + */ + + if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) { + cmd.actuator_id = starting_servo + 1; + + if (option_is_set(Options::USE_ACTUATOR_PWM)) { + cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_PWM; + cmd.command_value = _SRV_conf[starting_servo].pulse; + } else { + cmd.command_type = UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS; + cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0); + } + + msg.commands.data[i] = cmd; + + i++; + } + } + msg.commands.len = i; + if (i > 0) { + if (act_out_array.broadcast(msg) > 0) { + _srv_send_count++; + } else { + _fail_send_count++; + } + + if (i == 15) { + repeat_send = true; + } + } + } while (repeat_send); +} + +void AP_DroneCAN::SRV_send_esc(void) +{ + static const int cmd_max = ((1<<13)-1); + uavcan_equipment_esc_RawCommand esc_msg; + + uint8_t active_esc_num = 0, max_esc_num = 0; + uint8_t k = 0; + + WITH_SEMAPHORE(SRV_sem); + + // esc offset allows for efficient packing of higher ESC numbers in RawCommand + const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER); + + // find out how many esc we have enabled and if they are active at all + for (uint8_t i = esc_offset; i < DRONECAN_SRV_NUMBER; i++) { + if ((((uint32_t) 1) << i) & _esc_bm) { + max_esc_num = i + 1; + if (_SRV_conf[i].esc_pending) { + active_esc_num++; + } + } + } + + // if at least one is active (update) we need to send to all + if (active_esc_num > 0) { + k = 0; + + for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) { + if ((((uint32_t) 1) << i) & _esc_bm) { + // TODO: ESC negative scaling for reverse thrust and reverse rotation + float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0; + + scaled = constrain_float(scaled, 0, cmd_max); + + esc_msg.cmd.data[k] = static_cast(scaled); + } else { + esc_msg.cmd.data[k] = static_cast(0); + } + + k++; + } + esc_msg.cmd.len = k; + + if (esc_raw.broadcast(esc_msg)) { + _esc_send_count++; + } else { + _fail_send_count++; + } + } +} + +void AP_DroneCAN::SRV_push_servos() +{ + WITH_SEMAPHORE(SRV_sem); + + for (uint8_t i = 0; i < DRONECAN_SRV_NUMBER; i++) { + // Check if this channels has any function assigned + if (SRV_Channels::channel_function(i) >= SRV_Channel::k_none) { + _SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm(); + _SRV_conf[i].esc_pending = true; + _SRV_conf[i].servo_pending = true; + } + } + + _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; +} + + +///// LED ///// + +void AP_DroneCAN::led_out_send() +{ + uint64_t now = AP_HAL::native_micros64(); + + if ((now - _led_conf.last_update) < LED_DELAY_US) { + return; + } + + uavcan_equipment_indication_LightsCommand msg; + { + WITH_SEMAPHORE(_led_out_sem); + + if (_led_conf.devices_count == 0) { + return; + } + + msg.commands.len = _led_conf.devices_count; + for (uint8_t i = 0; i < _led_conf.devices_count; i++) { + msg.commands.data[i].light_id =_led_conf.devices[i].led_index; + msg.commands.data[i].color.red = _led_conf.devices[i].red >> 3; + msg.commands.data[i].color.green = _led_conf.devices[i].green >> 2; + msg.commands.data[i].color.blue = _led_conf.devices[i].blue >> 3; + } + } + + rgb_led.broadcast(msg); + _led_conf.last_update = now; +} + +bool AP_DroneCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue) +{ + if (_led_conf.devices_count >= AP_DRONECAN_MAX_LED_DEVICES) { + return false; + } + + WITH_SEMAPHORE(_led_out_sem); + + // check if a device instance exists. if so, break so the instance index is remembered + uint8_t instance = 0; + for (; instance < _led_conf.devices_count; instance++) { + if (_led_conf.devices[instance].led_index == led_index) { + break; + } + } + + // load into the correct instance. + // if an existing instance was found in above for loop search, + // then instance value is < _led_conf.devices_count. + // otherwise a new one was just found so we increment the count. + // Either way, the correct instance is the current value of instance + _led_conf.devices[instance].led_index = led_index; + _led_conf.devices[instance].red = red; + _led_conf.devices[instance].green = green; + _led_conf.devices[instance].blue = blue; + + if (instance == _led_conf.devices_count) { + _led_conf.devices_count++; + } + + return true; +} + +// buzzer send +void AP_DroneCAN::buzzer_send() +{ + uavcan_equipment_indication_BeepCommand msg; + WITH_SEMAPHORE(_buzzer.sem); + uint8_t mask = (1U << _driver_index); + if ((_buzzer.pending_mask & mask) == 0) { + return; + } + _buzzer.pending_mask &= ~mask; + msg.frequency = _buzzer.frequency; + msg.duration = _buzzer.duration; + buzzer.broadcast(msg); +} + +// buzzer support +void AP_DroneCAN::set_buzzer_tone(float frequency, float duration_s) +{ + WITH_SEMAPHORE(_buzzer.sem); + _buzzer.frequency = frequency; + _buzzer.duration = duration_s; + _buzzer.pending_mask = 0xFF; +} + +// notify state send +void AP_DroneCAN::notify_state_send() +{ + uint32_t now = AP_HAL::native_millis(); + + if (_notify_state_hz == 0 || (now - _last_notify_state_ms) < uint32_t(1000 / _notify_state_hz)) { + return; + } + + ardupilot_indication_NotifyState msg; + msg.vehicle_state = 0; + if (AP_Notify::flags.initialising) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_INITIALISING; + } + if (AP_Notify::flags.armed) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ARMED; + } + if (AP_Notify::flags.flying) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FLYING; + } + if (AP_Notify::flags.compass_cal_running) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_MAGCAL_RUN; + } + if (AP_Notify::flags.ekf_bad) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_EKF_BAD; + } + if (AP_Notify::flags.esc_calibration) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_ESC_CALIBRATION; + } + if (AP_Notify::flags.failsafe_battery) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_BATT; + } + if (AP_Notify::flags.failsafe_gcs) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_GCS; + } + if (AP_Notify::flags.failsafe_radio) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FAILSAFE_RADIO; + } + if (AP_Notify::flags.firmware_update) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_FW_UPDATE; + } + if (AP_Notify::flags.gps_fusion) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_FUSION; + } + if (AP_Notify::flags.gps_glitching) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_GPS_GLITCH; + } + if (AP_Notify::flags.have_pos_abs) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POS_ABS_AVAIL; + } + if (AP_Notify::flags.leak_detected) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LEAK_DET; + } + if (AP_Notify::flags.parachute_release) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_CHUTE_RELEASED; + } + if (AP_Notify::flags.powering_off) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_POWERING_OFF; + } + if (AP_Notify::flags.pre_arm_check) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM; + } + if (AP_Notify::flags.pre_arm_gps_check) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_PREARM_GPS; + } + if (AP_Notify::flags.save_trim) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_SAVE_TRIM; + } + if (AP_Notify::flags.vehicle_lost) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_LOST; + } + if (AP_Notify::flags.video_recording) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_VIDEO_RECORDING; + } + if (AP_Notify::flags.waiting_for_throw) { + msg.vehicle_state |= 1 << ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_STATE_THROW_READY; + } + + msg.aux_data_type = ARDUPILOT_INDICATION_NOTIFYSTATE_VEHICLE_YAW_EARTH_CENTIDEGREES; + uint16_t yaw_cd = (uint16_t)(360.0f - degrees(AP::ahrs().get_yaw()))*100.0f; + const uint8_t *data = (uint8_t *)&yaw_cd; + for (uint8_t i=0; i<2; i++) { + msg.aux_data.data[i] = data[i]; + } + msg.aux_data.len = 2; + notify_state.broadcast(msg); + _last_notify_state_ms = AP_HAL::native_millis(); +} + +#if AP_DRONECAN_SEND_GPS +void AP_DroneCAN::gnss_send_fix() +{ + const AP_GPS &gps = AP::gps(); + + const uint32_t gps_lib_time_ms = gps.last_message_time_ms(); + if (_gnss.last_gps_lib_fix_ms == gps_lib_time_ms) { + return; + } + _gnss.last_gps_lib_fix_ms = gps_lib_time_ms; + + /* + send Fix2 packet + */ + + uavcan_equipment_gnss_Fix2 pkt {}; + const Location &loc = gps.location(); + const Vector3f &vel = gps.velocity(); + + pkt.timestamp.usec = AP_HAL::native_micros64(); + pkt.gnss_timestamp.usec = gps.time_epoch_usec(); + if (pkt.gnss_timestamp.usec == 0) { + pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_NONE; + } else { + pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC; + } + pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; + pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; + pkt.height_ellipsoid_mm = loc.alt * 10; + pkt.height_msl_mm = loc.alt * 10; + for (uint8_t i=0; i<3; i++) { + pkt.ned_velocity[i] = vel[i]; + } + pkt.sats_used = gps.num_sats(); + switch (gps.status()) { + case AP_GPS::GPS_Status::NO_GPS: + case AP_GPS::GPS_Status::NO_FIX: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_2D: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT; + break; + case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: + pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX; + pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK; + pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED; + break; + } + + pkt.covariance.len = 6; + float hacc; + if (gps.horizontal_accuracy(hacc)) { + pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc); + } + float vacc; + if (gps.vertical_accuracy(vacc)) { + pkt.covariance.data[2] = sq(vacc); + } + float sacc; + if (gps.speed_accuracy(sacc)) { + const float vc3 = sq(sacc); + pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3; + } + + gnss_fix2.broadcast(pkt); + + + + const uint32_t now_ms = AP_HAL::native_millis(); + if (now_ms - _gnss.last_send_status_ms >= 1000) { + _gnss.last_send_status_ms = now_ms; + + /* + send aux packet + */ + uavcan_equipment_gnss_Auxiliary pkt_auxiliary {}; + pkt_auxiliary.hdop = gps.get_hdop() * 0.01; + pkt_auxiliary.vdop = gps.get_vdop() * 0.01; + + gnss_auxiliary.broadcast(pkt_auxiliary); + + + /* + send Status packet + */ + ardupilot_gnss_Status pkt_status {}; + pkt_status.healthy = gps.is_healthy(); + if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) { + pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING; + } + uint8_t idx; // unused + if (pkt_status.healthy && !gps.first_unconfigured_gps(idx)) { + pkt_status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE; + } + + uint32_t error_codes; + if (gps.get_error_codes(error_codes)) { + pkt_status.error_codes = error_codes; + } + + gnss_status.broadcast(pkt_status); + } +} + +void AP_DroneCAN::gnss_send_yaw() +{ + const AP_GPS &gps = AP::gps(); + + if (!gps.have_gps_yaw()) { + return; + } + + float yaw_deg, yaw_acc_deg; + uint32_t yaw_time_ms; + if (!gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != _gnss.last_lib_yaw_time_ms) { + return; + } + + _gnss.last_lib_yaw_time_ms = yaw_time_ms; + + ardupilot_gnss_Heading pkt_heading {}; + pkt_heading.heading_valid = true; + pkt_heading.heading_accuracy_valid = is_positive(yaw_acc_deg); + pkt_heading.heading_rad = radians(yaw_deg); + pkt_heading.heading_accuracy_rad = radians(yaw_acc_deg); + + gnss_heading.broadcast(pkt_heading); +} +#endif // AP_DRONECAN_SEND_GPS + + +void AP_DroneCAN::rtcm_stream_send() +{ + WITH_SEMAPHORE(_rtcm_stream.sem); + if (_rtcm_stream.buf == nullptr || + _rtcm_stream.buf->available() == 0) { + // nothing to send + return; + } + uint32_t now = AP_HAL::native_millis(); + if (now - _rtcm_stream.last_send_ms < 20) { + // don't send more than 50 per second + return; + } + _rtcm_stream.last_send_ms = now; + uavcan_equipment_gnss_RTCMStream msg; + uint32_t len = _rtcm_stream.buf->available(); + if (len > 128) { + len = 128; + } + msg.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3; + for (uint8_t i=0; iread_byte(&b)) { + return; + } + msg.data.data[i] = b; + } + msg.data.len = len; + rtcm_stream.broadcast(msg); +} + +// SafetyState send +void AP_DroneCAN::safety_state_send() +{ + uint32_t now = AP_HAL::native_millis(); + if (now - _last_safety_state_ms < 500) { + // update at 2Hz + return; + } + _last_safety_state_ms = now; + + { // handle SafetyState + ardupilot_indication_SafetyState safety_msg; + switch (hal.util->safety_switch_state()) { + case AP_HAL::Util::SAFETY_ARMED: + safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_OFF; + safety_state.broadcast(safety_msg); + break; + case AP_HAL::Util::SAFETY_DISARMED: + safety_msg.status = ARDUPILOT_INDICATION_SAFETYSTATE_STATUS_SAFETY_ON; + safety_state.broadcast(safety_msg); + break; + default: + // nothing to send + break; + } + } + + { // handle ArmingStatus + uavcan_equipment_safety_ArmingStatus arming_msg; + arming_msg.status = hal.util->get_soft_armed() ? UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_FULLY_ARMED : + UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_STATUS_DISARMED; + arming_status.broadcast(arming_msg); + } +} + +/* + send RTCMStream packet on all active DroneCAN drivers +*/ +void AP_DroneCAN::send_RTCMStream(const uint8_t *data, uint32_t len) +{ + WITH_SEMAPHORE(_rtcm_stream.sem); + if (_rtcm_stream.buf == nullptr) { + // give enough space for a full round from a NTRIP server with all + // constellations + _rtcm_stream.buf = new ByteBuffer(2400); + } + if (_rtcm_stream.buf == nullptr) { + return; + } + _rtcm_stream.buf->write(data, len); +} + +/* + handle Button message + */ +void AP_DroneCAN::handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg) +{ + switch (msg.button) { + case ARDUPILOT_INDICATION_BUTTON_BUTTON_SAFETY: { + AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton(); + if (brdconfig && brdconfig->safety_button_handle_pressed(msg.press_time)) { + AP_HAL::Util::safety_state state = hal.util->safety_switch_state(); + if (state == AP_HAL::Util::SAFETY_ARMED) { + hal.rcout->force_safety_on(); + } else { + hal.rcout->force_safety_off(); + } + } + break; + } + } +} + +/* + handle traffic report + */ +void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg) +{ +#if HAL_ADSB_ENABLED + AP_ADSB *adsb = AP::ADSB(); + if (!adsb || !adsb->enabled()) { + // ADSB not enabled + return; + } + + AP_ADSB::adsb_vehicle_t vehicle; + mavlink_adsb_vehicle_t &pkt = vehicle.info; + + pkt.ICAO_address = msg.icao_address; + pkt.tslc = msg.tslc; + pkt.lat = msg.latitude_deg_1e7; + pkt.lon = msg.longitude_deg_1e7; + pkt.altitude = msg.alt_m * 1000; + pkt.heading = degrees(msg.heading) * 100; + pkt.hor_velocity = norm(msg.velocity[0], msg.velocity[1]) * 100; + pkt.ver_velocity = -msg.velocity[2] * 100; + pkt.squawk = msg.squawk; + for (uint8_t i=0; i<9; i++) { + pkt.callsign[i] = msg.callsign[i]; + } + pkt.emitter_type = msg.traffic_type; + + if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_PRESSURE_AMSL) { + pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE; + pkt.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; + } else if (msg.alt_type == ARDUPILOT_EQUIPMENT_TRAFFICMONITOR_TRAFFICREPORT_ALT_TYPE_WGS84) { + pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE; + pkt.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; + } + + if (msg.lat_lon_valid) { + pkt.flags |= ADSB_FLAGS_VALID_COORDS; + } + if (msg.heading_valid) { + pkt.flags |= ADSB_FLAGS_VALID_HEADING; + } + if (msg.velocity_valid) { + pkt.flags |= ADSB_FLAGS_VALID_VELOCITY; + } + if (msg.callsign_valid) { + pkt.flags |= ADSB_FLAGS_VALID_CALLSIGN; + } + if (msg.ident_valid) { + pkt.flags |= ADSB_FLAGS_VALID_SQUAWK; + } + if (msg.simulated_report) { + pkt.flags |= ADSB_FLAGS_SIMULATED; + } + if (msg.vertical_velocity_valid) { + pkt.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID; + } + if (msg.baro_valid) { + pkt.flags |= ADSB_FLAGS_BARO_VALID; + } + + vehicle.last_update_ms = AP_HAL::native_millis() - (vehicle.info.tslc * 1000); + adsb->handle_adsb_vehicle(vehicle); +#endif +} + +/* + handle actuator status message + */ +void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg) +{ + // log as CSRV message + AP::logger().Write_ServoStatus(AP_HAL::native_micros64(), + msg.actuator_id, + msg.position, + msg.force, + msg.speed, + msg.power_rating_pct); +} + +#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED +void AP_DroneCAN::handle_actuator_status_Volz(AP_DroneCAN* ap_dronecan, uint8_t node_id, const ActuatorStatusVolzCb &cb) +{ + AP::logger().WriteStreaming( + "CVOL", + "TimeUS,Id,Pos,Cur,V,Pow,T", + "s#dAv%O", + "F-00000", + "QBfffBh", + AP_HAL::native_micros64(), + cb.msg->actuator_id, + ToDeg(cb.msg->actual_position), + cb.msg->current * 0.025f, + cb.msg->voltage * 0.2f, + cb.msg->motor_pwm * (100.0/255.0), + int16_t(cb.msg->motor_temperature) - 50); +} +#endif + +/* + handle ESC status message + */ +void AP_DroneCAN::handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg) +{ +#if HAL_WITH_ESC_TELEM + const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, DRONECAN_SRV_NUMBER); + const uint8_t esc_index = msg.esc_index + esc_offset; + + if (!is_esc_data_index_valid(esc_index)) { + return; + } + + TelemetryData t { + .temperature_cdeg = int16_t((KELVIN_TO_C(msg.temperature)) * 100), + .voltage = msg.voltage, + .current = msg.current, + }; + + update_rpm(esc_index, msg.rpm, msg.error_count); + update_telem_data(esc_index, t, + (isnan(msg.current) ? 0 : AP_ESC_Telem_Backend::TelemetryType::CURRENT) + | (isnan(msg.voltage) ? 0 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) + | (isnan(msg.temperature) ? 0 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE)); +#endif +} + +bool AP_DroneCAN::is_esc_data_index_valid(const uint8_t index) { + if (index > DRONECAN_SRV_NUMBER) { + // printf("DroneCAN: invalid esc index: %d. max index allowed: %d\n\r", index, DRONECAN_SRV_NUMBER); + return false; + } + return true; +} + +/* + handle LogMessage debug + */ +void AP_DroneCAN::handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg) +{ +#if HAL_LOGGING_ENABLED + if (AP::can().get_log_level() != AP_CANManager::LOG_NONE) { + // log to onboard log and mavlink + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CAN[%u] %s", transfer.source_node_id, msg.text.data); + } else { + // only log to onboard log + AP::logger().Write_MessageF("CAN[%u] %s", transfer.source_node_id, msg.text.data); + } +#endif +} + +void AP_DroneCAN::send_parameter_request() +{ + WITH_SEMAPHORE(_param_sem); + if (param_request_sent) { + return; + } + param_get_set_client.request(param_request_node_id, param_getset_req); + param_request_sent = true; +} + +bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb) +{ + WITH_SEMAPHORE(_param_sem); + if (param_int_cb != nullptr || + param_float_cb != nullptr) { + //busy + return false; + } + param_getset_req.index = 0; + param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1); + param_getset_req.value.real_value = value; + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + param_float_cb = cb; + param_request_sent = false; + param_request_node_id = node_id; + return true; +} + +bool AP_DroneCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb) +{ + WITH_SEMAPHORE(_param_sem); + if (param_int_cb != nullptr || + param_float_cb != nullptr) { + //busy + return false; + } + param_getset_req.index = 0; + param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)-1); + param_getset_req.value.integer_value = value; + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + param_int_cb = cb; + param_request_sent = false; + param_request_node_id = node_id; + return true; +} + +bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb) +{ + WITH_SEMAPHORE(_param_sem); + if (param_int_cb != nullptr || + param_float_cb != nullptr) { + //busy + return false; + } + param_getset_req.index = 0; + param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)); + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY; + param_float_cb = cb; + param_request_sent = false; + param_request_node_id = node_id; + return true; +} + +bool AP_DroneCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb) +{ + WITH_SEMAPHORE(_param_sem); + if (param_int_cb != nullptr || + param_float_cb != nullptr) { + //busy + return false; + } + param_getset_req.index = 0; + param_getset_req.name.len = strncpy_noterm((char*)param_getset_req.name.data, name, sizeof(param_getset_req.name.data)); + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY; + param_int_cb = cb; + param_request_sent = false; + param_request_node_id = node_id; + return true; +} + +void AP_DroneCAN::handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp) +{ + WITH_SEMAPHORE(_param_sem); + if (!param_int_cb && + !param_float_cb) { + return; + } + if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) && param_int_cb) { + int32_t val = rsp.value.integer_value; + if ((*param_int_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) { + // we want the parameter to be set with val + param_getset_req.index = 0; + memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len); + param_getset_req.value.integer_value = val; + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE; + param_request_sent = false; + param_request_node_id = transfer.source_node_id; + return; + } + } else if ((rsp.value.union_tag == UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE) && param_float_cb) { + float val = rsp.value.real_value; + if ((*param_float_cb)(this, transfer.source_node_id, (const char*)rsp.name.data, val)) { + // we want the parameter to be set with val + param_getset_req.index = 0; + memcpy(param_getset_req.name.data, rsp.name.data, rsp.name.len); + param_getset_req.value.real_value = val; + param_getset_req.value.union_tag = UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE; + param_request_sent = false; + param_request_node_id = transfer.source_node_id; + return; + } + } + param_int_cb = nullptr; + param_float_cb = nullptr; +} + + +void AP_DroneCAN::send_parameter_save_request() +{ + WITH_SEMAPHORE(_param_save_sem); + if (param_save_request_sent) { + return; + } + param_save_client.request(param_save_request_node_id, param_save_req); + param_save_request_sent = true; +} + +bool AP_DroneCAN::save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb) +{ + WITH_SEMAPHORE(_param_save_sem); + if (save_param_cb != nullptr) { + //busy + return false; + } + + param_save_req.opcode = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE; + param_save_request_sent = false; + param_save_request_node_id = node_id; + save_param_cb = cb; + return true; +} + +// handle parameter save request response +void AP_DroneCAN::handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp) +{ + WITH_SEMAPHORE(_param_save_sem); + if (!save_param_cb) { + return; + } + (*save_param_cb)(this, transfer.source_node_id, rsp.ok); + save_param_cb = nullptr; +} + +// Send Reboot command +// Note: Do not call this from outside DroneCAN thread context, +// THIS IS NOT A THREAD SAFE API! +void AP_DroneCAN::send_reboot_request(uint8_t node_id) +{ + uavcan_protocol_RestartNodeRequest request; + request.magic_number = UAVCAN_PROTOCOL_RESTARTNODE_REQUEST_MAGIC_NUMBER; + restart_node_client.request(node_id, request); +} + +// check if a option is set and if it is then reset it to 0. +// return true if it was set +bool AP_DroneCAN::check_and_reset_option(Options option) +{ + bool ret = option_is_set(option); + if (ret) { + _options.set_and_save(int16_t(_options.get() & ~uint16_t(option))); + } + return ret; +} + +// handle prearm check +bool AP_DroneCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const +{ + // forward this to DNA_Server + return _dna_server.prearm_check(fail_msg, fail_msg_len); +} + +/* + periodic logging + */ +void AP_DroneCAN::logging(void) +{ +#if HAL_LOGGING_ENABLED + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - last_log_ms < 1000) { + return; + } + last_log_ms = now_ms; + if (HAL_NUM_CAN_IFACES <= _driver_index) { + // no interface? + return; + } + const auto *iface = hal.can[_driver_index]; + if (iface == nullptr) { + return; + } + const auto *stats = iface->get_statistics(); + if (stats == nullptr) { + // statistics not implemented on this interface + return; + } + const auto &s = *stats; + AP::logger().WriteStreaming("CANS", + "TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx", + "s#-------------", + "F--------------", + "QBIIIIIIIIIIIII", + AP_HAL::micros64(), + _driver_index, + s.tx_success, + s.tx_requests, + s.tx_rejected, + s.tx_overflow, + s.tx_timedout, + s.tx_abort, + s.rx_received, + s.rx_overflow, + s.rx_errors, + s.num_busoff_err, + _esc_send_count, + _srv_send_count, + _fail_send_count); +#endif // HAL_LOGGING_ENABLED +} + +#endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h new file mode 100644 index 0000000000000..a2df3a8263fbd --- /dev/null +++ b/libraries/AP_DroneCAN/AP_DroneCAN.h @@ -0,0 +1,332 @@ +/* + * 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 . + * + * Author: Eugene Shamaev, Siddharth Bharat Purohit + */ +#pragma once + +#include + +#if HAL_ENABLE_DRONECAN_DRIVERS + +#include "AP_Canard_iface.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "AP_DroneCAN_DNA_Server.h" +#include +#include + +#ifndef DRONECAN_SRV_NUMBER +#define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS +#endif + +#ifndef AP_DRONECAN_SEND_GPS +#define AP_DRONECAN_SEND_GPS (BOARD_FLASH_SIZE > 1024) +#endif + +#define AP_DRONECAN_SW_VERS_MAJOR 1 +#define AP_DRONECAN_SW_VERS_MINOR 0 + +#define AP_DRONECAN_HW_VERS_MAJOR 1 +#define AP_DRONECAN_HW_VERS_MINOR 0 + +#define AP_DRONECAN_MAX_LED_DEVICES 4 + +// fwd-declare callback classes +class AP_DroneCAN_DNA_Server; + +class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { + friend class AP_DroneCAN_DNA_Server; +public: + AP_DroneCAN(const int driver_index); + ~AP_DroneCAN(); + + static const struct AP_Param::GroupInfo var_info[]; + + // Return uavcan from @driver_index or nullptr if it's not ready or doesn't exist + static AP_DroneCAN *get_dronecan(uint8_t driver_index); + bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const; + + void init(uint8_t driver_index, bool enable_filters) override; + bool add_interface(AP_HAL::CANIface* can_iface) override; + + uint8_t get_driver_index() const { return _driver_index; } + + FUNCTOR_TYPEDEF(ParamGetSetIntCb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &); + FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_DroneCAN*, const uint8_t, const char*, float &); + FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_DroneCAN*, const uint8_t, bool); + + void send_node_status(); + + ///// SRV output ///// + void SRV_push_servos(void); + + ///// LED ///// + bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue); + + // buzzer + void set_buzzer_tone(float frequency, float duration_s); + + // send RTCMStream packets + void send_RTCMStream(const uint8_t *data, uint32_t len); + + // Send Reboot command + // Note: Do not call this from outside UAVCAN thread context, + // you can call this from dronecan callbacks and handlers. + // THIS IS NOT A THREAD SAFE API! + void send_reboot_request(uint8_t node_id); + + // set param value + bool set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb); + bool set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb); + bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb); + bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb); + + // Save parameters + bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb); + + // options bitmask + enum class Options : uint16_t { + DNA_CLEAR_DATABASE = (1U<<0), + DNA_IGNORE_DUPLICATE_NODE = (1U<<1), + CANFD_ENABLED = (1U<<2), + DNA_IGNORE_UNHEALTHY_NODE = (1U<<3), + USE_ACTUATOR_PWM = (1U<<4), + SEND_GNSS = (1U<<5), + }; + + // check if a option is set + bool option_is_set(Options option) const { + return (uint16_t(_options.get()) & uint16_t(option)) != 0; + } + + // check if a option is set and if it is then reset it to + // 0. return true if it was set + bool check_and_reset_option(Options option); + + CanardInterface& get_canard_iface() { return canard_iface; } + +private: + void loop(void); + + ///// SRV output ///// + void SRV_send_actuator(); + void SRV_send_esc(); + + ///// LED ///// + void led_out_send(); + + // buzzer + void buzzer_send(); + + // SafetyState + void safety_state_send(); + + // send notify vehicle state + void notify_state_send(); + + // send GNSS injection + void rtcm_stream_send(); + + // send parameter get/set request + void send_parameter_request(); + + // send parameter save request + void send_parameter_save_request(); + + // periodic logging + void logging(); + + // set parameter on a node + ParamGetSetIntCb *param_int_cb; + ParamGetSetFloatCb *param_float_cb; + bool param_request_sent = true; + HAL_Semaphore _param_sem; + uint8_t param_request_node_id; + + // save parameters on a node + ParamSaveCb *save_param_cb; + bool param_save_request_sent = true; + HAL_Semaphore _param_save_sem; + uint8_t param_save_request_node_id; + + // UAVCAN parameters + AP_Int8 _dronecan_node; + AP_Int32 _servo_bm; + AP_Int32 _esc_bm; + AP_Int8 _esc_offset; + AP_Int16 _servo_rate_hz; + AP_Int16 _options; + AP_Int16 _notify_state_hz; + AP_Int16 _pool_size; + + uint32_t *mem_pool; + + AP_DroneCAN_DNA_Server _dna_server; + + uint8_t _driver_index; + + char _thread_name[13]; + bool _initialized; + ///// SRV output ///// + struct { + uint16_t pulse; + bool esc_pending; + bool servo_pending; + } _SRV_conf[DRONECAN_SRV_NUMBER]; + + uint32_t _esc_send_count; + uint32_t _srv_send_count; + uint32_t _fail_send_count; + + uint8_t _SRV_armed; + uint32_t _SRV_last_send_us; + HAL_Semaphore SRV_sem; + + // last log time + uint32_t last_log_ms; + + ///// LED ///// + struct led_device { + uint8_t led_index; + uint8_t red; + uint8_t green; + uint8_t blue; + }; + + struct { + led_device devices[AP_DRONECAN_MAX_LED_DEVICES]; + uint8_t devices_count; + uint64_t last_update; + } _led_conf; + + HAL_Semaphore _led_out_sem; + + // buzzer + struct { + HAL_Semaphore sem; + float frequency; + float duration; + uint8_t pending_mask; // mask of interfaces to send to + } _buzzer; + +#if AP_DRONECAN_SEND_GPS + // send GNSS Fix and yaw, same thing AP_GPS_DroneCAN would receive + void gnss_send_fix(); + void gnss_send_yaw(); + + // GNSS Fix and Status + struct { + uint32_t last_gps_lib_fix_ms; + uint32_t last_send_status_ms; + uint32_t last_lib_yaw_time_ms; + } _gnss; +#endif + + // GNSS RTCM injection + struct { + HAL_Semaphore sem; + uint32_t last_send_ms; + ByteBuffer *buf; + } _rtcm_stream; + + // ESC + + static HAL_Semaphore _telem_sem; + + // node status send + uint32_t _node_status_last_send_ms; + + // safety status send state + uint32_t _last_safety_state_ms; + + // notify vehicle state + uint32_t _last_notify_state_ms; + uavcan_protocol_NodeStatus node_status_msg; + + CanardInterface canard_iface; + Canard::Publisher node_status{canard_iface}; + Canard::Publisher act_out_array{canard_iface}; + Canard::Publisher esc_raw{canard_iface}; + Canard::Publisher rgb_led{canard_iface}; + Canard::Publisher buzzer{canard_iface}; + Canard::Publisher safety_state{canard_iface}; + Canard::Publisher arming_status{canard_iface}; + Canard::Publisher rtcm_stream{canard_iface}; + Canard::Publisher notify_state{canard_iface}; + +#if AP_DRONECAN_SEND_GPS + Canard::Publisher gnss_fix2{canard_iface}; + Canard::Publisher gnss_auxiliary{canard_iface}; + Canard::Publisher gnss_heading{canard_iface}; + Canard::Publisher gnss_status{canard_iface}; +#endif + // incoming messages + Canard::ObjCallback safety_button_cb{this, &AP_DroneCAN::handle_button}; + Canard::Subscriber safety_button_listener{safety_button_cb, _driver_index}; + + Canard::ObjCallback traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report}; + Canard::Subscriber traffic_report_listener{traffic_report_cb, _driver_index}; + + Canard::ObjCallback actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status}; + Canard::Subscriber actuator_status_listener{actuator_status_cb, _driver_index}; + + Canard::ObjCallback esc_status_cb{this, &AP_DroneCAN::handle_ESC_status}; + Canard::Subscriber esc_status_listener{esc_status_cb, _driver_index}; + + Canard::ObjCallback debug_cb{this, &AP_DroneCAN::handle_debug}; + Canard::Subscriber debug_listener{debug_cb, _driver_index}; + + // param client + Canard::ObjCallback param_get_set_res_cb{this, &AP_DroneCAN::handle_param_get_set_response}; + Canard::Client param_get_set_client{canard_iface, param_get_set_res_cb}; + + Canard::ObjCallback param_save_res_cb{this, &AP_DroneCAN::handle_param_save_response}; + Canard::Client param_save_client{canard_iface, param_save_res_cb}; + + // reboot client + void handle_restart_node_response(const CanardRxTransfer& transfer, const uavcan_protocol_RestartNodeResponse& msg) {} + Canard::ObjCallback restart_node_res_cb{this, &AP_DroneCAN::handle_restart_node_response}; + Canard::Client restart_node_client{canard_iface, restart_node_res_cb}; + + uavcan_protocol_param_ExecuteOpcodeRequest param_save_req; + uavcan_protocol_param_GetSetRequest param_getset_req; + + // Node Info Server + Canard::ObjCallback node_info_req_cb{this, &AP_DroneCAN::handle_node_info_request}; + Canard::Server node_info_server{canard_iface, node_info_req_cb}; + uavcan_protocol_GetNodeInfoResponse node_info_rsp; + + // incoming button handling + void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg); + void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg); + void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg); + void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg); + void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg); + static bool is_esc_data_index_valid(const uint8_t index); + void handle_debug(const CanardRxTransfer& transfer, const uavcan_protocol_debug_LogMessage& msg); + void handle_param_get_set_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_GetSetResponse& rsp); + void handle_param_save_response(const CanardRxTransfer& transfer, const uavcan_protocol_param_ExecuteOpcodeResponse& rsp); + void handle_node_info_request(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoRequest& req); +}; + +#endif // #if HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp similarity index 59% rename from libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp rename to libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp index 0e1b50cd035cd..d20a728bd007e 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp @@ -18,78 +18,36 @@ #include -#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#if HAL_ENABLE_DRONECAN_DRIVERS -#include "AP_UAVCAN_DNA_Server.h" -#include "AP_UAVCAN.h" +#include "AP_DroneCAN_DNA_Server.h" +#include "AP_DroneCAN.h" #include #include -#include #include #include -#include "AP_UAVCAN_Clock.h" #include +#include extern const AP_HAL::HAL& hal; #define NODEDATA_MAGIC 0xAC01 #define NODEDATA_MAGIC_LEN 2 #define MAX_NODE_ID 125 -#define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0) +#define debug_dronecan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "DroneCAN", fmt, ##args); } while (0) -//Callback Object Definitions -UC_REGISTRY_BINDER(AllocationCb, uavcan::protocol::dynamic_node_id::Allocation); -UC_REGISTRY_BINDER(NodeStatusCb, uavcan::protocol::NodeStatus); -UC_CLIENT_CALL_REGISTRY_BINDER(GetNodeInfoCb, uavcan::protocol::GetNodeInfo); - -static uavcan::ServiceClient* getNodeInfo_client[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -static uavcan::Publisher* allocation_pub[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -AP_UAVCAN_DNA_Server::AP_UAVCAN_DNA_Server(AP_UAVCAN *ap_uavcan, StorageAccess _storage) : - _ap_uavcan(ap_uavcan), - storage(_storage) +AP_DroneCAN_DNA_Server::AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan) : + _ap_dronecan(ap_dronecan), + _canard_iface(ap_dronecan.canard_iface), + storage(StorageManager::StorageCANDNA), + allocation_sub(allocation_cb, _ap_dronecan.get_driver_index()), + node_status_sub(node_status_cb, _ap_dronecan.get_driver_index()), + node_info_client(_canard_iface, node_info_cb) {} -/* Subscribe to all the messages we are going to handle for -Server registry and Node allocation. */ -void AP_UAVCAN_DNA_Server::subscribe_msgs(AP_UAVCAN* ap_uavcan) -{ - if (ap_uavcan == nullptr) { - return; - } - - auto* node = ap_uavcan->get_node(); - //Register Allocation Message Handler - uavcan::Subscriber *AllocationListener; - AllocationListener = new uavcan::Subscriber(*node); - if (AllocationListener == nullptr) { - AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: AllocationListener"); - } - const int alloc_listener_res = AllocationListener->start(AllocationCb(ap_uavcan, &trampoline_handleAllocation)); - if (alloc_listener_res < 0) { - AP_HAL::panic("Allocation Subscriber start problem\n\r"); - return; - } - //We allow anonymous transfers, as they are specifically for Node Allocation - AllocationListener->allowAnonymousTransfers(); - - //Register Node Status Listener - uavcan::Subscriber *NodeStatusListener; - NodeStatusListener = new uavcan::Subscriber(*node); - if (NodeStatusListener == nullptr) { - AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: NodeStatusListener"); - } - const int nodestatus_listener_res = NodeStatusListener->start(NodeStatusCb(ap_uavcan, &trampoline_handleNodeStatus)); - if (nodestatus_listener_res < 0) { - AP_HAL::panic("NodeStatus Subscriber start problem\n\r"); - return; - } -} - /* Method to generate 6byte hash from the Unique ID. We return it packed inside the referenced NodeData structure */ -void AP_UAVCAN_DNA_Server::getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const +void AP_DroneCAN_DNA_Server::getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const { uint64_t hash = FNV_1_OFFSET_BASIS_64; hash_fnv_1a(size, unique_id, &hash); @@ -104,7 +62,7 @@ void AP_UAVCAN_DNA_Server::getHash(NodeData &node_data, const uint8_t unique_id[ } //Read Node Data from Storage Region -bool AP_UAVCAN_DNA_Server::readNodeData(NodeData &data, uint8_t node_id) +bool AP_DroneCAN_DNA_Server::readNodeData(NodeData &data, uint8_t node_id) { if (node_id > MAX_NODE_ID) { return false; @@ -119,7 +77,7 @@ bool AP_UAVCAN_DNA_Server::readNodeData(NodeData &data, uint8_t node_id) } //Write Node Data to Storage Region -bool AP_UAVCAN_DNA_Server::writeNodeData(const NodeData &data, uint8_t node_id) +bool AP_DroneCAN_DNA_Server::writeNodeData(const NodeData &data, uint8_t node_id) { if (node_id > MAX_NODE_ID) { return false; @@ -135,7 +93,7 @@ bool AP_UAVCAN_DNA_Server::writeNodeData(const NodeData &data, uint8_t node_id) /* Set Occupation Mask, handy for keeping track of all node ids that are allocated and all that are available. */ -bool AP_UAVCAN_DNA_Server::setOccupationMask(uint8_t node_id) +bool AP_DroneCAN_DNA_Server::setOccupationMask(uint8_t node_id) { if (node_id > MAX_NODE_ID) { return false; @@ -146,7 +104,7 @@ bool AP_UAVCAN_DNA_Server::setOccupationMask(uint8_t node_id) /* Remove Node Data from Server Record in Storage, and also clear Occupation Mask */ -bool AP_UAVCAN_DNA_Server::freeNodeID(uint8_t node_id) +bool AP_DroneCAN_DNA_Server::freeNodeID(uint8_t node_id) { if (node_id > MAX_NODE_ID) { return false; @@ -167,7 +125,7 @@ bool AP_UAVCAN_DNA_Server::freeNodeID(uint8_t node_id) /* Sets the verification mask. This is to be called, once The Seen Node has been both registered and verified against the Server Records. */ -void AP_UAVCAN_DNA_Server::setVerificationMask(uint8_t node_id) +void AP_DroneCAN_DNA_Server::setVerificationMask(uint8_t node_id) { if (node_id > MAX_NODE_ID) { return; @@ -177,7 +135,7 @@ void AP_UAVCAN_DNA_Server::setVerificationMask(uint8_t node_id) /* Checks if the NodeID is occupied, i.e. its recorded in the Server Records against a unique ID */ -bool AP_UAVCAN_DNA_Server::isNodeIDOccupied(uint8_t node_id) const +bool AP_DroneCAN_DNA_Server::isNodeIDOccupied(uint8_t node_id) const { if (node_id > MAX_NODE_ID) { return false; @@ -187,7 +145,7 @@ bool AP_UAVCAN_DNA_Server::isNodeIDOccupied(uint8_t node_id) const /* Checks if NodeID is verified, i.e. the unique id in Storage Records matches the one provided by Device with this node id. */ -bool AP_UAVCAN_DNA_Server::isNodeIDVerified(uint8_t node_id) const +bool AP_DroneCAN_DNA_Server::isNodeIDVerified(uint8_t node_id) const { if (node_id > MAX_NODE_ID) { return false; @@ -198,7 +156,7 @@ bool AP_UAVCAN_DNA_Server::isNodeIDVerified(uint8_t node_id) const /* Go through Server Records, and fetch node id that matches the provided Unique IDs hash. Returns 255 if no Node ID was detected */ -uint8_t AP_UAVCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size) +uint8_t AP_DroneCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size) { uint8_t node_id = 255; NodeData node_data, cmp_node_data; @@ -221,7 +179,7 @@ uint8_t AP_UAVCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[], ui /* Hash the Unique ID and add it to the Server Record for specified Node ID. */ -bool AP_UAVCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size) +bool AP_DroneCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size) { NodeData node_data; getHash(node_data, unique_id, size); @@ -240,7 +198,7 @@ bool AP_UAVCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t u } //Checks if a valid Server Record is present for specified Node ID -bool AP_UAVCAN_DNA_Server::isValidNodeDataAvailable(uint8_t node_id) +bool AP_DroneCAN_DNA_Server::isValidNodeDataAvailable(uint8_t node_id) { NodeData node_data; readNodeData(node_data, node_id); @@ -255,43 +213,10 @@ bool AP_UAVCAN_DNA_Server::isValidNodeDataAvailable(uint8_t node_id) Also resets the Server Record in case there is a mismatch between specified node id and unique id against the existing Server Record. */ -bool AP_UAVCAN_DNA_Server::init() +bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id) { - //Read the details from ap_uavcan - uavcan::Node<0>* _node = _ap_uavcan->get_node(); - uint8_t node_id = _node->getNodeID().get(); - uint8_t own_unique_id[16] = {0}; - uint8_t own_unique_id_len = 16; - - driver_index = _ap_uavcan->get_driver_index(); - - //copy unique id from node to uint8_t array - uavcan::copy(_node->getHardwareVersion().unique_id.begin(), - _node->getHardwareVersion().unique_id.end(), - own_unique_id); - + //Read the details from AP_DroneCAN server_state = HEALTHY; - - //Setup publisher for this driver index - allocation_pub[driver_index] = new uavcan::Publisher(*_node, true); - if (allocation_pub[driver_index] == nullptr) { - AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: allocation_pub[%d]", driver_index); - } - - int res = allocation_pub[driver_index]->init(uavcan::TransferPriority::Default); - if (res < 0) { - return false; - } - allocation_pub[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::dynamic_node_id::Allocation::FOLLOWUP_TIMEOUT_MS)); - - - //Setup GetNodeInfo Client - getNodeInfo_client[driver_index] = new uavcan::ServiceClient(*_node, GetNodeInfoCb(_ap_uavcan, &trampoline_handleNodeInfo)); - if (getNodeInfo_client[driver_index] == nullptr) { - AP_BoardConfig::allocation_error("AP_UAVCAN_DNA: getNodeInfo_client[%d]", driver_index); - } - - /* Go through our records and look for valid NodeData, to initialise occupation mask */ for (uint8_t i = 0; i <= MAX_NODE_ID; i++) { @@ -310,7 +235,7 @@ bool AP_UAVCAN_DNA_Server::init() //Its not there a reset should write it in the Storage reset(); } - if (_ap_uavcan->check_and_reset_option(AP_UAVCAN::Options::DNA_CLEAR_DATABASE)) { + if (_ap_dronecan.check_and_reset_option(AP_DroneCAN::Options::DNA_CLEAR_DATABASE)) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UC DNA database reset"); reset(); } @@ -350,13 +275,13 @@ bool AP_UAVCAN_DNA_Server::init() addToSeenNodeMask(node_id); setVerificationMask(node_id); node_healthy_mask.set(node_id); - self_node_id[driver_index] = node_id; + self_node_id = node_id; return true; } //Reset the Server Records -void AP_UAVCAN_DNA_Server::reset() +void AP_DroneCAN_DNA_Server::reset() { NodeData node_data; memset(&node_data, 0, sizeof(node_data)); @@ -375,7 +300,7 @@ void AP_UAVCAN_DNA_Server::reset() /* Go through the Occupation mask for available Node ID based on pseudo code provided in uavcan/protocol/dynamic_node_id/1.Allocation.uavcan */ -uint8_t AP_UAVCAN_DNA_Server::findFreeNodeID(uint8_t preferred) +uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred) { // Search up uint8_t candidate = (preferred > 0) ? preferred : 125; @@ -398,7 +323,7 @@ uint8_t AP_UAVCAN_DNA_Server::findFreeNodeID(uint8_t preferred) } //Check if we have received Node Status from this node_id -bool AP_UAVCAN_DNA_Server::isNodeSeen(uint8_t node_id) +bool AP_DroneCAN_DNA_Server::isNodeSeen(uint8_t node_id) { if (node_id > MAX_NODE_ID) { return false; @@ -408,7 +333,7 @@ bool AP_UAVCAN_DNA_Server::isNodeSeen(uint8_t node_id) /* Set the Seen Node Mask, to be called when received Node Status from the node id */ -void AP_UAVCAN_DNA_Server::addToSeenNodeMask(uint8_t node_id) +void AP_DroneCAN_DNA_Server::addToSeenNodeMask(uint8_t node_id) { if (node_id > MAX_NODE_ID) { return; @@ -420,9 +345,9 @@ void AP_UAVCAN_DNA_Server::addToSeenNodeMask(uint8_t node_id) than once per 5 second. We continually verify the nodes in our seen list, So that we can raise issue if there are duplicates on the bus. */ -void AP_UAVCAN_DNA_Server::verify_nodes() +void AP_DroneCAN_DNA_Server::verify_nodes() { - uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec(); + uint32_t now = AP_HAL::millis(); if ((now - last_verification_request) < 5000) { return; } @@ -435,7 +360,7 @@ void AP_UAVCAN_DNA_Server::verify_nodes() //Check if we got acknowledgement from previous request //except for requests using our own node_id - if (curr_verifying_node == self_node_id[driver_index]) { + if (curr_verifying_node == self_node_id) { nodeInfo_resp_rcvd = true; } @@ -456,13 +381,16 @@ void AP_UAVCAN_DNA_Server::verify_nodes() //Find the next registered Node ID to be verified. for (uint8_t i = 0; i <= MAX_NODE_ID; i++) { curr_verifying_node = (curr_verifying_node + 1) % (MAX_NODE_ID + 1); + if ((curr_verifying_node == self_node_id) || (curr_verifying_node == 0)) { + continue; + } if (isNodeSeen(curr_verifying_node)) { break; } } - if (getNodeInfo_client[driver_index] != nullptr && isNodeIDOccupied(curr_verifying_node)) { - uavcan::protocol::GetNodeInfo::Request request; - getNodeInfo_client[driver_index]->call(curr_verifying_node, request); + if (isNodeIDOccupied(curr_verifying_node)) { + uavcan_protocol_GetNodeInfoRequest request; + node_info_client.request(curr_verifying_node, request); nodeInfo_resp_rcvd = false; } } @@ -470,63 +398,50 @@ void AP_UAVCAN_DNA_Server::verify_nodes() /* Handles Node Status Message, adds to the Seen Node list Also starts the Service call for Node Info to complete the Verification process. */ -void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb) +void AP_DroneCAN_DNA_Server::handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg) { - if (node_id > MAX_NODE_ID) { + if (transfer.source_node_id > MAX_NODE_ID || transfer.source_node_id == 0) { return; } - if ((cb.msg->health != uavcan::protocol::NodeStatus::HEALTH_OK || - cb.msg->mode != uavcan::protocol::NodeStatus::MODE_OPERATIONAL) && - !_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) { + if ((msg.health != UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK || + msg.mode != UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL) && + !_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) { //if node is not healthy or operational, clear resp health mask, and set fault_node_id - fault_node_id = node_id; + fault_node_id = transfer.source_node_id; server_state = NODE_STATUS_UNHEALTHY; - node_healthy_mask.clear(node_id); + node_healthy_mask.clear(transfer.source_node_id); } else { - node_healthy_mask.set(node_id); + node_healthy_mask.set(transfer.source_node_id); if (node_healthy_mask == verified_mask) { server_state = HEALTHY; } } - if (!isNodeIDVerified(node_id)) { + if (!isNodeIDVerified(transfer.source_node_id)) { //immediately begin verification of the node_id - if (getNodeInfo_client[driver_index] != nullptr) { - uavcan::protocol::GetNodeInfo::Request request; - getNodeInfo_client[driver_index]->call(node_id, request); - } + uavcan_protocol_GetNodeInfoRequest request; + node_info_client.request(transfer.source_node_id, request); } //Add node to seen list if not seen before - addToSeenNodeMask(node_id); -} - -//Trampoline call for handleNodeStatus member method -void AP_UAVCAN_DNA_Server::trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb) -{ - if (ap_uavcan == nullptr) { - return; - } - - ap_uavcan->_dna_server->handleNodeStatus(node_id, cb); + addToSeenNodeMask(transfer.source_node_id); } - /* Node Info message handler Handle responses from GetNodeInfo Request. We verify the node info against our records. Marks Verification mask if already recorded, Or register if the node id is available and not recorded for the received Unique ID */ -void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit) +void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp) { - if (node_id > MAX_NODE_ID) { + if (transfer.source_node_id > MAX_NODE_ID) { return; } /* if we haven't logged this node then log it now */ - if (!logged.get(node_id) && AP::logger().logging_started()) { - logged.set(node_id); + if (!logged.get(transfer.source_node_id) && AP::logger().logging_started()) { + logged.set(transfer.source_node_id); uint64_t uid[2]; - memcpy(uid, unique_id, sizeof(uid)); + memcpy(uid, rsp.hardware_version.unique_id, sizeof(rsp.hardware_version.unique_id)); // @LoggerMessage: CAND // @Description: Info from GetNodeInfo request // @Field: TimeUS: Time since system startup @@ -540,105 +455,82 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], AP::logger().Write("CAND", "TimeUS,NodeId,UID1,UID2,Name,Major,Minor,Version", "s#------", "F-------", "QBQQZBBI", AP_HAL::micros64(), - node_id, + transfer.source_node_id, uid[0], uid[1], - name, - major, - minor, - vcs_commit); + rsp.name.data, + rsp.software_version.major, + rsp.software_version.minor, + rsp.software_version.vcs_commit); } - if (isNodeIDOccupied(node_id)) { + if (isNodeIDOccupied(transfer.source_node_id)) { //if node_id already registered, just verify if Unique ID matches as well - if (node_id == getNodeIDForUniqueID(unique_id, 16)) { - if (node_id == curr_verifying_node) { + if (transfer.source_node_id == getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) { + if (transfer.source_node_id == curr_verifying_node) { nodeInfo_resp_rcvd = true; } - setVerificationMask(node_id); - } else if (!_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) { + setVerificationMask(transfer.source_node_id); + } else if (!_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) { /* This is a device with node_id already registered for another device */ server_state = DUPLICATE_NODES; - fault_node_id = node_id; - memcpy(fault_node_name, name, sizeof(fault_node_name)); + fault_node_id = transfer.source_node_id; + memcpy(fault_node_name, rsp.name.data, sizeof(fault_node_name)); } } else { /* Node Id was not allocated by us, or during this boot, let's register this in our records Check if we allocated this Node before */ - uint8_t prev_node_id = getNodeIDForUniqueID(unique_id, 16); + uint8_t prev_node_id = getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16); if (prev_node_id != 255) { //yes we did, remove this registration freeNodeID(prev_node_id); } //add a new server record - addNodeIDForUniqueID(node_id, unique_id, 16); + addNodeIDForUniqueID(transfer.source_node_id, rsp.hardware_version.unique_id, 16); //Verify as well - setVerificationMask(node_id); - if (node_id == curr_verifying_node) { + setVerificationMask(transfer.source_node_id); + if (transfer.source_node_id == curr_verifying_node) { nodeInfo_resp_rcvd = true; } } } -//Trampoline call for handleNodeInfo member call -void AP_UAVCAN_DNA_Server::trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp) -{ - uint8_t unique_id[16] = {0}; - char name[50] = {0}; - - //copy the unique id from message to uint8_t array - auto &r = resp.rsp->getResponse(); - uavcan::copy(r.hardware_version.unique_id.begin(), - r.hardware_version.unique_id.end(), - unique_id); - strncpy_noterm(name, r.name.c_str(), sizeof(name)-1); - - ap_uavcan->_dna_server->handleNodeInfo(node_id, unique_id, name, - r.software_version.major, - r.software_version.minor, - r.software_version.vcs_commit); -} - /* Handle the allocation message from the devices supporting dynamic node allocation. */ -void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t node_id, const AllocationCb &cb) +void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg) { - if (allocation_pub[driver_index] == nullptr) { - //init has not been called for this driver. - return; - } - if (!cb.msg->isAnonymousTransfer()) { + if (transfer.source_node_id != 0) { //Ignore Allocation messages that are not DNA requests return; } - uint32_t now = uavcan::SystemClock::instance().getMonotonic().toMSec(); + uint32_t now = AP_HAL::millis(); if (rcvd_unique_id_offset == 0 || (now - last_alloc_msg_ms) > 500) { - if (cb.msg->first_part_of_unique_id) { + if (msg.first_part_of_unique_id) { rcvd_unique_id_offset = 0; memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id)); } else { //we are only accepting first part return; } - } else if (cb.msg->first_part_of_unique_id) { + } else if (msg.first_part_of_unique_id) { // we are only accepting follow up messages return; } if (rcvd_unique_id_offset) { - debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting Followup part! %u\n", - long(uavcan::SystemClock::instance().getMonotonic().toUSec()/1000), + debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting Followup part! %u\n", + (long int)AP_HAL::millis(), unsigned((now - last_alloc_msg_ms))); } else { - debug_uavcan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting First part! %u\n", - long(uavcan::SystemClock::instance().getMonotonic().toUSec()/1000), + debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %ld -- Accepting First part! %u\n", + (long int)AP_HAL::millis(), unsigned((now - last_alloc_msg_ms))); } last_alloc_msg_ms = now; - if ((rcvd_unique_id_offset + cb.msg->unique_id.size()) > 16) { + if ((rcvd_unique_id_offset + msg.unique_id.len) > 16) { //This request is malformed, Reset! rcvd_unique_id_offset = 0; memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id)); @@ -646,54 +538,44 @@ void AP_UAVCAN_DNA_Server::handleAllocation(uint8_t node_id, const AllocationCb } //copy over the unique_id - for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + cb.msg->unique_id.size()); i++) { - rcvd_unique_id[i] = cb.msg->unique_id[i - rcvd_unique_id_offset]; + for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + msg.unique_id.len); i++) { + rcvd_unique_id[i] = msg.unique_id.data[i - rcvd_unique_id_offset]; } - rcvd_unique_id_offset += cb.msg->unique_id.size(); + rcvd_unique_id_offset += msg.unique_id.len; //send follow up message - uavcan::protocol::dynamic_node_id::Allocation msg; + uavcan_protocol_dynamic_node_id_Allocation rsp {}; /* Respond with the message containing the received unique ID so far or with node id if we successfully allocated one. */ - for (uint8_t i = 0; i < rcvd_unique_id_offset; i++) { - msg.unique_id.push_back(rcvd_unique_id[i]); - } + memcpy(rsp.unique_id.data, rcvd_unique_id, rcvd_unique_id_offset); + rsp.unique_id.len = rcvd_unique_id_offset; if (rcvd_unique_id_offset == 16) { //We have received the full Unique ID, time to do allocation uint8_t resp_node_id = getNodeIDForUniqueID((const uint8_t*)rcvd_unique_id, 16); if (resp_node_id == 255) { - resp_node_id = findFreeNodeID(cb.msg->node_id); + resp_node_id = findFreeNodeID(msg.node_id); if (resp_node_id != 255) { if (addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16)) { - msg.node_id = resp_node_id; + rsp.node_id = resp_node_id; } } else { GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!"); } } else { - msg.node_id = resp_node_id; + rsp.node_id = resp_node_id; } //reset states as well rcvd_unique_id_offset = 0; memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id)); } - allocation_pub[driver_index]->broadcast(msg); -} - -//Trampoline Call for handleAllocation member call -void AP_UAVCAN_DNA_Server::trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb) -{ - if (ap_uavcan == nullptr) { - return; - } - ap_uavcan->_dna_server->handleAllocation(node_id, cb); + allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD } //report the server state, along with failure message if any -bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) const +bool AP_DroneCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) const { switch (server_state) { case HEALTHY: @@ -703,7 +585,7 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co return false; } case DUPLICATE_NODES: { - if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) { + if (_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) { // ignore error return true; } @@ -715,7 +597,7 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co return false; } case NODE_STATUS_UNHEALTHY: { - if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) { + if (_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) { // ignore error return true; } diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h similarity index 66% rename from libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h rename to libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h index 5443423d1edcf..4dba03b5e3dee 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h @@ -2,20 +2,19 @@ #include #include -#if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include +#if HAL_ENABLE_DRONECAN_DRIVERS #include #include #include +#include +#include +#include +#include "AP_Canard_iface.h" +#include +class AP_DroneCAN; //Forward declaring classes -class AllocationCb; -class NodeStatusCb; -class NodeInfoCb; -class GetNodeInfoCb; -class AP_UAVCAN; - -class AP_UAVCAN_DNA_Server +class AP_DroneCAN_DNA_Server { StorageAccess storage; @@ -34,7 +33,7 @@ class AP_UAVCAN_DNA_Server uint32_t last_verification_request; uint8_t curr_verifying_node; - uint8_t self_node_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; + uint8_t self_node_id; bool nodeInfo_resp_rcvd; Bitmask<128> occupation_mask; @@ -89,24 +88,30 @@ class AP_UAVCAN_DNA_Server //Look in the storage and check if there's a valid Server Record there bool isValidNodeDataAvailable(uint8_t node_id); - static void trampoline_handleNodeInfo(AP_UAVCAN* ap_uavcan, uint8_t node_id, const GetNodeInfoCb& resp); - static void trampoline_handleAllocation(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AllocationCb &cb); - static void trampoline_handleNodeStatus(AP_UAVCAN* ap_uavcan, uint8_t node_id, const NodeStatusCb &cb); + HAL_Semaphore storage_sem; + AP_DroneCAN &_ap_dronecan; + CanardInterface &_canard_iface; + Canard::Publisher allocation_pub{_canard_iface}; - HAL_Semaphore storage_sem; - AP_UAVCAN *_ap_uavcan; - uint8_t driver_index; + Canard::ObjCallback allocation_cb{this, &AP_DroneCAN_DNA_Server::handleAllocation}; + Canard::Subscriber allocation_sub; + + Canard::ObjCallback node_status_cb{this, &AP_DroneCAN_DNA_Server::handleNodeStatus}; + Canard::Subscriber node_status_sub; + + Canard::ObjCallback node_info_cb{this, &AP_DroneCAN_DNA_Server::handleNodeInfo}; + Canard::Client node_info_client; public: - AP_UAVCAN_DNA_Server(AP_UAVCAN *ap_uavcan, StorageAccess _storage); + AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan); // Do not allow copies - CLASS_NO_COPY(AP_UAVCAN_DNA_Server); + CLASS_NO_COPY(AP_DroneCAN_DNA_Server); //Initialises publisher and Server Record for specified uavcan driver - bool init(); + bool init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id); //Reset the Server Record void reset(); @@ -119,15 +124,15 @@ class AP_UAVCAN_DNA_Server /* Subscribe to the messages to be handled for maintaining and allocating Node ID list */ - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); //report the server state, along with failure message if any bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const; //Callbacks - void handleAllocation(uint8_t node_id, const AllocationCb &cb); - void handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb); - void handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit); + void handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg); + void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg); + void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp); //Run through the list of seen node ids for verification void verify_nodes(); diff --git a/libraries/AP_DroneCAN/canard/canard_helpers_user.h b/libraries/AP_DroneCAN/canard/canard_helpers_user.h new file mode 100644 index 0000000000000..ff4e1d6c98916 --- /dev/null +++ b/libraries/AP_DroneCAN/canard/canard_helpers_user.h @@ -0,0 +1,5 @@ +#include // to include SEMAPHORE + +namespace Canard { +typedef ::HAL_Semaphore Semaphore; +} diff --git a/libraries/AP_UAVCAN/dsdl/README.md b/libraries/AP_DroneCAN/dsdl/README.md similarity index 100% rename from libraries/AP_UAVCAN/dsdl/README.md rename to libraries/AP_DroneCAN/dsdl/README.md diff --git a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp new file mode 100644 index 0000000000000..7f09bc2c3313d --- /dev/null +++ b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp @@ -0,0 +1,246 @@ +/* + simple DroneCAN network sniffer as an ArduPilot firmware + */ +#include +#include + +#if HAL_ENABLE_DRONECAN_DRIVERS + +#include + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#include +#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS +#include +#include +#endif + +void setup(); +void loop(); + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +#define DRONECAN_NODE_POOL_SIZE 8192 + +static uint8_t node_memory_pool[DRONECAN_NODE_POOL_SIZE]; + +#define debug_dronecan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) + +class DroneCAN_sniffer { +public: + DroneCAN_sniffer(); + ~DroneCAN_sniffer(); + + void init(void); + void loop(void); + void print_stats(void); + +private: + uint8_t driver_index = 0; + + AP_CANManager can_mgr; +}; + +static struct { + const char *msg_name; + uint32_t count; +} counters[100]; + +static void count_msg(const char *name) +{ + for (uint16_t i=0; i *node_status_pub; +Canard::Server *node_info_srv; + +static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan_protocol_GetNodeInfoRequest& msg) +{ + if (node_info_srv == nullptr) { + return; + } + node_info_srv->respond(transfer, node_info); +} + +void DroneCAN_sniffer::init(void) +{ + const_cast (hal).can[driver_index] = new HAL_CANIface(driver_index); + + if (hal.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); + + if (!hal.can[driver_index]->is_initialized()) { + debug_dronecan("Can not initialised\n"); + return; + } + _uavcan_iface_mgr = new CanardInterface{driver_index}; + + if (_uavcan_iface_mgr == nullptr) { + return; + } + + if (!_uavcan_iface_mgr->add_interface(hal.can[driver_index])) { + debug_dronecan("Failed to add iface"); + return; + } + + _uavcan_iface_mgr->init(node_memory_pool, sizeof(node_memory_pool), 9); + + node_status_pub = new Canard::Publisher{*_uavcan_iface_mgr}; + if (node_status_pub == nullptr) { + return; + } + + node_info_srv = new Canard::Server{*_uavcan_iface_mgr, *Canard::allocate_static_callback(cb_GetNodeInfoRequest)}; + if (node_info_srv == nullptr) { + return; + } + + node_info.name.len = snprintf((char*)node_info.name.data, sizeof(node_info.name.data), "org.ardupilot:%u", driver_index); + + node_info.software_version.major = AP_DRONECAN_SW_VERS_MAJOR; + node_info.software_version.minor = AP_DRONECAN_SW_VERS_MINOR; + + node_info.hardware_version.major = AP_DRONECAN_HW_VERS_MAJOR; + node_info.hardware_version.minor = AP_DRONECAN_HW_VERS_MINOR; + +#define START_CB(mtype, cbname) Canard::allocate_sub_static_callback(cb_ ## cbname,driver_index) + + START_CB(uavcan_protocol_NodeStatus, NodeStatus); + START_CB(uavcan_equipment_gnss_Fix2, Fix2); + START_CB(uavcan_equipment_gnss_Auxiliary, Auxiliary); + START_CB(uavcan_equipment_ahrs_MagneticFieldStrength, MagneticFieldStrength); + START_CB(uavcan_equipment_ahrs_MagneticFieldStrength2, MagneticFieldStrength2); + START_CB(uavcan_equipment_air_data_StaticPressure, StaticPressure); + START_CB(uavcan_equipment_air_data_StaticTemperature, StaticTemperature); + START_CB(uavcan_equipment_power_BatteryInfo, BatteryInfo); + START_CB(uavcan_equipment_actuator_ArrayCommand, ArrayCommand); + START_CB(uavcan_equipment_esc_RawCommand, RawCommand); + START_CB(uavcan_equipment_indication_LightsCommand, LightsCommand); + START_CB(com_hex_equipment_flow_Measurement, Measurement); + + debug_dronecan("DroneCAN: init done\n\r"); +} + +static void send_node_status() +{ + uavcan_protocol_NodeStatus msg; + msg.uptime_sec = AP_HAL::millis() / 1000; + msg.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + msg.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + msg.sub_mode = 0; + msg.vendor_specific_status_code = 0; + node_status_pub->broadcast(msg); +} + +uint32_t last_status_send = 0; +void DroneCAN_sniffer::loop(void) +{ + if (AP_HAL::millis() - last_status_send > 1000) { + last_status_send = AP_HAL::millis(); + send_node_status(); + } + _uavcan_iface_mgr->process(1); +} + +void DroneCAN_sniffer::print_stats(void) +{ + hal.console->printf("%lu\n", (unsigned long)AP_HAL::micros()); + for (uint16_t i=0;i<100;i++) { + if (counters[i].msg_name == nullptr) { + break; + } + hal.console->printf("%s: %u\n", counters[i].msg_name, unsigned(counters[i].count)); + counters[i].count = 0; + } + hal.console->printf("\n"); +} + +static DroneCAN_sniffer sniffer; + +DroneCAN_sniffer::DroneCAN_sniffer() +{} + +DroneCAN_sniffer::~DroneCAN_sniffer() +{ +} + +void setup(void) +{ + hal.scheduler->delay(2000); + hal.console->printf("Starting DroneCAN sniffer\n"); + sniffer.init(); +} + +void loop(void) +{ + sniffer.loop(); + static uint32_t last_print_ms; + uint32_t now = AP_HAL::millis(); + if (now - last_print_ms >= 1000) { + last_print_ms = now; + sniffer.print_stats(); + } + + // auto-reboot for --upload + if (hal.console->available() > 50) { + hal.console->printf("rebooting\n"); + hal.console->discard_input(); + hal.scheduler->reboot(false); + } + hal.console->discard_input(); +} + +AP_HAL_MAIN(); + +#else + +#include + +const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +static void loop() { } +static void setup() +{ + printf("Board not currently supported\n"); +} + +AP_HAL_MAIN(); +#endif diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/README.md b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/README.md similarity index 100% rename from libraries/AP_UAVCAN/examples/UAVCAN_sniffer/README.md rename to libraries/AP_DroneCAN/examples/DroneCAN_sniffer/README.md diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/nobuild.txt b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/nobuild.txt similarity index 100% rename from libraries/AP_UAVCAN/examples/UAVCAN_sniffer/nobuild.txt rename to libraries/AP_DroneCAN/examples/DroneCAN_sniffer/nobuild.txt diff --git a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/wscript b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/wscript new file mode 100644 index 0000000000000..3e81beeee182a --- /dev/null +++ b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/wscript @@ -0,0 +1,21 @@ +#!/usr/bin/env python +# encoding: utf-8 +from waflib.TaskGen import after_method, before_method, feature + +def build(bld): + vehicle = bld.path.name + + bld.ap_stlib( + name=vehicle + '_libs', + ap_vehicle='UNKNOWN', + dynamic_source='modules/DroneCAN/libcanard/dsdlc_generated/src/**.c', + ap_libraries=bld.ap_common_vehicle_libraries() + [ + 'AP_OSD', + 'AP_RCMapper', + 'AP_Arming' + ], + ) + bld.ap_program( + program_groups=['tool'], + use=[vehicle + '_libs'], + ) diff --git a/libraries/AP_EFI/AP_EFI_Backend.h b/libraries/AP_EFI/AP_EFI_Backend.h index 9506c7ff091b1..3f637dba3f441 100644 --- a/libraries/AP_EFI/AP_EFI_Backend.h +++ b/libraries/AP_EFI/AP_EFI_Backend.h @@ -42,7 +42,7 @@ class AP_EFI_Backend { // Internal state for this driver (before copying to frontend) EFI_State internal_state; - int8_t get_uavcan_node_id(void) const; + int8_t get_dronecan_node_id(void) const; float get_coef1(void) const; float get_coef2(void) const; float get_ecu_fuel_density(void) const; diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp index 0df6d05484153..c242598cf092a 100644 --- a/libraries/AP_EFI/AP_EFI_DroneCAN.cpp +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.cpp @@ -5,17 +5,13 @@ #if AP_EFI_DRONECAN_ENABLED #include -#include - -#include +#include +#include extern const AP_HAL::HAL& hal; AP_EFI_DroneCAN *AP_EFI_DroneCAN::driver; -// DroneCAN Frontend Registry Binder -UC_REGISTRY_BINDER(EFIStatusCb, uavcan::equipment::ice::reciprocating::Status); - // constructor AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) : AP_EFI_Backend(_frontend) @@ -24,21 +20,14 @@ AP_EFI_DroneCAN::AP_EFI_DroneCAN(AP_EFI &_frontend) : } // links the DroneCAN message to this backend -void AP_EFI_DroneCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) +void AP_EFI_DroneCAN::subscribe_msgs(AP_DroneCAN *ap_dronecan) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return; } - auto* node = ap_uavcan->get_node(); - uavcan::Subscriber *status_listener; - status_listener = new uavcan::Subscriber(*node); - - // Register method to handle incoming status - const int status_listener_res = status_listener->start(EFIStatusCb(ap_uavcan, &trampoline_status)); - if (status_listener_res < 0) { - AP_HAL::panic("DroneCAN EFI subscriber start problem\n\r"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &trampoline_status, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); } } @@ -48,86 +37,85 @@ void AP_EFI_DroneCAN::update() } // EFI message handler -void AP_EFI_DroneCAN::trampoline_status(AP_UAVCAN *ap_uavcan, uint8_t node_id, const EFIStatusCb &cb) +void AP_EFI_DroneCAN::trampoline_status(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg) { if (driver == nullptr) { return; } - const uavcan::equipment::ice::reciprocating::Status &pkt = *cb.msg; - driver->handle_status(pkt); + driver->handle_status(msg); } /* handle reciprocating ICE status message from DroneCAN */ -void AP_EFI_DroneCAN::handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt) +void AP_EFI_DroneCAN::handle_status(const uavcan_equipment_ice_reciprocating_Status &pkt) { auto &istate = internal_state; // state maps 1:1 from Engine_State istate.engine_state = Engine_State(pkt.state); - if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) { + if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED)) { istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::NOT_SUPPORTED; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_CRANKSHAFT_SENSOR_ERROR) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_CRANKSHAFT_SENSOR_ERROR) { istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::ERROR; } else { istate.crankshaft_sensor_status = Crankshaft_Sensor_Status::OK; } - if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_SUPPORTED)) { + if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_SUPPORTED)) { istate.temperature_status = Temperature_Status::NOT_SUPPORTED; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_BELOW_NOMINAL) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_BELOW_NOMINAL) { istate.temperature_status = Temperature_Status::BELOW_NOMINAL; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_ABOVE_NOMINAL) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_ABOVE_NOMINAL) { istate.temperature_status = Temperature_Status::ABOVE_NOMINAL; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_OVERHEATING) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_OVERHEATING) { istate.temperature_status = Temperature_Status::OVERHEATING; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL) { istate.temperature_status = Temperature_Status::EGT_ABOVE_NOMINAL; } else { istate.temperature_status = Temperature_Status::OK; } - if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_SUPPORTED)) { + if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_SUPPORTED)) { istate.fuel_pressure_status = Fuel_Pressure_Status::NOT_SUPPORTED; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_BELOW_NOMINAL) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_BELOW_NOMINAL) { istate.fuel_pressure_status = Fuel_Pressure_Status::BELOW_NOMINAL; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_FUEL_PRESSURE_ABOVE_NOMINAL) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_FUEL_PRESSURE_ABOVE_NOMINAL) { istate.fuel_pressure_status = Fuel_Pressure_Status::ABOVE_NOMINAL; } else { istate.fuel_pressure_status = Fuel_Pressure_Status::OK; } - if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_SUPPORTED)) { + if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_SUPPORTED)) { istate.oil_pressure_status = Oil_Pressure_Status::NOT_SUPPORTED; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_BELOW_NOMINAL) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_BELOW_NOMINAL) { istate.oil_pressure_status = Oil_Pressure_Status::BELOW_NOMINAL; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_OIL_PRESSURE_ABOVE_NOMINAL) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_OIL_PRESSURE_ABOVE_NOMINAL) { istate.oil_pressure_status = Oil_Pressure_Status::ABOVE_NOMINAL; } else { istate.oil_pressure_status = Oil_Pressure_Status::OK; } - if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_SUPPORTED)) { + if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_SUPPORTED)) { istate.detonation_status = Detonation_Status::NOT_SUPPORTED; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DETONATION_OBSERVED) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DETONATION_OBSERVED) { istate.detonation_status = Detonation_Status::OBSERVED; } else { istate.detonation_status = Detonation_Status::NOT_OBSERVED; } - if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_SUPPORTED)) { + if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_SUPPORTED)) { istate.misfire_status = Misfire_Status::NOT_SUPPORTED; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_MISFIRE_OBSERVED) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_MISFIRE_OBSERVED) { istate.misfire_status = Misfire_Status::OBSERVED; } else { istate.misfire_status = Misfire_Status::NOT_OBSERVED; } - if (!(pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_SUPPORTED)) { + if (!(pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_SUPPORTED)) { istate.debris_status = Debris_Status::NOT_SUPPORTED; - } else if (pkt.flags & uavcan::equipment::ice::reciprocating::Status::FLAG_DEBRIS_DETECTED) { + } else if (pkt.flags & UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_FLAG_DEBRIS_DETECTED) { istate.debris_status = Debris_Status::DETECTED; } else { istate.debris_status = Debris_Status::NOT_DETECTED; @@ -152,8 +140,8 @@ void AP_EFI_DroneCAN::handle_status(const uavcan::equipment::ice::reciprocating: istate.spark_plug_usage = Spark_Plug_Usage(pkt.spark_plug_usage); // assume max one cylinder status - if (pkt.cylinder_status.size() > 0) { - const auto &cs = pkt.cylinder_status[0]; + if (pkt.cylinder_status.len > 0) { + const auto &cs = pkt.cylinder_status.data[0]; auto &c = istate.cylinder_status; c.ignition_timing_deg = cs.ignition_timing_deg; c.injection_time_ms = cs.injection_time_ms; diff --git a/libraries/AP_EFI/AP_EFI_DroneCAN.h b/libraries/AP_EFI/AP_EFI_DroneCAN.h index b84e4be1b0c9f..c4f9dde22c472 100644 --- a/libraries/AP_EFI/AP_EFI_DroneCAN.h +++ b/libraries/AP_EFI/AP_EFI_DroneCAN.h @@ -4,10 +4,7 @@ #include "AP_EFI_Backend.h" #if AP_EFI_DRONECAN_ENABLED -#include -#include - -class EFIStatusCb; +#include class AP_EFI_DroneCAN : public AP_EFI_Backend { public: @@ -15,11 +12,11 @@ class AP_EFI_DroneCAN : public AP_EFI_Backend { void update() override; - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static void trampoline_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const EFIStatusCb &cb); + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static void trampoline_status(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ice_reciprocating_Status &msg); private: - void handle_status(const uavcan::equipment::ice::reciprocating::Status &pkt); + void handle_status(const uavcan_equipment_ice_reciprocating_Status &pkt); // singleton for trampoline static AP_EFI_DroneCAN *driver; diff --git a/libraries/AP_EFI/AP_EFI_config.h b/libraries/AP_EFI/AP_EFI_config.h index 29f07638f74a5..a880817073c45 100644 --- a/libraries/AP_EFI/AP_EFI_config.h +++ b/libraries/AP_EFI/AP_EFI_config.h @@ -16,7 +16,7 @@ #endif #ifndef AP_EFI_DRONECAN_ENABLED -#define AP_EFI_DRONECAN_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED && HAL_MAX_CAN_PROTOCOL_DRIVERS && HAL_CANMANAGER_ENABLED +#define AP_EFI_DRONECAN_ENABLED AP_EFI_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS #endif #ifndef AP_EFI_NWPWU_ENABLED diff --git a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp index 2c3929f08db88..8aedfc6455643 100644 --- a/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp +++ b/libraries/AP_FlashIface/AP_FlashIface_JEDEC.cpp @@ -929,13 +929,13 @@ bool AP_FlashIface_JEDEC::start_xip_mode(void** addr) { // Set QSPI module for XIP mode AP_HAL::QSPIDevice::CommandHeader cmd; - cmd.cmd = _desc.fast_read_ins; - cmd.alt = 0xA5; + cmd.cmd = _desc.fast_read_ins; // generally 0xEB for 1-4-4 access + cmd.alt = 0xF0; // add M0-7 bits in alt to make up 32-bit address phase, sec 8.2.11 W25Q64JV reference cmd.cfg = AP_HAL::QSPI::CFG_ADDR_SIZE_24 | AP_HAL::QSPI::CFG_CMD_MODE_ONE_LINE | AP_HAL::QSPI::CFG_ADDR_MODE_FOUR_LINES | AP_HAL::QSPI::CFG_DATA_MODE_FOUR_LINES | - AP_HAL::QSPI::CFG_ALT_MODE_FOUR_LINES | /* Always 4 lines, note.*/ + AP_HAL::QSPI::CFG_ALT_MODE_FOUR_LINES | AP_HAL::QSPI::CFG_ALT_SIZE_8; cmd.addr = 0; cmd.dummy = _desc.fast_read_dummy_cycles; diff --git a/libraries/AP_FlashIface/examples/jedec_test_bl/jedec_test.cpp b/libraries/AP_FlashIface/examples/jedec_test_bl/jedec_test.cpp index a6d4ed9241d11..cd9aeb6a0ef44 100644 --- a/libraries/AP_FlashIface/examples/jedec_test_bl/jedec_test.cpp +++ b/libraries/AP_FlashIface/examples/jedec_test_bl/jedec_test.cpp @@ -2,6 +2,7 @@ #include #include #include +#include AP_FlashIface_JEDEC jedec_dev; @@ -16,7 +17,7 @@ static UNUSED_FUNCTION void test_page_program() uprintf("Failed to allocate data for read"); } - // fill program data with its own adress + // fill program data with its own address for (uint32_t i = 0; i < jedec_dev.get_page_size(); i++) { data[i] = i; } diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e3adaf8059a55..d78191f8761de 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -41,10 +41,10 @@ #include "AP_GPS_SITL.h" #endif -#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#if HAL_ENABLE_DRONECAN_DRIVERS #include -#include -#include "AP_GPS_UAVCAN.h" +#include +#include "AP_GPS_DroneCAN.h" #endif #include @@ -384,7 +384,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { AP_GROUPINFO("_PRIMARY", 27, AP_GPS, _primary, 0), #endif -#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#if HAL_ENABLE_DRONECAN_DRIVERS // @Param: _CAN_NODEID1 // @DisplayName: GPS Node ID 1 // @Description: GPS Node id for first-discovered GPS. @@ -413,7 +413,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @User: Advanced AP_GROUPINFO("2_CAN_OVRIDE", 31, AP_GPS, _override_node_id[1], 0), #endif // GPS_MAX_RECEIVERS > 1 -#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS +#endif // HAL_ENABLE_DRONECAN_DRIVERS AP_GROUPEND }; @@ -706,9 +706,9 @@ AP_GPS_Backend *AP_GPS::_detect_instance(uint8_t instance) case GPS_TYPE_UAVCAN: case GPS_TYPE_UAVCAN_RTK_BASE: case GPS_TYPE_UAVCAN_RTK_ROVER: -#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#if HAL_ENABLE_DRONECAN_DRIVERS dstate->auto_detected_baud = false; // specified, not detected - return AP_GPS_UAVCAN::probe(*this, state[instance]); + return AP_GPS_DroneCAN::probe(*this, state[instance]); #endif return nullptr; // We don't do anything here if UAVCAN is not supported #if HAL_MSP_GPS_ENABLED @@ -2119,11 +2119,11 @@ bool AP_GPS::prepare_for_arming(void) { bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { -#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#if HAL_ENABLE_DRONECAN_DRIVERS if (_type[i] == GPS_TYPE_UAVCAN || _type[i] == GPS_TYPE_UAVCAN_RTK_BASE || _type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) { - if (!AP_GPS_UAVCAN::backends_healthy(failure_msg, failure_msg_len)) { + if (!AP_GPS_DroneCAN::backends_healthy(failure_msg, failure_msg_len)) { return false; } } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 25c138a0fba6e..96013c72fec1d 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -86,7 +86,7 @@ class AP_GPS friend class AP_GPS_SIRF; friend class AP_GPS_UBLOX; friend class AP_GPS_Backend; - friend class AP_GPS_UAVCAN; + friend class AP_GPS_DroneCAN; public: AP_GPS(); @@ -593,7 +593,7 @@ class AP_GPS AP_Float _blend_tc; AP_Int16 _driver_options; AP_Int8 _primary; -#if HAL_ENABLE_LIBUAVCAN_DRIVERS +#if HAL_ENABLE_DRONECAN_DRIVERS AP_Int32 _node_id[GPS_MAX_RECEIVERS]; AP_Int32 _override_node_id[GPS_MAX_RECEIVERS]; #endif diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp similarity index 57% rename from libraries/AP_GPS/AP_GPS_UAVCAN.cpp rename to libraries/AP_GPS/AP_GPS_DroneCAN.cpp index c108f64d96189..83ca20ec1f3cc 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -18,24 +18,16 @@ // #include -#if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include "AP_GPS_UAVCAN.h" +#if HAL_ENABLE_DRONECAN_DRIVERS +#include "AP_GPS_DroneCAN.h" #include -#include +#include #include #include -#include -#include -#include -#include -#if GPS_MOVING_BASELINE -#include -#include -#endif - +#include #include #define GPS_PPS_EMULATION 0 @@ -59,35 +51,26 @@ extern const AP_HAL::HAL& hal; #define LOG_TAG "GPS" -UC_REGISTRY_BINDER(Fix2Cb, uavcan::equipment::gnss::Fix2); -UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary); -UC_REGISTRY_BINDER(HeadingCb, ardupilot::gnss::Heading); -UC_REGISTRY_BINDER(StatusCb, ardupilot::gnss::Status); -#if GPS_MOVING_BASELINE -UC_REGISTRY_BINDER(MovingBaselineDataCb, ardupilot::gnss::MovingBaselineData); -UC_REGISTRY_BINDER(RelPosHeadingCb, ardupilot::gnss::RelPosHeading); -#endif - #if CONFIG_HAL_BOARD == HAL_BOARD_SITL #define NATIVE_TIME_OFFSET (AP_HAL::micros64() - AP_HAL::native_micros64()) #else #define NATIVE_TIME_OFFSET 0 #endif -AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[]; -HAL_Semaphore AP_GPS_UAVCAN::_sem_registry; +AP_GPS_DroneCAN::DetectedModules AP_GPS_DroneCAN::_detected_modules[]; +HAL_Semaphore AP_GPS_DroneCAN::_sem_registry; // Member Methods -AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) : +AP_GPS_DroneCAN::AP_GPS_DroneCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) : AP_GPS_Backend(_gps, _state, nullptr), interim_state(_state), role(_role) { - param_int_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_int, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &); - param_float_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_get_set_response_float, bool, AP_UAVCAN*, const uint8_t, const char*, float &); - param_save_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_UAVCAN::handle_param_save_response, void, AP_UAVCAN*, const uint8_t, bool); + param_int_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_DroneCAN::handle_param_get_set_response_int, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &); + param_float_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_DroneCAN::handle_param_get_set_response_float, bool, AP_DroneCAN*, const uint8_t, const char*, float &); + param_save_cb = FUNCTOR_BIND_MEMBER(&AP_GPS_DroneCAN::handle_param_save_response, void, AP_DroneCAN*, const uint8_t, bool); } -AP_GPS_UAVCAN::~AP_GPS_UAVCAN() +AP_GPS_DroneCAN::~AP_GPS_DroneCAN() { WITH_SEMAPHORE(_sem_registry); @@ -100,85 +83,46 @@ AP_GPS_UAVCAN::~AP_GPS_UAVCAN() #endif } -void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +void AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return; } - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *gnss_fix2; - gnss_fix2 = new uavcan::Subscriber(*node); - if (gnss_fix2 == nullptr) { - AP_BoardConfig::allocation_error("gnss_fix2"); - } - const int gnss_fix2_start_res = gnss_fix2->start(Fix2Cb(ap_uavcan, &handle_fix2_msg_trampoline)); - if (gnss_fix2_start_res < 0) { - AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); - } - - uavcan::Subscriber *gnss_aux; - gnss_aux = new uavcan::Subscriber(*node); - if (gnss_aux == nullptr) { - AP_BoardConfig::allocation_error("gnss_aux"); - } - const int gnss_aux_start_res = gnss_aux->start(AuxCb(ap_uavcan, &handle_aux_msg_trampoline)); - if (gnss_aux_start_res < 0) { - AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); } - uavcan::Subscriber *gnss_heading; - gnss_heading = new uavcan::Subscriber(*node); - if (gnss_heading == nullptr) { - AP_BoardConfig::allocation_error("gnss_heading"); - } - const int gnss_heading_start_res = gnss_heading->start(HeadingCb(ap_uavcan, &handle_heading_msg_trampoline)); - if (gnss_heading_start_res < 0) { - AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); } - uavcan::Subscriber *gnss_status; - gnss_status = new uavcan::Subscriber(*node); - if (gnss_status == nullptr) { - AP_BoardConfig::allocation_error("gnss_status"); - } - const int gnss_status_start_res = gnss_status->start(StatusCb(ap_uavcan, &handle_status_msg_trampoline)); - if (gnss_status_start_res < 0) { - AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); } -#if GPS_MOVING_BASELINE - uavcan::Subscriber *gnss_moving_baseline; - gnss_moving_baseline = new uavcan::Subscriber(*node); - if (gnss_moving_baseline == nullptr) { - AP_BoardConfig::allocation_error("gnss_moving_baseline"); + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("status_sub"); } - const int gnss_moving_baseline_start_res = gnss_moving_baseline->start(MovingBaselineDataCb(ap_uavcan, &handle_moving_baseline_msg_trampoline)); - if (gnss_moving_baseline_start_res < 0) { - AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); +#if GPS_MOVING_BASELINE + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("moving_baseline_sub"); } - uavcan::Subscriber *gnss_relposheading; - gnss_relposheading = new uavcan::Subscriber(*node); - if (gnss_relposheading == nullptr) { - AP_BoardConfig::allocation_error("gnss_relposheading"); - } - const int gnss_relposheading_start_res = gnss_relposheading->start(RelPosHeadingCb(ap_uavcan, &handle_relposheading_msg_trampoline)); - if (gnss_relposheading_start_res < 0) { - AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r"); + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("relposheading_sub"); } #endif } -AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) +AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) { WITH_SEMAPHORE(_sem_registry); int8_t found_match = -1, last_match = -1; - AP_GPS_UAVCAN* backend = nullptr; + AP_GPS_DroneCAN* backend = nullptr; bool bad_override_config = false; for (int8_t i = GPS_MAX_RECEIVERS - 1; i >= 0; i--) { - if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) { + if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_dronecan != nullptr) { if (_gps._override_node_id[_state.instance] != 0 && _gps._override_node_id[_state.instance] != _detected_modules[i].node_id) { continue; // This device doesn't match the correct node @@ -220,14 +164,14 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) // initialise the backend based on the UAVCAN Moving baseline selection switch (_gps.get_type(_state.instance)) { case AP_GPS::GPS_TYPE_UAVCAN: - backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL); + backend = new AP_GPS_DroneCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL); break; #if GPS_MOVING_BASELINE case AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE: - backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_BASE); + backend = new AP_GPS_DroneCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_BASE); break; case AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER: - backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_ROVER); + backend = new AP_GPS_DroneCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_ROVER); break; #endif default: @@ -236,19 +180,19 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) if (backend == nullptr) { AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, - "Failed to register UAVCAN GPS Node %d on Bus %d\n", + "Failed to register DroneCAN GPS Node %d on Bus %d\n", _detected_modules[found_match].node_id, - _detected_modules[found_match].ap_uavcan->get_driver_index()); + _detected_modules[found_match].ap_dronecan->get_driver_index()); } else { _detected_modules[found_match].driver = backend; backend->_detected_module = found_match; AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, - "Registered UAVCAN GPS Node %d on Bus %d as instance %d\n", + "Registered DroneCAN GPS Node %d on Bus %d as instance %d\n", _detected_modules[found_match].node_id, - _detected_modules[found_match].ap_uavcan->get_driver_index(), + _detected_modules[found_match].ap_dronecan->get_driver_index(), _state.instance); - snprintf(backend->_name, ARRAY_SIZE(backend->_name), "UAVCAN%u-%u", _detected_modules[found_match].ap_uavcan->get_driver_index()+1, _detected_modules[found_match].node_id); + snprintf(backend->_name, ARRAY_SIZE(backend->_name), "DroneCAN%u-%u", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id); _detected_modules[found_match].instance = _state.instance; for (uint8_t i=0; i < GPS_MAX_RECEIVERS; i++) { if (_detected_modules[found_match].node_id == AP::gps()._node_id[i]) { @@ -266,7 +210,7 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) if (backend->role == AP_GPS::GPS_ROLE_MB_BASE) { backend->rtcm3_parser = new RTCM3_Parser; if (backend->rtcm3_parser == nullptr) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UAVCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_uavcan->get_driver_index()+1, _detected_modules[found_match].node_id); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DroneCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_dronecan->get_driver_index()+1, _detected_modules[found_match].node_id); } } #endif // GPS_MOVING_BASELINE @@ -275,7 +219,7 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) return backend; } -bool AP_GPS_UAVCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_len) +bool AP_GPS_DroneCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_len) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { bool overriden_node_found = false; @@ -310,15 +254,15 @@ bool AP_GPS_UAVCAN::backends_healthy(char failure_msg[], uint16_t failure_msg_le return true; } -AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id) +AP_GPS_DroneCAN* AP_GPS_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return nullptr; } for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { if (_detected_modules[i].driver != nullptr && - _detected_modules[i].ap_uavcan == ap_uavcan && + _detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) { return _detected_modules[i].driver; } @@ -327,7 +271,7 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n bool already_detected = false; // Check if there's an empty spot for possible registeration for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].node_id == node_id) { + if (_detected_modules[i].ap_dronecan == ap_dronecan && _detected_modules[i].node_id == node_id) { // Already Detected already_detected = true; break; @@ -335,8 +279,8 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n } if (!already_detected) { for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_detected_modules[i].ap_uavcan == nullptr) { - _detected_modules[i].ap_uavcan = ap_uavcan; + if (_detected_modules[i].ap_dronecan == nullptr) { + _detected_modules[i].ap_dronecan = ap_dronecan; _detected_modules[i].node_id = node_id; // Just set the Node ID in order of appearance // This will be used to set select ids @@ -372,9 +316,9 @@ AP_GPS_UAVCAN* AP_GPS_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t n /* handle velocity element of message */ -void AP_GPS_UAVCAN::handle_velocity(const float vx, const float vy, const float vz) +void AP_GPS_DroneCAN::handle_velocity(const float vx, const float vy, const float vz) { - if (!uavcan::isNaN(vx)) { + if (!isnanf(vx)) { const Vector3f vel(vx, vy, vz); interim_state.velocity = vel; velocity_to_speed_course(interim_state); @@ -389,28 +333,28 @@ void AP_GPS_UAVCAN::handle_velocity(const float vx, const float vy, const float } } -void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) +void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec) { bool process = false; seen_fix2 = true; WITH_SEMAPHORE(sem); - if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_NO_FIX) { + if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX) { interim_state.status = AP_GPS::GPS_Status::NO_FIX; } else { - if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_TIME_ONLY) { + if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_TIME_ONLY) { interim_state.status = AP_GPS::GPS_Status::NO_FIX; - } else if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_2D_FIX) { + } else if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX) { interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_2D; process = true; - } else if (cb.msg->status == uavcan::equipment::gnss::Fix2::STATUS_3D_FIX) { + } else if (msg.status == UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX) { interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D; process = true; } - if (cb.msg->gnss_time_standard == uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC) { - uint64_t epoch_ms = uavcan::UtcTime(cb.msg->gnss_timestamp).toUSec(); + if (msg.gnss_time_standard == UAVCAN_EQUIPMENT_GNSS_FIX2_GNSS_TIME_STANDARD_UTC) { + uint64_t epoch_ms = msg.gnss_timestamp.usec; if (epoch_ms != 0) { epoch_ms /= 1000; uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC; @@ -420,12 +364,12 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) } if (interim_state.status == AP_GPS::GPS_Status::GPS_OK_FIX_3D) { - if (cb.msg->mode == uavcan::equipment::gnss::Fix2::MODE_DGPS) { + if (msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS) { interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS; - } else if (cb.msg->mode == uavcan::equipment::gnss::Fix2::MODE_RTK) { - if (cb.msg->sub_mode == uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT) { + } else if (msg.mode == UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK) { + if (msg.sub_mode == UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT) { interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT; - } else if (cb.msg->sub_mode == uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED) { + } else if (msg.sub_mode == UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED) { interim_state.status = AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED; } } @@ -434,39 +378,39 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) if (process) { Location loc = { }; - loc.lat = cb.msg->latitude_deg_1e8 / 10; - loc.lng = cb.msg->longitude_deg_1e8 / 10; - loc.alt = cb.msg->height_msl_mm / 10; + loc.lat = msg.latitude_deg_1e8 / 10; + loc.lng = msg.longitude_deg_1e8 / 10; + loc.alt = msg.height_msl_mm / 10; interim_state.have_undulation = true; - interim_state.undulation = (cb.msg->height_msl_mm - cb.msg->height_ellipsoid_mm) * 0.001; + interim_state.undulation = (msg.height_msl_mm - msg.height_ellipsoid_mm) * 0.001; interim_state.location = loc; - handle_velocity(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]); + handle_velocity(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]); - if (cb.msg->covariance.size() == 6) { - if (!uavcan::isNaN(cb.msg->covariance[0])) { - interim_state.horizontal_accuracy = sqrtf(cb.msg->covariance[0]); + if (msg.covariance.len == 6) { + if (!isnanf(msg.covariance.data[0])) { + interim_state.horizontal_accuracy = sqrtf(msg.covariance.data[0]); interim_state.have_horizontal_accuracy = true; } else { interim_state.have_horizontal_accuracy = false; } - if (!uavcan::isNaN(cb.msg->covariance[2])) { - interim_state.vertical_accuracy = sqrtf(cb.msg->covariance[2]); + if (!isnanf(msg.covariance.data[2])) { + interim_state.vertical_accuracy = sqrtf(msg.covariance.data[2]); interim_state.have_vertical_accuracy = true; } else { interim_state.have_vertical_accuracy = false; } - if (!uavcan::isNaN(cb.msg->covariance[3]) && - !uavcan::isNaN(cb.msg->covariance[4]) && - !uavcan::isNaN(cb.msg->covariance[5])) { - interim_state.speed_accuracy = sqrtf((cb.msg->covariance[3] + cb.msg->covariance[4] + cb.msg->covariance[5])/3); + if (!isnanf(msg.covariance.data[3]) && + !isnanf(msg.covariance.data[4]) && + !isnanf(msg.covariance.data[5])) { + interim_state.speed_accuracy = sqrtf((msg.covariance.data[3] + msg.covariance.data[4] + msg.covariance.data[5])/3); interim_state.have_speed_accuracy = true; } else { interim_state.have_speed_accuracy = false; } } - interim_state.num_sats = cb.msg->sats_used; + interim_state.num_sats = msg.sats_used; } else { interim_state.have_vertical_velocity = false; interim_state.have_vertical_accuracy = false; @@ -478,18 +422,18 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) if (!seen_aux) { // if we haven't seen an Aux message then populate vdop and // hdop from pdop. Some GPS modules don't provide the Aux message - interim_state.hdop = interim_state.vdop = cb.msg->pdop * 100.0; + interim_state.hdop = interim_state.vdop = msg.pdop * 100.0; } - if ((cb.msg->timestamp.usec > cb.msg->gnss_timestamp.usec) && (cb.msg->gnss_timestamp.usec > 0)) { + if ((msg.timestamp.usec > msg.gnss_timestamp.usec) && (msg.gnss_timestamp.usec > 0)) { // we have a valid timestamp based on gnss_timestamp timescale, we can use that to correct our gps message time - interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(cb.msg->timestamp.usec, (cb.msg->getUtcTimestamp().toUSec() + NATIVE_TIME_OFFSET)); + interim_state.last_corrected_gps_time_us = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, (timestamp_usec + NATIVE_TIME_OFFSET)); interim_state.last_gps_time_ms = interim_state.last_corrected_gps_time_us/1000U; - interim_state.last_corrected_gps_time_us -= cb.msg->timestamp.usec - cb.msg->gnss_timestamp.usec; + interim_state.last_corrected_gps_time_us -= msg.timestamp.usec - msg.gnss_timestamp.usec; // this is also the time the message was received on the UART on other end. interim_state.corrected_timestamp_updated = true; } else { - interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(cb.msg->timestamp.usec, cb.msg->getUtcTimestamp().toUSec() + NATIVE_TIME_OFFSET)/1000U; + interim_state.last_gps_time_ms = jitter_correction.correct_offboard_timestamp_usec(msg.timestamp.usec, timestamp_usec + NATIVE_TIME_OFFSET)/1000U; } #if GPS_PPS_EMULATION @@ -507,7 +451,7 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) static uint64_t next_toggle, last_toggle; - next_toggle = (cb.msg->timestamp.usec) + (1000000ULL - ((cb.msg->timestamp.usec) % 1000000ULL)); + next_toggle = (msg.timestamp.usec) + (1000000ULL - ((msg.timestamp.usec) % 1000000ULL)); next_toggle += jitter_correction.get_link_offset_usec(); if (next_toggle != last_toggle) { @@ -520,7 +464,7 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) if (!seen_message) { if (interim_state.status == AP_GPS::GPS_Status::NO_GPS) { // the first time we see a fix message we change from - // NO_GPS to NO_FIX, indicating to user that a UAVCAN GPS + // NO_GPS to NO_FIX, indicating to user that a DroneCAN GPS // has been seen interim_state.status = AP_GPS::GPS_Status::NO_FIX; } @@ -528,22 +472,22 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb) } } -void AP_GPS_UAVCAN::handle_aux_msg(const AuxCb &cb) +void AP_GPS_DroneCAN::handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg) { WITH_SEMAPHORE(sem); - if (!uavcan::isNaN(cb.msg->hdop)) { + if (!isnanf(msg.hdop)) { seen_aux = true; - interim_state.hdop = cb.msg->hdop * 100.0; + interim_state.hdop = msg.hdop * 100.0; } - if (!uavcan::isNaN(cb.msg->vdop)) { + if (!isnanf(msg.vdop)) { seen_aux = true; - interim_state.vdop = cb.msg->vdop * 100.0; + interim_state.vdop = msg.vdop * 100.0; } } -void AP_GPS_UAVCAN::handle_heading_msg(const HeadingCb &cb) +void AP_GPS_DroneCAN::handle_heading_msg(const ardupilot_gnss_Heading& msg) { #if GPS_MOVING_BASELINE if (seen_relposheading && gps.mb_params[interim_state.instance].type.get() != 0) { @@ -556,33 +500,33 @@ void AP_GPS_UAVCAN::handle_heading_msg(const HeadingCb &cb) WITH_SEMAPHORE(sem); if (interim_state.gps_yaw_configured == false) { - interim_state.gps_yaw_configured = cb.msg->heading_valid; + interim_state.gps_yaw_configured = msg.heading_valid; } - interim_state.have_gps_yaw = cb.msg->heading_valid; - interim_state.gps_yaw = degrees(cb.msg->heading_rad); + interim_state.have_gps_yaw = msg.heading_valid; + interim_state.gps_yaw = degrees(msg.heading_rad); if (interim_state.have_gps_yaw) { interim_state.gps_yaw_time_ms = AP_HAL::millis(); } - interim_state.have_gps_yaw_accuracy = cb.msg->heading_accuracy_valid; - interim_state.gps_yaw_accuracy = degrees(cb.msg->heading_accuracy_rad); + interim_state.have_gps_yaw_accuracy = msg.heading_accuracy_valid; + interim_state.gps_yaw_accuracy = degrees(msg.heading_accuracy_rad); } -void AP_GPS_UAVCAN::handle_status_msg(const StatusCb &cb) +void AP_GPS_DroneCAN::handle_status_msg(const ardupilot_gnss_Status& msg) { WITH_SEMAPHORE(sem); seen_status = true; - healthy = cb.msg->healthy; - status_flags = cb.msg->status; - if (error_code != cb.msg->error_codes) { + healthy = msg.healthy; + status_flags = msg.status; + if (error_code != msg.error_codes) { AP::logger().Write_MessageF("GPS %d: error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1), error_code, - cb.msg->error_codes); - error_code = cb.msg->error_codes; + msg.error_codes); + error_code = msg.error_codes; } } @@ -590,26 +534,26 @@ void AP_GPS_UAVCAN::handle_status_msg(const StatusCb &cb) /* handle moving baseline data. */ -void AP_GPS_UAVCAN::handle_moving_baseline_msg(const MovingBaselineDataCb &cb, uint8_t node_id) +void AP_GPS_DroneCAN::handle_moving_baseline_msg(const ardupilot_gnss_MovingBaselineData& msg, uint8_t node_id) { WITH_SEMAPHORE(sem); if (role != AP_GPS::GPS_ROLE_MB_BASE) { - GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Incorrect Role set for UAVCAN GPS, %d should be Base", node_id); + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Incorrect Role set for DroneCAN GPS, %d should be Base", node_id); return; } if (rtcm3_parser == nullptr) { return; } - for (const auto &c : cb.msg->data) { - rtcm3_parser->read(c); + for (int i=0; i < msg.data.len; i++) { + rtcm3_parser->read(msg.data.data[i]); } } /* handle relposheading message */ -void AP_GPS_UAVCAN::handle_relposheading_msg(const RelPosHeadingCb &cb, uint8_t node_id) +void AP_GPS_DroneCAN::handle_relposheading_msg(const ardupilot_gnss_RelPosHeading& msg, uint8_t node_id) { WITH_SEMAPHORE(sem); @@ -617,18 +561,18 @@ void AP_GPS_UAVCAN::handle_relposheading_msg(const RelPosHeadingCb &cb, uint8_t seen_relposheading = true; // push raw heading data to calculate moving baseline heading states if (calculate_moving_base_yaw(interim_state, - cb.msg->reported_heading_deg, - cb.msg->relative_distance_m, - cb.msg->relative_down_pos_m)) { - if (cb.msg->reported_heading_acc_available) { - interim_state.gps_yaw_accuracy = cb.msg->reported_heading_acc_deg; + msg.reported_heading_deg, + msg.relative_distance_m, + msg.relative_down_pos_m)) { + if (msg.reported_heading_acc_available) { + interim_state.gps_yaw_accuracy = msg.reported_heading_acc_deg; } - interim_state.have_gps_yaw_accuracy = cb.msg->reported_heading_acc_available; + interim_state.have_gps_yaw_accuracy = msg.reported_heading_acc_available; } } // support for retrieving RTCMv3 data from a moving baseline base -bool AP_GPS_UAVCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) +bool AP_GPS_DroneCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) { WITH_SEMAPHORE(sem); if (rtcm3_parser != nullptr) { @@ -639,7 +583,7 @@ bool AP_GPS_UAVCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) } // clear previous RTCM3 packet -void AP_GPS_UAVCAN::clear_RTCMV3(void) +void AP_GPS_DroneCAN::clear_RTCMV3(void) { WITH_SEMAPHORE(sem); if (rtcm3_parser != nullptr) { @@ -649,90 +593,90 @@ void AP_GPS_UAVCAN::clear_RTCMV3(void) #endif // GPS_MOVING_BASELINE -void AP_GPS_UAVCAN::handle_fix2_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Fix2Cb &cb) +void AP_GPS_DroneCAN::handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg) { WITH_SEMAPHORE(_sem_registry); - AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); if (driver != nullptr) { - driver->handle_fix2_msg(cb); + driver->handle_fix2_msg(msg, transfer.timestamp_usec); } } -void AP_GPS_UAVCAN::handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb) +void AP_GPS_DroneCAN::handle_aux_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& msg) { WITH_SEMAPHORE(_sem_registry); - AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); if (driver != nullptr) { - driver->handle_aux_msg(cb); + driver->handle_aux_msg(msg); } } -void AP_GPS_UAVCAN::handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb) +void AP_GPS_DroneCAN::handle_heading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& msg) { WITH_SEMAPHORE(_sem_registry); - AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); if (driver != nullptr) { - driver->handle_heading_msg(cb); + driver->handle_heading_msg(msg); } } -void AP_GPS_UAVCAN::handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const StatusCb &cb) +void AP_GPS_DroneCAN::handle_status_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg) { WITH_SEMAPHORE(_sem_registry); - AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); if (driver != nullptr) { - driver->handle_status_msg(cb); + driver->handle_status_msg(msg); } } #if GPS_MOVING_BASELINE // Moving Baseline msg trampoline -void AP_GPS_UAVCAN::handle_moving_baseline_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MovingBaselineDataCb &cb) +void AP_GPS_DroneCAN::handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg) { WITH_SEMAPHORE(_sem_registry); - AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); if (driver != nullptr) { - driver->handle_moving_baseline_msg(cb, node_id); + driver->handle_moving_baseline_msg(msg, transfer.source_node_id); } } // RelPosHeading msg trampoline -void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const RelPosHeadingCb &cb) +void AP_GPS_DroneCAN::handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg) { WITH_SEMAPHORE(_sem_registry); - AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_GPS_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id); if (driver != nullptr) { - driver->handle_relposheading_msg(cb, node_id); + driver->handle_relposheading_msg(msg, transfer.source_node_id); } } #endif -bool AP_GPS_UAVCAN::do_config() +bool AP_GPS_DroneCAN::do_config() { - AP_UAVCAN *ap_uavcan = _detected_modules[_detected_module].ap_uavcan; - if (ap_uavcan == nullptr) { + AP_DroneCAN *ap_dronecan = _detected_modules[_detected_module].ap_dronecan; + if (ap_dronecan == nullptr) { return false; } uint8_t node_id = _detected_modules[_detected_module].node_id; switch(cfg_step) { case STEP_SET_TYPE: - ap_uavcan->get_parameter_on_node(node_id, "GPS_TYPE", ¶m_int_cb); + ap_dronecan->get_parameter_on_node(node_id, "GPS_TYPE", ¶m_int_cb); break; case STEP_SET_MB_CAN_TX: if (role != AP_GPS::GPS_Role::GPS_ROLE_NORMAL) { - ap_uavcan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", ¶m_int_cb); + ap_dronecan->get_parameter_on_node(node_id, "GPS_MB_ONLY_PORT", ¶m_int_cb); } else { cfg_step++; } break; case STEP_SAVE_AND_REBOOT: if (requires_save_and_reboot) { - ap_uavcan->save_parameters_on_node(node_id, ¶m_save_cb); + ap_dronecan->save_parameters_on_node(node_id, ¶m_save_cb); } else { cfg_step++; } @@ -746,7 +690,7 @@ bool AP_GPS_UAVCAN::do_config() } // Consume new data and mark it received -bool AP_GPS_UAVCAN::read(void) +bool AP_GPS_DroneCAN::read(void) { if (gps._auto_config >= AP_GPS::GPS_AUTO_CONFIG_ENABLE_ALL) { if (!do_config()) { @@ -758,7 +702,7 @@ bool AP_GPS_UAVCAN::read(void) if (_new_data) { _new_data = false; - // the encoding of accuracies in UAVCAN can result in infinite + // the encoding of accuracies in DroneCAN can result in infinite // values. These cause problems with blending. Use 1000m and 1000m/s instead interim_state.horizontal_accuracy = MIN(interim_state.horizontal_accuracy, 1000.0); interim_state.vertical_accuracy = MIN(interim_state.vertical_accuracy, 1000.0); @@ -782,7 +726,7 @@ bool AP_GPS_UAVCAN::read(void) return false; } -bool AP_GPS_UAVCAN::is_healthy(void) const +bool AP_GPS_DroneCAN::is_healthy(void) const { // if we don't have any health reports, assume it's healthy if (!seen_status) { @@ -791,47 +735,47 @@ bool AP_GPS_UAVCAN::is_healthy(void) const return healthy; } -bool AP_GPS_UAVCAN::logging_healthy(void) const +bool AP_GPS_DroneCAN::logging_healthy(void) const { // if we don't have status, assume it's valid if (!seen_status) { return true; } - return (status_flags & ardupilot::gnss::Status::STATUS_LOGGING) != 0; + return (status_flags & ARDUPILOT_GNSS_STATUS_STATUS_LOGGING) != 0; } -bool AP_GPS_UAVCAN::is_configured(void) const +bool AP_GPS_DroneCAN::is_configured(void) const { // if we don't have status assume it's configured if (!seen_status) { return true; } - return (status_flags & ardupilot::gnss::Status::STATUS_ARMABLE) != 0; + return (status_flags & ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE) != 0; } /* handle RTCM data from MAVLink GPS_RTCM_DATA, forwarding it over MAVLink */ -void AP_GPS_UAVCAN::inject_data(const uint8_t *data, uint16_t len) +void AP_GPS_DroneCAN::inject_data(const uint8_t *data, uint16_t len) { - // we only handle this if we are the first UAVCAN GPS or we are + // we only handle this if we are the first DroneCAN GPS or we are // using a different uavcan instance than the first GPS, as we - // send the data as broadcast on all UAVCAN devive ports and we + // send the data as broadcast on all DroneCAN devive ports and we // don't want to send duplicates if (_detected_module == 0 || - _detected_modules[_detected_module].ap_uavcan != _detected_modules[0].ap_uavcan) { - _detected_modules[_detected_module].ap_uavcan->send_RTCMStream(data, len); + _detected_modules[_detected_module].ap_dronecan != _detected_modules[0].ap_dronecan) { + _detected_modules[_detected_module].ap_dronecan->send_RTCMStream(data, len); } } /* handle param get/set response */ -bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, int32_t &value) +bool AP_GPS_DroneCAN::handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, int32_t &value) { - Debug("AP_GPS_UAVCAN: param set/get response from %d %s %ld\n", node_id, name, value); + Debug("AP_GPS_DroneCAN: param set/get response from %d %s %ld\n", node_id, name, value); if (strcmp(name, "GPS_TYPE") == 0 && cfg_step == STEP_SET_TYPE) { if (role == AP_GPS::GPS_ROLE_MB_BASE && value != AP_GPS::GPS_TYPE_UBLOX_RTK_BASE) { value = (int32_t)AP_GPS::GPS_TYPE_UBLOX_RTK_BASE; @@ -866,15 +810,15 @@ bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, uint return false; } -bool AP_GPS_UAVCAN::handle_param_get_set_response_float(AP_UAVCAN* ap_uavcan, uint8_t node_id, const char* name, float &value) +bool AP_GPS_DroneCAN::handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, uint8_t node_id, const char* name, float &value) { - Debug("AP_GPS_UAVCAN: param set/get response from %d %s %f\n", node_id, name, value); + Debug("AP_GPS_DroneCAN: param set/get response from %d %s %f\n", node_id, name, value); return false; } -void AP_GPS_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8_t node_id, bool success) +void AP_GPS_DroneCAN::handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success) { - Debug("AP_GPS_UAVCAN: param save response from %d %s\n", node_id, success ? "success" : "failure"); + Debug("AP_GPS_DroneCAN: param save response from %d %s\n", node_id, success ? "success" : "failure"); if (cfg_step != STEP_SAVE_AND_REBOOT) { return; @@ -884,16 +828,16 @@ void AP_GPS_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8 cfg_step++; } // Also send reboot command - // this is ok as we are sending from UAVCAN thread context - Debug("AP_GPS_UAVCAN: sending reboot command %d\n", node_id); - ap_uavcan->send_reboot_request(node_id); + // this is ok as we are sending from DroneCAN thread context + Debug("AP_GPS_DroneCAN: sending reboot command %d\n", node_id); + ap_dronecan->send_reboot_request(node_id); } #if AP_DRONECAN_SEND_GPS -bool AP_GPS_UAVCAN::instance_exists(const AP_UAVCAN* ap_uavcan) +bool AP_GPS_DroneCAN::instance_exists(const AP_DroneCAN* ap_dronecan) { for (uint8_t i=0; i #include +#if HAL_ENABLE_DRONECAN_DRIVERS #include "AP_GPS.h" #include "GPS_Backend.h" #include "RTCM3_Parser.h" -#include +#include -class FixCb; -class Fix2Cb; -class AuxCb; -class HeadingCb; -class StatusCb; -#if GPS_MOVING_BASELINE -class MovingBaselineDataCb; -class RelPosHeadingCb; -#endif - -class AP_GPS_UAVCAN : public AP_GPS_Backend { +class AP_GPS_DroneCAN : public AP_GPS_Backend { public: - AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role); - ~AP_GPS_UAVCAN(); + AP_GPS_DroneCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role); + ~AP_GPS_DroneCAN(); bool read() override; @@ -50,16 +41,17 @@ class AP_GPS_UAVCAN : public AP_GPS_Backend { const char *name() const override { return _name; } - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state); - static void handle_fix2_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const Fix2Cb &cb); - static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb); - static void handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb); - static void handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const StatusCb &cb); + static void handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg); + + static void handle_aux_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& msg); + static void handle_heading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& msg); + static void handle_status_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg); #if GPS_MOVING_BASELINE - static void handle_moving_baseline_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MovingBaselineDataCb &cb); - static void handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const RelPosHeadingCb &cb); + static void handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg); + static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg); #endif static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len); void inject_data(const uint8_t *data, uint16_t len) override; @@ -72,7 +64,7 @@ class AP_GPS_UAVCAN : public AP_GPS_Backend { #endif #if AP_DRONECAN_SEND_GPS - static bool instance_exists(const AP_UAVCAN* ap_uavcan); + static bool instance_exists(const AP_DroneCAN* ap_dronecan); #endif private: @@ -90,20 +82,20 @@ class AP_GPS_UAVCAN : public AP_GPS_Backend { // returns true once configuration has finished bool do_config(void); - void handle_fix2_msg(const Fix2Cb &cb); - void handle_aux_msg(const AuxCb &cb); - void handle_heading_msg(const HeadingCb &cb); - void handle_status_msg(const StatusCb &cb); + void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec); + void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg); + void handle_heading_msg(const ardupilot_gnss_Heading& msg); + void handle_status_msg(const ardupilot_gnss_Status& msg); void handle_velocity(const float vx, const float vy, const float vz); #if GPS_MOVING_BASELINE - void handle_moving_baseline_msg(const MovingBaselineDataCb &cb, uint8_t node_id); - void handle_relposheading_msg(const RelPosHeadingCb &cb, uint8_t node_id); + void handle_moving_baseline_msg(const ardupilot_gnss_MovingBaselineData& msg, uint8_t node_id); + void handle_relposheading_msg(const ardupilot_gnss_RelPosHeading& msg, uint8_t node_id); #endif static bool take_registry(); static void give_registry(); - static AP_GPS_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id); + static AP_GPS_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id); bool _new_data; AP_GPS::GPS_State interim_state; @@ -120,14 +112,14 @@ class AP_GPS_UAVCAN : public AP_GPS_Backend { bool healthy; uint32_t status_flags; uint32_t error_code; - char _name[15]; + char _name[16]; // Module Detection Registry static struct DetectedModules { - AP_UAVCAN* ap_uavcan; + AP_DroneCAN* ap_dronecan; uint8_t node_id; uint8_t instance; - AP_GPS_UAVCAN* driver; + AP_GPS_DroneCAN* driver; } _detected_modules[GPS_MAX_RECEIVERS]; static HAL_Semaphore _sem_registry; @@ -139,11 +131,12 @@ class AP_GPS_UAVCAN : public AP_GPS_Backend { // the role set from GPS_TYPE AP_GPS::GPS_Role role; - FUNCTOR_DECLARE(param_int_cb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &); - FUNCTOR_DECLARE(param_float_cb, bool, AP_UAVCAN*, const uint8_t, const char*, float &); - FUNCTOR_DECLARE(param_save_cb, void, AP_UAVCAN*, const uint8_t, bool); + FUNCTOR_DECLARE(param_int_cb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &); + FUNCTOR_DECLARE(param_float_cb, bool, AP_DroneCAN*, const uint8_t, const char*, float &); + FUNCTOR_DECLARE(param_save_cb, void, AP_DroneCAN*, const uint8_t, bool); - bool handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, int32_t &value); - bool handle_param_get_set_response_float(AP_UAVCAN* ap_uavcan, const uint8_t node_id, const char* name, float &value); - void handle_param_save_response(AP_UAVCAN* ap_uavcan, const uint8_t node_id, bool success); + bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value); + bool handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, float &value); + void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success); }; +#endif diff --git a/libraries/AP_GPS/MovingBase.h b/libraries/AP_GPS/MovingBase.h index 77de2a02fd4d8..7df25fe0a2ae6 100644 --- a/libraries/AP_GPS/MovingBase.h +++ b/libraries/AP_GPS/MovingBase.h @@ -18,6 +18,6 @@ class MovingBase { }; AP_Int8 type; // an option from MovingBaseType - AP_Vector3f base_offset; // base position offset from the selected GPS reciever + AP_Vector3f base_offset; // base position offset from the selected GPS receiver }; diff --git a/libraries/AP_Gripper/AP_Gripper_EPM.cpp b/libraries/AP_Gripper/AP_Gripper_EPM.cpp index 574d6ff99c296..5b55219c3a24e 100644 --- a/libraries/AP_Gripper/AP_Gripper_EPM.cpp +++ b/libraries/AP_Gripper/AP_Gripper_EPM.cpp @@ -32,7 +32,7 @@ void AP_Gripper_EPM::init_gripper() #ifdef UAVCAN_NODE_FILE _uavcan_fd = ::open(UAVCAN_NODE_FILE, O_CLOEXEC); // https://ardupilot.org/dev/docs/learning-ardupilot-uarts-and-the-console.html - ::printf("EPM: UAVCAN fd %d\n", _uavcan_fd); + ::printf("EPM: DroneCAN fd %d\n", _uavcan_fd); #endif // initialise the EPM to the neutral position diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 3934350e3a3e9..d6159d7e45d52 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -218,8 +218,12 @@ #define HAL_CANMANAGER_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS > 0) #endif -#ifndef HAL_ENABLE_LIBUAVCAN_DRIVERS -#define HAL_ENABLE_LIBUAVCAN_DRIVERS HAL_CANMANAGER_ENABLED +#ifndef HAL_ENABLE_DRONECAN_DRIVERS +#define HAL_ENABLE_DRONECAN_DRIVERS HAL_CANMANAGER_ENABLED +#endif + +#ifndef AP_TEST_DRONECAN_DRIVERS +#define AP_TEST_DRONECAN_DRIVERS 0 #endif #ifndef AP_AIRSPEED_BACKEND_DEFAULT_ENABLED @@ -241,10 +245,12 @@ #define HAL_HAVE_DUAL_USB_CDC 0 #endif +#ifndef AP_CAN_SLCAN_ENABLED #if HAL_NUM_CAN_IFACES && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#define AP_UAVCAN_SLCAN_ENABLED 1 +#define AP_CAN_SLCAN_ENABLED 1 #else -#define AP_UAVCAN_SLCAN_ENABLED 0 +#define AP_CAN_SLCAN_ENABLED 0 +#endif #endif #ifndef USE_LIBC_REALLOC diff --git a/libraries/AP_HAL/CANIface.cpp b/libraries/AP_HAL/CANIface.cpp index e5a712390d1eb..1bc85bc969f74 100644 --- a/libraries/AP_HAL/CANIface.cpp +++ b/libraries/AP_HAL/CANIface.cpp @@ -98,14 +98,16 @@ AP_HAL::CANFrame::CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t dat id(can_id), canfd(canfd_frame) { - if ((can_data == nullptr) || (data_len == 0) || (data_len > MaxDataLen)) { + const uint8_t data_len_max = canfd_frame ? MaxDataLen : NonFDCANMaxDataLen; + if ((can_data == nullptr) || (data_len == 0) || (data_len > data_len_max)) { + dlc = 0; + memset(data, 0, MaxDataLen); return; } memcpy(this->data, can_data, data_len); - if (data_len <= 8) { + memset(&this->data[data_len], 0, MaxDataLen-data_len); + if (data_len <= NonFDCANMaxDataLen) { dlc = data_len; - } else if (!canfd) { - dlc = 8; } else { /* Data Length Code 9 10 11 12 13 14 15 diff --git a/libraries/AP_HAL/board/esp32.h b/libraries/AP_HAL/board/esp32.h index 8be292c0d633c..560920fc09c8b 100644 --- a/libraries/AP_HAL/board/esp32.h +++ b/libraries/AP_HAL/board/esp32.h @@ -13,7 +13,7 @@ #define HAL_BOARD_NAME "ESP32" #define HAL_CPU_CLASS HAL_CPU_CLASS_150 -#define HAL_WITH_UAVCAN 0 +#define HAL_WITH_DRONECAN 0 #define HAL_HAVE_SAFETY_SWITCH 0 #define HAL_HAVE_BOARD_VOLTAGE 0 #define HAL_HAVE_SERVO_VOLTAGE 0 diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index c36205d73759c..58e714b1f73c7 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -396,3 +396,10 @@ #ifndef HAL_GYROFFT_ENABLED #define HAL_GYROFFT_ENABLED 0 #endif + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NONE +// we can use virtual CAN on native builds +#define HAL_LINUX_USE_VIRTUAL_CAN 1 +#else +#define HAL_LINUX_USE_VIRTUAL_CAN 0 +#endif diff --git a/libraries/AP_HAL/board/sitl.h b/libraries/AP_HAL/board/sitl.h index 883d0a814a03d..3d288adc25a56 100644 --- a/libraries/AP_HAL/board/sitl.h +++ b/libraries/AP_HAL/board/sitl.h @@ -72,3 +72,7 @@ #ifndef HAL_WITH_EKF_DOUBLE #define HAL_WITH_EKF_DOUBLE HAL_HAVE_HARDWARE_DOUBLE #endif + +#ifndef HAL_CAN_DRIVER_DEFAULT +#define HAL_CAN_DRIVER_DEFAULT 1 +#endif diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp index cc50bbacad0aa..e05a09883eafa 100644 --- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp @@ -31,7 +31,7 @@ #include "hal.h" static const struct I2CInfo { - struct I2CDriver *i2c; + I2CDriver *i2c; uint8_t instance; uint8_t dma_channel_rx; uint8_t dma_channel_tx; diff --git a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp index 4a7b7e681147e..b0afeccfd924e 100644 --- a/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/QSPIDevice.cpp @@ -45,16 +45,19 @@ static const struct QSPIDriverInfo { QSPIDesc QSPIDeviceManager::device_table[] = { HAL_QSPI_DEVICE_LIST }; // Check clock sanity during runtime -#define XSTR(x) STR(x) -#define STR(x) #x - +#if (STM32_QSPICLK < HAL_QSPI1_CLK) +#error "Flash speed must not be greater than QSPI Clock" +#endif #if (STM32_QSPICLK % HAL_QSPI1_CLK) -#error "QSPI Clock" XSTR(STM32_QSPICLK) " needs to be divisible by selected clock" XSTR(HAL_QSPI1_CLK) +#warning "QSPI clock not an integer multiple of flash speed" #endif #if defined(STM32_WSPI_USE_QUADSPI2) && STM32_WSPI_USE_QUADSPI2 +#if (STM32_QSPICLK < HAL_QSPI2_CLK) +#error "Flash speed must not be greater than QSPI Clock" +#endif #if (STM32_QSPICLK % HAL_QSPI2_CLK) -#error "QSPI Clock" XSTR(STM32_QSPICLK) " needs to be divisible by selected clock" XSTR(HAL_QSPI2_CLK) +#warning "QSPI clock not an integer multiple of flash speed" #endif #endif diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 53e2b002d084a..1acb4a43545ea 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -212,7 +212,7 @@ void RCOutput::rcout_thread() } } -__RAMFUNC__ void RCOutput::dshot_update_tick(void* p) +__RAMFUNC__ void RCOutput::dshot_update_tick(virtual_timer_t* vt, void* p) { chSysLockFromISR(); RCOutput* rcout = (RCOutput*)p; @@ -1582,7 +1582,9 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length) up with this great method. */ TOGGLE_PIN_DEBUG(54); - +#if STM32_DMA_SUPPORTS_DMAMUX + dmaSetRequestSource(group.dma, group.dma_up_channel); +#endif dmaStreamSetPeripheral(group.dma, &(group.pwm_drv->tim->DMAR)); stm32_cacheBufferFlush(group.dma_buffer, buffer_length); dmaStreamSetMemory0(group.dma, group.dma_buffer); @@ -1612,7 +1614,7 @@ void RCOutput::send_pulses_DMAR(pwm_group &group, uint32_t buffer_length) /* unlock DMA channel after a dshot send completes and no return value is expected */ -__RAMFUNC__ void RCOutput::dma_unlock(void *p) +__RAMFUNC__ void RCOutput::dma_unlock(virtual_timer_t* vt, void *p) { chSysLockFromISR(); pwm_group *group = (pwm_group *)p; @@ -1656,9 +1658,15 @@ void RCOutput::dma_cancel(pwm_group& group) chSysLock(); dmaStreamDisable(group.dma); #ifdef HAL_WITH_BIDIR_DSHOT - if (group.ic_dma_enabled()) { + if (group.ic_dma_enabled() && !group.has_shared_ic_up_dma()) { dmaStreamDisable(group.bdshot.ic_dma[group.bdshot.curr_telem_chan]); } +#if STM32_DMA_SUPPORTS_DMAMUX + // the DMA request source has been switched by the receive path, so reinstate the correct one + if (group.dshot_state == DshotState::RECV_START && group.has_shared_ic_up_dma()) { + dmaSetRequestSource(group.dma, group.dma_up_channel); + } +#endif #endif // normally the CCR registers are reset by the final 0 in the DMA buffer // since we are cancelling early they need to be reset to avoid infinite pulses @@ -1879,7 +1887,7 @@ void RCOutput::serial_bit_irq(void) /* timeout a byte read */ -void RCOutput::serial_byte_timeout(void *ctx) +void RCOutput::serial_byte_timeout(virtual_timer_t* vt, void *ctx) { chSysLockFromISR(); irq.timed_out = true; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 6ee05e85cd1fd..c259929704a8c 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -602,12 +602,12 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput void dshot_send_groups(uint64_t time_out_us); void dshot_send(pwm_group &group, uint64_t time_out_us); bool dshot_send_command(pwm_group &group, uint8_t command, uint8_t chan); - static void dshot_update_tick(void* p); + static void dshot_update_tick(virtual_timer_t*, void* p); static void dshot_send_next_group(void* p); // release locks on the groups that are pending in reverse order void dshot_collect_dma_locks(uint64_t last_run_us); static void dma_up_irq_callback(void *p, uint32_t flags); - static void dma_unlock(void *p); + static void dma_unlock(virtual_timer_t*, void *p); void dma_cancel(pwm_group& group); bool mode_requires_dma(enum output_mode mode) const; bool setup_group_DMA(pwm_group &group, uint32_t bitrate, uint32_t bit_width, bool active_high, @@ -627,7 +627,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan); static uint8_t bdshot_find_next_ic_channel(const pwm_group& group); static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags); - static void bdshot_finish_dshot_gcr_transaction(void *p); + static void bdshot_finish_dshot_gcr_transaction(virtual_timer_t* vt, void *p); bool bdshot_setup_group_ic_DMA(pwm_group &group); static void bdshot_receive_pulses_DMAR(pwm_group* group); static void bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t ccr_ch); @@ -650,7 +650,7 @@ class ChibiOS::RCOutput : public AP_HAL::RCOutput bool serial_read_byte(uint8_t &b); void fill_DMA_buffer_byte(uint32_t *buffer, uint8_t stride, uint8_t b , uint32_t bitval); static void serial_bit_irq(void); - static void serial_byte_timeout(void *ctx); + static void serial_byte_timeout(virtual_timer_t* vt, void *ctx); }; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index fea8b8f16b97f..33d6f2808cc81 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -372,7 +372,7 @@ void RCOutput::bdshot_config_icu_dshot(stm32_tim_t* TIMx, uint8_t chan, uint8_t /* unlock DMA channel after a bi-directional dshot transaction completes */ -__RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(void *p) +__RAMFUNC__ void RCOutput::bdshot_finish_dshot_gcr_transaction(virtual_timer_t* vt, void *p) { pwm_group *group = (pwm_group *)p; chSysLockFromISR(); diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp index ae1b8da471d2c..9b0dd2c3c3c19 100644 --- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp +++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp @@ -363,9 +363,9 @@ bool SPIDevice::acquire_bus(bool set, bool skip_cs) } else { bus.dma_handle->lock(); spiAcquireBus(spi_devices[device_desc.bus].driver); /* Acquire ownership of the bus. */ - bus.spicfg.end_cb = nullptr; bus.spicfg.ssport = PAL_PORT(device_desc.pal_line); bus.spicfg.sspad = PAL_PAD(device_desc.pal_line); + bus.spicfg.end_cb = nullptr; #if defined(STM32H7) bus.spicfg.cfg1 = freq_flag; bus.spicfg.cfg2 = device_desc.mode; diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index be69b92479101..81c8c0d92c9ea 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -417,7 +417,7 @@ bool Util::was_watchdog_reset() const __RAMFUNC__ void Util::thread_info(ExpandingString &str) { #if HAL_ENABLE_THREAD_STATISTICS - uint64_t cumulative_cycles = ch.kernel_stats.m_crit_isr.cumulative; + uint64_t cumulative_cycles = currcore->kernel_stats.m_crit_isr.cumulative; for (thread_t *tp = chRegFirstThread(); tp; tp = chRegNextThread(tp)) { if (tp->stats.best > 0) { // not run cumulative_cycles += (uint64_t)tp->stats.cumulative; @@ -430,8 +430,8 @@ __RAMFUNC__ void Util::thread_info(ExpandingString &str) str.printf("ThreadsV2\nISR PRI=255 sp=%p STACK=%u/%u LOAD=%4.1f%%\n", &__main_stack_base__, unsigned(stack_free(&__main_stack_base__)), - unsigned(isr_stack_size), 100.0f * float(ch.kernel_stats.m_crit_isr.cumulative) / float(cumulative_cycles)); - ch.kernel_stats.m_crit_isr.cumulative = 0U; + unsigned(isr_stack_size), 100.0f * float(currcore->kernel_stats.m_crit_isr.cumulative) / float(cumulative_cycles)); + currcore->kernel_stats.m_crit_isr.cumulative = 0U; #else str.printf("ThreadsV2\nISR PRI=255 sp=%p STACK=%u/%u\n", &__main_stack_base__, diff --git a/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef-bl.dat index 87c992d08e273..7284b25fbc529 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef-bl.dat @@ -29,15 +29,15 @@ PE8 UART7_TX UART7 NODMA define BOOTLOADER_DEBUG SD7 # QuadSPI Flash -PD11 QUADSPI_BK1_IO0 QUADSPI1 -PD12 QUADSPI_BK1_IO1 QUADSPI1 -PE2 QUADSPI_BK1_IO2 QUADSPI1 -PD13 QUADSPI_BK1_IO3 QUADSPI1 -PB6 QUADSPI_BK1_NCS QUADSPI1 -PB2 QUADSPI_CLK QUADSPI1 +PD11 QUADSPI_BK1_IO0 QUADSPI1 SPEED_HIGH +PD12 QUADSPI_BK1_IO1 QUADSPI1 SPEED_HIGH +PE2 QUADSPI_BK1_IO2 QUADSPI1 SPEED_HIGH +PD13 QUADSPI_BK1_IO3 QUADSPI1 SPEED_HIGH +PB6 QUADSPI_BK1_NCS QUADSPI1 SPEED_HIGH +PB2 QUADSPI_CLK QUADSPI1 SPEED_HIGH # IFace Device Name Bus QSPI Mode Clk Freq Size (Pow2) NCS Delay -QSPIDEV w25q QUADSPI1 MODE3 120*MHZ 23 2 +QSPIDEV w25q QUADSPI1 MODE3 100*MHZ 23 1 # USART1 PA10 USART1_RX USART1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef.dat index 2a8bc1d34a807..ebd3003de9afb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/DevEBoxH7v2/hwdef.dat @@ -102,12 +102,12 @@ PC12 SDMMC1_CK SDMMC1 PD2 SDMMC1_CMD SDMMC1 # QuadSPI Flash -PD11 QUADSPI_BK1_IO0 QUADSPI1 -PD12 QUADSPI_BK1_IO1 QUADSPI1 -PE2 QUADSPI_BK1_IO2 QUADSPI1 -PD13 QUADSPI_BK1_IO3 QUADSPI1 -PB6 QUADSPI_BK1_NCS QUADSPI1 -PB2 QUADSPI_CLK QUADSPI1 +PD11 QUADSPI_BK1_IO0 QUADSPI1 SPEED_HIGH +PD12 QUADSPI_BK1_IO1 QUADSPI1 SPEED_HIGH +PE2 QUADSPI_BK1_IO2 QUADSPI1 SPEED_HIGH +PD13 QUADSPI_BK1_IO3 QUADSPI1 SPEED_HIGH +PB6 QUADSPI_BK1_NCS QUADSPI1 SPEED_HIGH +PB2 QUADSPI_CLK QUADSPI1 SPEED_HIGH # Motors PA0 TIM5_CH1 TIM5 PWM(1) GPIO(50) # M1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat index 97617a3192af4..ce9c39a20559b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat @@ -58,7 +58,7 @@ PA14 JTCK-SWCLK SWD define USB_USE_WAIT TRUE -define HAL_USE_USB_MSD TRUE +define HAL_USE_USB_MSD 1 define HAL_DEVICE_THREAD_STACK 2048 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat index f81a6003814da..7b7644417c0e1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat @@ -70,7 +70,7 @@ define HAL_NO_MONITOR_THREAD define HAL_DISABLE_LOOP_DELAY define USB_USE_WAIT TRUE -define HAL_USE_USB_MSD TRUE +define HAL_USE_USB_MSD 1 define HAL_DEVICE_THREAD_STACK 2048 @@ -142,10 +142,10 @@ PD2 SDMMC1_CMD SDMMC1 define BOARD_SER1_RTSCTS_DEFAULT 0 define HAL_OS_FATFS_IO 1 -define HAL_WITH_UAVCAN 1 -env HAL_WITH_UAVCAN 1 +define HAL_WITH_DRONECAN 1 +env HAL_WITH_DRONECAN 1 define HAL_PICCOLO_CAN_ENABLE 0 -define AP_UAVCAN_SLCAN_ENABLED 1 +define AP_CAN_SLCAN_ENABLED 1 PA1 ANALOG_CAN_VOLTAGE1 ADC1 SCALE(21.404) PA2 ANALOG_CAN_VOLTAGE2 ADC1 SCALE(21.404) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat index 81c2f5e50449e..07270c07b5561 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JHEMCU-GSF405A/hwdef.dat @@ -161,7 +161,7 @@ define HAL_PARACHUTE_ENABLED 0 define HAL_SPRAYER_ENABLED 0 define HAL_GENERATOR_ENABLED 0 define AC_OAPATHPLANNER_ENABLED 0 -define PRECISION_LANDING 0 +define AC_PRECLAND_ENABLED 0 define HAL_BARO_WIND_COMP_ENABLED 0 define AP_GRIPPER_ENABLED 0 define HAL_HOTT_TELEM_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef-bl.dat index 1f19d9056cb28..783bb4f089b6c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef-bl.dat @@ -23,22 +23,9 @@ SERIAL_ORDER OTG1 USART1 PA9 USART1_TX USART1 PA10 USART1_RX USART1 -PA2 USART2_TX USART2 -PA3 USART2_RX USART2 - -PC10 USART3_TX USART3 -PC11 USART3_RX USART3 - -PA0 UART4_TX UART4 -PA1 UART4_RX UART4 - -PC12 UART5_TX UART5 -PD2 UART5_RX UART5 - PA11 OTG_FS_DM OTG1 PA12 OTG_FS_DP OTG1 - # Add CS pins to ensure they are high in bootloader PA4 MPU_CS CS PB12 MAG_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat index 127439f7c8a2b..985bc0cbd1394 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-CAN/hwdef.dat @@ -7,7 +7,7 @@ MCU STM32F4xx STM32F405xx FLASH_RESERVE_START_KB 64 FLASH_SIZE_KB 1024 -# store parameters in pages 11 and 12 +# store parameters in pages 2 and 3 STORAGE_FLASH_PAGE 1 define HAL_STORAGE_SIZE 15360 @@ -82,7 +82,7 @@ PB9 CAN1_TX CAN1 PC13 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70) # reduce memory usage -define UAVCAN_NODE_POOL_SIZE 1024 +define DRONECAN_NODE_POOL_SIZE 1024 # --------------------- UARTs --------------------------- diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE-bdshot/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE-bdshot/hwdef-bl.dat new file mode 100644 index 0000000000000..581d05154ec2d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE-bdshot/hwdef-bl.dat @@ -0,0 +1 @@ +include ../MatekF405-TE/hwdef-bl.dat diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE-bdshot/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE-bdshot/hwdef.dat new file mode 100644 index 0000000000000..aea8d620fed48 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MatekF405-TE-bdshot/hwdef.dat @@ -0,0 +1,17 @@ +# hw definition file for MATEK F405 VTOL + +include ../MatekF405-TE/hwdef.dat + +undef PC9 PC8 PB15 PA8 PB11 PB10 PB3 PA15 + +# --------------------- PWM ----------------------- +PC9 TIM8_CH4 TIM8 PWM(1) GPIO(50) BIDIR +PC8 TIM8_CH3 TIM8 PWM(2) GPIO(51) +PB15 TIM12_CH2 TIM12 PWM(3) GPIO(52) NODMA +PA8 TIM1_CH1 TIM1 PWM(4) GPIO(53) BIDIR +PB11 TIM2_CH4 TIM2 PWM(5) GPIO(54) +PB10 TIM2_CH3 TIM2 PWM(6) GPIO(55) BIDIR +PB3 TIM2_CH2 TIM2 PWM(7) GPIO(56) BIDIR +PA15 TIM2_CH1 TIM2 PWM(8) GPIO(57) + +DMA_PRIORITY TIM8* TIM1* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat index d1e7543e1a84b..51200e048de84 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef-bl.dat @@ -10,6 +10,8 @@ APJ_BOARD_ID 1060 # crystal frequency, setup to use external oscillator OSCILLATOR_HZ 8000000 +MCU_CLOCKRATE_MHZ 480 + # limited flash that can't be used FLASH_SIZE_KB 128 FLASH_RESERVE_START_KB 0 @@ -39,15 +41,15 @@ PE3 LED_BOOTLOADER OUTPUT LOW # Red LED define HAL_LED_ON 0 # QuadSPI Flash -PD11 QUADSPI_BK1_IO0 QUADSPI1 -PD12 QUADSPI_BK1_IO1 QUADSPI1 -PE2 QUADSPI_BK1_IO2 QUADSPI1 -PD13 QUADSPI_BK1_IO3 QUADSPI1 -PB10 QUADSPI_BK1_NCS QUADSPI1 -PB2 QUADSPI_CLK QUADSPI1 +PD11 QUADSPI_BK1_IO0 QUADSPI1 SPEED_HIGH +PD12 QUADSPI_BK1_IO1 QUADSPI1 SPEED_HIGH +PE2 QUADSPI_BK1_IO2 QUADSPI1 SPEED_HIGH +PD13 QUADSPI_BK1_IO3 QUADSPI1 SPEED_HIGH +PB10 QUADSPI_BK1_NCS QUADSPI1 SPEED_HIGH +PB2 QUADSPI_CLK QUADSPI1 SPEED_HIGH # IFace Device Name Bus QSPI Mode Clk Freq Size (Pow2) NCS Delay -QSPIDEV w25q-dtr QUADSPI1 MODE3 120*MHZ 24 2 +QSPIDEV w25q-dtr QUADSPI1 MODE3 100*MHZ 24 1 define HAL_USE_EMPTY_STORAGE 1 define HAL_STORAGE_SIZE 16384 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat index 13538e105fa55..43b6901a0706b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/SPRacingH7/hwdef.dat @@ -127,12 +127,12 @@ PC12 SDMMC1_CK SDMMC1 PD2 SDMMC1_CMD SDMMC1 # QuadSPI Flash -PD11 QUADSPI_BK1_IO0 QUADSPI1 -PD12 QUADSPI_BK1_IO1 QUADSPI1 -PE2 QUADSPI_BK1_IO2 QUADSPI1 -PD13 QUADSPI_BK1_IO3 QUADSPI1 -PB10 QUADSPI_BK1_NCS QUADSPI1 -PB2 QUADSPI_CLK QUADSPI1 +PD11 QUADSPI_BK1_IO0 QUADSPI1 SPEED_HIGH +PD12 QUADSPI_BK1_IO1 QUADSPI1 SPEED_HIGH +PE2 QUADSPI_BK1_IO2 QUADSPI1 SPEED_HIGH +PD13 QUADSPI_BK1_IO3 QUADSPI1 SPEED_HIGH +PB10 QUADSPI_BK1_NCS QUADSPI1 SPEED_HIGH +PB2 QUADSPI_CLK QUADSPI1 SPEED_HIGH # Motors PA0 TIM5_CH1 TIM5 PWM(1) GPIO(50) BIDIR # M1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc index e95f668d4622f..119173883bd16 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-16MHz/H7-16MHz.ioc @@ -237,6 +237,12 @@ Dma.SPI4_TX.5.SyncEnable=DISABLE Dma.SPI4_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT Dma.SPI4_TX.5.SyncRequestNumber=1 Dma.SPI4_TX.5.SyncSignalID=NONE +FDCAN1.CalculateBaudRateNominal=1000000 +FDCAN1.CalculateTimeQuantumNominal=200.0 +FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal +FDCAN2.CalculateBaudRateNominal=1000000 +FDCAN2.CalculateTimeQuantumNominal=200.0 +FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal File.Version=6 I2C1.IPParameters=Timing I2C1.Timing=0x00707CBB @@ -245,27 +251,29 @@ I2C2.Timing=0x00707CBB I2C4.IPParameters=Timing I2C4.Timing=0x00707CBB KeepUserPlacement=false +Mcu.CPN=STM32H743ZIT6 Mcu.Family=STM32H7 Mcu.IP0=ADC1 Mcu.IP1=BDMA -Mcu.IP10=RCC -Mcu.IP11=RTC -Mcu.IP12=SDMMC1 -Mcu.IP13=SPI1 -Mcu.IP14=SPI2 -Mcu.IP15=SPI4 -Mcu.IP16=SPI5 -Mcu.IP17=SPI6 -Mcu.IP18=SYS -Mcu.IP19=TIM1 +Mcu.IP10=QUADSPI +Mcu.IP11=RCC +Mcu.IP12=RTC +Mcu.IP13=SDMMC1 +Mcu.IP14=SPI1 +Mcu.IP15=SPI2 +Mcu.IP16=SPI4 +Mcu.IP17=SPI5 +Mcu.IP18=SPI6 +Mcu.IP19=SYS Mcu.IP2=CORTEX_M7 -Mcu.IP20=UART4 -Mcu.IP21=UART7 -Mcu.IP22=UART8 -Mcu.IP23=USART2 -Mcu.IP24=USART3 -Mcu.IP25=USART6 -Mcu.IP26=USB_OTG_FS +Mcu.IP20=TIM1 +Mcu.IP21=UART4 +Mcu.IP22=UART7 +Mcu.IP23=UART8 +Mcu.IP24=USART2 +Mcu.IP25=USART3 +Mcu.IP26=USART6 +Mcu.IP27=USB_OTG_FS Mcu.IP3=DMA Mcu.IP4=FDCAN1 Mcu.IP5=FDCAN2 @@ -273,95 +281,101 @@ Mcu.IP6=I2C1 Mcu.IP7=I2C2 Mcu.IP8=I2C4 Mcu.IP9=NVIC -Mcu.IPNb=27 +Mcu.IPNb=28 Mcu.Name=STM32H743ZITx Mcu.Package=LQFP144 Mcu.Pin0=PE2 Mcu.Pin1=PE5 -Mcu.Pin10=PH1-OSC_OUT (PH1) -Mcu.Pin11=PC1 -Mcu.Pin12=PC2_C -Mcu.Pin13=PA0 -Mcu.Pin14=PA1 -Mcu.Pin15=PA2 -Mcu.Pin16=PA3 -Mcu.Pin17=PA5 -Mcu.Pin18=PA6 -Mcu.Pin19=PA7 +Mcu.Pin10=PH0-OSC_IN (PH0) +Mcu.Pin11=PH1-OSC_OUT (PH1) +Mcu.Pin12=PC1 +Mcu.Pin13=PC2_C +Mcu.Pin14=PA0 +Mcu.Pin15=PA1 +Mcu.Pin16=PA2 +Mcu.Pin17=PA3 +Mcu.Pin18=PA5 +Mcu.Pin19=PA6 Mcu.Pin2=PE6 -Mcu.Pin20=PC4 -Mcu.Pin21=PF11 -Mcu.Pin22=PF14 -Mcu.Pin23=PF15 -Mcu.Pin24=PE8 -Mcu.Pin25=PE9 -Mcu.Pin26=PB10 -Mcu.Pin27=PB11 -Mcu.Pin28=PB12 -Mcu.Pin29=PB13 +Mcu.Pin20=PA7 +Mcu.Pin21=PC4 +Mcu.Pin22=PF11 +Mcu.Pin23=PF14 +Mcu.Pin24=PF15 +Mcu.Pin25=PE7 +Mcu.Pin26=PE8 +Mcu.Pin27=PE9 +Mcu.Pin28=PE12 +Mcu.Pin29=PB10 Mcu.Pin3=PF0 -Mcu.Pin30=PC6 -Mcu.Pin31=PC7 -Mcu.Pin32=PC8 -Mcu.Pin33=PC9 -Mcu.Pin34=PA9 -Mcu.Pin35=PA11 -Mcu.Pin36=PA12 -Mcu.Pin37=PC10 -Mcu.Pin38=PC11 -Mcu.Pin39=PC12 +Mcu.Pin30=PB11 +Mcu.Pin31=PB12 +Mcu.Pin32=PB13 +Mcu.Pin33=PD8 +Mcu.Pin34=PD11 +Mcu.Pin35=PD12 +Mcu.Pin36=PC6 +Mcu.Pin37=PC7 +Mcu.Pin38=PC8 +Mcu.Pin39=PC9 Mcu.Pin4=PF1 -Mcu.Pin40=PD0 -Mcu.Pin41=PD1 -Mcu.Pin42=PD2 -Mcu.Pin43=PD7 -Mcu.Pin44=PG9 -Mcu.Pin45=PG11 -Mcu.Pin46=PG12 -Mcu.Pin47=PB6 -Mcu.Pin48=PB7 -Mcu.Pin49=PE0 +Mcu.Pin40=PA9 +Mcu.Pin41=PA11 +Mcu.Pin42=PA12 +Mcu.Pin43=PC10 +Mcu.Pin44=PC11 +Mcu.Pin45=PC12 +Mcu.Pin46=PD0 +Mcu.Pin47=PD1 +Mcu.Pin48=PD2 +Mcu.Pin49=PD7 Mcu.Pin5=PF6 -Mcu.Pin50=PE1 -Mcu.Pin51=VP_RTC_VS_RTC_Activate -Mcu.Pin52=VP_SYS_VS_Systick +Mcu.Pin50=PG9 +Mcu.Pin51=PG11 +Mcu.Pin52=PG12 +Mcu.Pin53=PB6 +Mcu.Pin54=PB7 +Mcu.Pin55=PE0 +Mcu.Pin56=PE1 +Mcu.Pin57=VP_RTC_VS_RTC_Activate +Mcu.Pin58=VP_SYS_VS_Systick Mcu.Pin6=PF7 Mcu.Pin7=PF8 Mcu.Pin8=PF9 -Mcu.Pin9=PH0-OSC_IN (PH0) -Mcu.PinsNb=53 +Mcu.Pin9=PF10 +Mcu.PinsNb=59 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32H743ZITx -MxCube.Version=5.6.1 -MxDb.Version=DB.5.0.60 -NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false +MxCube.Version=6.6.1 +MxDb.Version=DB.6.0.60 +NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.ForceEnableDMAVector=true -NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 -NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false PA0.Mode=Asynchronous PA0.Signal=UART4_TX PA1.Mode=Asynchronous @@ -381,16 +395,20 @@ PA7.Mode=Full_Duplex_Master PA7.Signal=SPI6_MOSI PA9.Mode=Full_Duplex_Master PA9.Signal=SPI2_SCK -PB10.Mode=Asynchronous -PB10.Signal=USART3_TX +PB10.Mode=Single Bank 1 +PB10.Signal=QUADSPI_BK1_NCS PB11.Mode=Asynchronous PB11.Signal=USART3_RX -PB12.Mode=FDCANSlave +PB12.Mode=FDCAN_Activate PB12.Signal=FDCAN2_RX -PB13.Mode=FDCANSlave +PB13.Mode=FDCAN_Activate PB13.Signal=FDCAN2_TX +PB6.GPIOParameters=GPIO_Pu +PB6.GPIO_Pu=GPIO_PULLUP PB6.Mode=I2C PB6.Signal=I2C1_SCL +PB7.GPIOParameters=GPIO_Pu +PB7.GPIO_Pu=GPIO_PULLUP PB7.Mode=I2C PB7.Signal=I2C1_SDA PC1.Mode=Full_Duplex_Master @@ -412,39 +430,59 @@ PC8.Mode=SD_4_bits_Wide_bus PC8.Signal=SDMMC1_D0 PC9.Mode=SD_4_bits_Wide_bus PC9.Signal=SDMMC1_D1 -PD0.Mode=FDCANMaster +PD0.Mode=FDCAN_Activate PD0.Signal=FDCAN1_RX -PD1.Mode=FDCANMaster +PD1.Mode=FDCAN_Activate PD1.Signal=FDCAN1_TX +PD11.Mode=Single Bank 1 +PD11.Signal=QUADSPI_BK1_IO0 +PD12.Mode=Single Bank 1 +PD12.Signal=QUADSPI_BK1_IO1 PD2.Mode=SD_4_bits_Wide_bus PD2.Signal=SDMMC1_CMD PD7.Mode=Full_Duplex_Master PD7.Signal=SPI1_MOSI +PD8.Mode=Asynchronous +PD8.Signal=USART3_TX PE0.Mode=Asynchronous PE0.Signal=UART8_RX PE1.Mode=Asynchronous PE1.Signal=UART8_TX -PE2.Mode=Full_Duplex_Master -PE2.Signal=SPI4_SCK +PE12.Mode=Full_Duplex_Master +PE12.Signal=SPI4_SCK +PE2.Mode=Single Bank 1 +PE2.Signal=QUADSPI_BK1_IO2 PE5.Mode=Full_Duplex_Master PE5.Signal=SPI4_MISO PE6.Mode=Full_Duplex_Master PE6.Signal=SPI4_MOSI +PE7.Mode=Asynchronous +PE7.Signal=UART7_RX PE8.Mode=Asynchronous PE8.Signal=UART7_TX PE9.Signal=S_TIM1_CH1 +PF0.GPIOParameters=GPIO_Pu +PF0.GPIO_Pu=GPIO_PULLUP PF0.Mode=I2C PF0.Signal=I2C2_SDA +PF1.GPIOParameters=GPIO_Pu +PF1.GPIO_Pu=GPIO_PULLUP PF1.Mode=I2C PF1.Signal=I2C2_SCL +PF10.Mode=Single Bank 1 +PF10.Signal=QUADSPI_CLK PF11.Mode=IN2-Single-Ended PF11.Signal=ADC1_INP2 +PF14.GPIOParameters=GPIO_Pu +PF14.GPIO_Pu=GPIO_PULLUP PF14.Mode=I2C PF14.Signal=I2C4_SCL +PF15.GPIOParameters=GPIO_Pu +PF15.GPIO_Pu=GPIO_PULLUP PF15.Mode=I2C PF15.Signal=I2C4_SDA -PF6.Mode=Asynchronous -PF6.Signal=UART7_RX +PF6.Mode=Single Bank 1 +PF6.Signal=QUADSPI_BK1_IO3 PF7.Mode=Full_Duplex_Master PF7.Signal=SPI5_SCK PF8.Mode=Full_Duplex_Master @@ -471,7 +509,7 @@ ProjectManager.CustomerFirmwarePackage= ProjectManager.DefaultFWLocation=true ProjectManager.DeletePrevious=true ProjectManager.DeviceId=STM32H743ZITx -ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.7.0 +ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.10.0 ProjectManager.FreePins=false ProjectManager.HalAssertFull=false ProjectManager.HeapSize=0x2000 @@ -516,21 +554,22 @@ RCC.DIVM1=2 RCC.DIVM2=2 RCC.DIVM3=4 RCC.DIVN1=100 -RCC.DIVN2=45 +RCC.DIVN2=75 RCC.DIVN3=72 RCC.DIVP1Freq_Value=400000000 -RCC.DIVP2Freq_Value=180000000 +RCC.DIVP2=3 +RCC.DIVP2Freq_Value=200000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=96000000 RCC.DIVQ1=10 RCC.DIVQ1Freq_Value=80000000 -RCC.DIVQ2=5 -RCC.DIVQ2Freq_Value=72000000 +RCC.DIVQ2=6 +RCC.DIVQ2Freq_Value=100000000 RCC.DIVQ3=6 RCC.DIVQ3Freq_Value=48000000 RCC.DIVR1Freq_Value=400000000 -RCC.DIVR2=1 -RCC.DIVR2Freq_Value=360000000 +RCC.DIVR2=3 +RCC.DIVR2Freq_Value=200000000 RCC.DIVR3=9 RCC.DIVR3Freq_Value=32000000 RCC.FDCANFreq_Value=80000000 @@ -546,22 +585,25 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=32000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=32000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL1_VCO_SEL-AdvancedSettings,PLL2_VCI_Range-AdvancedSettings,PLL2_VCO_SEL-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2I2SEnable-ClockTree,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL1_VCO_SEL-AdvancedSettings,PLL2_VCI_Range-AdvancedSettings,PLL2_VCO_SEL-AdvancedSettings,PLL3FRACN,PLL3_VCI_Range-AdvancedSettings,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPICLockSelection,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFEnable-ClockTree,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6CLockSelection,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Spi45ClockSelection,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16CLockSelection,USART16Freq_Value,USART234578CLockSelection,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 RCC.LPUART1Freq_Value=100000000 RCC.LTDCFreq_Value=32000000 RCC.MCO1PinFreq_Value=16000000 +RCC.MCO2I2SEnable-ClockTree=true RCC.MCO2PinFreq_Value=400000000 RCC.PLL1_VCI_Range-AdvancedSettings=RCC_PLL1VCIRANGE_0 RCC.PLL1_VCO_SEL-AdvancedSettings=RCC_PLL1VCOMEDIUM RCC.PLL2_VCI_Range-AdvancedSettings=RCC_PLL2VCIRANGE_0 RCC.PLL2_VCO_SEL-AdvancedSettings=RCC_PLL2VCOMEDIUM RCC.PLL3FRACN=0 +RCC.PLL3_VCI_Range-AdvancedSettings=RCC_PLL3VCIRANGE_0 RCC.PLLFRACN=0 RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 +RCC.QSPICLockSelection=RCC_QSPICLKSOURCE_PLL2 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 @@ -570,22 +612,27 @@ RCC.SAI23Freq_Value=80000000 RCC.SAI4AFreq_Value=80000000 RCC.SAI4BFreq_Value=80000000 RCC.SDMMCFreq_Value=80000000 +RCC.SPDIFEnable-ClockTree=true RCC.SPDIFRXFreq_Value=80000000 RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 +RCC.SPI6CLockSelection=RCC_SPI6CLKSOURCE_PLL2 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 RCC.SYSCLKFreq_VALUE=400000000 RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.Spi45ClockSelection=RCC_SPI45CLKSOURCE_PLL2 RCC.Tim1OutputFreq_Value=200000000 RCC.Tim2OutputFreq_Value=200000000 RCC.TraceFreq_Value=16000000 +RCC.USART16CLockSelection=RCC_USART16CLKSOURCE_PLL2 RCC.USART16Freq_Value=100000000 +RCC.USART234578CLockSelection=RCC_USART234578CLKSOURCE_PLL2 RCC.USART234578Freq_Value=100000000 RCC.USBCLockSelection=RCC_USBCLKSOURCE_PLL3 RCC.USBFreq_Value=48000000 RCC.VCO1OutputFreq_Value=800000000 -RCC.VCO2OutputFreq_Value=360000000 +RCC.VCO2OutputFreq_Value=600000000 RCC.VCO3OutputFreq_Value=288000000 RCC.VCOInput1Freq_Value=8000000 RCC.VCOInput2Freq_Value=8000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc index 721b69d23ec4d..92d86b18b3282 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-24MHz/H7-24MHz.ioc @@ -237,6 +237,12 @@ Dma.SPI4_TX.5.SyncEnable=DISABLE Dma.SPI4_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT Dma.SPI4_TX.5.SyncRequestNumber=1 Dma.SPI4_TX.5.SyncSignalID=NONE +FDCAN1.CalculateBaudRateNominal=1000000 +FDCAN1.CalculateTimeQuantumNominal=200.0 +FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal +FDCAN2.CalculateBaudRateNominal=1000000 +FDCAN2.CalculateTimeQuantumNominal=200.0 +FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal File.Version=6 I2C1.IPParameters=Timing I2C1.Timing=0x00707CBB @@ -245,27 +251,29 @@ I2C2.Timing=0x00707CBB I2C4.IPParameters=Timing I2C4.Timing=0x00707CBB KeepUserPlacement=false +Mcu.CPN=STM32H743ZIT6 Mcu.Family=STM32H7 Mcu.IP0=ADC1 Mcu.IP1=BDMA -Mcu.IP10=RCC -Mcu.IP11=RTC -Mcu.IP12=SDMMC1 -Mcu.IP13=SPI1 -Mcu.IP14=SPI2 -Mcu.IP15=SPI4 -Mcu.IP16=SPI5 -Mcu.IP17=SPI6 -Mcu.IP18=SYS -Mcu.IP19=TIM1 +Mcu.IP10=QUADSPI +Mcu.IP11=RCC +Mcu.IP12=RTC +Mcu.IP13=SDMMC1 +Mcu.IP14=SPI1 +Mcu.IP15=SPI2 +Mcu.IP16=SPI4 +Mcu.IP17=SPI5 +Mcu.IP18=SPI6 +Mcu.IP19=SYS Mcu.IP2=CORTEX_M7 -Mcu.IP20=UART4 -Mcu.IP21=UART7 -Mcu.IP22=UART8 -Mcu.IP23=USART2 -Mcu.IP24=USART3 -Mcu.IP25=USART6 -Mcu.IP26=USB_OTG_FS +Mcu.IP20=TIM1 +Mcu.IP21=UART4 +Mcu.IP22=UART7 +Mcu.IP23=UART8 +Mcu.IP24=USART2 +Mcu.IP25=USART3 +Mcu.IP26=USART6 +Mcu.IP27=USB_OTG_FS Mcu.IP3=DMA Mcu.IP4=FDCAN1 Mcu.IP5=FDCAN2 @@ -273,95 +281,101 @@ Mcu.IP6=I2C1 Mcu.IP7=I2C2 Mcu.IP8=I2C4 Mcu.IP9=NVIC -Mcu.IPNb=27 +Mcu.IPNb=28 Mcu.Name=STM32H743ZITx Mcu.Package=LQFP144 Mcu.Pin0=PE2 Mcu.Pin1=PE5 -Mcu.Pin10=PH1-OSC_OUT (PH1) -Mcu.Pin11=PC1 -Mcu.Pin12=PC2_C -Mcu.Pin13=PA0 -Mcu.Pin14=PA1 -Mcu.Pin15=PA2 -Mcu.Pin16=PA3 -Mcu.Pin17=PA5 -Mcu.Pin18=PA6 -Mcu.Pin19=PA7 +Mcu.Pin10=PH0-OSC_IN (PH0) +Mcu.Pin11=PH1-OSC_OUT (PH1) +Mcu.Pin12=PC1 +Mcu.Pin13=PC2_C +Mcu.Pin14=PA0 +Mcu.Pin15=PA1 +Mcu.Pin16=PA2 +Mcu.Pin17=PA3 +Mcu.Pin18=PA5 +Mcu.Pin19=PA6 Mcu.Pin2=PE6 -Mcu.Pin20=PC4 -Mcu.Pin21=PF11 -Mcu.Pin22=PF14 -Mcu.Pin23=PF15 -Mcu.Pin24=PE8 -Mcu.Pin25=PE9 -Mcu.Pin26=PB10 -Mcu.Pin27=PB11 -Mcu.Pin28=PB12 -Mcu.Pin29=PB13 +Mcu.Pin20=PA7 +Mcu.Pin21=PC4 +Mcu.Pin22=PF11 +Mcu.Pin23=PF14 +Mcu.Pin24=PF15 +Mcu.Pin25=PE7 +Mcu.Pin26=PE8 +Mcu.Pin27=PE9 +Mcu.Pin28=PE12 +Mcu.Pin29=PB10 Mcu.Pin3=PF0 -Mcu.Pin30=PC6 -Mcu.Pin31=PC7 -Mcu.Pin32=PC8 -Mcu.Pin33=PC9 -Mcu.Pin34=PA9 -Mcu.Pin35=PA11 -Mcu.Pin36=PA12 -Mcu.Pin37=PC10 -Mcu.Pin38=PC11 -Mcu.Pin39=PC12 +Mcu.Pin30=PB11 +Mcu.Pin31=PB12 +Mcu.Pin32=PB13 +Mcu.Pin33=PD8 +Mcu.Pin34=PD11 +Mcu.Pin35=PD12 +Mcu.Pin36=PC6 +Mcu.Pin37=PC7 +Mcu.Pin38=PC8 +Mcu.Pin39=PC9 Mcu.Pin4=PF1 -Mcu.Pin40=PD0 -Mcu.Pin41=PD1 -Mcu.Pin42=PD2 -Mcu.Pin43=PD7 -Mcu.Pin44=PG9 -Mcu.Pin45=PG11 -Mcu.Pin46=PG12 -Mcu.Pin47=PB6 -Mcu.Pin48=PB7 -Mcu.Pin49=PE0 +Mcu.Pin40=PA9 +Mcu.Pin41=PA11 +Mcu.Pin42=PA12 +Mcu.Pin43=PC10 +Mcu.Pin44=PC11 +Mcu.Pin45=PC12 +Mcu.Pin46=PD0 +Mcu.Pin47=PD1 +Mcu.Pin48=PD2 +Mcu.Pin49=PD7 Mcu.Pin5=PF6 -Mcu.Pin50=PE1 -Mcu.Pin51=VP_RTC_VS_RTC_Activate -Mcu.Pin52=VP_SYS_VS_Systick +Mcu.Pin50=PG9 +Mcu.Pin51=PG11 +Mcu.Pin52=PG12 +Mcu.Pin53=PB6 +Mcu.Pin54=PB7 +Mcu.Pin55=PE0 +Mcu.Pin56=PE1 +Mcu.Pin57=VP_RTC_VS_RTC_Activate +Mcu.Pin58=VP_SYS_VS_Systick Mcu.Pin6=PF7 Mcu.Pin7=PF8 Mcu.Pin8=PF9 -Mcu.Pin9=PH0-OSC_IN (PH0) -Mcu.PinsNb=53 +Mcu.Pin9=PF10 +Mcu.PinsNb=59 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32H743ZITx -MxCube.Version=5.6.1 -MxDb.Version=DB.5.0.60 -NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false +MxCube.Version=6.6.1 +MxDb.Version=DB.6.0.60 +NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.ForceEnableDMAVector=true -NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 -NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false PA0.Mode=Asynchronous PA0.Signal=UART4_TX PA1.Mode=Asynchronous @@ -381,16 +395,20 @@ PA7.Mode=Full_Duplex_Master PA7.Signal=SPI6_MOSI PA9.Mode=Full_Duplex_Master PA9.Signal=SPI2_SCK -PB10.Mode=Asynchronous -PB10.Signal=USART3_TX +PB10.Mode=Single Bank 1 +PB10.Signal=QUADSPI_BK1_NCS PB11.Mode=Asynchronous PB11.Signal=USART3_RX -PB12.Mode=FDCANSlave +PB12.Mode=FDCAN_Activate PB12.Signal=FDCAN2_RX -PB13.Mode=FDCANSlave +PB13.Mode=FDCAN_Activate PB13.Signal=FDCAN2_TX +PB6.GPIOParameters=GPIO_Pu +PB6.GPIO_Pu=GPIO_PULLUP PB6.Mode=I2C PB6.Signal=I2C1_SCL +PB7.GPIOParameters=GPIO_Pu +PB7.GPIO_Pu=GPIO_PULLUP PB7.Mode=I2C PB7.Signal=I2C1_SDA PC1.Mode=Full_Duplex_Master @@ -412,39 +430,59 @@ PC8.Mode=SD_4_bits_Wide_bus PC8.Signal=SDMMC1_D0 PC9.Mode=SD_4_bits_Wide_bus PC9.Signal=SDMMC1_D1 -PD0.Mode=FDCANMaster +PD0.Mode=FDCAN_Activate PD0.Signal=FDCAN1_RX -PD1.Mode=FDCANMaster +PD1.Mode=FDCAN_Activate PD1.Signal=FDCAN1_TX +PD11.Mode=Single Bank 1 +PD11.Signal=QUADSPI_BK1_IO0 +PD12.Mode=Single Bank 1 +PD12.Signal=QUADSPI_BK1_IO1 PD2.Mode=SD_4_bits_Wide_bus PD2.Signal=SDMMC1_CMD PD7.Mode=Full_Duplex_Master PD7.Signal=SPI1_MOSI +PD8.Mode=Asynchronous +PD8.Signal=USART3_TX PE0.Mode=Asynchronous PE0.Signal=UART8_RX PE1.Mode=Asynchronous PE1.Signal=UART8_TX -PE2.Mode=Full_Duplex_Master -PE2.Signal=SPI4_SCK +PE12.Mode=Full_Duplex_Master +PE12.Signal=SPI4_SCK +PE2.Mode=Single Bank 1 +PE2.Signal=QUADSPI_BK1_IO2 PE5.Mode=Full_Duplex_Master PE5.Signal=SPI4_MISO PE6.Mode=Full_Duplex_Master PE6.Signal=SPI4_MOSI +PE7.Mode=Asynchronous +PE7.Signal=UART7_RX PE8.Mode=Asynchronous PE8.Signal=UART7_TX PE9.Signal=S_TIM1_CH1 +PF0.GPIOParameters=GPIO_Pu +PF0.GPIO_Pu=GPIO_PULLUP PF0.Mode=I2C PF0.Signal=I2C2_SDA +PF1.GPIOParameters=GPIO_Pu +PF1.GPIO_Pu=GPIO_PULLUP PF1.Mode=I2C PF1.Signal=I2C2_SCL +PF10.Mode=Single Bank 1 +PF10.Signal=QUADSPI_CLK PF11.Mode=IN2-Single-Ended PF11.Signal=ADC1_INP2 +PF14.GPIOParameters=GPIO_Pu +PF14.GPIO_Pu=GPIO_PULLUP PF14.Mode=I2C PF14.Signal=I2C4_SCL +PF15.GPIOParameters=GPIO_Pu +PF15.GPIO_Pu=GPIO_PULLUP PF15.Mode=I2C PF15.Signal=I2C4_SDA -PF6.Mode=Asynchronous -PF6.Signal=UART7_RX +PF6.Mode=Single Bank 1 +PF6.Signal=QUADSPI_BK1_IO3 PF7.Mode=Full_Duplex_Master PF7.Signal=SPI5_SCK PF8.Mode=Full_Duplex_Master @@ -471,7 +509,7 @@ ProjectManager.CustomerFirmwarePackage= ProjectManager.DefaultFWLocation=true ProjectManager.DeletePrevious=true ProjectManager.DeviceId=STM32H743ZITx -ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.7.0 +ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.10.0 ProjectManager.FreePins=false ProjectManager.HalAssertFull=false ProjectManager.HeapSize=0x2000 @@ -489,7 +527,7 @@ ProjectManager.StackSize=0x4000 ProjectManager.TargetToolchain=Makefile ProjectManager.ToolChainLocation= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_BDMA_Init-BDMA-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-SystemClock_Config-RCC-false-HAL-false,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_I2C1_Init-I2C1-false-HAL-true,10-MX_I2C2_Init-I2C2-false-HAL-true,11-MX_I2C4_Init-I2C4-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_USART6_UART_Init-USART6-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_UART7_Init-UART7-false-HAL-true,17-MX_SPI2_Init-SPI2-false-HAL-true,18-MX_SPI4_Init-SPI4-false-HAL-true,19-MX_RTC_Init-RTC-false-HAL-true,20-MX_SPI5_Init-SPI5-false-HAL-true,21-MX_SPI6_Init-SPI6-false-HAL-true,22-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-true,23-MX_FDCAN1_Init-FDCAN1-false-HAL-true,24-MX_FDCAN2_Init-FDCAN2-false-HAL-true,25-MX_UART8_Init-UART8-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true +ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_BDMA_Init-BDMA-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-SystemClock_Config-RCC-false-HAL-false,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_I2C1_Init-I2C1-false-HAL-true,10-MX_I2C2_Init-I2C2-false-HAL-true,11-MX_I2C4_Init-I2C4-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_USART6_UART_Init-USART6-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_UART7_Init-UART7-false-HAL-true,17-MX_SPI2_Init-SPI2-false-HAL-true,18-MX_SPI4_Init-SPI4-false-HAL-true,19-MX_RTC_Init-RTC-false-HAL-true,20-MX_SPI5_Init-SPI5-false-HAL-true,21-MX_SPI6_Init-SPI6-false-HAL-true,22-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-true,23-MX_FDCAN1_Init-FDCAN1-false-HAL-true,24-MX_FDCAN2_Init-FDCAN2-false-HAL-true,25-MX_UART8_Init-UART8-false-HAL-true,26-MX_QUADSPI_Init-QUADSPI-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true RCC.ADCCLockSelection=RCC_ADCCLKSOURCE_PLL3 RCC.ADCFreq_Value=32000000 RCC.AHB12Freq_Value=200000000 @@ -516,21 +554,22 @@ RCC.DIVM1=3 RCC.DIVM2=2 RCC.DIVM3=6 RCC.DIVN1=100 -RCC.DIVN2=30 +RCC.DIVN2=50 RCC.DIVN3=72 RCC.DIVP1Freq_Value=400000000 -RCC.DIVP2Freq_Value=180000000 +RCC.DIVP2=3 +RCC.DIVP2Freq_Value=200000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=96000000 RCC.DIVQ1=10 RCC.DIVQ1Freq_Value=80000000 -RCC.DIVQ2=5 -RCC.DIVQ2Freq_Value=72000000 +RCC.DIVQ2=6 +RCC.DIVQ2Freq_Value=100000000 RCC.DIVQ3=6 RCC.DIVQ3Freq_Value=48000000 RCC.DIVR1Freq_Value=400000000 -RCC.DIVR2=1 -RCC.DIVR2Freq_Value=360000000 +RCC.DIVR2=3 +RCC.DIVR2Freq_Value=200000000 RCC.DIVR3=9 RCC.DIVR3Freq_Value=32000000 RCC.FDCANFreq_Value=80000000 @@ -546,7 +585,7 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=32000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=32000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCO_SEL-AdvancedSettings,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLL3_VCI_Range-AdvancedSettings,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPICLockSelection,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6CLockSelection,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Spi45ClockSelection,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16CLockSelection,USART16Freq_Value,USART234578CLockSelection,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 @@ -554,11 +593,14 @@ RCC.LPUART1Freq_Value=100000000 RCC.LTDCFreq_Value=32000000 RCC.MCO1PinFreq_Value=16000000 RCC.MCO2PinFreq_Value=400000000 +RCC.PLL1_VCO_SEL-AdvancedSettings=RCC_PLL1VCOMEDIUM RCC.PLL2_VCI_Range-AdvancedSettings=RCC_PLL2VCIRANGE_0 RCC.PLL3FRACN=0 +RCC.PLL3_VCI_Range-AdvancedSettings=RCC_PLL3VCIRANGE_0 RCC.PLLFRACN=0 RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 +RCC.QSPICLockSelection=RCC_QSPICLKSOURCE_PLL2 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 @@ -570,19 +612,23 @@ RCC.SDMMCFreq_Value=80000000 RCC.SPDIFRXFreq_Value=80000000 RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 +RCC.SPI6CLockSelection=RCC_SPI6CLKSOURCE_PLL2 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 RCC.SYSCLKFreq_VALUE=400000000 RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.Spi45ClockSelection=RCC_SPI45CLKSOURCE_PLL2 RCC.Tim1OutputFreq_Value=200000000 RCC.Tim2OutputFreq_Value=200000000 RCC.TraceFreq_Value=16000000 +RCC.USART16CLockSelection=RCC_USART16CLKSOURCE_PLL2 RCC.USART16Freq_Value=100000000 +RCC.USART234578CLockSelection=RCC_USART234578CLKSOURCE_PLL2 RCC.USART234578Freq_Value=100000000 RCC.USBCLockSelection=RCC_USBCLKSOURCE_PLL3 RCC.USBFreq_Value=48000000 RCC.VCO1OutputFreq_Value=800000000 -RCC.VCO2OutputFreq_Value=360000000 +RCC.VCO2OutputFreq_Value=600000000 RCC.VCO3OutputFreq_Value=288000000 RCC.VCOInput1Freq_Value=8000000 RCC.VCOInput2Freq_Value=12000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc index f324f458d9fc6..66c4b455d763f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-25MHz/H7-25MHz.ioc @@ -237,6 +237,12 @@ Dma.SPI4_TX.5.SyncEnable=DISABLE Dma.SPI4_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT Dma.SPI4_TX.5.SyncRequestNumber=1 Dma.SPI4_TX.5.SyncSignalID=NONE +FDCAN1.CalculateBaudRateNominal=1000000 +FDCAN1.CalculateTimeQuantumNominal=200.0 +FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal +FDCAN2.CalculateBaudRateNominal=1000000 +FDCAN2.CalculateTimeQuantumNominal=200.0 +FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal File.Version=6 I2C1.IPParameters=Timing I2C1.Timing=0x007074AF @@ -245,27 +251,29 @@ I2C2.Timing=0x007074AF I2C4.IPParameters=Timing I2C4.Timing=0x007074AF KeepUserPlacement=false +Mcu.CPN=STM32H743ZIT6 Mcu.Family=STM32H7 Mcu.IP0=ADC1 Mcu.IP1=BDMA -Mcu.IP10=RCC -Mcu.IP11=RTC -Mcu.IP12=SDMMC1 -Mcu.IP13=SPI1 -Mcu.IP14=SPI2 -Mcu.IP15=SPI4 -Mcu.IP16=SPI5 -Mcu.IP17=SPI6 -Mcu.IP18=SYS -Mcu.IP19=TIM1 +Mcu.IP10=QUADSPI +Mcu.IP11=RCC +Mcu.IP12=RTC +Mcu.IP13=SDMMC1 +Mcu.IP14=SPI1 +Mcu.IP15=SPI2 +Mcu.IP16=SPI4 +Mcu.IP17=SPI5 +Mcu.IP18=SPI6 +Mcu.IP19=SYS Mcu.IP2=CORTEX_M7 -Mcu.IP20=UART4 -Mcu.IP21=UART7 -Mcu.IP22=UART8 -Mcu.IP23=USART2 -Mcu.IP24=USART3 -Mcu.IP25=USART6 -Mcu.IP26=USB_OTG_FS +Mcu.IP20=TIM1 +Mcu.IP21=UART4 +Mcu.IP22=UART7 +Mcu.IP23=UART8 +Mcu.IP24=USART2 +Mcu.IP25=USART3 +Mcu.IP26=USART6 +Mcu.IP27=USB_OTG_FS Mcu.IP3=DMA Mcu.IP4=FDCAN1 Mcu.IP5=FDCAN2 @@ -273,95 +281,101 @@ Mcu.IP6=I2C1 Mcu.IP7=I2C2 Mcu.IP8=I2C4 Mcu.IP9=NVIC -Mcu.IPNb=27 +Mcu.IPNb=28 Mcu.Name=STM32H743ZITx Mcu.Package=LQFP144 Mcu.Pin0=PE2 Mcu.Pin1=PE5 -Mcu.Pin10=PH1-OSC_OUT (PH1) -Mcu.Pin11=PC1 -Mcu.Pin12=PC2_C -Mcu.Pin13=PA0 -Mcu.Pin14=PA1 -Mcu.Pin15=PA2 -Mcu.Pin16=PA3 -Mcu.Pin17=PA5 -Mcu.Pin18=PA6 -Mcu.Pin19=PA7 +Mcu.Pin10=PH0-OSC_IN (PH0) +Mcu.Pin11=PH1-OSC_OUT (PH1) +Mcu.Pin12=PC1 +Mcu.Pin13=PC2_C +Mcu.Pin14=PA0 +Mcu.Pin15=PA1 +Mcu.Pin16=PA2 +Mcu.Pin17=PA3 +Mcu.Pin18=PA5 +Mcu.Pin19=PA6 Mcu.Pin2=PE6 -Mcu.Pin20=PC4 -Mcu.Pin21=PF11 -Mcu.Pin22=PF14 -Mcu.Pin23=PF15 -Mcu.Pin24=PE8 -Mcu.Pin25=PE9 -Mcu.Pin26=PB10 -Mcu.Pin27=PB11 -Mcu.Pin28=PB12 -Mcu.Pin29=PB13 +Mcu.Pin20=PA7 +Mcu.Pin21=PC4 +Mcu.Pin22=PF11 +Mcu.Pin23=PF14 +Mcu.Pin24=PF15 +Mcu.Pin25=PE7 +Mcu.Pin26=PE8 +Mcu.Pin27=PE9 +Mcu.Pin28=PE12 +Mcu.Pin29=PB10 Mcu.Pin3=PF0 -Mcu.Pin30=PC6 -Mcu.Pin31=PC7 -Mcu.Pin32=PC8 -Mcu.Pin33=PC9 -Mcu.Pin34=PA9 -Mcu.Pin35=PA11 -Mcu.Pin36=PA12 -Mcu.Pin37=PC10 -Mcu.Pin38=PC11 -Mcu.Pin39=PC12 +Mcu.Pin30=PB11 +Mcu.Pin31=PB12 +Mcu.Pin32=PB13 +Mcu.Pin33=PD8 +Mcu.Pin34=PD11 +Mcu.Pin35=PD12 +Mcu.Pin36=PC6 +Mcu.Pin37=PC7 +Mcu.Pin38=PC8 +Mcu.Pin39=PC9 Mcu.Pin4=PF1 -Mcu.Pin40=PD0 -Mcu.Pin41=PD1 -Mcu.Pin42=PD2 -Mcu.Pin43=PD7 -Mcu.Pin44=PG9 -Mcu.Pin45=PG11 -Mcu.Pin46=PG12 -Mcu.Pin47=PB6 -Mcu.Pin48=PB7 -Mcu.Pin49=PE0 +Mcu.Pin40=PA9 +Mcu.Pin41=PA11 +Mcu.Pin42=PA12 +Mcu.Pin43=PC10 +Mcu.Pin44=PC11 +Mcu.Pin45=PC12 +Mcu.Pin46=PD0 +Mcu.Pin47=PD1 +Mcu.Pin48=PD2 +Mcu.Pin49=PD7 Mcu.Pin5=PF6 -Mcu.Pin50=PE1 -Mcu.Pin51=VP_RTC_VS_RTC_Activate -Mcu.Pin52=VP_SYS_VS_Systick +Mcu.Pin50=PG9 +Mcu.Pin51=PG11 +Mcu.Pin52=PG12 +Mcu.Pin53=PB6 +Mcu.Pin54=PB7 +Mcu.Pin55=PE0 +Mcu.Pin56=PE1 +Mcu.Pin57=VP_RTC_VS_RTC_Activate +Mcu.Pin58=VP_SYS_VS_Systick Mcu.Pin6=PF7 Mcu.Pin7=PF8 Mcu.Pin8=PF9 -Mcu.Pin9=PH0-OSC_IN (PH0) -Mcu.PinsNb=53 +Mcu.Pin9=PF10 +Mcu.PinsNb=59 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32H743ZITx -MxCube.Version=5.6.1 -MxDb.Version=DB.5.0.60 -NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false +MxCube.Version=6.6.1 +MxDb.Version=DB.6.0.60 +NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.ForceEnableDMAVector=true -NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 -NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false PA0.Mode=Asynchronous PA0.Signal=UART4_TX PA1.Mode=Asynchronous @@ -381,16 +395,20 @@ PA7.Mode=Full_Duplex_Master PA7.Signal=SPI6_MOSI PA9.Mode=Full_Duplex_Master PA9.Signal=SPI2_SCK -PB10.Mode=Asynchronous -PB10.Signal=USART3_TX +PB10.Mode=Single Bank 1 +PB10.Signal=QUADSPI_BK1_NCS PB11.Mode=Asynchronous PB11.Signal=USART3_RX -PB12.Mode=FDCANSlave +PB12.Mode=FDCAN_Activate PB12.Signal=FDCAN2_RX -PB13.Mode=FDCANSlave +PB13.Mode=FDCAN_Activate PB13.Signal=FDCAN2_TX +PB6.GPIOParameters=GPIO_Pu +PB6.GPIO_Pu=GPIO_PULLUP PB6.Mode=I2C PB6.Signal=I2C1_SCL +PB7.GPIOParameters=GPIO_Pu +PB7.GPIO_Pu=GPIO_PULLUP PB7.Mode=I2C PB7.Signal=I2C1_SDA PC1.Mode=Full_Duplex_Master @@ -412,39 +430,65 @@ PC8.Mode=SD_4_bits_Wide_bus PC8.Signal=SDMMC1_D0 PC9.Mode=SD_4_bits_Wide_bus PC9.Signal=SDMMC1_D1 -PD0.Mode=FDCANMaster +PD0.Mode=FDCAN_Activate PD0.Signal=FDCAN1_RX -PD1.Mode=FDCANMaster +PD1.Mode=FDCAN_Activate PD1.Signal=FDCAN1_TX +PD11.Mode=Single Bank 1 +PD11.Signal=QUADSPI_BK1_IO0 +PD12.Mode=Single Bank 1 +PD12.Signal=QUADSPI_BK1_IO1 PD2.Mode=SD_4_bits_Wide_bus PD2.Signal=SDMMC1_CMD PD7.Mode=Full_Duplex_Master PD7.Signal=SPI1_MOSI +PD8.Mode=Asynchronous +PD8.Signal=USART3_TX PE0.Mode=Asynchronous PE0.Signal=UART8_RX PE1.Mode=Asynchronous PE1.Signal=UART8_TX -PE2.Mode=Full_Duplex_Master -PE2.Signal=SPI4_SCK +PE12.GPIOParameters=PinAttribute +PE12.Mode=Full_Duplex_Master +PE12.PinAttribute=Free +PE12.Signal=SPI4_SCK +PE2.Mode=Single Bank 1 +PE2.Signal=QUADSPI_BK1_IO2 PE5.Mode=Full_Duplex_Master PE5.Signal=SPI4_MISO PE6.Mode=Full_Duplex_Master PE6.Signal=SPI4_MOSI +PE7.Mode=Asynchronous +PE7.Signal=UART7_RX +PE8.GPIOParameters=PinAttribute PE8.Mode=Asynchronous +PE8.PinAttribute=Free PE8.Signal=UART7_TX +PE9.GPIOParameters=PinAttribute +PE9.PinAttribute=Free PE9.Signal=S_TIM1_CH1 +PF0.GPIOParameters=GPIO_Pu +PF0.GPIO_Pu=GPIO_PULLUP PF0.Mode=I2C PF0.Signal=I2C2_SDA +PF1.GPIOParameters=GPIO_Pu +PF1.GPIO_Pu=GPIO_PULLUP PF1.Mode=I2C PF1.Signal=I2C2_SCL +PF10.Mode=Single Bank 1 +PF10.Signal=QUADSPI_CLK PF11.Mode=IN2-Single-Ended PF11.Signal=ADC1_INP2 +PF14.GPIOParameters=GPIO_Pu +PF14.GPIO_Pu=GPIO_PULLUP PF14.Mode=I2C PF14.Signal=I2C4_SCL +PF15.GPIOParameters=GPIO_Pu +PF15.GPIO_Pu=GPIO_PULLUP PF15.Mode=I2C PF15.Signal=I2C4_SDA -PF6.Mode=Asynchronous -PF6.Signal=UART7_RX +PF6.Mode=Single Bank 1 +PF6.Signal=QUADSPI_BK1_IO3 PF7.Mode=Full_Duplex_Master PF7.Signal=SPI5_SCK PF8.Mode=Full_Duplex_Master @@ -471,7 +515,7 @@ ProjectManager.CustomerFirmwarePackage= ProjectManager.DefaultFWLocation=true ProjectManager.DeletePrevious=true ProjectManager.DeviceId=STM32H743ZITx -ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.7.0 +ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.10.0 ProjectManager.FreePins=false ProjectManager.HalAssertFull=false ProjectManager.HeapSize=0x2000 @@ -489,7 +533,7 @@ ProjectManager.StackSize=0x4000 ProjectManager.TargetToolchain=Makefile ProjectManager.ToolChainLocation= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_BDMA_Init-BDMA-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-SystemClock_Config-RCC-false-HAL-false,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_I2C1_Init-I2C1-false-HAL-true,10-MX_I2C2_Init-I2C2-false-HAL-true,11-MX_I2C4_Init-I2C4-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_USART6_UART_Init-USART6-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_UART7_Init-UART7-false-HAL-true,17-MX_SPI2_Init-SPI2-false-HAL-true,18-MX_SPI4_Init-SPI4-false-HAL-true,19-MX_RTC_Init-RTC-false-HAL-true,20-MX_SPI5_Init-SPI5-false-HAL-true,21-MX_SPI6_Init-SPI6-false-HAL-true,22-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-true,23-MX_FDCAN1_Init-FDCAN1-false-HAL-true,24-MX_FDCAN2_Init-FDCAN2-false-HAL-true,25-MX_UART8_Init-UART8-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true +ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_BDMA_Init-BDMA-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-SystemClock_Config-RCC-false-HAL-false,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_I2C1_Init-I2C1-false-HAL-true,10-MX_I2C2_Init-I2C2-false-HAL-true,11-MX_I2C4_Init-I2C4-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_USART6_UART_Init-USART6-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_UART7_Init-UART7-false-HAL-true,17-MX_SPI2_Init-SPI2-false-HAL-true,18-MX_SPI4_Init-SPI4-false-HAL-true,19-MX_RTC_Init-RTC-false-HAL-true,20-MX_SPI5_Init-SPI5-false-HAL-true,21-MX_SPI6_Init-SPI6-false-HAL-true,22-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-true,23-MX_FDCAN1_Init-FDCAN1-false-HAL-true,24-MX_FDCAN2_Init-FDCAN2-false-HAL-true,25-MX_UART8_Init-UART8-false-HAL-true,26-MX_QUADSPI_Init-QUADSPI-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true RCC.ADCCLockSelection=RCC_ADCCLKSOURCE_PLL3 RCC.ADCFreq_Value=30000000 RCC.AHB12Freq_Value=200000000 @@ -499,41 +543,62 @@ RCC.APB2Freq_Value=100000000 RCC.APB3Freq_Value=100000000 RCC.APB4Freq_Value=100000000 RCC.AXIClockFreq_Value=200000000 +RCC.CECCLockSelection=RCC_CECCLKSOURCE_LSI RCC.CECFreq_Value=32000 RCC.CKPERFreq_Value=16000000 +RCC.CKPERSourceSelection=RCC_CLKPSOURCE_HSI RCC.CPU2Freq_Value=100000000 RCC.CPU2SystikFreq_Value=100000000 +RCC.CSI_VALUE=4000000 RCC.CortexFreq_Value=400000000 +RCC.Cortex_Div=SYSTICK_CLKSOURCE_HCLK +RCC.Cortex_DivARG=SystemCoreClock/1000 RCC.CpuClockFreq_Value=400000000 +RCC.D1CPRE=RCC_SYSCLK_DIV1 RCC.D1CPREFreq_Value=400000000 RCC.D1PPRE=RCC_APB3_DIV2 RCC.D2PPRE1=RCC_APB1_DIV2 RCC.D2PPRE2=RCC_APB2_DIV2 RCC.D3PPRE=RCC_APB4_DIV2 RCC.DFSDMACLkFreq_Value=80000000 +RCC.DFSDMCLockSelection=RCC_DFSDM1CLKSOURCE_D2PCLK1 RCC.DFSDMFreq_Value=100000000 RCC.DIVM1=2 RCC.DIVM2=5 RCC.DIVM3=5 RCC.DIVN1=64 -RCC.DIVN2=72 +RCC.DIVN2=120 RCC.DIVN3=48 +RCC.DIVP1=2 RCC.DIVP1Freq_Value=400000000 -RCC.DIVP2Freq_Value=180000000 +RCC.DIVP2=3 +RCC.DIVP2Freq_Value=200000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=80000000 RCC.DIVQ1=10 RCC.DIVQ1Freq_Value=80000000 -RCC.DIVQ2=5 -RCC.DIVQ2Freq_Value=72000000 +RCC.DIVQ2=6 +RCC.DIVQ2Freq_Value=100000000 RCC.DIVQ3=5 RCC.DIVQ3Freq_Value=48000000 +RCC.DIVR1=2 RCC.DIVR1Freq_Value=400000000 -RCC.DIVR2=1 -RCC.DIVR2Freq_Value=360000000 +RCC.DIVR2=3 +RCC.DIVR2Freq_Value=200000000 RCC.DIVR3=8 RCC.DIVR3Freq_Value=30000000 +RCC.DSICLockSelection=RCC_DSICLKSOURCE_PHY +RCC.DSIFreq_Value=96000000 +RCC.DSIPHYCLKFreq_Value=96000000 +RCC.DSIPHY_Div=8 +RCC.DSITXEscFreq_Value=20000000 +RCC.DSITX_Div=4 +RCC.DSI_PHY_VALUE=12288000 +RCC.EXTERNAL_CLOCK_VALUE=12288000 +RCC.EnbaleCSS=false +RCC.FDCANCLockSelection=RCC_FDCANCLKSOURCE_PLL RCC.FDCANFreq_Value=80000000 +RCC.FMCCLockSelection=RCC_FMCCLKSOURCE_D1HCLK RCC.FMCFreq_Value=200000000 RCC.FamilyName=M RCC.HCLK3ClockFreq_Value=200000000 @@ -542,51 +607,86 @@ RCC.HPRE=RCC_HCLK_DIV2 RCC.HRTIMFreq_Value=200000000 RCC.HSE_VALUE=25000000 RCC.HSIDiv=RCC_PLLSAIDIVR_4 +RCC.HSI_VALUE=64000000 RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=30000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=30000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECCLockSelection,CECFreq_Value,CKPERFreq_Value,CKPERSourceSelection,CPU2Freq_Value,CPU2SystikFreq_Value,CSI_VALUE,CortexFreq_Value,Cortex_Div,Cortex_DivARG,CpuClockFreq_Value,D1CPRE,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMCLockSelection,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1,DIVP1Freq_Value,DIVP2,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,DSICLockSelection,DSIFreq_Value,DSIPHYCLKFreq_Value,DSIPHY_Div,DSITXEscFreq_Value,DSITX_Div,DSI_PHY_VALUE,EXTERNAL_CLOCK_VALUE,EnbaleCSS,FDCANCLockSelection,FDCANFreq_Value,FMCCLockSelection,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,HSI_VALUE,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1CLockSelection,LPTIM1Freq_Value,LPTIM2CLockSelection,LPTIM2Freq_Value,LPTIM345CLockSelection,LPTIM345Freq_Value,LPUART1CLockSelection,LPUART1Freq_Value,LSEState,LSE_VALUE,LSI_VALUE,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLDSIDev,PLLDSIFreq_Value,PLLDSIIDF,PLLDSIMult,PLLDSINDIV,PLLDSIODF,PLLDSIVCOFreq_Value,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPICLockSelection,QSPIFreq_Value,RC48_VALUE,RCC_MCO1Source,RCC_MCO2Source,RCC_MCODiv1,RCC_MCODiv2,RCC_RTC_Clock_Source_FROM_HSE,RNGCLockSelection,RNGFreq_Value,RTCFreq_Value,SAI1CLockSelection,SAI1Freq_Value,SAI23CLockSelection,SAI23Freq_Value,SAI4ACLockSelection,SAI4AFreq_Value,SAI4BCLockSelection,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFCLockSelection,SPDIFRXFreq_Value,SPI123CLockSelection,SPI123Freq_Value,SPI45Freq_Value,SPI6CLockSelection,SPI6Freq_Value,SWPCLockSelection,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Spi45ClockSelection,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16CLockSelection,USART16Freq_Value,USART234578CLockSelection,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value,WatchDogFreq_Value +RCC.LPTIM1CLockSelection=RCC_LPTIM1CLKSOURCE_D2PCLK1 RCC.LPTIM1Freq_Value=100000000 +RCC.LPTIM2CLockSelection=RCC_LPTIM2CLKSOURCE_D3PCLK1 RCC.LPTIM2Freq_Value=100000000 +RCC.LPTIM345CLockSelection=RCC_LPTIM345CLKSOURCE_D3PCLK1 RCC.LPTIM345Freq_Value=100000000 +RCC.LPUART1CLockSelection=RCC_LPUART1CLKSOURCE_D3PCLK1 RCC.LPUART1Freq_Value=100000000 +RCC.LSEState=RCC_LSE_OFF +RCC.LSE_VALUE=32768 +RCC.LSI_VALUE=32000 RCC.LTDCFreq_Value=30000000 RCC.MCO1PinFreq_Value=16000000 RCC.MCO2PinFreq_Value=400000000 RCC.PLL2_VCI_Range-AdvancedSettings=RCC_PLL2VCIRANGE_0 RCC.PLL3FRACN=0 +RCC.PLLDSIDev=2 +RCC.PLLDSIFreq_Value=500000000 +RCC.PLLDSIIDF=DSI_PLL_IN_DIV1 +RCC.PLLDSIMult=2 +RCC.PLLDSINDIV=20 +RCC.PLLDSIODF=DSI_PLL_OUT_DIV1 +RCC.PLLDSIVCOFreq_Value=500000000 RCC.PLLFRACN=0 RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 +RCC.QSPICLockSelection=RCC_QSPICLKSOURCE_PLL2 RCC.QSPIFreq_Value=200000000 +RCC.RC48_VALUE=48000000 +RCC.RCC_MCO1Source=RCC_MCO1SOURCE_HSI +RCC.RCC_MCO2Source=RCC_MCO2SOURCE_SYSCLK +RCC.RCC_MCODiv1=RCC_MCODIV_1 +RCC.RCC_MCODiv2=RCC_MCODIV_1 +RCC.RCC_RTC_Clock_Source_FROM_HSE=RCC_RTCCLKSOURCE_HSE_DIV2 +RCC.RNGCLockSelection=RCC_RNGCLKSOURCE_HSI48 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 +RCC.SAI1CLockSelection=RCC_SAI1CLKSOURCE_PLL RCC.SAI1Freq_Value=80000000 +RCC.SAI23CLockSelection=RCC_SAI23CLKSOURCE_PLL RCC.SAI23Freq_Value=80000000 +RCC.SAI4ACLockSelection=RCC_SAI4ACLKSOURCE_PLL RCC.SAI4AFreq_Value=80000000 +RCC.SAI4BCLockSelection=RCC_SAI4BCLKSOURCE_PLL RCC.SAI4BFreq_Value=80000000 RCC.SDMMCFreq_Value=80000000 +RCC.SPDIFCLockSelection=RCC_SPDIFRXCLKSOURCE_PLL RCC.SPDIFRXFreq_Value=80000000 +RCC.SPI123CLockSelection=RCC_SPI123CLKSOURCE_PLL RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 +RCC.SPI6CLockSelection=RCC_SPI6CLKSOURCE_PLL2 RCC.SPI6Freq_Value=100000000 +RCC.SWPCLockSelection=RCC_SWPMI1CLKSOURCE_D2PCLK1 RCC.SWPMI1Freq_Value=100000000 RCC.SYSCLKFreq_VALUE=400000000 RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.Spi45ClockSelection=RCC_SPI45CLKSOURCE_PLL2 RCC.Tim1OutputFreq_Value=200000000 RCC.Tim2OutputFreq_Value=200000000 RCC.TraceFreq_Value=16000000 +RCC.USART16CLockSelection=RCC_USART16CLKSOURCE_PLL2 RCC.USART16Freq_Value=100000000 +RCC.USART234578CLockSelection=RCC_USART234578CLKSOURCE_PLL2 RCC.USART234578Freq_Value=100000000 RCC.USBCLockSelection=RCC_USBCLKSOURCE_PLL3 RCC.USBFreq_Value=48000000 RCC.VCO1OutputFreq_Value=800000000 -RCC.VCO2OutputFreq_Value=360000000 +RCC.VCO2OutputFreq_Value=600000000 RCC.VCO3OutputFreq_Value=240000000 RCC.VCOInput1Freq_Value=12500000 RCC.VCOInput2Freq_Value=5000000 RCC.VCOInput3Freq_Value=5000000 +RCC.WatchDogFreq_Value=32000 SH.ADCx_INP3.0=ADC1_INP3,IN3-Single-Ended SH.ADCx_INP3.ConfNb=1 SH.ADCx_INP4.0=ADC1_INP4,IN4-Single-Ended diff --git a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc index 862b90d42d766..2c84b4045a623 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc +++ b/libraries/AP_HAL_ChibiOS/hwdef/STM32CubeConf/H7-8MHz/H7-8MHz.ioc @@ -237,6 +237,12 @@ Dma.SPI4_TX.5.SyncEnable=DISABLE Dma.SPI4_TX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT Dma.SPI4_TX.5.SyncRequestNumber=1 Dma.SPI4_TX.5.SyncSignalID=NONE +FDCAN1.CalculateBaudRateNominal=1000000 +FDCAN1.CalculateTimeQuantumNominal=200.0 +FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal +FDCAN2.CalculateBaudRateNominal=1000000 +FDCAN2.CalculateTimeQuantumNominal=200.0 +FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateBaudRateNominal File.Version=6 I2C1.IPParameters=Timing I2C1.Timing=0x00707CBB @@ -245,27 +251,29 @@ I2C2.Timing=0x00707CBB I2C4.IPParameters=Timing I2C4.Timing=0x00707CBB KeepUserPlacement=false +Mcu.CPN=STM32H743ZIT6 Mcu.Family=STM32H7 Mcu.IP0=ADC1 Mcu.IP1=BDMA -Mcu.IP10=RCC -Mcu.IP11=RTC -Mcu.IP12=SDMMC1 -Mcu.IP13=SPI1 -Mcu.IP14=SPI2 -Mcu.IP15=SPI4 -Mcu.IP16=SPI5 -Mcu.IP17=SPI6 -Mcu.IP18=SYS -Mcu.IP19=TIM1 +Mcu.IP10=QUADSPI +Mcu.IP11=RCC +Mcu.IP12=RTC +Mcu.IP13=SDMMC1 +Mcu.IP14=SPI1 +Mcu.IP15=SPI2 +Mcu.IP16=SPI4 +Mcu.IP17=SPI5 +Mcu.IP18=SPI6 +Mcu.IP19=SYS Mcu.IP2=CORTEX_M7 -Mcu.IP20=UART4 -Mcu.IP21=UART7 -Mcu.IP22=UART8 -Mcu.IP23=USART2 -Mcu.IP24=USART3 -Mcu.IP25=USART6 -Mcu.IP26=USB_OTG_FS +Mcu.IP20=TIM1 +Mcu.IP21=UART4 +Mcu.IP22=UART7 +Mcu.IP23=UART8 +Mcu.IP24=USART2 +Mcu.IP25=USART3 +Mcu.IP26=USART6 +Mcu.IP27=USB_OTG_FS Mcu.IP3=DMA Mcu.IP4=FDCAN1 Mcu.IP5=FDCAN2 @@ -273,95 +281,101 @@ Mcu.IP6=I2C1 Mcu.IP7=I2C2 Mcu.IP8=I2C4 Mcu.IP9=NVIC -Mcu.IPNb=27 +Mcu.IPNb=28 Mcu.Name=STM32H743ZITx Mcu.Package=LQFP144 Mcu.Pin0=PE2 Mcu.Pin1=PE5 -Mcu.Pin10=PH1-OSC_OUT (PH1) -Mcu.Pin11=PC1 -Mcu.Pin12=PC2_C -Mcu.Pin13=PA0 -Mcu.Pin14=PA1 -Mcu.Pin15=PA2 -Mcu.Pin16=PA3 -Mcu.Pin17=PA5 -Mcu.Pin18=PA6 -Mcu.Pin19=PA7 +Mcu.Pin10=PH0-OSC_IN (PH0) +Mcu.Pin11=PH1-OSC_OUT (PH1) +Mcu.Pin12=PC1 +Mcu.Pin13=PC2_C +Mcu.Pin14=PA0 +Mcu.Pin15=PA1 +Mcu.Pin16=PA2 +Mcu.Pin17=PA3 +Mcu.Pin18=PA5 +Mcu.Pin19=PA6 Mcu.Pin2=PE6 -Mcu.Pin20=PC4 -Mcu.Pin21=PF11 -Mcu.Pin22=PF14 -Mcu.Pin23=PF15 -Mcu.Pin24=PE8 -Mcu.Pin25=PE9 -Mcu.Pin26=PB10 -Mcu.Pin27=PB11 -Mcu.Pin28=PB12 -Mcu.Pin29=PB13 +Mcu.Pin20=PA7 +Mcu.Pin21=PC4 +Mcu.Pin22=PF11 +Mcu.Pin23=PF14 +Mcu.Pin24=PF15 +Mcu.Pin25=PE7 +Mcu.Pin26=PE8 +Mcu.Pin27=PE9 +Mcu.Pin28=PE12 +Mcu.Pin29=PB10 Mcu.Pin3=PF0 -Mcu.Pin30=PC6 -Mcu.Pin31=PC7 -Mcu.Pin32=PC8 -Mcu.Pin33=PC9 -Mcu.Pin34=PA9 -Mcu.Pin35=PA11 -Mcu.Pin36=PA12 -Mcu.Pin37=PC10 -Mcu.Pin38=PC11 -Mcu.Pin39=PC12 +Mcu.Pin30=PB11 +Mcu.Pin31=PB12 +Mcu.Pin32=PB13 +Mcu.Pin33=PD8 +Mcu.Pin34=PD11 +Mcu.Pin35=PD12 +Mcu.Pin36=PC6 +Mcu.Pin37=PC7 +Mcu.Pin38=PC8 +Mcu.Pin39=PC9 Mcu.Pin4=PF1 -Mcu.Pin40=PD0 -Mcu.Pin41=PD1 -Mcu.Pin42=PD2 -Mcu.Pin43=PD7 -Mcu.Pin44=PG9 -Mcu.Pin45=PG11 -Mcu.Pin46=PG12 -Mcu.Pin47=PB6 -Mcu.Pin48=PB7 -Mcu.Pin49=PE0 +Mcu.Pin40=PA9 +Mcu.Pin41=PA11 +Mcu.Pin42=PA12 +Mcu.Pin43=PC10 +Mcu.Pin44=PC11 +Mcu.Pin45=PC12 +Mcu.Pin46=PD0 +Mcu.Pin47=PD1 +Mcu.Pin48=PD2 +Mcu.Pin49=PD7 Mcu.Pin5=PF6 -Mcu.Pin50=PE1 -Mcu.Pin51=VP_RTC_VS_RTC_Activate -Mcu.Pin52=VP_SYS_VS_Systick +Mcu.Pin50=PG9 +Mcu.Pin51=PG11 +Mcu.Pin52=PG12 +Mcu.Pin53=PB6 +Mcu.Pin54=PB7 +Mcu.Pin55=PE0 +Mcu.Pin56=PE1 +Mcu.Pin57=VP_RTC_VS_RTC_Activate +Mcu.Pin58=VP_SYS_VS_Systick Mcu.Pin6=PF7 Mcu.Pin7=PF8 Mcu.Pin8=PF9 -Mcu.Pin9=PH0-OSC_IN (PH0) -Mcu.PinsNb=53 +Mcu.Pin9=PF10 +Mcu.PinsNb=59 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32H743ZITx -MxCube.Version=5.6.1 -MxDb.Version=DB.5.0.60 -NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false +MxCube.Version=6.6.1 +MxDb.Version=DB.6.0.60 +NVIC.BDMA_Channel0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BDMA_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.DMA1_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA1_Stream7_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream0_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.ForceEnableDMAVector=true -NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 -NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false -NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false +NVIC.SPI1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SPI6_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false +NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:false +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false PA0.Mode=Asynchronous PA0.Signal=UART4_TX PA1.Mode=Asynchronous @@ -381,16 +395,20 @@ PA7.Mode=Full_Duplex_Master PA7.Signal=SPI6_MOSI PA9.Mode=Full_Duplex_Master PA9.Signal=SPI2_SCK -PB10.Mode=Asynchronous -PB10.Signal=USART3_TX +PB10.Mode=Single Bank 1 +PB10.Signal=QUADSPI_BK1_NCS PB11.Mode=Asynchronous PB11.Signal=USART3_RX -PB12.Mode=FDCANSlave +PB12.Mode=FDCAN_Activate PB12.Signal=FDCAN2_RX -PB13.Mode=FDCANSlave +PB13.Mode=FDCAN_Activate PB13.Signal=FDCAN2_TX +PB6.GPIOParameters=GPIO_Pu +PB6.GPIO_Pu=GPIO_PULLUP PB6.Mode=I2C PB6.Signal=I2C1_SCL +PB7.GPIOParameters=GPIO_Pu +PB7.GPIO_Pu=GPIO_PULLUP PB7.Mode=I2C PB7.Signal=I2C1_SDA PC1.Mode=Full_Duplex_Master @@ -412,39 +430,59 @@ PC8.Mode=SD_4_bits_Wide_bus PC8.Signal=SDMMC1_D0 PC9.Mode=SD_4_bits_Wide_bus PC9.Signal=SDMMC1_D1 -PD0.Mode=FDCANMaster +PD0.Mode=FDCAN_Activate PD0.Signal=FDCAN1_RX -PD1.Mode=FDCANMaster +PD1.Mode=FDCAN_Activate PD1.Signal=FDCAN1_TX +PD11.Mode=Single Bank 1 +PD11.Signal=QUADSPI_BK1_IO0 +PD12.Mode=Single Bank 1 +PD12.Signal=QUADSPI_BK1_IO1 PD2.Mode=SD_4_bits_Wide_bus PD2.Signal=SDMMC1_CMD PD7.Mode=Full_Duplex_Master PD7.Signal=SPI1_MOSI +PD8.Mode=Asynchronous +PD8.Signal=USART3_TX PE0.Mode=Asynchronous PE0.Signal=UART8_RX PE1.Mode=Asynchronous PE1.Signal=UART8_TX -PE2.Mode=Full_Duplex_Master -PE2.Signal=SPI4_SCK +PE12.Mode=Full_Duplex_Master +PE12.Signal=SPI4_SCK +PE2.Mode=Single Bank 1 +PE2.Signal=QUADSPI_BK1_IO2 PE5.Mode=Full_Duplex_Master PE5.Signal=SPI4_MISO PE6.Mode=Full_Duplex_Master PE6.Signal=SPI4_MOSI +PE7.Mode=Asynchronous +PE7.Signal=UART7_RX PE8.Mode=Asynchronous PE8.Signal=UART7_TX PE9.Signal=S_TIM1_CH1 +PF0.GPIOParameters=GPIO_Pu +PF0.GPIO_Pu=GPIO_PULLUP PF0.Mode=I2C PF0.Signal=I2C2_SDA +PF1.GPIOParameters=GPIO_Pu +PF1.GPIO_Pu=GPIO_PULLUP PF1.Mode=I2C PF1.Signal=I2C2_SCL +PF10.Mode=Single Bank 1 +PF10.Signal=QUADSPI_CLK PF11.Mode=IN2-Single-Ended PF11.Signal=ADC1_INP2 +PF14.GPIOParameters=GPIO_Pu +PF14.GPIO_Pu=GPIO_PULLUP PF14.Mode=I2C PF14.Signal=I2C4_SCL +PF15.GPIOParameters=GPIO_Pu +PF15.GPIO_Pu=GPIO_PULLUP PF15.Mode=I2C PF15.Signal=I2C4_SDA -PF6.Mode=Asynchronous -PF6.Signal=UART7_RX +PF6.Mode=Single Bank 1 +PF6.Signal=QUADSPI_BK1_IO3 PF7.Mode=Full_Duplex_Master PF7.Signal=SPI5_SCK PF8.Mode=Full_Duplex_Master @@ -471,7 +509,7 @@ ProjectManager.CustomerFirmwarePackage= ProjectManager.DefaultFWLocation=true ProjectManager.DeletePrevious=true ProjectManager.DeviceId=STM32H743ZITx -ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.7.0 +ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.10.0 ProjectManager.FreePins=false ProjectManager.HalAssertFull=false ProjectManager.HeapSize=0x2000 @@ -489,7 +527,7 @@ ProjectManager.StackSize=0x4000 ProjectManager.TargetToolchain=Makefile ProjectManager.ToolChainLocation= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_BDMA_Init-BDMA-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-SystemClock_Config-RCC-false-HAL-false,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_I2C1_Init-I2C1-false-HAL-true,10-MX_I2C2_Init-I2C2-false-HAL-true,11-MX_I2C4_Init-I2C4-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_USART6_UART_Init-USART6-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_UART7_Init-UART7-false-HAL-true,17-MX_SPI2_Init-SPI2-false-HAL-true,18-MX_SPI4_Init-SPI4-false-HAL-true,19-MX_RTC_Init-RTC-false-HAL-true,20-MX_SPI5_Init-SPI5-false-HAL-true,21-MX_SPI6_Init-SPI6-false-HAL-true,22-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-true,23-MX_FDCAN1_Init-FDCAN1-false-HAL-true,24-MX_FDCAN2_Init-FDCAN2-false-HAL-true,25-MX_UART8_Init-UART8-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true +ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_BDMA_Init-BDMA-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-SystemClock_Config-RCC-false-HAL-false,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_USB_OTG_FS_PCD_Init-USB_OTG_FS-false-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true,9-MX_I2C1_Init-I2C1-false-HAL-true,10-MX_I2C2_Init-I2C2-false-HAL-true,11-MX_I2C4_Init-I2C4-false-HAL-true,12-MX_USART2_UART_Init-USART2-false-HAL-true,13-MX_USART3_UART_Init-USART3-false-HAL-true,14-MX_USART6_UART_Init-USART6-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_UART7_Init-UART7-false-HAL-true,17-MX_SPI2_Init-SPI2-false-HAL-true,18-MX_SPI4_Init-SPI4-false-HAL-true,19-MX_RTC_Init-RTC-false-HAL-true,20-MX_SPI5_Init-SPI5-false-HAL-true,21-MX_SPI6_Init-SPI6-false-HAL-true,22-MX_SDMMC1_SD_Init-SDMMC1-false-HAL-true,23-MX_FDCAN1_Init-FDCAN1-false-HAL-true,24-MX_FDCAN2_Init-FDCAN2-false-HAL-true,25-MX_UART8_Init-UART8-false-HAL-true,26-MX_QUADSPI_Init-QUADSPI-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true RCC.ADCCLockSelection=RCC_ADCCLKSOURCE_PLL3 RCC.ADCFreq_Value=32000000 RCC.AHB12Freq_Value=200000000 @@ -516,23 +554,25 @@ RCC.DIVM1=1 RCC.DIVM2=1 RCC.DIVM3=2 RCC.DIVN1=100 -RCC.DIVN2=45 +RCC.DIVN2=75 RCC.DIVN3=72 RCC.DIVP1Freq_Value=400000000 -RCC.DIVP2Freq_Value=180000000 +RCC.DIVP2=3 +RCC.DIVP2Freq_Value=200000000 RCC.DIVP3=3 RCC.DIVP3Freq_Value=96000000 RCC.DIVQ1=10 RCC.DIVQ1Freq_Value=80000000 -RCC.DIVQ2=5 -RCC.DIVQ2Freq_Value=72000000 +RCC.DIVQ2=6 +RCC.DIVQ2Freq_Value=100000000 RCC.DIVQ3=6 RCC.DIVQ3Freq_Value=48000000 RCC.DIVR1Freq_Value=400000000 -RCC.DIVR2=1 -RCC.DIVR2Freq_Value=360000000 +RCC.DIVR2=3 +RCC.DIVR2Freq_Value=200000000 RCC.DIVR3=9 RCC.DIVR3Freq_Value=32000000 +RCC.EnbaleCSS=false RCC.FDCANFreq_Value=80000000 RCC.FMCFreq_Value=200000000 RCC.FamilyName=M @@ -546,7 +586,7 @@ RCC.I2C123CLockSelection=RCC_I2C123CLKSOURCE_PLL3 RCC.I2C123Freq_Value=32000000 RCC.I2C4CLockSelection=RCC_I2C4CLKSOURCE_PLL3 RCC.I2C4Freq_Value=32000000 -RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Freq_Value,SPI45Freq_Value,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16Freq_Value,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value +RCC.IPParameters=ADCCLockSelection,ADCFreq_Value,AHB12Freq_Value,AHB4Freq_Value,APB1Freq_Value,APB2Freq_Value,APB3Freq_Value,APB4Freq_Value,AXIClockFreq_Value,CECFreq_Value,CKPERFreq_Value,CPU2Freq_Value,CPU2SystikFreq_Value,CortexFreq_Value,CpuClockFreq_Value,D1CPREFreq_Value,D1PPRE,D2PPRE1,D2PPRE2,D3PPRE,DFSDMACLkFreq_Value,DFSDMFreq_Value,DIVM1,DIVM2,DIVM3,DIVN1,DIVN2,DIVN3,DIVP1Freq_Value,DIVP2,DIVP2Freq_Value,DIVP3,DIVP3Freq_Value,DIVQ1,DIVQ1Freq_Value,DIVQ2,DIVQ2Freq_Value,DIVQ3,DIVQ3Freq_Value,DIVR1Freq_Value,DIVR2,DIVR2Freq_Value,DIVR3,DIVR3Freq_Value,EnbaleCSS,FDCANFreq_Value,FMCFreq_Value,FamilyName,HCLK3ClockFreq_Value,HCLKFreq_Value,HPRE,HRTIMFreq_Value,HSE_VALUE,HSIDiv,I2C123CLockSelection,I2C123Freq_Value,I2C4CLockSelection,I2C4Freq_Value,LPTIM1Freq_Value,LPTIM2Freq_Value,LPTIM345Freq_Value,LPUART1Freq_Value,LTDCFreq_Value,MCO1PinFreq_Value,MCO2PinFreq_Value,PLL1_VCI_Range-AdvancedSettings,PLL2_VCI_Range-AdvancedSettings,PLL3FRACN,PLLFRACN,PLLSourceVirtual,PWR_Regulator_Voltage_Scale,QSPICLockSelection,QSPIFreq_Value,RNGFreq_Value,RTCFreq_Value,SAI1Freq_Value,SAI23Freq_Value,SAI4AFreq_Value,SAI4BFreq_Value,SDMMCFreq_Value,SPDIFRXFreq_Value,SPI123Enable-ClockTree,SPI123Freq_Value,SPI45Freq_Value,SPI6CLockSelection,SPI6Freq_Value,SWPMI1Freq_Value,SYSCLKFreq_VALUE,SYSCLKSource,Spi45ClockSelection,Tim1OutputFreq_Value,Tim2OutputFreq_Value,TraceFreq_Value,USART16CLockSelection,USART16Freq_Value,USART234578CLockSelection,USART234578Freq_Value,USBCLockSelection,USBFreq_Value,VCO1OutputFreq_Value,VCO2OutputFreq_Value,VCO3OutputFreq_Value,VCOInput1Freq_Value,VCOInput2Freq_Value,VCOInput3Freq_Value RCC.LPTIM1Freq_Value=100000000 RCC.LPTIM2Freq_Value=100000000 RCC.LPTIM345Freq_Value=100000000 @@ -554,11 +594,13 @@ RCC.LPUART1Freq_Value=100000000 RCC.LTDCFreq_Value=32000000 RCC.MCO1PinFreq_Value=16000000 RCC.MCO2PinFreq_Value=400000000 +RCC.PLL1_VCI_Range-AdvancedSettings=RCC_PLL1VCIRANGE_0 RCC.PLL2_VCI_Range-AdvancedSettings=RCC_PLL2VCIRANGE_0 RCC.PLL3FRACN=0 RCC.PLLFRACN=0 RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE RCC.PWR_Regulator_Voltage_Scale=PWR_REGULATOR_VOLTAGE_SCALE1 +RCC.QSPICLockSelection=RCC_QSPICLKSOURCE_PLL2 RCC.QSPIFreq_Value=200000000 RCC.RNGFreq_Value=48000000 RCC.RTCFreq_Value=32000 @@ -568,21 +610,26 @@ RCC.SAI4AFreq_Value=80000000 RCC.SAI4BFreq_Value=80000000 RCC.SDMMCFreq_Value=80000000 RCC.SPDIFRXFreq_Value=80000000 +RCC.SPI123Enable-ClockTree=true RCC.SPI123Freq_Value=80000000 RCC.SPI45Freq_Value=100000000 +RCC.SPI6CLockSelection=RCC_SPI6CLKSOURCE_PLL2 RCC.SPI6Freq_Value=100000000 RCC.SWPMI1Freq_Value=100000000 RCC.SYSCLKFreq_VALUE=400000000 RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.Spi45ClockSelection=RCC_SPI45CLKSOURCE_PLL2 RCC.Tim1OutputFreq_Value=200000000 RCC.Tim2OutputFreq_Value=200000000 RCC.TraceFreq_Value=16000000 +RCC.USART16CLockSelection=RCC_USART16CLKSOURCE_PLL2 RCC.USART16Freq_Value=100000000 +RCC.USART234578CLockSelection=RCC_USART234578CLKSOURCE_PLL2 RCC.USART234578Freq_Value=100000000 RCC.USBCLockSelection=RCC_USBCLKSOURCE_PLL3 RCC.USBFreq_Value=48000000 RCC.VCO1OutputFreq_Value=800000000 -RCC.VCO2OutputFreq_Value=360000000 +RCC.VCO2OutputFreq_Value=600000000 RCC.VCO3OutputFreq_Value=288000000 RCC.VCOInput1Freq_Value=8000000 RCC.VCOInput2Freq_Value=8000000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/Readme.md b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/Readme.md new file mode 100644 index 0000000000000..7f53b54bd9932 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/Readme.md @@ -0,0 +1,102 @@ +# SpeedyBeeF405WING Flight Controller + +The SpeedyBeeF405WING is a flight controller produced by [SpeedyBee](http://www.speedybee.com/). + +## Features + Processor + STM32F405 168Mhz, 1MB 32-bit processor + AT7456E OSD + Sensors + ICM42688P Acc/Gyro + SPL006 barometer + Power + 2S - 6S Lipo input voltage with voltage monitoring + 90A Cont., 215A peak current monitor + 9V/12/5V, 1.8A BEC for powering Video Transmitter + 4.9V/6V/7.2V, 4.5A BEC for servos + 5V, 2.4A BEC for internal and peripherals + Interfaces + 12x PWM outputs DShot capable (Serail LED output is PWM12) + 1x RC input + 5x UARTs/serial for GPS and other peripherals, 6th UART internally tied to Wireless board) + I2C port for external compass, airspeed, etc. + microSDCard for logging, etc. + USB-C port + + +## Pinout + +![SpeedyBeeF405WING](SpeedyBeeF405WING.png) + +## Wiring Diagram + +![SpeedyBeeF405WING Wiring](SpeedyBeeF405WING_wiring.png) + +## UART Mapping + +The UARTs are marked Rn and Tn in the above pinouts. The Rn pin is the +receive pin for UARTn. The Tn pin is the transmit pin for UARTn. + + - SERIAL0 -> USB + - SERIAL1 -> USART1 (Serial RC input) (DMA capable) + - SERIAL2 -> USART2 (RX tied to inverted SBUS RC input, but can be used as normal UART if :ref:`BRD_ALT_CONFIG<>` =1) + - SERIAL3 -> UART3 (GPS) (TX DMA capable) + - SERIAL4 -> UART4 (User) (TX DMA capable) + - SERIAL5 -> UART5 (User, available on DJI air unit connector) (TX DMA capable) + - SERIAL6 -> UART6 (tied to internal wireless module, MAVLink2 telem) + + +## RC Input + +RC input is configured on the SBUS pin (inverted and sent to UART2_RX). It supports all RC +protocols except serial protocols such as CRSF, ELRS, etc. Those devices can be connected to USART1 TX and RX, instead. +Fport can be connected to USART1 TX also, but will require an external bi-directional inverter and the ref:`SERIAL1_OPTION' = 4 (HalfDuplex) set. + +## OSD Support + +The SpeedyBeeF405Wing supports using its internal OSD using OSD_TYPE 1 (MAX7456 driver). External OSD support such as DJI or DisplayPort is supported using UART5 or any other free UART5. See :ref:`common-msp-osd-overview-4.2` for more info. + +## PWM Output + +The SpeedyBeeF405Wing supports up to 12 PWM outputs (PWM12 is the serial LED output, by default). All outputs support DShot. + +The PWM is in 5 groups: + + - PWM 1,2 in group1 + - PWM 2,4 in group2 + - PWM 5-7 in group3 + - PWM 8-10 in group4 + - PWM 11,12 in group5 Note: PWM12 is setup for LED use by default, if PWM11 is used, you must re-assign PMW12 to a normal PWM output or nothing + +Channels within the same group need to use the same output rate. If +any channel in a group uses DShot then all channels in that group would need +to use DShot. + +## Battery Monitoring + +The board has a builting voltage and current sensor. The current +sensor can read up to 90A continuosly, 215 Amps peak. The voltage sensor can handle up to 6S +LiPo batteries. + +The correct battery setting parameters are set by default and are: + + - BATT_MONITOR 4 + - BATT_VOLT_PIN 10 + - BATT_CURR_PIN 11 + - BATT_VOLT_MULT 11.5 + - BATT_AMP_PERVLT 50 + +## Compass + +The SpeedyBeeF405Wing does not have a built-in compass, but you can attach an external compass using I2C on the SDA and SCL pads. + +## Loading Firmware +Firmware for these boards can be found at https://firmware.ardupilot.org in sub-folders labeled “SpeedyBeeF405Wing”. + +Initial firmware load can be done with DFU by plugging in USB with the +boot button pressed. Then you should load the "SpeedyBeeF405Wing_bl.hex" +firmware, using your favourite DFU loading tool. + +Subsequently, you can update firmware with Mission Planner. + + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/SpeedyBeeF405WING.png b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/SpeedyBeeF405WING.png new file mode 100644 index 0000000000000..ac32c47e9d7b0 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/SpeedyBeeF405WING.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/SpeedyBeeF405WING_wiring.png b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/SpeedyBeeF405WING_wiring.png new file mode 100644 index 0000000000000..e2f8a47030068 Binary files /dev/null and b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/SpeedyBeeF405WING_wiring.png differ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/defaults.parm new file mode 100644 index 0000000000000..fe1e4e70961a0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/defaults.parm @@ -0,0 +1,9 @@ +# WS2812 LED +NTF_LED_LEN,4 +NTF_LED_TYPES 257 +SERVO12_FUNCTION,120 + +#Serial Port defaults +SERIAL1_PROTOCOL 23 +SERIAL4_PROTOCOL -1 +SERIAL6_PROTOCOL 2 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef-bl.dat new file mode 100644 index 0000000000000..84d03b16a6f6b --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef-bl.dat @@ -0,0 +1,37 @@ +# hw definition file for processing by chibios_pins.py +# for speedybeef4 bootloader + +# MCU class and specific type +MCU STM32F4xx STM32F405xx + +# board ID for firmware load +APJ_BOARD_ID 1106 + +# crystal frequency +OSCILLATOR_HZ 8000000 + +FLASH_SIZE_KB 1024 + +# bootloader is installed at zero offset +FLASH_RESERVE_START_KB 0 + +# the location where the bootloader will put the firmware +FLASH_BOOTLOADER_LOAD_KB 64 + +# LEDs +PA14 LED_BOOTLOADER OUTPUT LOW GPIO(0) +PA13 LED_ACTIVITY OUTPUT LOW GPIO(1) +define HAL_LED_ON 0 + +# order of UARTs +SERIAL_ORDER OTG1 + +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +DEFAULTGPIO OUTPUT LOW PULLDOWN + +# Add CS pins to ensure they are high in bootloader +PA4 MPU_CS CS +PB12 OSD_CS CS +PC14 SDCARD_CS CS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat new file mode 100644 index 0000000000000..0c0604d55b8ba --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat @@ -0,0 +1,224 @@ +# hw definition file for SpeedyBee F4 WING hardware +# tested on the Speedybee F405 WING board +# + +# MCU class and specific type +MCU STM32F4xx STM32F405xx + +# board ID for firmware load +APJ_BOARD_ID 1106 + +# crystal frequency +OSCILLATOR_HZ 8000000 + +define STM32_ST_USE_TIMER 5 +define CH_CFG_ST_RESOLUTION 32 + +# reserve 16k for bootloader, 16k for OSD and 32k for flash storage +FLASH_RESERVE_START_KB 64 +FLASH_SIZE_KB 1024 + +define HAL_STORAGE_SIZE 15360 +STORAGE_FLASH_PAGE 1 + +# only one I2C bus +I2C_ORDER I2C1 + +# order of UARTs (and USB) +SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 UART5 USART6 + +# LEDs +PA13 LED_GREEN OUTPUT LOW GPIO(0) +PA14 LED_BLUE OUTPUT LOW GPIO(1) + +define HAL_GPIO_A_LED_PIN 0 +define HAL_GPIO_B_LED_PIN 1 + +# buzzer +PC15 BUZZER OUTPUT GPIO(80) LOW +define HAL_BUZZER_PIN 80 +define HAL_BUZZER_ON 1 +define HAL_BUZZER_OFF 0 + +# spi1 bus for IMU +PA5 SPI1_SCK SPI1 +PA6 SPI1_MISO SPI1 +PA7 SPI1_MOSI SPI1 +PA4 MPU_CS CS + +# spi2 for OSD +PB13 SPI2_SCK SPI2 +PC2 SPI2_MISO SPI2 +PC3 SPI2_MOSI SPI2 +PB12 OSD_CS CS + +# spi3 for sdcard +PB3 SPI3_SCK SPI3 +PB4 SPI3_MISO SPI3 +PB5 SPI3_MOSI SPI3 +PC14 SDCARD_CS CS + +# only one I2C bus in normal config +PB8 I2C1_SCL I2C1 +PB9 I2C1_SDA I2C1 + +# analog pins +PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1) +PC1 BATT_CURRENT_SENS ADC1 SCALE(1) +PC4 RSSI_ADC_PIN ADC1 SCALE(1) + +PC5 PRESSURE_SENS ADC1 SCALE(1) +define HAL_DEFAULT_AIRSPEED_PIN 15 + +# define default battery setup +define HAL_BATT_MONITOR_DEFAULT 4 +define HAL_BATT_VOLT_PIN 10 +define HAL_BATT_CURR_PIN 11 +define HAL_BATT_VOLT_SCALE 11.05 # matched to PDB board +define HAL_BATT_CURR_SCALE 50 # matched to PDB board + +# analog rssi pin +define BOARD_RSSI_ANA_PIN 14 + +# USART1 (ELRS) +PA9 USART1_TX USART1 +PA10 USART1_RX USART1 + + +# USART2 (RCIN with inverter) + +PA3 TIM9_CH2 TIM9 RCININT PULLDOWN LOW + +# alternative with PA3 as USART2_RX +PA2 USART2_TX USART2 NODMA +PA3 USART2_RX USART2 NODMA ALT(1) + +# USART3 (GPS) +PC10 USART3_TX USART3 +PC11 USART3_RX USART3 NODMA + +# UART4 serial4 +PA0 UART4_TX UART4 +PA1 UART4_RX UART4 NODMA + +# USART5 (DJI / VTX) +PC12 UART5_TX UART5 +PD2 UART5_RX UART5 NODMA + +# UART6 (onboard Telemetry) +PC6 USART6_TX USART6 +PC7 USART6_RX USART6 +define HAL_SERIAL6_BAUD 115 + +# USB +PA11 OTG_FS_DM OTG1 +PA12 OTG_FS_DP OTG1 + +# PWM out pins. Note that channel order follows the ArduPilot motor +# order conventions +PB7 TIM4_CH2 TIM4 PWM(1) GPIO(50) +PB6 TIM4_CH1 TIM4 PWM(2) GPIO(51) +PB0 TIM3_CH3 TIM3 PWM(3) GPIO(52) +PB1 TIM3_CH4 TIM3 PWM(4) GPIO(53) +PC8 TIM8_CH3 TIM8 PWM(5) GPIO(54) +PC9 TIM8_CH4 TIM8 PWM(6) GPIO(55) +PB14 TIM8_CH2N TIM8 PWM(7) GPIO(56) + +PA15 TIM2_CH1 TIM2 PWM(8) GPIO(57) +PB10 TIM2_CH3 TIM2 PWM(9) GPIO(58) +PB11 TIM2_CH4 TIM2 PWM(10) GPIO(59) + +PB15 TIM1_CH3N TIM1 PWM(11) GPIO(60) +PA8 TIM1_CH1 TIM1 PWM(12) GPIO(61)# LED + +# one IMU +IMU Invensensev3 SPI:icm42605 ROTATION_ROLL_180_YAW_270 +define HAL_DEFAULT_INS_FAST_SAMPLE 1 + +# one baro +BARO SPL06 I2C:0:0x76 +define AP_BARO_BACKEND_DEFAULT_ENABLED 0 +define AP_BARO_SPL06_ENABLED 1 + +# no built-in compass, but probe the i2c bus for all possible +# external compass types +define ALLOW_ARM_NO_COMPASS +define HAL_PROBE_EXTERNAL_I2C_COMPASSES +define HAL_I2C_INTERNAL_MASK 0 +define HAL_COMPASS_AUTO_ROT_DEFAULT 2 + +# ICM42688P on SPI1 +SPIDEV icm42605 SPI1 DEVID1 MPU_CS MODE3 2*MHZ 8*MHZ + +# OSD on SPI2 +SPIDEV osd SPI2 DEVID2 OSD_CS MODE0 10*MHZ 10*MHZ + +# SD Card on SPI3 +SPIDEV sdcard SPI3 DEVID3 SDCARD_CS MODE0 400*KHZ 25*MHZ + +# filesystem setup on sdcard +define HAL_OS_FATFS_IO 1 +define HAL_BOARD_LOG_DIRECTORY "/APM/LOGS" +define HAL_BOARD_TERRAIN_DIRECTORY "/APM/TERRAIN" + +# setup for OSD +define OSD_ENABLED 1 +define HAL_OSD_TYPE_DEFAULT 1 +ROMFS_WILDCARD libraries/AP_OSD/fonts/font0.bin + +define STM32_PWM_USE_ADVANCED TRUE + +# reduce max size of embedded params for apj_tool.py +define AP_PARAM_MAX_EMBEDDED_PARAM 1024 +define HAL_WITH_DSP FALSE + +# save some flash + +# disable SMBUS and fuel battery monitors to save flash +define AP_BATTMON_SMBUS_ENABLE 0 +define AP_BATTMON_FUEL_ENABLE 0 +define AP_BATTMON_SYNTHETIC_CURRENT_ENABLED 0 +define HAL_BATTMON_INA2XX_ENABLED 0 + +# disable parachute and sprayer to save flash +define HAL_PARACHUTE_ENABLED 0 +define HAL_SPRAYER_ENABLED 0 +define AP_GRIPPER_ENABLED 0 +define HAL_GENERATOR_ENABLED 0 +define AP_ICENGINE_ENABLED 0 +#define LANDING_GEAR_ENABLED 0 +define WINCH_ENABLED 0 +define HAL_ADSB_ENABLED 0 + +define AC_OAPATHPLANNER_ENABLED 0 +define PRECISION_LANDING 0 +#define HAL_BARO_WIND_COMP_ENABLED 0 +define AP_OPTICALFLOW_ENABLED 0 + + +# Disable un-needed hardware drivers +define HAL_WITH_ESC_TELEM 0 +define AP_FETTEC_ONEWIRE_ENABLED 0 + +define AP_VOLZ_ENABLED 0 +define AP_ROBOTISSERVO_ENABLE 0 +define HAL_PICCOLO_CAN_ENABLE 0 +define HAL_TORQEEDO_ENABLED 0 +define HAL_RUNCAM_ENABLED 0 +define HAL_HOTT_TELEM_ENABLED 0 +define HAL_NMEA_OUTPUT_ENABLED 0 +define HAL_BUTTON_ENABLED 0 +define AP_NOTIFY_OREOLED_ENABLED 0 + +#only support MS4525 ANALOG ASP5033 driver +define AP_AIRSPEED_BACKEND_DEFAULT_ENABLED 0 +define AP_AIRSPEED_MS4525_ENABLED 1 +define AP_AIRSPEED_ANALOG_ENABLED 1 +define AP_AIRSPEED_ASP5033_ENABLED 1 + +#only support UBLOX and NMEA GPS driver +define AP_GPS_BACKEND_DEFAULT_ENABLED 0 +define AP_GPS_UBLOX_ENABLED 1 +define AP_GPS_NMEA_ENABLED 1 + +define AP_TRAMP_ENABLED 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h index 195b0e521e3c5..f673dcc4b93f6 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h @@ -31,7 +31,7 @@ #include "hwdef.h" #define _CHIBIOS_RT_CONF_ -#define _CHIBIOS_RT_CONF_VER_6_1_ +#define _CHIBIOS_RT_CONF_VER_7_0_ /*===========================================================================*/ /** * @name System timers settings @@ -76,6 +76,27 @@ extern "C" { #define CH_DBG_STATISTICS FALSE #endif +/** + * @brief Handling of instances. + * @note If enabled then threads assigned to various instances can + * interact each other using the same synchronization objects. + * If disabled then each OS instance is a separate world, no + * direct interactions are handled by the OS. + */ +#if !defined(CH_CFG_SMP_MODE) +#define CH_CFG_SMP_MODE FALSE +#endif + +/** + * @brief Time Stamps APIs. + * @details If enabled then the time stamps APIs are included in the kernel. + * + * @note The default is @p TRUE. + */ +#if !defined(CH_CFG_USE_TIMESTAMP) +#define CH_CFG_USE_TIMESTAMP TRUE +#endif + /** * @brief System time counter resolution. * @note Allowed values are 16 or 32 bits. @@ -803,6 +824,39 @@ extern "C" { /* Trace code here.*/ \ } +/** + * @brief System initialization hook. + * @details User initialization code added to the @p chSysInit() function + * just before interrupts are enabled globally. + */ +#define CH_CFG_SYSTEM_INIT_HOOK() { \ + /* Add system initialization code here.*/ \ +} + +/** + * @brief OS instance structure extension. + * @details User fields added to the end of the @p os_instance_t structure. + */ +#define CH_CFG_OS_INSTANCE_EXTRA_FIELDS \ + /* Add OS instance custom fields here.*/ + +/** + * @brief Runtime Faults Collection Unit hook. + * @details This hook is invoked each time new faults are collected and stored. + */ +#define CH_CFG_RUNTIME_FAULTS_HOOK(mask) { \ + /* Faults handling code here.*/ \ +} + +/** + * @brief OS instance initialization hook. + * + * @param[in] oip pointer to the @p os_instance_t structure + */ +#define CH_CFG_OS_INSTANCE_INIT_HOOK(oip) { \ + /* Add OS instance initialization code here.*/ \ +} + /** @} */ /*===========================================================================*/ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk index 14382747e7289..3da8419dc3936 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chibios_board.mk @@ -109,7 +109,7 @@ include $(CHIBIOS)/$(CHIBIOS_PLATFORM_MK) include $(CHIBIOS)/os/hal/osal/rt-nil/osal.mk # RTOS files (optional). include $(CHIBIOS)/os/rt/rt.mk -include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk +include $(CHIBIOS)/os/common/ports/ARMv7-M/compilers/GCC/mk/port.mk # Other files (optional). #include $(CHIBIOS)/test/rt/test.mk include $(CHIBIOS)/os/hal/lib/streams/streams.mk @@ -136,6 +136,11 @@ CSRC += $(HWDEF)/common/stubs.c \ $(HWDEF)/common/bouncebuffer.c \ $(HWDEF)/common/watchdog.c +ifeq ($(USE_USB_MSD),yes) +CSRC += $(CHIBIOS)/os/various/scsi_bindings/lib_scsi.c \ + $(CHIBIOS)/os/hal/src/hal_usb_msd.c +endif + # $(TESTSRC) \ # test.c ifneq ($(CRASHCATCHER),) @@ -179,6 +184,11 @@ INCDIR = $(CHIBIOS)/os/license \ ifneq ($(CRASHCATCHER),) INCDIR += $(CRASHCATCHER)/include endif + +ifeq ($(USE_USB_MSD),yes) +INCDIR += $(CHIBIOS)/os/various/scsi_bindings +endif + # # Project, sources and paths ############################################################################## diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/crashdump.c b/libraries/AP_HAL_ChibiOS/hwdef/common/crashdump.c index 5eec28ba98f14..61093b69e0b69 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/crashdump.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/crashdump.c @@ -99,7 +99,7 @@ const CrashCatcherMemoryRegion* CrashCatcher_GetMemoryRegions(void) // do a full dump if on serial static CrashCatcherMemoryRegion regions[60] = { {(uint32_t)&__ram0_start__, (uint32_t)&__ram0_end__, CRASH_CATCHER_BYTE}, - {(uint32_t)&ch, (uint32_t)&ch + sizeof(ch), CRASH_CATCHER_BYTE}}; + {(uint32_t)&ch_system, (uint32_t)&ch_system + sizeof(ch_system), CRASH_CATCHER_BYTE}}; uint32_t total_dump_size = dump_size + buf_off + REMAINDER_MEM_REGION_SIZE; // loop through chibios threads and add their stack info uint8_t curr_region = 2; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/ffconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/ffconf.h index abfa15e523b8b..b0dbfa3fc55ea 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/ffconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/ffconf.h @@ -1,87 +1,95 @@ -#pragma once - -/* CHIBIOS FIX */ -#include "ch.h" +#include "hwdef.h" /*---------------------------------------------------------------------------/ -/ FatFs - FAT file system module configuration file +/ FatFs Functional Configurations /---------------------------------------------------------------------------*/ -#define FFCONF_DEF 80196 /* Revision ID */ +#define FFCONF_DEF 86631 /* Revision ID */ /*---------------------------------------------------------------------------/ / Function Configurations /---------------------------------------------------------------------------*/ -#define FF_FS_READONLY 0 +#define FF_FS_READONLY 0 /* This option switches read-only configuration. (0:Read/Write or 1:Read-only) / Read-only configuration removes writing API functions, f_write(), f_sync(), / f_unlink(), f_mkdir(), f_chmod(), f_rename(), f_truncate(), f_getfree() / and optional writing functions as well. */ -#define FF_FS_MINIMIZE 0 +#define FF_FS_MINIMIZE 0 /* This option defines minimization level to remove some basic API functions. / -/ 0: All basic functions are enabled. +/ 0: Basic functions are fully enabled. / 1: f_stat(), f_getfree(), f_unlink(), f_mkdir(), f_truncate() and f_rename() / are removed. / 2: f_opendir(), f_readdir() and f_closedir() are removed in addition to 1. / 3: f_lseek() function is removed in addition to 2. */ -#define FF_USE_STRFUNC 0 -/* This option switches string functions, f_gets(), f_putc(), f_puts() and -/ f_printf(). -/ -/ 0: Disable string functions. -/ 1: Enable without LF-CRLF conversion. -/ 2: Enable with LF-CRLF conversion. */ - - -#define FF_USE_FIND 0 +#define FF_USE_FIND 0 /* This option switches filtered directory read functions, f_findfirst() and / f_findnext(). (0:Disable, 1:Enable 2:Enable with matching altname[] too) */ -#ifndef FF_USE_MKFS -// f_mkfs() only supported on SDC so far -#define FF_USE_MKFS HAL_USE_SDC -#endif +#define FF_USE_MKFS HAL_USE_SDC /* This option switches f_mkfs() function. (0:Disable or 1:Enable) */ -#define FF_USE_FASTSEEK 0 +#define FF_USE_FASTSEEK 0 /* This option switches fast seek function. (0:Disable or 1:Enable) */ -#define FF_USE_EXPAND 0 +#define FF_USE_EXPAND 0 /* This option switches f_expand function. (0:Disable or 1:Enable) */ -#define FF_USE_CHMOD 1 +#define FF_USE_CHMOD 1 /* This option switches attribute manipulation functions, f_chmod() and f_utime(). -/ (0:Disable or 1:Enable) Also _FS_READONLY needs to be 0 to enable this option. */ +/ (0:Disable or 1:Enable) Also FF_FS_READONLY needs to be 0 to enable this option. */ -#define FF_USE_LABEL 0 +#define FF_USE_LABEL 0 /* This option switches volume label functions, f_getlabel() and f_setlabel(). / (0:Disable or 1:Enable) */ -#define FF_USE_FORWARD 0 +#define FF_USE_FORWARD 0 /* This option switches f_forward() function. (0:Disable or 1:Enable) */ +#define FF_USE_STRFUNC 0 +#define FF_PRINT_LLI 0 +#define FF_PRINT_FLOAT 0 +#define FF_STRF_ENCODE 0 +/* FF_USE_STRFUNC switches string functions, f_gets(), f_putc(), f_puts() and +/ f_printf(). +/ +/ 0: Disable. FF_PRINT_LLI, FF_PRINT_FLOAT and FF_STRF_ENCODE have no effect. +/ 1: Enable without LF-CRLF conversion. +/ 2: Enable with LF-CRLF conversion. +/ +/ FF_PRINT_LLI = 1 makes f_printf() support long long argument and FF_PRINT_FLOAT = 1/2 + makes f_printf() support floating point argument. These features want C99 or later. +/ When FF_LFN_UNICODE >= 1 with LFN enabled, string functions convert the character +/ encoding in it. FF_STRF_ENCODE selects assumption of character encoding ON THE FILE +/ to be read/written via those functions. +/ +/ 0: ANSI/OEM in current CP +/ 1: Unicode in UTF-16LE +/ 2: Unicode in UTF-16BE +/ 3: Unicode in UTF-8 +*/ + + /*---------------------------------------------------------------------------/ / Locale and Namespace Configurations /---------------------------------------------------------------------------*/ -#define FF_CODE_PAGE 850 +#define FF_CODE_PAGE 850 /* This option specifies the OEM code page to be used on the target system. -/ Incorrect setting of the code page can cause a file open failure. +/ Incorrect code page setting can cause a file open failure. / -/ 1 - ASCII (No extended character. Non-LFN cfg. only) / 437 - U.S. / 720 - Arabic / 737 - Greek @@ -103,53 +111,52 @@ / 936 - Simplified Chinese (DBCS) / 949 - Korean (DBCS) / 950 - Traditional Chinese (DBCS) +/ 0 - Include all code pages above and configured by f_setcp() */ -#define FF_USE_LFN 3 -#define FF_MAX_LFN 255 -/* The _USE_LFN switches the support of long file name (LFN). +#define FF_USE_LFN 3 +#define FF_MAX_LFN 255 +/* The FF_USE_LFN switches the support for LFN (long file name). / -/ 0: Disable support of LFN. _MAX_LFN has no effect. -/ 1: Enable LFN with static working buffer on the BSS. Always NOT thread-safe. +/ 0: Disable LFN. FF_MAX_LFN has no effect. +/ 1: Enable LFN with static working buffer on the BSS. Always NOT thread-safe. / 2: Enable LFN with dynamic working buffer on the STACK. / 3: Enable LFN with dynamic working buffer on the HEAP. / -/ To enable the LFN, Unicode handling functions (option/unicode.c) must be added -/ to the project. The working buffer occupies (_MAX_LFN + 1) * 2 bytes and -/ additional 608 bytes at exFAT enabled. _MAX_LFN can be in range from 12 to 255. -/ It should be set 255 to support full featured LFN operations. +/ To enable the LFN, ffunicode.c needs to be added to the project. The LFN function +/ requiers certain internal working buffer occupies (FF_MAX_LFN + 1) * 2 bytes and +/ additional (FF_MAX_LFN + 44) / 15 * 32 bytes when exFAT is enabled. +/ The FF_MAX_LFN defines size of the working buffer in UTF-16 code unit and it can +/ be in range of 12 to 255. It is recommended to be set it 255 to fully support LFN +/ specification. / When use stack for the working buffer, take care on stack overflow. When use heap / memory for the working buffer, memory management functions, ff_memalloc() and -/ ff_memfree(), must be added to the project. */ +/ ff_memfree() exemplified in ffsystem.c, need to be added to the project. */ -#define FF_LFN_UNICODE 0 -/* This option switches character encoding on the API. (0:ANSI/OEM or 1:UTF-16) -/ To use Unicode string for the path name, enable LFN and set _LFN_UNICODE = 1. -/ This option also affects behavior of string I/O functions. */ +#define FF_LFN_UNICODE 0 +/* This option switches the character encoding on the API when LFN is enabled. +/ +/ 0: ANSI/OEM in current CP (TCHAR = char) +/ 1: Unicode in UTF-16 (TCHAR = WCHAR) +/ 2: Unicode in UTF-8 (TCHAR = char) +/ 3: Unicode in UTF-32 (TCHAR = DWORD) +/ +/ Also behavior of string I/O functions will be affected by this option. +/ When LFN is not enabled, this option has no effect. */ + -#define FF_LFN_BUF 255 -#define FF_SFN_BUF 12 +#define FF_LFN_BUF 255 +#define FF_SFN_BUF 12 /* This set of options defines size of file name members in the FILINFO structure / which is used to read out directory items. These values should be suffcient for / the file names to read. The maximum possible length of the read file name depends / on character encoding. When LFN is not enabled, these options have no effect. */ -#define FF_STRF_ENCODE 3 -/* When _LFN_UNICODE == 1, this option selects the character encoding ON THE FILE to -/ be read/written via string I/O functions, f_gets(), f_putc(), f_puts and f_printf(). -/ -/ 0: ANSI/OEM -/ 1: UTF-16LE -/ 2: UTF-16BE -/ 3: UTF-8 -/ -/ This option has no effect when _LFN_UNICODE == 0. */ - -#define FF_FS_RPATH 1 -/* This option configures support of relative path. +#define FF_FS_RPATH 1 +/* This option configures support for relative path. / / 0: Disable relative path and remove related functions. / 1: Enable relative path. f_chdir() and f_chdrive() are available. @@ -161,99 +168,106 @@ / Drive/Volume Configurations /---------------------------------------------------------------------------*/ -#define FF_VOLUMES 1 -/* Number of volumes (logical drives) to be used. */ +#define FF_VOLUMES 1 +/* Number of volumes (logical drives) to be used. (1-10) */ -#define FF_STR_VOLUME_ID 0 -#define FF_VOLUME_STRS "RAM","NAND","CF","SD","SD2","USB","USB2","USB3" -/* _STR_VOLUME_ID switches string support of volume ID. -/ When _STR_VOLUME_ID is set to 1, also pre-defined strings can be used as drive -/ number in the path name. _VOLUME_STRS defines the drive ID strings for each -/ logical drives. Number of items must be equal to _VOLUMES. Valid characters for -/ the drive ID strings are: A-Z and 0-9. */ +#define FF_STR_VOLUME_ID 0 +#define FF_VOLUME_STRS "RAM","NAND","CF","SD","SD2","USB","USB2","USB3" +/* FF_STR_VOLUME_ID switches support for volume ID in arbitrary strings. +/ When FF_STR_VOLUME_ID is set to 1 or 2, arbitrary strings can be used as drive +/ number in the path name. FF_VOLUME_STRS defines the volume ID strings for each +/ logical drives. Number of items must not be less than FF_VOLUMES. Valid +/ characters for the volume ID strings are A-Z, a-z and 0-9, however, they are +/ compared in case-insensitive. If FF_STR_VOLUME_ID >= 1 and FF_VOLUME_STRS is +/ not defined, a user defined volume string table needs to be defined as: +/ +/ const char* VolumeStr[FF_VOLUMES] = {"ram","flash","sd","usb",... +*/ -#define FF_MULTI_PARTITION 0 -/* This option switches support of multi-partition on a physical drive. +#define FF_MULTI_PARTITION 0 +/* This option switches support for multiple volumes on the physical drive. / By default (0), each logical drive number is bound to the same physical drive / number and only an FAT volume found on the physical drive will be mounted. -/ When multi-partition is enabled (1), each logical drive number can be bound to +/ When this function is enabled (1), each logical drive number can be bound to / arbitrary physical drive and partition listed in the VolToPart[]. Also f_fdisk() / funciton will be available. */ -#define FF_MIN_SS 512 -#define FF_MAX_SS 512 -/* These options configure the range of sector size to be supported. (512, 1024, -/ 2048 or 4096) Always set both 512 for most systems, all type of memory cards and -/ harddisk. But a larger value may be required for on-board flash memory and some -/ type of optical media. When _MAX_SS is larger than _MIN_SS, FatFs is configured -/ to variable sector size and GET_SECTOR_SIZE command must be implemented to the -/ disk_ioctl() function. */ +#define FF_MIN_SS 512 +#define FF_MAX_SS 512 +/* This set of options configures the range of sector size to be supported. (512, +/ 1024, 2048 or 4096) Always set both 512 for most systems, generic memory card and +/ harddisk, but a larger value may be required for on-board flash memory and some +/ type of optical media. When FF_MAX_SS is larger than FF_MIN_SS, FatFs is configured +/ for variable sector size mode and disk_ioctl() function needs to implement +/ GET_SECTOR_SIZE command. */ -#define FF_LBA64 0 + +#define FF_LBA64 0 /* This option switches support for 64-bit LBA. (0:Disable or 1:Enable) / To enable the 64-bit LBA, also exFAT needs to be enabled. (FF_FS_EXFAT == 1) */ -#define FF_MIN_GPT 0x100000000 -/* Minimum number of sectors to switch GPT format to create partition in f_mkfs and +#define FF_MIN_GPT 0x100000000 +/* Minimum number of sectors to switch GPT as partitioning format in f_mkfs and / f_fdisk function. 0x100000000 max. This option has no effect when FF_LBA64 == 0. */ -#define FF_USE_TRIM 0 -/* This option switches support of ATA-TRIM. (0:Disable or 1:Enable) + +#define FF_USE_TRIM 0 +/* This option switches support for ATA-TRIM. (0:Disable or 1:Enable) / To enable Trim function, also CTRL_TRIM command should be implemented to the / disk_ioctl() function. */ -#define FF_FS_NOFSINFO 0 -/* If you need to know correct free space on the FAT32 volume, set bit 0 of this -/ option, and f_getfree() function at first time after volume mount will force -/ a full FAT scan. Bit 1 controls the use of last allocated cluster number. -/ -/ bit0=0: Use free cluster count in the FSINFO if available. -/ bit0=1: Do not trust free cluster count in the FSINFO. -/ bit1=0: Use last allocated cluster number in the FSINFO if available. -/ bit1=1: Do not trust last allocated cluster number in the FSINFO. -*/ - - /*---------------------------------------------------------------------------/ / System Configurations /---------------------------------------------------------------------------*/ -#define FF_FS_TINY 0 +#define FF_FS_TINY 0 /* This option switches tiny buffer configuration. (0:Normal or 1:Tiny) -/ At the tiny configuration, size of file object (FIL) is reduced _MAX_SS bytes. +/ At the tiny configuration, size of file object (FIL) is shrinked FF_MAX_SS bytes. / Instead of private sector buffer eliminated from the file object, common sector -/ buffer in the file system object (FATFS) is used for the file data transfer. */ +/ buffer in the filesystem object (FATFS) is used for the file data transfer. */ -#define FF_FS_EXFAT 1 -/* This option switches support of exFAT file system. (0:Disable or 1:Enable) -/ When enable exFAT, also LFN needs to be enabled. (_USE_LFN >= 1) -/ Note that enabling exFAT discards C89 compatibility. */ +#define FF_FS_EXFAT 1 +/* This option switches support for exFAT filesystem. (0:Disable or 1:Enable) +/ To enable exFAT, also LFN needs to be enabled. (FF_USE_LFN >= 1) +/ Note that enabling exFAT discards ANSI C (C89) compatibility. */ -#define FF_FS_NORTC 0 -#define FF_NORTC_MON 1 -#define FF_NORTC_MDAY 1 -#define FF_NORTC_YEAR 2016 -/* The option _FS_NORTC switches timestamp functiton. If the system does not have -/ any RTC function or valid timestamp is not needed, set _FS_NORTC = 1 to disable -/ the timestamp function. All objects modified by FatFs will have a fixed timestamp -/ defined by _NORTC_MON, _NORTC_MDAY and _NORTC_YEAR in local time. -/ To enable timestamp function (_FS_NORTC = 0), get_fattime() function need to be -/ added to the project to get current time form real-time clock. _NORTC_MON, -/ _NORTC_MDAY and _NORTC_YEAR have no effect. -/ These options have no effect at read-only configuration (_FS_READONLY = 1). */ +#define FF_FS_NORTC 0 +#define FF_NORTC_MON 1 +#define FF_NORTC_MDAY 1 +#define FF_NORTC_YEAR 2016 +/* The option FF_FS_NORTC switches timestamp functiton. If the system does not have +/ any RTC function or valid timestamp is not needed, set FF_FS_NORTC = 1 to disable +/ the timestamp function. Every object modified by FatFs will have a fixed timestamp +/ defined by FF_NORTC_MON, FF_NORTC_MDAY and FF_NORTC_YEAR in local time. +/ To enable timestamp function (FF_FS_NORTC = 0), get_fattime() function need to be +/ added to the project to read current time form real-time clock. FF_NORTC_MON, +/ FF_NORTC_MDAY and FF_NORTC_YEAR have no effect. +/ These options have no effect in read-only configuration (FF_FS_READONLY = 1). */ + + +#define FF_FS_NOFSINFO 0 +/* If you need to know correct free space on the FAT32 volume, set bit 0 of this +/ option, and f_getfree() function at first time after volume mount will force +/ a full FAT scan. Bit 1 controls the use of last allocated cluster number. +/ +/ bit0=0: Use free cluster count in the FSINFO if available. +/ bit0=1: Do not trust free cluster count in the FSINFO. +/ bit1=0: Use last allocated cluster number in the FSINFO if available. +/ bit1=1: Do not trust last allocated cluster number in the FSINFO. +*/ -#define FF_FS_LOCK 0 -/* The option _FS_LOCK switches file lock function to control duplicated file open -/ and illegal operation to open objects. This option must be 0 when _FS_READONLY +#define FF_FS_LOCK 0 +/* The option FF_FS_LOCK switches file lock function to control duplicated file open +/ and illegal operation to open objects. This option must be 0 when FF_FS_READONLY / is 1. / / 0: Disable file lock function. To avoid volume corruption, application program @@ -263,27 +277,27 @@ / lock control is independent of re-entrancy. */ -#define FF_FS_REENTRANT 0 -#define FF_FS_TIMEOUT chTimeMS2I(1000) -#define FF_SYNC_t semaphore_t* -/* The option _FS_REENTRANT switches the re-entrancy (thread safe) of the FatFs +/* #include // O/S definitions */ +#define FF_FS_REENTRANT 0 +#define FF_FS_TIMEOUT chTimeMS2I(1000) +#define FF_SYNC_t semaphore_t* +/* The option FF_FS_REENTRANT switches the re-entrancy (thread safe) of the FatFs / module itself. Note that regardless of this option, file access to different / volume is always re-entrant and volume control functions, f_mount(), f_mkfs() / and f_fdisk() function, are always not re-entrant. Only file/directory access / to the same volume is under control of this function. / -/ 0: Disable re-entrancy. _FS_TIMEOUT and _SYNC_t have no effect. +/ 0: Disable re-entrancy. FF_FS_TIMEOUT and FF_SYNC_t have no effect. / 1: Enable re-entrancy. Also user provided synchronization handlers, / ff_req_grant(), ff_rel_grant(), ff_del_syncobj() and ff_cre_syncobj() / function, must be added to the project. Samples are available in / option/syscall.c. / -/ The _FS_TIMEOUT defines timeout period in unit of time tick. -/ The _SYNC_t defines O/S dependent sync object type. e.g. HANDLE, ID, OS_EVENT*, -/ SemaphoreHandle_t and etc.. A header file for O/S definitions needs to be +/ The FF_FS_TIMEOUT defines timeout period in unit of time tick. +/ The FF_SYNC_t defines O/S dependent sync object type. e.g. HANDLE, ID, OS_EVENT*, +/ SemaphoreHandle_t and etc. A header file for O/S definitions needs to be / included somewhere in the scope of ff.h. */ -/* #include // O/S definitions */ /*--- End of configuration options ---*/ diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/halconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/halconf.h index f84b7fcdc84b5..09d559300854e 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/halconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/halconf.h @@ -43,7 +43,7 @@ #pragma once #define _CHIBIOS_HAL_CONF_ -#define _CHIBIOS_HAL_CONF_VER_7_1_ +#define _CHIBIOS_HAL_CONF_VER_8_4_ #include "mcuconf.h" @@ -350,15 +350,18 @@ /*===========================================================================*/ /** - * @brief Delays insertions. - * @details If enabled this options inserts delays into the MMC waiting - * routines releasing some extra CPU time for the threads with - * lower priority, this may slow down the driver a bit however. - * This option is recommended also if the SPI driver does not - * use a DMA channel and heavily loads the CPU. + * @brief Timeout before assuming a failure while waiting for card idle. + * #note Time is in milliseconds. */ -#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__) -#define MMC_NICE_WAITING TRUE +#if !defined(MMC_IDLE_TIMEOUT_MS) || defined(__DOXYGEN__) +#define MMC_IDLE_TIMEOUT_MS 1000 +#endif + +/** + * @brief Mutual exclusion on the SPI bus. + */ +#if !defined(MMC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) +#define MMC_USE_MUTUAL_EXCLUSION TRUE #endif /*===========================================================================*/ @@ -434,6 +437,26 @@ #endif #endif +/*===========================================================================*/ +/* SIO driver related settings. */ +/*===========================================================================*/ + +/** + * @brief Default bit rate. + * @details Configuration parameter, this is the baud rate selected for the + * default configuration. + */ +#if !defined(SIO_DEFAULT_BITRATE) || defined(__DOXYGEN__) +#define SIO_DEFAULT_BITRATE 38400 +#endif + +/** + * @brief Support for thread synchronization API. + */ +#if !defined(SIO_USE_SYNCHRONIZATION) || defined(__DOXYGEN__) +#define SIO_USE_SYNCHRONIZATION TRUE +#endif + /*===========================================================================*/ /* SERIAL_USB driver related setting. */ /*===========================================================================*/ @@ -478,6 +501,13 @@ #define SPI_USE_WAIT TRUE #endif +/** + * @brief Inserts an assertion on function errors before returning. + */ +#if !defined(SPI_USE_ASSERT_ON_ERROR) || defined(__DOXYGEN__) +#define SPI_USE_ASSERT_ON_ERROR TRUE +#endif + /** * @brief Enables circular transfers APIs. * @note Disabling this option saves both code and data space. diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c index 7a415c99a3db0..8f7f8d8a98d8d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c @@ -458,7 +458,7 @@ void system_halt_hook(void) #ifdef HAL_GPIO_PIN_FAULT // optionally print the message on a fault pin while (true) { - fault_printf("PANIC:%s\n", ch.dbg.panic_msg); + fault_printf("PANIC:%s\n", currcore->dbg.panic_msg); fault_printf("RA0:0x%08x\n", __builtin_return_address(0)); } #endif diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index 97ca23092c0a5..9ab42b1260531 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -63,9 +63,10 @@ /* * Memory attributes settings. */ -#define STM32_NOCACHE_MPU_REGION MPU_REGION_6 -#define STM32_NOCACHE_SRAM1_SRAM2 FALSE -#define STM32_NOCACHE_SRAM3 FALSE +#define STM32_NOCACHE_ENABLE FALSE +//#define STM32_NOCACHE_MPU_REGION MPU_REGION_6 +//#define STM32_NOCACHE_RBAR 0x24000000U +//#define STM32_NOCACHE_RASR MPU_RASR_SIZE_16K /* * PWR system settings. @@ -152,10 +153,9 @@ #define STM32_PLL2_DIVN_VALUE 45 #define STM32_PLL2_DIVP_VALUE 2 #define STM32_PLL2_DIVQ_VALUE 5 -#define STM32_PLL2_DIVR_VALUE 1 +#define STM32_PLL2_DIVR_VALUE 8 #define STM32_PLL3_DIVN_VALUE 15 -#define STM32_PLL3_DIVP_VALUE 3 #define STM32_PLL3_DIVQ_VALUE 5 #define STM32_PLL3_DIVR_VALUE 8 @@ -164,23 +164,23 @@ #ifdef HAL_CUSTOM_MCU_CLOCKRATE #if HAL_CUSTOM_MCU_CLOCKRATE == 480000000 #define STM32_PLL1_DIVN_VALUE 120 +#define STM32_PLL1_DIVQ_VALUE 12 #else #error "Unable to configure custom clockrate" #endif #else #define STM32_PLL1_DIVN_VALUE 100 +#define STM32_PLL1_DIVQ_VALUE 10 #endif #define STM32_PLL1_DIVP_VALUE 2 -#define STM32_PLL1_DIVQ_VALUE 10 #define STM32_PLL1_DIVR_VALUE 2 -#define STM32_PLL2_DIVN_VALUE 45 -#define STM32_PLL2_DIVP_VALUE 2 -#define STM32_PLL2_DIVQ_VALUE 5 -#define STM32_PLL2_DIVR_VALUE 1 +#define STM32_PLL2_DIVN_VALUE 75 +#define STM32_PLL2_DIVP_VALUE 3 +#define STM32_PLL2_DIVQ_VALUE 6 +#define STM32_PLL2_DIVR_VALUE 3 #define STM32_PLL3_DIVN_VALUE 72 -#define STM32_PLL3_DIVP_VALUE 3 #define STM32_PLL3_DIVQ_VALUE 6 #define STM32_PLL3_DIVR_VALUE 9 @@ -198,13 +198,12 @@ #define STM32_PLL1_DIVQ_VALUE 10 #define STM32_PLL1_DIVR_VALUE 2 -#define STM32_PLL2_DIVN_VALUE 30 -#define STM32_PLL2_DIVP_VALUE 2 -#define STM32_PLL2_DIVQ_VALUE 5 -#define STM32_PLL2_DIVR_VALUE 1 +#define STM32_PLL2_DIVN_VALUE 50 +#define STM32_PLL2_DIVP_VALUE 3 +#define STM32_PLL2_DIVQ_VALUE 6 +#define STM32_PLL2_DIVR_VALUE 3 #define STM32_PLL3_DIVN_VALUE 72 -#define STM32_PLL3_DIVP_VALUE 3 #define STM32_PLL3_DIVQ_VALUE 6 #define STM32_PLL3_DIVR_VALUE 9 @@ -217,13 +216,12 @@ #define STM32_PLL1_DIVQ_VALUE 10 #define STM32_PLL1_DIVR_VALUE 2 -#define STM32_PLL2_DIVN_VALUE 72 -#define STM32_PLL2_DIVP_VALUE 2 -#define STM32_PLL2_DIVQ_VALUE 5 -#define STM32_PLL2_DIVR_VALUE 1 +#define STM32_PLL2_DIVN_VALUE 120 +#define STM32_PLL2_DIVP_VALUE 3 +#define STM32_PLL2_DIVQ_VALUE 6 +#define STM32_PLL2_DIVR_VALUE 3 #define STM32_PLL3_DIVN_VALUE 48 -#define STM32_PLL3_DIVP_VALUE 3 #define STM32_PLL3_DIVQ_VALUE 5 #define STM32_PLL3_DIVR_VALUE 8 #endif // clock selection @@ -251,7 +249,7 @@ #define STM32_PLL2_FRACN_VALUE 0 #define STM32_PLL3_ENABLED TRUE -#define STM32_PLL3_P_ENABLED TRUE +#define STM32_PLL3_P_ENABLED FALSE #define STM32_PLL3_Q_ENABLED TRUE #define STM32_PLL3_R_ENABLED TRUE #define STM32_PLL3_FRACN_VALUE 0 @@ -294,7 +292,7 @@ #define STM32_FDCANSEL STM32_FDCANSEL_PLL1_Q_CK #define STM32_DFSDM1SEL STM32_DFSDM1SEL_PCLK2 #define STM32_SPDIFSEL STM32_SPDIFSEL_PLL1_Q_CK -#define STM32_SPI45SEL STM32_SPI45SEL_PCLK2 +#define STM32_SPI45SEL STM32_SPI45SEL_PLL2_Q_CK #define STM32_SPI123SEL STM32_SPI123SEL_PLL1_Q_CK #define STM32_SAI23SEL STM32_SAI23SEL_PLL1_Q_CK #define STM32_SAI1SEL STM32_SAI1SEL_PLL1_Q_CK @@ -303,9 +301,9 @@ #define STM32_USBSEL STM32_USBSEL_PLL3_Q_CK #define STM32_I2C123SEL STM32_I2C123SEL_PLL3_R_CK #define STM32_RNGSEL STM32_RNGSEL_HSI48_CK -#define STM32_USART16SEL STM32_USART16SEL_PCLK2 -#define STM32_USART234578SEL STM32_USART234578SEL_PCLK1 -#define STM32_SPI6SEL STM32_SPI6SEL_PCLK4 +#define STM32_USART16SEL STM32_USART16SEL_PLL2_Q_CK +#define STM32_USART234578SEL STM32_USART234578SEL_PLL2_Q_CK +#define STM32_SPI6SEL STM32_SPI6SEL_PLL2_Q_CK #define STM32_SAI4BSEL STM32_SAI4BSEL_PLL1_Q_CK #define STM32_SAI4ASEL STM32_SAI4ASEL_PLL1_Q_CK #define STM32_ADCSEL STM32_ADCSEL_PLL3_R_CK @@ -368,6 +366,7 @@ * ADC driver system settings. */ #define STM32_ADC_DUAL_MODE FALSE +#define STM32_ADC_SAMPLES_SIZE 16 #define STM32_ADC_COMPACT_SAMPLES FALSE #define STM32_ADC_USE_ADC12 TRUE #ifndef STM32H750xx @@ -483,6 +482,7 @@ #define STM32_SDC_SDMMC_CLOCK_DELAY 10 #define STM32_SDC_SDMMC1_DMA_PRIORITY 3 #define STM32_SDC_SDMMC1_IRQ_PRIORITY 9 +#define STM32_SDC_SDMMC_PWRSAV TRUE /* * SERIAL driver system settings. @@ -496,14 +496,18 @@ #define STM32_SERIAL_UART7_PRIORITY 12 #define STM32_SERIAL_UART8_PRIORITY 12 -#define STM32_UART1CLK STM32_PCLK1 -#define STM32_UART2CLK STM32_PCLK1 -#define STM32_UART3CLK STM32_PCLK1 -#define STM32_UART4CLK STM32_PCLK1 -#define STM32_UART5CLK STM32_PCLK1 -#define STM32_UART6CLK STM32_PCLK1 -#define STM32_UART7CLK STM32_PCLK1 -#define STM32_UART8CLK STM32_PCLK1 +/* + * SIO driver system settings. + */ +#define STM32_SIO_USE_USART1 FALSE +#define STM32_SIO_USE_USART2 FALSE +#define STM32_SIO_USE_USART3 FALSE +#define STM32_SIO_USE_UART4 FALSE +#define STM32_SIO_USE_UART5 FALSE +#define STM32_SIO_USE_USART6 FALSE +#define STM32_SIO_USE_UART7 FALSE +#define STM32_SIO_USE_UART8 FALSE +#define STM32_SIO_USE_LPUART1 FALSE /* * SPI driver system settings. @@ -548,6 +552,11 @@ #define STM32_ST_USE_TIMER 5 #endif +/* + * TRNG driver system settings. + */ +#define STM32_TRNG_USE_RNG1 FALSE + /* * UART driver system settings. */ @@ -603,7 +612,7 @@ #if STM32_WSPI_USE_QUADSPI1 #define STM32_WSPI_QUADSPI1_MDMA_CHANNEL STM32_MDMA_CHANNEL_ID_ANY #define STM32_WSPI_QUADSPI1_MDMA_PRIORITY 1 -#define STM32_WSPI_QUADSPI1_PRESCALER_VALUE ((STM32_QSPICLK / HAL_QSPI1_CLK) - 1) +#define STM32_WSPI_QUADSPI1_PRESCALER_VALUE (STM32_QSPICLK / HAL_QSPI1_CLK) #endif /* diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h index 73dea046289c4..2c76eb2151622 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32l4_mcuconf.h @@ -20,6 +20,7 @@ #ifndef MCUCONF_H #define MCUCONF_H +#define STM32L431_MCUCONF #define STM32L4xx_MCUCONF #define STM32L496_MCUCONF #define STM32L4A6_MCUCONF diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat index eee65e240a73e..002fbad6143b3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv3/hwdef-bl.dat @@ -30,7 +30,7 @@ USB_STRING_MANUFACTURER "ArduPilot" define BOOTLOADER_BAUDRATE 115200 # uarts and USB to run bootloader protocol on -SERIAL_ORDER OTG1 USART2 USART3 UART7 +SERIAL_ORDER OTG1 USART2 # this is the pin that senses USB being connected. It is an input pin # setup as OPENDRAIN @@ -50,16 +50,9 @@ PD6 USART2_RX USART2 PD3 USART2_CTS USART2 PD4 USART2_RTS USART2 -# the telem2 USART, also with RTS/CTS available -# USART3 serial3 telem2 -PD8 USART3_TX USART3 -PD9 USART3_RX USART3 -PD11 USART3_CTS USART3 -PD12 USART3_RTS USART3 - # UART7 maps to uartF in the HAL (serial5 in SERIALn_ parameters) -PE7 UART7_RX UART7 -PE8 UART7_TX UART7 +#PE7 UART7_RX UART7 +#PE8 UART7_TX UART7 # define a LED PE12 LED_BOOTLOADER OUTPUT diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/SimOnHW.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/SimOnHW.inc index 1f90fd8397ad3..63d09f3db949b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/SimOnHW.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/SimOnHW.inc @@ -1,6 +1,5 @@ env SIM_ENABLED 1 -define INS_MAX_INSTANCES 2 define HAL_COMPASS_MAX_SENSORS 2 define HAL_NAVEKF2_AVAILABLE 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimal_Airspeed.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimal_Airspeed.inc index 3201ff23e4e0d..1770d8587866f 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimal_Airspeed.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimal_Airspeed.inc @@ -7,4 +7,4 @@ define AP_AIRSPEED_MS5525_ENABLED 1 define AP_AIRSPEED_SDP3X_ENABLED 1 define AP_AIRSPEED_NMEA_ENABLED 1 # additional checks for vehicle type in .cpp -define AP_AIRSPEED_UAVCAN_ENABLED HAL_ENABLE_LIBUAVCAN_DRIVERS +define AP_AIRSPEED_DRONECAN_ENABLED HAL_ENABLE_DRONECAN_DRIVERS diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc index 0bcda34f3957c..25b624026771c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/minimize_features.inc @@ -61,3 +61,6 @@ define AP_CAMERA_SERVO_ENABLED AP_CAMERA_ENABLED # no SLCAN on these boards (use can-over-mavlink if required) define AP_CAN_SLCAN_ENABLED 0 + +# no beacon support on minimized boards: +define AP_BEACON_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/include/save_some_flash.inc b/libraries/AP_HAL_ChibiOS/hwdef/include/save_some_flash.inc index 428ea63143b1f..73a10ff1ea26c 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/include/save_some_flash.inc +++ b/libraries/AP_HAL_ChibiOS/hwdef/include/save_some_flash.inc @@ -1,7 +1,7 @@ # save some flash define HAL_GENERATOR_ENABLED 0 define AC_OAPATHPLANNER_ENABLED 0 -define PRECISION_LANDING 0 +define AC_PRECLAND_ENABLED 0 define AP_OPTICALFLOW_ENABLED 0 define AP_ICENGINE_ENABLED 0 define HAL_BARO_WIND_COMP_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py index 82c860195e055..86bee245e89c3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py @@ -28,7 +28,7 @@ 'RAM_MAP' : [ (0x30000000, 256, 0), # SRAM1, SRAM2 (0x20000000, 128, 2), # DTCM, tightly coupled, no DMA, fast - (0x24000000, 512, 4), # AXI SRAM. Use this for SDMMC IDMA ops + (0x24000000, 512, 4), # AXI SRAM. (0x00000400, 63, 2), # ITCM (first 1k removed, to keep address 0 unused) (0x30040000, 32, 0), # SRAM3. (0x38000000, 64, 1), # SRAM4. @@ -36,7 +36,7 @@ # alternative RAM_MAP needed for px4 bootloader compatibility 'ALT_RAM_MAP' : [ - (0x24000000, 512, 4), # AXI SRAM. Use this for SDMMC IDMA ops + (0x24000000, 512, 4), # AXI SRAM. (0x30000000, 256, 0), # SRAM1, SRAM2 (0x20000000, 128, 2), # DTCM, tightly coupled, no DMA, fast (0x00000400, 63, 2), # ITCM (first 1k removed, to keep address 0 unused) @@ -58,6 +58,14 @@ 'EXPECTED_CLOCK' : 400000000, + 'EXPECTED_CLOCKS' : [ + ('STM32_SYS_CK', 400000000), + ('STM32_QSPICLK', 200000000), + ('STM32_SDMMC1CLK', 80000000), + ('STM32_SPI45CLK', 100000000), + ('STM32_FDCANCLK', 80000000), + ], + # this MCU has M7 instructions and hardware double precision 'CORTEX' : 'cortex-m7', 'CPU_FLAGS' : '-mcpu=cortex-m7 -mfpu=fpv5-d16 -mfloat-abi=hard', diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py index 1d7d9ac4334a0..c81e630310b0b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H750xx.py @@ -63,6 +63,13 @@ 'EXPECTED_CLOCK' : 400000000, + 'EXPECTED_CLOCKS' : [ + ('STM32_SYS_CK', 400000000), + ('STM32_QSPICLK', 200000000), + ('STM32_SDMMC1CLK', 80000000), + ('STM32_SPI45CLK', 100000000), + ], + # this MCU has M7 instructions and hardware double precision 'CORTEX' : 'cortex-m7', 'CPU_FLAGS' : '-mcpu=cortex-m7 -mfpu=fpv5-d16 -mfloat-abi=hard', diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index a52997873e340..e383f752a4951 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -840,7 +840,7 @@ def write_mcu_config(f): f.write('// MCU type (ChibiOS define)\n') f.write('#define %s_MCUCONF\n' % get_config('MCU')) mcu_subtype = get_config('MCU', 1) - if mcu_subtype.endswith('xx'): + if mcu_subtype[-1:] == 'x' or mcu_subtype[-2:-1] == 'x': f.write('#define %s_MCUCONF\n\n' % mcu_subtype[:-2]) f.write('#define %s\n\n' % mcu_subtype) f.write('// crystal frequency\n') @@ -942,6 +942,9 @@ def write_mcu_config(f): if result: intdefines[result.group(1)] = int(result.group(2)) + if intdefines.get('HAL_USE_USB_MSD',0) == 1: + build_flags.append('USE_USB_MSD=yes') + if have_type_prefix('CAN') and not using_chibios_can: enable_can(f) flash_size = get_config('FLASH_SIZE_KB', type=int) @@ -963,13 +966,13 @@ def write_mcu_config(f): env_vars['EXT_FLASH_SIZE_MB'] = get_config('EXT_FLASH_SIZE_MB', default=0, type=int) if env_vars['EXT_FLASH_SIZE_MB'] and not args.bootloader: - f.write('#define CRT1_AREAS_NUMBER 3\n') + f.write('#define CRT0_AREAS_NUMBER 3\n') f.write('#define CRT1_RAMFUNC_ENABLE TRUE\n') # this will enable loading program sections to RAM f.write('#define __FASTRAMFUNC__ __attribute__ ((__section__(".fastramfunc")))\n') f.write('#define __RAMFUNC__ __attribute__ ((__section__(".ramfunc")))\n') f.write('#define PORT_IRQ_ATTRIBUTES __FASTRAMFUNC__\n') else: - f.write('#define CRT1_AREAS_NUMBER 1\n') + f.write('#define CRT0_AREAS_NUMBER 1\n') f.write('#define CRT1_RAMFUNC_ENABLE FALSE\n') storage_flash_page = get_storage_flash_page() @@ -1082,6 +1085,14 @@ def write_mcu_config(f): elif get_mcu_config('EXPECTED_CLOCK', required=True): f.write('#define HAL_EXPECTED_SYSCLOCK %u\n' % get_mcu_config('EXPECTED_CLOCK')) + if get_mcu_config('EXPECTED_CLOCKS', required=False): + clockrate = get_config('MCU_CLOCKRATE_MHZ', required=False) + for mcu_clock, mcu_clock_speed in get_mcu_config('EXPECTED_CLOCKS'): + if (mcu_clock == 'STM32_HCLK' or mcu_clock == 'STM32_SYS_CK') and clockrate: + f.write('#define HAL_EXPECTED_%s %u\n' % (mcu_clock, int(clockrate) * 1000000)) + else: + f.write('#define HAL_EXPECTED_%s %u\n' % (mcu_clock, mcu_clock_speed)) + env_vars['CORTEX'] = cortex if not args.bootloader: @@ -1153,6 +1164,7 @@ def write_mcu_config(f): #endif #define CH_CFG_USE_EVENTS FALSE #define CH_CFG_USE_EVENTS_TIMEOUT FALSE +#define CH_CFG_OPTIMIZE_SPEED FALSE #define HAL_USE_EMPTY_STORAGE 1 #ifndef HAL_STORAGE_SIZE #define HAL_STORAGE_SIZE 16384 @@ -1451,6 +1463,11 @@ def write_QSPI_table(f): def write_QSPI_config(f): '''write SPI config defines''' global qspi_list + # only the bootloader must reset the QSPI clock otherwise it is not possible to + # bootstrap into external flash + if not args.bootloader: + f.write('#define STM32_QSPI_NO_RESET TRUE\n') + if len(qspidev) == 0: # nothing to do return diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat index 12a1c79a3c46b..ef7814bd6d0fb 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat @@ -73,7 +73,7 @@ define AP_AIRSPEED_ENABLED 0 define AP_BEACON_ENABLED 0 define AP_OPTICALFLOW_ENABLED 0 define AP_FRSKY_TELEM_ENABLED 0 -define BEACON_ENABLED 0 +define AP_BEACON_ENABLED 0 define GPS_MOVING_BASELINE 0 define HAL_ADSB_SAGETECH_ENABLED 0 define HAL_ADSB_UAVIONIX_MAVLINK_ENABLED 0 @@ -173,4 +173,8 @@ define AP_CAMERA_MAVLINK_ENABLED 1 // remove RC Protocol support: define AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED 0 +// few prelcnad backends are applicable +define AC_PRECLAND_BACKEND_DEFAULT_ENABLED 0 +define AC_PRECLAND_COMPANION_ENABLED 1 + AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ChibiOS/sdcard.cpp b/libraries/AP_HAL_ChibiOS/sdcard.cpp index efc50ce481f09..eaedca7214f98 100644 --- a/libraries/AP_HAL_ChibiOS/sdcard.cpp +++ b/libraries/AP_HAL_ChibiOS/sdcard.cpp @@ -22,6 +22,7 @@ #include #include #include "bouncebuffer.h" +#include "stm32_util.h" extern const AP_HAL::HAL& hal; @@ -35,7 +36,6 @@ static bool sdcard_running; #if HAL_USE_SDC static SDCConfig sdcconfig = { - NULL, SDC_MODE_4BIT, 0 }; @@ -99,6 +99,11 @@ bool sdcard_init() return true; } #elif HAL_USE_MMC_SPI + if (MMCD1.buffer == nullptr) { + // allocate 16 byte non-cacheable buffer for microSD + MMCD1.buffer = (uint8_t*)malloc_axi_sram(MMC_BUFFER_SIZE); + } + if (sdcard_running) { sdcard_stop(); } @@ -113,7 +118,7 @@ bool sdcard_init() } device->set_slowdown(sd_slowdown); - mmcObjectInit(&MMCD1); + mmcObjectInit(&MMCD1, MMCD1.buffer); mmcconfig.spip = static_cast(device.get())->get_driver(); diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp index da1490e2353a9..d9e177c596309 100644 --- a/libraries/AP_HAL_ChibiOS/system.cpp +++ b/libraries/AP_HAL_ChibiOS/system.cpp @@ -53,6 +53,26 @@ static_assert(HAL_EXPECTED_SYSCLOCK == STM32_HCLK, "unexpected STM32_HCLK value" #define AP_FAULTHANDLER_DEBUG_VARIABLES_ENABLED 1 #endif +#define QUOTE(str) #str +#define EXPAND_AND_QUOTE(str) QUOTE(str) +#define ASSERT_CLOCK(clk) static_assert(HAL_EXPECTED_ ##clk == (clk), "unexpected " #clk " value: '" EXPAND_AND_QUOTE(clk) "'") + +#if defined(HAL_EXPECTED_STM32_SYS_CK) && defined(STM32_SYS_CK) +ASSERT_CLOCK(STM32_SYS_CK); +#endif +#if defined(HAL_EXPECTED_STM32_HCLK) && defined(STM32_HCLK) +ASSERT_CLOCK(STM32_HCLK); +#endif +#if defined(HAL_EXPECTED_STM32_SDMMC1CLK) && defined(STM32_SDMMC1CLK) +ASSERT_CLOCK(STM32_SDMMC1CLK); +#endif +#if defined(HAL_EXPECTED_STM32_SPI45CLK) && defined(STM32_SPI45CLK) +ASSERT_CLOCK(STM32_SPI45CLK); +#endif +#if defined(HAL_EXPECTED_STM32_FDCANCLK) && defined(STM32_FDCANCLK) +ASSERT_CLOCK(STM32_FDCANCLK); +#endif + extern const AP_HAL::HAL& hal; extern "C" { @@ -107,9 +127,9 @@ void HardFault_Handler(void) { fault_printf("HARDFAULT(%d)\n", int(faultType)); } fault_printf("CSFR=0x%08x\n", cfsr); - fault_printf("CUR=0x%08x\n", ch.rlist.current); - if (ch.rlist.current) { - fault_printf("NAME=%s\n", ch.rlist.current->name); + fault_printf("CUR=0x%08x\n", currcore->rlist.current); + if (currcore->rlist.current) { + fault_printf("NAME=%s\n", currcore->rlist.current->name); } fault_printf("FA=0x%08x\n", faultAddress); fault_printf("PC=0x%08x\n", ctx.pc); diff --git a/libraries/AP_HAL_ESP32/README.md b/libraries/AP_HAL_ESP32/README.md index 1fbebf13f018d..6c67ed0bc0241 100644 --- a/libraries/AP_HAL_ESP32/README.md +++ b/libraries/AP_HAL_ESP32/README.md @@ -169,13 +169,13 @@ After flashing the esp32 , u can connect with a terminal app of your preference | GND | GND | | 5v | Pwr | -### RC reciever connection: +### RC receiver connection: -|ESP32| RCRECIEVER | -| --- | --- | -| D4 | CPPM-out | -| GND | GND | -| 5v | Pwr | +|ESP32| RC Receiver | +| --- | --- | +| D4 | CPPM-out | +| GND | GND | +| 5v | Pwr | ### I2C connection ( for gps with leds/compass-es/etc onboard, or digital airspeed sensorrs, etc): diff --git a/libraries/AP_HAL_Linux/CANSocketIface.cpp b/libraries/AP_HAL_Linux/CANSocketIface.cpp index c2203afa39c50..3c5cbae954330 100644 --- a/libraries/AP_HAL_Linux/CANSocketIface.cpp +++ b/libraries/AP_HAL_Linux/CANSocketIface.cpp @@ -462,8 +462,11 @@ void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd) bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) { char iface_name[16]; +#if HAL_LINUX_USE_VIRTUAL_CAN + sprintf(iface_name, "vcan%u", _self_index); +#else sprintf(iface_name, "can%u", _self_index); - +#endif if (_initialized) { return _initialized; } diff --git a/libraries/AP_HAL_SITL/CANSocketIface.cpp b/libraries/AP_HAL_SITL/CANSocketIface.cpp index 84c6147509a54..536ec62db27ac 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.cpp +++ b/libraries/AP_HAL_SITL/CANSocketIface.cpp @@ -57,7 +57,8 @@ uint8_t CANIface::next_interface; static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) { - can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } }; + can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { }, }; + memset(sockcan_frame.data, 0, sizeof(sockcan_frame.data)); std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data); if (uavcan_frame.isExtended()) { sockcan_frame.can_id |= CAN_EFF_FLAG; @@ -73,7 +74,8 @@ static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) static canfd_frame makeSocketCanFDFrame(const AP_HAL::CANFrame& uavcan_frame) { - canfd_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), CANFD_BRS, 0, 0, { } }; + canfd_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), CANFD_BRS, 0, 0, { }, }; + memset(sockcan_frame.data, 0, sizeof(sockcan_frame.data)); std::copy(uavcan_frame.data, uavcan_frame.data + AP_HAL::CANFrame::dlcToDataLength(uavcan_frame.dlc), sockcan_frame.data); if (uavcan_frame.isExtended()) { sockcan_frame.can_id |= CAN_EFF_FLAG; @@ -104,7 +106,7 @@ static AP_HAL::CANFrame makeCanFrame(const can_frame& sockcan_frame) static AP_HAL::CANFrame makeCanFDFrame(const canfd_frame& sockcan_frame) { - AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.len); + AP_HAL::CANFrame can_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.len, true); if (sockcan_frame.can_id & CAN_EFF_FLAG) { can_frame.id |= AP_HAL::CANFrame::FlagEFF; } @@ -339,16 +341,11 @@ bool CANIface::_pollRead() uint8_t iterations_count = 0; while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT) { - iterations_count++; CanRxItem rx; rx.timestamp_us = AP_HAL::native_micros64(); // Monotonic timestamp is not required to be precise (unlike UTC) bool loopback = false; int res; - if (iterations_count % 2 == 0) { - res = _read(rx.frame, rx.timestamp_us, loopback); - } else { - res = _readfd(rx.frame, rx.timestamp_us, loopback); - } + res = _read(rx.frame, rx.timestamp_us, loopback); if (res == 1) { bool accept = true; if (loopback) { // We receive loopback for all CAN frames @@ -409,76 +406,20 @@ int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopb if (_fd < 0) { return -1; } - auto iov = iovec(); - auto sockcan_frame = can_frame(); - iov.iov_base = &sockcan_frame; - iov.iov_len = sizeof(sockcan_frame); - union { - uint8_t data[CMSG_SPACE(sizeof(::timeval))]; - struct cmsghdr align; - } control; - - auto msg = msghdr(); - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control.data; - msg.msg_controllen = sizeof(control.data); - - const int res = recvmsg(_fd, &msg, MSG_DONTWAIT); - if (res <= 0) { - return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; - } - /* - * Flags - */ - loopback = (msg.msg_flags & static_cast(MSG_CONFIRM)) != 0; - - if (!loopback && !_checkHWFilters(sockcan_frame)) { - return 0; - } - - frame = makeCanFrame(sockcan_frame); - /* - * Timestamp - */ - timestamp_us = AP_HAL::native_micros64(); - return 1; -} - -int CANIface::_readfd(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const -{ - if (_fd < 0) { - return -1; - } - auto iov = iovec(); - auto sockcan_frame = canfd_frame(); - iov.iov_base = &sockcan_frame; - iov.iov_len = sizeof(sockcan_frame); union { - uint8_t data[CMSG_SPACE(sizeof(::timeval))]; - struct cmsghdr align; - } control; - - auto msg = msghdr(); - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control.data; - msg.msg_controllen = sizeof(control.data); + can_frame frame; + canfd_frame frame_fd; + } frames; - const int res = recvmsg(_fd, &msg, MSG_DONTWAIT); + const int res = read(_fd, &frames, sizeof(frames)); if (res <= 0) { return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; } - /* - * Flags - */ - loopback = (msg.msg_flags & static_cast(MSG_CONFIRM)) != 0; - - if (!loopback && !_checkHWFilters(sockcan_frame)) { - return 0; + if (res == sizeof(can_frame)) { + frame = makeCanFrame(frames.frame); + } else { + frame = makeCanFDFrame(frames.frame_fd); } - - frame = makeCanFDFrame(sockcan_frame); /* * Timestamp */ diff --git a/libraries/AP_HAL_SITL/CANSocketIface.h b/libraries/AP_HAL_SITL/CANSocketIface.h index 37dd3fd5847de..7d1e8831e3746 100644 --- a/libraries/AP_HAL_SITL/CANSocketIface.h +++ b/libraries/AP_HAL_SITL/CANSocketIface.h @@ -144,8 +144,6 @@ class CANIface: public AP_HAL::CANIface { int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const; - int _readfd(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const; - void _incrementNumFramesInSocketTxQueue(); void _confirmSentFrame(); diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.h b/libraries/AP_HAL_SITL/HAL_SITL_Class.h index 2f3a890b6449b..4ddc20b2cb666 100644 --- a/libraries/AP_HAL_SITL/HAL_SITL_Class.h +++ b/libraries/AP_HAL_SITL/HAL_SITL_Class.h @@ -47,7 +47,7 @@ class HAL_SITL : public AP_HAL::HAL { void setup_signal_handlers() const; static void exit_signal_handler(int); - bool storage_posix_enabled; + bool storage_posix_enabled = true; bool storage_flash_enabled; bool storage_fram_enabled; diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp index ce0a51809b2b4..db09856c7afd1 100644 --- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp @@ -67,8 +67,8 @@ void SITL_State::wait_clock(uint64_t wait_time_usec) { // when Periph can use SITL simulated devices we should remove these // stubs: -ssize_t SITL::SerialDevice::read_from_device(char*, unsigned int) const { return -1; } +ssize_t SITL::SerialDevice::read_from_device(char*, size_t) const { return -1; } -ssize_t SITL::SerialDevice::write_to_device(char const*, unsigned int) const { return -1; } +ssize_t SITL::SerialDevice::write_to_device(char const*, size_t) const { return -1; } #endif //CONFIG_HAL_BOARD == HAL_BOARD_SITL && defined(HAL_BUILD_AP_PERIPH) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 48cff7d103347..1bf532972f971 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -86,7 +86,9 @@ void SITL_State::_sitl_setup() sitl_model->set_precland(&_sitl->precland_sim); _sitl->i2c_sim.init(); sitl_model->set_i2c(&_sitl->i2c_sim); - +#if AP_TEST_DRONECAN_DRIVERS + sitl_model->set_dronecan_device(&_sitl->dronecan_sim); +#endif if (_use_fg_view) { fg_socket.connect(_fg_address, _fg_view_port); } diff --git a/libraries/AP_HAL_SITL/system.cpp b/libraries/AP_HAL_SITL/system.cpp index 8b1c35d7ddde3..518a2f3e16837 100644 --- a/libraries/AP_HAL_SITL/system.cpp +++ b/libraries/AP_HAL_SITL/system.cpp @@ -196,12 +196,20 @@ uint64_t millis64() uint32_t native_micros() { +#if AP_TEST_DRONECAN_DRIVERS + return micros(); +#else return native_micros64() & 0xFFFFFFFF; +#endif } uint32_t native_millis() { +#if AP_TEST_DRONECAN_DRIVERS + return millis(); +#else return native_millis64() & 0xFFFFFFFF; +#endif } /* @@ -209,28 +217,40 @@ uint32_t native_millis() */ uint16_t native_millis16() { +#if AP_TEST_DRONECAN_DRIVERS + return millis16(); +#else return native_millis64() & 0xFFFF; +#endif } uint64_t native_micros64() { +#if AP_TEST_DRONECAN_DRIVERS + return micros64(); +#else struct timeval tp; gettimeofday(&tp, nullptr); uint64_t ret = 1.0e6 * ((tp.tv_sec + (tp.tv_usec * 1.0e-6)) - (state.start_time.tv_sec + (state.start_time.tv_usec * 1.0e-6))); return ret; +#endif } uint64_t native_millis64() { +#if AP_TEST_DRONECAN_DRIVERS + return millis64(); +#else 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; +#endif } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index be46e9a2b50a4..a3decab52787f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -840,7 +840,7 @@ AP_InertialSensor_Backend *AP_InertialSensor::_find_backend(int16_t backend_id, } bool AP_InertialSensor::set_gyro_window_size(uint16_t size) { -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED _gyro_window_size = size; // allocate FFT gyro window @@ -909,7 +909,7 @@ AP_InertialSensor::init(uint16_t loop_rate) batchsampler.init(); #endif -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED AP_GyroFFT* fft = AP::fft(); bool fft_enabled = fft != nullptr && fft->enabled(); if (fft_enabled) { @@ -954,7 +954,7 @@ AP_InertialSensor::init(uint16_t loop_rate) notch.num_dynamic_notches = 1; #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) if (notch.params.hasOption(HarmonicNotchFilterParams::Options::DynamicHarmonic)) { -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED if (notch.params.tracking_mode() == HarmonicNotchDynamicMode::UpdateGyroFFT) { notch.num_dynamic_notches = AP_HAL::DSP::MAX_TRACKED_PEAKS; // only 3 peaks supported currently } else @@ -1675,6 +1675,7 @@ AP_InertialSensor::_init_gyro() // stop flashing leds AP_Notify::flags.initialising = false; + AP_Notify::flags.gyro_calibrated = true; } // save parameters to eeprom diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 4e2b8b623bcd4..db108eb1991c3 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -152,7 +152,7 @@ class AP_InertialSensor : AP_AccelCal_Client uint16_t get_accel_rate_hz(uint8_t instance) const { return uint16_t(_accel_raw_sample_rates[instance] * _accel_over_sampling[instance]); } // FFT support access -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_primary_gyro]; } FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; } FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_primary_gyro, axis); } @@ -329,6 +329,7 @@ class AP_InertialSensor : AP_AccelCal_Client // Parameters AP_Int16 _required_count; + uint16_t _real_required_count; AP_Int8 _sensor_mask; AP_Int8 _batch_options_mask; @@ -501,7 +502,7 @@ class AP_InertialSensor : AP_AccelCal_Client LowPassFilter2pVector3f _gyro_filter[INS_MAX_INSTANCES]; Vector3f _accel_filtered[INS_MAX_INSTANCES]; Vector3f _gyro_filtered[INS_MAX_INSTANCES]; -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED // Thread-safe public version of _last_raw_gyro Vector3f _gyro_for_fft[INS_MAX_INSTANCES]; Vector3f _last_gyro_for_fft[INS_MAX_INSTANCES]; @@ -723,7 +724,7 @@ class AP_InertialSensor : AP_AccelCal_Client // instance number for logging #if INS_AUX_INSTANCES uint8_t tcal_instance(const AP_InertialSensor_TCal &tc) const { - for (uint8_t i=0; i 0) { @@ -243,7 +243,7 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const // if the filtering failed in any way then reset the filters and keep the old value if (gyro_filtered.is_nan() || gyro_filtered.is_inf()) { _imu._gyro_filter[instance].reset(); -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED _imu._post_filter_gyro_filter[instance].reset(); #endif for (auto ¬ch : _imu.harmonic_notches) { @@ -756,7 +756,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ } if (_imu._new_gyro_data[instance]) { _publish_gyro(instance, _imu._gyro_filtered[instance]); -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED // copy the gyro samples from the backend to the frontend window for FFTs sampling at less than IMU rate _imu._gyro_for_fft[instance] = _imu._last_gyro_for_fft[instance]; #endif @@ -768,7 +768,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */ if (_last_gyro_filter_hz != _gyro_filter_cutoff() || sensors_converging()) { _imu._gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff()); -#if HAL_WITH_DSP +#if HAL_GYROFFT_ENABLED _imu._post_filter_gyro_filter[instance].set_cutoff_frequency(gyro_rate, _gyro_filter_cutoff()); #endif _last_gyro_filter_hz = _gyro_filter_cutoff(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp index f62913071ca38..e103f6ee9d2f1 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensense.cpp @@ -894,7 +894,7 @@ void AP_InertialSensor_Invensense::_set_filter_register(void) _gyro_backend_rate_hz *= fast_sampling_rate; // calculate rate we will be giving accel samples to the backend - if (_mpu_type >= Invensense_MPU9250) { + if (_mpu_type >= Invensense_MPU6500) { _accel_fifo_downsample_rate = MAX(4 / fast_sampling_rate, 1); _accel_backend_rate_hz *= MIN(fast_sampling_rate, 4); } else { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 48f7c4105018d..cbf5f8db6836c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -111,7 +111,7 @@ bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) con sensor_type : (uint8_t)type, instance : instance_to_write, multiplier : multiplier, - sample_count : (uint16_t)_required_count, + sample_count : (uint16_t)_real_required_count, sample_us : measurement_started_us, sample_rate_hz : sample_rate_hz, }; diff --git a/libraries/AP_InertialSensor/BatchSampler.cpp b/libraries/AP_InertialSensor/BatchSampler.cpp index a125f1a286839..e86a3f559c0e7 100644 --- a/libraries/AP_InertialSensor/BatchSampler.cpp +++ b/libraries/AP_InertialSensor/BatchSampler.cpp @@ -58,12 +58,14 @@ void AP_InertialSensor::BatchSampler::init() _required_count.set(_required_count - (_required_count % 32)); // round down to nearest multiple of 32 - const uint32_t total_allocation = 3*_required_count*sizeof(uint16_t); + _real_required_count = _required_count; + + const uint32_t total_allocation = 3*_real_required_count*sizeof(uint16_t); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "INS: alloc %u bytes for ISB (free=%u)", (unsigned int)total_allocation, (unsigned int)hal.util->available_memory()); - data_x = (int16_t*)calloc(_required_count, sizeof(int16_t)); - data_y = (int16_t*)calloc(_required_count, sizeof(int16_t)); - data_z = (int16_t*)calloc(_required_count, sizeof(int16_t)); + data_x = (int16_t*)calloc(_real_required_count, sizeof(int16_t)); + data_y = (int16_t*)calloc(_real_required_count, sizeof(int16_t)); + data_z = (int16_t*)calloc(_real_required_count, sizeof(int16_t)); if (data_x == nullptr || data_y == nullptr || data_z == nullptr) { free(data_x); free(data_y); @@ -226,7 +228,7 @@ void AP_InertialSensor::BatchSampler::push_data_to_log() data_read_offset += samples_per_msg; last_sent_ms = AP_HAL::millis(); - if (data_read_offset >= _required_count) { + if (data_read_offset >= _real_required_count) { // that was the last one. Clean up: data_read_offset = 0; isb_seqnum++; @@ -251,7 +253,7 @@ bool AP_InertialSensor::BatchSampler::should_log(uint8_t _instance, IMU_SENSOR_T if (_type != type) { return false; } - if (data_write_offset >= _required_count) { + if (data_write_offset >= _real_required_count) { return false; } AP_Logger *logger = AP_Logger::get_singleton(); diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 82fa893fd75d8..143f485fd052b 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -267,8 +267,8 @@ void AP_Mission::reset() /// returns true if mission was running so it could not be cleared bool AP_Mission::clear() { - // do not allow clearing the mission while it is running - if (_flags.state == MISSION_RUNNING) { + // do not allow clearing the mission while it is running unless disarmed + if (hal.util->get_soft_armed() && _flags.state == MISSION_RUNNING) { return false; } @@ -280,7 +280,7 @@ bool AP_Mission::clear() _do_cmd.index = AP_MISSION_CMD_INDEX_NONE; _flags.nav_cmd_loaded = false; _flags.do_cmd_loaded = false; - + _flags.state = MISSION_STOPPED; // return success return true; } diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index e915526d75a93..20aba55537138 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -200,32 +200,38 @@ uint32_t AP_MotorsMatrix::get_motor_mask() return mask; } +// helper to return value scaled between boost and normal based on the value of _thrust_boost_ratio +// _thrust_boost_ratio of 1 -> return = boost_value +// _thrust_boost_ratio of 0 -> return = normal_value +float AP_MotorsMatrix::boost_ratio(float boost_value, float normal_value) const +{ + return _thrust_boost_ratio * boost_value + (1.0 - _thrust_boost_ratio) * normal_value; +} + // output_armed - sends commands to the motors // includes new scaling stability patch void AP_MotorsMatrix::output_armed_stabilizing() { - uint8_t i; // general purpose counter - float roll_thrust; // roll thrust input value, +/- 1.0 - float pitch_thrust; // pitch thrust input value, +/- 1.0 - float yaw_thrust; // yaw thrust input value, +/- 1.0 - float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 - float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0 - float throttle_thrust_max; // throttle thrust maximum value, 0.0 - 1.0 - float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing - float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits - float yaw_allowed = 1.0f; // amount of yaw we can fit in - float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy - // apply voltage and air pressure compensation const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude - roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain; - pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain; - yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain; - throttle_thrust = get_throttle() * compensation_gain; - throttle_avg_max = _throttle_avg_max * compensation_gain; - // If thrust boost is active then do not limit maximum thrust - throttle_thrust_max = _thrust_boost_ratio + (1.0f - _thrust_boost_ratio) * _throttle_thrust_max * compensation_gain; + // pitch thrust input value, +/- 1.0 + const float roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain; + + // pitch thrust input value, +/- 1.0 + const float pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain; + + // yaw thrust input value, +/- 1.0 + float yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain; + + // throttle thrust input value, 0.0 - 1.0 + float throttle_thrust = get_throttle() * compensation_gain; + + // throttle thrust average maximum value, 0.0 - 1.0 + float throttle_avg_max = _throttle_avg_max * compensation_gain; + + // throttle thrust maximum value, 0.0 - 1.0, If thrust boost is active then do not limit maximum thrust + const float throttle_thrust_max = boost_ratio(1.0, _throttle_thrust_max * compensation_gain); // sanity check throttle is above zero and below current limited throttle if (throttle_thrust <= 0.0f) { @@ -240,8 +246,9 @@ void AP_MotorsMatrix::output_armed_stabilizing() // ensure that throttle_avg_max is between the input throttle and the maximum throttle throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max); + // throttle providing maximum roll, pitch and yaw range // calculate the highest allowed average thrust that will provide maximum control range - throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max); + float throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max); // calculate throttle that gives most possible room for yaw which is the lower of: // 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest @@ -266,29 +273,26 @@ void AP_MotorsMatrix::output_armed_stabilizing() // calculate amount of yaw we can fit into the throttle range // this is always equal to or less than the requested yaw from the pilot or rate controller - float rp_low = 1.0f; // lowest thrust value - float rp_high = -1.0f; // highest thrust value - for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + float yaw_allowed = 1.0f; // amount of yaw we can fit in + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { // calculate the thrust outputs for roll and pitch _thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i]; - // record lowest roll + pitch command - if (_thrust_rpyt_out[i] < rp_low) { - rp_low = _thrust_rpyt_out[i]; - } - // record highest roll + pitch command - if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) { - rp_high = _thrust_rpyt_out[i]; - } // Check the maximum yaw control that can be used on this channel // Exclude any lost motors if thrust boost is enabled - if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)){ + if (!is_zero(_yaw_factor[i]) && (!_thrust_boost || i != _motor_lost_index)) { + const float thrust_rp_best_throttle = throttle_thrust_best_rpy + _thrust_rpyt_out[i]; + float motor_room; if (is_positive(yaw_thrust * _yaw_factor[i])) { - yaw_allowed = MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]), 0.0f)/_yaw_factor[i])); + // room to upper limit + motor_room = 1.0 - thrust_rp_best_throttle; } else { - yaw_allowed = MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[i], 0.0f)/_yaw_factor[i])); + // room to lower limit + motor_room = thrust_rp_best_throttle; } + const float motor_yaw_allowed = MAX(motor_room, 0.0)/fabsf(_yaw_factor[i]); + yaw_allowed = MIN(yaw_allowed, motor_yaw_allowed); } } } @@ -298,26 +302,25 @@ void AP_MotorsMatrix::output_armed_stabilizing() float yaw_allowed_min = (float)_yaw_headroom * 0.001f; // increase yaw headroom to 50% if thrust boost enabled - yaw_allowed_min = _thrust_boost_ratio * 0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed_min; + yaw_allowed_min = boost_ratio(0.5, yaw_allowed_min); // Let yaw access minimum amount of head room yaw_allowed = MAX(yaw_allowed, yaw_allowed_min); // Include the lost motor scaled by _thrust_boost_ratio to smoothly transition this motor in and out of the calculation if (_thrust_boost && motor_enabled[_motor_lost_index]) { - // record highest roll + pitch command - if (_thrust_rpyt_out[_motor_lost_index] > rp_high) { - rp_high = _thrust_boost_ratio * rp_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index]; - } - // Check the maximum yaw control that can be used on this channel // Exclude any lost motors if thrust boost is enabled if (!is_zero(_yaw_factor[_motor_lost_index])){ + const float thrust_rp_best_throttle = throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index]; + float motor_room; if (is_positive(yaw_thrust * _yaw_factor[_motor_lost_index])) { - yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(1.0f - (throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index]), 0.0f)/_yaw_factor[_motor_lost_index])); + motor_room = 1.0 - thrust_rp_best_throttle; } else { - yaw_allowed = _thrust_boost_ratio * yaw_allowed + (1.0f - _thrust_boost_ratio) * MIN(yaw_allowed, fabsf(MAX(throttle_thrust_best_rpy + _thrust_rpyt_out[_motor_lost_index], 0.0f)/_yaw_factor[_motor_lost_index])); + motor_room = thrust_rp_best_throttle; } + const float motor_yaw_allowed = MAX(motor_room, 0.0)/fabsf(_yaw_factor[_motor_lost_index]); + yaw_allowed = boost_ratio(yaw_allowed, MIN(yaw_allowed, motor_yaw_allowed)); } } @@ -330,7 +333,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() // add yaw control to thrust outputs float rpy_low = 1.0f; // lowest thrust value float rpy_high = -1.0f; // highest thrust value - for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { _thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i]; @@ -349,11 +352,12 @@ void AP_MotorsMatrix::output_armed_stabilizing() if (_thrust_boost) { // record highest roll + pitch + yaw command if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) { - rpy_high = _thrust_boost_ratio * rpy_high + (1.0f - _thrust_boost_ratio) * _thrust_rpyt_out[_motor_lost_index]; + rpy_high = boost_ratio(rpy_high, _thrust_rpyt_out[_motor_lost_index]); } } // calculate any scaling needed to make the combined thrust outputs fit within the output range + float rpy_scale = 1.0f; if (rpy_high - rpy_low > 1.0f) { rpy_scale = 1.0f / (rpy_high - rpy_low); } @@ -365,7 +369,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() rpy_high *= rpy_scale; rpy_low *= rpy_scale; throttle_thrust_best_rpy = -rpy_low; - thr_adj = throttle_thrust - throttle_thrust_best_rpy; + float thr_adj = throttle_thrust - throttle_thrust_best_rpy; if (rpy_scale < 1.0f) { // Full range is being used by roll, pitch, and yaw. limit.roll = true; @@ -375,21 +379,19 @@ void AP_MotorsMatrix::output_armed_stabilizing() limit.throttle_upper = true; } thr_adj = 0.0f; - } else { - if (thr_adj < 0.0f) { - // Throttle can't be reduced to desired value - // todo: add lower limit flag and ensure it is handled correctly in altitude controller - thr_adj = 0.0f; - } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) { - // Throttle can't be increased to desired value - thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high); - limit.throttle_upper = true; - } + } else if (thr_adj < 0.0f) { + // Throttle can't be reduced to desired value + // todo: add lower limit flag and ensure it is handled correctly in altitude controller + thr_adj = 0.0f; + } else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) { + // Throttle can't be increased to desired value + thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high); + limit.throttle_upper = true; } // add scaled roll, pitch, constrained yaw and throttle for each motor const float throttle_thrust_best_plus_adj = throttle_thrust_best_rpy + thr_adj; - for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { + for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { _thrust_rpyt_out[i] = (throttle_thrust_best_plus_adj * _throttle_factor[i]) + (rpy_scale * _thrust_rpyt_out[i]); } @@ -597,7 +599,7 @@ bool AP_MotorsMatrix::setup_quad_matrix(motor_frame_type frame_type) add_motors(motors, ARRAY_SIZE(motors)); break; } -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN) case MOTOR_FRAME_TYPE_NYT_PLUS: { _frame_type_string = "NYT_PLUS"; static const AP_MotorsMatrix::MotorDef motors[] { @@ -620,7 +622,7 @@ bool AP_MotorsMatrix::setup_quad_matrix(motor_frame_type frame_type) add_motors(motors, ARRAY_SIZE(motors)); break; } -#endif //APM_BUILD_TYPE(APM_BUILD_ArduPlane) +#endif //APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN) case MOTOR_FRAME_TYPE_BF_X: { // betaflight quad X order // see: https://fpvfrenzy.com/betaflight-motor-order/ @@ -1365,5 +1367,29 @@ void AP_MotorsMatrix::disable_yaw_torque(void) } } +#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) +// examples can pull values direct +float AP_MotorsMatrix::get_thrust_rpyt_out(uint8_t i) const +{ + if (i < AP_MOTORS_MAX_NUM_MOTORS) { + return _thrust_rpyt_out[i]; + } + return 0.0; +} + +bool AP_MotorsMatrix::get_factors(uint8_t i, float &roll, float &pitch, float &yaw, float &throttle, uint8_t &testing_order) const +{ + if ((i < AP_MOTORS_MAX_NUM_MOTORS) && motor_enabled[i]) { + roll = _roll_factor[i]; + pitch = _pitch_factor[i]; + yaw = _yaw_factor[i]; + throttle = _throttle_factor[i]; + testing_order = _test_order[i]; + return true; + } + return false; +} +#endif + // singleton instance AP_MotorsMatrix *AP_MotorsMatrix::_singleton; diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 5b2061546fc2e..ad36a0e0cc7bc 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -101,6 +101,10 @@ class AP_MotorsMatrix : public AP_MotorsMulticopter { }; void add_motors_raw(const struct MotorDefRaw *motors, uint8_t num_motors); + // pull values direct, (examples only) + float get_thrust_rpyt_out(uint8_t i) const; + bool get_factors(uint8_t i, float &roll, float &pitch, float &yaw, float &throttle, uint8_t &testing_order) const; + protected: // output - sends commands to the motors void output_armed_stabilizing() override; @@ -152,6 +156,10 @@ class AP_MotorsMatrix : public AP_MotorsMulticopter { const char* _frame_type_string = ""; // string representation of frame type private: + + // helper to return value scaled between boost and normal based on the value of _thrust_boost_ratio + float boost_ratio(float boost_value, float normal_value) const; + // setup motors matrix bool setup_quad_matrix(motor_frame_type frame_type); bool setup_hexa_matrix(motor_frame_type frame_type); diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 0a861ac6fbd38..1b07b8a36bb85 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -873,3 +873,16 @@ bool AP_MotorsMulticopter::arming_checks(size_t buflen, char *buffer) const return true; } + +#if APM_BUILD_TYPE(APM_BUILD_UNKNOWN) +// Getters for AP_Motors example, not used by vehicles +float AP_MotorsMulticopter::get_throttle_avg_max() const +{ + return _throttle_avg_max; +} + +int16_t AP_MotorsMulticopter::get_yaw_headroom() const +{ + return _yaw_headroom; +} +#endif diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 5971b6d144e0d..2e4fdcd615755 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -105,6 +105,10 @@ class AP_MotorsMulticopter : public AP_Motors { // Run arming checks bool arming_checks(size_t buflen, char *buffer) const override; + // Getters for AP_Motors example, not used by vehicles + float get_throttle_avg_max() const; + int16_t get_yaw_headroom() const; + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 418a7b88fbc95..f55f462e99da4 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -39,18 +39,12 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty motor_enabled[AP_MOTORS_MOT_2] = true; motor_enabled[AP_MOTORS_MOT_4] = true; -#if !APM_BUILD_TYPE(APM_BUILD_ArduPlane) // Tilt Rotors do not need a yaw servo - // find the yaw servo - if (!SRV_Channels::get_channel_for(SRV_Channel::k_motor7, AP_MOTORS_CH_TRI_YAW)) { - gcs().send_text(MAV_SEVERITY_ERROR, "MotorsTri: unable to setup yaw channel"); - // don't set initialised_ok - return; - } -#endif - // allow mapping of motor7 add_motor_num(AP_MOTORS_CH_TRI_YAW); + // Check for tail servo + _have_tail_servo = SRV_Channels::function_assigned(SRV_Channel::k_motor7); + SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100); // check for reverse tricopter @@ -175,11 +169,16 @@ void AP_MotorsTri::output_armed_stabilizing() pitch_thrust *= -1.0f; } - // calculate angle of yaw pivot - _pivot_angle = safe_asin(yaw_thrust); - if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) { - limit.yaw = true; - _pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg)); + // VTOL plane may not have tail servo + if (!_have_tail_servo) { + _pivot_angle = 0.0; + } else { + // calculate angle of yaw pivot + _pivot_angle = safe_asin(yaw_thrust); + if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) { + limit.yaw = true; + _pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg)); + } } float pivot_thrust_max = cosf(_pivot_angle); @@ -360,3 +359,18 @@ float AP_MotorsTri::get_roll_factor(uint8_t i) return ret; } + +// Run arming checks +bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const +{ +#if !APM_BUILD_TYPE(APM_BUILD_ArduPlane) // Tilt Rotors do not need a yaw servo + // Check for yaw servo + if (!_have_tail_servo) { + hal.util->snprintf(buffer, buflen, "no SERVOx_FUNCTION set to Motor7 for tail servo"); + return false; + } +#endif + + // run base class checks + return AP_MotorsMulticopter::arming_checks(buflen, buffer); +} diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 03a1fe6284459..430e5c9b706a2 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -48,6 +48,9 @@ class AP_MotorsTri : public AP_MotorsMulticopter { // using copter motors for forward flight float get_roll_factor(uint8_t i) override; + // Run arming checks + bool arming_checks(size_t buflen, char *buffer) const override; + protected: // output - sends commands to the motors void output_armed_stabilizing() override; @@ -71,4 +74,5 @@ class AP_MotorsTri : public AP_MotorsMulticopter { // reverse pitch bool _pitch_reversed; + bool _have_tail_servo; }; 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 b68a58fda5043..a782261a4d154 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 @@ -3,6 +3,12 @@ * Code by Randy Mackay. DIYDrones.com */ +/* on Linux run with + ./waf configure --board linux + ./waf --targets examples/AP_Motors_test + ./build/linux/examples/AP_Motors_test +*/ + // Libraries #include #include @@ -11,6 +17,7 @@ #include #include #include +#include const AP_HAL::HAL& hal = AP_HAL::get_HAL(); @@ -35,20 +42,80 @@ AP_MotorsMatrix motors(400); AP_BattMonitor _battmonitor{0, nullptr, nullptr}; +bool thrust_boost = false; + // setup void setup() { hal.console->printf("AP_Motors library test ver 1.0\n"); // motor initialisation + motors.set_dt(1.0/400.0); motors.set_update_rate(490); + +#if NUM_OUTPUTS == 8 + motors.init(AP_Motors::MOTOR_FRAME_OCTA, AP_Motors::MOTOR_FRAME_TYPE_X); +#elif NUM_OUTPUTS == 6 + motors.init(AP_Motors::MOTOR_FRAME_HEXA, AP_Motors::MOTOR_FRAME_TYPE_X); +#else motors.init(AP_Motors::MOTOR_FRAME_QUAD, AP_Motors::MOTOR_FRAME_TYPE_X); +#endif + + char frame_and_type_string[30]; + motors.get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string)); + hal.console->printf("%s\n", frame_and_type_string); + #if HELI_TEST == 0 motors.update_throttle_range(); motors.set_throttle_avg_max(0.5f); #endif motors.output_min(); + // allow command line args for single run + uint8_t argc; + char * const *argv; + hal.util->commandline_arguments(argc, argv); + if (argc > 1) { + for (uint8_t i = 2; i < argc; i++) { + const char* arg = argv[i]; + + const char *eq = strchr(arg, '='); + if (eq == NULL) { + ::printf("Expected argument with \"=\"\n"); + exit(1); + } + char cmd[20] {}; + strncpy(cmd, arg, eq-arg); + const float value = atof(eq+1); + if (strcmp(cmd,"yaw_headroom") == 0) { + motors.set_yaw_headroom(value); + + } else if (strcmp(cmd,"throttle_avg_max") == 0) { + motors.set_throttle_avg_max(value); + + } else if (strcmp(cmd,"thrust_boost") == 0) { + thrust_boost = value > 0.0; + + } else { + ::printf("Expected \"yaw_headroom\" or \"throttle_avg_max\"\n"); + exit(1); + } + + } + if (strcmp(argv[1],"t") == 0) { + motor_order_test(); + + } else if (strcmp(argv[1],"s") == 0) { + stability_test(); + + } else { + ::printf("Expected first argument, 't' or 's'\n"); + + } + hal.scheduler->delay(1000); + exit(0); + } + hal.scheduler->delay(1000); } @@ -71,9 +138,11 @@ void loop() // test motors if (value == 't' || value == 'T') { motor_order_test(); + hal.console->printf("finished test.\n"); } if (value == 's' || value == 'S') { stability_test(); + hal.console->printf("finished test.\n"); } } @@ -90,75 +159,59 @@ void motor_order_test() hal.scheduler->delay(2000); } motors.armed(false); - hal.console->printf("finished test.\n"); } // stability_test void stability_test() { - int16_t roll_in, pitch_in, yaw_in, avg_out; - float throttle_in; + hal.console->printf("Throttle average max: %0.4f\n", motors.get_throttle_avg_max()); + hal.console->printf("Yaw headroom: %i\n", motors.get_yaw_headroom()); + hal.console->printf("Thrust boost: %s\n", thrust_boost?"True":"False"); - int16_t throttle_tests[] = {0, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000}; - uint8_t throttle_tests_num = sizeof(throttle_tests) / sizeof(int16_t); - int16_t rpy_tests[] = {0, 1000, 2000, 3000, 4500, -1000, -2000, -3000, -4500}; - uint8_t rpy_tests_num = sizeof(rpy_tests) / sizeof(int16_t); + const float throttle_tests[] = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}; + const uint8_t throttle_tests_num = ARRAY_SIZE(throttle_tests); + const float rpy_tests[] = {-1.0, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}; + const uint8_t rpy_tests_num = ARRAY_SIZE(rpy_tests); // arm motors motors.armed(true); motors.set_interlock(true); SRV_Channels::enable_aux_servos(); -#if NUM_OUTPUTS <= 4 - hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // quad -#elif NUM_OUTPUTS <= 6 - hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // hexa -#else - hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,Mot5,Mot6,Mot7,Mot8,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // octa -#endif + hal.console->printf("Roll,Pitch,Yaw,Thr,"); + for (uint8_t i=0; iprintf("Mot%i,",i+1); + } + for (uint8_t i=0; iprintf("Mot%i_norm,",i+1); + } + hal.console->printf("LimR,LimP,LimY,LimThD,LimThU\n"); // run stability test for (uint8_t y=0; yread(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4); // display input and output -#if NUM_OUTPUTS <= 4 - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // quad -#elif NUM_OUTPUTS <= 6 - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // hexa -#else - hal.console->printf("%d,%d,%d,%3.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", // octa -#endif - (int)roll_in, - (int)pitch_in, - (int)yaw_in, - (double)throttle_in, - (int)hal.rcout->read(0), - (int)hal.rcout->read(1), - (int)hal.rcout->read(2), - (int)hal.rcout->read(3), -#if NUM_OUTPUTS >= 6 - (int)hal.rcout->read(4), - (int)hal.rcout->read(5), -#endif -#if NUM_OUTPUTS >= 8 - (int)hal.rcout->read(6), - (int)hal.rcout->read(7), -#endif - (int)avg_out, + hal.console->printf("%0.2f,%0.2f,%0.2f,%0.2f,", roll_in, pitch_in, yaw_in, throttle_in); + for (uint8_t i=0; iprintf("%d,",(int)hal.rcout->read(i)); + } + for (uint8_t i=0; iprintf("%0.4f,", motors.get_thrust_rpyt_out(i)); + } + hal.console->printf("%d,%d,%d,%d,%d\n", (int)motors.limit.roll, (int)motors.limit.pitch, (int)motors.limit.yaw, @@ -176,13 +229,13 @@ void stability_test() motors.set_throttle(0); motors.armed(false); - hal.console->printf("finished test.\n"); } void update_motors() { // call update motors 1000 times to get any ramp limiting complete for (uint16_t i=0; i<1000; i++) { + motors.set_thrust_boost(thrust_boost); motors.output(); } } diff --git a/libraries/AP_Motors/examples/AP_Motors_test/MotorTestSweep.sh b/libraries/AP_Motors/examples/AP_Motors_test/MotorTestSweep.sh new file mode 100755 index 0000000000000..767e018153bd0 --- /dev/null +++ b/libraries/AP_Motors/examples/AP_Motors_test/MotorTestSweep.sh @@ -0,0 +1,29 @@ +# Build and run the motors example stability test at a range of yaw headroom and throttle average max values +# Output results to files for comparison + +cd "$(dirname "$0")" +cd ../../../.. + +mkdir -p MotorTestSweep + +./waf configure --board linux +./waf build --target examples/AP_Motors_test +echo + +YAW_HEADROOM="0 100 200 300 400 500 600 700 800 900 1000" +THR_AVERAGE_MAX="0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0" + +COUNTER=0 +for headroom in $YAW_HEADROOM; do + echo "Yaw Headroom: $headroom" + for Thr in $THR_AVERAGE_MAX; do + echo " Throttle average max: $Thr" + # Test with and without boost + ./build/linux/examples/AP_Motors_test s yaw_headroom=$headroom throttle_avg_max=$Thr thrust_boost=0 > MotorTestSweep/$COUNTER.csv + let COUNTER++ + ./build/linux/examples/AP_Motors_test s yaw_headroom=$headroom throttle_avg_max=$Thr thrust_boost=1 > MotorTestSweep/$COUNTER.csv + let COUNTER++ + done + echo +done + diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index c44014155ebb1..c52ecf96a9b00 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -337,6 +337,73 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com return MAV_RESULT_FAILED; } +void AP_Mount::handle_gimbal_manager_set_attitude(const mavlink_message_t &msg){ + mavlink_gimbal_manager_set_attitude_t packet; + mavlink_msg_gimbal_manager_set_attitude_decode(&msg,&packet); + + AP_Mount_Backend *backend; + + // check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is + // 2nd gimbal, etc + const uint8_t instance = packet.gimbal_device_id; + if (instance == 0) { + backend = get_primary(); + } else { + backend = get_instance(instance - 1); + } + + if (backend == nullptr) { + return; + } + + // check flags for change to RETRACT + const uint32_t flags = packet.flags; + if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) { + backend->set_mode(MAV_MOUNT_MODE_RETRACT); + return; + } + + // check flags for change to NEUTRAL + if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) { + backend->set_mode(MAV_MOUNT_MODE_NEUTRAL); + return; + } + + const Quaternion att_quat{packet.q}; + const Vector3f att_rate_degs { + packet.angular_velocity_x, + packet.angular_velocity_y, + packet.angular_velocity_y + }; + + // ensure that we are only demanded to a specific attitude or to + // achieve a specific rate. Do not allow both to be specified at + // the same time: + if (!att_quat.is_nan() && !att_rate_degs.is_nan()) { + return; + } + + if (!att_quat.is_nan()) { + // convert quaternion to euler angles + float roll_rad, pitch_rad, yaw_rad; + att_quat.to_euler(roll_rad, pitch_rad, yaw_rad); + + // radian to deg conversion + const float roll_deg = degrees(roll_rad); + const float pitch_deg = degrees(pitch_rad); + const float yaw_deg = degrees(yaw_rad); + backend->set_angle_target(roll_deg, pitch_deg, yaw_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return; + } + + { + const float roll_rate_degs = degrees(packet.angular_velocity_x); + const float pitch_rate_degs = degrees(packet.angular_velocity_y); + const float yaw_rate_degs = degrees(packet.angular_velocity_z); + backend->set_rate_target(roll_rate_degs, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return; + } +} MAV_RESULT AP_Mount::handle_command_long(const mavlink_command_long_t &packet) { @@ -528,6 +595,16 @@ void AP_Mount::set_roi_target(uint8_t instance, const Location &target_loc) backend->set_roi_target(target_loc); } +// clear_roi_target - clears target location that mount should attempt to point towards +void AP_Mount::clear_roi_target(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + backend->clear_roi_target(); +} + // // camera controls for gimbals that include a camera // @@ -624,6 +701,9 @@ void AP_Mount::handle_message(mavlink_channel_t chan, const mavlink_message_t &m case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: handle_global_position_int(msg); break; + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: + handle_gimbal_manager_set_attitude(msg); + break; case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: handle_gimbal_device_information(msg); break; diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index d2b5d62c6f2ae..1b97c29f14f37 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -139,6 +139,10 @@ class AP_Mount void set_roi_target(const Location &target_loc) { set_roi_target(_primary,target_loc); } void set_roi_target(uint8_t instance, const Location &target_loc); + // clear_roi_target - clears target location that mount should attempt to point towards + void clear_roi_target() { clear_roi_target(_primary); } + void clear_roi_target(uint8_t instance); + // point at system ID sysid void set_target_sysid(uint8_t sysid) { set_target_sysid(_primary, sysid); } void set_target_sysid(uint8_t instance, uint8_t sysid); @@ -215,6 +219,7 @@ class AP_Mount MAV_RESULT handle_command_do_mount_configure(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_mount_control(const mavlink_command_long_t &packet); MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet); + void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg); void handle_global_position_int(const mavlink_message_t &msg); void handle_gimbal_device_information(const mavlink_message_t &msg); void handle_gimbal_device_attitude_status(const mavlink_message_t &msg); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9033a91f05f76..c161315c777b0 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -48,6 +48,19 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc) set_mode(MAV_MOUNT_MODE_GPS_POINT); } +// clear_roi_target - clears target location that mount should attempt to point towards +void AP_Mount_Backend::clear_roi_target() +{ + // clear the target GPS location + _roi_target_set = false; + + // reset the mode if in GPS tracking mode + if (_mode == MAV_MOUNT_MODE_GPS_POINT) { + MAV_MOUNT_MODE default_mode = (MAV_MOUNT_MODE)_params.default_mode.get(); + set_mode(default_mode); + } +} + // set_sys_target - sets system that mount should attempt to point towards void AP_Mount_Backend::set_target_sysid(uint8_t sysid) { diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index f433d7aa187e6..e62c0d8df3e4b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -49,7 +49,9 @@ class AP_Mount_Backend // returns true if this mount can control its pan (required for multicopters) virtual bool has_pan_control() const = 0; - // get attitude as a quaternion. returns true on success + // get attitude as a quaternion. returns true on success. + // att_quat will be an earth-frame quaternion rotated such that + // yaw is in body-frame. virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0; // get mount's mode @@ -72,6 +74,8 @@ class AP_Mount_Backend // set_roi_target - sets target location that mount should attempt to point towards void set_roi_target(const Location &target_loc); + // clear_roi_target - clears target location that mount should attempt to point towards + void clear_roi_target(); // set_sys_target - sets system that mount should attempt to point towards void set_target_sysid(uint8_t sysid); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index d135c3099f808..9fa9f5fd4f6a4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -1116,12 +1116,18 @@ void NavEKF3_core::CovariancePrediction(Vector3F *rotVarVecPtr) lastInhibitMagStates = inhibitMagStates; if (!inhibitWindStates) { - ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); - if (!tasDataDelayed.allowFusion) { - // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor - windVelVar *= 10.0f; + const bool isDragFusionDeadReckoning = filterStatus.flags.dead_reckoning && !dragTimeout; + if (isDragFusionDeadReckoning) { + // when dead reckoning using drag fusion stop learning wind states to provide a more stable velocity estimate + P[23][23] = P[22][22] = 0.0f; + } else { + ftype windVelVar = sq(dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate))); + if (!tasDataDelayed.allowFusion) { + // Allow wind states to recover faster when using sideslip fusion with a failed airspeed sesnor + windVelVar *= 10.0f; + } + for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar; } - for (uint8_t i=12; i<=13; i++) processNoiseVariance[i] = windVelVar; } // set variables used to calculate covariance growth diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 80103df72320b..3a2319b12c528 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -32,7 +32,7 @@ #include "DiscreteRGBLed.h" #include "DiscoLED.h" #include "Led_Sysfs.h" -#include "UAVCAN_RGB_LED.h" +#include "DroneCAN_RGB_LED.h" #include "SITL_SFML_LED.h" #include #include "AP_BoardLED2.h" @@ -79,7 +79,7 @@ AP_Notify *AP_Notify::_singleton; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS |\ - Notify_LED_UAVCAN) + Notify_LED_DroneCAN) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET || \ @@ -347,10 +347,10 @@ void AP_Notify::add_backends(void) } #endif break; - case Notify_LED_UAVCAN: -#if HAL_CANMANAGER_ENABLED - ADD_BACKEND(new UAVCAN_RGB_LED(0)); -#endif // HAL_CANMANAGER_ENABLED + case Notify_LED_DroneCAN: +#if HAL_ENABLE_DRONECAN_DRIVERS + ADD_BACKEND(new DroneCAN_RGB_LED(0)); +#endif // HAL_ENABLE_DRONECAN_DRIVERS break; case Notify_LED_Scripting: diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 5649966f3722a..08cbd462cc9d4 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -69,7 +69,7 @@ class AP_Notify Notify_LED_PCA9685LED_I2C_External = (1 << 3), // External PCA9685_I2C #endif Notify_LED_OreoLED = (1 << 4), // Oreo - Notify_LED_UAVCAN = (1 << 5), // UAVCAN RGB LED + Notify_LED_DroneCAN = (1 << 5), // UAVCAN RGB LED #if AP_NOTIFY_NCP5623_ENABLED Notify_LED_NCP5623_I2C_External = (1 << 6), // External NCP5623 Notify_LED_NCP5623_I2C_Internal = (1 << 7), // Internal NCP5623 @@ -123,6 +123,7 @@ class AP_Notify bool powering_off; // true when the vehicle is powering off bool video_recording; // true when the vehicle is recording video bool temp_cal_running; // true if a temperature calibration is running + bool gyro_calibrated; // true if calibrated gyro/acc }; /// notify_events_type - bitmask of active events. diff --git a/libraries/AP_Notify/Buzzer.cpp b/libraries/AP_Notify/Buzzer.cpp index acfde7ce5e1cb..14d7b3299670a 100644 --- a/libraries/AP_Notify/Buzzer.cpp +++ b/libraries/AP_Notify/Buzzer.cpp @@ -66,6 +66,18 @@ void Buzzer::update_pattern_to_play() return; } + // initializing? + if (_flags.gyro_calibrated != AP_Notify::flags.gyro_calibrated) { + _flags.gyro_calibrated = AP_Notify::flags.gyro_calibrated; + play_pattern(INIT_GYRO); + } + + // check if prearm check are good + if (AP_Notify::flags.pre_arm_check && !_flags.pre_arm_check) { + _flags.pre_arm_check = true; + play_pattern(PRE_ARM_GOOD); + } + // check if armed status has changed if (_flags.armed != AP_Notify::flags.armed) { _flags.armed = AP_Notify::flags.armed; diff --git a/libraries/AP_Notify/Buzzer.h b/libraries/AP_Notify/Buzzer.h index 4098148e76d37..8bf81d18fc5ac 100644 --- a/libraries/AP_Notify/Buzzer.h +++ b/libraries/AP_Notify/Buzzer.h @@ -43,6 +43,8 @@ class Buzzer: public NotifyDevice static const uint32_t ARMING_BUZZ = 0b11111111111111111111111111111100UL; // 3s static const uint32_t BARO_BUZZ = 0b10101010100000000000000000000000UL; static const uint32_t EKF_BAD = 0b11101101010000000000000000000000UL; + static const uint32_t INIT_GYRO = 0b10010010010010010010000000000000UL; + static const uint32_t PRE_ARM_GOOD = 0b10101011111111110000000000000000UL; /// play_pattern - plays the defined buzzer pattern void play_pattern(const uint32_t pattern); @@ -54,6 +56,8 @@ class Buzzer: public NotifyDevice uint8_t armed : 1; // 0 = disarmed, 1 = armed uint8_t failsafe_battery : 1; // 1 if battery failsafe has triggered uint8_t ekf_bad : 1; // 1 if ekf position has gone bad + uint8_t gyro_calibrated : 1; // 1 if calibrating gyro + uint8_t pre_arm_check : 1; // 1 if pre-arm check has passed } _flags; uint32_t _pattern; // current pattern diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp b/libraries/AP_Notify/DroneCAN_RGB_LED.cpp similarity index 77% rename from libraries/AP_Notify/UAVCAN_RGB_LED.cpp rename to libraries/AP_Notify/DroneCAN_RGB_LED.cpp index 3d21c095374c8..069f23d5b14b5 100644 --- a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp +++ b/libraries/AP_Notify/DroneCAN_RGB_LED.cpp @@ -17,10 +17,10 @@ #include #include -#if HAL_CANMANAGER_ENABLED -#include "UAVCAN_RGB_LED.h" +#if HAL_ENABLE_DRONECAN_DRIVERS +#include "DroneCAN_RGB_LED.h" -#include +#include #include @@ -29,13 +29,13 @@ #define LED_MEDIUM ((LED_FULL_BRIGHT / 5) * 4) #define LED_DIM ((LED_FULL_BRIGHT / 5) * 2) -UAVCAN_RGB_LED::UAVCAN_RGB_LED(uint8_t led_index) - : UAVCAN_RGB_LED(led_index, LED_OFF, +DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_index) + : DroneCAN_RGB_LED(led_index, LED_OFF, LED_FULL_BRIGHT, LED_MEDIUM, LED_DIM) { } -UAVCAN_RGB_LED::UAVCAN_RGB_LED(uint8_t led_index, uint8_t led_off, +DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_index, uint8_t led_off, uint8_t led_full, uint8_t led_medium, uint8_t led_dim) : RGBLed(led_off, led_full, led_medium, led_dim) @@ -43,11 +43,11 @@ UAVCAN_RGB_LED::UAVCAN_RGB_LED(uint8_t led_index, uint8_t led_off, { } -bool UAVCAN_RGB_LED::init() +bool DroneCAN_RGB_LED::init() { const uint8_t can_num_drivers = AP::can().get_num_drivers(); for (uint8_t i = 0; i < can_num_drivers; i++) { - AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); + AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i); if (uavcan != nullptr) { return true; } @@ -57,13 +57,13 @@ bool UAVCAN_RGB_LED::init() } -bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +bool DroneCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) { bool success = false; uint8_t can_num_drivers = AP::can().get_num_drivers(); for (uint8_t i = 0; i < can_num_drivers; i++) { - AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); + AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i); if (uavcan != nullptr) { success = uavcan->led_write(_led_index, red, green, blue) || success; } diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.h b/libraries/AP_Notify/DroneCAN_RGB_LED.h similarity index 62% rename from libraries/AP_Notify/UAVCAN_RGB_LED.h rename to libraries/AP_Notify/DroneCAN_RGB_LED.h index ddb19eee2b327..e2c2d10f645a1 100644 --- a/libraries/AP_Notify/UAVCAN_RGB_LED.h +++ b/libraries/AP_Notify/DroneCAN_RGB_LED.h @@ -2,11 +2,11 @@ #include "RGBLed.h" -class UAVCAN_RGB_LED: public RGBLed { +class DroneCAN_RGB_LED: public RGBLed { public: - UAVCAN_RGB_LED(uint8_t led_index, uint8_t led_off, uint8_t led_full, + DroneCAN_RGB_LED(uint8_t led_index, uint8_t led_off, uint8_t led_full, uint8_t led_medium, uint8_t led_dim); - UAVCAN_RGB_LED(uint8_t led_index); + DroneCAN_RGB_LED(uint8_t led_index); bool init() override; protected: diff --git a/libraries/AP_Notify/MMLPlayer.cpp b/libraries/AP_Notify/MMLPlayer.cpp index a48fc1f127689..7bde0a03eb45f 100644 --- a/libraries/AP_Notify/MMLPlayer.cpp +++ b/libraries/AP_Notify/MMLPlayer.cpp @@ -7,7 +7,7 @@ #include #if HAL_CANMANAGER_ENABLED -#include +#include #include #endif @@ -64,12 +64,12 @@ void MMLPlayer::start_note(float duration, float frequency, float volume) _note_duration_us = duration*1e6; hal.util->toneAlarm_set_buzzer_tone(frequency, volume, _note_duration_us/1000U); -#if HAL_CANMANAGER_ENABLED +#if HAL_ENABLE_DRONECAN_DRIVERS // support CAN buzzers too uint8_t can_num_drivers = AP::can().get_num_drivers(); for (uint8_t i = 0; i < can_num_drivers; i++) { - AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); + AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i); if (uavcan != nullptr && (AP::notify().get_buzzer_types() & AP_Notify::Notify_Buzz_UAVCAN)) { uavcan->set_buzzer_tone(frequency, _note_duration_us*1.0e-6); diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 3088cd96c6fe7..e883c8c1defbc 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -85,7 +85,7 @@ const AP_Param::GroupInfo AP_OSD::var_info[] = { // @Param: _OPTIONS // @DisplayName: OSD Options // @Description: This sets options that change the display - // @Bitmask: 0:UseDecimalPack, 1:InvertedWindPointer, 2:InvertedAHRoll, 3:Convert feet to miles at 5280ft instead of 10000ft, 4:DisableCrosshair + // @Bitmask: 0:UseDecimalPack, 1:InvertedWindArrow, 2:InvertedAHRoll, 3:Convert feet to miles at 5280ft instead of 10000ft, 4:DisableCrosshair, 5:TranslateArrows // @User: Standard AP_GROUPINFO("_OPTIONS", 8, AP_OSD, options, OPTION_DECIMAL_PACK), diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index eaadd0e0d1150..9dcc8b1fc8d9f 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -271,6 +271,7 @@ class AP_OSD_Screen : public AP_OSD_AbstractScreen //helper functions void draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude); void draw_distance(uint8_t x, uint8_t y, float distance); + char get_arrow_font_index (int32_t angle_cd); #if HAL_WITH_ESC_TELEM void draw_esc_temp(uint8_t x, uint8_t y); void draw_esc_rpm(uint8_t x, uint8_t y); @@ -537,6 +538,7 @@ class AP_OSD OPTION_INVERTED_AH_ROLL = 1U<<2, OPTION_IMPERIAL_MILES = 1U<<3, OPTION_DISABLE_CROSSHAIR = 1U<<4, + OPTION_BF_ARROWS = 1U<<5, }; enum { diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index ca931576a9902..86ca4647dbb46 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -1291,6 +1291,17 @@ float AP_OSD_AbstractScreen::u_scale(enum unit_type unit, float value) return value * scale[units][unit] + (offsets[units]?offsets[units][unit]:0); } +char AP_OSD_Screen::get_arrow_font_index(int32_t angle_cd) +{ + uint32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT); + angle_cd = wrap_360_cd(angle_cd); + // if using BF font table must translate arrows + if (check_option(AP_OSD::OPTION_BF_ARROWS)) { + angle_cd = angle_cd > 18000? 54000 - angle_cd : 18000- angle_cd; + } + return SYMBOL(SYM_ARROW_START) + ((angle_cd + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT); +} + void AP_OSD_Screen::draw_altitude(uint8_t x, uint8_t y) { float alt; @@ -1510,8 +1521,8 @@ void AP_OSD_Screen::draw_message(uint8_t x, uint8_t y) // draw a arrow at the given angle, and print the given magnitude void AP_OSD_Screen::draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude) { - static const int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT); - char arrow = SYMBOL(SYM_ARROW_START) + ((int32_t(angle_rad*DEGX100) + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT); + int32_t angle_cd = angle_rad * DEGX100; + char arrow = get_arrow_font_index(angle_cd); if (u_scale(SPEED, magnitude) < 9.95) { backend->write(x, y, false, "%c %1.1f%c", arrow, u_scale(SPEED, magnitude), u_icon(SPEED)); } else { @@ -1525,13 +1536,11 @@ void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y) WITH_SEMAPHORE(ahrs.get_semaphore()); Vector2f v = ahrs.groundspeed_vector(); backend->write(x, y, false, "%c", SYMBOL(SYM_GSPD)); - float angle = 0; const float length = v.length(); if (length > 1.0f) { - angle = wrap_2PI(atan2f(v.y, v.x) - ahrs.yaw); + angle = atan2f(v.y, v.x) - ahrs.yaw; } - draw_speed(x + 1, y, angle, length); } @@ -1617,13 +1626,12 @@ void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y) if (ahrs.get_location(loc) && ahrs.home_is_set()) { const Location &home_loc = ahrs.get_home(); float distance = home_loc.get_distance(loc); - int32_t angle = wrap_360_cd(loc.get_bearing_to(home_loc) - ahrs.yaw_sensor); - int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT); + int32_t angle_cd = loc.get_bearing_to(home_loc) - ahrs.yaw_sensor; if (distance < 2.0f) { //avoid fast rotating arrow at small distances - angle = 0; + angle_cd = 0; } - char arrow = SYMBOL(SYM_ARROW_START) + ((angle + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT); + char arrow = get_arrow_font_index(angle_cd); backend->write(x, y, false, "%c%c", SYMBOL(SYM_HOME), arrow); draw_distance(x+2, y, distance); } else { @@ -1755,14 +1763,14 @@ void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y) if (check_option(AP_OSD::OPTION_INVERTED_WIND)) { angle = M_PI; } - angle = wrap_2PI(angle + atan2f(v.y, v.x) - ahrs.yaw); - } + angle = angle + atan2f(v.y, v.x) - ahrs.yaw; + } draw_speed(x + 1, y, angle, length); #else const AP_WindVane* windvane = AP_WindVane::get_singleton(); if (windvane != nullptr) { - draw_speed(x + 1, y, wrap_2PI(windvane->get_apparent_wind_direction_rad() + M_PI), windvane->get_apparent_wind_speed()); + draw_speed(x + 1, y, windvane->get_apparent_wind_direction_rad() + M_PI, windvane->get_apparent_wind_speed()); } #endif @@ -1924,13 +1932,12 @@ void AP_OSD_Screen::draw_hdop(uint8_t x, uint8_t y) void AP_OSD_Screen::draw_waypoint(uint8_t x, uint8_t y) { AP_AHRS &ahrs = AP::ahrs(); - int32_t angle = wrap_360_cd(osd->nav_info.wp_bearing - ahrs.yaw_sensor); - int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT); + int32_t angle_cd = osd->nav_info.wp_bearing - ahrs.yaw_sensor; if (osd->nav_info.wp_distance < 2.0f) { //avoid fast rotating arrow at small distances - angle = 0; + angle_cd = 0; } - char arrow = SYMBOL(SYM_ARROW_START) + ((angle + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT); + char arrow = get_arrow_font_index(angle_cd); backend->write(x,y, false, "%c%2u%c",SYMBOL(SYM_WPNO), osd->nav_info.wp_number, arrow); draw_distance(x+4, y, osd->nav_info.wp_distance); } diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.h b/libraries/AP_OpenDroneID/AP_OpenDroneID.h index e3d8eaa21a997..217ada58b7422 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.h +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.h @@ -63,7 +63,7 @@ #define ODID_AREA_COUNT_MIN 1 #define ODID_AREA_COUNT_MAX 65000 -class AP_UAVCAN; +class AP_DroneCAN; class AP_OpenDroneID { @@ -81,7 +81,7 @@ class AP_OpenDroneID void update(); // send pending dronecan messages - void dronecan_send(AP_UAVCAN *); + void dronecan_send(AP_DroneCAN *); // handle a message from the GCS void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg); @@ -193,12 +193,12 @@ class AP_OpenDroneID uint8_t dronecan_done_init; uint8_t dronecan_init_failed; - void dronecan_init(AP_UAVCAN *uavcan); - void dronecan_send_location(AP_UAVCAN *uavcan); - void dronecan_send_basic_id(AP_UAVCAN *uavcan); - void dronecan_send_system(AP_UAVCAN *uavcan); - void dronecan_send_self_id(AP_UAVCAN *uavcan); - void dronecan_send_operator_id(AP_UAVCAN *uavcan); + void dronecan_init(AP_DroneCAN *uavcan); + void dronecan_send_location(AP_DroneCAN *uavcan); + void dronecan_send_basic_id(AP_DroneCAN *uavcan); + void dronecan_send_system(AP_DroneCAN *uavcan); + void dronecan_send_self_id(AP_DroneCAN *uavcan); + void dronecan_send_operator_id(AP_DroneCAN *uavcan); }; namespace AP diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp index bd7a1df8647cf..9928f0e9d427b 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID_DroneCAN.cpp @@ -20,31 +20,20 @@ #if AP_OPENDRONEID_ENABLED -#include - -#if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include -#include -#include -#include -#include -#include +#include +#if HAL_ENABLE_DRONECAN_DRIVERS #include -static uavcan::Publisher* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -// handle ArmStatus -UC_REGISTRY_BINDER(ArmStatusCb, dronecan::remoteid::ArmStatus); -static uavcan::Subscriber *arm_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_location[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_basic_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_self_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_system[HAL_MAX_CAN_PROTOCOL_DRIVERS]; +static Canard::Publisher* dc_operator_id[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static void handle_arm_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ArmStatusCb &cb); +static void handle_arm_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer &transfer, const dronecan_remoteid_ArmStatus &msg); -void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan) +void AP_OpenDroneID::dronecan_init(AP_DroneCAN *uavcan) { const uint8_t driver_index = uavcan->get_driver_index(); const uint8_t driver_mask = 1U<(*uavcan->get_node()); + dc_location[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_location[driver_index] == nullptr) { goto alloc_failed; } - dc_location[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_location[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_location[driver_index]->set_timeout_ms(20); + dc_location[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_basic_id[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_basic_id[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_basic_id[driver_index] == nullptr) { goto alloc_failed; } - dc_basic_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_basic_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_basic_id[driver_index]->set_timeout_ms(20); + dc_basic_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_self_id[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_self_id[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_self_id[driver_index] == nullptr) { goto alloc_failed; } - dc_self_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_self_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_self_id[driver_index]->set_timeout_ms(20); + dc_self_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_system[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_system[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_system[driver_index] == nullptr) { goto alloc_failed; } - dc_system[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_system[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_system[driver_index]->set_timeout_ms(20); + dc_system[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - dc_operator_id[driver_index] = new uavcan::Publisher(*uavcan->get_node()); + dc_operator_id[driver_index] = new Canard::Publisher(uavcan->get_canard_iface()); if (dc_operator_id[driver_index] == nullptr) { goto alloc_failed; } - dc_operator_id[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - dc_operator_id[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); + dc_operator_id[driver_index]->set_timeout_ms(20); + dc_operator_id[driver_index]->set_priority(CANARD_TRANSFER_PRIORITY_LOW); - arm_status_listener[driver_index] = new uavcan::Subscriber(*uavcan->get_node()); - if (arm_status_listener[driver_index] == nullptr) { + if (Canard::allocate_sub_arg_callback(uavcan, &handle_arm_status, driver_index) == nullptr) + { goto alloc_failed; } - arm_status_listener[driver_index]->start(ArmStatusCb(uavcan, &handle_arm_status)); dronecan_done_init |= driver_mask; return; @@ -105,7 +93,7 @@ void AP_OpenDroneID::dronecan_init(AP_UAVCAN *uavcan) /* send pending DroneCAN OpenDroneID packets */ -void AP_OpenDroneID::dronecan_send(AP_UAVCAN *uavcan) +void AP_OpenDroneID::dronecan_send(AP_DroneCAN *uavcan) { const uint8_t driver_index = uavcan->get_driver_index(); const uint8_t driver_mask = 1U<get_driver_index()]->broadcast(msg); } -void AP_OpenDroneID::dronecan_send_basic_id(AP_UAVCAN *uavcan) +void AP_OpenDroneID::dronecan_send_basic_id(AP_DroneCAN *uavcan) { - dronecan::remoteid::BasicID msg {}; + dronecan_remoteid_BasicID msg {}; const auto &pkt = pkt_basic_id; ODID_COPY_STR(id_or_mac); ODID_COPY(id_type); @@ -187,9 +175,9 @@ void AP_OpenDroneID::dronecan_send_basic_id(AP_UAVCAN *uavcan) dc_basic_id[uavcan->get_driver_index()]->broadcast(msg); } -void AP_OpenDroneID::dronecan_send_system(AP_UAVCAN *uavcan) +void AP_OpenDroneID::dronecan_send_system(AP_DroneCAN *uavcan) { - dronecan::remoteid::System msg {}; + dronecan_remoteid_System msg {}; const auto &pkt = pkt_system; ODID_COPY_STR(id_or_mac); ODID_COPY(operator_location_type); @@ -207,9 +195,9 @@ void AP_OpenDroneID::dronecan_send_system(AP_UAVCAN *uavcan) dc_system[uavcan->get_driver_index()]->broadcast(msg); } -void AP_OpenDroneID::dronecan_send_self_id(AP_UAVCAN *uavcan) +void AP_OpenDroneID::dronecan_send_self_id(AP_DroneCAN *uavcan) { - dronecan::remoteid::SelfID msg {}; + dronecan_remoteid_SelfID msg {}; const auto &pkt = pkt_self_id; ODID_COPY_STR(id_or_mac); ODID_COPY(description_type); @@ -217,9 +205,9 @@ void AP_OpenDroneID::dronecan_send_self_id(AP_UAVCAN *uavcan) dc_self_id[uavcan->get_driver_index()]->broadcast(msg); } -void AP_OpenDroneID::dronecan_send_operator_id(AP_UAVCAN *uavcan) +void AP_OpenDroneID::dronecan_send_operator_id(AP_DroneCAN *uavcan) { - dronecan::remoteid::OperatorID msg {}; + dronecan_remoteid_OperatorID msg {}; const auto &pkt = pkt_operator_id; ODID_COPY_STR(id_or_mac); ODID_COPY(operator_id_type); @@ -230,13 +218,12 @@ void AP_OpenDroneID::dronecan_send_operator_id(AP_UAVCAN *uavcan) /* handle ArmStatus message from DroneCAN */ -static void handle_arm_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ArmStatusCb &cb) +static void handle_arm_status(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const dronecan_remoteid_ArmStatus &msg) { - const auto &msg = *cb.msg; mavlink_open_drone_id_arm_status_t status {}; status.status = msg.status; - strncpy_noterm(status.error, msg.error.c_str(), sizeof(status.error)); + strncpy_noterm((char*)status.error, (const char*)msg.error.data, sizeof(status.error)); AP::opendroneid().set_arm_status(status); @@ -252,5 +239,5 @@ void AP_OpenDroneID::set_arm_status(mavlink_open_drone_id_arm_status_t &status) last_arm_status_ms = AP_HAL::millis(); } -#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS +#endif // HAL_ENABLE_DRONECAN_DRIVERS #endif // AP_OPENDRONEID_ENABLED diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp index bbf37c87ade11..e7a94704420d0 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp @@ -5,18 +5,14 @@ #include #include -#include - -#include +#include +#include extern const AP_HAL::HAL& hal; -//UAVCAN Frontend Registry Binder -UC_REGISTRY_BINDER(MeasurementCb, com::hex::equipment::flow::Measurement); - uint8_t AP_OpticalFlow_HereFlow::_node_id = 0; AP_OpticalFlow_HereFlow* AP_OpticalFlow_HereFlow::_driver = nullptr; -AP_UAVCAN* AP_OpticalFlow_HereFlow::_ap_uavcan = nullptr; +AP_DroneCAN* AP_OpticalFlow_HereFlow::_ap_dronecan = nullptr; /* constructor - registers instance at top Flow driver */ @@ -30,44 +26,37 @@ AP_OpticalFlow_HereFlow::AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow) : } //links the HereFlow messages to the backend -void AP_OpticalFlow_HereFlow::subscribe_msgs(AP_UAVCAN* ap_uavcan) +void AP_OpticalFlow_HereFlow::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return; } - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *measurement_listener; - measurement_listener = new uavcan::Subscriber(*node); - // Register method to handle incoming HereFlow measurement - const int measurement_listener_res = measurement_listener->start(MeasurementCb(ap_uavcan, &handle_measurement)); - if (measurement_listener_res < 0) { - AP_HAL::panic("UAVCAN Flow subscriber start problem\n\r"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("measurement_sub"); } } //updates driver states based on received HereFlow messages -void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb) +void AP_OpticalFlow_HereFlow::handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const com_hex_equipment_flow_Measurement &msg) { if (_driver == nullptr) { return; } //protect from data coming from duplicate sensors, //as we only handle one Here Flow at a time as of now - if (_ap_uavcan == nullptr) { - _ap_uavcan = ap_uavcan; - _node_id = node_id; + if (_ap_dronecan == nullptr) { + _ap_dronecan = ap_dronecan; + _node_id = transfer.source_node_id; } - if (_ap_uavcan == ap_uavcan && _node_id == node_id) { + if (_ap_dronecan == ap_dronecan && _node_id == transfer.source_node_id) { WITH_SEMAPHORE(_driver->_sem); _driver->new_data = true; - _driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]); - _driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]); - _driver->integral_time = cb.msg->integration_interval; - _driver->surface_quality = cb.msg->quality; + _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->integral_time = msg.integration_interval; + _driver->surface_quality = msg.quality; } } diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h index e279a118ebffb..fa340584ff2f2 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h @@ -3,14 +3,12 @@ #include "AP_OpticalFlow.h" #ifndef AP_OPTICALFLOW_HEREFLOW_ENABLED -#define AP_OPTICALFLOW_HEREFLOW_ENABLED (AP_OPTICALFLOW_ENABLED && HAL_ENABLE_LIBUAVCAN_DRIVERS) +#define AP_OPTICALFLOW_HEREFLOW_ENABLED (AP_OPTICALFLOW_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS) #endif #if AP_OPTICALFLOW_HEREFLOW_ENABLED -#include - -class MeasurementCb; +#include class AP_OpticalFlow_HereFlow : public OpticalFlow_backend { public: @@ -20,9 +18,9 @@ class AP_OpticalFlow_HereFlow : public OpticalFlow_backend { void update() override; - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); - static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb); + static void handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const com_hex_equipment_flow_Measurement &msg); private: @@ -33,7 +31,7 @@ class AP_OpticalFlow_HereFlow : public OpticalFlow_backend { static uint8_t _node_id; static AP_OpticalFlow_HereFlow* _driver; - static AP_UAVCAN* _ap_uavcan; + static AP_DroneCAN* _ap_dronecan; void _push_state(void); }; diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 2a1f9d95bd386..866e5b3460ae3 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -173,8 +173,8 @@ void AP_Proximity::init() } break; - case Type::CYGBOT_D1: #if AP_PROXIMITY_CYGBOT_ENABLED + case Type::CYGBOT_D1: if (AP_Proximity_Cygbot_D1::detect(serial_instance)) { state[instance].instance = instance; drivers[instance] = new AP_Proximity_Cygbot_D1(*this, state[instance], params[instance], serial_instance); diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index d521d87359ce3..e5ebb32a33a4e 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -59,7 +59,9 @@ class AP_Proximity SITL = 10, AirSimSITL = 12, #endif +#if AP_PROXIMITY_CYGBOT_ENABLED CYGBOT_D1 = 13, +#endif DroneCAN = 14, }; diff --git a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h index 3e844732f9127..aaa1de91c8d69 100644 --- a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h +++ b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h @@ -2,10 +2,6 @@ #include "AP_Proximity.h" -#ifndef AP_PROXIMITY_CYGBOT_ENABLED -#define AP_PROXIMITY_CYGBOT_ENABLED HAL_PROXIMITY_ENABLED -#endif - #if (HAL_PROXIMITY_ENABLED && AP_PROXIMITY_CYGBOT_ENABLED) #include "AP_Proximity_Backend_Serial.h" diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp index dd94bca0746af..b5b3e8265ab69 100644 --- a/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp +++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp @@ -19,16 +19,12 @@ #include #include -#include +#include #include #include -#include - extern const AP_HAL::HAL& hal; -//DroneCAN Frontend Registry Binder ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY -UC_REGISTRY_BINDER(MeasurementCb, ardupilot::equipment::proximity_sensor::Proximity); ObjectBuffer_TS AP_Proximity_DroneCAN::items(50); @@ -36,32 +32,21 @@ ObjectBuffer_TS AP_Proximity_DroneCAN::item //links the Proximity DroneCAN message to this backend -void AP_Proximity_DroneCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +void AP_Proximity_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return; } - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *measurement_listener; - measurement_listener = new uavcan::Subscriber(*node); - if (measurement_listener == nullptr) { - AP_BoardConfig::allocation_error("DroneCAN_PRX"); - } - - // Register method to handle incoming Proximity measurement - const int measurement_listener_res = measurement_listener->start(MeasurementCb(ap_uavcan, &handle_measurement)); - if (measurement_listener_res < 0) { - AP_BoardConfig::allocation_error("DroneCAN Proximity subscriber start problem\n\r"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("measurement_sub"); } } //Method to find the backend relating to the node id -AP_Proximity_DroneCAN* AP_Proximity_DroneCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new) +AP_Proximity_DroneCAN* AP_Proximity_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return nullptr; } @@ -79,7 +64,7 @@ AP_Proximity_DroneCAN* AP_Proximity_DroneCAN::get_uavcan_backend(AP_UAVCAN* ap_u } //Double check if the driver was initialised as DroneCAN Type if (driver != nullptr && (driver->_backend_type == AP_Proximity::Type::DroneCAN)) { - if (driver->_ap_uavcan == ap_uavcan && + if (driver->_ap_dronecan == ap_dronecan && driver->_node_id == node_id) { return driver; } else { @@ -114,8 +99,8 @@ AP_Proximity_DroneCAN* AP_Proximity_DroneCAN::get_uavcan_backend(AP_UAVCAN* ap_u unsigned(i), unsigned(i)); } //Assign node id and respective dronecan driver, for identification - if (driver->_ap_uavcan == nullptr) { - driver->_ap_uavcan = ap_uavcan; + if (driver->_ap_dronecan == nullptr) { + driver->_ap_dronecan = ap_dronecan; driver->_node_id = node_id; break; } @@ -169,20 +154,20 @@ float AP_Proximity_DroneCAN::distance_min() const } //Proximity message handler -void AP_Proximity_DroneCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb) +void AP_Proximity_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_proximity_sensor_Proximity &msg) { //fetch the matching DroneCAN driver, node id and sensor id backend instance - AP_Proximity_DroneCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->sensor_id, true); + AP_Proximity_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.sensor_id, true); if (driver == nullptr) { return; } WITH_SEMAPHORE(driver->_sem); - switch (cb.msg->reading_type) { - case ardupilot::equipment::proximity_sensor::Proximity::READING_TYPE_GOOD: { + switch (msg.reading_type) { + case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_GOOD: { //update the states in backend instance driver->_last_update_ms = AP_HAL::millis(); driver->_status = AP_Proximity::Status::Good; - const ObstacleItem item = {cb.msg->yaw, cb.msg->pitch, cb.msg->distance}; + const ObstacleItem item = {msg.yaw, msg.pitch, msg.distance}; if (driver->items.space()) { // ignore reading if no place to put it in the queue @@ -191,12 +176,12 @@ void AP_Proximity_DroneCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t nod break; } //Additional states supported by Proximity message - case ardupilot::equipment::proximity_sensor::Proximity::READING_TYPE_NOT_CONNECTED: { + case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NOT_CONNECTED: { driver->_last_update_ms = AP_HAL::millis(); driver->_status = AP_Proximity::Status::NotConnected; break; } - case ardupilot::equipment::proximity_sensor::Proximity::READING_TYPE_NO_DATA: { + case ARDUPILOT_EQUIPMENT_PROXIMITY_SENSOR_PROXIMITY_READING_TYPE_NO_DATA: { driver->_last_update_ms = AP_HAL::millis(); driver->_status = AP_Proximity::Status::NoData; break; diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h index 3a1eab9a40407..a5c3c731d17de 100644 --- a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h +++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h @@ -2,14 +2,13 @@ #include "AP_Proximity_Backend.h" -#include +#include #ifndef AP_PROXIMITY_DRONECAN_ENABLED -#define AP_PROXIMITY_DRONECAN_ENABLED (HAL_CANMANAGER_ENABLED && HAL_PROXIMITY_ENABLED) +#define AP_PROXIMITY_DRONECAN_ENABLED (HAL_ENABLE_DRONECAN_DRIVERS && HAL_PROXIMITY_ENABLED) #endif #if AP_PROXIMITY_DRONECAN_ENABLED -class MeasurementCb; class AP_Proximity_DroneCAN : public AP_Proximity_Backend { @@ -25,18 +24,18 @@ class AP_Proximity_DroneCAN : public AP_Proximity_Backend float distance_min() const override; - static AP_Proximity_DroneCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new); + static AP_Proximity_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new); - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); - static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb); + static void handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_proximity_sensor_Proximity &msg); private: uint32_t _last_update_ms; // system time of last message received - AP_UAVCAN* _ap_uavcan; + AP_DroneCAN* _ap_dronecan; uint8_t _node_id; struct ObstacleItem { diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index 92e87818bdc76..c6ca9e029132a 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -5,3 +5,7 @@ #ifndef HAL_PROXIMITY_ENABLED #define HAL_PROXIMITY_ENABLED (!HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024) #endif + +#ifndef AP_PROXIMITY_CYGBOT_ENABLED +#define AP_PROXIMITY_CYGBOT_ENABLED HAL_PROXIMITY_ENABLED +#endif diff --git a/libraries/AP_Radio/AP_Radio_cc2500.cpp b/libraries/AP_Radio/AP_Radio_cc2500.cpp index e5e3b443af154..fd6fc0a676e43 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.cpp +++ b/libraries/AP_Radio/AP_Radio_cc2500.cpp @@ -414,7 +414,7 @@ void AP_Radio_cc2500::trigger_irq_radio_event() chSysUnlockFromISR(); } -void AP_Radio_cc2500::trigger_timeout_event(void *arg) +void AP_Radio_cc2500::trigger_timeout_event(virtual_timer_t* vt, void *arg) { (void)arg; //we are called from ISR context diff --git a/libraries/AP_Radio/AP_Radio_cc2500.h b/libraries/AP_Radio/AP_Radio_cc2500.h index ce5feb0c1fa94..1ddc608173ad3 100644 --- a/libraries/AP_Radio/AP_Radio_cc2500.h +++ b/libraries/AP_Radio/AP_Radio_cc2500.h @@ -87,7 +87,7 @@ class AP_Radio_cc2500 : public AP_Radio_backend static void irq_handler_thd(void* arg); static void trigger_irq_radio_event(void); - static void trigger_timeout_event(void *arg); + static void trigger_timeout_event(virtual_timer_t* vt, void *arg); void radio_init(void); diff --git a/libraries/AP_Radio/AP_Radio_cypress.cpp b/libraries/AP_Radio/AP_Radio_cypress.cpp index a66de153587e5..ff6f99b4b65ef 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.cpp +++ b/libraries/AP_Radio/AP_Radio_cypress.cpp @@ -1199,7 +1199,7 @@ void AP_Radio_cypress::irq_handler_thd(void *arg) } } -void AP_Radio_cypress::trigger_timeout_event(void *arg) +void AP_Radio_cypress::trigger_timeout_event(virtual_timer_t* vt, void *arg) { (void)arg; //we are called from ISR context diff --git a/libraries/AP_Radio/AP_Radio_cypress.h b/libraries/AP_Radio/AP_Radio_cypress.h index 7aa49f615a67e..15c63d528d707 100644 --- a/libraries/AP_Radio/AP_Radio_cypress.h +++ b/libraries/AP_Radio/AP_Radio_cypress.h @@ -131,7 +131,7 @@ class AP_Radio_cypress : public AP_Radio_backend static void irq_handler_thd(void* arg); static void trigger_irq_radio_event(void); - static void trigger_timeout_event(void *arg); + static void trigger_timeout_event(virtual_timer_t* vt, void *arg); static const uint8_t max_channels = 16; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index 977c5af6306ab..ce2be113c2cd1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder.cpp @@ -44,7 +44,7 @@ #include "AP_RangeFinder_HC_SR04.h" #include "AP_RangeFinder_Bebop.h" #include "AP_RangeFinder_BLPing.h" -#include "AP_RangeFinder_UAVCAN.h" +#include "AP_RangeFinder_DroneCAN.h" #include "AP_RangeFinder_Lanbao.h" #include "AP_RangeFinder_LeddarVu8.h" #include "AP_RangeFinder_SITL.h" @@ -488,7 +488,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance) break; case Type::UAVCAN: -#if AP_RANGEFINDER_UAVCAN_ENABLED +#if AP_RANGEFINDER_DRONECAN_ENABLED /* the UAVCAN driver gets created when we first receive a measurement. We take the instance slot now, even if we don't diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 29231548ed167..3a66a5aa6de50 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -57,7 +57,7 @@ class RangeFinder { friend class AP_RangeFinder_Backend; //UAVCAN drivers are initialised in the Backend, hence list of drivers is needed there. - friend class AP_RangeFinder_UAVCAN; + friend class AP_RangeFinder_DroneCAN; public: RangeFinder(); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp similarity index 63% rename from libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp rename to libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp index cba78545cbb2d..19cc404e48083 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp @@ -1,57 +1,45 @@ -#include "AP_RangeFinder_UAVCAN.h" +#include "AP_RangeFinder_DroneCAN.h" -#if AP_RANGEFINDER_UAVCAN_ENABLED +#if AP_RANGEFINDER_DRONECAN_ENABLED #include #include -#include +#include #include - -#include +#include extern const AP_HAL::HAL& hal; #define debug_range_finder_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { hal.console->printf(fmt, ##args); }} while (0) -//UAVCAN Frontend Registry Binder -UC_REGISTRY_BINDER(MeasurementCb, uavcan::equipment::range_sensor::Measurement); - //links the rangefinder uavcan message to this backend -void AP_RangeFinder_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +void AP_RangeFinder_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return; } - - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *measurement_listener; - measurement_listener = new uavcan::Subscriber(*node); - // Register method to handle incoming RangeFinder measurement - const int measurement_listener_res = measurement_listener->start(MeasurementCb(ap_uavcan, &handle_measurement)); - if (measurement_listener_res < 0) { - AP_HAL::panic("UAVCAN RangeFinder subscriber start problem\n\r"); - return; + if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("measurement_sub"); } } //Method to find the backend relating to the node id -AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new) +AP_RangeFinder_DroneCAN* AP_RangeFinder_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new) { - if (ap_uavcan == nullptr) { + if (ap_dronecan == nullptr) { return nullptr; } - AP_RangeFinder_UAVCAN* driver = nullptr; + AP_RangeFinder_DroneCAN* driver = nullptr; RangeFinder &frontend = *AP::rangefinder(); //Scan through the Rangefinder params to find UAVCAN RFND with matching address. for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { if ((RangeFinder::Type)frontend.params[i].type.get() == RangeFinder::Type::UAVCAN && frontend.params[i].address == address) { - driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i]; + driver = (AP_RangeFinder_DroneCAN*)frontend.drivers[i]; } //Double check if the driver was initialised as UAVCAN Type if (driver != nullptr && (driver->_backend_type == RangeFinder::Type::UAVCAN)) { - if (driver->_ap_uavcan == ap_uavcan && + if (driver->_ap_dronecan == ap_dronecan && driver->_node_id == node_id) { return driver; } else { @@ -72,16 +60,16 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u //it up as UAVCAN type return nullptr; } - frontend.drivers[i] = new AP_RangeFinder_UAVCAN(frontend.state[i], frontend.params[i]); - driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i]; + frontend.drivers[i] = new AP_RangeFinder_DroneCAN(frontend.state[i], frontend.params[i]); + driver = (AP_RangeFinder_DroneCAN*)frontend.drivers[i]; if (driver == nullptr) { break; } - gcs().send_text(MAV_SEVERITY_INFO, "RangeFinder[%u]: added UAVCAN node %u addr %u", + gcs().send_text(MAV_SEVERITY_INFO, "RangeFinder[%u]: added DroneCAN node %u addr %u", unsigned(i), unsigned(node_id), unsigned(address)); //Assign node id and respective uavcan driver, for identification - if (driver->_ap_uavcan == nullptr) { - driver->_ap_uavcan = ap_uavcan; + if (driver->_ap_dronecan == nullptr) { + driver->_ap_dronecan = ap_dronecan; driver->_node_id = node_id; break; } @@ -93,7 +81,7 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u } //Called from frontend to update with the readings received by handler -void AP_RangeFinder_UAVCAN::update() +void AP_RangeFinder_DroneCAN::update() { WITH_SEMAPHORE(_sem); if ((AP_HAL::millis() - _last_reading_ms) > 500) { @@ -112,32 +100,32 @@ void AP_RangeFinder_UAVCAN::update() } //RangeFinder message handler -void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb) +void AP_RangeFinder_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_range_sensor_Measurement &msg) { //fetch the matching uavcan driver, node id and sensor id backend instance - AP_RangeFinder_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id, cb.msg->sensor_id, true); + AP_RangeFinder_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.sensor_id, true); if (driver == nullptr) { return; } WITH_SEMAPHORE(driver->_sem); - switch (cb.msg->reading_type) { - case uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE: + switch (msg.reading_type) { + case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE: { //update the states in backend instance - driver->_distance_cm = cb.msg->range*100.0f; + driver->_distance_cm = msg.range*100.0f; driver->_last_reading_ms = AP_HAL::millis(); driver->_status = RangeFinder::Status::Good; driver->new_data = true; break; } //Additional states supported by RFND message - case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE: + case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_CLOSE: { driver->_last_reading_ms = AP_HAL::millis(); driver->_status = RangeFinder::Status::OutOfRangeLow; break; } - case uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR: + case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR: { driver->_last_reading_ms = AP_HAL::millis(); driver->_status = RangeFinder::Status::OutOfRangeHigh; @@ -149,18 +137,18 @@ void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t nod } } //copy over the sensor type of Rangefinder - switch (cb.msg->sensor_type) { - case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR: + switch (msg.sensor_type) { + case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_SONAR: { driver->_sensor_type = MAV_DISTANCE_SENSOR_ULTRASOUND; break; } - case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR: + case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR: { driver->_sensor_type = MAV_DISTANCE_SENSOR_LASER; break; } - case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR: + case UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_RADAR: { driver->_sensor_type = MAV_DISTANCE_SENSOR_RADAR; break; @@ -173,4 +161,4 @@ void AP_RangeFinder_UAVCAN::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t nod } } -#endif // AP_RANGEFINDER_UAVCAN_ENABLED +#endif // AP_RANGEFINDER_DRONECAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h new file mode 100644 index 0000000000000..62b01782def33 --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h @@ -0,0 +1,42 @@ +#pragma once + +#include "AP_RangeFinder_Backend.h" + +#ifndef AP_RANGEFINDER_DRONECAN_ENABLED +#define AP_RANGEFINDER_DRONECAN_ENABLED (HAL_ENABLE_DRONECAN_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) +#endif + +#if AP_RANGEFINDER_DRONECAN_ENABLED + +#include + +class MeasurementCb; + +class AP_RangeFinder_DroneCAN : public AP_RangeFinder_Backend { +public: + //constructor - registers instance at top RangeFinder driver + using AP_RangeFinder_Backend::AP_RangeFinder_Backend; + + void update() override; + + static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static AP_RangeFinder_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new); + static AP_RangeFinder_Backend* detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); + + static void handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_range_sensor_Measurement &msg); + +protected: + virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return _sensor_type; + } +private: + uint8_t _instance; + RangeFinder::Status _status; + uint16_t _distance_cm; + uint32_t _last_reading_ms; + AP_DroneCAN* _ap_dronecan; + uint8_t _node_id; + bool new_data; + MAV_DISTANCE_SENSOR _sensor_type; +}; +#endif // AP_RANGEFINDER_DRONECAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h deleted file mode 100644 index 5a9282274486d..0000000000000 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include "AP_RangeFinder_Backend.h" - -#ifndef AP_RANGEFINDER_UAVCAN_ENABLED -#define AP_RANGEFINDER_UAVCAN_ENABLED (HAL_CANMANAGER_ENABLED && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED) -#endif - -#if AP_RANGEFINDER_UAVCAN_ENABLED - -#include - -class MeasurementCb; - -class AP_RangeFinder_UAVCAN : public AP_RangeFinder_Backend { -public: - //constructor - registers instance at top RangeFinder driver - using AP_RangeFinder_Backend::AP_RangeFinder_Backend; - - void update() override; - - static void subscribe_msgs(AP_UAVCAN* ap_uavcan); - static AP_RangeFinder_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, uint8_t address, bool create_new); - static AP_RangeFinder_Backend* detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params); - - static void handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MeasurementCb &cb); - -protected: - virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { - return _sensor_type; - } -private: - uint8_t _instance; - RangeFinder::Status _status; - uint16_t _distance_cm; - uint32_t _last_reading_ms; - AP_UAVCAN* _ap_uavcan; - uint8_t _node_id; - bool new_data; - MAV_DISTANCE_SENSOR _sensor_type; -}; -#endif // AP_RANGEFINDER_UAVCAN_ENABLED diff --git a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua index 5ce9690daf0d9..0ea2a38faef4c 100644 --- a/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua +++ b/libraries/AP_Scripting/applets/Aerobatics/FixedWing/plane_aerobatics.lua @@ -31,17 +31,48 @@ ERR_CORR_D = bind_add_param('ERR_COR_D', 13, 2.8) AEROM_ENTRY_RATE = bind_add_param('ENTRY_RATE', 14, 60) AEROM_THR_LKAHD = bind_add_param('THR_LKAHD', 15, 1) AEROM_DEBUG = bind_add_param('DEBUG', 16, 0) +--[[ + // @Param: AEROM_THR_MIN + // @DisplayName: Minimum Throttle + // @Description: Lowest throttle used during maneuvers + // @Units: % +--]] AEROM_THR_MIN = bind_add_param('THR_MIN', 17, 0) AEROM_THR_BOOST = bind_add_param('THR_BOOST', 18, 50) AEROM_YAW_ACCEL = bind_add_param('YAW_ACCEL', 19, 1500) AEROM_LKAHD = bind_add_param('LKAHD', 20, 0.5) +--[[ + // @Param: AEROM_PATH_SCALE + // @DisplayName: Path Scale + // @Description: Scale factor for Path/Box size. 0.5 would half the distances in maneuvers. Radii are unaffected. + // @Range: 0.1 100 +--]] AEROM_PATH_SCALE = bind_add_param('PATH_SCALE', 21, 1.0) +--[[ + // @Param: AEROM_BOX_WIDTH + // @DisplayName: Box Width + // @Description: Length of aerobatic "box" + // @Units: m +--]] AEROM_BOX_WIDTH = bind_add_param('BOX_WIDTH', 22, 400) AEROM_STALL_THR = bind_add_param('STALL_THR', 23, 40) AEROM_STALL_PIT = bind_add_param('STALL_PIT', 24, -20) -- 25 was AEROM_KE_TC + +--[[ + // @Param: AEROM_KE_RUDD + // @DisplayName: KnifeEdge Rudder + // @Description: Percent of rudder normally uses to sustain knife-edge at trick speed + // @Units: % +--]] AEROM_KE_RUDD = bind_add_param('KE_RUDD', 26, 25) AEROM_KE_RUDD_LK = bind_add_param('KE_RUDD_LK', 27, 0.25) +--[[ + // @Param: AEROM_ALT_ABORT + // @DisplayName: Altitude Abort + // @Description: Maximum allowable loss in altitude during a trick or sequence from its starting altitude. + // @Units: m +--]] AEROM_ALT_ABORT = bind_add_param('ALT_ABORT',28,15) -- cope with old param values @@ -77,7 +108,11 @@ function bind_add_param2(name, idx, default_value) assert(param:add_param(PARAM_TABLE_KEY2, idx, name, default_value), string.format('could not add param %s', name)) return Parameter(PARAM_TABLE_PREFIX2 .. name) end - +--[[ + // @Param: TRIK_ENABLE + // @DisplayName: Tricks on Switch Enable + // @Description: Enables Tricks on Switch. TRIK params hidden until enabled +--]] local TRIK_ENABLE = bind_add_param2("_ENABLE", 1, 0) local TRICKS = nil local TRIK_SEL_FN = nil @@ -107,8 +142,26 @@ local function sq(x) end if TRIK_ENABLE:get() > 0 then +--[[ + // @Param: TRIK_SEL_FN + // @DisplayName: Trik Selection Scripting Function + // @Description: Setting an RC channel's _OPTION to this value will use it for trick selection + // @Range: 301 307 +--]] TRIK_SEL_FN = bind_add_param2("_SEL_FN", 2, 301) +--[[ + // @Param: TRIK_ACT_FN + // @DisplayName: Trik Action Scripting Function + // @Description: Setting an RC channel's _OPTION to this value will use it for trick action (abort,announce,execute) + // @Range: 301 307 +--]] TRIK_ACT_FN = bind_add_param2("_ACT_FN", 3, 300) +--[[ + // @Param: TRIK_COUNT + // @DisplayName: Trik Count + // @Description: Number of tricks which can be selected over the range of the trik selection RC channel + // @Range: 1 11 +--]] TRIK_COUNT = bind_add_param2("_COUNT", 4, 3) TRICKS = {} @@ -117,7 +170,7 @@ if TRIK_ENABLE:get() > 0 then for i = 1, count do local k = 5*i local prefix = string.format("%u", i) - TRICKS[i] = TrickDef(bind_add_param2(prefix .. "_ID", k+0, i), + TRICKS[i] = TrickDef(bind_add_param2(prefix .. "_ID", k+0, -1), bind_add_param2(prefix .. "_ARG1", k+1, 30), bind_add_param2(prefix .. "_ARG2", k+2, 0), bind_add_param2(prefix .. "_ARG3", k+3, 0), @@ -2481,6 +2534,11 @@ function check_trick() end if action == 1 and selection ~= last_trick_selection then local id = TRICKS[selection].id:get() + if id == -1 then + gcs:send_text(0,string.format("Trick %u not setup",selection)) + last_trick_selection = selection + return + end load_trick(id) if command_table[id] ~= nil then local cmd = command_table[id] @@ -2501,6 +2559,11 @@ function check_trick() return end local id = TRICKS[selection].id:get() + if id == -1 then + gcs:send_text(0,string.format("Trick %u not setup",selection)) + last_trick_selection = selection + return + end load_trick(id) if command_table[id] == nil then gcs:send_text(0, string.format("Invalid trick ID %u", id)) diff --git a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua b/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua index cc7ce6a042c71..a1e3b1e8dd52f 100644 --- a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua @@ -6,7 +6,7 @@ Set SERIALx_PROTOCOL = 28 (Scripting) where "x" corresponds to the serial port connected to the gimbal Set SCR_ENABLE = 1 to enable scripting and reboot the autopilot Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver - Set CAM_TRIGG_TYPE = 3 (Mount) to enable the camera driver + Set CAM1_TYPE = 4 (Mount) to enable the camera driver Set RCx_OPTION = 300 (Scripting1) to allow real-time selection of the video feed and camera control Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot Set VIEP_CAM_SWLOW, VIEP_CAM_SWMID, VIEP_CAM_SWHIGH to which cameras are controlled by the auxiliary switch @@ -41,10 +41,50 @@ assert(param:add_param(PARAM_TABLE_KEY, 5, "ZOOM_SPEED", 7), "could not add VIEP -- bind parameters to variables local MNT1_TYPE = Parameter("MNT1_TYPE") -- should be 9:Scripting + +--[[ + // @Param: VIEP_DEBUG + // @DisplayName: ViewPro debug + // @Description: ViewPro debug + // @Values: 0:Disabled, 1:Enabled, 2:Enabled including attitude reporting + // @User: Advanced +--]] local VIEP_DEBUG = Parameter("VIEP_DEBUG") -- debug level. 0:disabled 1:enabled 2:enabled with attitude reporting + +--[[ + // @Param: VIEP_CAM_SWLOW + // @DisplayName: ViewPro Camera For Switch Low + // @Description: Camera selection when switch is in low position + // @Values: 0:No change in camera selection, 1:EO1, 2:IR thermal, 3:EO1 + IR Picture-in-picture, 4:IR + EO1 Picture-in-picture, 5:Fusion, 6:IR1 13mm, 7:IR2 52mm + // @User: Standard +--]] local VIEP_CAM_SWLOW = Parameter("VIEP_CAM_SWLOW") -- RC swith low position's camera selection + +--[[ + // @Param: VIEP_CAM_SWMID + // @DisplayName: ViewPro Camera For Switch Mid + // @Description: Camera selection when switch is in middle position + // @Values: 0:No change in camera selection, 1:EO1, 2:IR thermal, 3:EO1 + IR Picture-in-picture, 4:IR + EO1 Picture-in-picture, 5:Fusion, 6:IR1 13mm, 7:IR2 52mm + // @User: Standard +--]] local VIEP_CAM_SWMID = Parameter("VIEP_CAM_SWMID") -- RC swith middle position's camera selection + +--[[ + // @Param: VIEP_CAM_SWHIGH + // @DisplayName: ViewPro Camera For Switch High + // @Description: Camera selection when switch is in high position + // @Values: 0:No change in camera selection, 1:EO1, 2:IR thermal, 3:EO1 + IR Picture-in-picture, 4:IR + EO1 Picture-in-picture, 5:Fusion, 6:IR1 13mm, 7:IR2 52mm + // @User: Standard +--]] local VIEP_CAM_SWHIGH = Parameter("VIEP_CAM_SWHIGH") -- RC swith high position's camera selection + +--[[ + // @Param: VIEP_ZOOM_SPEED + // @DisplayName: ViewPro Zoom Speed + // @Description: ViewPro Zoom Speed. Higher numbers result in faster zooming + // @Range: 0 7 + // @User: Standard +--]] local VIEP_ZOOM_SPEED = Parameter("VIEP_ZOOM_SPEED") -- zoom speed from 0 (slow) to 7 (fast) -- global definitions diff --git a/libraries/AP_Scripting/drivers/mount-viewpro-driver.md b/libraries/AP_Scripting/drivers/mount-viewpro-driver.md index 6f941ac38c14a..1c2f3609388dd 100644 --- a/libraries/AP_Scripting/drivers/mount-viewpro-driver.md +++ b/libraries/AP_Scripting/drivers/mount-viewpro-driver.md @@ -2,12 +2,13 @@ Viewpro gimbal driver lua script -How to use +# How To Use + Connect gimbal UART to one of the autopilot's serial ports Set SERIALx_PROTOCOL = 28 (Scripting) where "x" corresponds to the serial port connected to the gimbal Set SCR_ENABLE = 1 to enable scripting and reboot the autopilot Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver - Set CAM_TRIGG_TYPE = 3 (Mount) to enable camera control using the mount driver + Set CAM1_TYPE = 4 (Mount) to enable camera control using the mount driver Set RCx_OPTION = 300 (Scripting1) to allow real-time selection of the video feed and camera control Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot Set VIEP_CAM_SWLOW, VIEP_CAM_SWMID, VIEP_CAM_SWHIGH to which cameras are controlled by the auxiliary switch diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c9584d8e0b6f6..254a5417e7062 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -604,8 +604,8 @@ singleton AP_Mount method get_location_target boolean uint8_t'skip_check Locatio singleton AP_Mount method set_attitude_euler void uint8_t'skip_check float'skip_check float'skip_check float'skip_check singleton AP_Mount method get_camera_state boolean uint8_t'skip_check uint16_t'Null boolean'Null int8_t'Null int8_t'Null boolean'Null -include AP_Winch/AP_Winch.h depends APM_BUILD_COPTER_OR_HELI -singleton AP_Winch depends APM_BUILD_COPTER_OR_HELI +include AP_Winch/AP_Winch.h +singleton AP_Winch depends AP_WINCH_ENABLED && APM_BUILD_COPTER_OR_HELI singleton AP_Winch rename winch singleton AP_Winch method healthy boolean singleton AP_Winch method relax void diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 64d34730f0d8a..2f70f96eb72f2 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -28,6 +28,12 @@ extern const AP_HAL::HAL& hal; +#ifdef HAL_SERIAL1_PROTOCOL +#define SERIAL1_PROTOCOL HAL_SERIAL1_PROTOCOL +#else +#define SERIAL1_PROTOCOL SerialProtocol_MAVLink2 +#endif + #ifdef HAL_SERIAL2_PROTOCOL #define SERIAL2_PROTOCOL HAL_SERIAL2_PROTOCOL #else @@ -139,7 +145,7 @@ const AP_Param::GroupInfo AP_SerialManager::var_info[] = { // @Values: -1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial, 8:SToRM32 Gimbal Serial, 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 // @User: Standard // @RebootRequired: True - AP_GROUPINFO("1_PROTOCOL", 1, AP_SerialManager, state[1].protocol, SerialProtocol_MAVLink2), + AP_GROUPINFO("1_PROTOCOL", 1, AP_SerialManager, state[1].protocol, SERIAL1_PROTOCOL), // @Param: 1_BAUD // @DisplayName: Telem1 Baud Rate diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 5ea9eba7bbb77..1f6d2a4ee1e3a 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -268,10 +268,9 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = { // @Param: FLARE_HGT // @DisplayName: Flare holdoff height - // @Description: When height above ground is below this, the sink rate will be held at TECS_LAND_SINK. Use this to perform a hold-of manoeuvre when combined with small values for TECS_LAND_SINK. - // @Range: -10 15 - // @Units: deg - // @Increment: 1 + // @Description: When height above ground is below this, the sink rate will be held at TECS_LAND_SINK. Use this to perform a hold-off manoeuvre when combined with small values for TECS_LAND_SINK. + // @Range: 0 15 + // @Units: m // @User: Advanced AP_GROUPINFO("FLARE_HGT", 32, AP_TECS, _flare_holdoff_hgt, 1.0f), diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp deleted file mode 100644 index 41f1ef3ac6582..0000000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ /dev/null @@ -1,1513 +0,0 @@ -/* - * 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 . - * - * Author: Eugene Shamaev, Siddharth Bharat Purohit - */ - -#include -#include - -#if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include "AP_UAVCAN.h" -#include - -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if AP_DRONECAN_SEND_GPS -#include -#include -#include -#include -#endif -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "AP_UAVCAN_DNA_Server.h" -#include -#include -#include -#include "AP_UAVCAN_pool.h" -#include - -#define LED_DELAY_US 50000 - -extern const AP_HAL::HAL& hal; - -// setup default pool size -#ifndef UAVCAN_NODE_POOL_SIZE -#if HAL_CANFD_SUPPORTED -#define UAVCAN_NODE_POOL_SIZE 16384 -#else -#define UAVCAN_NODE_POOL_SIZE 8192 -#endif -#endif - -#if HAL_CANFD_SUPPORTED -#define UAVCAN_STACK_SIZE 8192 -#else -#define UAVCAN_STACK_SIZE 4096 -#endif - -#ifndef AP_DRONECAN_VOLZ_FEEDBACK_ENABLED -#define AP_DRONECAN_VOLZ_FEEDBACK_ENABLED 0 -#endif - -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED -#include -#endif - -#define debug_uavcan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "UAVCAN", fmt, ##args); } while (0) - -// Translation of all messages from UAVCAN structures into AP structures is done -// in AP_UAVCAN and not in corresponding drivers. -// The overhead of including definitions of DSDL is very high and it is best to -// concentrate in one place. - -// table of user settable CAN bus parameters -const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { - // @Param: NODE - // @DisplayName: UAVCAN node that is used for this network - // @Description: UAVCAN node should be set implicitly - // @Range: 1 250 - // @User: Advanced - AP_GROUPINFO("NODE", 1, AP_UAVCAN, _uavcan_node, 10), - - // @Param: SRV_BM - // @DisplayName: Output channels to be transmitted as servo over UAVCAN - // @Description: Bitmask with one set for channel to be transmitted as a servo command over UAVCAN - // @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15, 15: Servo 16, 16: Servo 17, 17: Servo 18, 18: Servo 19, 19: Servo 20, 20: Servo 21, 21: Servo 22, 22: Servo 23, 23: Servo 24, 24: Servo 25, 25: Servo 26, 26: Servo 27, 27: Servo 28, 28: Servo 29, 29: Servo 30, 30: Servo 31, 31: Servo 32 - - // @User: Advanced - AP_GROUPINFO("SRV_BM", 2, AP_UAVCAN, _servo_bm, 0), - - // @Param: ESC_BM - // @DisplayName: Output channels to be transmitted as ESC over UAVCAN - // @Description: Bitmask with one set for channel to be transmitted as a ESC command over UAVCAN - // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32 - // @User: Advanced - AP_GROUPINFO("ESC_BM", 3, AP_UAVCAN, _esc_bm, 0), - - // @Param: SRV_RT - // @DisplayName: Servo output rate - // @Description: Maximum transmit rate for servo outputs - // @Range: 1 200 - // @Units: Hz - // @User: Advanced - AP_GROUPINFO("SRV_RT", 4, AP_UAVCAN, _servo_rate_hz, 50), - - // @Param: OPTION - // @DisplayName: UAVCAN options - // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy,4:SendServoAsPWM,5:SendGNSS - // @User: Advanced - AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), - - // @Param: NTF_RT - // @DisplayName: Notify State rate - // @Description: Maximum transmit rate for Notify State Message - // @Range: 1 200 - // @Units: Hz - // @User: Advanced - AP_GROUPINFO("NTF_RT", 6, AP_UAVCAN, _notify_state_hz, 20), - - // @Param: ESC_OF - // @DisplayName: ESC Output channels offset - // @Description: Offset for ESC numbering in DroneCAN ESC RawCommand messages. This allows for more efficient packing of ESC command messages. If your ESCs are on servo functions 5 to 8 and you set this parameter to 4 then the ESC RawCommand will be sent with the first 4 slots filled. This can be used for more efficint usage of CAN bandwidth - // @Range: 0 18 - // @User: Advanced - AP_GROUPINFO("ESC_OF", 7, AP_UAVCAN, _esc_offset, 0), - - // @Param: POOL - // @DisplayName: CAN pool size - // @Description: Amount of memory in bytes to allocate for the DroneCAN memory pool. More memory is needed for higher CAN bus loads - // @Range: 1024 16384 - // @User: Advanced - AP_GROUPINFO("POOL", 8, AP_UAVCAN, _pool_size, UAVCAN_NODE_POOL_SIZE), - - AP_GROUPEND -}; - -// this is the timeout in milliseconds for periodic message types. We -// set this to 1 to minimise resend of stale msgs -#define CAN_PERIODIC_TX_TIMEOUT_MS 2 - -// publisher interfaces -static uavcan::Publisher* act_out_array[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* esc_raw[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* rgb_led[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* buzzer[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* safety_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* arming_status[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -#if AP_DRONECAN_SEND_GPS -static uavcan::Publisher* gnss_fix2[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* gnss_auxiliary[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* gnss_heading[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* gnss_status[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -#endif -static uavcan::Publisher* rtcm_stream[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::Publisher* notify_state[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -// Clients -UC_CLIENT_CALL_REGISTRY_BINDER(ParamGetSetCb, uavcan::protocol::param::GetSet); -static uavcan::ServiceClient* param_get_set_client[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::protocol::param::GetSet::Request param_getset_req[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -UC_CLIENT_CALL_REGISTRY_BINDER(ParamExecuteOpcodeCb, uavcan::protocol::param::ExecuteOpcode); -static uavcan::ServiceClient* param_execute_opcode_client[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -static uavcan::protocol::param::ExecuteOpcode::Request param_save_req[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - - -// subscribers - -// handler SafteyButton -UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button); -static uavcan::Subscriber *safety_button_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -// handler TrafficReport -UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport); -static uavcan::Subscriber *traffic_report_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -// handler actuator status -UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status); -static uavcan::Subscriber *actuator_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED -UC_REGISTRY_BINDER(ActuatorStatusVolzCb, com::volz::servo::ActuatorStatus); -static uavcan::Subscriber *actuator_status_Volz_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -#endif - -// handler ESC status -UC_REGISTRY_BINDER(ESCStatusCb, uavcan::equipment::esc::Status); -static uavcan::Subscriber *esc_status_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -// handler DEBUG -UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage); -static uavcan::Subscriber *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; - -AP_UAVCAN::AP_UAVCAN() -{ - AP_Param::setup_object_defaults(this, var_info); - - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - _SRV_conf[i].esc_pending = false; - _SRV_conf[i].servo_pending = false; - } - - debug_uavcan(AP_CANManager::LOG_INFO, "AP_UAVCAN constructed\n\r"); -} - -AP_UAVCAN::~AP_UAVCAN() -{ -} - -AP_UAVCAN *AP_UAVCAN::get_uavcan(uint8_t driver_index) -{ - if (driver_index >= AP::can().get_num_drivers() || - AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_UAVCAN) { - return nullptr; - } - return static_cast(AP::can().get_driver(driver_index)); -} - -bool AP_UAVCAN::add_interface(AP_HAL::CANIface* can_iface) { - - if (_iface_mgr == nullptr) { - _iface_mgr = new uavcan::CanIfaceMgr(); - } - - if (_iface_mgr == nullptr) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't create UAVCAN interface manager\n\r"); - return false; - } - - if (!_iface_mgr->add_interface(can_iface)) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't add UAVCAN interface\n\r"); - return false; - } - return true; -} - -#pragma GCC diagnostic push -#pragma GCC diagnostic error "-Wframe-larger-than=2200" -void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) -{ - _driver_index = driver_index; - - if (_initialized) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: init called more than once\n\r"); - return; - } - - if (_iface_mgr == nullptr) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: can't get UAVCAN interface driver\n\r"); - return; - } - - _allocator = new AP_PoolAllocator(_pool_size); - - if (_allocator == nullptr || !_allocator->init()) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node pool\n"); - return; - } - - _node = new uavcan::Node<0>(*_iface_mgr, uavcan::SystemClock::instance(), *_allocator); - - if (_node == nullptr) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate node\n\r"); - return; - } - - if (_node->isStarted()) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node was already started?\n\r"); - return; - } - { - uavcan::NodeID self_node_id(_uavcan_node); - _node->setNodeID(self_node_id); - - char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); - - uavcan::NodeStatusProvider::NodeName name(ndname); - _node->setName(name); - } - { - uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion - sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; - sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; - _node->setSoftwareVersion(sw_version); - - uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion - - hw_version.major = AP_UAVCAN_HW_VERS_MAJOR; - hw_version.minor = AP_UAVCAN_HW_VERS_MINOR; - - const uint8_t uid_buf_len = hw_version.unique_id.capacity(); - uint8_t uid_len = uid_buf_len; - uint8_t unique_id[uid_buf_len]; - - - if (hal.util->get_system_id_unformatted(unique_id, uid_len)) { - //This is because we are maintaining a common Server Record for all UAVCAN Instances. - //In case the node IDs are different, and unique id same, it will create - //conflict in the Server Record. - unique_id[uid_len - 1] += _uavcan_node; - uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin()); - } - _node->setHardwareVersion(hw_version); - } - -#if UAVCAN_SUPPORT_CANFD - if (option_is_set(Options::CANFD_ENABLED)) { - _node->enableCanFd(); - } -#endif - - int start_res = _node->start(); - if (start_res < 0) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: node start problem, error %d\n\r", start_res); - return; - } - - _dna_server = new AP_UAVCAN_DNA_Server(this, StorageAccess(StorageManager::StorageCANDNA)); - if (_dna_server == nullptr) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't allocate DNA server\n\r"); - return; - } - - //Start Servers - if (!_dna_server->init()) { - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: Failed to start DNA Server\n\r"); - return; - } - - // Roundup all subscribers from supported drivers - AP_UAVCAN_DNA_Server::subscribe_msgs(this); - AP_GPS_UAVCAN::subscribe_msgs(this); -#if AP_COMPASS_UAVCAN_ENABLED - AP_Compass_UAVCAN::subscribe_msgs(this); -#endif -#if AP_BARO_UAVCAN_ENABLED - AP_Baro_UAVCAN::subscribe_msgs(this); -#endif - AP_BattMonitor_UAVCAN::subscribe_msgs(this); -#if AP_AIRSPEED_UAVCAN_ENABLED - AP_Airspeed_UAVCAN::subscribe_msgs(this); -#endif -#if AP_OPTICALFLOW_HEREFLOW_ENABLED - AP_OpticalFlow_HereFlow::subscribe_msgs(this); -#endif -#if AP_RANGEFINDER_UAVCAN_ENABLED - AP_RangeFinder_UAVCAN::subscribe_msgs(this); -#endif -#if AP_EFI_DRONECAN_ENABLED - AP_EFI_DroneCAN::subscribe_msgs(this); -#endif - -#if AP_PROXIMITY_DRONECAN_ENABLED - AP_Proximity_DroneCAN::subscribe_msgs(this); -#endif - - act_out_array[driver_index] = new uavcan::Publisher(*_node); - act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); - act_out_array[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - - esc_raw[driver_index] = new uavcan::Publisher(*_node); - esc_raw[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); - esc_raw[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - - rgb_led[driver_index] = new uavcan::Publisher(*_node); - rgb_led[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - rgb_led[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - - buzzer[driver_index] = new uavcan::Publisher(*_node); - buzzer[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - buzzer[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - - safety_state[driver_index] = new uavcan::Publisher(*_node); - safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - - arming_status[driver_index] = new uavcan::Publisher(*_node); - arming_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - arming_status[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - -#if AP_DRONECAN_SEND_GPS - gnss_fix2[driver_index] = new uavcan::Publisher(*_node); - gnss_fix2[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - gnss_fix2[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - - gnss_auxiliary[driver_index] = new uavcan::Publisher(*_node); - gnss_auxiliary[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - gnss_auxiliary[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - - gnss_heading[driver_index] = new uavcan::Publisher(*_node); - gnss_heading[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - gnss_heading[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); - - gnss_status[driver_index] = new uavcan::Publisher(*_node); - gnss_status[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - gnss_status[driver_index]->setPriority(uavcan::TransferPriority::OneLowerThanHighest); -#endif - - rtcm_stream[driver_index] = new uavcan::Publisher(*_node); - rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - - notify_state[driver_index] = new uavcan::Publisher(*_node); - notify_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20)); - notify_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest); - - param_get_set_client[driver_index] = new uavcan::ServiceClient(*_node, ParamGetSetCb(this, &AP_UAVCAN::handle_param_get_set_response)); - - param_execute_opcode_client[driver_index] = new uavcan::ServiceClient(*_node, ParamExecuteOpcodeCb(this, &AP_UAVCAN::handle_param_save_response)); - - safety_button_listener[driver_index] = new uavcan::Subscriber(*_node); - if (safety_button_listener[driver_index]) { - safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button)); - } - - traffic_report_listener[driver_index] = new uavcan::Subscriber(*_node); - if (traffic_report_listener[driver_index]) { - traffic_report_listener[driver_index]->start(TrafficReportCb(this, &handle_traffic_report)); - } - - actuator_status_listener[driver_index] = new uavcan::Subscriber(*_node); - if (actuator_status_listener[driver_index]) { - actuator_status_listener[driver_index]->start(ActuatorStatusCb(this, &handle_actuator_status)); - } - -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED - actuator_status_Volz_listener[driver_index] = new uavcan::Subscriber(*_node); - if (actuator_status_Volz_listener[driver_index]) { - actuator_status_Volz_listener[driver_index]->start(ActuatorStatusVolzCb(this, &handle_actuator_status_Volz)); - } -#endif - - esc_status_listener[driver_index] = new uavcan::Subscriber(*_node); - if (esc_status_listener[driver_index]) { - esc_status_listener[driver_index]->start(ESCStatusCb(this, &handle_ESC_status)); - } - - debug_listener[driver_index] = new uavcan::Subscriber(*_node); - if (debug_listener[driver_index]) { - debug_listener[driver_index]->start(DebugCb(this, &handle_debug)); - } - - _led_conf.devices_count = 0; - - /* - * Informing other nodes that we're ready to work. - * Default mode is INITIALIZING. - */ - _node->setModeOperational(); - - // Spin node for device discovery - _node->spin(uavcan::MonotonicDuration::fromMSec(5000)); - - snprintf(_thread_name, sizeof(_thread_name), "uavcan_%u", driver_index); - - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_UAVCAN::loop, void), _thread_name, UAVCAN_STACK_SIZE, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { - _node->setModeOfflineAndPublish(); - debug_uavcan(AP_CANManager::LOG_ERROR, "UAVCAN: couldn't create thread\n\r"); - return; - } - - _initialized = true; - debug_uavcan(AP_CANManager::LOG_INFO, "UAVCAN: init done\n\r"); -} -#pragma GCC diagnostic pop - -void AP_UAVCAN::loop(void) -{ - while (true) { - if (!_initialized) { - hal.scheduler->delay_microseconds(1000); - continue; - } - - const int error = _node->spin(uavcan::MonotonicDuration::fromMSec(1)); - - if (error < 0) { - hal.scheduler->delay_microseconds(100); - continue; - } - - if (_SRV_armed) { - bool sent_servos = false; - - if (_servo_bm > 0) { - // if we have any Servos in bitmask - uint32_t now = AP_HAL::native_micros(); - const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get()); - if (now - _SRV_last_send_us >= servo_period_us) { - _SRV_last_send_us = now; - SRV_send_actuator(); - sent_servos = true; - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - _SRV_conf[i].servo_pending = false; - } - } - } - - // if we have any ESC's in bitmask - if (_esc_bm > 0 && !sent_servos) { - SRV_send_esc(); - } - - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - _SRV_conf[i].esc_pending = false; - } - } - - led_out_send(); - buzzer_send(); - rtcm_stream_send(); - safety_state_send(); - notify_state_send(); - send_parameter_request(); - send_parameter_save_request(); - _dna_server->verify_nodes(); -#if AP_OPENDRONEID_ENABLED - AP::opendroneid().dronecan_send(this); -#endif - -#if AP_DRONECAN_SEND_GPS - if (option_is_set(AP_UAVCAN::Options::SEND_GNSS) && !AP_GPS_UAVCAN::instance_exists(this)) { - // send if enabled and this interface/driver is not used by the AP_GPS driver - gnss_send_fix(); - gnss_send_yaw(); - } -#endif - - logging(); - } -} - - -///// SRV output ///// - -void AP_UAVCAN::SRV_send_actuator(void) -{ - uint8_t starting_servo = 0; - bool repeat_send; - - WITH_SEMAPHORE(SRV_sem); - - do { - repeat_send = false; - uavcan::equipment::actuator::ArrayCommand msg; - - uint8_t i; - // UAVCAN can hold maximum of 15 commands in one frame - for (i = 0; starting_servo < UAVCAN_SRV_NUMBER && i < 15; starting_servo++) { - uavcan::equipment::actuator::Command cmd; - - /* - * Servo output uses a range of 1000-2000 PWM for scaling. - * This converts output PWM from [1000:2000] range to [-1:1] range that - * is passed to servo as unitless type via UAVCAN. - * This approach allows for MIN/TRIM/MAX values to be used fully on - * autopilot side and for servo it should have the setup to provide maximum - * physically possible throws at [-1:1] limits. - */ - - if (_SRV_conf[starting_servo].servo_pending && ((((uint32_t) 1) << starting_servo) & _servo_bm)) { - cmd.actuator_id = starting_servo + 1; - - if (option_is_set(Options::USE_ACTUATOR_PWM)) { - cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_PWM; - cmd.command_value = _SRV_conf[starting_servo].pulse; - } else { - cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS; - cmd.command_value = constrain_float(((float) _SRV_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0); - } - - msg.commands.push_back(cmd); - - i++; - } - } - - if (i > 0) { - if (act_out_array[_driver_index]->broadcast(msg) > 0) { - _srv_send_count++; - } else { - _fail_send_count++; - } - - if (i == 15) { - repeat_send = true; - } - } - } while (repeat_send); -} - -void AP_UAVCAN::SRV_send_esc(void) -{ - static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); - uavcan::equipment::esc::RawCommand esc_msg; - - uint8_t active_esc_num = 0, max_esc_num = 0; - uint8_t k = 0; - - WITH_SEMAPHORE(SRV_sem); - - // esc offset allows for efficient packing of higher ESC numbers in RawCommand - const uint8_t esc_offset = constrain_int16(_esc_offset.get(), 0, UAVCAN_SRV_NUMBER); - - // find out how many esc we have enabled and if they are active at all - for (uint8_t i = esc_offset; i < UAVCAN_SRV_NUMBER; i++) { - if ((((uint32_t) 1) << i) & _esc_bm) { - max_esc_num = i + 1; - if (_SRV_conf[i].esc_pending) { - active_esc_num++; - } - } - } - - // if at least one is active (update) we need to send to all - if (active_esc_num > 0) { - k = 0; - - for (uint8_t i = esc_offset; i < max_esc_num && k < 20; i++) { - if ((((uint32_t) 1) << i) & _esc_bm) { - // TODO: ESC negative scaling for reverse thrust and reverse rotation - float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_SRV_conf[i].pulse) + 1.0) / 2.0; - - scaled = constrain_float(scaled, 0, cmd_max); - - esc_msg.cmd.push_back(static_cast(scaled)); - } else { - esc_msg.cmd.push_back(static_cast(0)); - } - - k++; - } - - if (esc_raw[_driver_index]->broadcast(esc_msg) > 0) { - _esc_send_count++; - } else { - _fail_send_count++; - } - } -} - -void AP_UAVCAN::SRV_push_servos() -{ - WITH_SEMAPHORE(SRV_sem); - - for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++) { - // Check if this channels has any function assigned - if (SRV_Channels::channel_function(i) >= SRV_Channel::k_none) { - _SRV_conf[i].pulse = SRV_Channels::srv_channel(i)->get_output_pwm(); - _SRV_conf[i].esc_pending = true; - _SRV_conf[i].servo_pending = true; - } - } - - _SRV_armed = hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED; -} - - -///// LED ///// - -void AP_UAVCAN::led_out_send() -{ - uint64_t now = AP_HAL::native_micros64(); - - if ((now - _led_conf.last_update) < LED_DELAY_US) { - return; - } - - uavcan::equipment::indication::LightsCommand msg; - { - WITH_SEMAPHORE(_led_out_sem); - - if (_led_conf.devices_count == 0) { - return; - } - - uavcan::equipment::indication::SingleLightCommand cmd; - - for (uint8_t i = 0; i < _led_conf.devices_count; i++) { - cmd.light_id =_led_conf.devices[i].led_index; - cmd.color.red = _led_conf.devices[i].red >> 3; - cmd.color.green = _led_conf.devices[i].green >> 2; - cmd.color.blue = _led_conf.devices[i].blue >> 3; - - msg.commands.push_back(cmd); - } - } - - rgb_led[_driver_index]->broadcast(msg); - _led_conf.last_update = now; -} - -bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue) -{ - if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) { - return false; - } - - WITH_SEMAPHORE(_led_out_sem); - - // check if a device instance exists. if so, break so the instance index is remembered - uint8_t instance = 0; - for (; instance < _led_conf.devices_count; instance++) { - if (_led_conf.devices[instance].led_index == led_index) { - break; - } - } - - // load into the correct instance. - // if an existing instance was found in above for loop search, - // then instance value is < _led_conf.devices_count. - // otherwise a new one was just found so we increment the count. - // Either way, the correct instance is the current value of instance - _led_conf.devices[instance].led_index = led_index; - _led_conf.devices[instance].red = red; - _led_conf.devices[instance].green = green; - _led_conf.devices[instance].blue = blue; - - if (instance == _led_conf.devices_count) { - _led_conf.devices_count++; - } - - return true; -} - -// buzzer send -void AP_UAVCAN::buzzer_send() -{ - uavcan::equipment::indication::BeepCommand msg; - WITH_SEMAPHORE(_buzzer.sem); - uint8_t mask = (1U << _driver_index); - if ((_buzzer.pending_mask & mask) == 0) { - return; - } - _buzzer.pending_mask &= ~mask; - msg.frequency = _buzzer.frequency; - msg.duration = _buzzer.duration; - buzzer[_driver_index]->broadcast(msg); -} - -// buzzer support -void AP_UAVCAN::set_buzzer_tone(float frequency, float duration_s) -{ - WITH_SEMAPHORE(_buzzer.sem); - _buzzer.frequency = frequency; - _buzzer.duration = duration_s; - _buzzer.pending_mask = 0xFF; -} - -// notify state send -void AP_UAVCAN::notify_state_send() -{ - uint32_t now = AP_HAL::native_millis(); - - if (_notify_state_hz == 0 || (now - _last_notify_state_ms) < uint32_t(1000 / _notify_state_hz)) { - return; - } - - ardupilot::indication::NotifyState msg; - msg.vehicle_state = 0; - if (AP_Notify::flags.initialising) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_INITIALISING; - } - if (AP_Notify::flags.armed) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_ARMED; - } - if (AP_Notify::flags.flying) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FLYING; - } - if (AP_Notify::flags.compass_cal_running) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_MAGCAL_RUN; - } - if (AP_Notify::flags.ekf_bad) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_EKF_BAD; - } - if (AP_Notify::flags.esc_calibration) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_ESC_CALIBRATION; - } - if (AP_Notify::flags.failsafe_battery) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_BATT; - } - if (AP_Notify::flags.failsafe_gcs) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_GCS; - } - if (AP_Notify::flags.failsafe_radio) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FAILSAFE_RADIO; - } - if (AP_Notify::flags.firmware_update) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_FW_UPDATE; - } - if (AP_Notify::flags.gps_fusion) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_GPS_FUSION; - } - if (AP_Notify::flags.gps_glitching) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_GPS_GLITCH; - } - if (AP_Notify::flags.have_pos_abs) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_POS_ABS_AVAIL; - } - if (AP_Notify::flags.leak_detected) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_LEAK_DET; - } - if (AP_Notify::flags.parachute_release) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_CHUTE_RELEASED; - } - if (AP_Notify::flags.powering_off) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_POWERING_OFF; - } - if (AP_Notify::flags.pre_arm_check) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_PREARM; - } - if (AP_Notify::flags.pre_arm_gps_check) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_PREARM_GPS; - } - if (AP_Notify::flags.save_trim) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_SAVE_TRIM; - } - if (AP_Notify::flags.vehicle_lost) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_LOST; - } - if (AP_Notify::flags.video_recording) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_VIDEO_RECORDING; - } - if (AP_Notify::flags.waiting_for_throw) { - msg.vehicle_state |= 1 << ardupilot::indication::NotifyState::VEHICLE_STATE_THROW_READY; - } - - msg.aux_data_type = ardupilot::indication::NotifyState::VEHICLE_YAW_EARTH_CENTIDEGREES; - uint16_t yaw_cd = (uint16_t)(360.0f - degrees(AP::ahrs().get_yaw()))*100.0f; - const uint8_t *data = (uint8_t *)&yaw_cd; - for (uint8_t i=0; i<2; i++) { - msg.aux_data.push_back(data[i]); - } - notify_state[_driver_index]->broadcast(msg); - _last_notify_state_ms = AP_HAL::native_millis(); -} - -#if AP_DRONECAN_SEND_GPS -void AP_UAVCAN::gnss_send_fix() -{ - const AP_GPS &gps = AP::gps(); - - const uint32_t gps_lib_time_ms = gps.last_message_time_ms(); - if (_gnss.last_gps_lib_fix_ms == gps_lib_time_ms) { - return; - } - _gnss.last_gps_lib_fix_ms = gps_lib_time_ms; - - /* - send Fix2 packet - */ - - uavcan::equipment::gnss::Fix2 pkt {}; - const Location &loc = gps.location(); - const Vector3f &vel = gps.velocity(); - - pkt.timestamp.usec = AP_HAL::native_micros64(); - pkt.gnss_timestamp.usec = gps.time_epoch_usec(); - if (pkt.gnss_timestamp.usec == 0) { - pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_NONE; - } else { - pkt.gnss_time_standard = uavcan::equipment::gnss::Fix2::GNSS_TIME_STANDARD_UTC; - } - pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; - pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; - pkt.height_ellipsoid_mm = loc.alt * 10; - pkt.height_msl_mm = loc.alt * 10; - for (uint8_t i=0; i<3; i++) { - pkt.ned_velocity[i] = vel[i]; - } - pkt.sats_used = gps.num_sats(); - switch (gps.status()) { - case AP_GPS::GPS_Status::NO_GPS: - case AP_GPS::GPS_Status::NO_FIX: - pkt.status = uavcan::equipment::gnss::Fix2::STATUS_NO_FIX; - pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; - pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_2D: - pkt.status = uavcan::equipment::gnss::Fix2::STATUS_2D_FIX; - pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; - pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D: - pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; - pkt.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE; - pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS: - pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; - pkt.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS; - pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_SBAS; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT: - pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; - pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; - pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT; - break; - case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED: - pkt.status = uavcan::equipment::gnss::Fix2::STATUS_3D_FIX; - pkt.mode = uavcan::equipment::gnss::Fix2::MODE_RTK; - pkt.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED; - break; - } - - pkt.covariance.resize(6); - float hacc; - if (gps.horizontal_accuracy(hacc)) { - pkt.covariance[0] = pkt.covariance[1] = sq(hacc); - } - float vacc; - if (gps.vertical_accuracy(vacc)) { - pkt.covariance[2] = sq(vacc); - } - float sacc; - if (gps.speed_accuracy(sacc)) { - const float vc3 = sq(sacc); - pkt.covariance[3] = pkt.covariance[4] = pkt.covariance[5] = vc3; - } - - gnss_fix2[_driver_index]->broadcast(pkt); - - - - const uint32_t now_ms = AP_HAL::native_millis(); - if (now_ms - _gnss.last_send_status_ms >= 1000) { - _gnss.last_send_status_ms = now_ms; - - /* - send aux packet - */ - uavcan::equipment::gnss::Auxiliary pkt_auxiliary {}; - pkt_auxiliary.hdop = gps.get_hdop() * 0.01; - pkt_auxiliary.vdop = gps.get_vdop() * 0.01; - - gnss_auxiliary[_driver_index]->broadcast(pkt_auxiliary); - - - /* - send Status packet - */ - ardupilot::gnss::Status pkt_status {}; - pkt_status.healthy = gps.is_healthy(); - if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) { - pkt_status.status |= ardupilot::gnss::Status::STATUS_LOGGING; - } - uint8_t idx; // unused - if (pkt_status.healthy && !gps.first_unconfigured_gps(idx)) { - pkt_status.status |= ardupilot::gnss::Status::STATUS_ARMABLE; - } - - uint32_t error_codes; - if (gps.get_error_codes(error_codes)) { - pkt_status.error_codes = error_codes; - } - - gnss_status[_driver_index]->broadcast(pkt_status); - } -} - -void AP_UAVCAN::gnss_send_yaw() -{ - const AP_GPS &gps = AP::gps(); - - if (!gps.have_gps_yaw()) { - return; - } - - float yaw_deg, yaw_acc_deg; - uint32_t yaw_time_ms; - if (!gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != _gnss.last_lib_yaw_time_ms) { - return; - } - - _gnss.last_lib_yaw_time_ms = yaw_time_ms; - - ardupilot::gnss::Heading pkt_heading {}; - pkt_heading.heading_valid = true; - pkt_heading.heading_accuracy_valid = is_positive(yaw_acc_deg); - pkt_heading.heading_rad = radians(yaw_deg); - pkt_heading.heading_accuracy_rad = radians(yaw_acc_deg); - - gnss_heading[_driver_index]->broadcast(pkt_heading); -} -#endif // AP_DRONECAN_SEND_GPS - - -void AP_UAVCAN::rtcm_stream_send() -{ - WITH_SEMAPHORE(_rtcm_stream.sem); - if (_rtcm_stream.buf == nullptr || - _rtcm_stream.buf->available() == 0) { - // nothing to send - return; - } - uint32_t now = AP_HAL::native_millis(); - if (now - _rtcm_stream.last_send_ms < 20) { - // don't send more than 50 per second - return; - } - _rtcm_stream.last_send_ms = now; - uavcan::equipment::gnss::RTCMStream msg; - uint32_t len = _rtcm_stream.buf->available(); - if (len > 128) { - len = 128; - } - msg.protocol_id = uavcan::equipment::gnss::RTCMStream::PROTOCOL_ID_RTCM3; - for (uint8_t i=0; iread_byte(&b)) { - return; - } - msg.data.push_back(b); - } - rtcm_stream[_driver_index]->broadcast(msg); -} - -// SafetyState send -void AP_UAVCAN::safety_state_send() -{ - uint32_t now = AP_HAL::native_millis(); - if (now - _last_safety_state_ms < 500) { - // update at 2Hz - return; - } - _last_safety_state_ms = now; - - { // handle SafetyState - ardupilot::indication::SafetyState safety_msg; - switch (hal.util->safety_switch_state()) { - case AP_HAL::Util::SAFETY_ARMED: - safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_OFF; - break; - case AP_HAL::Util::SAFETY_DISARMED: - safety_msg.status = ardupilot::indication::SafetyState::STATUS_SAFETY_ON; - break; - default: - // nothing to send - break; - } - safety_state[_driver_index]->broadcast(safety_msg); - } - - { // handle ArmingStatus - uavcan::equipment::safety::ArmingStatus arming_msg; - arming_msg.status = hal.util->get_soft_armed() ? uavcan::equipment::safety::ArmingStatus::STATUS_FULLY_ARMED : - uavcan::equipment::safety::ArmingStatus::STATUS_DISARMED; - arming_status[_driver_index]->broadcast(arming_msg); - } -} - -/* - send RTCMStream packet on all active UAVCAN drivers -*/ -void AP_UAVCAN::send_RTCMStream(const uint8_t *data, uint32_t len) -{ - WITH_SEMAPHORE(_rtcm_stream.sem); - if (_rtcm_stream.buf == nullptr) { - // give enough space for a full round from a NTRIP server with all - // constellations - _rtcm_stream.buf = new ByteBuffer(2400); - } - if (_rtcm_stream.buf == nullptr) { - return; - } - _rtcm_stream.buf->write(data, len); -} - -/* - handle Button message - */ -void AP_UAVCAN::handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb) -{ - switch (cb.msg->button) { - case ardupilot::indication::Button::BUTTON_SAFETY: { - AP_BoardConfig *brdconfig = AP_BoardConfig::get_singleton(); - if (brdconfig && brdconfig->safety_button_handle_pressed(cb.msg->press_time)) { - AP_HAL::Util::safety_state state = hal.util->safety_switch_state(); - if (state == AP_HAL::Util::SAFETY_ARMED) { - hal.rcout->force_safety_on(); - } else { - hal.rcout->force_safety_off(); - } - } - break; - } - } -} - -/* - handle traffic report - */ -void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb) -{ -#if HAL_ADSB_ENABLED - AP_ADSB *adsb = AP::ADSB(); - if (!adsb || !adsb->enabled()) { - // ADSB not enabled - return; - } - - const ardupilot::equipment::trafficmonitor::TrafficReport &msg = cb.msg[0]; - AP_ADSB::adsb_vehicle_t vehicle; - mavlink_adsb_vehicle_t &pkt = vehicle.info; - - pkt.ICAO_address = msg.icao_address; - pkt.tslc = msg.tslc; - pkt.lat = msg.latitude_deg_1e7; - pkt.lon = msg.longitude_deg_1e7; - pkt.altitude = msg.alt_m * 1000; - pkt.heading = degrees(msg.heading) * 100; - pkt.hor_velocity = norm(msg.velocity[0], msg.velocity[1]) * 100; - pkt.ver_velocity = -msg.velocity[2] * 100; - pkt.squawk = msg.squawk; - for (uint8_t i=0; i<9; i++) { - pkt.callsign[i] = msg.callsign[i]; - } - pkt.emitter_type = msg.traffic_type; - - if (msg.alt_type == ardupilot::equipment::trafficmonitor::TrafficReport::ALT_TYPE_PRESSURE_AMSL) { - pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE; - pkt.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; - } else if (msg.alt_type == ardupilot::equipment::trafficmonitor::TrafficReport::ALT_TYPE_WGS84) { - pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE; - pkt.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; - } - - if (msg.lat_lon_valid) { - pkt.flags |= ADSB_FLAGS_VALID_COORDS; - } - if (msg.heading_valid) { - pkt.flags |= ADSB_FLAGS_VALID_HEADING; - } - if (msg.velocity_valid) { - pkt.flags |= ADSB_FLAGS_VALID_VELOCITY; - } - if (msg.callsign_valid) { - pkt.flags |= ADSB_FLAGS_VALID_CALLSIGN; - } - if (msg.ident_valid) { - pkt.flags |= ADSB_FLAGS_VALID_SQUAWK; - } - if (msg.simulated_report) { - pkt.flags |= ADSB_FLAGS_SIMULATED; - } - if (msg.vertical_velocity_valid) { - pkt.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID; - } - if (msg.baro_valid) { - pkt.flags |= ADSB_FLAGS_BARO_VALID; - } - - vehicle.last_update_ms = AP_HAL::native_millis() - (vehicle.info.tslc * 1000); - adsb->handle_adsb_vehicle(vehicle); -#endif -} - -/* - handle actuator status message - */ -void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb) -{ - // log as CSRV message - AP::logger().Write_ServoStatus(AP_HAL::native_micros64(), - cb.msg->actuator_id, - cb.msg->position, - cb.msg->force, - cb.msg->speed, - cb.msg->power_rating_pct); -} - -#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED -void AP_UAVCAN::handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb) -{ - AP::logger().WriteStreaming( - "CVOL", - "TimeUS,Id,Pos,Cur,V,Pow,T", - "s#dAv%O", - "F-00000", - "QBfffBh", - AP_HAL::native_micros64(), - cb.msg->actuator_id, - ToDeg(cb.msg->actual_position), - cb.msg->current * 0.025f, - cb.msg->voltage * 0.2f, - cb.msg->motor_pwm * (100.0/255.0), - int16_t(cb.msg->motor_temperature) - 50); -} -#endif - -/* - handle ESC status message - */ -void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb) -{ -#if HAL_WITH_ESC_TELEM - const uint8_t esc_offset = constrain_int16(ap_uavcan->_esc_offset.get(), 0, UAVCAN_SRV_NUMBER); - const uint8_t esc_index = cb.msg->esc_index + esc_offset; - - if (!is_esc_data_index_valid(esc_index)) { - return; - } - - TelemetryData t { - .temperature_cdeg = int16_t((KELVIN_TO_C(cb.msg->temperature)) * 100), - .voltage = cb.msg->voltage, - .current = cb.msg->current, - }; - - ap_uavcan->update_rpm(esc_index, cb.msg->rpm, cb.msg->error_count); - ap_uavcan->update_telem_data(esc_index, t, - (isnan(cb.msg->current) ? 0 : AP_ESC_Telem_Backend::TelemetryType::CURRENT) - | (isnan(cb.msg->voltage) ? 0 : AP_ESC_Telem_Backend::TelemetryType::VOLTAGE) - | (isnan(cb.msg->temperature) ? 0 : AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE)); -#endif -} - -bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) { - if (index > UAVCAN_SRV_NUMBER) { - // printf("UAVCAN: invalid esc index: %d. max index allowed: %d\n\r", index, UAVCAN_SRV_NUMBER); - return false; - } - return true; -} - -/* - handle LogMessage debug - */ -void AP_UAVCAN::handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb) -{ -#if HAL_LOGGING_ENABLED - const auto &msg = *cb.msg; - if (AP::can().get_log_level() != AP_CANManager::LOG_NONE) { - // log to onboard log and mavlink - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CAN[%u] %s", node_id, msg.text.c_str()); - } else { - // only log to onboard log - AP::logger().Write_MessageF("CAN[%u] %s", node_id, msg.text.c_str()); - } -#endif -} - -void AP_UAVCAN::send_parameter_request() -{ - WITH_SEMAPHORE(_param_sem); - if (param_request_sent) { - return; - } - param_get_set_client[_driver_index]->call(param_request_node_id, param_getset_req[_driver_index]); - param_request_sent = true; -} - -bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb) -{ - WITH_SEMAPHORE(_param_sem); - if (param_int_cb != nullptr || - param_float_cb != nullptr) { - //busy - return false; - } - param_getset_req[_driver_index].index = 0; - param_getset_req[_driver_index].name = name; - param_getset_req[_driver_index].value.to() = value; - param_float_cb = cb; - param_request_sent = false; - param_request_node_id = node_id; - return true; -} - -bool AP_UAVCAN::set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb) -{ - WITH_SEMAPHORE(_param_sem); - if (param_int_cb != nullptr || - param_float_cb != nullptr) { - //busy - return false; - } - param_getset_req[_driver_index].index = 0; - param_getset_req[_driver_index].name = name; - param_getset_req[_driver_index].value.to() = value; - param_int_cb = cb; - param_request_sent = false; - param_request_node_id = node_id; - return true; -} - -bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb) -{ - WITH_SEMAPHORE(_param_sem); - if (param_int_cb != nullptr || - param_float_cb != nullptr) { - //busy - return false; - } - param_getset_req[_driver_index].index = 0; - param_getset_req[_driver_index].name = name; - param_getset_req[_driver_index].value.to(); - param_float_cb = cb; - param_request_sent = false; - param_request_node_id = node_id; - return true; -} - -bool AP_UAVCAN::get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb) -{ - WITH_SEMAPHORE(_param_sem); - if (param_int_cb != nullptr || - param_float_cb != nullptr) { - //busy - return false; - } - param_getset_req[_driver_index].index = 0; - param_getset_req[_driver_index].name = name; - param_getset_req[_driver_index].value.to(); - param_int_cb = cb; - param_request_sent = false; - param_request_node_id = node_id; - return true; -} - -void AP_UAVCAN::handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb) -{ - WITH_SEMAPHORE(ap_uavcan->_param_sem); - if (!ap_uavcan->param_int_cb && - !ap_uavcan->param_float_cb) { - return; - } - uavcan::protocol::param::GetSet::Response rsp = cb.rsp->getResponse(); - if (rsp.value.is(uavcan::protocol::param::Value::Tag::integer_value) && ap_uavcan->param_int_cb) { - int32_t val = rsp.value.to(); - if ((*ap_uavcan->param_int_cb)(ap_uavcan, node_id, rsp.name.c_str(), val)) { - // we want the parameter to be set with val - param_getset_req[ap_uavcan->_driver_index].index = 0; - param_getset_req[ap_uavcan->_driver_index].name = rsp.name; - param_getset_req[ap_uavcan->_driver_index].value.to() = val; - ap_uavcan->param_int_cb = ap_uavcan->param_int_cb; - ap_uavcan->param_request_sent = false; - ap_uavcan->param_request_node_id = node_id; - return; - } - } else if (rsp.value.is(uavcan::protocol::param::Value::Tag::real_value) && ap_uavcan->param_float_cb) { - float val = rsp.value.to(); - if ((*ap_uavcan->param_float_cb)(ap_uavcan, node_id, rsp.name.c_str(), val)) { - // we want the parameter to be set with val - param_getset_req[ap_uavcan->_driver_index].index = 0; - param_getset_req[ap_uavcan->_driver_index].name = rsp.name; - param_getset_req[ap_uavcan->_driver_index].value.to() = val; - ap_uavcan->param_float_cb = ap_uavcan->param_float_cb; - ap_uavcan->param_request_sent = false; - ap_uavcan->param_request_node_id = node_id; - return; - } - } - ap_uavcan->param_int_cb = nullptr; - ap_uavcan->param_float_cb = nullptr; -} - - -void AP_UAVCAN::send_parameter_save_request() -{ - WITH_SEMAPHORE(_param_save_sem); - if (param_save_request_sent) { - return; - } - param_execute_opcode_client[_driver_index]->call(param_save_request_node_id, param_save_req[_driver_index]); - param_save_request_sent = true; -} - -bool AP_UAVCAN::save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb) -{ - WITH_SEMAPHORE(_param_save_sem); - if (save_param_cb != nullptr) { - //busy - return false; - } - - param_save_req[_driver_index].opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; - param_save_request_sent = false; - param_save_request_node_id = node_id; - save_param_cb = cb; - return true; -} - -// handle parameter save request response -void AP_UAVCAN::handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb) -{ - WITH_SEMAPHORE(ap_uavcan->_param_save_sem); - if (!ap_uavcan->save_param_cb) { - return; - } - uavcan::protocol::param::ExecuteOpcode::Response rsp = cb.rsp->getResponse(); - (*ap_uavcan->save_param_cb)(ap_uavcan, node_id, rsp.ok); - ap_uavcan->save_param_cb = nullptr; -} - -// Send Reboot command -// Note: Do not call this from outside UAVCAN thread context, -// THIS IS NOT A THREAD SAFE API! -void AP_UAVCAN::send_reboot_request(uint8_t node_id) -{ - if (_node == nullptr) { - return; - } - uavcan::protocol::RestartNode::Request request; - request.magic_number = uavcan::protocol::RestartNode::Request::MAGIC_NUMBER; - uavcan::ServiceClient client(*_node); - client.setCallback([](const uavcan::ServiceCallResult& call_result){}); - - client.call(node_id, request); -} - -// check if a option is set and if it is then reset it to 0. -// return true if it was set -bool AP_UAVCAN::check_and_reset_option(Options option) -{ - bool ret = option_is_set(option); - if (ret) { - _options.set_and_save(int16_t(_options.get() & ~uint16_t(option))); - } - return ret; -} - -// handle prearm check -bool AP_UAVCAN::prearm_check(char* fail_msg, uint8_t fail_msg_len) const -{ - // forward this to DNA_Server - return _dna_server->prearm_check(fail_msg, fail_msg_len); -} - -/* - periodic logging - */ -void AP_UAVCAN::logging(void) -{ -#if HAL_LOGGING_ENABLED - const uint32_t now_ms = AP_HAL::millis(); - if (now_ms - last_log_ms < 1000) { - return; - } - last_log_ms = now_ms; - if (HAL_NUM_CAN_IFACES <= _driver_index) { - // no interface? - return; - } - const auto *iface = hal.can[_driver_index]; - if (iface == nullptr) { - return; - } - const auto *stats = iface->get_statistics(); - if (stats == nullptr) { - // statistics not implemented on this interface - return; - } - const auto &s = *stats; - AP::logger().WriteStreaming("CANS", - "TimeUS,I,T,Trq,Trej,Tov,Tto,Tab,R,Rov,Rer,Bo,Etx,Stx,Ftx", - "s#-------------", - "F--------------", - "QBIIIIIIIIIIIII", - AP_HAL::micros64(), - _driver_index, - s.tx_success, - s.tx_requests, - s.tx_rejected, - s.tx_overflow, - s.tx_timedout, - s.tx_abort, - s.rx_received, - s.rx_overflow, - s.rx_errors, - s.num_busoff_err, - _esc_send_count, - _srv_send_count, - _fail_send_count); -#endif // HAL_LOGGING_ENABLED -} - -#endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h deleted file mode 100644 index cf11dcf773ffc..0000000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ /dev/null @@ -1,376 +0,0 @@ -/* - * 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 . - * - * Author: Eugene Shamaev, Siddharth Bharat Purohit - */ -#pragma once - -#include - -#if HAL_ENABLE_LIBUAVCAN_DRIVERS - -#include -#include "AP_UAVCAN_IfaceMgr.h" -#include "AP_UAVCAN_Clock.h" -#include -#include -#include -#include -#include -#include -#include - - -#ifndef UAVCAN_SRV_NUMBER -#define UAVCAN_SRV_NUMBER NUM_SERVO_CHANNELS -#endif - -#ifndef AP_DRONECAN_SEND_GPS -#define AP_DRONECAN_SEND_GPS (BOARD_FLASH_SIZE > 1024) -#endif - -#define AP_UAVCAN_SW_VERS_MAJOR 1 -#define AP_UAVCAN_SW_VERS_MINOR 0 - -#define AP_UAVCAN_HW_VERS_MAJOR 1 -#define AP_UAVCAN_HW_VERS_MINOR 0 - -#define AP_UAVCAN_MAX_LED_DEVICES 4 - -// fwd-declare callback classes -class ButtonCb; -class TrafficReportCb; -class ActuatorStatusCb; -class ActuatorStatusVolzCb; -class ESCStatusCb; -class DebugCb; -class ParamGetSetCb; -class ParamExecuteOpcodeCb; -class AP_PoolAllocator; -class AP_UAVCAN_DNA_Server; - -#if defined(__GNUC__) && (__GNUC__ > 8) -#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH \ - _Pragma("GCC diagnostic push") \ - _Pragma("GCC diagnostic ignored \"-Wcast-function-type\"") -#define DISABLE_W_CAST_FUNCTION_TYPE_POP \ - _Pragma("GCC diagnostic pop") -#else -#define DISABLE_W_CAST_FUNCTION_TYPE_PUSH -#define DISABLE_W_CAST_FUNCTION_TYPE_POP -#endif -#if defined(__GNUC__) && (__GNUC__ >= 11) -#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID (void*) -#else -#define DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID -#endif - -/* - Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received, - the Callback will invoke registry to register the node as separate backend. -*/ -#define UC_REGISTRY_BINDER(ClassName_, DataType_) \ - class ClassName_ : public AP_UAVCAN::RegistryBinder { \ - typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \ - public: \ - ClassName_() : RegistryBinder() {} \ - DISABLE_W_CAST_FUNCTION_TYPE_PUSH \ - ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \ - RegistryBinder(uc, (Registry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \ - DISABLE_W_CAST_FUNCTION_TYPE_POP \ - } - -#define UC_CLIENT_CALL_REGISTRY_BINDER(ClassName_, DataType_) \ - class ClassName_ : public AP_UAVCAN::ClientCallRegistryBinder { \ - typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \ - public: \ - ClassName_() : ClientCallRegistryBinder() {} \ - DISABLE_W_CAST_FUNCTION_TYPE_PUSH \ - ClassName_(AP_UAVCAN* uc, CN_Registry ffunc) : \ - ClientCallRegistryBinder(uc, (ClientCallRegistry)DISABLE_W_CAST_FUNCTION_TYPE_WITH_VOID ffunc) {} \ - DISABLE_W_CAST_FUNCTION_TYPE_POP \ - } - -class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { - friend class AP_UAVCAN_DNA_Server; -public: - AP_UAVCAN(); - ~AP_UAVCAN(); - - static const struct AP_Param::GroupInfo var_info[]; - - // Return uavcan from @driver_index or nullptr if it's not ready or doesn't exist - static AP_UAVCAN *get_uavcan(uint8_t driver_index); - bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const; - - void init(uint8_t driver_index, bool enable_filters) override; - bool add_interface(AP_HAL::CANIface* can_iface) override; - - uavcan::Node<0>* get_node() { return _node; } - uint8_t get_driver_index() const { return _driver_index; } - - FUNCTOR_TYPEDEF(ParamGetSetIntCb, bool, AP_UAVCAN*, const uint8_t, const char*, int32_t &); - FUNCTOR_TYPEDEF(ParamGetSetFloatCb, bool, AP_UAVCAN*, const uint8_t, const char*, float &); - FUNCTOR_TYPEDEF(ParamSaveCb, void, AP_UAVCAN*, const uint8_t, bool); - - ///// SRV output ///// - void SRV_push_servos(void); - - ///// LED ///// - bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue); - - // buzzer - void set_buzzer_tone(float frequency, float duration_s); - - // send RTCMStream packets - void send_RTCMStream(const uint8_t *data, uint32_t len); - - // Send Reboot command - // Note: Do not call this from outside UAVCAN thread context, - // you can call this from uavcan callbacks and handlers. - // THIS IS NOT A THREAD SAFE API! - void send_reboot_request(uint8_t node_id); - - // set param value - bool set_parameter_on_node(uint8_t node_id, const char *name, float value, ParamGetSetFloatCb *cb); - bool set_parameter_on_node(uint8_t node_id, const char *name, int32_t value, ParamGetSetIntCb *cb); - bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetFloatCb *cb); - bool get_parameter_on_node(uint8_t node_id, const char *name, ParamGetSetIntCb *cb); - - // Save parameters - bool save_parameters_on_node(uint8_t node_id, ParamSaveCb *cb); - - template - class RegistryBinder { - protected: - typedef void (*Registry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const RegistryBinder& _cb); - AP_UAVCAN* _uc; - Registry _ffunc; - - public: - RegistryBinder() : - _uc(), - _ffunc(), - msg() {} - - RegistryBinder(AP_UAVCAN* uc, Registry ffunc) : - _uc(uc), - _ffunc(ffunc), - msg(nullptr) {} - - void operator()(const uavcan::ReceivedDataStructure& _msg) { - msg = &_msg; - _ffunc(_uc, _msg.getSrcNodeID().get(), *this); - } - - const uavcan::ReceivedDataStructure *msg; - }; - - // ClientCallRegistryBinder - template - class ClientCallRegistryBinder { - protected: - typedef void (*ClientCallRegistry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const ClientCallRegistryBinder& _cb); - AP_UAVCAN* _uc; - ClientCallRegistry _ffunc; - public: - ClientCallRegistryBinder() : - _uc(), - _ffunc(), - rsp() {} - - ClientCallRegistryBinder(AP_UAVCAN* uc, ClientCallRegistry ffunc) : - _uc(uc), - _ffunc(ffunc), - rsp(nullptr) {} - - void operator()(const uavcan::ServiceCallResult& _rsp) { - rsp = &_rsp; - _ffunc(_uc, _rsp.getCallID().server_node_id.get(), *this); - } - const uavcan::ServiceCallResult *rsp; - }; - - // options bitmask - enum class Options : uint16_t { - DNA_CLEAR_DATABASE = (1U<<0), - DNA_IGNORE_DUPLICATE_NODE = (1U<<1), - CANFD_ENABLED = (1U<<2), - DNA_IGNORE_UNHEALTHY_NODE = (1U<<3), - USE_ACTUATOR_PWM = (1U<<4), - SEND_GNSS = (1U<<5), - }; - - // check if a option is set - bool option_is_set(Options option) const { - return (uint16_t(_options.get()) & uint16_t(option)) != 0; - } - - // check if a option is set and if it is then reset it to - // 0. return true if it was set - bool check_and_reset_option(Options option); - - // This will be needed to implement if UAVCAN is used with multithreading - // Such cases will be firmware update, etc. - class RaiiSynchronizer {}; - -private: - void loop(void); - - ///// SRV output ///// - void SRV_send_actuator(); - void SRV_send_esc(); - - ///// LED ///// - void led_out_send(); - - // buzzer - void buzzer_send(); - - // SafetyState - void safety_state_send(); - - // send notify vehicle state - void notify_state_send(); - - // send GNSS injection - void rtcm_stream_send(); - - // send parameter get/set request - void send_parameter_request(); - - // send parameter save request - void send_parameter_save_request(); - - // periodic logging - void logging(); - - // set parameter on a node - ParamGetSetIntCb *param_int_cb; - ParamGetSetFloatCb *param_float_cb; - bool param_request_sent = true; - HAL_Semaphore _param_sem; - uint8_t param_request_node_id; - - // save parameters on a node - ParamSaveCb *save_param_cb; - bool param_save_request_sent = true; - HAL_Semaphore _param_save_sem; - uint8_t param_save_request_node_id; - - // UAVCAN parameters - AP_Int8 _uavcan_node; - AP_Int32 _servo_bm; - AP_Int32 _esc_bm; - AP_Int8 _esc_offset; - AP_Int16 _servo_rate_hz; - AP_Int16 _options; - AP_Int16 _notify_state_hz; - AP_Int16 _pool_size; - - AP_PoolAllocator *_allocator; - AP_UAVCAN_DNA_Server *_dna_server; - - uavcan::Node<0> *_node; - - uint8_t _driver_index; - - uavcan::CanIfaceMgr* _iface_mgr; - char _thread_name[13]; - bool _initialized; - ///// SRV output ///// - struct { - uint16_t pulse; - bool esc_pending; - bool servo_pending; - } _SRV_conf[UAVCAN_SRV_NUMBER]; - - uint32_t _esc_send_count; - uint32_t _srv_send_count; - uint32_t _fail_send_count; - - uint8_t _SRV_armed; - uint32_t _SRV_last_send_us; - HAL_Semaphore SRV_sem; - - // last log time - uint32_t last_log_ms; - - ///// LED ///// - struct led_device { - uint8_t led_index; - uint8_t red; - uint8_t green; - uint8_t blue; - }; - - struct { - led_device devices[AP_UAVCAN_MAX_LED_DEVICES]; - uint8_t devices_count; - uint64_t last_update; - } _led_conf; - - HAL_Semaphore _led_out_sem; - - // buzzer - struct { - HAL_Semaphore sem; - float frequency; - float duration; - uint8_t pending_mask; // mask of interfaces to send to - } _buzzer; - -#if AP_DRONECAN_SEND_GPS - // send GNSS Fix and yaw, same thing AP_GPS_UAVCAN would receive - void gnss_send_fix(); - void gnss_send_yaw(); - - // GNSS Fix and Status - struct { - uint32_t last_gps_lib_fix_ms; - uint32_t last_send_status_ms; - uint32_t last_lib_yaw_time_ms; - } _gnss; -#endif - - // GNSS RTCM injection - struct { - HAL_Semaphore sem; - uint32_t last_send_ms; - ByteBuffer *buf; - } _rtcm_stream; - - // ESC - - static HAL_Semaphore _telem_sem; - - // safety status send state - uint32_t _last_safety_state_ms; - - // notify vehicle state - uint32_t _last_notify_state_ms; - - // incoming button handling - static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb); - static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb); - static void handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusCb &cb); - static void handle_actuator_status_Volz(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ActuatorStatusVolzCb &cb); - static void handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb); - static bool is_esc_data_index_valid(const uint8_t index); - static void handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugCb &cb); - static void handle_param_get_set_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamGetSetCb &cb); - static void handle_param_save_response(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ParamExecuteOpcodeCb &cb); -}; - -#endif // #if HAL_ENABLE_LIBUAVCAN_DRIVERS diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h b/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h deleted file mode 100644 index eba4923a3c2f6..0000000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_Clock.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * 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 . - * - * Author: Siddharth Bharat Purohit - */ - - -#pragma once - -#include - -namespace uavcan -{ -class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable -{ -public: - SystemClock() = default; - - void adjustUtc(uavcan::UtcDuration adjustment) override - { - utc_adjustment_usec = adjustment.toUSec(); - } - - uavcan::MonotonicTime getMonotonic() const override - { - return uavcan::MonotonicTime::fromUSec(AP_HAL::native_micros64()); - } - - uavcan::UtcTime getUtc() const override - { - return uavcan::UtcTime::fromUSec(AP_HAL::native_micros64() + utc_adjustment_usec); - } - - int64_t getAdjustUsec() const - { - return utc_adjustment_usec; - } - - static SystemClock& instance() - { - static SystemClock inst; - return inst; - } - -private: - int64_t utc_adjustment_usec; -}; -} \ No newline at end of file diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp deleted file mode 100644 index bfa04b408432e..0000000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.cpp +++ /dev/null @@ -1,238 +0,0 @@ - -/* - * 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 . - * - * Author: Siddharth Bharat Purohit - */ - -#include "AP_UAVCAN_IfaceMgr.h" - -#if HAL_ENABLE_LIBUAVCAN_DRIVERS -#include "AP_UAVCAN_Clock.h" -#include -#include -using namespace uavcan; -extern const AP_HAL::HAL& hal; -#define LOG_TAG "UAVCANIface" - -/***************************************************** - * * - * CAN Iface * - * * - * ***************************************************/ - -/** - * Non-blocking transmission. - * - * If the frame wasn't transmitted upon TX deadline, the driver should discard it. - * - * Note that it is LIKELY that the library will want to send the frames that were passed into the select() - * method as the next ones to transmit, but it is NOT guaranteed. The library can replace those with new - * frames between the calls. - * - * @return 1 = one frame transmitted, 0 = TX buffer full, negative for error. - */ -int16_t CanIface::send(const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags) -{ - if (can_iface_ == UAVCAN_NULLPTR) { - return -1; - } - return can_iface_->send(AP_HAL::CANFrame(frame.id, frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.isCanFDFrame()), tx_deadline.toUSec(), flags); -} - -/** - * Non-blocking reception. - * - * Timestamps should be provided by the CAN driver, ideally by the hardware CAN controller. - * - * Monotonic timestamp is required and can be not precise since it is needed only for - * protocol timing validation (transfer timeouts and inter-transfer intervals). - * - * UTC timestamp is optional, if available it will be used for precise time synchronization; - * must be set to zero if not available. - * - * Refer to @ref ISystemClock to learn more about timestamps. - * - * @param [out] out_ts_monotonic Monotonic timestamp, mandatory. - * @param [out] out_ts_utc UTC timestamp, optional, zero if unknown. - * @return 1 = one frame received, 0 = RX buffer empty, negative for error. - */ -int16_t CanIface::receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, UtcTime& out_ts_utc, - CanIOFlags& out_flags) -{ - - if (can_iface_ == UAVCAN_NULLPTR) { - return -1; - } - AP_HAL::CANFrame frame; - uint64_t rx_timestamp; - uint16_t flags; - out_ts_monotonic = SystemClock::instance().getMonotonic(); - int16_t ret = can_iface_->receive(frame, rx_timestamp, flags); - if (ret < 0) { - return ret; - } - out_frame = CanFrame(frame.id, (const uint8_t*)frame.data, AP_HAL::CANFrame::dlcToDataLength(frame.dlc), frame.canfd); - out_flags = flags; - if (rx_timestamp != 0) { - out_ts_utc = uavcan::UtcTime::fromUSec(SystemClock::instance().getAdjustUsec() + rx_timestamp); - } else { - out_ts_utc = uavcan::UtcTime::fromUSec(0); - } - return ret; -} - -/** - * Number of available hardware filters. - */ -uint16_t CanIface::getNumFilters() const -{ - if (can_iface_ == UAVCAN_NULLPTR) { - return 0; - } - return can_iface_->getNumFilters(); -} - -/** - * Continuously incrementing counter of hardware errors. - * Arbitration lost should not be treated as a hardware error. - */ -uint64_t CanIface::getErrorCount() const -{ - if (can_iface_ == UAVCAN_NULLPTR) { - return 0; - } - return can_iface_->getErrorCount(); -} - -/***************************************************** - * * - * CAN Driver * - * * - * ***************************************************/ - -bool CanIfaceMgr::add_interface(AP_HAL::CANIface *can_iface) -{ - if (num_ifaces > HAL_NUM_CAN_IFACES) { - AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Num Ifaces Exceeded\n"); - return false; - } - if (can_iface == nullptr) { - AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface Null\n"); - return false; - } - if (ifaces[num_ifaces] != nullptr) { - AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Iface already added\n"); - return false; - } - ifaces[num_ifaces] = new CanIface(can_iface); - if (ifaces[num_ifaces] == nullptr) { - AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Can't alloc uavcan::iface\n"); - return false; - } - if (!ifaces[num_ifaces]->can_iface_->set_event_handle(&_event_handle)) { - AP::can().log_text(AP_CANManager::LOG_ERROR, LOG_TAG, "UAVCANIfaceMgr: Setting event handle failed\n"); - return false; - } - AP::can().log_text(AP_CANManager::LOG_INFO, LOG_TAG, "UAVCANIfaceMgr: Successfully added interface %d\n", int(num_ifaces)); - num_ifaces++; - return true; -} -/** - * Returns an interface by index, or null pointer if the index is out of range. - */ -ICanIface* CanIfaceMgr::getIface(uint8_t iface_index) -{ - if (iface_index >= num_ifaces) { - return UAVCAN_NULLPTR; - } - return ifaces[iface_index]; -} - -/** - * Total number of available CAN interfaces. - * This value shall not change after initialization. - */ -uint8_t CanIfaceMgr::getNumIfaces() const -{ - return num_ifaces; -} - -CanSelectMasks CanIfaceMgr::makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const -{ - CanSelectMasks msk; - for (uint8_t i = 0; i < num_ifaces; i++) { - bool read = in_mask.read & (1 << i); - bool write = in_mask.write & (1 << i); - CanIface* iface = ifaces[i]; - if (iface == nullptr) { - continue; - } - if (pending_tx[i] == UAVCAN_NULLPTR) { - if (iface->can_iface_->select(read, write, nullptr, 0)) { - msk.read |= (read ? 1 : 0) << i; - msk.write |= (write ? 1 : 0) << i; - } - } else { - AP_HAL::CANFrame frame {pending_tx[i]->id, pending_tx[i]->data, AP_HAL::CANFrame::dlcToDataLength(pending_tx[i]->dlc)}; - if (iface->can_iface_->select(read, write, &frame, 0)) { - msk.read |= (read ? 1 : 0) << i; - msk.write |= (write ? 1 : 0) << i; - } - } - } - - return msk; -} - -/** - * Block until the deadline, or one of the specified interfaces becomes available for read or write. - * - * Iface masks will be modified by the driver to indicate which exactly interfaces are available for IO. - * - * Bit position in the masks defines interface index. - * - * Note that it is allowed to return from this method even if no requested events actually happened, or if - * there are events that were not requested by the library. - * - * The pending TX argument contains an array of pointers to CAN frames that the library wants to transmit - * next, per interface. This is intended to allow the driver to properly prioritize transmissions; many - * drivers will not need to use it. If a write flag for the given interface is set to one in the select mask - * structure, then the corresponding pointer is guaranteed to be valid (not UAVCAN_NULLPTR). - * - * @param [in,out] inout_masks Masks indicating which interfaces are needed/available for IO. - * @param [in] pending_tx Array of frames, per interface, that are likely to be transmitted next. - * @param [in] blocking_deadline Zero means non-blocking operation. - * @return Positive number of ready interfaces or negative error code. - */ -int16_t CanIfaceMgr::select(CanSelectMasks& inout_masks, - const CanFrame* (& pending_tx)[MaxCanIfaces], - const MonotonicTime blocking_deadline) -{ - const CanSelectMasks in_masks = inout_masks; - const uint64_t time = SystemClock::instance().getMonotonic().toUSec(); - - inout_masks = makeSelectMasks(in_masks, pending_tx); // Check if we already have some of the requested events - if ((inout_masks.read & in_masks.read) != 0 || - (inout_masks.write & in_masks.write) != 0) { - return 1; - } - if (time < blocking_deadline.toUSec()) { - _event_handle.wait(blocking_deadline.toUSec() - time); // Block until timeout expires or any iface updates - } - - inout_masks = makeSelectMasks(in_masks, pending_tx); // Return what we got even if none of the requested events are set - return 1; // Return value doesn't matter as long as it is non-negative -} -#endif //HAL_ENABLE_LIBUAVCAN_DRIVERSs diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h b/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h deleted file mode 100644 index 0c3b23ba18f1b..0000000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_IfaceMgr.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * 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 . - * - * Author: Siddharth Bharat Purohit - */ -#pragma once - -#include -#if HAL_ENABLE_LIBUAVCAN_DRIVERS - -#include - -namespace uavcan -{ - -class CanDriver; - -class CanIface : public ICanIface, Noncopyable -{ - friend class CanIfaceMgr; - AP_HAL::CANIface* can_iface_; -public: - CanIface(AP_HAL::CANIface *can_iface) : can_iface_(can_iface) {} - - virtual int16_t send(const CanFrame& frame, MonotonicTime tx_deadline, - CanIOFlags flags) override; - - virtual int16_t receive(CanFrame& out_frame, MonotonicTime& out_ts_monotonic, - UtcTime& out_ts_utc, CanIOFlags& out_flags) override; - - virtual int16_t configureFilters(const CanFilterConfig* filter_configs, - uint16_t num_configs) override { - return 0; - } - - uint16_t getNumFilters() const override; - - uint64_t getErrorCount() const override; -}; - -/** - * Generic CAN driver. - */ -class CanIfaceMgr : public ICanDriver, Noncopyable -{ - CanIface* ifaces[HAL_NUM_CAN_IFACES]; - uint8_t num_ifaces; - HAL_EventHandle _event_handle; -public: - bool add_interface(AP_HAL::CANIface *can_drv); - - ICanIface* getIface(uint8_t iface_index) override; - - uint8_t getNumIfaces() const override; - - CanSelectMasks makeSelectMasks(const CanSelectMasks in_mask, const CanFrame* (& pending_tx)[MaxCanIfaces]) const; - - int16_t select(CanSelectMasks& inout_masks, - const CanFrame* (& pending_tx)[MaxCanIfaces], - const MonotonicTime blocking_deadline) override; -}; - -} -#endif diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp deleted file mode 100644 index 152381c3b829d..0000000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_pool.cpp +++ /dev/null @@ -1,90 +0,0 @@ -/* - * 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 . - * - */ -/* - based on dynamic_memory.hpp which is: - Copyright (C) 2014 Pavel Kirienko - */ - -#include - -#if HAL_ENABLE_LIBUAVCAN_DRIVERS - -#include "AP_UAVCAN.h" -#include "AP_UAVCAN_pool.h" -#include - -AP_PoolAllocator::AP_PoolAllocator(uint16_t _pool_size) : - num_blocks(_pool_size / UAVCAN_NODE_POOL_BLOCK_SIZE) -{ -} - -bool AP_PoolAllocator::init(void) -{ - WITH_SEMAPHORE(sem); - pool_nodes = (Node *)calloc(num_blocks, UAVCAN_NODE_POOL_BLOCK_SIZE); - if (pool_nodes == nullptr) { - return false; - } - for (uint16_t i=0; i<(num_blocks-1); i++) { - pool_nodes[i].next = &pool_nodes[i+1]; - } - free_list = &pool_nodes[0]; - return true; -} - -void* AP_PoolAllocator::allocate(std::size_t size) -{ - WITH_SEMAPHORE(sem); - if (free_list == nullptr || size > UAVCAN_NODE_POOL_BLOCK_SIZE) { - return nullptr; - } - Node *ret = free_list; - const uint32_t blk = ret - pool_nodes; - if (blk >= num_blocks) { - INTERNAL_ERROR(AP_InternalError::error_t::mem_guard); - return nullptr; - } - free_list = free_list->next; - - used++; - if (used > max_used) { - max_used = used; - } - - return ret; -} - -void AP_PoolAllocator::deallocate(const void* ptr) -{ - if (ptr == nullptr) { - return; - } - WITH_SEMAPHORE(sem); - - Node *p = reinterpret_cast(const_cast(ptr)); - const uint32_t blk = p - pool_nodes; - if (blk >= num_blocks) { - INTERNAL_ERROR(AP_InternalError::error_t::mem_guard); - return; - } - p->next = free_list; - free_list = p; - - used--; -} - -#endif // HAL_ENABLE_LIBUAVCAN_DRIVERS - diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_pool.h b/libraries/AP_UAVCAN/AP_UAVCAN_pool.h deleted file mode 100644 index 2549526bb7912..0000000000000 --- a/libraries/AP_UAVCAN/AP_UAVCAN_pool.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * 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 . - * - */ -/* - based on dynamic_memory.hpp which is: - Copyright (C) 2014 Pavel Kirienko - */ - -#pragma once - -#include "AP_UAVCAN.h" - -#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE -#if HAL_CANFD_SUPPORTED -#define UAVCAN_NODE_POOL_BLOCK_SIZE 128 -#else -#define UAVCAN_NODE_POOL_BLOCK_SIZE 64 -#endif -#endif - -class AP_PoolAllocator : public uavcan::IPoolAllocator -{ -public: - AP_PoolAllocator(uint16_t _pool_size); - - bool init(void); - void *allocate(std::size_t size) override; - void deallocate(const void* ptr) override; - - uint16_t getBlockCapacity() const override { - return num_blocks; - } - -private: - const uint16_t num_blocks; - - union Node { - uint8_t data[UAVCAN_NODE_POOL_BLOCK_SIZE]; - Node* next; - }; - - Node *free_list; - Node *pool_nodes; - HAL_Semaphore sem; - - uint16_t used; - uint16_t max_used; -}; diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp deleted file mode 100644 index bf2c76257cf04..0000000000000 --- a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp +++ /dev/null @@ -1,285 +0,0 @@ -/* - simple UAVCAN network sniffer as an ArduPilot firmware - */ -#include -#include - -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - -#include - -#include - -#include - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include -#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include -#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS -#include -#endif - -void setup(); -void loop(); - -const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - -#define UAVCAN_NODE_POOL_SIZE 8192 -#ifdef UAVCAN_NODE_POOL_BLOCK_SIZE -#undef UAVCAN_NODE_POOL_BLOCK_SIZE -#endif -#define UAVCAN_NODE_POOL_BLOCK_SIZE 256 - -#define debug_uavcan(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0) - -class UAVCAN_sniffer { -public: - UAVCAN_sniffer(); - ~UAVCAN_sniffer(); - - void init(void); - void loop(void); - void print_stats(void); - -private: - uint8_t driver_index = 0; - uavcan::Node<0> *_node; - - // This will be needed to implement if UAVCAN is used with multithreading - // Such cases will be firmware update, etc. - class RaiiSynchronizer { - public: - RaiiSynchronizer() - { - } - - ~RaiiSynchronizer() - { - } - }; - - uavcan::HeapBasedPoolAllocator _node_allocator; - - AP_CANManager can_mgr; -}; - -static struct { - const char *msg_name; - uint32_t count; -} counters[100]; - -static void count_msg(const char *name) -{ - for (uint16_t i=0; i& msg) { count_msg(msg.getDataTypeFullName()); } - -MSG_CB(uavcan::protocol::NodeStatus, NodeStatus) -MSG_CB(uavcan::equipment::gnss::Fix2, Fix2) -MSG_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary) -MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength) -MSG_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2); -MSG_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure) -MSG_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature) -MSG_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo); -MSG_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand) -MSG_CB(uavcan::equipment::esc::RawCommand, RawCommand) -MSG_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); -MSG_CB(com::hex::equipment::flow::Measurement, Measurement); - -void UAVCAN_sniffer::init(void) -{ - const_cast (hal).can[driver_index] = new HAL_CANIface(driver_index); - - if (hal.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); - - if (!hal.can[driver_index]->is_initialized()) { - debug_uavcan("Can not initialised\n"); - return; - } - uavcan::CanIfaceMgr *_uavcan_iface_mgr = new uavcan::CanIfaceMgr; - - if (_uavcan_iface_mgr == nullptr) { - return; - } - - if (!_uavcan_iface_mgr->add_interface(hal.can[driver_index])) { - debug_uavcan("Failed to add iface"); - return; - } - - _node = new uavcan::Node<0>(*_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator); - - if (_node == nullptr) { - return; - } - - if (_node->isStarted()) { - return; - } - - uavcan::NodeID self_node_id(9); - _node->setNodeID(self_node_id); - - char ndname[20]; - snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", driver_index); - - uavcan::NodeStatusProvider::NodeName name(ndname); - _node->setName(name); - - uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion - sw_version.major = AP_UAVCAN_SW_VERS_MAJOR; - sw_version.minor = AP_UAVCAN_SW_VERS_MINOR; - _node->setSoftwareVersion(sw_version); - - uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion - - hw_version.major = AP_UAVCAN_HW_VERS_MAJOR; - hw_version.minor = AP_UAVCAN_HW_VERS_MINOR; - _node->setHardwareVersion(hw_version); - - int start_res = _node->start(); - if (start_res < 0) { - debug_uavcan("UAVCAN: node start problem\n\r"); - return; - } - -#define START_CB(mtype, cbname) (new uavcan::Subscriber(*_node))->start(cb_ ## cbname) - - START_CB(uavcan::protocol::NodeStatus, NodeStatus); - START_CB(uavcan::equipment::gnss::Fix2, Fix2); - START_CB(uavcan::equipment::gnss::Auxiliary, Auxiliary); - START_CB(uavcan::equipment::ahrs::MagneticFieldStrength, MagneticFieldStrength); - START_CB(uavcan::equipment::ahrs::MagneticFieldStrength2, MagneticFieldStrength2); - START_CB(uavcan::equipment::air_data::StaticPressure, StaticPressure); - START_CB(uavcan::equipment::air_data::StaticTemperature, StaticTemperature); - START_CB(uavcan::equipment::power::BatteryInfo, BatteryInfo); - START_CB(uavcan::equipment::actuator::ArrayCommand, ArrayCommand); - START_CB(uavcan::equipment::esc::RawCommand, RawCommand); - START_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); - START_CB(com::hex::equipment::flow::Measurement, Measurement); - - - /* - * Informing other nodes that we're ready to work. - * Default mode is INITIALIZING. - */ - _node->setModeOperational(); - - debug_uavcan("UAVCAN: init done\n\r"); -} - -void UAVCAN_sniffer::loop(void) -{ - if (_node == nullptr) { - return; - } - - _node->spin(uavcan::MonotonicDuration::fromMSec(1)); -} - -void UAVCAN_sniffer::print_stats(void) -{ - hal.console->printf("%lu\n", (unsigned long)AP_HAL::micros()); - for (uint16_t i=0;i<100;i++) { - if (counters[i].msg_name == nullptr) { - break; - } - hal.console->printf("%s: %u\n", counters[i].msg_name, unsigned(counters[i].count)); - counters[i].count = 0; - } - hal.console->printf("\n"); -} - -static UAVCAN_sniffer sniffer; - -UAVCAN_sniffer::UAVCAN_sniffer() : - _node_allocator(UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_SIZE) -{} - -UAVCAN_sniffer::~UAVCAN_sniffer() -{ -} - -void setup(void) -{ - hal.scheduler->delay(2000); - hal.console->printf("Starting UAVCAN sniffer\n"); - sniffer.init(); -} - -void loop(void) -{ - sniffer.loop(); - static uint32_t last_print_ms; - uint32_t now = AP_HAL::millis(); - if (now - last_print_ms >= 1000) { - last_print_ms = now; - sniffer.print_stats(); - } - - // auto-reboot for --upload - if (hal.console->available() > 50) { - hal.console->printf("rebooting\n"); - hal.console->discard_input(); - hal.scheduler->reboot(false); - } - hal.console->discard_input(); -} - -AP_HAL_MAIN(); - -#else - -#include - -const AP_HAL::HAL& hal = AP_HAL::get_HAL(); - -static void loop() { } -static void setup() -{ - printf("Board not currently supported\n"); -} - -AP_HAL_MAIN(); -#endif diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript deleted file mode 100644 index 719ec151ba9d4..0000000000000 --- a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/wscript +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python -# encoding: utf-8 - -def build(bld): - bld.ap_example( - use='ap', - ) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c0337a9d8b19a..94e26f7f8c19e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -82,7 +82,7 @@ #include #endif #include - #include + #include #endif #if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) @@ -3812,6 +3812,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) #if AP_CAMERA_ENABLED case MAVLINK_MSG_ID_DIGICAM_CONTROL: case MAVLINK_MSG_ID_GOPRO_HEARTBEAT: // heartbeat from a GoPro in Solo gimbal + case MAVLINK_MSG_ID_CAMERA_INFORMATION: { AP_Camera *camera = AP::camera(); if (camera == nullptr) { @@ -3868,6 +3869,7 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_GIMBAL_REPORT: case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: handle_mount_message(msg); break; #endif @@ -4389,7 +4391,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_ case AP_CANManager::Driver_Type_PiccoloCAN: // TODO - Run PiccoloCAN pre-flight checks here break; - case AP_CANManager::Driver_Type_UAVCAN: + case AP_CANManager::Driver_Type_DroneCAN: case AP_CANManager::Driver_Type_None: default: break; @@ -4969,12 +4971,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_roi(const Location &roi_loc) } if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { - // switch off the camera tracking if enabled - if (mount->get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - mount->set_mode_to_default(); - } + mount->clear_roi_target(); } else { - // send the command to the camera mount mount->set_roi_target(roi_loc); } return MAV_RESULT_ACCEPTED; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index bd5d65e261d71..9d94b6655276e 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -46,6 +46,7 @@ extern const AP_HAL::HAL& hal; #include #include #include +#include #include #include #include diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index e8fedae4c7194..f19ab87a19abc 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -582,7 +582,7 @@ class RC_Channels { // get mask of enabled protocols uint32_t enabled_protocols() const; - // returns true if we have had a direct detach RC reciever, does not include overrides + // returns true if we have had a direct detach RC receiver, does not include overrides bool has_had_rc_receiver() const { return _has_had_rc_receiver; } // returns true if we have had an override on any channel @@ -653,7 +653,7 @@ class RC_Channels { uint32_t last_update_ms; bool has_new_overrides; - bool _has_had_rc_receiver; // true if we have had a direct detach RC reciever, does not include overrides + bool _has_had_rc_receiver; // true if we have had a direct detach RC receiver, does not include overrides bool _has_had_override; // true if we have had an override on any channel AP_Float _override_timeout; diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 24220c4f96b8c..7c8082b460f71 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1016,6 +1016,12 @@ void Aircraft::update_external_payload(const struct sitl_input &input) if (ie24) { ie24->update(input); } + +#if AP_TEST_DRONECAN_DRIVERS + if (dronecan) { + dronecan->update(); + } +#endif } void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 236630fbb17e0..0a4d5768932cb 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -147,7 +147,9 @@ class Aircraft { void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; } void set_precland(SIM_Precland *_precland); void set_i2c(class I2C *_i2c) { i2c = _i2c; } - +#if AP_TEST_DRONECAN_DRIVERS + void set_dronecan_device(DroneCANDevice *_dronecan) { dronecan = _dronecan; } +#endif float get_battery_voltage() const { return battery_voltage; } protected: @@ -174,7 +176,7 @@ class Aircraft { Vector3f accel_body{0.0f, 0.0f, -GRAVITY_MSS}; // m/s/s NED, body frame float airspeed; // m/s, apparent airspeed float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube - float battery_voltage = -1.0f; + float battery_voltage = 0.0f; float battery_current; float local_ground_level; // ground level at local position bool lock_step_scheduled; @@ -335,6 +337,9 @@ class Aircraft { IntelligentEnergy24 *ie24; SIM_Precland *precland; class I2C *i2c; +#if AP_TEST_DRONECAN_DRIVERS + DroneCANDevice *dronecan; +#endif }; } // namespace SITL diff --git a/libraries/SITL/SIM_DroneCANDevice.cpp b/libraries/SITL/SIM_DroneCANDevice.cpp new file mode 100644 index 0000000000000..51d1c9ac3d734 --- /dev/null +++ b/libraries/SITL/SIM_DroneCANDevice.cpp @@ -0,0 +1,238 @@ +/* + 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 . + */ +/* + base class for CAN simulated devices +*/ +#include "SIM_DroneCANDevice.h" +#if AP_TEST_DRONECAN_DRIVERS + +#include +#include +#include +#include +#include +#include +#include + + +using namespace SITL; + +void DroneCANDevice::update_baro() { + const uint64_t now = AP_HAL::micros64(); + if (((now - _baro_last_update_us) < 10000) && (_baro_last_update_us != 0)) { + return; + } + _baro_last_update_us = now; + const uint32_t now_ms = AP_HAL::millis(); + float sim_alt = AP::sitl()->state.altitude; + + if (AP::sitl()->baro_count < 1) { + // barometer is disabled + return; + } + + sim_alt += AP::sitl()->baro[0].drift * now_ms * 0.001f; + sim_alt += AP::sitl()->baro[0].noise * rand_float(); + + + // add baro glitch + sim_alt += AP::sitl()->baro[0].glitch; + + // add delay + uint32_t best_time_delta = 200; // initialise large time representing buffer entry closest to current time - delay. + uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay. + + // storing data from sensor to buffer + if (now_ms - _last_store_time >= 10) { // store data every 10 ms. + _last_store_time = now_ms; + if (_store_index > _buffer_length - 1) { // reset buffer index if index greater than size of buffer + _store_index = 0; + } + + // if freezed barometer, report altitude to last recorded altitude + if (AP::sitl()->baro[0].freeze == 1) { + sim_alt = _last_altitude; + } else { + _last_altitude = sim_alt; + } + + _buffer[_store_index].data = sim_alt; // add data to current index + _buffer[_store_index].time = _last_store_time; // add time_stamp to current index + _store_index = _store_index + 1; // increment index + } + + // return delayed measurement + const uint32_t delayed_time = now_ms - AP::sitl()->baro[0].delay; // get time corresponding to delay + + // find data corresponding to delayed time in buffer + for (uint8_t i = 0; i <= _buffer_length - 1; i++) { + // find difference between delayed time and time stamp in buffer + uint32_t time_delta = abs( + (int32_t)(delayed_time - _buffer[i].time)); + // if this difference is smaller than last delta, store this time + if (time_delta < best_time_delta) { + best_index = i; + best_time_delta = time_delta; + } + } + if (best_time_delta < 200) { // only output stored state if < 200 msec retrieval error + sim_alt = _buffer[best_index].data; + } + +#if !APM_BUILD_TYPE(APM_BUILD_ArduSub) + float sigma, delta, theta; + + AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta); + float p = SSL_AIR_PRESSURE * delta; + float T = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta); + + AP_Baro_SITL::temperature_adjustment(p, T); + T = C_TO_KELVIN(T); +#else + float rho, delta, theta; + AP_Baro::SimpleUnderWaterAtmosphere(-sim_alt * 0.001f, rho, delta, theta); + float p = SSL_AIR_PRESSURE * delta; + float T = SSL_AIR_TEMPERATURE * theta; +#endif + + // add in correction for wind effects + p += AP_Baro_SITL::wind_pressure_correction(0); + static Canard::Publisher press_pub{CanardInterface::get_test_iface()}; + static Canard::Publisher temp_pub{CanardInterface::get_test_iface()}; + uavcan_equipment_air_data_StaticPressure press_msg {}; + press_msg.static_pressure = p; + press_pub.broadcast(press_msg); + uavcan_equipment_air_data_StaticTemperature temp_msg {}; + temp_msg.static_temperature = T; + temp_pub.broadcast(temp_msg); +} + +void DroneCANDevice::update_airspeed() { + const uint32_t now = AP_HAL::micros64(); + if ((now - _airspeed_last_update_us < 50000) && (_airspeed_last_update_us != 0)) { + return; + } + _airspeed_last_update_us = now; + uavcan_equipment_air_data_RawAirData msg {}; + msg.differential_pressure = AP::sitl()->state.airspeed_raw_pressure[0]; + + // this was mostly swiped from SIM_Airspeed_DLVR: + const float sim_alt = AP::sitl()->state.altitude; + + float sigma, delta, theta; + AP_Baro::SimpleAtmosphere(sim_alt * 0.001f, sigma, delta, theta); + + // To Do: Add a sensor board temperature offset parameter + msg.static_air_temperature = SSL_AIR_TEMPERATURE * theta + 25.0; + + static Canard::Publisher raw_air_pub{CanardInterface::get_test_iface()}; + raw_air_pub.broadcast(msg); +} + +void DroneCANDevice::_setup_eliptical_correcion(uint8_t i) +{ + Vector3f diag = AP::sitl()->mag_diag[i].get(); + if (diag.is_zero()) { + diag = {1,1,1}; + } + const Vector3f &diagonals = diag; + const Vector3f &offdiagonals = AP::sitl()->mag_offdiag[i]; + + if (diagonals == _last_dia && offdiagonals == _last_odi) { + return; + } + + _eliptical_corr = Matrix3f(diagonals.x, offdiagonals.x, offdiagonals.y, + offdiagonals.x, diagonals.y, offdiagonals.z, + offdiagonals.y, offdiagonals.z, diagonals.z); + if (!_eliptical_corr.invert()) { + _eliptical_corr.identity(); + } + _last_dia = diag; + _last_odi = offdiagonals; +} + +void DroneCANDevice::update_compass() { + + // Sampled at 100Hz + const uint32_t now = AP_HAL::micros64(); + if ((now - _compass_last_update_us < 10000) && (_compass_last_update_us != 0)) { + return; + } + _compass_last_update_us = now; + + // calculate sensor noise and add to 'truth' field in body frame + // units are milli-Gauss + Vector3f noise = rand_vec3f() * AP::sitl()->mag_noise; + Vector3f new_mag_data = AP::sitl()->state.bodyMagField + noise; + + _setup_eliptical_correcion(0); + Vector3f f = (_eliptical_corr * new_mag_data) - AP::sitl()->mag_ofs[0].get(); + // rotate compass + f.rotate_inverse((enum Rotation)AP::sitl()->mag_orient[0].get()); + f.rotate(AP::compass().get_board_orientation()); + // scale the compass to simulate sensor scale factor errors + f *= AP::sitl()->mag_scaling[0]; + + static Canard::Publisher mag_pub{CanardInterface::get_test_iface()}; + uavcan_equipment_ahrs_MagneticFieldStrength mag_msg {}; + mag_msg.magnetic_field_ga[0] = f.x/1000.0f; + mag_msg.magnetic_field_ga[1] = f.y/1000.0f; + mag_msg.magnetic_field_ga[2] = f.z/1000.0f; + mag_msg.magnetic_field_covariance.len = 0; + mag_pub.broadcast(mag_msg); + static Canard::Publisher mag2_pub{CanardInterface::get_test_iface()}; + uavcan_equipment_ahrs_MagneticFieldStrength2 mag2_msg; + mag2_msg.magnetic_field_ga[0] = f.x/1000.0f; + mag2_msg.magnetic_field_ga[1] = f.y/1000.0f; + mag2_msg.magnetic_field_ga[2] = f.z/1000.0f; + mag2_msg.sensor_id = 0; + mag2_msg.magnetic_field_covariance.len = 0; + mag2_pub.broadcast(mag2_msg); +} + +void DroneCANDevice::update_rangefinder() { + + // Sampled at 100Hz + const uint32_t now = AP_HAL::micros64(); + if ((now - _rangefinder_last_update_us < 10000) && (_rangefinder_last_update_us != 0)) { + return; + } + _rangefinder_last_update_us = now; + static Canard::Publisher pub{CanardInterface::get_test_iface()}; + uavcan_equipment_range_sensor_Measurement msg; + msg.timestamp.usec = AP_HAL::micros64(); + msg.sensor_id = 0; + msg.sensor_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SENSOR_TYPE_LIDAR; + const float dist = AP::sitl()->get_rangefinder(0); + if (is_positive(dist)) { + msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_VALID_RANGE; + msg.range = dist; + } else { + msg.reading_type = UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_READING_TYPE_TOO_FAR; + msg.range = 0; + } + pub.broadcast(msg); +} + +void DroneCANDevice::update() +{ + update_baro(); + update_airspeed(); + update_compass(); + update_rangefinder(); +} + +#endif // AP_TEST_DRONECAN_DRIVERS diff --git a/libraries/SITL/SIM_DroneCANDevice.h b/libraries/SITL/SIM_DroneCANDevice.h new file mode 100644 index 0000000000000..98e5c7c9e0378 --- /dev/null +++ b/libraries/SITL/SIM_DroneCANDevice.h @@ -0,0 +1,60 @@ +/* + 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 . + */ +/* + base class for CAN simulated devices +*/ + +#pragma once + +#include +#include +#include +#include + +#if AP_TEST_DRONECAN_DRIVERS +namespace SITL { + +class DroneCANDevice { +public: + void update(void); + +private: + // barometer delay buffer variables + struct readings_baro { + uint32_t time; + float data; + }; + uint8_t _store_index; + uint32_t _last_store_time; + static const uint8_t _buffer_length = 50; + VectorN _buffer; + float _last_altitude; + void update_baro(void); + + void update_airspeed(void); + void update_compass(void); + void update_rangefinder(void); + void _setup_eliptical_correcion(uint8_t i); + uint64_t _baro_last_update_us; + uint64_t _airspeed_last_update_us; + uint64_t _compass_last_update_us; + uint64_t _rangefinder_last_update_us; + Matrix3f _eliptical_corr; + Vector3f _last_dia; + Vector3f _last_odi; +}; + +} +#endif // AP_TEST_DRONECAN_DRIVERS \ No newline at end of file diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index 0d37e7529adad..5c32227d5d459 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/libraries/SITL/SIM_JSON.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #define UDP_TIMEOUT_MS 100 @@ -100,20 +101,34 @@ void JSON::set_interface_ports(const char* address, const int port_in, const int */ void JSON::output_servos(const struct sitl_input &input) { - servo_packet pkt; - pkt.frame_rate = rate_hz; - pkt.frame_count = frame_counter; - for (uint8_t i=0; i<16; i++) { - pkt.pwm[i] = input.servos[i]; + size_t pkt_size = 0; + ssize_t send_ret = -1; + if (SRV_Channels::have_32_channels()) { + servo_packet_32 pkt; + pkt.frame_rate = rate_hz; + pkt.frame_count = frame_counter; + for (uint8_t i=0; i<32; i++) { + pkt.pwm[i] = input.servos[i]; + } + pkt_size = sizeof(pkt); + send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port); + } else { + servo_packet_16 pkt; + pkt.frame_rate = rate_hz; + pkt.frame_count = frame_counter; + for (uint8_t i=0; i<16; i++) { + pkt.pwm[i] = input.servos[i]; + } + pkt_size = sizeof(pkt); + send_ret = sock.sendto(&pkt, pkt_size, target_ip, control_port); } - size_t send_ret = sock.sendto(&pkt, sizeof(pkt), target_ip, control_port); - if (send_ret != sizeof(pkt)) { + if ((size_t)send_ret != pkt_size) { if (send_ret <= 0) { printf("Unable to send servo output to %s:%u - Error: %s, Return value: %ld\n", target_ip, control_port, strerror(errno), (long)send_ret); } else { - printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)sizeof(pkt)); + printf("Sent %ld bytes instead of %lu bytes\n", (long)send_ret, (unsigned long)pkt_size); } } } diff --git a/libraries/SITL/SIM_JSON.h b/libraries/SITL/SIM_JSON.h index 3f1aeab5e7a80..22582f0ae4808 100644 --- a/libraries/SITL/SIM_JSON.h +++ b/libraries/SITL/SIM_JSON.h @@ -44,13 +44,20 @@ class JSON : public Aircraft { private: - struct servo_packet { + struct servo_packet_16 { uint16_t magic = 18458; // constant magic value uint16_t frame_rate; uint32_t frame_count; uint16_t pwm[16]; }; + struct servo_packet_32 { + uint16_t magic = 29569; // constant magic value + uint16_t frame_rate; + uint32_t frame_count; + uint16_t pwm[32]; + }; + // default connection_info_.ip_address const char *target_ip = "127.0.0.1"; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 3b273a25931d2..1596f9f929017 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -26,6 +26,7 @@ #include "SIM_IntelligentEnergy24.h" #include "SIM_Ship.h" #include "SIM_GPS.h" +#include "SIM_DroneCANDevice.h" namespace SITL { @@ -448,6 +449,9 @@ class SIM { RichenPower richenpower_sim; IntelligentEnergy24 ie24_sim; FETtecOneWireESC fetteconewireesc_sim; +#if AP_TEST_DRONECAN_DRIVERS + DroneCANDevice dronecan_sim; +#endif // ESC telemetry AP_Int8 esc_telem; diff --git a/libraries/SITL/examples/JSON/readme.md b/libraries/SITL/examples/JSON/readme.md index 15e66fb25dc18..47b4a85674c53 100644 --- a/libraries/SITL/examples/JSON/readme.md +++ b/libraries/SITL/examples/JSON/readme.md @@ -14,13 +14,26 @@ Data is output from SITL in a binary format: uint16 pwm[16] ``` -The magic value is a constant of 18458, this is used to confirm the packet is from ArduPilot, in the future this may also be used for protocol versioning. +The magic value is a constant of 18458, this is used to confirm the packet is from ArduPilot and is used for protocol versioning. + +The number of output channels may be increased to 32 by setting the parameter +SERVO_32_ENABLE = 1. The SITL output packet is then + +``` + uint16 magic = 29569 + uint16 frame_rate + uint32 frame_count + uint16 pwm[32] +``` + +and uses a magic value 29569. + The frame rate represents the time step the simulation should take, this can be changed with the SIM_RATE_HZ ArduPilot parameter. The physics backend is free to ignore this value, a maximum time step size would typically be set. The SIM_RATE_HZ should value be kept above the vehicle loop rate, by default this 400hz on copter and quadplanes and 50 hz on plane and rover. The frame_count will increment for each output frame sent by ArduPilot, this count can be used to detect lost or duplicate frames. This count will be reset when SITL is re-started allowing the physics backend to reset the vehicle. If not input data is received after 10 seconds ArduPilot will re-send the output frame without incrementing the counter. This allows the physics model to be restarted and re-connect. Note that this may fill up the input buffer of the physics backend after some time. -PWM is a array of 16 servo values in micro seconds, typically in the 1000 to 2000 range as set by the servo output functions. +PWM is a array of 16 (or 32) servo values in micro seconds, typically in the 1000 to 2000 range as set by the servo output functions. SITL input Data is received from the physics backend in a plain text JSON format. The data must contain the following fields: diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index 105eb21c2f72a..a3246a58a9038 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/libraries/SRV_Channel/SRV_Channel.cpp @@ -64,10 +64,98 @@ const AP_Param::GroupInfo SRV_Channel::var_info[] = { // @Param: FUNCTION // @DisplayName: Servo output function // @Description: Function assigned to this servo. Setting this to Disabled(0) will setup this output for control by auto missions or MAVLink servo set commands. any other value will enable the corresponding function - // @Values: -1:GPIO,0:Disabled,1:RCPassThru,2:Flap,3:FlapAuto,4:Aileron,6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract,10:CameraTrigger,12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract,16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1,19:Elevator,21:Rudder,22:SprayerPump,23:SprayerSpinner,24:FlaperonLeft,25:FlaperonRight,26:GroundSteering,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,69:Starter,70:Throttle,71:TrackerYaw,72:TrackerPitch,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,77:ElevonLeft,78:ElevonRight,79:VTailLeft,80:VTailRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12,86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2,88:Winch,89:Main Sail,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,128:WingSailElevator,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,137:SailMastRotation,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled - // @Values{Plane}: -1:GPIO,0:Disabled,1:RCPassThru,2:Flap,3:FlapAuto,4:Aileron,6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract,10:CameraTrigger,12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract,16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1,19:Elevator,21:Rudder,22:SprayerPump,23:SprayerSpinner,24:FlaperonLeft,25:FlaperonRight,26:GroundSteering,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7/TailTiltServo,40:Motor8,41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,67:Ignition,69:Starter,70:Throttle,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,77:ElevonLeft,78:ElevonRight,79:VTailLeft,80:VTailRight,82:Motor9,83:Motor10,84:Motor11,85:Motor12,86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,110:Airbrakes,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled - // @Values{Copter}: -1:GPIO,0:Disabled,1:RCPassThru,6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract,10:CameraTrigger,12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract,22:SprayerPump,23:SprayerSpinner,27:Parachute,28:Gripper,29:LandingGear,30:EngineRunEnable,31:HeliRSC,32:HeliTailRSC,33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,73:ThrottleLeft,74:ThrottleRight,75:TiltMotorFrontLeft,76:TiltMotorFrontRight,81:BoostThrottle,82:Motor9,83:Motor10,84:Motor11,85:Motor12,88:Winch,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled - // @Values{Rover}: -1:GPIO,0:Disabled,1:RCPassThru,6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract,10:CameraTrigger,12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract,22:SprayerPump,23:SprayerSpinner,26:GroundSteering,28:Gripper,33:Motor1,34:Motor2,35:Motor3,36:Motor4,51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16,70:Throttle,73:ThrottleLeft,74:ThrottleRight,88:Winch,89:Main Sail,90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed,94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16,120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4,128:WingSailElevator,129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock,133:Winch Clutch,134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX,137:SailMastRotation,138:Alarm,139:Alarm Inverted,140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled + // @Values: -1:GPIO + // @Values{Plane, Copter, Rover}: -1:GPIO + // @Values: 0:Disabled + // @Values{Plane, Copter, Rover}: 0:Disabled + // @Values: 1:RCPassThru + // @Values{Plane, Copter, Rover}: 1:RCPassThru + // @Values: 2:Flap, 3:FlapAuto + // @Values{Plane}: 2:Flap,3:FlapAuto + // @Values: 4:Aileron + // @Values{Plane}: 4:Aileron + // @Values: 6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract + // @Values{Plane, Copter, Rover}: 6:Mount1Yaw,7:Mount1Pitch,8:Mount1Roll,9:Mount1Retract + // @Values: 10:CameraTrigger + // @Values{Plane, Copter, Rover}: 10:CameraTrigger + // @Values: 12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract + // @Values{Plane, Copter, Rover}: 12:Mount2Yaw,13:Mount2Pitch,14:Mount2Roll,15:Mount2Retract + // @Values: 16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1 + // @Values{Plane}: 16:DifferentialSpoilerLeft1,17:DifferentialSpoilerRight1 + // @Values: 19:Elevator + // @Values{Plane}: 19:Elevator + // @Values: 21:Rudder + // @Values{Plane}: 21:Rudder + // @Values: 22:SprayerPump,23:SprayerSpinner + // @Values{Plane, Copter, Rover}: 22:SprayerPump,23:SprayerSpinner + // @Values: 24:FlaperonLeft,25:FlaperonRight + // @Values{Plane}: 24:FlaperonLeft,25:FlaperonRight + // @Values: 26:GroundSteering + // @Values{Plane, Rover}: 26:GroundSteering + // @Values: 27:Parachute + // @Values{Plane, Copter}: 27:Parachute + // @Values: 28:Gripper + // @Values{Plane, Copter, Rover}: 28:Gripper + // @Values: 29:LandingGear + // @Values{Plane, Copter}: 29:LandingGear + // @Values: 30:EngineRunEnable + // @Values{Plane, Copter}: 30:EngineRunEnable + // @Values: 31:HeliRSC,32:HeliTailRSC + // @Values{Copter}: 31:HeliRSC,32:HeliTailRSC + // @Values: 33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8 + // @Values{Plane}: 33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7/TailTiltServo,40:Motor8 + // @Values{Copter}: 33:Motor1,34:Motor2,35:Motor3,36:Motor4,37:Motor5,38:Motor6,39:Motor7,40:Motor8 + // @Values{Rover}: 33:Motor1,34:Motor2,35:Motor3,36:Motor4 + // @Values: 41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight + // @Values{Plane}: 41:TiltMotorsFront,45:TiltMotorsRear,46:TiltMotorRearLeft,47:TiltMotorRearRight + // @Values: 51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16 + // @Values{Plane, Copter, Rover}: 51:RCIN1,52:RCIN2,53:RCIN3,54:RCIN4,55:RCIN5,56:RCIN6,57:RCIN7,58:RCIN8,59:RCIN9,60:RCIN10,61:RCIN11,62:RCIN12,63:RCIN13,64:RCIN14,65:RCIN15,66:RCIN16 + // @Values: 67:Ignition,69:Starter + // @Values{Plane}: 67:Ignition,69:Starter + // @Values: 70:Throttle + // @Values{Plane, Rover}: 70:Throttle + // @Values: 71:TrackerYaw,72:TrackerPitch + // @Values: 73:ThrottleLeft,74:ThrottleRight + // @Values{Plane, Copter, Rover}: 73:ThrottleLeft,74:ThrottleRight + // @Values: 75:TiltMotorFrontLeft,76:TiltMotorFrontRight + // @Values{Plane, Copter}: 75:TiltMotorFrontLeft,76:TiltMotorFrontRight + // @Values: 77:ElevonLeft,78:ElevonRight + // @Values{Plane}: 77:ElevonLeft,78:ElevonRight + // @Values: 79:VTailLeft,80:VTailRight + // @Values{Plane}: 79:VTailLeft,80:VTailRight + // @Values: 81:BoostThrottle + // @Values{Copter}: 81:BoostThrottle + // @Values: 82:Motor9,83:Motor10,84:Motor11,85:Motor12 + // @Values{Plane, Copter}: 82:Motor9,83:Motor10,84:Motor11,85:Motor12 + // @Values: 86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2 + // @Values{Plane}: 86:DifferentialSpoilerLeft2,87:DifferentialSpoilerRight2 + // @Values: 88:Winch + // @Values{Copter, Rover}: 88:Winch + // @Values: 89:Main Sail + // @Values{Rover}: 89:Main Sail + // @Values: 90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed + // @Values{Plane, Copter, Rover}: 90:CameraISO,91:CameraAperture,92:CameraFocus,93:CameraShutterSpeed + // @Values: 94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16 + // @Values{Plane, Copter, Rover}: 94:Script1,95:Script2,96:Script3,97:Script4,98:Script5,99:Script6,100:Script7,101:Script8,102:Script9,103:Script10,104:Script11,105:Script12,106:Script13,107:Script14,108:Script15,109:Script16 + // @Values{Plane}: 110:Airbrakes + // @Values: 120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4 + // @Values{Plane, Copter, Rover}: 120:NeoPixel1,121:NeoPixel2,122:NeoPixel3,123:NeoPixel4 + // @Values: 124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw + // @Values{Plane, Copter}: 124:RateRoll,125:RatePitch,126:RateThrust,127:RateYaw + // @Values: 128:WingSailElevator + // @Values{Rover}: 128:WingSailElevator + // @Values: 129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock + // @Values{Plane, Copter, Rover}: 129:ProfiLED1,130:ProfiLED2,131:ProfiLED3,132:ProfiLEDClock + // @Values: 133:Winch Clutch + // @Values{Copter, Rover}: 133:Winch Clutch + // @Values: 134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX + // @Values{Plane, Copter, Rover}: 134:SERVOn_MIN,135:SERVOn_TRIM,136:SERVOn_MAX + // @Values: 137:SailMastRotation + // @Values{Rover}: 137:SailMastRotation + // @Values: 138:Alarm,139:Alarm Inverted + // @Values{Plane, Copter, Rover}: 138:Alarm,139:Alarm Inverted + // @Values: 140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled + // @Values{Plane, Copter, Rover}: 140:RCIN1Scaled,141:RCIN2Scaled,142:RCIN3Scaled,143:RCIN4Scaled,144:RCIN5Scaled,145:RCIN6Scaled,146:RCIN7Scaled,147:RCIN8Scaled,148:RCIN9Scaled,149:RCIN10Scaled,150:RCIN11Scaled,151:RCIN12Scaled,152:RCIN13Scaled,153:RCIN14Scaled,154:RCIN15Scaled,155:RCIN16Scaled // @User: Standard // @RebootRequired: True AP_GROUPINFO("FUNCTION", 5, SRV_Channel, function, 0), diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 6e0a1edd8759f..a930a37c8dfab 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -481,7 +481,7 @@ class SRV_Channels { static bool find_channel(SRV_Channel::Aux_servo_function_t function, uint8_t &chan); // find first channel that a function is assigned to, returning SRV_Channel object - static SRV_Channel *get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan=-1); + static SRV_Channel *get_channel_for(SRV_Channel::Aux_servo_function_t function); // call set_angle() on matching channels static void set_angle(SRV_Channel::Aux_servo_function_t function, uint16_t angle); @@ -573,6 +573,15 @@ class SRV_Channels { return channel_function(channel) == SRV_Channel::k_alarm_inverted; } + // return true if 32 channels are enabled + static bool have_32_channels() { +#if NUM_SERVO_CHANNELS >= 17 + return _singleton->enable_32_channels.get() > 0; +#else + return false; +#endif + } + private: static bool disabled_passthrough; diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index 1416e3a190542..bed75a2363988 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -559,12 +559,9 @@ bool SRV_Channels::find_channel(SRV_Channel::Aux_servo_function_t function, uint /* get a pointer to first auxiliary channel for a channel function */ -SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t function, int8_t default_chan) +SRV_Channel *SRV_Channels::get_channel_for(SRV_Channel::Aux_servo_function_t function) { uint8_t chan; - if (default_chan >= 0) { - set_aux_channel_default(function, default_chan); - } if (!find_channel(function, chan)) { return nullptr; } diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 5f0d47fc90bd6..8279740e2fe8f 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/libraries/SRV_Channel/SRV_Channels.cpp @@ -27,7 +27,7 @@ #if HAL_MAX_CAN_PROTOCOL_DRIVERS #include - #include + #include // To be replaced with macro saying if KDECAN library is included #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub) @@ -534,17 +534,17 @@ void SRV_Channels::push() fetteconwire_ptr->update(); #endif -#if HAL_CANMANAGER_ENABLED +#if HAL_ENABLE_DRONECAN_DRIVERS // push outputs to CAN uint8_t can_num_drivers = AP::can().get_num_drivers(); for (uint8_t i = 0; i < can_num_drivers; i++) { switch (AP::can().get_driver_type(i)) { - case AP_CANManager::Driver_Type_UAVCAN: { - AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i); - if (ap_uavcan == nullptr) { + case AP_CANManager::Driver_Type_DroneCAN: { + AP_DroneCAN *ap_dronecan = AP_DroneCAN::get_dronecan(i); + if (ap_dronecan == nullptr) { continue; } - ap_uavcan->SRV_push_servos(); + ap_dronecan->SRV_push_servos(); break; } case AP_CANManager::Driver_Type_KDECAN: { diff --git a/modules/ChibiOS b/modules/ChibiOS index 1ec9f168b2e64..75de0f2e5db9e 160000 --- a/modules/ChibiOS +++ b/modules/ChibiOS @@ -1 +1 @@ -Subproject commit 1ec9f168b2e64a043470a87225b405e79d187354 +Subproject commit 75de0f2e5db9e2078e4a5612e6838c4dbcdea867 diff --git a/modules/DroneCAN/dronecan_dsdlc b/modules/DroneCAN/dronecan_dsdlc index 01586259d5a8b..ebaf96860a11a 160000 --- a/modules/DroneCAN/dronecan_dsdlc +++ b/modules/DroneCAN/dronecan_dsdlc @@ -1 +1 @@ -Subproject commit 01586259d5a8b11f9c4a034a976f886630ae4fb7 +Subproject commit ebaf96860a11a4cc43c01df6b651df143c6cde2d diff --git a/modules/DroneCAN/libcanard b/modules/DroneCAN/libcanard index aec8028798e84..603f62ecf5ea3 160000 --- a/modules/DroneCAN/libcanard +++ b/modules/DroneCAN/libcanard @@ -1 +1 @@ -Subproject commit aec8028798e8485851644e85093c50b1bd6e8ade +Subproject commit 603f62ecf5ea39d1fc4278df7746e7f802162acb diff --git a/modules/uavcan b/modules/uavcan deleted file mode 160000 index f845e02dcd2e9..0000000000000 --- a/modules/uavcan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f845e02dcd2e97f6c4743c96f3b2380c4b03ab5f diff --git a/wscript b/wscript index 8c4e40d2211a8..a9101b821ecac 100644 --- a/wscript +++ b/wscript @@ -13,6 +13,7 @@ sys.path.insert(0, 'Tools/ardupilotwaf/') import ardupilotwaf import boards +import shutil from waflib import Build, ConfigSet, Configure, Context, Utils from waflib.Configure import conf @@ -255,6 +256,9 @@ submodules at specific revisions. help="Enable the dds client to connect with ROS2/DDS" ) + g.add_option('--enable-dronecan-tests', action='store_true', + default=False, + help="Enables DroneCAN tests in sitl") g = opt.ap_groups['linux'] linux_options = ('--prefix', '--destdir', '--bindir', '--libdir') @@ -324,9 +328,9 @@ configuration in order to save typing. default=False, help="Enable SITL RGBLed") - g.add_option('--sitl-32bit', action='store_true', + g.add_option('--force-32bit', action='store_true', default=False, - help="Enable SITL 32bit") + help="Force 32bit build") g.add_option('--build-dates', action='store_true', default=False, @@ -436,7 +440,7 @@ def configure(cfg): cfg.env.BOARD = cfg.options.board cfg.env.DEBUG = cfg.options.debug cfg.env.COVERAGE = cfg.options.coverage - cfg.env.SITL32BIT = cfg.options.sitl_32bit + cfg.env.FORCE32BIT = cfg.options.force_32bit cfg.env.ENABLE_ASSERTS = cfg.options.enable_asserts cfg.env.BOOTLOADER = cfg.options.bootloader cfg.env.ENABLE_MALLOC_GUARD = cfg.options.enable_malloc_guard @@ -474,10 +478,7 @@ def configure(cfg): cfg.load('clang_compilation_database') cfg.load('waf_unit_test') cfg.load('mavgen') - if cfg.options.board in cfg.ap_periph_boards(): - cfg.load('dronecangen') - else: - cfg.load('uavcangen') + cfg.load('dronecangen') cfg.env.SUBMODULE_UPDATE = cfg.options.submodule_update @@ -536,8 +537,8 @@ def configure(cfg): else: cfg.end_msg('disabled', color='YELLOW') - cfg.start_msg('SITL 32-bit build') - if cfg.env.SITL32BIT: + cfg.start_msg('Force 32-bit build') + if cfg.env.FORCE32BIT: cfg.end_msg('enabled') else: cfg.end_msg('disabled', color='YELLOW') @@ -665,19 +666,20 @@ def _build_dynamic_sources(bld): if (bld.get_board().with_can or bld.env.HAL_NUM_CAN_IFACES) and not bld.env.AP_PERIPH: bld( - features='uavcangen', - source=bld.srcnode.ant_glob('modules/DroneCAN/DSDL/* libraries/AP_UAVCAN/dsdl/*', dir=True, src=False), - output_dir='modules/uavcan/libuavcan/include/dsdlc_generated', - name='uavcan', + features='dronecangen', + source=bld.srcnode.ant_glob('modules/DroneCAN/DSDL/* libraries/AP_DroneCAN/dsdl/*', dir=True, src=False), + output_dir='modules/DroneCAN/libcanard/dsdlc_generated/', + name='dronecan', export_includes=[ - bld.bldnode.make_node('modules/uavcan/libuavcan/include/dsdlc_generated').abspath(), - bld.srcnode.find_dir('modules/uavcan/libuavcan/include').abspath() - ] - ) + bld.bldnode.make_node('modules/DroneCAN/libcanard/dsdlc_generated/include').abspath(), + bld.srcnode.find_dir('modules/DroneCAN/libcanard/').abspath(), + bld.srcnode.find_dir('libraries/AP_DroneCAN/canard/').abspath(), + ] + ) elif bld.env.AP_PERIPH: bld( features='dronecangen', - source=bld.srcnode.ant_glob('modules/DroneCAN/DSDL/* libraries/AP_UAVCAN/dsdl/*', dir=True, src=False), + source=bld.srcnode.ant_glob('modules/DroneCAN/DSDL/* libraries/AP_DroneCAN/dsdl/*', dir=True, src=False), output_dir='modules/DroneCAN/libcanard/dsdlc_generated/', name='dronecan', export_includes=[ @@ -812,8 +814,6 @@ def build(bld): _load_pre_build(bld) if bld.get_board().with_can: - bld.env.AP_LIBRARIES_OBJECTS_KW['use'] += ['uavcan'] - if bld.env.AP_PERIPH: bld.env.AP_LIBRARIES_OBJECTS_KW['use'] += ['dronecan'] _build_cmd_tweaks(bld)