diff --git a/.travis.yml b/.travis.yml index eb1d9e0b72..34bc45c9f5 100644 --- a/.travis.yml +++ b/.travis.yml @@ -32,10 +32,9 @@ cache: before_install: - echo 0 | sudo dd of=/proc/sys/kernel/yama/ptrace_scope - Tools/scripts/configure-ci.sh - - if [ "$TRAVIS_EVENT_TYPE" = "cron" ]; then export CI_CRON_JOB=1 ; fi script: - - Tools/scripts/build_ci.sh + - Tools/scripts/mttr-build-ci.sh notifications: webhooks: @@ -45,6 +44,9 @@ notifications: on_failure: always # options: [always|never|change] default: always on_start: false # default: false +before_cache: + - ccache -z + compiler: - gcc @@ -58,30 +60,13 @@ env: matrix: fast_finish: true include: - - if: type != cron - compiler: "gcc" - env: CI_BUILD_TARGET="revo-bootloader periph-build CubeOrange-bootloader iofirmware stm32f7 stm32h7 fmuv2-plane" - - if: type != cron - compiler: "gcc" - env: CI_BUILD_TARGET="sitltest-copter" - - if: type != cron - compiler: "gcc" - env: CI_BUILD_TARGET="sitltest-quadplane sitltest-plane" - - if: type != cron - compiler: "clang-7" - env: CI_BUILD_TARGET="sitltest-rover sitltest-sub sitltest-balancebot" - - if: type != cron - compiler: "gcc" - env: CI_BUILD_TARGET="unit-tests" - - if: type != cron - compiler: "clang-7" - env: CI_BUILD_TARGET="sitl" - - language: python - python: 3.7 - addons: # speedup: This test does not need addons - compiler: - dist: xenial # required for Python >= 3.7 (travis-ci/travis-ci#9069) - before_install: pip install flake8 - script: - - EXCLUDE=./.*,./modules/gtest,./modules/ChibiOS/test,./modules/uavcan/libuavcan,./modules/libcanard - - flake8 . --count --exclude=$EXCLUDE --select=E901,E999,F821,F822,F823 --show-source --statistics + - compiler: "gcc" + +deploy: + provider: releases + api_key: "$GITHUB_DEPLOY_API_KEY" + file_glob: true + file: "$HOME/deploy_files/*" + on: + tags: true +>>>>>>> build: switched to MttrCubeBlack for travis testing diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index c28a027b9b..6554e098a5 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -3,6 +3,12 @@ // If you used to define your CONFIG_APM_HARDWARE setting here, it is no longer // valid! You should switch to using a HAL_BOARD flag in your local config.mk. +#define ARMING_DELAY_SEC 3.0f + +#define RANGEFINDER_WPNAV_FILT_HZ 3.0f + +#define LAND_DETECTOR_TRIGGER_SEC 0.5f + // uncomment the lines below to disable features (flash sizes listed are for APM2 boards and will underestimate savings on Pixhawk and other boards) //#define LOGGING_ENABLED DISABLED // disable logging to save 11K of flash space //#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 1a039a2bf8..d110039fdb 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -13,6 +13,8 @@ void AP_Arming_Copter::update(void) } pre_arm_checks(display_fail); + + copter.update_armed_pin(); } bool AP_Arming_Copter::pre_arm_checks(bool display_failure) @@ -153,7 +155,7 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // failsafe parameter checks if (copter.g.failsafe_throttle) { // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 - if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10 || copter.g.failsafe_throttle_value < 910) { + if (copter.channel_throttle->get_radio_min() <= copter.g.failsafe_throttle_value+10) { check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Check FS_THR_VALUE"); return false; } @@ -284,6 +286,9 @@ bool AP_Arming_Copter::motor_checks(bool display_failure) bool AP_Arming_Copter::pilot_throttle_checks(bool display_failure) { + if (rc_check_disabled) { + return true; + } // check throttle is above failsafe throttle // this is near the bottom to allow other failures to be displayed before checking pilot throttle if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { @@ -379,6 +384,27 @@ bool AP_Arming_Copter::gps_checks(bool display_failure) return false; } + float hacc, vacc; + if (!copter.gps.horizontal_accuracy(hacc) || + !copter.gps.vertical_accuracy(vacc)) { + if (display_failure) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: No GPS accuracy"); + } + return false; + } + if (hacc > copter.matternet.arm_gps_hacc) { + if (display_failure) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS hacc %.2f", hacc); + } + return false; + } + if (vacc > copter.matternet.arm_gps_vacc) { + if (display_failure) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: GPS vacc %.2f", vacc); + } + return false; + } + // if we got here all must be ok AP_Notify::flags.pre_arm_gps_check = true; return true; @@ -521,6 +547,10 @@ bool AP_Arming_Copter::mandatory_gps_checks(bool display_failure) check_failed(display_failure, "EKF-home variance"); return false; } + + if (!compass_checks(display_failure)) { + return false; + } // if we got here all must be ok return true; @@ -561,6 +591,13 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) } #endif + if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_FTS)) { + if (copter.parachute.enabled() && !copter.parachute.get_mttr_prearm_pass()) { + check_failed(true, "FTS state"); + return false; + } + } + Mode::Number control_mode = copter.control_mode; // always check if the current mode allows arming @@ -615,7 +652,7 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method) #endif // check throttle - if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { + if (((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) && !rc_check_disabled) { #if FRAME_CONFIG == HELI_FRAME const char *rc_item = "Collective"; #else @@ -690,6 +727,7 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ if (!AP_Arming::arm(method, do_arming_checks)) { AP_Notify::events.arming_failed = true; in_arm_motors = false; + AP::logger().arming_failure(); return false; } @@ -756,6 +794,8 @@ bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_ // finally actually arm the motors copter.motors->armed(true); + copter.update_armed_pin(); + AP::logger().Write_Event(DATA_ARMED); // log flight mode in case it was changed while vehicle was disarmed @@ -829,6 +869,7 @@ bool AP_Arming_Copter::disarm() // send disarm command to motors copter.motors->armed(false); + copter.update_armed_pin(); #if MODE_AUTO_ENABLED == ENABLED // reset the mission @@ -845,3 +886,30 @@ bool AP_Arming_Copter::disarm() return true; } + + +/* + checks run before takeoff to check that GPS movement is within + acceptable range + */ +bool AP_Arming_Copter::pre_takeoff_checks(void) +{ + if (!hal.util->get_soft_armed()) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"Takeoff: not armed"); + return false; + } + float pos_change, alt_change; + if (!copter.gps.get_pre_arm_pos_change(pos_change, alt_change)) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"Takeoff: no GPS data"); + return false; + } + if (pos_change > copter.matternet.tkoff_gps_pos_change) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"Takeoff: pos change %.2f", pos_change); + return false; + } + if (fabsf(alt_change) > copter.matternet.tkoff_gps_alt_change) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"Takeoff: alt change %.2f", alt_change); + return false; + } + return true; +} diff --git a/ArduCopter/AP_Arming.h b/ArduCopter/AP_Arming.h index 69962530d3..c437e02de9 100644 --- a/ArduCopter/AP_Arming.h +++ b/ArduCopter/AP_Arming.h @@ -26,6 +26,9 @@ class AP_Arming_Copter : public AP_Arming bool disarm() override; bool arm(AP_Arming::Method method, bool do_arming_checks=true) override; + // checks run before auto-takeoff on MISSION_START allowed + bool pre_takeoff_checks(void); + protected: bool pre_arm_checks(bool display_failure) override; diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 1ec6fd815e..568dc522c5 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -10,6 +10,10 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle) } float yaw_request; + if (control_mode == Mode::Number::AUTO) { + return 0; + } + // calculate yaw rate request if (g2.acro_y_expo <= 0) { yaw_request = stick_angle * g.acro_yaw_p; diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 6d8dd0032f..d6b6ecd17f 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -130,9 +130,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if AC_FENCE == ENABLED SCHED_TASK_CLASS(AC_Fence, &copter.fence, update, 10, 100), #endif -#if PRECISION_LANDING == ENABLED - SCHED_TASK(update_precland, 400, 50), -#endif #if FRAME_CONFIG == HELI_FRAME SCHED_TASK(check_dynamic_flight, 50, 75), #endif @@ -209,7 +206,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #endif }; -constexpr int8_t Copter::_failsafe_priorities[7]; +constexpr int8_t Copter::_failsafe_priorities[8]; void Copter::setup() { @@ -248,6 +245,10 @@ void Copter::fast_loop() // -------------------- read_AHRS(); +#if PRECISION_LANDING == ENABLED + update_precland(); +#endif + #if FRAME_CONFIG == HELI_FRAME update_heli_control_dynamics(); #if MODE_AUTOROTATE_ENABLED == ENABLED diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index fa41849be8..1dda868f94 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -198,6 +198,7 @@ class Copter : public AP_Vehicle { friend class AP_Rally_Copter; friend class Parameters; friend class ParametersG2; + friend class ParametersMTTR; friend class AP_Avoidance_Copter; #if ADVANCED_FAILSAFE == ENABLED @@ -252,6 +253,9 @@ class Copter : public AP_Vehicle { // Global parameters are all contained within the 'g' class. Parameters g; ParametersG2 g2; + ParametersMTTR matternet; + + void update_armed_pin(); // main loop scheduler AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&Copter::fast_loop, void)}; @@ -275,6 +279,7 @@ class Copter : public AP_Vehicle { bool enabled:1; bool alt_healthy:1; // true if we can trust the altitude from the rangefinder int16_t alt_cm; // tilt compensated altitude (in cm) from rangefinder + float inertial_alt_cm; // inertial alt at time of last rangefinder sample uint32_t last_healthy_ms; LowPassFilterFloat alt_cm_filt; // altitude filter int16_t alt_cm_glitch_protected; // last glitch protected altitude @@ -282,6 +287,11 @@ class Copter : public AP_Vehicle { uint32_t glitch_cleared_ms; // system time glitch cleared } rangefinder_state, rangefinder_up_state; + /* + return rangefinder height interpolated using inertial altitude + */ + bool get_rangefinder_height_interpolated_cm(int32_t& ret); + class SurfaceTracking { public: // get desired climb rate (in cm/s) to achieve surface tracking @@ -306,7 +316,7 @@ class Copter : public AP_Vehicle { void set_surface(Surface new_surface); private: - Surface surface = Surface::GROUND; + Surface surface = Surface::NONE; float target_dist_cm; // desired distance in cm from ground or ceiling uint32_t last_update_ms; // system time of last update to target_alt_cm uint32_t last_glitch_cleared_ms; // system time of last handle glitch recovery @@ -400,6 +410,11 @@ class Copter : public AP_Vehicle { Mode::Number prev_control_mode; ModeReason prev_control_mode_reason = ModeReason::UNKNOWN; + // minimum recorded barometric alt during takeoff. Used to detect + // the "dip" in barometric alt that happens on takeoff + float takeoff_baro_min_alt_m; + bool takeoff_liftoff_complete; + RCMapper rcmap; // intertial nav alt when we armed @@ -421,7 +436,11 @@ class Copter : public AP_Vehicle { } failsafe; bool any_failsafe_triggered() const { - return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb; + bool ret = battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb; + if (control_mode != Mode::Number::GUIDED && control_mode != Mode::Number::AUTO && control_mode != Mode::Number::BRAKE) { + ret |= failsafe.radio; + } + return ret; } // sensor health for logging @@ -590,6 +609,13 @@ class Copter : public AP_Vehicle { int16_t hover_roll_trim_scalar_slew; #endif + struct { + bool angle_error_excessive; + bool vel_z_error_excessive; + uint32_t angle_error_excessive_begin_ms; + uint32_t vel_z_error_excessive_begin_ms; + } parachute_check_state; + // ground effect detector struct { bool takeoff_expected; @@ -622,7 +648,8 @@ class Copter : public AP_Vehicle { Failsafe_Action_RTL = 2, Failsafe_Action_SmartRTL = 3, Failsafe_Action_SmartRTL_Land = 4, - Failsafe_Action_Terminate = 5 + Failsafe_Action_Terminate = 5, + Failsafe_Action_LandIfManual = 6 }; enum class FailsafeOption { @@ -636,6 +663,7 @@ class Copter : public AP_Vehicle { static constexpr int8_t _failsafe_priorities[] = { Failsafe_Action_Terminate, Failsafe_Action_Land, + Failsafe_Action_LandIfManual, Failsafe_Action_RTL, Failsafe_Action_SmartRTL_Land, Failsafe_Action_SmartRTL, diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 75eeda4ab8..3d28280d8f 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -166,4 +166,14 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) } } #endif + + // give GCS status of prearm checks. This is enabled if any arming checks are enabled. + // it is healthy if armed or checks are passing + control_sensors_present |= MAV_SYS_STATUS_PREARM_CHECK; + if (copter.arming.get_enabled_checks()) { + control_sensors_enabled |= MAV_SYS_STATUS_PREARM_CHECK; + if (hal.util->get_soft_armed() || AP_Notify::flags.pre_arm_check) { + control_sensors_health |= MAV_SYS_STATUS_PREARM_CHECK; + } + } } diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 0e1f3daa33..3b6b9a71b7 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -265,6 +265,15 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) #endif break; + case MSG_LANDING_TARGET: +#if PRECISION_LANDING == ENABLED + CHECK_PAYLOAD_SIZE(LANDING_TARGET); + if (copter.precland.enabled()) { + copter.precland.send_landing_target(chan); + } +#endif + break; + case MSG_WIND: case MSG_SERVO_OUT: case MSG_AOA_SSA: @@ -451,6 +460,7 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_VIBRATION, MSG_RPM, MSG_ESC_TELEMETRY, + MSG_LANDING_TARGET, }; static const ap_message STREAM_PARAMS_msgs[] = { MSG_NEXT_PARAM @@ -703,6 +713,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ #if MODE_AUTO_ENABLED == ENABLED case MAV_CMD_MISSION_START: + // when we are not yet flying we run additional + // pre-takeoff checks for GPS movement + if (copter.motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED && + !copter.arming.pre_takeoff_checks()) { + return MAV_RESULT_FAILED; + } if (copter.motors->armed() && copter.set_mode(Mode::Number::AUTO, ModeReason::GCS_COMMAND)) { copter.set_auto_armed(true); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 0e331dadb1..7ba18105cd 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -717,6 +717,10 @@ const AP_Param::Info Copter::var_info[] = { // @Path: Parameters.cpp GOBJECT(g2, "", ParametersG2), + // @Group: + // @Path: Parameters.cpp + GOBJECT(matternet, "", ParametersMTTR), + AP_VAREND }; @@ -961,8 +965,14 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(arot, "AROT_", 37, ParametersG2, AC_Autorotation), #endif - - + // @Param: WP_NAVALT_MAX + // @DisplayName: Maximum navigation altitude + // @Description: This is the altitude in meters above which full navigation attitude can begin. This applies in auto takeoff + // @Units: m + // @Range: 0 5 + // @User: Standard + AP_GROUPINFO("WP_NAVALT_MAX", 38, ParametersG2, wp_navalt_max, 0), + AP_GROUPEND }; @@ -1056,6 +1066,86 @@ ParametersG2::ParametersG2(void) AP_Param::setup_object_defaults(this, var_info); } + +/* + Matternet specific parameters + */ +const AP_Param::GroupInfo ParametersMTTR::var_info[] = { + + // @Param: TOFF_POS_CHANGE + // @DisplayName: Maximum GPS pos change + // @Description: This is the maximum allowed position change since arming for automatic takeoff + // @Units: m + // @Range: 0 5 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("TOFF_POS_CHANGE", 1, ParametersMTTR, tkoff_gps_pos_change, 1.0), + + // @Param: TOFF_ALT_CHANGE + // @DisplayName: Maximum GPS alt change + // @Description: This is the maximum allowed altitude change since arming for automatic takeoff + // @Units: m + // @Range: 0 5 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("TOFF_ALT_CHANGE", 2, ParametersMTTR, tkoff_gps_alt_change, 1.0), + + // @Param: ARM_GPS_HACC + // @DisplayName: Maximum GPS HAcc on arming + // @Description: This is the maximum allowed GPS horizontal accuracy for arming + // @Units: m + // @Range: 0 5 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("ARM_GPS_HACC", 3, ParametersMTTR, arm_gps_hacc, 1.0), + + // @Param: ARM_GPS_VACC + // @DisplayName: Maximum GPS VAcc on arming + // @Description: This is the maximum allowed GPS vertical accuracy for arming + // @Units: m + // @Range: 0 5 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("ARM_GPS_VACC", 4, ParametersMTTR, arm_gps_vacc, 1.0), + + // @Param: TOFF_BARO_DIP + // @DisplayName: Barometric rise for liftoff completion + // @Description: This is amount of rise in barometer above min reading in takeoff before we consider that liftoff has completed + // @Units: m + // @Range: 0 5 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("TOFF_BARO_DIP", 5, ParametersMTTR, tkoff_baro_dip, 2.0), + + // @Param: TOFF_HOV_PCT + // @DisplayName: Percent of hover throttle for liftoff + // @Description: This is percentage of hover throttle before we consider that liftoff is complete + // @Units: % + // @Range: 0 127 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("TOFF_HOV_PCT", 6, ParametersMTTR, tkoff_hover_pct, 80), + + // @Param: ARM_PIN + // @DisplayName: Pin number for armed state + // @Description: If set to value >= 0 then this pin will go high on arming and low on disarm + // @Range: -1 127 + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO("ARMED_PIN", 7, ParametersMTTR, arm_pin, -1), + + AP_GROUPEND +}; + +/* + constructor for matternet object + */ +ParametersMTTR::ParametersMTTR(void) +{ + AP_Param::setup_object_defaults(this, var_info); +} + + /* This is a conversion table from old parameter values to new parameter names. The startup code looks for saved values of the old diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index bc0dc53c6a..9853b91e92 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -53,7 +53,7 @@ class Parameters { k_param_software_type, // deprecated k_param_ins_old, // *** Deprecated, remove with next eeprom number change k_param_ins, // libraries/AP_InertialSensor variables - k_param_NavEKF2_old, // deprecated - remove + k_param_matternet, k_param_NavEKF2, k_param_g2, // 2nd block of parameters k_param_NavEKF3, @@ -485,6 +485,9 @@ class ParametersG2 { // altitude at which nav control can start in takeoff AP_Float wp_navalt_min; + // altitude at which nav control reaches full range in takeoff + AP_Float wp_navalt_max; + #if BUTTON_ENABLED == ENABLED // button checking AP_Button button; @@ -622,4 +625,24 @@ class ParametersG2 { #endif }; +/* + Matternet specific parameters. This is done as a separate block to + reduce the change of conflict on rebase + */ +class ParametersMTTR { +public: + ParametersMTTR(void); + + // var_info for holding Parameter information + static const struct AP_Param::GroupInfo var_info[]; + + AP_Float tkoff_gps_pos_change; + AP_Float tkoff_gps_alt_change; + AP_Float arm_gps_hacc; + AP_Float arm_gps_vacc; + AP_Float tkoff_baro_dip; + AP_Int8 tkoff_hover_pct; + AP_Int8 arm_pin; +}; + extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 8795e83f45..76b0b25910 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -125,6 +125,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) init_rc_out(); enable_motor_output(); motors->armed(true); + update_armed_pin(); hal.util->set_soft_armed(true); // initialise run time @@ -231,6 +232,7 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) // stop motors motors->output_min(); motors->armed(false); + update_armed_pin(); hal.util->set_soft_armed(false); // set and save motor compensation diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 4fe7ad1c93..2628b27415 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -145,19 +145,23 @@ void Copter::thrust_loss_check() #if PARACHUTE == ENABLED // Code to detect a crash main ArduCopter code -#define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute -#define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control - -// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected -// vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second +#define PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG 30.0f +#define PARACHUTE_ANGLE_ERROR_EXCESSIVE_TIMEOUT_SEC 0.5f +#define PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_LIMIT_MPS 3.0f +#define PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_TIMEOUT_SEC 0.5f + +// parachute_check - checks whether the parachute should be deployed. There are several criteria for parachute deployment. The parachute will deploy when: +// - Attitude error is greater than PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG for PARACHUTE_ANGLE_ERROR_EXCESSIVE_TIMEOUT_SEC seconds +// - Attitude error is greater than PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG *AND* tilt angle is greater than ANGLE_MAX + PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG +// - Height control is active *AND* vertical velocity error is above PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_LIMIT_MPS *AND* abs(vertical velocity) is above PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_LIMIT_MPS *AND* all of these conditions are true for PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_TIMEOUT_SEC +// - Flight mode is STABILIZE *AND* throttle is zero *AND* downward velocity is greater than 5 m/s // called at MAIN_LOOP_RATE + void Copter::parachute_check() { - static uint16_t control_loss_count; // number of iterations we have been out of control - static int32_t baro_alt_start; - - // exit immediately if parachute is not enabled + // Return immediately if parachute is not enabled if (!parachute.enabled()) { + parachute_check_state.angle_error_excessive = false; return; } @@ -171,58 +175,66 @@ void Copter::parachute_check() // return immediately if motors are not armed or pilot's throttle is above zero if (!motors->armed()) { - control_loss_count = 0; return; } // return immediately if we are not in an angle stabilize flight mode or we are flipping if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::FLIP) { - control_loss_count = 0; return; } // ensure we are flying if (ap.land_complete) { - control_loss_count = 0; + parachute_check_state.angle_error_excessive = false; return; } - // ensure the first control_loss event is from above the min altitude - if (control_loss_count == 0 && parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100)) { - return; - } - - // check for angle error over 30 degrees + // Retrieve useful values const float angle_error = attitude_control->get_att_error_angle_deg(); - if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) { - if (control_loss_count > 0) { - control_loss_count--; - } - return; + const float tilt_angle = acosf(ahrs.get_rotation_body_to_ned().c.z); + const float tilt_angle_limit = attitude_control->get_tilt_limit_rad() + radians(PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG); + const float vel_z = -inertial_nav.get_velocity_z()*0.01f; // Convert cm/s to m/s and convert NEU to NED + const float vel_z_error = -pos_control->get_vel_error_z()*0.01f; // Convert cm/s to m/s and convert NEU to NED + const float speed_z_excessive_limit_mps = MAX(fabsf(pos_control->get_max_speed_down()), fabsf(pos_control->get_max_speed_up()))*0.01f + 1.0f; + + // Start attitude error timer + bool new_angle_error_excessive = angle_error > PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG; + if (new_angle_error_excessive && !parachute_check_state.angle_error_excessive) { + parachute_check_state.angle_error_excessive_begin_ms = millis(); } + parachute_check_state.angle_error_excessive = new_angle_error_excessive; - // increment counter - if (control_loss_count < (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) { - control_loss_count++; + // Start vertical velocity error timer + bool new_vel_z_error_excessive = pos_control->is_active_z() && fabsf(vel_z) > speed_z_excessive_limit_mps && fabsf(vel_z_error) > PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_LIMIT_MPS; + if (new_vel_z_error_excessive && !parachute_check_state.vel_z_error_excessive) { + parachute_check_state.vel_z_error_excessive_begin_ms = millis(); } + parachute_check_state.vel_z_error_excessive = new_vel_z_error_excessive; - // record baro alt if we have just started losing control - if (control_loss_count == 1) { - baro_alt_start = baro_alt; + // Check for criterion: "Attitude error is greater than PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG for PARACHUTE_ANGLE_ERROR_EXCESSIVE_TIMEOUT_SEC seconds" + bool angle_error_excessive_timeout = parachute_check_state.angle_error_excessive && (millis()-parachute_check_state.angle_error_excessive_begin_ms)*1e-3f > PARACHUTE_ANGLE_ERROR_EXCESSIVE_TIMEOUT_SEC; - // exit if baro altitude change indicates we are not falling - } else if (baro_alt >= baro_alt_start) { - control_loss_count = 0; - return; + // Check for criterion: "Attitude error is greater than PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG *AND* tilt angle is greater than ANGLE_MAX + PARACHUTE_ANGLE_ERROR_EXCESSIVE_LIMIT_DEG" + bool tilt_angle_excessive = parachute_check_state.angle_error_excessive && tilt_angle > tilt_angle_limit; + + // Check for criterion: "Height control is active *AND* vertical velocity error is above PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_LIMIT_MPS *AND* abs(vertical velocity) is above PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_LIMIT_MPS *AND* all of these conditions are true for PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_TIMEOUT_SEC" + bool vel_z_error_excessive_timeout = parachute_check_state.vel_z_error_excessive && (millis()-parachute_check_state.vel_z_error_excessive_begin_ms)*1e-3f > PARACHUTE_VERT_VEL_ERROR_EXCESSIVE_TIMEOUT_SEC; - // To-Do: add check that the vehicle is actually falling + // Check for criterion: "Flight mode is STABILIZE *AND* throttle is zero *AND* downward velocity is greater than 5 m/s" + bool stabilize_throttle_cut = control_mode == Mode::Number::STABILIZE && ap.throttle_zero && vel_z > 5.0f; - // check if loss of control for at least 1 second - } else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*scheduler.get_loop_rate_hz())) { - // reset control loss counter - control_loss_count = 0; - AP::logger().Write_Error(LogErrorSubsystem::CRASH_CHECK, LogErrorCode::CRASH_CHECK_LOSS_OF_CONTROL); - // release parachute + // Deploy the parachute if a criterion is met + if (angle_error_excessive_timeout) { + AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_ANGLE_ERROR_EXCESSIVE_TIMEOUT); + parachute_release(); + } else if (tilt_angle_excessive) { + AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_TILT_ANGLE_EXCESSIVE); + parachute_release(); + } else if (vel_z_error_excessive_timeout) { + AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_VEL_Z_ERROR_EXCESSIVE_TIMEOUT); + parachute_release(); + } else if (stabilize_throttle_cut) { + AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_STABILIZE_THROTTLE_CUT); parachute_release(); } @@ -262,12 +274,14 @@ void Copter::parachute_manual_release() } // do not release if we are landed or below the minimum altitude above home +#if 0 if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) { // warn user of reason for failure gcs().send_text(MAV_SEVERITY_ALERT,"Parachute: Too low"); AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_TOO_LOW); return; } +#endif // if we get this far release parachute parachute_release(); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 69341bc101..917f90b24f 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -208,6 +208,7 @@ enum HarmonicNotchDynamicMode { #define FS_THR_ENABLED_ALWAYS_LAND 3 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4 #define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5 +#define FS_THR_ENABLED_CONTINUE_MISSION_ALWAYS_LAND 6 // GCS failsafe definitions (FS_GCS_ENABLE parameter) #define FS_GCS_DISABLED 0 @@ -221,6 +222,7 @@ enum HarmonicNotchDynamicMode { #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe #define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe #define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize +#define FS_EKF_ACTION_PARACHUTE 4 // for mavlink SET_POSITION_TARGET messages #define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ((1<<0) | (1<<1) | (1<<2)) diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index a69ae93b0a..2f79bc3de5 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -162,6 +162,12 @@ void Copter::failsafe_ekf_event() set_mode_land_with_pause(ModeReason::EKF_FAILSAFE); } break; + + case FS_EKF_ACTION_PARACHUTE: + AP::logger().Write_Error(LogErrorSubsystem::PARACHUTES, LogErrorCode::PARACHUTE_REASON_EKF_FAILSAFE); + parachute_release(); + break; + case FS_EKF_ACTION_LAND: case FS_EKF_ACTION_LAND_EVEN_STABILIZE: default: diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index 152388d1ed..b1a5f226b2 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -178,6 +178,7 @@ void Copter::esc_calibration_setup() // arm and enable motors motors->armed(true); + update_armed_pin(); SRV_Channels::enable_by_mask(motors->get_motor_mask()); hal.util->set_soft_armed(true); } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 2a56baece3..8cff47720f 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -24,6 +24,9 @@ void Copter::failsafe_radio_on_event() case FS_THR_ENABLED_CONTINUE_MISSION: desired_action = Failsafe_Action_RTL; break; + case FS_THR_ENABLED_CONTINUE_MISSION_ALWAYS_LAND: + desired_action = Failsafe_Action_Land; + break; case FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL: desired_action = Failsafe_Action_SmartRTL; break; @@ -336,7 +339,7 @@ void Copter::set_mode_SmartRTL_or_RTL(ModeReason reason) } bool Copter::should_disarm_on_failsafe() { - if (ap.in_arming_delay) { + if (ap.in_arming_delay && control_mode != Mode::Number::GUIDED) { return true; } @@ -348,6 +351,14 @@ bool Copter::should_disarm_on_failsafe() { case Mode::Number::AUTO: // if mission has not started AND vehicle is landed, disarm motors return !ap.auto_armed && ap.land_complete; + case Mode::Number::GUIDED: + if (g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION || + g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION_ALWAYS_LAND) { + // prevent needing to arm twice in GUIDED + return false; + } + return ap.land_complete; + default: // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, Autotune, PosHold // if landed disarm @@ -382,6 +393,12 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){ #else arming.disarm(); #endif + break; + case Failsafe_Action_LandIfManual: + if (!flightmode->is_autopilot()) { + set_mode_land_with_pause(ModeReason::BATTERY_FAILSAFE); + } + break; } } diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index c3ee045c88..e14746440b 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -66,6 +66,7 @@ void Copter::failsafe_check() failsafe_last_timestamp = tnow; if(motors->armed()) { motors->armed(false); + update_armed_pin(); motors->output(); } } diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index d8ed9b1870..ebc05edcd2 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -159,6 +159,22 @@ void Copter::update_throttle_thr_mix() } else { // autopilot controlled throttle + float xy_des_speed_cms = 0.0f; + float xy_speed_cms = 0.0f; + float des_climb_rate_cms = pos_control->get_desired_velocity().z; + + if (pos_control->is_active_xy()) { + Vector3f vel_target = pos_control->get_vel_target(); + vel_target.z = 0.0f; + xy_des_speed_cms = vel_target.length(); + } + + if (position_ok() || optflow_position_ok()) { + Vector3f vel = inertial_nav.get_velocity(); + vel.z = 0.0f; + xy_speed_cms = vel.length(); + } + // check for aggressive flight requests - requested roll or pitch angle below 15 degrees const Vector3f angle_target = attitude_control->get_att_target_euler_cd(); bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD); @@ -175,10 +191,16 @@ void Copter::update_throttle_thr_mix() // check for requested decent bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f; - if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) { - attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio()); - } else { + bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f; + bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f; + bool slow_descent_demanded = pos_control->is_active_z() && des_climb_rate_cms < 0.0f && des_climb_rate_cms >= -100.0f; + + bool slow_auto_descent = xy_speed_demand_low && xy_speed_low && slow_descent_demanded; + + if ( slow_auto_descent || !(large_angle_request || large_angle_error || accel_moving || descent_not_demanded) ) { attitude_control->set_throttle_mix_min(); + } else { + attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio()); } } #endif diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index a2a3da4b9c..a97150b740 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -26,6 +26,7 @@ Mode::Mode(void) : { }; float Mode::auto_takeoff_no_nav_alt_cm = 0; +float Mode::auto_takeoff_max_nav_alt_cm = 0; // return the static controller object corresponding to supplied mode Mode *Copter::mode_from_mode_num(const Mode::Number mode) @@ -192,7 +193,17 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) return true; } + if (motors->armed() && + (control_mode == Mode::Number::STABILIZE || + control_mode == Mode::Number::ALT_HOLD || + control_mode == Mode::Number::LOITER) && + reason == ModeReason::GCS_COMMAND) { + gcs().send_text(MAV_SEVERITY_WARNING,"Refusing GCS mode change"); + return false; + } + Mode *new_flightmode = mode_from_mode_num((Mode::Number)mode); + if (new_flightmode == nullptr) { gcs().send_text(MAV_SEVERITY_WARNING,"No such mode"); AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode)); @@ -293,6 +304,9 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) // update notify object notify_flight_mode(); + // we want to allow arming with no RC in GUIDED mode + arming.disable_RC_check(mode == Mode::Number::GUIDED); + // return success return true; } @@ -508,9 +522,7 @@ void Mode::make_safe_spool_down() int32_t Mode::get_alt_above_ground_cm(void) { int32_t alt_above_ground; - if (copter.rangefinder_alt_ok()) { - alt_above_ground = copter.rangefinder_state.alt_cm_filt.get(); - } else { + if (!copter.get_rangefinder_height_interpolated_cm(alt_above_ground)) { bool navigating = pos_control->is_active_xy(); if (!navigating || !copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) { alt_above_ground = copter.current_loc.alt; @@ -529,6 +541,10 @@ void Mode::land_run_vertical_control(bool pause_descent) #endif // compute desired velocity + const float precland_acceptable_error_high = 100.0f; + const float precland_slow_descent_speed = 40.0f; + const float precland_slowdown_height = 50.0f; + const float precland_commit_height = 35.0f; const float precland_acceptable_error = 15.0f; const float precland_min_descent_speed = 10.0f; @@ -545,16 +561,29 @@ void Mode::land_run_vertical_control(bool pause_descent) max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. - cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); + cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z()*0.25, G_Dt); // 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 (doing_precision_landing && copter.rangefinder_alt_ok() && copter.rangefinder_state.alt_cm > 35.0f && copter.rangefinder_state.alt_cm < 200.0f) { - float max_descent_speed = abs(g.land_speed)*0.5f; + int32_t rangefinder_height_above_terrain_cm; + bool rangefinder_height_above_terrain_cm_valid = copter.get_rangefinder_height_interpolated_cm(rangefinder_height_above_terrain_cm); + if (doing_precision_landing && rangefinder_height_above_terrain_cm_valid) { + // Smoothly slow for precision landing + const float p_alt_hold = pos_control->get_pos_z_p().kP(); + cmb_rate = MAX(cmb_rate, AC_AttitudeControl::sqrt_controller(precland_slowdown_height-rangefinder_height_above_terrain_cm, p_alt_hold, pos_control->get_max_accel_z()*0.25f, G_Dt)); + cmb_rate = MIN(cmb_rate, -precland_slow_descent_speed); + } + + if (doing_precision_landing && rangefinder_height_above_terrain_cm_valid && rangefinder_height_above_terrain_cm > precland_commit_height && rangefinder_height_above_terrain_cm < precland_slowdown_height) { + float max_descent_speed = precland_slow_descent_speed; float land_slowdown = MAX(0.0f, pos_control->get_horizontal_error()*(max_descent_speed/precland_acceptable_error)); cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown); } + + if (doing_precision_landing && pos_control->get_horizontal_error() > precland_acceptable_error_high) { + cmb_rate = -precland_min_descent_speed; + } } // update altitude target and call position controller diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index e9e1d2dac6..a28fe32493 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -172,6 +172,7 @@ class Mode { void auto_takeoff_attitude_run(float target_yaw_rate); // altitude below which we do no navigation in auto takeoff static float auto_takeoff_no_nav_alt_cm; + static float auto_takeoff_max_nav_alt_cm; public: // Navigation Yaw control diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index d10c0106ec..33ff77356b 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -533,6 +533,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) // exit_mission - function that is called once the mission completes void ModeAuto::exit_mission() { + gcs().send_text(MAV_SEVERITY_INFO,"Mission completed"); // play a tone AP_Notify::events.mission_complete = 1; // if we are not on the ground switch to loiter or land @@ -1167,6 +1168,7 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { // set state to fly to location land_state = LandStateType_FlyToLocation; + gcs().send_text(MAV_SEVERITY_INFO,"Flying to land location"); const Location target_loc = terrain_adjusted_location(cmd); @@ -1174,6 +1176,7 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd) } else { // set landing state land_state = LandStateType_Descending; + gcs().send_text(MAV_SEVERITY_INFO,"Landing"); // initialise landing controller land_start(); @@ -1531,12 +1534,26 @@ bool ModeAuto::verify_land() // advance to next state land_state = LandStateType_Descending; + gcs().send_text(MAV_SEVERITY_INFO,"Landing"); } break; case LandStateType_Descending: // rely on THROTTLE_LAND mode to correctly update landing status retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE); + if (retval) { + gcs().send_text(MAV_SEVERITY_INFO,"Landed"); + } + if (retval && !mission.continue_after_land() && copter.motors->armed()) { + /* + we want to stop mission processing on land + completion. Disarm now, then return false. This + leaves mission state machine in the current NAV_LAND + mission item. After disarming the mission will reset + */ + copter.arming.disarm(); + retval = false; + } break; default: diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 1b5fc8a0c7..811ac984a4 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -40,6 +40,10 @@ struct Guided_Limit { // guided_init - initialise guided controller bool ModeGuided::init(bool ignore_checks) { + if (motors->armed()) { + return false; + } + // start in position control mode pos_control_start(); return true; @@ -381,6 +385,9 @@ void Mode::auto_takeoff_run() if (!motors->armed() || !copter.ap.auto_armed) { make_safe_spool_down(); wp_nav->shift_wp_origin_to_current_pos(); + + // get initial alt for WP_NAVALT_MIN + auto_takeoff_set_start_alt(); return; } diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index c403852b3c..ab55b164ba 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -41,6 +41,7 @@ void Copter::motor_test_output() motor_test_start_ms = now; if (!motors->armed()) { motors->armed(true); + update_armed_pin(); hal.util->set_soft_armed(true); } } @@ -153,6 +154,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t init_rc_out(); enable_motor_output(); motors->armed(true); + update_armed_pin(); hal.util->set_soft_armed(true); } @@ -199,6 +201,7 @@ void Copter::motor_test_stop() // disarm motors motors->armed(false); + update_armed_pin(); hal.util->set_soft_armed(false); // reset timeout diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 1982632c8d..b693025b62 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -54,6 +54,9 @@ void Copter::read_rangefinder(void) // tilt corrected but unfiltered, not glitch protected alt rf_state.alt_cm = tilt_correction * rangefinder.distance_cm_orient(rf_orient); + // remember inertial alt to allow us to interpolate rangefinder + rf_state.inertial_alt_cm = inertial_nav.get_altitude(); + // glitch handling. rangefinder readings more than RANGEFINDER_GLITCH_ALT_CM from the last good reading // are considered a glitch and glitch_count becomes non-zero // glitches clear after RANGEFINDER_GLITCH_NUM_SAMPLES samples in a row. @@ -120,6 +123,24 @@ bool Copter::rangefinder_up_ok() return (rangefinder_up_state.enabled && rangefinder_up_state.alt_healthy); } +/* + get inertially interpolated rangefinder height. Inertial height is + recorded whenever we update the rangefinder height, then we use the + difference between the inertial height at that time and the current + inertial height to give us interpolation of height from rangefinder + */ +bool Copter::get_rangefinder_height_interpolated_cm(int32_t& ret) +{ + if (!rangefinder_alt_ok()) { + return false; + } + ret = rangefinder_state.alt_cm_filt.get(); + float inertial_alt_cm = inertial_nav.get_altitude(); + ret += inertial_alt_cm - rangefinder_state.inertial_alt_cm; + return true; +} + + /* update RPM sensors */ diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 811e1dba34..aa1c592be1 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -166,6 +166,10 @@ void Copter::init_ardupilot() camera_mount.init(); #endif +#if PARACHUTE == ENABLED + parachute.init(serial_manager); +#endif + #if PRECISION_LANDING == ENABLED // initialise precision landing init_precland(); @@ -421,7 +425,7 @@ void Copter::update_auto_armed() } // if motors are armed and throttle is above zero auto_armed should be true // if motors are armed and we are in throw mode, then auto_armed should be true - } else if (motors->armed() && !ap.using_interlock) { + } else if (motors->armed() && !ap.using_interlock && control_mode != Mode::Number::AUTO && control_mode != Mode::Number::GUIDED) { if(!ap.throttle_zero || control_mode == Mode::Number::THROW) { set_auto_armed(true); } @@ -682,3 +686,15 @@ bool Copter::is_tradheli() const return false; #endif } + + +/* + update arming pin set with ARM_PIN, if any + */ +void Copter::update_armed_pin(void) +{ + if (matternet.arm_pin != -1) { + hal.gpio->pinMode(matternet.arm_pin, HAL_GPIO_OUTPUT); + hal.gpio->write(matternet.arm_pin, motors->armed()); + } +} diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 4840530adf..b569a4b7de 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -142,11 +142,18 @@ void Mode::auto_takeoff_set_start_alt(void) { // start with our current altitude auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude(); - + auto_takeoff_max_nav_alt_cm = inertial_nav.get_altitude(); + if (is_disarmed_or_landed() || !motors->get_interlock()) { // we are not flying, add the wp_navalt_min auto_takeoff_no_nav_alt_cm += g2.wp_navalt_min * 100; + auto_takeoff_max_nav_alt_cm += g2.wp_navalt_max * 100; + auto_takeoff_max_nav_alt_cm = MAX(auto_takeoff_max_nav_alt_cm,auto_takeoff_no_nav_alt_cm); } + + // reset the min baro alt seen in takeoff + copter.takeoff_baro_min_alt_m = copter.barometer.get_altitude(); + copter.takeoff_liftoff_complete = false; } @@ -157,16 +164,75 @@ void Mode::auto_takeoff_set_start_alt(void) void Mode::auto_takeoff_attitude_run(float target_yaw_rate) { float nav_roll, nav_pitch; - - if (g2.wp_navalt_min > 0 && inertial_nav.get_altitude() < auto_takeoff_no_nav_alt_cm) { - // we haven't reached the takeoff navigation altitude yet + + nav_roll = wp_nav->get_roll(); + nav_pitch = wp_nav->get_pitch(); + + /* + record the minimum barometer altitude we see in the + takeoff. This is used to detect the "dip" that happens on the + baro when the motors start applying thrust. + */ + float barometer_alt = copter.barometer.get_altitude(); + if (barometer_alt < copter.takeoff_baro_min_alt_m) { + copter.takeoff_baro_min_alt_m = barometer_alt; + } + + /* + check conditions for liftoff completion + We consider liftoff to be completed under one of 3 conditions: + 1) the TOFF_BARO_DIP and TOFF_HOV_PCT are both zero + 2) the barometric height has risen from the minimum by at least TOFF_BARO_DIP + 2) the throttle has reached TOFF_HOV_PCT of the hover throttle + */ + if (!copter.takeoff_liftoff_complete) { + if (copter.matternet.tkoff_baro_dip > 0 && + barometer_alt - copter.takeoff_baro_min_alt_m >= copter.matternet.tkoff_baro_dip) { + copter.takeoff_liftoff_complete = true; + gcs().send_text(MAV_SEVERITY_INFO, "LIFTOFF: baro %.1f thr=%.2f\n", barometer_alt - copter.takeoff_baro_min_alt_m, motors->get_throttle()); + } + if (copter.matternet.tkoff_hover_pct > 0 && + motors->get_throttle() >= copter.matternet.tkoff_hover_pct * 0.01 * motors->get_throttle_hover()) { + copter.takeoff_liftoff_complete = true; + gcs().send_text(MAV_SEVERITY_INFO, "LIFTOFF: throttle %.2f baro=%.1f\n", motors->get_throttle(), barometer_alt - copter.takeoff_baro_min_alt_m); + } + if (copter.matternet.tkoff_baro_dip <= 0 && + copter.matternet.tkoff_hover_pct <= 0) { + // both are disabled, do direct takeoff + copter.takeoff_liftoff_complete = true; + } + } + + + float alt_cm = inertial_nav.get_altitude(); + if (!copter.takeoff_liftoff_complete) { + // below no nav alt we zero roll and pitch demand nav_roll = 0; nav_pitch = 0; // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup pos_control->set_limit_accel_xy(); - } else { - nav_roll = wp_nav->get_roll(); - nav_pitch = wp_nav->get_pitch(); + + // clear rate integrators till we reach min navigation altitude + attitude_control->get_rate_roll_pid().set_integrator(0); + attitude_control->get_rate_pitch_pid().set_integrator(0); + attitude_control->get_rate_yaw_pid().set_integrator(0); + + wp_nav->shift_wp_origin_to_current_pos(); + + auto_takeoff_max_nav_alt_cm = alt_cm + g2.wp_navalt_max * 100; + } else if (g2.wp_navalt_max > 0 && alt_cm < auto_takeoff_max_nav_alt_cm) { + // between no nav alt and max nav alt we interpolate + // roll/pitch demand + float lean_limit = linear_interpolate(0, copter.aparm.angle_max, + alt_cm, + auto_takeoff_no_nav_alt_cm, auto_takeoff_max_nav_alt_cm); + if (fabsf(nav_roll) > lean_limit || + fabsf(nav_pitch) > lean_limit) { + nav_roll = constrain_float(nav_roll, -lean_limit, lean_limit); + nav_pitch = constrain_float(nav_pitch, -lean_limit, lean_limit); + // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup + pos_control->set_limit_accel_xy(); + } } // roll & pitch from waypoint controller, yaw rate from pilot diff --git a/Tools/bootloaders/CubeOrange_bl.bin b/Tools/bootloaders/CubeOrange_bl.bin index b54d6fd2cb..022f275dca 100755 Binary files a/Tools/bootloaders/CubeOrange_bl.bin and b/Tools/bootloaders/CubeOrange_bl.bin differ diff --git a/Tools/bootloaders/CubeOrange_bl.elf b/Tools/bootloaders/CubeOrange_bl.elf index 6e8538cbca..d9d8cccaba 100755 Binary files a/Tools/bootloaders/CubeOrange_bl.elf and b/Tools/bootloaders/CubeOrange_bl.elf differ diff --git a/Tools/bootloaders/CubeOrange_bl.hex b/Tools/bootloaders/CubeOrange_bl.hex index ab71a9ae70..0d5a27aa50 100644 --- a/Tools/bootloaders/CubeOrange_bl.hex +++ b/Tools/bootloaders/CubeOrange_bl.hex @@ -1,19 +1,19 @@ :020000040800F2 -:1000000000040020A1020008391000083D1000087B -:10001000911000083D10000865100008A3020008B8 -:10002000A3020008A3020008A3020008A1280008F8 +:1000000000040020A10200082D1000083110000893 +:10001000851000083110000859100008A3020008DC +:10002000A3020008A3020008A3020008A5280008F4 :10003000A3020008A3020008A3020008A30200080C :10004000A3020008A3020008A3020008A3020008FC -:10005000A3020008A3020008E53D0008113E0008C5 -:100060003D3E0008693E0008953E0008A3020008D6 +:10005000A3020008A3020008F13D00081D3E0008AD +:10006000493E0008753E0008A13E0008A3020008B2 :10007000A3020008A3020008A3020008A3020008CC :10008000A3020008A3020008A3020008A3020008BC -:10009000A3020008A3020008A3020008C13E000852 +:10009000A3020008A3020008A3020008CD3E000846 :1000A000A3020008A3020008A3020008A30200089C :1000B000A3020008A3020008A3020008A30200088C :1000C000A3020008A3020008A3020008A30200087C :1000D000A3020008A3020008A3020008A30200086C -:1000E000293F0008A3020008A3020008A302000899 +:1000E000353F0008A3020008A3020008A30200088D :1000F000A3020008A3020008A3020008A30200084C :10010000A3020008A3020008653A0008A302000841 :10011000A3020008A3020008A3020008A30200082B @@ -22,13 +22,13 @@ :10014000A3020008A3020008A3020008A3020008FB :10015000A3020008A3020008A3020008A3020008EB :10016000A3020008A3020008A3020008A3020008DB -:10017000A3020008D9330008A3020008A302000864 -:10018000A3020008A302000821110008A30200082E +:10017000A3020008DD330008A3020008A302000860 +:10018000A3020008A302000815110008A30200083A :10019000A3020008A3020008A3020008A3020008AB :1001A000A3020008A3020008A3020008A30200089B :1001B000A3020008A3020008A3020008A30200088B :1001C000A3020008A3020008A3020008A30200087B -:1001D000A3020008C5330008A3020008A302000818 +:1001D000A3020008C9330008A3020008A302000814 :1001E000A3020008A3020008A3020008A30200085B :1001F000A3020008A3020008A3020008A30200084B :10020000A3020008A3020008A3020008A30200083A @@ -48,279 +48,279 @@ :1002E000C0F2F0004EF68851CEF200010860BFF374 :1002F0004F8FBFF36F8F4FF00000E1EE100A4EF604 :100300003C71CEF200010860062080F31488BFF330 -:100310006F8F02F003FB03F023FB4FF055301F49B2 +:100310006F8F02F005FB03F023FB4FF055301F49B0 :100320001B4A91423CBF41F8040BFAE71C49194AA9 :1003300091423CBF41F8040BFAE71A491A4A1B4B99 :100340009A423EBF51F8040B42F8040BF8E7002034 :100350001749184A91423CBF41F8040BFAE702F0F2 -:1003600021FB03F071FB144C144DAC4203DA54F83A +:1003600023FB03F071FB144C144DAC4203DA54F838 :10037000041B8847F9E700F03FF8114C114DAC42DF -:1003800003DA54F8041B8847F9E702F009BB0000C0 +:1003800003DA54F8041B8847F9E702F00BBB0000BE :1003900000040020002400200000000800000020CD -:1003A0000004002090430008002400204424002082 +:1003A000000400209843000800240020442400207A :1003B0004824002060350020A0020008A0020008A8 :1003C000A0020008A00200082DE9F04F2DED108AD0 :1003D000C1F80CD0D0F80CD0BDEC108ABDE8F08F7D :1003E000002383F311882846A047002001F0BEFEB9 -:1003F00001F06EFE00DFFEE738B5204C6FF07303AE +:1003F00001F06EFE00DFFEE738B51F4C6FF07303AF :10040000002523701E236570A570E5702571657148 -:10041000A571E57125726572A372E57200F04AFD5F -:1004200020B10E2325726572A372E57202F0FAF90B -:10043000044602F02FFA0546D8B9114B9C421AD057 -:1004400001339C4218BF054641F2883404BF0125A0 -:100450000024002002F0F0F90DB100F0B7F800F030 -:1004600091FD00F07DFC204600F070F900F0AEF840 -:10047000F9E70024EDE70446EBE700BF482400203D -:10048000010007B008B500F0E9FBA0F120035842D5 -:10049000584108BD07B5054B02211B88ADF8043053 -:1004A00001A800F047FC03B05DF804FB30400008F1 -:1004B000064991F8243033B100230822086A81F8F4 -:1004C000243000F071BC0120704700BF6824002078 -:1004D0002DE9F84F234C94F82430054688461746FA -:1004E0008BBB2E46DFF87C90002F38D094F824A0E8 -:1004F000BAF1000F05D12022FF214846266200F004 -:100500002FFDCAF10805BD4228BF3D465FFA85FBB5 -:10051000AD0041462A4604EB8A0000F0F7FC94F84F -:100520002430A7EB0B079B445FFA8BFBBBF1080F52 -:100530002E44A844FFB284F824B0D5D1FFF7B8FF09 -:100540000028D1D108E0266A06EB8306B042C9D064 -:10055000FFF7AEFF0028C4D10020BDE8F88F0120CE -:10056000BDE8F88F6824002010B5202383F311889C -:100570001248C3680BB101F07FFE0023104A0F48F8 -:100580004FF47A7101F03CFE002383F311880D4C87 -:10059000236813B12368013B2360636813B1636868 -:1005A000013B6360084B1B7833B9636823B90220B1 -:1005B00000F0AAFC3223636010BD00BF5424002069 -:1005C0006905000898250020902400202DE9F041BD -:1005D000544B554953F8042F013201D1BDE8F08145 -:1005E0008B42F7D1514C524B22689A4240F298808C -:1005F000504B9B6803F1006303F500339A4280F08F -:100600008F80002000F0C8FB4B4B0220187000F0D8 -:1006100079FC4A4BD3F8E8200022C3F8E820D3F84D -:100620001011C3F81021D3F81011D3F8EC10C3F84F -:10063000EC20D3F81411C3F81421D3F81411D3F813 -:10064000F010C3F8F020D3F81811C3F81821D3F82C -:100650001811D3F8801041F00061C3F88010D3F86E -:10066000801021F00061C3F88010D3F88010D3F817 -:10067000801041F00071C3F88010D3F8801021F091 -:100680000071C3F88010D3F8803072B62C492D4B1E -:100690000B601D682468BFF34F8FBFF36F8F2A4B29 -:1006A000C3F88420BFF34F8F5A6922F480325A6115 -:1006B000BFF34F8FD3F88020C2F3C906C2F34E3286 -:1006C0005201B70743F6E07E02EA0E08384631468B -:1006D000013948EA000CB1F1FF3FC3F874C200F1E0 -:1006E0004040F5D1203A12F1200FEDD1BFF34F8FEA -:1006F000BFF36F8FBFF34F8FBFF36F8F5A6922F431 -:1007000000325A610022C3F85022BFF34F8FBFF36B -:100710006F8F202383F31188AD4685F30888204727 -:10072000BDE8F081FCFF01081C000208040002087B -:10073000FFFF0108482400209024002000440258B4 -:1007400008ED00E00000020800ED00E02DE9F04FA8 -:1007500099B0B34C01902022FF2110A8A66800F0A8 -:10076000FFFBB04A1378A346A3B9AF480121C36089 -:100770001170202383F31188C3680BB101F07CFD55 -:100780000023AA4AA8484FF47A7101F039FD0023EA -:1007900083F31188019B13B1A54B019A1A60A54AF6 -:1007A000A349019F0292002313704B6099461C4697 -:1007B0001D469846012000F0A5FB002F00F0128294 -:1007C0009B4B1B68002B40F00D8219B0BDE8F08FE9 -:1007D0000220FFF757FE002840F0FB81019BB9F192 -:1007E000000F08BF1F46944B1B88ADF82C30022128 -:1007F0000BA800F09FFADDE74FF47A7000F02EFAB4 -:10080000031E0393EADB0220FFF73CFE824600282A -:10081000E4D0039B581E042800F2DD81DFE800F0DD -:10082000030E1114170018A8052340F8343D0421C5 -:1008300000F080FA54464FF0000856E00421784852 -:10084000F6E704217D48F3E704217D48F0E71C2406 -:10085000204600F0A3FA04340B9004210BA800F00A -:1008600069FA2C2CF4D1E5E7002DB7D0002CB5D0D7 -:100870000220FFF707FE0546002800F0AF810120A7 -:100880006C4C00F089FA0220207000F03BFB4FF026 -:1008900000095FFA89FA504600F08EFA074658B10F -:1008A000504600F099FA09F101090028F1D12C46CF -:1008B000A9460027634B97E701230220237000F02D -:1008C0001FFB3E46DBF808309E4206D2304600F061 -:1008D00065FA0130EBD10436F4E7029B00261E7066 -:1008E000534BA9465E602C46374600F041FB15B1DC -:1008F000002C18BF0027FFF7CDFD5BE7002D3FF46C -:100900006DAF002C3FF46AAF029B0220187000F01C -:10091000F9FA322000F0A2F9B0F1000AC0F25E81CB -:100920001AF0030540F05A81DBF8082006EB0A03B1 -:10093000934200F25381BAF5807F00F24F81554512 -:100940000DDA4FF47A7000F089F90490049B002BC3 -:10095000C0F244813C4A049BAB540135EFE7C82008 -:10096000FFF790FD0546002800F038811F2E11D8B2 -:10097000C6F12004544528BF544610AB26F00300AE -:1009800022463149184400F0C1FA2246FF212E4880 -:1009900000F0E6FA4FEAAA0A5FFA8AF22A493046DC -:1009A000FFF796FD0446002800F01A8106EB8A0640 -:1009B00005469AE70220FFF765FD00283FF40EAFD9 -:1009C000FFF776FD00283FF409AF4FF0000A5346C9 -:1009D000DBF8082092451CD2BAF11F0F12D8109AEA -:1009E00001320FD02AF0030218A90A4452F8202C31 -:1009F0000B92184604220BA900F05EFB0AF1040AD0 -:100A00000346E5E75046039300F0C8F9039B0B90BB -:100A1000EFE718A8042140F84C3D00F08BF964E79B -:100A20004824002094250020542400206905000853 -:100A30009825002090240020324000084C240020FB -:100A400050240020344000089424002018A80023DB -:100A500040F8343D642100F039F900287FF4BEAE3F -:100A60000220FFF70FFD00283FF4B8AE0B9800F00E -:100A7000C5F918AB43F8480D04211846CDE718A86E -:100A8000002340F8343D642100F020F900287FF471 -:100A9000A5AE0220FFF7F6FC00283FF49FAE0B98AE -:100AA00000F0AEF918AB43F8440DE5E70220FFF77C -:100AB000E9FC00283FF492AE00F0A8F918AB43F827 -:100AC000400DD9E70220FFF7DDFC00283FF486AE99 -:100AD0000BA9142000F0A0F9824618A8042140F8C0 -:100AE0003CAD00F027F951460BA896E7322000F004 -:100AF000B5F8B0F1000AFFF671AE1AF0030F7FF4FB -:100B00006DAEDBF808200AEB080393423FF666AEB1 -:100B10000220FFF7B7FC00283FF460AE2AF0030A7A -:100B2000C244D0453FF4E1AE404600F037F904211D -:100B30000A900AA800F0FEF808F10408F1E74FF463 -:100B40007A70FFF79FFC00283FF448AEFFF7B0FC37 -:100B500000283FF4AFAE109B01330CD0082210A93F -:100B60000020FFF7B5FC00283FF4A4AE2022FF21AF -:100B700010A800F0F5F9FFF78DFC374801F0FCFAFA -:100B800023E6002D3FF42AAE002C3FF427AE18A830 -:100B9000002340F8343D642100F098F88246002894 -:100BA0007FF41CAE0220FFF76DFC00283FF416AE68 -:100BB0000390FFF76FFC41F2883001F0DDFA0B98EB -:100BC00000F01AFA00F0D4F9039B57461C461D4664 -:100BD000F0E5054689E64FF00008FFE52546FDE50E -:100BE0002C4667E6002000F039F80490049B002BA7 -:100BF000FFF6E3AD012000F083F9049B213B122BAB -:100C00003FF6D8AD01A252F823F000BFD10700088B -:100C1000F907000869080008B5070008B5070008CB -:100C2000B5070008FD080008ED0A0008B50900082E -:100C30004D0A00087F0A0008AD0A0008B507000841 -:100C4000C50A0008B50700083F0B0008EB080008BC -:100C5000B5070008830B0008A08601002DE9F347C3 -:100C600002AE00244FF47A7506F8014D144FDFF8F8 -:100C70005880454397F900305A1C5FFA84F901D037 -:100C8000A34212D158F8240003680122D3F820A00F -:100C900031462B46D047012807D10A4B9DF8070063 -:100CA00083F8009002B0BDE8F0870134022CE1D156 -:100CB0004FF4FA7001F060FA4FF0FF30F2E700BF36 -:100CC00000240020BC250020484000082DE9F04702 -:100CD0004FF47A75144FDFF8588006464D430024D0 -:100CE00097F900305A1C5FFA84F901D0A34210D161 -:100CF00058F8240003680422D3F820A031462B467C -:100D0000D047042805D1094B83F800900020BDE8A6 -:100D1000F0870134022CE3D14FF4FA7001F02CFA81 -:100D20004FF0FF30BDE8F08700240020BC250020F4 -:100D30004840000830B4074B1A78074B53F822405C -:100D40002368DD69054B0A46AC460146204630BCA7 -:100D5000604700BFBC25002048400008A086010075 -:100D6000F8B501F051FC094C094E0A4F20700025DE -:100D700030682378834207D901F042FC3368054488 -:100D80000133BD423360F3D9F8BD00BFBD2500205B -:100D9000A0250020FFFF010001F0F8BC00F1006079 -:100DA00000F500300068704700F10060920000F527 -:100DB000003001F085BC0000054B1A68054B1B781C -:100DC0009B1A834202D9104401F01ABC00207047DC -:100DD000A0250020BD25002038B5074D0446286811 -:100DE000204401F013FC28B928682044BDE83840AD -:100DF00001F01EBC38BD00BFA025002000207047B8 -:100E0000014BC058704700BF00E8F11F014B186844 -:100E1000704700BF0010005CF0B5244B244C1B68E9 -:100E20002788656894F90820C3F30B06BE424FEA91 -:100E3000134328D0A789BE4206D101220C2505FB09 -:100E40000244656894F9082041F20104A3421CD0D1 -:100E500041F20304A3421AD042F20104A34218D083 -:100E600042F20304A34208BF5622441E0C44013D33 -:100E70000B46A34217D215F9016F581C5EB1034609 -:100E800000F8016CF5E70022D8E75A22EDE7592275 -:100E9000EBE75822E9E72C2584421D7001D9981C04 -:100EA0005A70401AF0BD1846FBE700BF0010005C06 -:100EB00004240020104B5B8841F201018B429AB25E -:100EC00011D041F203018B420FD042F201039A424A -:100ED0000DD042F203039A420BD10323074A02EBDF -:100EE0008303D87870470023F8E70123F6E702234D -:100EF000F4E70020704700BF0010005C3840000895 -:100F000070470000704700007047000010B50023D4 -:100F1000934203D0CC5CC4540133F9E710BD000008 -:100F200030B5441E14F9010F0B4660B193F900501F -:100F3000854201F1010106D11AB993F90020801A06 -:100F400030BD013AEEE7002AF7D1104630BD00006F -:100F500002460346981A13F9011B0029FAD170477B -:100F600002440346934202D003F8011BFAE770479C -:100F7000024B1A78024B1A70704700BFBC25002044 -:100F80000024002038B5164C164D204600F026FCF3 -:100F90002946204600F04EFC2D681348D5F89020D5 -:100FA000D2F8043843F00203C2F8043801F0E4F840 -:100FB0000E49284600F04EFDD5F890200C48D2F896 -:100FC00004380C4923F00203C2F80438A0424FF45D -:100FD000E1330B6003D0BDE8384000F05DBB38BDA5 -:100FE000EC2A00202841000840420F00544100082C -:100FF00040260020A425002038B50B4B1A780B4B57 -:1010000053F822400A4B9C4205460CD0094B002164 -:1010100018461822FFF7A4FF056001462046BDE8E8 -:10102000384000F039BB38BDBC25002048400008DE -:10103000EC2A0020A4250020FEE7000000B59BB0AC -:10104000EFF3098168226846FFF760FFEFF305833D -:10105000034B9A6B9A6A9A6A9A6A9A6A9B6AFEE743 -:1010600000ED00E000B59BB0EFF30981682268460F -:10107000FFF74CFFEFF30583044B9A6B9A6A9A6A69 -:101080009A6A9A6A9A6A9B6AFEE700BF00ED00E0DE -:1010900000B59BB0EFF3098168226846FFF736FF81 -:1010A000EFF30583034B5A6B9A6A9A6A9A6A9A6AB3 -:1010B0009B6AFEE700ED00E030B5084D0A4491421E -:1010C0000BD011F8013B09245840013CF7D040F304 -:1010D00000032B4083EA5000F7E730BD2083B8EDD2 -:1010E000002304491A465A50C8180833802B42601E -:1010F000F9D17047C0250020024A136843F0C003AD -:101100001360704700780040044B5A6810439A6897 -:1011100058600AB1181D1047704700BF40260020D4 -:101120002DE9F84F404EF56D2F68EC692C622107D0 -:1011300018D014F0080F0CBF00208020E20748BF31 -:1011400040F02000A30748BF40F04000610748BFBF -:1011500040F48070202383F31188FFF7D5FF00232C -:1011600083F31188E20509D5202383F311884FF416 -:101170000070FFF7C9FF002383F31188DFF8A8A0F0 -:101180004FF020094FF0000B14F0200832D13B063D -:1011900015D5DFF898A04FF0200920060FD589F368 -:1011A0001188504600F0ECF9002830DA0820FFF7EB -:1011B000ABFF27F080032B60002383F311887906AF -:1011C0000DD562060BD5202383F31188F26C336DA5 -:1011D0009A4201D1336C03BB002383F31188E306E9 -:1011E00004D5B26E13690BB150699847BDE8F84F4A -:1011F00001F066BB89F31188AB8C96F864105046F9 -:10120000194000F057FA8BF31188EC69BCE780B203 -:10121000288588F31188EC69BFE7102027F0400784 -:10122000FFF772FF2F60D7E74026002078260020C6 -:1012300013B5104C204600F023FA04F1140000947A -:1012400000234FF400720C4900F0E4F804F1380078 -:1012500000940A4B0A494FF4007200F05DF9094B03 -:10126000E365094B23660C21522002B0BDE8104013 -:1012700000F05EB840260020AC260020F9100008DF -:10128000AC2800200078004000E1F505254B00293E -:1012900008BF1946037C8166012B30B511D1224B62 -:1012A00098420ED1214BD3F8E82042F08042C3F897 -:1012B000E820D3F8102142F08042C3F81021D3F87F -:1012C00010310A68036EC46D03EB5203B3FBF2F3F3 -:1012D0004A68150442BF23F0070503F0070343EAF9 -:1012E0004503E3608B6843F040036360CB6843F0E1 -:1012F0000103A36042F4967343F001032360510598 -:101300004FF0FF33236205D512F0102205D0B2F161 -:10131000805F04D080F8643030BD7F23FAE73F233C -:10132000F8E700BF6C400008402600200044025847 -:1013300000F1604300F01F02400903F56143090119 -:101340008000C9B200F1604083F8001300F56140ED -:1013500001239340C0F8803103607047F8B515460B -:1013600082680669AA420B46816938BF8568761A89 -:10137000B542044607D218462A46FFF7C7FDA369BF -:101380002B44A3610DE011D932461846FFF7BEFD8C -:10139000AF1B3A46E1683044FFF7B8FDE2683A44D3 -:1013A000A261A3685B1BA3602846F8BD18462A46C5 -:1013B000FFF7ACFDE368E4E783682DE9F0410446FC -:1013C00093421546266938BF85684069361AB5428A -:1013D0000F4606D22A46FFF799FD63692B446361E5 -:1013E0000DE012D93246A5EB0608FFF78FFD424605 -:1013F000B919E068FFF78AFDE26842446261A368B8 -:101400005B1BA3602846BDE8F0812A46FFF77EFDFE -:10141000E368E4E710B50A440024C361029B00605E +:10041000A571E57125726572A372E57200F046FD63 +:1004200020B10E2325726572A372E57202F0FCF909 +:10043000044602F031FA0546D0B9104B9C4219D05F +:1004400001339C4241F2883412BF05460024012545 +:10045000002002F0F3F90DB100F0B6F800F08CFDC9 +:1004600000F07CFC204600F06FF900F0ADF8F9E7F1 +:100470000024EDE70446EBE748240020010007B024 +:1004800008B500F0E9FBA0F120035842584108BD2F +:10049000054B07B51B88022101A8ADF8043000F018 +:1004A00047FC03B05DF804FB3C400008064991F8A6 +:1004B000243033B100230822086A81F8243000F088 +:1004C00071BC0120704700BF682400202DE9F84F5F +:1004D000234C05468846174694F824308BBB2E469D +:1004E000DFF87C90002F38D094F824A0BAF1000FE8 +:1004F00005D12022FF214846266200F02BFDCAF1DB +:100500000805414604EB8A00BD4228BF3D465FFA1C +:1005100085FBAD00A7EB0B072A462E4400F0F2FC4A +:1005200094F82430A844FFB29B445FFA8BFBBBF1E4 +:10053000080F84F824B0D5D1FFF7B8FF0028D1D137 +:1005400008E0266A06EB8306B042C9D0FFF7AEFF8B +:100550000028C4D10020BDE8F88F0120BDE8F88F45 +:100560006824002010B5202383F311881248C36843 +:100570000BB101F081FE0023104A4FF47A710E484E +:1005800001F03EFE002383F311880D4C236813B164 +:100590002368013B2360636813B16368013B6360B8 +:1005A000084B1B7833B9636823B9022000F0A8FC1C +:1005B0003223636010BD00BF54240020650500088D +:1005C0009825002090240020554B56492DE9F041F4 +:1005D00053F8042F013201D1BDE8F0818B42F7D1ED +:1005E000514C524B22689A4240F29880504B9B6883 +:1005F00003F1006303F500339A4280F08F800020FE +:1006000000F0C8FB02204B4B187000F077FC4A4BFF +:10061000D3F8E8200022C3F8E820D3F81011C3F87B +:100620001021D3F81011D3F8EC10C3F8EC20D3F854 +:100630001411C3F81421D3F81411D3F8F010C3F82F +:10064000F020D3F81811C3F81821D3F81811D3F8F3 +:10065000801041F00061C3F88010D3F8801021F0C1 +:100660000061C3F88010D3F88010D3F8801041F0F7 +:100670000071C3F88010D3F8801021F00071C3F826 +:100680008010D3F8803072B62C4B2D490B601D685A +:100690002468BFF34F8FBFF36F8F2A4BC3F88420BA +:1006A000BFF34F8F5A6922F480325A61BFF34F8FE4 +:1006B000D3F8802043F6E07EC2F3C906C2F34E327F +:1006C000B707520102EA0E0838463146013948EAB6 +:1006D000000C00F14040B1F1FF3FC3F874C2F5D106 +:1006E000203A12F1200FEDD1BFF34F8FBFF36F8F80 +:1006F000BFF34F8FBFF36F8F5A6922F400325A61F4 +:100700000022C3F85022BFF34F8FBFF36F8F202317 +:1007100083F31188AD4685F308882047BDE8F08152 +:10072000FCFF01081C00020804000208FFFF01088A +:1007300048240020902400200044025800000208B1 +:1007400008ED00E000ED00E02DE9F04F99B0B34C6A +:100750002022FF21019010A8A66800F0FBFBB04A00 +:10076000A3461378A3B90121AE481170C3602023BA +:1007700083F31188C3680BB101F07EFD0023AA4A00 +:100780004FF47A71A74801F03BFD002383F31188F1 +:10079000019B13B1A54B019A1A600023A44AA349F7 +:1007A000019F99461C461D46984613704B60029265 +:1007B000012000F0A3FB002F00F012829B4B1B686E +:1007C000002B40F00D8219B0BDE8F08F0220FFF73A +:1007D00057FE002840F0FB81019BB9F1000F08BFD4 +:1007E0001F46944B1B8802210BA8ADF82C3000F05B +:1007F0009FFADDE74FF47A7000F02EFA031E0393A0 +:10080000EADB0220FFF73CFE82460028E4D0039B8F +:10081000581E042800F2DD81DFE800F0030E1114F9 +:10082000170018A80523042140F8343D00F080FA91 +:1008300054464FF0000856E004217848F6E70421BA +:100840007D48F3E704217D48F0E71C24204604346A +:1008500000F0A2FA04210B900BA800F069FA2C2CEE +:10086000F4D1E5E7002DB7D0002CB5D00220FFF77A +:1008700007FE0546002800F0AF8101206C4C00F017 +:1008800089FA4FF000090220207000F037FB5FFA70 +:1008900089FA504600F08EFA074658B1504609F1E1 +:1008A000010900F097FA0028F1D12C46A94600274B +:1008B000634B97E701233E460220237000F01CFBA8 +:1008C000DBF808309E4206D2304600F065FA01306F +:1008D000EBD10436F4E70026029BA9462C461E7095 +:1008E0003746524B5E6000F03DFB15B1002C18BF3F +:1008F0000027FFF7CDFD5BE7002D3FF46DAF002C27 +:100900003FF46AAF0220029B187000F0F7FA322021 +:1009100000F0A2F9B0F1000AC0F25E811AF00305FE +:1009200040F05A8106EB0A03DBF80820934200F2FC +:100930005381BAF5807F00F24F8155450DDA4FF4AF +:100940007A7000F089F90490049B002BC0F2448176 +:10095000049B3C4AAB540135EFE7C820FFF790FDFC +:100960000546002800F038811F2E11D8C6F120045A +:1009700010AB26F0030033495445184428BF5446B1 +:10098000224600F0BFFA2246FF212E4800F0E2FA8C +:100990004FEAAA0A2B4930465FFA8AF2FFF796FD22 +:1009A0000446002800F01A8106EB8A0605469AE7FD +:1009B0000220FFF765FD00283FF40EAFFFF776FD3C +:1009C00000283FF409AF4FF0000A5346DBF8082037 +:1009D00092451CD2BAF11F0F12D8109A01320FD0D3 +:1009E0002AF0030218A90A4452F8202C0B92184648 +:1009F00004220BA90AF1040A00F058FB0346E5E7BC +:100A00005046039300F0C8F9039B0B90EFE718A83A +:100A1000042140F84C3D00F08BF964E748240020A5 +:100A20009425002054240020650500089825002006 +:100A3000902400203E4000084C2400205024002038 +:100A4000404000089424002018A80023642140F8A6 +:100A5000343D00F039F900287FF4BEAE0220FFF7E4 +:100A60000FFD00283FF4B8AE0B9800F0C5F918ABA5 +:100A700043F8480D04211846CDE718A80023642147 +:100A800040F8343D00F020F900287FF4A5AE0220A4 +:100A9000FFF7F6FC00283FF49FAE0B9800F0AEF98C +:100AA00018AB43F8440DE5E70220FFF7E9FC002806 +:100AB0003FF492AE00F0A8F918AB43F8400DD9E727 +:100AC0000220FFF7DDFC00283FF486AE0BA91420BE +:100AD00000F0A0F9824618A8042140F83CAD00F0CF +:100AE00027F951460BA896E7322000F0B5F8B0F18F +:100AF000000AFFF671AE1AF0030F7FF46DAE0AEB39 +:100B00000803DBF8082093423FF666AE0220FFF7A9 +:100B1000B7FC00283FF460AE2AF0030AC244D04577 +:100B20003FF4E1AE404608F1040800F035F9042135 +:100B30000A900AA800F0FCF8F1E74FF47A70FFF78A +:100B40009FFC00283FF448AEFFF7B0FC00283FF4BC +:100B5000AFAE109B01330CD0082210A90020FFF784 +:100B6000B5FC00283FF4A4AE2022FF2110A800F01D +:100B7000F1F9FFF78DFC374801F0FEFA23E6002D6E +:100B80003FF42AAE002C3FF427AE18A800236421BE +:100B900040F8343D00F098F8824600287FF41CAEFF +:100BA0000220FFF76DFC00283FF416AE0390FFF71C +:100BB0006FFC41F28830574601F0DEFA0B9800F0E6 +:100BC00015FA00F0CFF9039B1C461D46F0E50546DB +:100BD00089E64FF00008FFE52546FDE52C4667E66F +:100BE000002000F039F80490049B002BFFF6E3ADE1 +:100BF000012000F081F9049B213B122B3FF6D8AD78 +:100C000001A252F823F000BFCD070008F507000845 +:100C100065080008B1070008B1070008B10700081F +:100C2000F9080008E90A0008B1090008490A0008A3 +:100C30007B0A0008A90A0008B1070008C10A0008D9 +:100C4000B10700083B0B0008E7080008B1070008DF +:100C50007F0B0008A08601002DE9F3474FF47A7559 +:100C6000002402AE154F4543DFF8588006F8014DC9 +:100C700097F900305FFA84F95A1C01D0A34212D1CF +:100C800058F82400012231460368D3F820A02B46EF +:100C9000D047012807D10A4B9DF8070083F8009040 +:100CA00002B0BDE8F0870134022CE1D14FF4FA70B4 +:100CB00001F062FA4FF0FF30F2E700BF002400209D +:100CC000BC250020544000082DE9F0474FF47A7508 +:100CD00006460024134F4D43DFF8508097F900304B +:100CE0005FFA84F95A1C01D0A34210D158F82400AD +:100CF000042231460368D3F820A02B46D0470428AD +:100D000005D1094B002083F80090BDE8F08701343D +:100D1000022CE3D14FF4FA7001F02EFA4FF0FF30BD +:100D2000BDE8F08700240020BC25002054400008C6 +:100D3000074B30B41A78074B53F822400A46014655 +:100D400023682046DD69044BAC4630BC604700BFD9 +:100D5000BC25002054400008A0860100F8B50A4CCC +:100D600000250A4E01F052FC094F207030682378AC +:100D7000834207D901F046FC336805440133BD4284 +:100D80003360F3D9F8BD00BFBD250020A0250020A9 +:100D9000FFFF010001F0FCBC00F1006000F5003035 +:100DA0000068704700F10060920000F5003001F02B +:100DB00089BC0000054B1A68054B1B789B1A8342BF +:100DC00002D9104401F01EBC00207047A02500206D +:100DD000BD25002038B5074D04462868204401F0A1 +:100DE00017FC28B928682044BDE8384001F022BC2F +:100DF00038BD00BFA025002000207047014BC0581F +:100E0000704700BF00E8F11F014B1868704700BF32 +:100E10000010005C234BF0B5234C1B682788C3F3FC +:100E20000B0665681B0C94F90820BE4228D0A789E0 +:100E3000BE4206D101220C2505FB0244656894F9E7 +:100E4000082041F20104A3421CD041F20304A34252 +:100E50001AD042F20104A34218D042F20304A34282 +:100E600008BF5622441E013D0B460C44A34217D234 +:100E700015F9016F581C5EB1034600F8016CF5E7E7 +:100E80000022D8E75A22EDE75922EBE75822E9E79A +:100E90002C2584421D7001D9981C5A70401AF0BD4F +:100EA0001846FBE70010005C04240020104B41F2C0 +:100EB00001015B888B429AB211D041F203018B424F +:100EC0000FD042F201039A420DD042F203039A423C +:100ED0000BD10323074A02EB8303D8787047002322 +:100EE000F8E70123F6E70223F4E70020704700BF8C +:100EF0000010005C4440000870470000704700008C +:100F000070470000002310B5934203D0CC5CC4545A +:100F10000133F9E710BD000030B5441E14F9010F8C +:100F20000B4658B193F900500131854206D11AB9E8 +:100F300093F90020801A30BD013AEFE7002AF7D17B +:100F4000104630BD02460346981A13F9011B0029CA +:100F5000FAD1704702440346934202D003F8011BC2 +:100F6000FAE77047024B1A78024B1A70704700BFBD +:100F7000BC2500200024002038B5164C164D204614 +:100F800000F02CFC2946204600F054FC2D68134844 +:100F9000D5F89020D2F8043843F00203C2F80438A0 +:100FA00001F0EAF80E49284600F054FDD5F89020EB +:100FB0000C48D2F804380C49A04223F00203C2F8CE +:100FC00004384FF4E1330B6003D0BDE8384000F043 +:100FD00063BB38BDEC2A00203441000840420F00BA +:100FE0006041000840260020A425002038B50B4BA6 +:100FF00005461A780A4B53F822400A4B9C420CD003 +:10100000094B002118221846FFF7A4FF056001468E +:101010002046BDE8384000F03FBB38BDBC2500206D +:1010200054400008EC2A0020A4250020FEE7000020 +:1010300000B59BB0EFF3098168226846FFF762FFB5 +:10104000EFF30583034B9A6B9A6A9A6A9A6A9A6AD3 +:101050009B6AFEE700ED00E000B59BB0EFF309816D +:1010600068226846FFF74EFFEFF30583044B9A6B47 +:101070009A6A9A6A9A6A9A6A9A6A9B6AFEE700BFB3 +:1010800000ED00E000B59BB0EFF3098168226846EF +:10109000FFF738FFEFF30583034B5A6B9A6A9A6A9E +:1010A0009A6A9A6A9B6AFEE700ED00E030B50A444E +:1010B000074D91420BD011F8013B09245840013CE7 +:1010C000F7D040F300032B4083EA5000F7E730BD30 +:1010D0002083B8ED002304491A465A50C818083333 +:1010E0004260802BF9D17047C0250020024A136866 +:1010F00043F0C0031360704700780040044B5A6807 +:1011000010439A6858600AB1181D1047704700BF15 +:10111000402600202DE9F84F414EF56D2F68EC690F +:1011200021072C6219D014F0080F0CBF002080207A +:10113000E20748BF40F02000A3074FF0200348BF5C +:1011400040F04000610748BF40F4807083F311888D +:10115000FFF7D4FF002383F31188E20509D520238C +:1011600083F311884FF40070FFF7C8FF002383F367 +:1011700011884FF02009DFF8A8A04FF0000B14F001 +:10118000200832D13B0615D54FF02009DFF894A096 +:1011900020060FD589F31188504600F0F1F9002898 +:1011A00030DA0820FFF7AAFF27F080032B60002326 +:1011B00083F3118879060DD562060BD5202383F3BE +:1011C0001188F26C336D9A4201D1336C03BB00235A +:1011D00083F31188E30604D5B26E13690BB150692D +:1011E0009847BDE8F84F01F06DBB89F31188AB8CCF +:1011F000504696F86410194000F05CFA8BF31188A1 +:10120000EC69BCE780B2288588F31188EC69BFE7F8 +:1012100027F040071020FFF771FF2F60D7E700BFCE +:10122000402600207826002013B5104C204600F000 +:1012300027FA04F11400009400234FF400720C49C3 +:1012400000F0E8F804F1380000944FF40072094B04 +:10125000094900F061F9094B0C215220E365084B64 +:10126000236602B0BDE8104000F05EB840260020C2 +:10127000AC260020ED100008AC28002000780040CB +:1012800000E1F505254B002908BF1946037C012B19 +:10129000816630B511D1224B98420ED1214BD3F843 +:1012A000E82042F08042C3F8E820D3F8102142F051 +:1012B0008042C3F81021D3F810310A68036EC46D60 +:1012C00003EB5203B3FBF2F34A68150442BF23F069 +:1012D000070503F0070343EA4503E3608B6843F027 +:1012E00040036360CB68510543F00103A36042F4FF +:1012F000967343F0010323604FF0FF33236205D55B +:1013000012F0102205D0B2F1805F04D080F8643072 +:1013100030BD7F23FAE73F23F8E700BF784000089D +:10132000402600200044025800F1604300F01F02F4 +:101330000901400903F56143C9B2800083F8001335 +:10134000012300F16040934000F56140C0F8803116 +:1013500003607047F8B51546826804460B46AA42FA +:1013600000D28568A1692669761AB54207D2184667 +:101370002A46FFF7C7FDA3692B44A3610DE011D9ED +:10138000AF1B32461846FFF7BDFD3A46E1683044D0 +:10139000FFF7B8FDE2683A44A261A36828465B1BE8 +:1013A000A360F8BD18462A46FFF7ACFDE368E4E702 +:1013B000836893422DE9F04104460F46154600D25A +:1013C000856860692669361AB54207D22A463946C9 +:1013D000FFF798FD63692B4463610EE013D9A5EB19 +:1013E000060832463946FFF78DFD4246B919E068D6 +:1013F000FFF788FDE26842446261A36828465B1BF0 +:10140000A360BDE8F0812A463946FFF77BFDE3681B +:10141000E2E7000010B50A440024C361029B0060AB :1014200040608460C160816141610261036210BDFE :1014300008B582694369934201D1826882B98268A2 :10144000013282605A1C42611970426903699A42F2 @@ -332,12 +332,12 @@ :1014A00070BD3146204600F073FE0028E2DA85F375 :1014B000118870BD2DE9F74F05460F4690469A46B4 :1014C000D0F81C90202686F311884FF0000B1446AC -:1014D00064B1224639462846FFF740FF034668B903 +:1014D00064B1224639462846FFF73CFF034668B907 :1014E0005146284600F054FE0028F1D0002383F333 :1014F0001188A8EB040003B0BDE8F08FB9F1000F2C :1015000003D001902846C847019B8BF31188E41A49 -:101510001F4486F31188DBE7C16081614161C361CB -:101520001144009B0060406082600161036270476B +:101510001F4486F31188DBE7C1608161416111449A +:10152000C361009B0060406082600161036270479C :10153000F8B504460E461746202383F31188A568A4 :10154000A5B1A368013BA36063695A1C62611E7068 :10155000236962699A4224BFE3686361E3690BB15E @@ -348,7 +348,7 @@ :1015A000C3688361002100F003FE204610BD4FF0A8 :1015B000FF3010BD2DE9F74F05460F4690469A467D :1015C000D0F81C90202686F311884FF0000B1446AB -:1015D00064B1224639462846FFF7EEFE034668B955 +:1015D00064B1224639462846FFF7EAFE034668B959 :1015E0005146284600F0D4FD0028F1D0002383F3B3 :1015F0001188A8EB040003B0BDE8F08FB9F1000F2B :1016000003D001902846C847019B8BF31188E41A48 @@ -358,96 +358,96 @@ :10164000B9BF00004FF0FF333830FFF7B3BF0000E1 :101650001430FFF709BF00004FF0FF311430FFF7DF :1016600003BF00003830FFF763BF00004FF0FF32C8 -:101670003830FFF75DBF000000207047FFF7D8BD8E +:101670003830FFF75DBF000000207047FFF7D4BD92 :10168000044B0360002343608360C3600123037441 -:10169000704700BF8440000810B52023044683F340 -:1016A0001188FFF7F3FD02232374002383F31188CD +:10169000704700BF9040000810B52023044683F334 +:1016A0001188FFF7EFFD02232374002383F31188D1 :1016B00010BD000038B5C36904460D461BB90421AE :1016C0000844FFF7A9FF294604F11400FFF7B0FE14 :1016D000002806DA201D4FF48061BDE83840FFF78E :1016E0009BBF38BD026843681143016003B11847CE :1016F0007047000013B5446BD4F894341A6811781D -:10170000042915D1217C022912D1197912890123CA -:101710008B4013420CD101A904F14C0001F08EFF63 -:10172000D4F89444019B21790246206800F0EAF93C -:1017300002B010BD143001F011BF00004FF0FF33B4 -:10174000143001F00BBF00004C3001F0E3BF00008B -:101750004FF0FF334C3001F0DDBF0000143001F0DA -:10176000DFBE00004FF0FF31143001F0D9BE0000A1 -:101770004C3001F0AFBF00004FF0FF324C3001F0B1 -:10178000A9BF0000D0F8942438B51368197804294B -:10179000054601D0012038BD017C0229FAD1127919 -:1017A0005C89012090400440F4D105F1140001F05F -:1017B00071FE02460028EDD0D5F894544FF48073A2 +:10170000042915D1217C022912D1197901231289CA +:101710008B4013420CD101A904F14C0001F090FF61 +:10172000D4F894440246019B2179206800F0EAF93C +:1017300002B010BD143001F013BF00004FF0FF33B2 +:10174000143001F00DBF00004C3001F0E5BF000087 +:101750004FF0FF334C3001F0DFBF0000143001F0D8 +:10176000E1BE00004FF0FF31143001F0DBBE00009D +:101770004C3001F0B1BF00004FF0FF324C3001F0AF +:10178000ABBF0000D0F8942438B51368054619782B +:10179000042901D0012038BD017C0229FAD1127937 +:1017A00001205C8990400440F4D105F1140001F05F +:1017B00073FE02460028EDD0D5F894544FF48073A0 :1017C0002868697900F08CF9204638BD406BFFF736 :1017D000D9BF000000207047704700007FB5124B52 -:1017E0000360002343608360C360012502260F4B22 -:1017F000057404460290019300F184022946009684 -:101800004FF48073143001F021FE094B01930294D0 -:1018100000964FF4807304F52372294604F14C00BE -:1018200001F0E8FE04B070BDAC400008CD17000820 +:1017E00001250226044603600023057400F18402EB +:1017F000436029468360C3600C4B02901430019310 +:101800004FF48073009601F023FE094B04F5237218 +:101810002946019304F14C004FF480730294009622 +:1018200001F0EAFE04B070BDB8400008CD17000812 :10183000F51600080A68202383F311880B790B330F :1018400042F823004B79133342F823008B7913B10C -:101850000B3342F8230002230374C0F894140023CE +:101850000B3342F823000223C0F8941403740023CE :1018600083F311887047000038B5037F044613B135 -:1018700090F85430ABB9201D01250221FFF732FF4B +:1018700090F85430ABB90125201D0221FFF732FF4B :1018800004F1140025776FF0010100F09DFC84F84D :10189000545004F14C006FF00101BDE8384000F0F5 :1018A00093BC38BD10B5012104460430FFF71AFF80 :1018B0000023237784F8543010BD000038B5044667 -:1018C0000025143001F0DAFD04F14C00257701F019 -:1018D000A9FE201D84F854500121FFF703FF204684 +:1018C0000025143001F0DCFD04F14C00257701F017 +:1018D000ABFE201D84F854500121FFF703FF204682 :1018E000BDE83840FFF74EBF90F8803003F060034A :1018F000202B19D190F88120212A0AD0222A4FF0DA :1019000000030ED0202A0FD1084A426707228267BF :1019100004E0064B4367072383670023C367012066 :10192000704743678367F9E7002070471C24002055 -:10193000D0F8943437B51A681178042904461AD1BE -:10194000017C022917D11979128901238B40134296 -:1019500011D100F14C05284601F024FF58B101A92E -:10196000284601F06BFED4F89444019B217902468D +:10193000D0F8943437B51A680446117804291AD1BE +:10194000017C022917D11979012312898B40134296 +:1019500011D100F14C05284601F026FF58B101A92C +:10196000284601F06DFED4F894440246019B21798B :10197000206800F0C7F803B030BD000001F10B0390 :10198000F7B550F8234005460E46F4B1202383F303 -:10199000118805EB8607201D0821FFF7A3FEFB68D1 -:1019A0005B691B684C3413B1204601F055FE01A958 -:1019B000204601F043FE024648B1019B31462846CD +:10199000118805EB8607201D08214C34FFF7A2FEB5 +:1019A000FB685B691B6813B1204601F057FE01A973 +:1019B000204601F045FE024648B1019B31462846CB :1019C00000F0A0F8002383F3118803B0F0BDFB689A :1019D0005A691268002AF5D01B8A013B1340F1D1E5 :1019E00005F18002EAE70000133138B550F82140D4 :1019F000DCB1202383F31188D4F89424136852793E :101A000003EB8203DB689B695D6845B104216018C4 -:101A1000FFF768FE294604F1140001F045FD204659 +:101A1000FFF768FE294604F1140001F047FD204657 :101A2000FFF7B0FE002383F3118838BD7047000034 -:101A300001F040B8012303700023436000F130023D -:101A400000F1500142F8043B8A42D361FAD103818C +:101A300001F042B8012300F1300200F150010370BF +:101A40000023436042F8043B8A42D361FAD1038108 :101A50004381704738B50446202383F3118800255D -:101A6000416000F10C0300F1300243F8045B934243 -:101A7000FBD1204601F040F80223237085F3118842 -:101A800038BD000070B500EB810305465069DA608F -:101A90000E46144618B110220021FFF761FAA06922 -:101AA00018B110220021FFF75BFA31462846BDE845 -:101AB000704001F033B90000038903F00103038192 -:101AC000438903F00103438100F1300200F1100368 -:101AD000002143F8041B9342FBD101F0B7B9000089 -:101AE000F0B400EB81044789E4680125A4698D40C6 -:101AF0003D43458123600023A2606360F0BC01F098 -:101B0000D3B90000F0B400EB81040789E468012533 -:101B100064698D403D43058123600023A2606360BA -:101B2000F0BC01F04BBA0000022310B50370002393 -:101B30000446A0F8883080F88A3080F88B30038122 -:101B4000438100F10C0200F1300142F8043B8A426B -:101B5000FBD184F87030204601F074F863681B688C +:101A600000F10C0300F13002416043F8045B934243 +:101A7000FBD1204601F042F80223237085F3118840 +:101A800038BD000070B500EB8103054650690E4675 +:101A90001446DA6018B110220021FFF75BFAA06942 +:101AA00018B110220021FFF755FA31462846BDE84B +:101AB000704001F035B90000038900F130020021C7 +:101AC00003F001030381438903F00103438100F123 +:101AD000100343F8041B9342FBD101F0B9B9000095 +:101AE000F0B4012500EB810447898D40E4683D4353 +:101AF000A469458123600023A2606360F0BC01F00B +:101B0000D5B90000F0B4012500EB810407898D40B0 +:101B1000E4683D436469058123600023A26063603B +:101B2000F0BC01F04DBA0000022300F10C0200F1FC +:101B3000300110B5037004460023A0F8883080F807 +:101B40008A3080F88B300381438142F8043B8A421B +:101B5000FBD184F87030204601F076F863681B688A :101B600023B120460021BDE81040184710BD0000F9 -:101B7000436802781B6880F88C20052202700BB144 +:101B70000278436880F88C2005221B6802700BB144 :101B80000421184770470000436890F88C201B68B8 :101B900002700BB1052118477047000090F87030B3 :101BA00070B5044613B1002380F8703004F1800250 -:101BB000204601F06FF963689B6863BB94F880501E +:101BB000204601F071F963689B6863BB94F880501C :101BC00015F0600615D194F8813005F07F0545EADF :101BD000032540F202339D4200F00E815BD8022DB6 :101BE00000F0DC803FD8002D00F08780012D00F050 -:101BF000CF800021204601F005FC0021204601F0A5 -:101C0000F5FB63681B6813B106212046984706233D +:101BF000CF800021204601F007FC0021204601F0A3 +:101C0000F7FB63681B6813B106212046984706233B :101C100084F8703070BD204698470028CED094F8E4 :101C2000872094F8863043EA0223A26F934238BF9C :101C3000A36794F98030A56F002B4FF0200380F24A @@ -458,79 +458,79 @@ :101C8000B7D1B4F8883023F00203A4F8883066672F :101C9000A667E667C3E740F201639D421ED8B5F52B :101CA000C06F3BD2B5F5A06FA3D1B4F88030B3F5C7 -:101CB000A06F0ED194F8823084F88A30204601F06B -:101CC0001FF863681B6813B101212046984703235E +:101CB000A06F0ED194F88230204684F88A3001F06B +:101CC00021F863681B6813B101212046984703235C :101CD000237000236367A367E367A0E7B5F5106F80 :101CE00032D040F602439D4252D0B5F5006F80D10C -:101CF00004F18B036367012324E004F18803636725 -:101D00000223A367E5678AE794F88230012B7FF40A +:101CF00004F18B036367012324E004F18803E567A3 +:101D000063670223A3678AE794F88230012B7FF48C :101D100070AFB4F8883043F00203B6E794F885203A -:101D2000616894F884304D6894F8831043EA022384 -:101D3000204694F88220A84700283FF45AAF436811 +:101D20002046616894F884304D6843EA022394F8B1 +:101D3000831094F88220A84700283FF45AAF4368E4 :101D400063670368A367A4E72378042B10D12023DB :101D500083F311882046FFF7AFFE86F3118863688E -:101D600084F88B601B68032121700BB120469847D3 +:101D6000032184F88B601B6821700BB120469847D3 :101D700094F88230002BACD084F88B30042323708D :101D800063681B68002BA4D0022120469847A0E777 :101D9000374B63670223A36700239DE794F8841001 -:101DA00011F0800F204601F00F010ED001F064F811 -:101DB000012806D002287FF41CAF2E4B6367A06772 -:101DC00067E72D4B6367A56763E701F047F8EFE727 +:101DA000204611F0800F01F00F010ED001F066F80F +:101DB000012806D002287FF41CAF2E4BA067636772 +:101DC00067E72D4BA567636763E701F049F8EFE725 :101DD00094F88230002B7FF40CAF94F8843013F029 -:101DE0000F013FF476AF1A06204602D501F022FB20 -:101DF0006FE701F013FB6CE794F88230002B7FF45F +:101DE0000F013FF476AF1A06204602D501F024FB1E +:101DF0006FE701F015FB6CE794F88230002B7FF45D :101E0000F8AE94F8843013F00F013FF462AF1B0674 -:101E1000204602D501F0F6FA5BE701F0E7FA58E751 +:101E1000204602D501F0F8FA5BE701F0E9FA58E74D :101E2000142284F8702083F311882B462A46294611 :101E30002046FFF755FE85F3118870BD5DB1152270 :101E400084F8702083F311880021A36F626F20460D :101E5000FFF746FE03E70B2284F8702083F3118816 :101E60002B462A4629462046FFF74CFEE3E700BFF3 -:101E7000DC400008D4400008D840000838B590F88D +:101E7000E8400008E0400008E440000838B590F869 :101E800070300446152B29D8DFE803F03E282828B7 :101E900028283E28280B29392828282828282828B7 :101EA0003E3E90F8871090F88620836F42EA012228 :101EB0009A4214D9C268128AB3FBF2F502FB1535B7 :101EC0006DB9202383F311882B462A462946FFF754 :101ED00019FE85F311880A2384F8703038BD142365 -:101EE00084F87030202383F3118800231A461946A2 -:101EF0002046FFF7F5FD002383F3118838BDC36F3B -:101F000003B198470023E7E7002101F07BFA0021A5 -:101F1000204601F06BFA63681B6813B10621204666 +:101EE00084F87030202383F31188002320461A469B +:101EF0001946FFF7F5FD002383F3118838BDC36F42 +:101F000003B198470023E7E7002101F07DFA0021A3 +:101F1000204601F06DFA63681B6813B10621204664 :101F200098470623D8E7000090F87020152A38B5A6 :101F3000044622D80123934040F6416213421DD14A :101F400013F480150FD19B0217D50B2380F8703046 :101F5000202383F311882B462A462946FFF7D2FD1A :101F600085F3118838BDC3689B695B682BB9C36F63 :101F700003B19847002384F8703038BD002101F088 -:101F800041FA0021204601F031FA63681B6813B161 +:101F800043FA0021204601F033FA63681B6813B15D :101F90000621204698470623EDE70000024B002269 -:101FA0001B605B609A607047AC2A002000230374BA -:101FB0008268054B1B6899689142FBD25A6803609E +:101FA0001B605B609A607047AC2A00200023826847 +:101FB0000374054B1B6899689142FBD25A68036011 :101FC0004260106058607047AC2A002008B520239A :101FD00083F31188037C032B05D0042B0DD02BB980 :101FE00083F3118808BD436900221A604FF0FF3364 :101FF0004361FFF7DBFF0023F2E790E80C001A6073 -:1020000002685360F2E70000002303748268054B06 +:1020000002685360F2E70000002382680374054B06 :102010001B6899689142FBD85A680360426010605F :1020200058607047AC2A0020054B1969087418687D -:1020300002681A605360186101230374FEF7C4B983 -:10204000AC2A002030B54B1C87B005460A4C10D096 -:1020500023690A4A01A800F0D3F82846FFF7E4FFF5 +:10203000026853601A60186101230374FEF7C4B983 +:10204000AC2A00204B1C30B5054687B00A4C10D096 +:10205000236901A8094A00F0D3F82846FFF7E4FFF6 :10206000049B13B101A800F007F92369586907B070 :1020700030BDFFF7D9FFF8E7AC2A0020CD1F0008DC -:1020800038B50C4D41612B6981689A6891420446CC +:1020800038B50C4D044641612B6981689A689142CC :1020900003D8BDE83840FFF789BF1846FFF786FF31 :1020A00001232C61014623742046BDE83840FEF729 :1020B0008BB900BFAC2A0020044B1A681B699068DA :1020C0009B68984294BF002001207047AC2A0020F2 -:1020D00010B5084C236820691A68226054600122F8 +:1020D00010B5084C236820691A68546022600122F8 :1020E00023611A74FFF790FF01462069BDE8104094 :1020F000FEF76AB9AC2A0020826002220274002234 :10210000427470478368A3F17C0243F80C2C026987 :1021100043F83C2C426943F8382C074A43F81C2CFE -:10212000C26843F8102C022203F8082C002203F89E -:10213000072CA3F118007047E103000810B5202315 +:10212000C268A3F1180043F8102C022203F8082C0F +:10213000002203F8072C7047E103000810B52023A4 :1021400083F31188FFF7DEFF00210446FFF798FFB5 :10215000002383F31188204610BD0000024B1B6949 :1021600058610F20FFF760BFAC2A0020202383F3C3 @@ -538,386 +538,386 @@ :1021800011880820FFF75EFF002383F3118808BD44 :1021900049B1064B42681B6918605A60136043607E :1021A0000420FFF74FBF4FF0FF307047AC2A0020EC -:1021B0000368984206D01A68026050605961184658 +:1021B0000368984206D01A68026050601846596158 :1021C000FFF7F4BE7047000038B504460D4620689E :1021D000844200D138BD036823605C604561FFF72D :1021E000E5FEF4E7054B03F114025A619A614FF0E2 :1021F000FF32DA6100221A62704700BFAC2A002069 -:10220000F8B503611A4BC2605C6A1A4B1A46022980 -:1022100052F8145F38BF0221954206461F460AD184 +:10220000F8B5036102291A4B0646C26038BF0221A5 +:102210005C6A184B1A461F4652F8145F95420AD161 :10222000586198611C620560456081600819BDE8CD -:10223000F84001F0D9BA186AAB68241A0C1902D315 -:10224000E41A2D6804E09C4202D2204401F0DCFA3A -:10225000AB689C42F4D86B68736035601E606E603A -:10226000B460A9684FF0FF33091BA960FB61F8BD9A +:10223000F84001F0DBBA186AAB68241A0C1902D313 +:10224000E41A2D6804E09C4202D2204401F0DEFA38 +:10225000AB689C42F4D86B68356073601E604FF0C9 +:10226000FF336E60B460A968091BA960FB61F8BD0B :10227000000C0040AC2A002010B41A4C234653F83E :10228000141F814210D0416802680A6002685160E0 :102290009A424FF00001C16003D0936881680B44FB -:1022A00093605DF8044B70470A68626100209A42AF -:1022B0005360C86003D15DF8044B01F09FBA936886 -:1022C000886803449360084A206A526A121A93424B -:1022D000E7D9991A012998BF931C18445DF8044B5B -:1022E00001F092BAAC2A0020000C00400020704798 +:1022A00093605DF8044B70470A6800209A426261AF +:1022B0005360C86003D15DF8044B01F0A1BA936884 +:1022C00088680344206A9360074A526A121A93424C +:1022D000E7D9991A5DF8044B012998BF931C18445B +:1022E00001F094BAAC2A0020000C00400020704796 :1022F000FEE70000704700004FF0FF30704700001D :10230000022906D0032906D00129064818BF00205B :10231000704705487047032A9ABF044800EBC20083 -:1023200000207047B04100086441000824240020C8 -:1023300070B59AB001AD064608462946144600F02D -:1023400095F82846FEF704FEC0B2431C5B0086E801 -:102350001800237003236370002302341946DAB295 -:10236000904204F1020401D81AB070BDEA5C04F88E -:10237000022C04F8011C0133F1E7000008B520230A +:1023200000207047BC4100087041000824240020B0 +:1023300070B59AB006460846144601AD294600F02D +:1023400095F82846FEF7FEFDC0B2431C5B0086E808 +:10235000180023700323023404F8013C00231946BB +:10236000DAB20234904201D81AB070BDEA5C01338F +:1023700004F8011C04F8022CF2E7000008B5202341 :1023800083F311880348FFF7D3FA002383F31188FE :1023900008BD00BFEC2A002010B50446052916D858 :1023A000DFE801F016150316161D202383F31188AC :1023B0000E4A0121FFF766FB20460D4A0221FFF776 :1023C00061FB0C48FFF77AFA002383F3118810BDF4 :1023D000202383F311880748FFF746FAF4E7202308 -:1023E00083F311880348FFF75DFAEDE7E04000084A -:1023F00004410008EC2A002038B50C4D0C4C0D4966 -:102400002A4604F10800FFF793FF05F1CA0204F120 +:1023E00083F311880348FFF75DFAEDE7EC4000083E +:1023F00010410008EC2A002038B50C4D0C4C2A4640 +:102400000C4904F10800FFF793FF05F1CA0204F13B :1024100010000949FFF78CFF05F5CA7204F1180096 :102420000649BDE83840FFF783BF00BFB42F002046 -:1024300024240020304100083D4100084B410008A1 -:1024400070B5044608460D46FEF782FDC6B220462A +:10243000242400203C41000849410008574100087D +:1024400070B5044608460D46FEF77CFDC6B2204630 :10245000013403780BB9184670BD32462946FEF7A1 -:102460005FFD0028F3D1012070BD00002DE9F84F79 -:1024700005460C46FEF76CFD2B49C6B22846FFF711 -:10248000DFFF08B10336F6B228492846FFF7D8FF28 -:1024900008B11036F6B2632E0DD8DFF88C80DFF865 -:1024A0008C90234FDFF890A0DFF890B02E7846B9DB +:102460005BFD0028F3D1012070BD00002DE9F84F7D +:1024700005460C46FEF766FD2C49C6B22846FFF716 +:10248000DFFF08B10336F6B229492846FFF7D8FF27 +:1024900008B11036F6B2632E0DD8DFF89080DFF861 +:1024A0009090244FDFF894A0DFF894B02E7846B9CE :1024B0002670BDE8F88F29462046BDE8F84F01F0A8 -:1024C000AFBD252E2CD1072241462846FEF728FD18 -:1024D00058B9DBF800302360DBF804306360BBF8E8 -:1024E0000830238107350A34E0E7082249462846A8 -:1024F000FEF716FDA0B90F4BA21C13F8011F090926 -:102500005345C95D02F8021C197801F00F0102F170 -:102510000202C95D02F8031CEFD118340835C5E783 -:10252000267001350134C1E7D04100084B41000855 -:10253000E3410008FFE7F11F0BE8F11FD841000855 -:10254000BFF34F8F044B1A695107FCD1D3F8102108 -:102550005207F8D1704700BF0020005208B50D4B5C -:102560001B78ABB9FFF7ECFF0B4BDA68D10741BF23 -:102570000A4A5A6002F188325A60D3F80C21D20715 -:1025800041BF064AC3F8042102F18832C3F804218E -:1025900008BD00BF12320020002000522301674511 -:1025A00008B5114B1B78F3B9104B1A69510742BF9C -:1025B000DA6842F04002DA60D3F81021520742BFD5 -:1025C000D3F80C2142F04002C3F80C21FFF7B8FF0A -:1025D000064BDA6842F00102DA60D3F80C2142F0CF -:1025E0000102C3F80C2108BD123200200020005265 -:1025F0000F289ABF00F5806040040020704700005B -:102600004FF4003070470000102070470F2808B5C5 -:102610000BD8FFF7EDFF00F500330268013204D15B -:1026200004308342F9D1012008BD002008BD00001C -:102630000F2810B504463FD8FFF782FFFFF78EFF43 -:102640001E484FF0FF33072C4361C0F814311DD8EA -:102650000361FFF775FF230243F02403C360C368DF -:1026600043F08003C36003695A07FCD4FFF768FF97 -:102670002046FFF7BDFF4FF4003100F0EDF8FFF703 -:102680008FFF2046BDE81040FFF7C0BFC0F81031F3 -:10269000FFF756FFA4F108031B0243F02403C0F820 -:1026A0000C31D0F80C3143F08003C0F80C31D0F875 -:1026B00010315B07FBD4D9E7002010BD0020005289 -:1026C0002DE9F84F40EA0203DB06064688469146AC -:1026D0004AD1FFF743FFDFF8B0B04546A9EB050349 -:1026E00043441F2B04D8FFF75BFF0120BDE8F88FA0 -:1026F000204A2148214F06F178439342204B214A3A -:1027000098BF9A4603F1FC0386BF1F46924658467F -:10271000FFF716FF4FF0FF3303603B6843F00203FF -:102720003B6005F11C022B1FA6EB050EDAF80040FA -:1027300014F00504FAD153F8041F4EF80310934225 -:10274000F4D1BFF34F8FFFF7FBFE4FF0FF33036071 -:102750003B6823F002033B6020222946304601F00B -:102760004FFC20B1FFF71CFF0020BDE8F88F20369A -:102770002035B3E7FFFF0F00142100520C20005258 -:1027800010200052102100521420005210B5084CA5 -:10279000237828B153B9FFF7E1FE0123237010BD60 -:1027A00023B12070BDE81040FFF7FABE10BD00BF96 -:1027B0001232002010B50244064B0439D2B29042C6 -:1027C00000D110BD441C00B253F8200041F8040FA2 -:1027D000E0B2F4E7504000580F4B30B51C6F2404B2 -:1027E00005D41C6F1C671C6F44F400441C670B4C21 -:1027F000236843F4807323600244094B0439D2B246 -:10280000904200D130BD441C00B251F8045F43F83F -:102810002050E0B2F4E700BF0044025800480258DC -:102820005040005807B5012201A90020FFF7C2FF60 -:10283000019803B05DF804FB13B50446FFF7F2FFFF -:10284000A04206D002A9012241F8044D0020FFF762 -:10285000C3FF02B010BD00000144BFF34F8F064B11 -:10286000884204D3BFF34F8FBFF36F8F7047C3F815 -:102870005C022030F4E700BF00ED00E0034B1B6872 -:102880005B0142BF024B01221A707047D0440258CC -:1028900013320020014B1878704700BF133200201C -:1028A000064A536823F001035360EFF3098368334A -:1028B00083F30988002383F31188704730EF00E029 -:1028C00010B5202383F31188104B5B6813F4006369 -:1028D00018D0F1EE103AEFF309844FF0807344F80A -:1028E0004C3C0B4BDB6844F8083CA4F1680383F3D1 -:1028F0000988FFF7E1FB18B1064B44F8503C10BDC6 -:10290000054BFAE783F3118810BD00BF00ED00E02E -:1029100030EF00E0F1030008F4030008F0B5BFF366 -:102920004F8FBFF36F8F1D4B0021C3F85012BFF3C1 -:102930004F8FBFF36F8F5A6942F400325A61BFF371 -:102940004F8FBFF36F8FC3F88410BFF34F8FD3F84F -:102950008020C2F3C904C2F34E325201A50743F6E8 -:10296000E07602EA060E284621464EEA00070139C3 -:10297000C3F860724F1C00F14040F6D1203A12F1CA -:10298000200FEED1BFF34F8F5A6942F480325A6163 -:10299000BFF34F8FBFF36F8FF0BD00BF00ED00E0BE -:1029A000FEE70000084A094B09498B4204D3094A53 -:1029B0000021934205D3704752F8040F43F8040BEB -:1029C000F3E743F8041BF4E7D04300086035002028 -:1029D0006035002060350020D0F8943030B50022FA -:1029E0009D68D0F8904011464FF0FF3004EB421341 -:1029F00001329542C3F80019C3F81019C3F8080949 -:102A0000C3F8001BC3F8101BC3F8080BEED24FF03D -:102A10000113C4F81C3830BD00EB81032DE9F04FE1 -:102A2000D3F80CE0DEF814404F1CD4F800C03F038C -:102A30004FEA41186568D0F8902065450AD3D2F86E -:102A40003438012000FA01F123EA0101C2F83418F8 -:102A5000BDE8F08FBEF81060ACEB0503B34228BFB1 -:102A6000334602EB0806D6F81869B6B2B3EB860F08 -:102A700013D8A6683A44A6F1040A99465AF804BF46 -:102A8000C2F800B0B9F1040F02D9A9F10409F5E7C1 -:102A90001E442B44A6606360CCE70020BDE8F08FA5 -:102AA000890141F02001016103699B06FCD41220D9 -:102AB00000F03CBF10B50A4C2046FEF7BBFF094BA7 -:102AC000C4F89030084BC4F89430084C2046FEF708 -:102AD000B1FF074BC4F89030064BC4F8943010BDDA -:102AE000143200200000084018420008B0320020D4 -:102AF00000000440244200080378012B70B505460D -:102B00005DD1494BD0F89040984259D1474BD3F80A -:102B1000D82042F00062C3F8D820D3F8002142F058 -:102B20000062C3F80021D3F80021D3F8802042F0DE -:102B30000062C3F88020D3F8802022F00062C3F83E -:102B40008020D3F880300E216520FEF7F1FB384B52 -:102B5000E360384BC4F800380023C4F8003EC023BB -:102B60002360D5F890604FF40413A3633369002BFE -:102B7000FCDA012333610C2000F0D8FE3369DB0757 -:102B8000FCD4122000F0D2FE3369002BFCDA0026C0 -:102B9000A6602846FFF720FF6B68C4F81068DB6862 -:102BA000C4F81468C4F81C68002B3AD1224BA36106 -:102BB0004FF0FF336361A36843F00103A36070BD6E -:102BC0001E4B9842C8D1194BD3F8D82042F000725E -:102BD000C3F8D820D3F8002142F00072C3F80021D6 -:102BE000D3F80021D3F8802042F00072C3F880208F -:102BF000D3F8802022F00072C3F88020D3F8802020 -:102C0000D3F8D82022F08062C3F8D820D3F800216E -:102C100022F08062C3F80021D3F800310E214D204C -:102C200093E7074BC3E700BF14320020004402586B -:102C30004014004003002002003C30C0B0320020AD -:102C4000083C30C0F8B5D0F8904005460021204639 -:102C5000FFF726FFD5F8941000234FF001128F687C -:102C6000C4F834384FF00066C4F81C284FF0FF3029 -:102C700004EB431201339F42C2F80069C2F8006BB3 -:102C8000C2F80809C2F8080BF2D20B68D5F89020F8 -:102C9000C5F89830636210231361166916F01006A8 -:102CA000FBD1122000F042FED4F8003823F4FE637A -:102CB000C4F80038A36943F4402343F01003A36130 -:102CC0000923C4F81038C4F814380B4BEB604FF0EC -:102CD000C043C4F8103B094BC4F8003BC4F810696A -:102CE000C4F80039D5F8983003F1100243F480138A -:102CF000C5F89820A362F8BDF44100084080001098 -:102D0000D0F8902090F88A10D2F8003823F4FE63AF -:102D100043EA0113C2F80038704700002DE9F04182 -:102D20000EB200EB86080D46D8F80C100F6807F0BD -:102D30000303022B53D0032B53D03F4A3F4F012BA9 -:102D400018BF1746D0F890404FEA451E04EB0E031B -:102D50000022C3F8102B8A6905F1100C002A42D01A -:102D60004A8A05F158035B013A43E2500123D4F843 -:102D70001C2803FA0CF31343A6444A69C4F81C3810 -:102D80000023CEF8103905F13F03002A3BD00A8A10 -:102D9000898B9208012988BF4A43D0F8981004EB28 -:102DA0008303561841EA0242C0F8986029465A60E7 -:102DB0002046FFF775FED8F80C301B8A43EA85538E -:102DC0001F4305F148035B01E7500123D4F81C2899 -:102DD00003FA05F51543C4F81C58BDE8F081184FF7 -:102DE000B0E7184FAEE704EB4613D3F8002B22F4FC -:102DF0000042C3F8002BD4F81C28012303FA0CF37B -:102E000022EA0303B8E704EB83030F4A5A6004EB9A -:102E1000461629462046FFF743FED6F8003923F42C -:102E20000043C6F80039D4F81C38012202FA05F52F -:102E300023EA0505CFE700BF0080001000800410E2 -:102E40000080081000800C1000040002D0F89420CC -:102E50001268C0F89820FFF7BFBD00005831D0F8C5 -:102E6000903049015B5813F4004004D013F4001F64 -:102E700014BF0120022070474831D0F8903049013A -:102E80005B5813F4004004D013F4001F14BF01205A -:102E90000220704700EB8101CB68196A0B68136050 -:102EA0004B6853607047000000EB810330B5DD686C -:102EB000AA691368D36019B9402B84BF40231360FB -:102EC0006B8A1468D0F890201C44013CB4FBF3F4E6 -:102ED0006343033323F0030302EB411043EAC4438B -:102EE00043F0C043C0F8103B2B6803F00303012BF1 -:102EF00009B20ED1D2F8083802EB411013F4807FEA -:102F0000D0F8003B14BF43F0805343F00053C0F8A7 -:102F1000003B02EB4112D2F8003B43F00443C2F8FD -:102F2000003B30BD2DE9F041D0F8906006EB411335 -:102F30000546D3F8087BC3F8087B3A070C4608D54A -:102F4000D6F814381B0704D500EB8103DB685B68F7 -:102F50009847FA072FD5D6F81438DB072BD505EBA1 -:102F60008403D968CCB98B69488A5E68B6FBF0F2F5 -:102F700000FB12628AB91868DA6890420DD2121A00 -:102F800083E81400202383F3118821462846FFF7A5 -:102F90008BFF84F31188BDE8F081012303FA04F26A -:102FA0006B8923EA02036B81CB6823B12146284653 -:102FB000BDE8F0411847BDE8F081000000EB810357 -:102FC00070B5DD68D0F890306C692668E6604A011B -:102FD00056BB1A444FF40020C2F810092A6802F0C8 -:102FE0000302012A0AB20ED1D3F8080803EB4214F7 -:102FF00010F4807FD4F8000914BF40F0805040F0F6 -:103000000050C4F8000903EB4212D2F8000940F066 -:103010000440C2F80009D3F83408012202FA01F191 -:103020000143C3F8341870BD19B9402E84BF402045 -:10303000206020682E8A841940F00050013C1A4418 -:10304000B4FBF6F440EAC440C6E700002DE9F041C5 -:10305000D0F8906006EB41130446D3F80879C3F822 -:103060000879FB070D461CD5D6F81038DA0718D5B5 -:1030700000EB8103D3F80CE0DEF81430D3F800C085 -:10308000DA6894451BD2A2EB0C024FF000081A60DC -:10309000C3F80480202383F31188FFF78FFF88F3A0 -:1030A00011883B0618D5D6F834280123AB401342CB -:1030B00012D029462046BDE8F041FFF7ADBC012300 -:1030C00003FA01F2038923EA02030381DEF80830E0 -:1030D000002BE6D09847E4E7BDE8F0812DE9F04702 -:1030E000D0F890506E69AB691E40F10404466E61E1 -:1030F00003D5BDE8F047FEF717BD002E11DAD5F86D -:10310000003E9A071EBFD5F8003E23F00303C5F822 -:10311000003ED5F8043823F00103C5F80438FEF763 -:1031200033FD330502D52046FEF722FDB7040CD54A -:10313000D5F8083813F0060FEB6823F470530CBF72 -:1031400043F4105343F4A053EB60300704D5636895 -:10315000DB680BB120469847F10200F1A180B20272 -:103160000BD5D4F8908000274FF00109D4F89430A3 -:103170009B68F9B28B4280F0D280F3061AD5D4F85E -:1031800090100A6AC2F30A1702F00F0302F4F01259 -:10319000B2F5802F00F0EC80B2F5402F0AD104EB9D -:1031A000830301F58051DB68186A00231A469F42A9 -:1031B00040F0D1803003D5F8185835D5E90303D550 -:1031C00000212046FFF7AEFEAA0303D501212046C9 -:1031D000FFF7A8FE6B0303D502212046FFF7A2FEEE -:1031E0002F0303D503212046FFF79CFEE80203D5F9 -:1031F00004212046FFF796FEA90203D505212046AB -:10320000FFF790FE6A0203D506212046FFF78AFEEB -:103210002B0203D507212046FFF784FEEF0103D5DB -:1032200008212046FFF77EFE700340F1C980E907C0 -:1032300003D500212046FFF709FFAA0703D5012186 -:103240002046FFF703FF6B0703D502212046FFF757 -:10325000FDFE2F0703D503212046FFF7F7FEEE06FC -:1032600003D504212046FFF7F1FEA80603D505216A -:103270002046FFF7EBFE690603D506212046FFF73F -:10328000E5FE2A0603D507212046FFF7DFFEEB0502 -:1032900040F1968020460821BDE8F047FFF7D6BEF2 -:1032A000D4F890904FF000084FF0010AD4F8943011 -:1032B0009B685FFA88F7BB42FFF451AF09EB4713F5 -:1032C000D3F8002902F44022B2F5802F24D1D3F89C -:1032D0000029002A20DAD3F8002942F09042C3F8EE -:1032E0000029D3F80029002AFBDB3946D4F89000E6 -:1032F000FFF7D6FB22890AFA07F322EA03032381A8 -:1033000004EB8703DB689B6813B139462046984776 -:1033100039462046FFF780FB08F10108C6E708EBB5 -:103320004112D2F8003B03F44023B3F5802F10D1B3 -:10333000D2F8003B002B0CDA628909FA01F322EA89 -:103340000303638104EB8103DB68DB680BB1204678 -:10335000984701370AE713F0030F08BF0A68072BE5 -:1033600098BF027003F101039CBF120A01301EE7EF -:1033700004EB830301F58051DA6890690268D0F8A4 -:1033800008C04068A2EB000E00221046974208D108 -:10339000DB689B699A683A449A605A6817445F6090 -:1033A00008E712F0030F08BF0868964588BF8CF83D -:1033B000000002F1010284BF000A0CF1010CE5E7F4 -:1033C000BDE8F08708B50348FFF788FEBDE8084070 -:1033D000FFF776BA1432002008B50348FFF77EFEE7 -:1033E000BDE80840FFF76CBAB0320020D0F890304A -:1033F00003EB4111D1F8003B43F40013C1F8003B4B -:1034000070470000D0F8903003EB4111D1F800393B -:1034100043F40013C1F8003970470000D0F8903031 -:1034200003EB4111D1F8003B23F40013C1F8003B3A -:1034300070470000D0F8903003EB4111D1F800390B -:1034400023F40013C1F800397047000030B5039C25 -:103450000172043304FB0325C361049B0363002151 -:10346000059B00604060C160426102618561046249 -:1034700042628162C162436330BD00000022416A42 -:1034800041610161C2608262C2626FF00101FEF7B8 -:103490009BBE000003694269934203D1C2680AB12E -:1034A00000207047181D704703691960C268013217 -:1034B000C260C269134482690361934224BF436AB4 -:1034C00003610021FEF774BE38B504460D46E3687B -:1034D0003BB16269131D1268A3621344E3620020CA -:1034E00038BD237A33B929462046FEF751FE00281D -:1034F000EDDA38BD6FF00100FBE70000C368C26978 -:10350000013BC3604369134482694361934224BF12 -:10351000436A436100238362036B03B1184770471A -:1035200070B52023044683F31188866A3EB9FFF7FD -:10353000CBFF054618B186F31188284670BDA36AF3 -:10354000E26A13F8015BA362934202D32046FFF7BD -:10355000D5FF002383F31188EFE700002DE9F84F32 -:1035600004460E4690469946202787F31188002589 -:10357000AA46D4F828B0BBF1000F09D1494620462D -:10358000FFF7A2FF20B18BF311882846BDE8F88F22 -:10359000A16AE36AA8EB050B5B1A9B4528BF9B4613 -:1035A000BBF1400F1BD9334601F1400251F8040B27 -:1035B00043F8040B9142F9D1A36A40334036A36229 -:1035C0004035A26AE36A9A4202D32046FFF796FF8B -:1035D0008AF311884545D8D287F31188C9E7304668 -:1035E0005A46FDF793FCA36A5B445E44A3625D44C4 -:1035F000E7E7000010B5029C0172043303FB0421CD -:10360000C36100238362C362039B0363049B006066 -:103610004060C4604261026181610462426243634E -:1036200010BD0000026AC260426A4261026100226B -:103630008262C2626FF00101FEF7C6BD4369026992 -:103640009A4203D1C2680AB100207047184650F868 -:10365000043B0B6070470000C368C2690133C3605C -:103660004369134482694361934224BF436A4361BF -:103670000021FEF79DBD000038B504460D46E36805 -:103680003BB123691A1DA262E2691344E362002080 -:1036900038BD237A33B929462046FEF779FD002844 -:1036A000EDDA38BD6FF00100FBE700000369196037 -:1036B000C268013AC260C2691344826903619342DD -:1036C00024BF436A036100238362036B03B118477D -:1036D0007047000070B5202304460E4683F311881E -:1036E000856A35B91146FFF7C7FF10B185F3118818 -:1036F00070BDA36A1E70A36AE26A01339342A3629B -:1037000004D3E16920460439FFF7D0FF002080F39D -:10371000118870BD2DE9F84F04460D4691469A4632 -:103720004FF0200888F311880026B346A76A4FB9E6 -:1037300051462046FFF7A0FF20B187F3118830469D -:10374000BDE8F88FA06AE76AA9EB06033F1A9F421B -:1037500028BF1F46402F1BD905F1400355F8042B05 -:1037600040F8042B9D42F9D1A36A4033A36240364E -:10377000A26AE36A9A4204D3E16920460439FFF75A -:1037800095FF8BF311884E45D9D288F31188CDE788 -:1037900029463A46FDF7BAFBA36A3B443D44A3627F -:1037A0003E44E5E7026943699A4203D1C3681BB905 -:1037B000184670470023FBE7836A002BF8D0043BD0 -:1037C0009B1AF5D01360C368013BC360C3691A44F8 -:1037D000836902619A4224BF436A036100238362C2 -:1037E0000123E5E700F052B9034B002258631A6148 -:1037F0000222DA60704700BF000C0040014B00223B -:10380000DA607047000C0040014B5863704700BFFE -:10381000000C0040FEE7000010B5194CFEF7BEFB9F -:10382000FEF7E0FC802217492046FEF765FC0123E5 -:1038300044F8180C0374144B144AD96821F4E0615D -:103840000904090C0A431249DA60CA6842F080721E -:10385000CA60104A1049C2F8B01F116841F0010156 -:1038600011601022DA77202283F82220002383F3CC -:10387000118862B60948BDE81040FEF75FBC00BF82 -:10388000D42A00203042000800ED00E00003FA05D1 -:10389000F0ED00E0001000E055CEACC53842000865 -:1038A0002DE9F84F1F4CDFF88090656904F114078B -:1038B0004FF00008D9F82430266AAA689E1B964269 -:1038C0001DD34FF0200AAA68236AD5F80CB0134420 -:1038D00023622B68BB425F60A6EB02066361C5F8FA -:1038E0000C8001D1FFF78AFF88F311882869D84737 -:1038F0008AF311886569AB689E42E4D2DAE76269AF -:10390000BA420CD0916823628E1B9660A868022888 -:103910002CBF1818981CBDE8F84FFFF775BFBDE81D -:10392000F88F00BFAC2A0020000C0040034B596800 +:1024C000B5BD252E2ED1072241462846FEF724FD14 +:1024D00070B9DBF8003007350A3444F80A3CDBF801 +:1024E000043044F8063CBBF8083024F8023CDDE731 +:1024F000082249462846FEF70FFD98B9A21C0E4B4C +:1025000013F8011F023209095345C95D02F8041C82 +:10251000197801F00F01C95D02F8031CF0D11834DD +:102520000835C3E7267001350134BFE7DC410008F8 +:1025300057410008EF410008FFE7F11F0BE8F11FCA +:10254000E4410008BFF34F8F044B1A695107FCD1D7 +:10255000D3F810215207F8D1704700BF0020005275 +:1025600008B50D4B1B78ABB9FFF7ECFF0B4BDA68E6 +:10257000D10704D50A4A5A6002F188325A60D3F86A +:102580000C21D20706D5064AC3F8042102F188328D +:10259000C3F8042108BD00BF123200200020005201 +:1025A0002301674508B5114B1B78F3B9104B1A6925 +:1025B000510703D5DA6842F04002DA60D3F81021FF +:1025C000520705D5D3F80C2142F04002C3F80C2184 +:1025D000FFF7B8FF064BDA6842F00102DA60D3F881 +:1025E0000C2142F00102C3F80C2108BD1232002078 +:1025F000002000520F289ABF00F5806040040020A0 +:10260000704700004FF40030704700001020704702 +:102610000F2808B50BD8FFF7EDFF00F5003302686F +:10262000013204D104308342F9D1012008BD0020D9 +:1026300008BD00000F2810B504463FD8FFF782FF01 +:10264000FFF78EFF1E484FF0FF33072C4361C0F8A1 +:1026500014311DD80361FFF775FF230243F02403F3 +:10266000C360C36843F08003C36003695A07FCD4A6 +:10267000FFF768FF2046FFF7BDFF4FF4003100F081 +:10268000EDF8FFF78FFF2046BDE81040FFF7C0BF11 +:10269000C0F81031FFF756FFA4F108031B0243F006 +:1026A0002403C0F80C31D0F80C3143F08003C0F89B +:1026B0000C31D0F810315B07FBD4D9E7002010BDF6 +:1026C000002000522DE9F84F40EA020306468846F2 +:1026D0009146DB064AD14546DFF8B0B0FFF740FF30 +:1026E000A9EB050343441F2B04D8FFF75BFF012030 +:1026F000BDE8F88F06F178431F4A20489342204BEB +:10270000204A98BF9A4603F1FC031F4F86BF9246AA +:102710001F465846FFF716FF4FF0FF3305F11C0226 +:10272000A6EB050E03603B6843F002033B602B1FE2 +:10273000DAF8004014F00504FAD153F8041F93426C +:102740004EF80310F4D1BFF34F8FFFF7FBFE4FF0AD +:10275000FF3320222946036030463B6823F0020302 +:102760003B6001F053FC20B1FFF71CFF0020BDE8E7 +:10277000F88F20362035B3E7FFFF0F0014210052F9 +:1027800010200052102100520C2000521420005240 +:1027900010B5084C237828B153B9FFF7E1FE0123A7 +:1027A000237010BD23B12070BDE81040FFF7FABEC2 +:1027B00010BD00BF1232002002440439064BD2B2D1 +:1027C00010B5904200D110BD441C00B253F8200057 +:1027D00041F8040FE0B2F4E7504000580F4B30B519 +:1027E0001C6F240405D41C6F1C671C6F44F4004448 +:1027F0001C670B4C024404392368D2B243F4807343 +:102800002360084B904200D130BD441C51F8045F56 +:1028100000B243F82050E0B2F4E700BF0044025891 +:10282000004802585040005807B5012201A9002075 +:10283000FFF7C2FF019803B05DF804FB13B504462F +:10284000FFF7F2FFA04206D002A90122002041F8C2 +:10285000044DFFF7C3FF02B010BD00000144BFF3F9 +:102860004F8F064B884204D3BFF34F8FBFF36F8F58 +:102870007047C3F85C022030F4E700BF00ED00E0D1 +:10288000034B1B685B0142BF0122024B1A70704769 +:10289000D044025813320020014B1878704700BF13 +:1028A00013320020064A536823F001035360EFF30C +:1028B0000983683383F30988002383F31188704701 +:1028C00030EF00E010B5202383F31188104B5B68D4 +:1028D00013F4006318D0F1EE103AEFF309844FF0CF +:1028E000807344F84C3C0B4BDB6844F8083CA4F183 +:1028F000680383F30988FFF7DFFB18B1064B44F840 +:10290000503C10BD054BFAE783F3118810BD00BFA2 +:1029100000ED00E030EF00E0F1030008F4030008F0 +:10292000F0B5BFF34F8FBFF36F8F1D4B0021C3F87E +:102930005012BFF34F8FBFF36F8F5A6942F40032CA +:102940005A61BFF34F8FBFF36F8FC3F88410BFF38B +:102950004F8FD3F8802043F6E076C2F3C904C2F368 +:102960004E32A507520102EA060E284621464EEADB +:102970000007013900F14040C3F860724F1CF6D1E6 +:10298000203A12F1200FEED1BFF34F8F5A6942F473 +:1029900080325A61BFF34F8FBFF36F8FF0BD00BF1E +:1029A00000ED00E0FEE70000084A094B09498B42B0 +:1029B00004D3094A0021934205D3704752F8040F0B +:1029C00043F8040BF3E743F8041BF4E7D84300088B +:1029D000603500206035002060350020D0F894304C +:1029E000002230B51146D0F890409D684FF0FF307E +:1029F00004EB421301329542C3F80019C3F81019D1 +:102A0000C3F80809C3F8001BC3F8101BC3F8080B70 +:102A1000EED24FF00113C4F81C3830BD00EB810337 +:102A20002DE9F04FD3F80CE04F1C4FEA4118DEF8C7 +:102A300014403F03D4F800C06568D0F89020654585 +:102A40000AD30120D2F8343800FA01F123EA010157 +:102A5000C2F83418BDE8F08FACEB0503BEF8106087 +:102A6000B34228BF334602EB0806D6F81869B6B25F +:102A7000B3EB860F13D8A6683A449946A6F1040A28 +:102A80005AF804BFB9F1040FC2F800B002D9A9F195 +:102A90000409F5E71E442B44A6606360CCE70020E0 +:102AA000BDE8F08F890141F02001016103699B06B7 +:102AB000FCD4122000F03ABF10B50A4C2046FEF7B5 +:102AC000B9FF094BC4F89030084BC4F89430084C57 +:102AD0002046FEF7AFFF074BC4F89030064BC4F812 +:102AE000943010BD14320020000008402442000839 +:102AF000B032002000000440304200080378012B6F +:102B000070B505465DD1494BD0F89040984259D1F7 +:102B1000474B0E216520D3F8D82042F00062C3F85D +:102B2000D820D3F8002142F00062C3F80021D3F886 +:102B30000021D3F8802042F00062C3F88020D3F84F +:102B4000802022F00062C3F88020D3F88030FEF7A6 +:102B5000EBFB384BE360384BC4F800380023D5F862 +:102B60009060C4F8003EC02323604FF40413A363B5 +:102B70003369002BFCDA01230C20336100F0D6FE10 +:102B80003369DB07FCD4122000F0D0FE3369002B40 +:102B9000FCDA00262846A660FFF720FF6B68C4F821 +:102BA0001068DB68C4F81468C4F81C68002B3AD1BC +:102BB000224BA3614FF0FF336361A36843F001032D +:102BC000A36070BD1E4B9842C8D1194B0E214D20F9 +:102BD000D3F8D82042F00072C3F8D820D3F80021EF +:102BE00042F00072C3F80021D3F80021D3F880200E +:102BF00042F00072C3F88020D3F8802022F00072E7 +:102C0000C3F88020D3F88020D3F8D82022F0806247 +:102C1000C3F8D820D3F8002122F08062C3F8002145 +:102C2000D3F8003193E7074BC3E700BF143200200D +:102C3000004402584014004003002002003C30C011 +:102C4000B0320020083C30C0F8B5D0F890400546BE +:102C500000214FF000662046FFF724FFD5F89410BE +:102C600000234FF001128F684FF0FF30C4F8343862 +:102C7000C4F81C2804EB431201339F42C2F80069D8 +:102C8000C2F8006BC2F80809C2F8080BF2D20B6850 +:102C9000D5F89020C5F89830636210231361166947 +:102CA00016F01006FBD1122000F040FED4F80038D8 +:102CB00023F4FE63C4F80038A36943F4402343F0CF +:102CC0001003A3610923C4F81038C4F814380B4B5F +:102CD000EB604FF0C043C4F8103B094BC4F8003B15 +:102CE000C4F81069C4F80039D5F8983003F110021F +:102CF00043F48013C5F89820A362F8BD0042000891 +:102D000040800010D0F8902090F88A10D2F8003857 +:102D100023F4FE6343EA0113C2F800387047000051 +:102D20002DE9F0410EB20D4600EB8608D8F80C10E4 +:102D30000F6807F00303022B53D0032B53D03F4AF5 +:102D40003F4F012B18BF1746D0F890404FEA451E61 +:102D5000002205F1100C04EB0E03C3F8102B8A6956 +:102D6000002A42D04A8A05F158033A435B01E250F7 +:102D70000123D4F81C2803FA0CF31343C4F81C38BD +:102D8000A64400234A69CEF8103905F13F03002A12 +:102D90003BD00A8A04EB8303898B9208012988BF00 +:102DA0004A43D0F89810561841EA02422946C0F822 +:102DB000986020465A60FFF775FED8F80C301B8AE1 +:102DC00043EA85531F4305F148035B01E7500123A4 +:102DD000D4F81C2803FA05F51543C4F81C58BDE8BF +:102DE000F081184FB0E7184FAEE704EB4613D3F865 +:102DF000002B22F40042C3F8002B0123D4F81C2836 +:102E000003FA0CF322EA0303B8E704EB83030F4A47 +:102E100004EB461629465A602046FFF743FED6F8D3 +:102E20000039012223F4004302FA05F5C6F80039FF +:102E3000D4F81C3823EA0505CFE700BF0080001056 +:102E4000008004100080081000800C1000040002B4 +:102E5000D0F894201268C0F89820FFF7BFBD00009A +:102E60005831D0F8903049015B5813F4004004D039 +:102E700013F4001F14BF0120022070474831D0F81E +:102E8000903049015B5813F4004004D013F4001F44 +:102E900014BF01200220704700EB8101CB68196A42 +:102EA0000B6813604B6853607047000000EB8103B0 +:102EB00030B5DD68AA691368D36019B9402B84BFA7 +:102EC000402313606B8A1468D0F890201C4402EBF6 +:102ED0004110013C09B2B4FBF3F46343033323F024 +:102EE000030343EAC44343F0C043C0F8103B2B68DC +:102EF00003F00303012B0ED1D2F8083802EB411086 +:102F000013F4807FD0F8003B14BF43F0805343F0AC +:102F10000053C0F8003B02EB4112D2F8003B43F0F3 +:102F20000443C2F8003B30BD2DE9F041D0F8906079 +:102F300005460C4606EB4113D3F8087B3A07C3F865 +:102F4000087B08D5D6F814381B0704D500EB81039D +:102F5000DB685B689847FA072FD5D6F81438DB078B +:102F60002BD505EB8403D968CCB98B69488A5E6898 +:102F7000B6FBF0F200FB12628AB91868DA68904278 +:102F80000DD2121A83E81400202383F311882146FE +:102F90002846FFF78BFF84F31188BDE8F0810123F9 +:102FA00003FA04F26B8923EA02036B81CB6823B135 +:102FB00021462846BDE8F0411847BDE8F0810000F1 +:102FC00000EB81034A0170B5DD68D0F890306C6980 +:102FD0002668E66056BB1A444FF40020C2F8100978 +:102FE0002A6802F00302012A0AB20ED1D3F80808B7 +:102FF00003EB421410F4807FD4F8000914BF40F0B2 +:10300000805040F00050C4F8000903EB4212D2F89F +:10301000000940F00440C2F800090122D3F8340846 +:1030200002FA01F10143C3F8341870BD19B9402EFA +:1030300084BF4020206020681A442E8A841940F002 +:103040000050013CB4FBF6F440EAC440C6E700007F +:103050002DE9F041D0F8906004460D4606EB41138F +:10306000D3F80879C3F80879FB071CD5D6F81038CF +:10307000DA0718D500EB8103D3F80CE0DEF8143042 +:10308000D3F800C0DA6894451BD2A2EB0C024FF0D3 +:1030900000081A60C3F80480202383F31188FFF727 +:1030A0008FFF88F311883B0618D50123D6F8342802 +:1030B000AB40134212D029462046BDE8F041FFF74D +:1030C000ADBC012303FA01F2038923EA0203038161 +:1030D000DEF80830002BE6D09847E4E7BDE8F08141 +:1030E0002DE9F047D0F8905004466E69AB691E4058 +:1030F000F1046E6103D5BDE8F047FEF715BD002E63 +:1031000012DAD5F8003E9A0705D0D5F8003E23F034 +:103110000303C5F8003ED5F80438204623F0010328 +:10312000C5F80438FEF730FD330502D52046FEF71A +:103130001FFDB7040CD5D5F8083813F0060FEB685F +:1031400023F470530CBF43F4105343F4A053EB60CB +:10315000300704D56368DB680BB120469847F1025D +:1031600000F1A180B2020BD5D4F8908000274FF077 +:103170000109D4F89430F9B29B688B4280F0D28078 +:10318000F3061AD5D4F890100A6AC2F30A1702F0AF +:103190000F0302F4F012B2F5802F00F0EB80B2F5CD +:1031A000402F0AD104EB830301F58051DB68186AD4 +:1031B00000231A469F4240F0D1803003D5F81858BA +:1031C00035D5E90303D500212046FFF7ADFEAA035C +:1031D00003D501212046FFF7A7FE6B0303D502218B +:1031E0002046FFF7A1FE2F0303D503212046FFF75A +:1031F0009BFEE80203D504212046FFF795FEA902B5 +:1032000003D505212046FFF78FFE6A0203D506216C +:103210002046FFF789FE2B0203D507212046FFF742 +:1032200083FEEF0103D508212046FFF77DFE7003E2 +:1032300040F1C780E90703D500212046FFF708FFCA +:10324000AA0703D501212046FFF702FF6B0703D52C +:1032500002212046FFF7FCFE2F0703D5032120465D +:10326000FFF7F6FEEE0603D504212046FFF7F0FE39 +:10327000A80603D505212046FFF7EAFE690603D517 +:1032800006212046FFF7E4FE2A0603D50721204643 +:10329000FFF7DEFEEB0540F1948020460821BDE8F3 +:1032A000F047FFF7D5BED4F890904FF000084FF0EC +:1032B000010AD4F894305FFA88F79B68BB42FFF4A8 +:1032C00051AF09EB4713D3F8002902F44022B2F5BD +:1032D000802F24D1D3F80029002A20DAD3F800293E +:1032E00042F09042C3F80029D3F80029002AFBDB02 +:1032F0003946D4F89000FFF7D5FB22890AFA07F384 +:1033000022EA0303238104EB8703DB689B6813B184 +:1033100039462046984739462046FFF77FFB08F19B +:103320000108C6E708EB4112D2F8003B03F4402342 +:10333000B3F5802F10D1D2F8003B002B0CDA628954 +:1033400009FA01F322EA0303638104EB8103DB68DA +:10335000DB680BB12046984701370AE713F0030FEB +:1033600000D10A68072B03F101039EBF0270120A05 +:1033700001301FE704EB830301F58051DA6890699F +:103380000268D0F808C04068A2EB000E0022104688 +:10339000974208D1DB689B699A683A449A605A68F8 +:1033A00017445F6009E712F0030F00D108689645E3 +:1033B00002F1010282BF8CF80000000A0CF1010C3E +:1033C000E6E7BDE8F087000008B50348FFF788FE90 +:1033D000BDE80840FFF776BA1432002008B503486C +:1033E000FFF77EFEBDE80840FFF76CBAB032002060 +:1033F000D0F8903003EB4111D1F8003B43F40013B7 +:10340000C1F8003B70470000D0F8903003EB411149 +:10341000D1F8003943F40013C1F8003970470000B7 +:10342000D0F8903003EB4111D1F8003B23F40013A6 +:10343000C1F8003B70470000D0F8903003EB411119 +:10344000D1F8003923F40013C1F8003970470000A7 +:1034500030B50433039C0172002104FB0325C361D2 +:10346000049B00600363059B4060C1604261026190 +:103470008561046242628162C162436330BD0000C3 +:103480000022416AC260416101616FF00101826204 +:10349000C262FEF799BE000003694269934203D1FC +:1034A000C2680AB100207047181D7047036919608F +:1034B0000021C2680132C260C2691344826993422A +:1034C000036124BF436A0361FEF772BE38B5044648 +:1034D0000D46E3683BB162690020131D1268A362C8 +:1034E0001344E36238BD237A33B929462046FEF7F8 +:1034F0004FFE0028EDDA38BD6FF00100FBE7000059 +:10350000C368C269013BC360436913448269934243 +:10351000436124BF436A436100238362036B03B1A9 +:103520001847704770B52023044683F31188866AD4 +:103530003EB9FFF7CBFF054618B186F31188284640 +:1035400070BDA36AE26A13F8015B9342A36202D3DF +:103550002046FFF7D5FF002383F31188EFE7000033 +:103560002DE9F84F04460E4690469946202787F3EA +:1035700011880025AA46D4F828B0BBF1000F09D164 +:1035800049462046FFF7A2FF20B18BF31188284659 +:10359000BDE8F88FA16AA8EB050BE36A5B1A9B45AF +:1035A00028BF9B46BBF1400F1BD9334601F14002B7 +:1035B00051F8040B914243F8040BF9D1A36A403649 +:1035C00040354033A362A26AE36A9A4202D320469E +:1035D000FFF796FF8AF311884545D8D287F3118803 +:1035E000C9E730465A46FDF78DFCA36A5E445D4448 +:1035F0005B44A362E7E7000010B5029C043301724C +:10360000C36103FB0421002300608362C362039B48 +:1036100040600363049BC460426102618161046293 +:103620004262436310BD0000026A6FF00101C26094 +:10363000426A4261026100228262C262FEF7C4BD38 +:10364000436902699A4203D1C2680AB100207047F7 +:10365000184650F8043B0B6070470000C368002117 +:10366000C2690133C3604369134482699342436171 +:1036700024BF436A4361FEF79BBD000038B5044692 +:103680000D46E3683BB1236900201A1DA262E2697E +:103690001344E36238BD237A33B929462046FEF746 +:1036A00077FD0028EDDA38BD6FF00100FBE7000080 +:1036B00003691960C268013AC260C2691344826931 +:1036C0009342036124BF436A036100238362036B57 +:1036D00003B118477047000070B5202304460E461A +:1036E00083F31188856A35B91146FFF7C7FF10B11A +:1036F00085F3118870BDA36A1E70A36AE26A013364 +:103700009342A36204D3E16920460439FFF7D0FF56 +:10371000002080F3118870BD2DE9F84F04460D4656 +:1037200091469A464FF0200888F311880026B34648 +:10373000A76A4FB951462046FFF7A0FF20B187F393 +:1037400011883046BDE8F88FA06AA9EB0603E76A46 +:103750003F1A9F4228BF1F46402F1BD905F1400347 +:1037600055F8042B9D4240F8042BF9D1A36A40364A +:103770004033A362A26AE36A9A4204D3E169204615 +:103780000439FFF795FF8BF311884E45D9D288F3A2 +:103790001188CDE729463A46FDF7B4FBA36A3D44BC +:1037A0003E443B44A362E5E7026943699A4203D180 +:1037B000C3681BB9184670470023FBE7836A002BD8 +:1037C000F8D0043B9B1AF5D01360C368013BC3607B +:1037D000C3691A4483699A42026124BF436A036140 +:1037E000002383620123E5E700F050B9034B002278 +:1037F00058631A610222DA60704700BF000C004073 +:103800000022014BDA607047000C0040014B586306 +:10381000704700BF000C0040FEE7000010B5194CD7 +:10382000FEF7BCFBFEF7DEFC802217492046FEF7C0 +:1038300063FC012344F8180C0374144B144AD96830 +:1038400021F4E0610904090C0A431249DA60CA68EC +:1038500042F08072CA60104A1049C2F8B01F116865 +:1038600041F0010111601022DA77202283F8222032 +:10387000002383F3118862B60948BDE81040FEF7C3 +:103880005DBC00BFD42A00203C42000800ED00E0EF +:103890000003FA05F0ED00E0001000E055CEACC5E5 +:1038A000444200082DE9F84F1E4C4FF00008DFF8A5 +:1038B0007890656904F11407D9F82430266AAA685B +:1038C0009E1B96421CD34FF0200AAA68236AD5F8A3 +:1038D0000CB0B61A134423622B68BB425F6063616D +:1038E000C5F80C8001D1FFF78BFF88F31188286998 +:1038F000D8478AF311886569AB689E42E5D2DBE759 +:103900006269BA420CD0916823628E1B9660A868E7 +:1039100002282CBF1818981CBDE8F84FFFF776BF97 +:10392000BDE8F88FAC2A0020000C0040034B59681A :103930005A68521A9042FBD8704700BF001000E04E :103940004B6843608B688360CB68C3600B694361DD :103950004B6903628B6943620B6803607047000028 -:1039600008B52C4B2C48D3F8882040F2FF710A434D +:1039600008B52C4B40F2FF712B48D3F888200A434E :10397000C3F88820D3F8882022F4FF6222F00702DF :10398000C3F88820D3F88820D3F8E0200A43C3F88E :10399000E020D3F808210A43C3F808211F4AD3F8CE @@ -928,21 +928,21 @@ :1039E000FFF7AEFF144802F1C401FFF7A9FF134827 :1039F00002F1E001FFF7A4FF114802F1FC01FFF71B :103A00009FFF104802F58C71FFF79AFFBDE8084050 -:103A100000F016B90044025800000258584200084D +:103A100000F016B900440258000002586442000841 :103A20000004025800080258000C02580010025806 :103A30000014025800180258001C025800200258B6 -:103A4000002402580028025808B500F0C7FAFFF712 -:103A5000E3FEFEF713FFBDE80840FEF7CDBC000013 +:103A4000002402580028025808B500F0CDFAFFF70C +:103A5000E5FEFEF715FFBDE80840FEF7CDBC00000F :103A600070470000084B1A69920710B508D500246A -:103A70001C61202383F31188FFF712FF84F3118860 -:103A8000BDE81040FEF71CBF000C0040124BD3F8FD -:103A9000E82042F00802C3F8E820D3F8102142F0F1 -:103AA0000802C3F810210D4AD3F81031D36B43F04C -:103AB0000803D3630A4B63229A624FF0FF32DA6243 -:103AC00000229A615A63DA605A6001225A61082121 -:103AD0001A603220FDF72CBC004402580010005C34 -:103AE000000C0040F8B5514BD3F880204FF0FF3266 -:103AF000C3F880200024D3F88010C3F88040D3F8A6 +:103A70001C61202383F31188FFF714FF84F311885E +:103A8000BDE81040FEF71EBF000C0040124B08219D +:103A90003220D3F8E82042F00802C3F8E820D3F837 +:103AA000102142F00802C3F810210C4AD3F810315B +:103AB000D36B43F00803D363C722094B9A624FF0DC +:103AC000FF32DA6200229A615A63DA605A60012298 +:103AD0005A611A60FDF728BC004402580010005CCF +:103AE000000C0040F8B5514B0024D3F880204FF073 +:103AF000FF32C3F88020D3F88010C3F88040D3F899 :103B00008010D3F88410C3F88420D3F88410C3F84D :103B10008440D3F88410D96F41F0FF4141F4FF0194 :103B200041F4DF4141F07F01D967D96F21F0FF41B6 @@ -955,134 +955,134 @@ :103B9000D3F89810C3F89840D3F89810D3F88C1045 :103BA000C3F88C20D3F88C10C3F88C40D3F88C1059 :103BB000D3F89C10C3F89C20D3F89C20C3F89C40F9 -:103BC000D3F89C3000F0EAF9194B07229A60194AA1 +:103BC000D3F89C3000F0F0F9194B07229A60194A9B :103BD000DA60194A1A6105225A60184A536A43F496 :103BE00080335362C2F88440BFF34F8FD2F88030E5 -:103BF000C3F3C904C3F34E335B01A50743F6E07674 -:103C000003EA060E284621464EEA00070139C2F8AB -:103C100074724F1C00F14040F6D1203B13F1200F8D +:103BF00043F6E076C3F3C904C3F34E33A5075B0174 +:103C000003EA060E284621464EEA0007013900F174 +:103C10004040C2F874724F1CF6D1203B13F1200FC4 :103C2000EED1BFF34F8FBFF36F8FF8BD0044025842 :103C300090ED00E0000004301B00080300ED00E000 -:103C40005A4B5B4901221A605A4B19609A605A4AD2 +:103C400001225D4B5D491A605D4B19609A605D4AC7 :103C5000DA6000221A614FF440429A619A69920434 -:103C6000FCD51A6842F480721A60544B1A6F12F431 -:103C7000407F1FBF4FF480321A6700221A671A680C -:103C800042F001021A604D4A134611684807FCD5FC +:103C6000FCD51A6842F480721A60574B1A6F12F42E +:103C7000407F04D04FF480321A6700221A671A6816 +:103C800042F001021A60504A134611684807FCD5F9 :103C9000002111611A6912F03802FBD1012119606B -:103CA0004FF0804159605A67454ADA62454A1A61C5 -:103CB0001A6842F480321A60404A13461168890338 +:103CA0004FF0804159605A67484ADA62484A1A61BF +:103CB0001A6842F480321A60434A13461168890335 :103CC000FCD5116841F0800111601A68D205FCD55D -:103CD0003D4A3E499A6200225A631963A1F5C02108 -:103CE0004B39DA6399635A64394A1A64394ADA6299 -:103CF0001A6842F0A8521A60304B1A6802F0285233 -:103D0000B2F1285FF9D109229A610022DA611A62C0 -:103D10004FF00052DA643049304A1A6559659A65A5 -:103D20002F4A212111601A6942F003021A61234BC4 -:103D30001A6902F03802182AFAD1D3F8DC2042F0CE -:103D40000052C3F8DC20D3F8042142F00052C3F83B -:103D50000421D3F80421D3F8DC2042F08042C3F8D8 -:103D6000DC20D3F8042142F08042C3F80421D3F8C8 -:103D70000421D3F8DC2042F00042C3F8DC20D3F861 -:103D8000042142F00042C3F80421D3F80421D3F8FF -:103D9000F42042F00202C3F8F420D3F81C2142F0D0 -:103DA0000202C3F81C21D3F81C317047088100516E -:103DB00000C000F000480258020000010044025810 -:103DC0000000FF01008890083230600063020701A4 -:103DD00047040508FD0BFF010010E0000000011082 -:103DE00000200052084A08B5936811680B4003F0A0 -:103DF0000103936023B1054A13680BB150689847DB -:103E0000BDE80840FEF75CBD80000058C0250020DA -:103E1000084A08B5936811680B4003F002039360E9 -:103E200023B1054A93680BB1D0689847BDE80840B4 -:103E3000FEF746BD80000058C0250020084A08B59E -:103E4000936811680B4003F00403936023B1054AA3 -:103E500013690BB150699847BDE80840FEF730BDC3 -:103E600080000058C0250020084A08B593681168F2 -:103E70000B4003F00803936023B1054A93690BB12B -:103E8000D0699847BDE80840FEF71ABD8000005889 -:103E9000C0250020084A08B5936811680B4003F05C -:103EA0001003936023B1054A136A0BB1506A984717 -:103EB000BDE80840FEF704BD80000058C025002082 -:103EC000174B10B59C681A68144004F478729A6015 -:103ED000A30604D5134A936A0BB1D06A98476006CB -:103EE00004D5104A136B0BB1506B9847210604D5CB -:103EF0000C4A936B0BB1D06B9847E20504D5094A85 -:103F0000136C0BB1506C9847A30504D5054A936C0C -:103F10000BB1D06C9847BDE81040FEF7D1BC00BF94 -:103F200080000058C02500201A4B10B59C681A6804 -:103F3000144004F47C429A60620504D5164A136D5D -:103F40000BB1506D9847230504D5134A936D0BB1FF -:103F5000D06D9847E00404D50F4A136E0BB1506E34 -:103F60009847A10404D50C4A936E0BB1D06E9847C4 -:103F7000620404D5084A136F0BB1506F98472304AD -:103F800004D5054A936F0BB1D06F9847BDE8104038 -:103F9000FEF796BC80000058C0250020062108B519 -:103FA0000846FDF7C5F906210720FDF7C1F90621EE -:103FB0000820FDF7BDF906210920FDF7B9F9062112 -:103FC0000A20FDF7B5F906211720FDF7B1F9062102 -:103FD0002820BDE80840FDF7ABB9000008B5FFF7A1 -:103FE00081FDFDF77DF8FDF749FBFDF721FDFDF7AC -:103FF000F3FBFFF735FDBDE80840FFF7F3BB00001A -:1040000010B501390244904201D1002010BD10F8D2 -:10401000013B11F8014FA342F5D0181B10BD000061 -:10402000034611F8012B03F8012B002AF9D1704740 -:10403000121012131211000001105A00031059003F -:104040000120580003205600EC2A002040260020C2 -:1040500053544D333248373F3F3F0053544D333272 -:10406000483734332F373533000000000096000006 -:104070000000000000000000000000000000000040 -:10408000000000000000000045160008311600087E -:104090006D1600085916000865160008511600082C -:1040A0003D160008291600087916000800000000D7 -:1040B000511700083D170008791700086517000818 -:1040C000711700085D170008491700083517000828 -:1040D000D5170008000000000100000000000000EB -:1040E00002000000000000007D190008E919000826 -:1040F00040004000842F0020942F00200200000088 -:104100000000000003000000000000002D1A00085D -:104110000000000010000000A42F0020000000009C -:104120000100000000000000143200200101020024 -:104130004865782F50726F6669434E43004375623D -:10414000654F72616E67652D424C0025534552499B -:10415000414C25009923000801230008E9180008B4 -:104160007D230008430000006C4100080902430061 -:10417000020100C03209040000010202010005240E -:104180000010010524010001042402020524060098 -:1041900001070582030800FF09040100020A00006C -:1041A00000070501024000000705810240000000F1 -:1041B00012000000B8410008120110010200004086 -:1041C000AE2D1610000201020301000004030904D1 -:1041D00025424F4152442500437562654F72616E1E -:1041E0006765003031323334353637383941424330 -:1041F00044454600000000009D1B00087D1E00088D -:10420000291F0008400040004C3300204C330020A0 -:10421000010000005C33002080000000400100002D -:104220000800000000010000000400000800000079 -:104230006D61696E00000000504200086833002084 -:104240006035002001000000153800080000000063 -:1042500069646C65000000000000802A0000000016 -:10426000AAAAAAAA00000000FFFF000000000000A8 -:1042700000A00A000001000000000000AAAAAAAAEB -:1042800000000000FFFF0000000000000000000030 -:104290000000000000000000AAAAAAAA0000000076 -:1042A000FFFF000000000000000000000000000010 -:1042B00000000000AAAAAAAA00000000FFFF000058 -:1042C000000000000000000000800200000000006C -:1042D000AAAAAAAA00400100FFFF00000000007087 -:1042E000070000000000000000000000AAAAAAAA1F -:1042F00000000000FFFF00000000000000000000C0 -:104300000000000000000000AAAAAAAA0000000005 -:10431000FFFF00000000000000000000000000009F -:1043200000000000AAAAAAAA00000000FFFF0000E7 -:10433000000000000000000000000000000000007D -:10434000AAAAAAAA00000000FFFF000000000000C7 -:10435000000000000000000000000000AAAAAAAAB5 -:1043600000000000FFFF000000000000000000004F -:104370000000000000000000AAAAAAAA0000000095 -:10438000FFFF00000000000000000000000000002F -:10439000FF00000000000000504000083F00000047 -:1043A000500400005B4000083F0000000096000041 -:1043B0000000080004000000CC41000800000000DC -:1043C00000000000000000000000000000000000ED -:0443D00000000000E9 +:103CD000404A41499A6200225A631963A1F5C02102 +:103CE000DA634B3999635A643C4A1A643C4ADA6293 +:103CF0001A6842F0A8521A60334B1A6802F0285230 +:103D0000B2F1285FF9D1482222219A614FF48862EA +:103D1000DA6140221A624FF00052DA64314A1A65C1 +:103D2000314A5A6502F1726202F571429A652F4A70 +:103D300011601A6942F003021A61234B1A6902F0FA +:103D40003802182AFAD1D3F8DC2042F00052C3F826 +:103D5000DC20D3F8042142F00052C3F80421D3F848 +:103D60000421D3F8DC2042F08042C3F8DC20D3F8F1 +:103D7000042142F08042C3F80421D3F80421D3F88F +:103D8000DC2042F00042C3F8DC20D3F8042142F0EA +:103D90000042C3F80421D3F80421D3F8F42042F000 +:103DA0000202C3F8F420D3F81C2142F00202C3F847 +:103DB0001C21D3F81C3170470881005100C000F06D +:103DC0000048025802000001004402580000FF01B0 +:103DD000008890083230600063020701470405083C +:103DE000FD0BFF01000001100010E0000020005258 +:103DF000084A08B5936811680B4003F0010393600B +:103E000023B1054A13680BB150689847BDE80840D4 +:103E1000FEF758BD80000058C0250020084A08B5AC +:103E2000936811680B4003F00203936023B1054AC5 +:103E300093680BB1D0689847BDE80840FEF742BDD3 +:103E400080000058C0250020084A08B59368116812 +:103E50000B4003F00403936023B1054A13690BB1CF +:103E600050699847BDE80840FEF72CBD8000005817 +:103E7000C0250020084A08B5936811680B4003F07C +:103E80000803936023B1054A93690BB1D069984741 +:103E9000BDE80840FEF716BD80000058C025002090 +:103EA000084A08B5936811680B4003F0100393604B +:103EB00023B1054A136A0BB1506A9847BDE8084020 +:103EC000FEF700BD80000058C0250020174B10B53C +:103ED0009C681A68144004F478729A60A30604D5AA +:103EE000134A936A0BB1D06A9847600604D5104A0A +:103EF000136B0BB1506B9847210604D50C4A936B9A +:103F00000BB1D06B9847E20504D5094A136C0BB18D +:103F1000506C9847A30504D5054A936C0BB1D06C3F +:103F20009847BDE81040FEF7CDBC00BF80000058A8 +:103F3000C02500201A4B10B59C681A68144004F480 +:103F40007C429A60620504D5164A136D0BB1506D20 +:103F50009847230504D5134A936D0BB1D06D98474C +:103F6000E00404D50F4A136E0BB1506E9847A104BC +:103F700004D50C4A936E0BB1D06E9847620404D5F9 +:103F8000084A136F0BB1506F9847230404D5054AB4 +:103F9000936F0BB1D06F9847BDE81040FEF792BC0D +:103FA00080000058C0250020062108B50846FDF70E +:103FB000BBF906210720FDF7B7F906210820FDF718 +:103FC000B3F906210920FDF7AFF906210A20FDF714 +:103FD000ABF906211720FDF7A7F906212820BDE837 +:103FE0000840FDF7A1B9000008B5FFF77BFDFDF71C +:103FF00071F8FDF743FBFDF71BFDFDF7EDFBFFF748 +:104000002FFDBDE80840FFF7EFBB000010B50139F8 +:104010000244904201D1002010BD10F8013B11F87C +:10402000014FA342F5D0181B10BD0000034611F844 +:10403000012B03F8012B002AF9D17047121012133B +:104040001211000001105A000310590001205800FD +:1040500003205600EC2A00204026002053544D3304 +:104060003248373F3F3F0053544D333248373433A3 +:104070002F373533000000000096000000000000DC +:104080000000000000000000000000000000000030 +:104090000000000045160008311600086D160008E3 +:1040A0005916000865160008511600083D1600084C +:1040B00029160008791600080000000051170008B2 +:1040C0003D170008791700086517000871170008E8 +:1040D0005D1700084917000835170008D5170008B4 +:1040E00000000000010000000000000002000000CD +:1040F000000000007D190008E91900084000400098 +:10410000842F0020942F00200200000000000000F7 +:1041100003000000000000002D1A0008000000004D +:1041200010000000A42F002000000000010000008B +:104130000000000014320020010102004865782FC1 +:1041400050726F6669434E4300437562654F7261FA +:104150006E67652D424C002553455249414C250060 +:104160009923000801230008E91800087D230008AE +:10417000430000007841000809024300020100C02A +:1041800032090400000102020100052400100105AB +:10419000240100010424020205240600010705820F +:1041A000030800FF09040100020A000000070501DE +:1041B00002400000070581024000000012000000DC +:1041C000C44100081201100102000040AE2D16107B +:1041D00000020102030100000403090425424F41CB +:1041E00052442500437562654F72616E6765003009 +:1041F000313233343536373839414243444546004D +:10420000000000009D1B00087D1E0008291F0008FB +:10421000400040004C3300204C33002001000000DF +:104220005C33002080000000400100000800000016 +:104230000001000000040000080000006D61696ECC +:10424000000000005C420008683300206035002058 +:1042500001000000193800080000000069646C6566 +:10426000000000000000802A00000000AAAAAAAAFC +:1042700000000000FFFF00000000000000A00A0096 +:104280000001000000000000AAAAAAAA0000000085 +:10429000FFFF0000000000000000000014000054B8 +:1042A00000000000AAAAAAAA14000054FFFF000000 +:1042B00000000000000000000040100000000000AE +:1042C000AAAA8AAA00401000FFFF00000000000018 +:1042D000000000000081020000000000AAAAAAAAB3 +:1042E00000410100FFFF0000000000700700000017 +:1042F0000000000000000000AAAAAAAA0000000016 +:10430000FFFF0000000000000000000000000000AF +:1043100000000000AAAAAAAA00000000FFFF0000F7 +:10432000000000000000000000000000000000008D +:10433000AAAAAAAA00000000FFFF000000000000D7 +:10434000000000000000000000000000AAAAAAAAC5 +:1043500000000000FFFF000000000000000000005F +:104360000000000000000000AAAAAAAA00000000A5 +:10437000FFFF00000000000000000000000000003F +:1043800000000000AAAAAAAA00000000FFFF000087 +:104390000000000000000000FF000000000000001E +:1043A0005C4000083F000000500400006740000827 +:1043B0003F0000000096000000000800040000001C +:1043C000D8410008000000000000000000000000CC +:0C43D000000000000000000000000000E1 :00000001FF diff --git a/Tools/bootloaders/MttrCubeBlack+_bl.bin b/Tools/bootloaders/MttrCubeBlack+_bl.bin new file mode 100755 index 0000000000..2667b2d0d6 Binary files /dev/null and b/Tools/bootloaders/MttrCubeBlack+_bl.bin differ diff --git a/Tools/bootloaders/MttrCubeBlack_bl.bin b/Tools/bootloaders/MttrCubeBlack_bl.bin new file mode 100755 index 0000000000..7a1c2d1102 Binary files /dev/null and b/Tools/bootloaders/MttrCubeBlack_bl.bin differ diff --git a/Tools/bootloaders/MttrCubeBlack_bl.elf b/Tools/bootloaders/MttrCubeBlack_bl.elf new file mode 100755 index 0000000000..a58fc9b8a2 Binary files /dev/null and b/Tools/bootloaders/MttrCubeBlack_bl.elf differ diff --git a/Tools/bootloaders/MttrCubeBlack_bl.hex b/Tools/bootloaders/MttrCubeBlack_bl.hex new file mode 100644 index 0000000000..827357081c --- /dev/null +++ b/Tools/bootloaders/MttrCubeBlack_bl.hex @@ -0,0 +1,1008 @@ +:020000040800F2 +:1000000000040020E1010008E3010008E30100080A +:10001000E3010008E3010008E3010008E301000830 +:10002000E3010008E3010008E30100084927000894 +:10003000E3010008E3010008E3010008E301000810 +:10004000E3010008E3010008E3010008E301000800 +:10005000E3010008E3010008B1380008DD380008BA +:10006000093900083539000861390008E301000842 +:10007000E3010008E3010008E3010008E3010008D0 +:10008000E3010008E3010008E3010008E3010008C0 +:10009000E3010008E3010008E30100088D390008CE +:1000A000E3010008E3010008E3010008E3010008A0 +:1000B000E3010008E3010008E3010008E301000890 +:1000C000E3010008E3010008E3010008E301000880 +:1000D000E3010008E301000875100008891000081A +:1000E000F5390008E3010008E3010008E301000816 +:1000F000E3010008E3010008E3010008E301000850 +:10010000E3010008E3010008C1360008E30100082C +:10011000E3010008E3010008E3010008E30100082F +:10012000E3010008E3010008E3010008E30100081F +:10013000E3010008E3010008E3010008E30100080F +:10014000E3010008E3010008E3010008352E000880 +:10015000E3010008E3010008E3010008E3010008EF +:10016000E3010008E3010008E3010008E3010008DF +:10017000E3010008E3010008E3010008E3010008CF +:10018000E3010008E30100089D100008E3010008F6 +:10019000E3010008E3010008E3010008E3010008AF +:1001A000E3010008E3010008E3010008E30100089F +:1001B000E3010008E3010008E3010008E30100088F +:1001C000E3010008E3010008E3010008E30100087F +:1001D000E3010008E3010008E3010008E30100086F +:1001E00002E000F000F8FEE772B6394880F30888B4 +:1001F000384880F3098838484EF60851CEF200019D +:10020000086040F20000CCF200004EF63471CEF2ED +:1002100000010860BFF34F8FBFF36F8F40F2000003 +:10022000C0F2F0004EF68851CEF200010860BFF334 +:100230004F8FBFF36F8F4FF00000E1EE100A4EF6C4 +:100240003C71CEF200010860062080F31488BFF3F1 +:100250006F8F02F0B7FA03F0C9F94FF055301F491C +:100260001B4A91423CBF41F8040BFAE71C49194A6A +:1002700091423CBF41F8040BFAE71A491A4A1B4B5A +:100280009A423EBF51F8040B42F8040BF8E70020F5 +:100290001749184A91423CBF41F8040BFAE702F0B3 +:1002A00095FA03F0FFF9144C144DAC4203DA54F8FC +:1002B000041B8847F9E700F03FF8114C114DAC42A0 +:1002C00003DA54F8041B8847F9E702F07DBA00000E +:1002D000000400200024002000000008000000208E +:1002E00000040020B03E0008002400202824002044 +:1002F0002824002090410020E0010008E0010008CF +:10030000E0010008E00100082DE9F04F2DED108A12 +:10031000C1F80CD0C3689D46BDEC108ABDE8F08FD3 +:10032000002383F311882846A047002001F094FEA3 +:1003300001F044FE00DFFEE770B5204C09230025E4 +:1003400023706FF03F061F236570A570E57025715F +:100350006571A571E57125726672A372E57200F090 +:100360000DFD20B10F2325726672A372E57202F0B3 +:10037000C3F9044602F0E2F90546D8B9104B9C4295 +:100380001AD001339C4218BF054641F2883404BF9D +:1003900001250024002002F0B9F90DB100F0B6F8F3 +:1003A00000F070FD00F028FC204600F00DF900F090 +:1003B000ADF8F9E70024EDE70446EBE72824002038 +:1003C000010007B008B500F095FBA0F120035842EA +:1003D000584108BD07B5054B02211B88ADF8043014 +:1003E00001A800F0F3FB03B05DF804FBF43A000849 +:1003F000064991F8243033B100230822086A81F8B5 +:10040000243000F01BBC0120704700BF48240020AE +:100410002DE9F84F234C94F82430054688461746BA +:100420008BBB2E46DFF87C90002F38D094F824A0A8 +:10043000BAF1000F05D12022FF214846266200F0C4 +:100440000FFDCAF10805BD4228BF3D465FFA85FB96 +:10045000AD0041462A4604EB8A0000F0D7FC94F830 +:100460002430A7EB0B079B445FFA8BFBBBF1080F13 +:100470002E44A844FFB284F824B0D5D1FFF7B8FFCA +:100480000028D1D108E0266A06EB8306B042C9D025 +:10049000FFF7AEFF0028C4D10020BDE8F88F01208F +:1004A000BDE8F88F4824002010B5202383F311887D +:1004B0001248C3680BB101F055FE0023104A0F48E3 +:1004C0004FF47A7101F012FE002383F311880D4C72 +:1004D000236813B12368013B2360636813B1636829 +:1004E000013B6360084B1B7833B9636823B9022072 +:1004F00000F082FC3223636010BD00BF3424002072 +:10050000A9040008782900207024002010B5254B8C +:10051000254953F8042F013200D110BD8B42F8D188 +:10052000224C234B22689A423AD9224B9B6803F112 +:10053000006303F580439A4232D2002000F076FB3C +:100540001D4B0220187000F04DFC1C4B1A6C002251 +:100550001A64196E1A66196E596C5A64596E5A6685 +:100560005A6E5A6942F080025A615A6922F080023A +:100570005A615A691A6942F000521A611A6922F0E6 +:1005800000521A611B6972B60D4A0E4B13601B684C +:100590002268202181F311889D4683F30888104743 +:1005A00010BD00BFFC3F00081C40000804400008CC +:1005B000FF3F00082824002070240020003802405B +:1005C00008ED00E0004000082DE9F04FB04C97B076 +:1005D00020228146FF210EA8A66800F041FCAD4A0A +:1005E0001378A346A3B9AC480121C360117020233E +:1005F00083F31188C3680BB101F0B4FD0023A74A4F +:10060000A5484FF47A7101F071FD002383F311883E +:10061000B9F1000F02D0A24BC3F80090A14BA04A41 +:1006200000241C705460254627469846012000F09F +:10063000D9FBB9F1000F00F02282994B1B68002B07 +:1006400040F01D8217B0BDE8F08F0220FFF7BAFE20 +:10065000002840F00E82944B1B88ADF82430022114 +:1006600009A800F0B3FAE1E74FF47A7000F042FA1B +:10067000031E0193EFDB0220FFF7A4FE8246002851 +:10068000E9D0019B581E042800F2F581DFE800F054 +:10069000030D1013160016A8052340F8343D04215D +:1006A00000F094FA5446002752E004217848F7E716 +:1006B00004217E48F4E704217D48F1E71C2420460C +:1006C00000F0B6FA04340990042109A800F07EFA7B +:1006D0002C2CF4D1E6E7002DBDD0002CBBD002209D +:1006E000FFF770FE0546002800F0C781012000F0EA +:1006F0009DFA022088F8000000F074FB0024E3B2A9 +:100700001846019300F0A2FA824648B1019B1846B0 +:1007100000F0ACFA01340028F1D12C46654B9BE780 +:100720000123022088F8003000F052FB5646DBF827 +:1007300008309E4206D2304600F07AFA0130ECD101 +:100740000436F4E7564B002688F800605E602C46BD +:1007500000F08EFB1DB1002C18BF4FF00009FFF711 +:1007600039FE63E7002D3FF476AF002C3FF473AF02 +:10077000022088F8000000F035FB322000F0BAF9C2 +:10078000B0F1000AC0F279811AF0030540F07581DA +:10079000DBF8082006EB0A03934200F26E81BAF5FB +:1007A000807F00F26A81444B019355450DDA4FF486 +:1007B0007A7000F09FF90290029A002AC0F25D81DF +:1007C000019B029AEA540135EEE7C820FFF7FAFDD3 +:1007D0000546002800F051811F2E11D8C6F12004D3 +:1007E000544528BF54460EAB26F00300224632493A +:1007F000184400F00BFB2246FF212F4800F030FB8D +:100800004FEAAA0A5FFA8AF22B493046FFF700FE48 +:100810000446002881D006EB8A06054698E70220A8 +:10082000FFF7D0FD00283FF416AFFFF7E1FD0028E9 +:100830003FF411AF00229246DBF808309A4259D2B9 +:100840001F2A1ED80E9B01331BD022F0030316A9CA +:100850000B4453F8203C09931848436883B3174866 +:1008600000210DF1240E11F80E3083EA0A03DBB2E9 +:10087000013150F82330042983EA1A2AF3D10432D3 +:10088000DAE71046019200F0D3F9019A0990E3E704 +:10089000282400207425002034240020A904000806 +:1008A0007829002070240020F63A00082C2400202B +:1008B00030240020F83A0008742400207825002015 +:1008C00019464FF0080E11F0010F4FEA51011FBFFA +:1008D00081F06D4181F4380181F4034181F0200100 +:1008E000BEF1010EEFD140F823100133B3F5807F44 +:1008F000E6D1B4E716A8042140F84CAD00F066F943 +:1009000026E716A8002340F8343D642100F02AF9B8 +:1009100000287FF4A0AE0220FFF754FD00283FF42A +:100920009AAE099800F0B4F916AB43F8480D0421CB +:100930001846E3E716A8002340F8343D642100F090 +:1009400011F900287FF487AE0220FFF73BFD002855 +:100950003FF481AE099800F0A9F916AB43F8440DB5 +:10096000E5E70220FFF72EFD00283FF474AE00F00B +:10097000A3F916AB43F8400DD9E70220FFF722FD9B +:1009800000283FF468AE09A9142000F09BF98246C4 +:1009900016A8042140F83CAD00F018F9514609A80A +:1009A000ACE7322000F0A6F8B0F1000AFFF653AE33 +:1009B0001AF0030F7FF44FAEDBF808200AEB0703B1 +:1009C00093423FF648AE0220FFF7FCFC00283FF4BC +:1009D00042AE2AF0030ABA44BA453FF4B9AE3846EB +:1009E00000F026F90421089008A800F0EFF8043779 +:1009F000F2E74FF47A70FFF7E5FC00283FF42BAEE6 +:100A0000FFF7F6FC00283FF489AE0E9B01330CD0B3 +:100A100008220EA90020FFF7FBFC00283FF47EAE61 +:100A20002022FF210EA800F01BFAFFF7D3FC30486C +:100A300001F018FB06E616A8002340F8343D6421B7 +:100A400000F090F8824600287FF405AE0220FFF700 +:100A5000B9FC00283FF4FFADFFF7BCFC41F2883041 +:100A600001F000FB099800F041FAD14600F000FACD +:100A7000DCE505466CE60027EDE52546EBE50020C4 +:100A800000F038F80290029B002BFFF6D2AD012057 +:100A900000F09EF9029B213B122B3FF6C7AD01A24D +:100AA00052F823F04B06000869060008D706000834 +:100AB0002D0600082D0600082D0600086507000811 +:100AC000A30900081F0800080309000835090008E9 +:100AD000630900082D0600087B0900082D060008A0 +:100AE000F3090008510700082D060008370A00081E +:100AF000A08601002DE9F34702AE00244FF47A7579 +:100B000006F8014D144FDFF85880454397F900303F +:100B10005A1C5FFA84F901D0A34212D158F824007C +:100B200003680122D3F820A031462B46D047012884 +:100B300007D10A4B9DF8070083F8009002B0BDE88A +:100B4000F0870134022CE1D14FF4FA7001F08AFAF7 +:100B50004FF0FF30F2E700BF00240020982900206A +:100B60004C3B00082DE9F0474FF47A75144FDFF83D +:100B7000588006464D43002497F900305A1C5FFA0E +:100B800084F901D0A34210D158F82400036804224C +:100B9000D3F820A031462B46D047042805D1094B75 +:100BA00083F800900020BDE8F0870134022CE3D1E7 +:100BB0004FF4FA7001F056FA4FF0FF30BDE8F087BD +:100BC00000240020982900204C3B000830B4074B3B +:100BD0001A78074B53F822402368DD69054B0A4613 +:100BE000AC460146204630BC604700BF9829002033 +:100BF0004C3B0008A086010070B501F07DFC094C5B +:100C0000094E2070002530682378834208D901F00E +:100C10006DFC336805440133B5F5804F3360F2D382 +:100C200070BD00BF992900208029002001F016BD69 +:100C300000F1006000F580400068704700F100603E +:100C4000920000F5804001F0A1BC0000054B1A683D +:100C5000054B1B789B1A834202D9104401F046BC15 +:100C600000207047802900209929002038B5074DC1 +:100C700004462868204401F041FC28B92868204433 +:100C8000BDE8384001F052BC38BD00BF80290020CB +:100C900010F0030308D1B0F5007F05D800F1005033 +:100CA000A0F508400068704700207047014BC0580D +:100CB000704700BF107AFF1F014B1868704700BFD4 +:100CC000002004E070B52A4B1C68C4F30B03240C0D +:100CD00063B140F21342934231D040F21942934241 +:100CE0002FD040F2214293422DD10323214A0C25DB +:100CF00005FB03235D6893F9082042F201039C423F +:100D000024D0B4F5805F23D041F201039C4221D06E +:100D100041F203039C421FD041F207039C4208BFEB +:100D20003122441E0C44013D0B46A3421ED215F94C +:100D3000016F581C96B1034600F8016CF5E70123DA +:100D4000D4E70223D2E73F220B4DD6E73322E3E775 +:100D50004122E1E75A22E4E75922E2E72C258442C6 +:100D60001D7001D9981C5A70401A70BD1846FBE7D7 +:100D7000002004E01C3B0008FC3A0008124B5A8893 +:100D800042F201018A4293B214D0B3F5805F13D0CE +:100D900041F20102934211D041F2030293420FD07B +:100DA00041F2070293420DD10423084A02EB830368 +:100DB000D87870470023F8E70123F6E70223F4E729 +:100DC0000323F2E700207047002004E0083B0008FE +:100DD000022802BF024B4FF480529A61704700BF55 +:100DE00000100240022802BF024B4FF080529A616D +:100DF000704700BF00100240022801BF024A536939 +:100E000083F48053536170470010024010B50023F3 +:100E1000934203D0CC5CC4540133F9E710BD000009 +:100E200030B5441E14F9010F0B4660B193F9005020 +:100E3000854201F1010106D11AB993F90020801A07 +:100E400030BD013AEEE7002AF7D1104630BD000070 +:100E500002460346981A13F9011B0029FAD170477C +:100E600002440346934202D003F8011BFAE770479D +:100E7000024B1A78024B1A70704700BF9829002065 +:100E80000024002038B5134C134D204600F032FCEE +:100E90002946204600F05AFC2D6810486A6D936B75 +:100EA00023F40023936301F0DDF80D49284600F098 +:100EB00059FD6A6D0B48936B0B4943F40023936310 +:100EC000A0424FF4E1330B6003D0BDE8384000F09E +:100ED0006FBB38BD143700202C3C000840420F0087 +:100EE000583C00089C2900208429002038B50B4B71 +:100EF0001A780B4B53F822400A4B9C4205460CD003 +:100F0000094B002118461422FFF7AAFF056001468D +:100F10002046BDE8384000F04BBB38BD9829002082 +:100F20004C3B0008143700208429002002684368E5 +:100F30001143016003B1184770470000024AD368AB +:100F400043F0C003D360704700440040024AD368B6 +:100F500043F0C003D360704700480040024AD368A2 +:100F600043F0C003D3607047007800402DE9F041A2 +:100F7000D0F85C64F7683368DA0505469CB20DD595 +:100F8000202383F311884FF400710430FFF7CEFF64 +:100F90006FF480733360002383F31188202383F37D +:100FA000118805F1040814F02F0333D183F311885D +:100FB000380615D5210613D5202383F3118805F1B2 +:100FC000380000F053FA002846DA0821281DFFF700 +:100FD000ADFF4FF67F733B40F360002383F311882E +:100FE0007A060ED563060CD5202383F31188EA6CAC +:100FF0002B6D9A4202D12B6C002B2FD1002383F34F +:101000001188D5F86424D368002B31D01069BDE86D +:10101000F0411847230713D014F0080F0CBF00212C +:101020008021E00748BF41F02001A20748BF41F0FE +:101030004001630748BF41F480714046FFF776FFE7 +:10104000A406736805D595F860142846194000F089 +:10105000BDFA3468A4B2A6E77060BEE727F0400787 +:101060003F041021281D3F0CFFF760FFF760C5E724 +:10107000BDE8F08108B50348FFF778FFBDE80840F8 +:1010800001F072BB9C29002008B50348FFF76EFFF2 +:10109000BDE8084001F068BB042E002008B50348F5 +:1010A000FFF764FFBDE8084001F05EBB6C32002032 +:1010B00010B50E4C0E4A2046002100F057FA0D4B99 +:1010C000C4F85C340C4C0D4A2046002100F04EFA66 +:1010D0000B4BC4F85C340B4C0B4A0021204600F04B +:1010E00045FA0A4BC4F85C3410BD00BF9C290020AF +:1010F0003D0F000800440040042E00204D0F000862 +:10110000004800406C3200205D0F0008007800406D +:1011100038B5394D037C002918BF0D46012B044614 +:10112000C0F8645410D1354B984242D1344B1A6CFC +:1011300042F400321A641A6E42F400321A661B6ED0 +:101140000B21262000F06AF82E4BD4F85C249A423A +:101150002B6802D02C498A424BD12C4901EB530118 +:10116000B1FBF3F3A988080442BF23F0070003F0A2 +:10117000070343EA40039360EB8843F040039BB2CC +:1011800013612B8943F001039BB2536141F4045373 +:1011900043F02C03D36001F4A05100231360B1F598 +:1011A000806F136853680CBF7F23FF2384F860347B +:1011B00038BD174B98420CD1114B1A6C42F4802267 +:1011C0001A641A6E42F480221A661B6E0B212720C5 +:1011D000B8E7104B9842B7D1094B1A6C42F08042E5 +:1011E0001A641A6E42F080421A661B6E0B2152205E +:1011F000A8E70949B2E700BF743B00089C2900201A +:1012000000380240001001400014014000BD0105FB +:10121000042E00206C32002080DE800200F160434A +:1012200000F01F02400903F5614309018000C9B2C3 +:1012300000F1604083F8001300F561400123934002 +:10124000C0F8803103607047F8B5154682680669BA +:10125000AA420B46816938BF8568761AB5420446B2 +:1012600007D218462A46FFF7D1FDA3692B44A36194 +:101270000DE011D932461846FFF7C8FDAF1B3A46BC +:10128000E1683044FFF7C2FDE2683A44A261A36816 +:101290005B1BA3602846F8BD18462A46FFF7B6FD3B +:1012A000E368E4E783682DE9F0410446934215467C +:1012B000266938BF85684069361AB5420F4606D29E +:1012C0002A46FFF7A3FD63692B4463610DE012D941 +:1012D0003246A5EB0608FFF799FD4246B919E068CA +:1012E000FFF794FDE26842446261A3685B1BA36060 +:1012F0002846BDE8F0812A46FFF788FDE368E4E769 +:1013000010B50A440024C361029B00604060846001 +:10131000C160816141610261036210BD08B58269EB +:101320004369934201D1826882B982680132826046 +:101330005A1C42611970426903699A4201D3C36819 +:101340004361002100F0AAFE002008BD4FF0FF30ED +:1013500008BD000070B5202304460E4683F31188B3 +:10136000A568A5B1A368A269013BA360531CA36152 +:1013700015782269934224BFE368A361E3690BB146 +:1013800020469847002383F31188284670BD3146D4 +:10139000204600F073FE0028E2DA85F3118870BD64 +:1013A0002DE9F74F05460F4690469A46D0F81C9017 +:1013B000202686F311884FF0000B144664B12246B4 +:1013C00039462846FFF740FF034668B9514628468C +:1013D00000F054FE0028F1D0002383F31188A8EB1D +:1013E000040003B0BDE8F08FB9F1000F03D0019005 +:1013F0002846C847019B8BF31188E41A1F4486F3E3 +:101400001188DBE7C16081614161C3611144009BC8 +:10141000006040608260016103627047F8B5044675 +:101420000E461746202383F31188A568A5B1A3684B +:10143000013BA36063695A1C62611E702369626983 +:101440009A4224BFE3686361E3690BB12046984781 +:10145000002080F31188F8BD3946204600F00EFECA +:101460000028E2DA85F31188F8BD0000836942693B +:101470009A4210B501D182687AB9826801328260DD +:101480005A1C82611C7803699A4201D3C368836144 +:10149000002100F003FE204610BD4FF0FF3010BDCC +:1014A0002DE9F74F05460F4690469A46D0F81C9016 +:1014B000202686F311884FF0000B144664B12246B3 +:1014C00039462846FFF7EEFE034668B951462846DE +:1014D00000F0D4FD0028F1D0002383F31188A8EB9D +:1014E000040003B0BDE8F08FB9F1000F03D0019004 +:1014F0002846C847019B8BF31188E41A1F4486F3E2 +:101500001188DBE7026843681143016003B11847A3 +:10151000704700001430FFF743BF00004FF0FF3367 +:101520001430FFF73DBF00003830FFF7B9BF0000AF +:101530004FF0FF333830FFF7B3BF00001430FFF730 +:1015400009BF00004FF0FF311430FFF703BF000068 +:101550003830FFF763BF00004FF0FF323830FFF73D +:101560005DBF000000207047FFF7A2BD37B50F4BED +:101570000360002343608360C36001230446037457 +:10158000154600900B464FF4007200F15C011430D8 +:10159000FFF7B6FE00942B464FF4007204F5177166 +:1015A00004F13800FFF72EFF03B030BD883B000880 +:1015B00010B52023044683F31188FFF7A9FD022309 +:1015C0002374002383F3118810BD000038B5C3696C +:1015D00004460D461BB904210844FFF793FF294632 +:1015E00004F11400FFF79AFE002806DA201D4FF4DC +:1015F0008061BDE83840FFF785BF38BD02684368A9 +:101600001143016003B118477047000013B5446BE4 +:10161000D4F894341A681178042915D1217C022950 +:1016200012D11979128901238B4013420CD101A9DF +:1016300004F14C0001F04EFED4F89444019B217952 +:101640000246206800F0DAF902B010BD143001F053 +:10165000D1BD00004FF0FF33143001F0CBBD0000CE +:101660004C3001F0A3BE00004FF0FF334C3001F0CE +:101670009DBE0000143001F09FBD00004FF0FF310F +:10168000143001F099BD00004C3001F06FBE000035 +:101690004FF0FF324C3001F069BE0000D0F89424C6 +:1016A00038B5136819780429054601D0012038BDE2 +:1016B000017C0229FAD112795C8901209040044012 +:1016C000F4D105F1140001F031FD02460028EDD0FF +:1016D000D5F894544FF480732868697900F07CF948 +:1016E000204638BD406BFFF7D9BF0000002070478F +:1016F000704700007FB5124B036000234360836096 +:10170000C360012502260F4B057404460290019325 +:1017100000F18402294600964FF48073143001F0E2 +:10172000E1FC094B0193029400964FF4807304F599 +:101730002372294604F14C0001F0A8FD04B070BDED +:10174000B03B0008E51600080D1600080B682022C3 +:1017500082F311880A7903EB820290614A79093297 +:1017600043F822008A7912B103EB820398610223C5 +:101770000374C0F89414002383F3118870470000A9 +:1017800038B5037F044613B190F85430ABB9201D2F +:1017900001250221FFF732FF04F1140025776FF0D5 +:1017A000010100F087FC84F8545004F14C006FF004 +:1017B0000101BDE8384000F07DBC38BD10B5012105 +:1017C00004460430FFF71AFF0023237784F85430CF +:1017D00010BD000038B504460025143001F09AFC15 +:1017E00004F14C00257701F069FD201D84F8545068 +:1017F0000121FFF703FF2046BDE83840FFF74EBF49 +:1018000090F8443003F06003202B19D190F8452064 +:10181000212A0AD0222A4FF000030ED0202A0FD10D +:10182000084A82630722C26304E0064B83630723EE +:10183000C36300230364012070478363C363F9E734 +:101840000020704701240020D0F8943437B51A687E +:101850001178042904461AD1017C022917D119797B +:10186000128901238B40134211D100F14C05284607 +:1018700001F0E4FD58B101A9284601F02BFDD4F890 +:101880009444019B21790246206800F0B7F803B028 +:1018900030BD000000EB8103F7B59C6905460E469C +:1018A000F4B1202383F3118805EB8607201D08215E +:1018B000FFF7A4FEFB685B691B684C3413B120463C +:1018C00001F016FD01A9204601F004FD024648B1D1 +:1018D000019B3146284600F091F8002383F31188DC +:1018E00003B0F0BDFB685A691268002AF5D01B8A64 +:1018F000013B1340F1D105F14402EAE7093138B563 +:1019000050F82140DCB1202383F31188D4F89424CB +:101910001368527903EB8203DB689B695D6845B10C +:1019200004216018FFF76AFE294604F1140001F053 +:1019300007FC2046FFF7B2FE002383F3118838BD71 +:101940007047000000F0C8BF01230370002343600C +:10195000C36183620362C3624362036303814381A1 +:101960007047000038B50446202383F31188002512 +:101970004160C56005614561856100F0BDFF0223DE +:10198000237085F3118838BD70B500EB81030546DF +:101990005069DA600E46144618B110220021FFF794 +:1019A0005FFAA06918B110220021FFF759FA3146F9 +:1019B0002846BDE8704001F069B80000028902F0D5 +:1019C00001020281428902F0010242810022026189 +:1019D0004261826101F0EEB8F0B400EB8104478906 +:1019E000E4680125A4698D403D43458123600023BF +:1019F000A2606360F0BC01F009B90000F0B400EB34 +:101A000081040789E468012564698D403D430581AF +:101A100023600023A2606360F0BC01F083B9000082 +:101A200070B50223002504460370A0F84C5080F8DE +:101A30004E5080F84F5005814581C5600561456174 +:101A4000856180F8345000F0BDFF63681B6823B1E6 +:101A500029462046BDE87040184770BD43680278AB +:101A60001B6880F85020052202700BB10421184732 +:101A700070470000436890F850201B6802700BB15B +:101A8000052118477047000090F8343070B50446BF +:101A900013B1002380F8343004F14402204601F0F1 +:101AA000ABF863689B6863BB94F8445015F060061C +:101AB00015D194F8453005F07F0545EA032540F23D +:101AC00002339D4200F00E815BD8022D00F0DC80D5 +:101AD0003FD8002D00F08780012D00F0CF8000213D +:101AE000204601F0E1FA0021204601F0D3FA6368B4 +:101AF0001B6813B1062120469847062384F834302A +:101B000070BD204698470028CED094F84B2094F81A +:101B10004A3043EA0223E26B934238BFE36394F90D +:101B20004430E56B002B4FF0200380F2FD80002D48 +:101B300000F0EC80092284F8342083F3118800211E +:101B4000E36BA26B2046FFF759FF002383F3118854 +:101B500070BDB5F5817F00F0B180B5F5407F49D00B +:101B6000B5F5807FBBD194F84630012BB7D1B4F8DE +:101B70004C3023F00203A4F84C30A663E6632664DD +:101B8000C3E740F201639D421ED8B5F5C06F3BD25A +:101B9000B5F5A06FA3D1B4F84430B3F5A06F0ED162 +:101BA00094F8463084F84E30204600F063FF6368B6 +:101BB0001B6813B10121204698470323237000239B +:101BC000A363E3632364A0E7B5F5106F32D040F65A +:101BD00002439D4252D0B5F5006F80D104F14F030E +:101BE000A363012324E004F14C03A3630223E36312 +:101BF00025648AE794F84630012B7FF470AFB4F87F +:101C00004C3043F00203B6E794F84920616894F839 +:101C100048304D6894F8471043EA0223204694F870 +:101C20004620A84700283FF45AAF4368A3630368DF +:101C3000E363A4E72378042B10D1202383F31188D6 +:101C40002046FFF7BBFE86F31188636884F84F6077 +:101C50001B68032121700BB12046984794F8463049 +:101C6000002BACD084F84F300423237063681B68CA +:101C7000002BA4D0022120469847A0E7374BA3634E +:101C80000223E36300239DE794F8481011F0800FCE +:101C9000204601F00F010ED000F0A0FF012806D071 +:101CA00002287FF41CAF2E4BA363E06367E72D4B44 +:101CB000A363E56363E700F083FFEFE794F8463042 +:101CC000002B7FF40CAF94F8483013F00F013FF471 +:101CD00076AF1A06204602D501F0FAF96FE701F057 +:101CE000EDF96CE794F84630002B7FF4F8AE94F8E9 +:101CF000483013F00F013FF462AF1B06204602D5B7 +:101D000001F0D2F95BE701F0C5F958E7142284F835 +:101D1000342083F311882B462A4629462046FFF7B4 +:101D20005BFE85F3118870BD5DB1152284F8342007 +:101D300083F311880021E36BA26B2046FFF74CFE72 +:101D400003E70B2284F8342083F311882B462A46BC +:101D500029462046FFF752FEE3E700BFE03B0008BC +:101D6000D83B0008DC3B000838B590F83430044616 +:101D7000152B29D8DFE803F03E28282828283E28FC +:101D8000280B293928282828282828283E3E90F87A +:101D90004B1090F84A20C36B42EA01229A4214D9B0 +:101DA000C268128AB3FBF2F502FB15356DB9202328 +:101DB00083F311882B462A462946FFF71FFE85F339 +:101DC00011880A2384F8343038BD142384F8343061 +:101DD000202383F3118800231A4619462046FFF773 +:101DE000FBFD002383F3118838BD036C03B19847D2 +:101DF0000023E7E7002101F057F90021204601F018 +:101E000049F963681B6813B10621204698470623E9 +:101E1000D8E7000090F83420152A38B5044622D8B7 +:101E20000123934040F6416213421DD113F4801503 +:101E30000FD19B0217D50B2380F83430202383F376 +:101E400011882B462A462946FFF7D8FD85F31188CD +:101E500038BDC3689B695B682BB9036C03B19847B5 +:101E6000002384F8343038BD002101F01DF9002131 +:101E7000204601F00FF963681B6813B10621204664 +:101E800098470623EDE70000024B00221B605B60D1 +:101E90009A607047D4360020002303748268054B93 +:101EA0001B6899689142FBD25A68036042601060D7 +:101EB00058607047D436002008B5202383F311887A +:101EC000037C032B05D0042B0DD02BB983F3118891 +:101ED00008BD436900221A604FF0FF334361FFF7EA +:101EE000DBFF0023F2E790E80C001A600268536001 +:101EF000F2E70000002303748268054B1B689968B1 +:101F00009142FBD85A680360426010605860704785 +:101F1000D4360020054B19690874186802681A60E5 +:101F20005360186101230374FEF7EEB9D436002024 +:101F300030B54B1C87B005460A4C10D023690A4ABD +:101F400001A800F0D3F82846FFF7E4FF049B13B183 +:101F500001A800F007F92369586907B030BDFFF701 +:101F6000D9FFF8E7D4360020B91E000838B50C4D6B +:101F700041612B6981689A689142044603D8BDE8A3 +:101F80003840FFF789BF1846FFF786FF01232C6111 +:101F9000014623742046BDE83840FEF7B5B900BFBE +:101FA000D4360020044B1A681B6990689B689842DD +:101FB00094BF002001207047D436002010B5084C93 +:101FC000236820691A6822605460012223611A7410 +:101FD000FFF790FF01462069BDE81040FEF794B975 +:101FE000D4360020826002220274002242747047BC +:101FF0008368A3F17C0243F80C2C026943F83C2C63 +:10200000426943F8382C074A43F81C2CC26843F84D +:10201000102C022203F8082C002203F8072CA3F14D +:10202000180070472103000810B5202383F311889E +:10203000FFF7DEFF00210446FFF798FF002383F33C +:102040001188204610BD0000024B1B6958610F200B +:10205000FFF760BFD4360020202383F31188FFF7F9 +:10206000F3BF000008B50146202383F31188082040 +:10207000FFF75EFF002383F3118808BD49B1064BCB +:1020800042681B6918605A60136043600420FFF7C0 +:102090004FBF4FF0FF307047D4360020036898429E +:1020A00006D01A680260506059611846FFF7F4BE06 +:1020B0007047000038B504460D462068844200D1C0 +:1020C00038BD036823605C604561FFF7E5FEF4E717 +:1020D000054B03F114025A619A614FF0FF32DA6145 +:1020E00000221A62704700BFD4360020F8B50361A1 +:1020F0001A4BC2605C6A1A4B1A46022952F8145FE6 +:1021000038BF0221954206461F460AD158619861A0 +:102110001C620560456081600819BDE8F84001F067 +:10212000AFB9186AAB68241A0C1902D3E41A2D68E7 +:1021300004E09C4202D2204401F0B2F9AB689C4218 +:10214000F4D86B68736035601E606E60B460A96817 +:102150004FF0FF33091BA960FB61F8BD000C004084 +:10216000D436002010B41A4C234653F8141F814271 +:1021700010D0416802680A60026851609A424FF0CC +:102180000001C16003D0936881680B4493605DF8DF +:10219000044B70470A68626100209A425360C8602D +:1021A00003D15DF8044B01F075B993688868034466 +:1021B0009360084A206A526A121A9342E7D9991A20 +:1021C000012998BF931C18445DF8044B01F068B9CD +:1021D000D4360020000C004000207047FEE70000CD +:1021E000704700004FF0FF3070470000022906D012 +:1021F000032906D00129064818BF0020704705486A +:102200007047032A9ABF044800EBC20000207047C1 +:10221000B43C0008683C00080824002070B59AB05F +:1022200001AD064608462946144600F095F82846B2 +:10223000FEF70EFEC0B2431C5B0086E81800237058 +:1022400003236370002302341946DAB2904204F18A +:10225000020401D81AB070BDEA5C04F8022C04F83C +:10226000011C0133F1E7000008B5202383F3118836 +:102270000348FFF7E9FA002383F3118808BD00BF84 +:102280001437002010B50446052916D8DFE801F000 +:1022900016150316161D202383F311880E4A0121FB +:1022A000FFF772FB20460D4A0221FFF76DFB0C4839 +:1022B000FFF790FA002383F3118810BD202383F3E6 +:1022C00011880748FFF75CFAF4E7202383F31188AD +:1022D0000348FFF773FAEDE7E43B0008083C000809 +:1022E0001437002038B50C4D0C4C0D492A4604F12A +:1022F0000800FFF793FF05F1CA0204F11000094935 +:10230000FFF78CFF05F5CA7204F118000649BDE815 +:102310003840FFF783BF00BFDC3B002008240020CB +:10232000343C0008413C00084E3C000870B50446AF +:1023300008460D46FEF78CFDC6B2204601340378F0 +:102340000BB9184670BD32462946FEF769FD0028D4 +:10235000F3D1012070BD00002DE9F84F05460C4671 +:10236000FEF776FD2B49C6B22846FFF7DFFF08B11E +:102370000236F6B228492846FFF7D8FF08B11036D2 +:10238000F6B2632E0DD8DFF88C80DFF88C90234FE7 +:10239000DFF890A0DFF890B02E7846B92670BDE83F +:1023A000F88F29462046BDE8F84F01F09BBB252E4B +:1023B0002CD1072241462846FEF732FD58B9DBF8FA +:1023C00000302360DBF8043063609BF80830237230 +:1023D00007350934E0E7082249462846FEF720FD84 +:1023E000A0B90F4BA21C13F8011F09095345C95D81 +:1023F00002F8021C197801F00F0102F10202C95D16 +:1024000002F8031CEFD118340835C5E726700135F2 +:102410000134C1E7D43C00084E3C0008E63C00080B +:102420000F7AFF1F1B7AFF1FDC3C0008BFF34F8FA2 +:10243000024AD368DB03FCD4704700BF003C024073 +:1024400008B5094B1B7873B9FFF7F0FF074B1A6902 +:10245000002ABFBF064A5A6002F188325A601A68E1 +:1024600022F480621A6008BD3A3E0020003C02401F +:102470002301674508B50B4B1B7893B9FFF7D6FFCF +:10248000094B1A6942F000421A611A6842F48052FC +:102490001A601A6822F480521A601A6842F4806244 +:1024A0001A6008BD3A3E0020003C0240172870B573 +:1024B00013D80B4A0B4C137863B90B4E4FF00061E5 +:1024C00044F8231056F823500133182B2944F7D130 +:1024D0000123137054F8200070BD002070BD00BFB0 +:1024E0009C3E00203C3E0020F83C0008014B53F885 +:1024F00020007047F83C000818207047172810B5D6 +:10250000044601D9002010BDFFF7D0FF064B53F859 +:1025100024301844C21A0BB9012010BD12680132D0 +:10252000F0D1043BF6E700BFF83C0008172810B5CF +:10253000044626D8FFF77AFFFFF782FF1249F323FC +:10254000CB60FFF773FF0C23B0FBF3F203FB120029 +:10255000D30143EAC003DBB243F4007343F0020348 +:102560000B610B6943F480330B61FFF75FFF20467B +:10257000FFF79CFFFFF77EFF2046BDE81040FFF706 +:10258000BDBF002010BD00BF003C02402DE9F84354 +:1025900012F00103154643D10244B2F1026F3FD25B +:1025A0002C4B1B6813F001033AD02B4CFFF748FF6C +:1025B000F323E360FFF73AFF40F20127032D16D81B +:1025C000254C0F46401A40F20118EB1B0B44012B1F +:1025D00000EB0706236926D823F001032361FFF7E8 +:1025E00049FF0120BDE8F883043D0430E6E7830796 +:1025F000E6D1236923F44073236123693B432361BC +:1026000051F8046B0660BFF34F8FFFF70FFF0368AD +:102610009E42E9D0236923F001032361FFF72AFFDB +:102620000020BDE8F88323F440732361236943EA63 +:1026300008032361B94637F8023B3380BFF34F8F5D +:10264000FFF7F4FE3688B9F80030B6B2B342BCD01A +:10265000E0E700BF00380240003C0240084908B5EE +:102660000B7828B153B9FFF7EBFE01230B7008BDBF +:1026700023B1BDE808400870FFF7FCBE08BD00BFED +:102680003A3E002010B50244064B0439D2B29042C3 +:1026900000D110BD441C00B253F8200041F8040FD3 +:1026A000E0B2F4E750280040104B30B51C6F240412 +:1026B00007D41C6F44F400741C671C6F44F400447E +:1026C0001C670B4C236843F4807323600244094B5E +:1026D0000439D2B2904200D130BD441C00B251F84E +:1026E000045F43F82050E0B2F4E700BF0038024036 +:1026F000007000405028004007B5012201A90020C9 +:10270000FFF7C0FF019803B05DF804FB07B502A90D +:10271000012241F8040D0020FFF7C6FF03B05DF869 +:1027200004FB0000034B1B689B0042BF024B0122CD +:102730001A707047743802409D3E0020014B187893 +:10274000704700BF9D3E0020064A536823F00103F6 +:102750005360EFF30983683383F30988002383F31D +:102760001188704730EF00E010B5202383F3118803 +:10277000104B5B6813F4006318D0F1EE103AEFF3DE +:1027800009844FF0807344F84C3C0B4BDB6844F8F1 +:10279000083CA4F1680383F30988FFF703FC18B130 +:1027A000064B44F8503C10BD054BFAE783F3118803 +:1027B00010BD00BF00ED00E030EF00E03103000885 +:1027C0003403000870470000FEE70000084A094B88 +:1027D00009498B4204D3094A0021934205D370472B +:1027E00052F8040F43F8040BF3E743F8041BF4E733 +:1027F000D43E0008904100209041002090410020EC +:10280000836D30B500229D68446D11464FF0FF3056 +:1028100004EB421301329542C3F80019C3F81019B2 +:10282000C3F80809C3F8001BC3F8101BC3F8080B52 +:10283000EED24FF00113C4F81C3830BD890141F0CD +:102840002001016103699B06FCD4122000F0B4BE94 +:10285000204B03EB80022DE9F047D2F80CE05D6DD0 +:10286000DEF81420461CD2F800C005EB063605EB56 +:102870004018516861450BD3D5F83428012303FA79 +:1028800000F022EA0000C5F834081846BDE8F087D9 +:10289000BEF81040ACEB0103A34228BF2346D8F892 +:1028A0001849A4B2B3EB840F10D894681F46A4F162 +:1028B000040959F804AFC6F800A0042F01D9043F59 +:1028C000F7E71C440B4494605360D2E70020BDE856 +:1028D000F08700BFA03E002010B5054C2046FFF752 +:1028E00033F84FF0A0436365024BA36510BD00BFF2 +:1028F000A03E00207C3D00080378012B70B5054602 +:1029000050D12A4B446D98421BD1294B5A6B42F04F +:1029100080025A635A6D42F080025A655A6D5A69B4 +:1029200042F080025A615A6922F080025A610E21F7 +:1029300043205B69FEF772FC1E4BE3601E4BC4F83C +:1029400000380023C4F8003EC02323606E6D4FF4AE +:102950005023A3633369002BFCDA012333610C207D +:1029600000F02AFE3369DB07FCD4122000F024FEBD +:102970003369002BFCDA0026A6602846FFF740FFEB +:102980006B68C4F81068DB68C4F81468C4F81C6885 +:102990004BB90A4BA3614FF0FF336361A36843F067 +:1029A0000103A36070BD064BF4E700BFA03E00200A +:1029B000003802404014004003002002003C30C0B8 +:1029C000083C30C0F8B5446D054600212046FFF7AD +:1029D00035FFA96D00234FF001128F68C4F8343819 +:1029E0004FF00066C4F81C284FF0FF3004EB431290 +:1029F00001339F42C2F80069C2F8006BC2F80809AF +:102A0000C2F8080BF2D20B686A6DEB6563621023A3 +:102A10001361166916F01006FBD1122000F0CCFDF0 +:102A2000D4F8003823F4FE63C4F80038A36943F4F3 +:102A3000402343F01003A3610923C4F81038C4F8FD +:102A400014380A4BEB604FF0C043C4F8103B084BFE +:102A5000C4F8003BC4F81069C4F80039EB6D03F109 +:102A6000100243F48013EA65A362F8BD583D0008E4 +:102A700040800010426D90F84E10D2F8003823F4D8 +:102A8000FE6343EA0113C2F8003870472DE9F041B4 +:102A90000EB200EB86080D46D8F80C100F6807F050 +:102AA0000303022B50D0032B50D03D4A3D4F012B46 +:102AB00018BF1746446D4FEA451E04EB0E03002273 +:102AC000C3F8102B8A6905F1100C002A40D04A8AFD +:102AD00005F158035B013A43E2500123D4F81C2866 +:102AE00003FA0CF31343A6444A69C4F81C380023C4 +:102AF000CEF8103905F13F03002A39D00A8A898BB4 +:102B00009208012988BF4A43C16D04EB830356181C +:102B100041EA0242C66529465A602046FFF78EFE0A +:102B2000D8F80C301B8A43EA85531F4305F148034C +:102B30005B01E7500123D4F81C2803FA05F515437F +:102B4000C4F81C58BDE8F081174FB3E7174FB1E741 +:102B500004EB4613D3F8002B22F40042C3F8002BF9 +:102B6000D4F81C28012303FA0CF322EA0303BAE782 +:102B700004EB83030E4A5A6004EB461629462046AE +:102B8000FFF75CFED6F8003923F40043C6F800399D +:102B9000D4F81C38012202FA05F523EA0505CFE72F +:102BA00000800010008004100080081000800C10CD +:102BB00000040002826D1268C265FFF721BE0000AA +:102BC0005831436D49015B5813F4004004D013F4AD +:102BD000001F14BF01200220704700004831436DE0 +:102BE00049015B5813F4004004D013F4001F14BFD4 +:102BF000012002207047000000EB8101CB68196AB8 +:102C00000B6813604B6853607047000000EB810352 +:102C100030B5DD68AA691368D36019B9402B84BF49 +:102C2000402313606B8A1468426D1C44013CB4FB62 +:102C3000F3F46343033323F0030302EB411043EA4D +:102C4000C44343F0C043C0F8103B2B6803F00303B8 +:102C5000012B09B20ED1D2F8083802EB411013F45F +:102C6000807FD0F8003B14BF43F0805343F0005303 +:102C7000C0F8003B02EB4112D2F8003B43F00443A2 +:102C8000C2F8003B30BD00002DE9F041244D6E6DCF +:102C900006EB40130446D3F8087BC3F8087B3807DB +:102CA0000AD5D6F81438190706D505EB8403214652 +:102CB000DB6828465B689847FA072FD5D6F81438A2 +:102CC000DB072BD505EB8403D968CCB98B69488A1F +:102CD0005E68B6FBF0F200FB12628AB91868DA6827 +:102CE00090420DD2121A83E81400202383F3118836 +:102CF0000B482146FFF78AFF84F31188BDE8F08175 +:102D0000012303FA04F26B8923EA02036B81CB6887 +:102D100023B121460248BDE8F0411847BDE8F081E3 +:102D2000A03E002000EB810370B5DD68436D6C6947 +:102D30002668E6604A0156BB1A444FF40020C2F8E8 +:102D400010092A6802F00302012A0AB20ED1D3F850 +:102D5000080803EB421410F4807FD4F8000914BF74 +:102D600040F0805040F00050C4F8000903EB4212DC +:102D7000D2F8000940F00440C2F80009D3F8340842 +:102D8000012202FA01F10143C3F8341870BD19B9E8 +:102D9000402E84BF4020206020682E8A841940F095 +:102DA0000050013C1A44B4FBF6F440EAC440C6E7C4 +:102DB000F8B504461E48456D05EB4413D3F8086981 +:102DC000C3F80869F10717D5D5F81038DA0713D515 +:102DD00000EB8403D9684B691F68DA68974218D200 +:102DE000D21B00271A605F60202383F311882146DD +:102DF000FFF798FF87F31188330617D5D5F83428E5 +:102E00000123A340134211D02046BDE8F840FFF74C +:102E10001FBD012303FA04F2038923EA020303819D +:102E20008B68002BE8D021469847E5E7F8BD00BF46 +:102E3000A03E002096482DE9F84F456D6E69AB69BC +:102E40001E4016F4805F6E61044605D0FEF7E8FD73 +:102E5000BDE8F84FFFF788BC002E12DAD5F8003E27 +:102E60008B489B071EBFD5F8003E23F00303C5F82F +:102E7000003ED5F8043823F00103C5F80438FEF706 +:102E8000F9FD370502D58248FEF7E8FDB0040CD500 +:102E9000D5F8083813F0060FEB6823F470530CBF15 +:102EA00043F4105343F4A053EB60310704D5636837 +:102EB000DB680BB176489847F2025AD4B3020CD5BE +:102EC000D4F85480DFF8C8A100274FF00109A36DA2 +:102ED0009B68F9B28B4280F08780F70619D5616D47 +:102EE0000A6AC2F30A1702F00F0302F4F012B2F5F5 +:102EF000802F00F0A180B2F5402F0AD104EB8303AC +:102F000001F58051DB68186A00231A469F4240F0A1 +:102F100087803003D5F8184813D5E10302D5002087 +:102F2000FFF7B2FEA20302D50120FFF7ADFE630357 +:102F300002D50220FFF7A8FE270302D50320FFF7E2 +:102F4000A3FE750384D5E00702D50020FFF730FF0C +:102F5000A10702D50120FFF72BFF620702D502204F +:102F6000FFF726FF23077FF573AF0320FFF720FF4E +:102F70006EE7D4F85480DFF818A100274FF001095C +:102F8000A36D9B685FFA87FB5B4597D308EB4B13F8 +:102F9000D3F8002902F44022B2F5802F22D1D3F8D1 +:102FA0000029002A1EDAD3F8002942F09042C3F823 +:102FB0000029D3F80029002AFBDB5946606DFFF792 +:102FC0003DFC228909FA0BF322EA0303238104EB77 +:102FD0008B03DB689B6813B15946504698475846A7 +:102FE000FFF736FC0137CBE708EB4112D2F8003B84 +:102FF00003F44023B3F5802F10D1D2F8003B002B0F +:103000000CDA628909FA01F322EA0303638104EB13 +:103010008103DB68DB680BB150469847013756E700 +:103020009C0708BF0A68072B98BF027003F10103D1 +:103030009CBF120A013069E7023304EB830201F5F9 +:103040008051526890690268D0F808C04068A2EBCD +:10305000000E0022104697420AD104EB83046368F5 +:103060009B699A683A449A605A6817445F6050E7CF +:1030700012F0030F08BF0868964588BF8CF800005F +:1030800002F1010284BF000A0CF1010CE3E700BF6A +:10309000A03E0020436D03EB4111D1F8003B43F407 +:1030A0000013C1F8003B7047436D03EB4111D1F8A9 +:1030B000003943F40013C1F800397047436D03EB46 +:1030C0004111D1F8003B23F40013C1F8003B7047D5 +:1030D000436D03EB4111D1F8003923F40013C1F81B +:1030E0000039704730B5039C0172043304FB03259B +:1030F000C361049B03630021059B00604060C160C5 +:10310000426102618561046242628162C16243631D +:1031100030BD00000022416A41610161C2608262EB +:10312000C2626FF00101FEF7C5BF0000036942698A +:10313000934203D1C2680AB100207047181D70473E +:1031400003691960C2680132C260C26913448269AE +:103150000361934224BF436A03610021FEF79EBFCF +:1031600038B504460D46E3683BB16269131D126829 +:10317000A3621344E362002038BD237A33B92946A1 +:103180002046FEF77BFF0028EDDA38BD6FF0010026 +:10319000FBE70000C368C269013BC3604369134495 +:1031A00082694361934224BF436A4361002383627F +:1031B000036B03B11847704770B52023044683F3AF +:1031C0001188866A3EB9FFF7CBFF054618B186F332 +:1031D0001188284670BDA36AE26A13F8015BA362F6 +:1031E000934202D32046FFF7D5FF002383F31188D3 +:1031F000EFE700002DE9F84F04460E469046994649 +:10320000202787F311880025AA46D4F828B0BBF1FF +:10321000000F09D149462046FFF7A2FF20B18BF3EA +:1032200011882846BDE8F88FA16AE36AA8EB050B70 +:103230005B1A9B4528BF9B46BBF1400F1BD9334609 +:1032400001F1400251F8040B43F8040B9142F9D10B +:10325000A36A40334036A3624035A26AE36A9A42C9 +:1032600002D32046FFF796FF8AF311884545D8D24E +:1032700087F31188C9E730465A46FDF7C7FDA36AB0 +:103280005B445E44A3625D44E7E7000010B5029C26 +:103290000172043303FB0421C36100238362C36210 +:1032A000039B0363049B00604060C4604261026151 +:1032B000816104624262436310BD0000026AC26021 +:1032C000426A4261026100228262C2626FF00101C1 +:1032D000FEF7F0BE436902699A4203D1C2680AB19F +:1032E00000207047184650F8043B0B607047000000 +:1032F000C368C2690133C36043691344826943618F +:10330000934224BF436A43610021FEF7C7BE000019 +:1033100038B504460D46E3683BB123691A1DA26225 +:10332000E2691344E362002038BD237A33B92946A9 +:103330002046FEF7A3FE0028EDDA38BD6FF001004D +:10334000FBE7000003691960C268013AC260C26904 +:10335000134482690361934224BF436A03610023DB +:103360008362036B03B118477047000070B52023D8 +:1033700004460E4683F31188856A35B91146FFF776 +:10338000C7FF10B185F3118870BDA36A1E70A36AD0 +:10339000E26A01339342A36204D3E169204604390F +:1033A000FFF7D0FF002080F3118870BD2DE9F84FA2 +:1033B00004460D4691469A464FF0200888F311883E +:1033C0000026B346A76A4FB951462046FFF7A0FF33 +:1033D00020B187F311883046BDE8F88FA06AE76A0C +:1033E000A9EB06033F1A9F4228BF1F46402F1BD957 +:1033F00005F1400355F8042B40F8042B9D42F9D108 +:10340000A36A4033A3624036A26AE36A9A4204D3B5 +:10341000E16920460439FFF795FF8BF311884E458B +:10342000D9D288F31188CDE729463A46FDF7EEFC5C +:10343000A36A3B443D44A3623E44E5E70269436915 +:103440009A4203D1C3681BB9184670470023FBE7B3 +:10345000836A002BF8D0043B9B1AF5D01360C36835 +:10346000013BC360C3691A44836902619A4224BF65 +:10347000436A0361002383620123E5E700F034B966 +:10348000034B002258631A610222DA60704700BFC2 +:10349000000C0040014B0022DA607047000C004035 +:1034A000014B5863704700BF000C0040FEE700006E +:1034B00010B5174CFEF7E8FCFEF70AFE802215490E +:1034C0002046FEF78FFD012344F8180C0374124BBD +:1034D000124AD96821F4E0610904090C0A43104931 +:1034E000DA60CA6842F08072CA600E490A6842F027 +:1034F00001020A601022DA77202283F822200023BA +:1035000083F3118862B60848BDE81040FEF78CBD11 +:10351000FC360020883D000800ED00E00003FA05BD +:10352000F0ED00E0001000E0903D00082DE9F84FBC +:103530001F4CDFF88090656904F114074FF0000814 +:10354000D9F82430266AAA689E1B96421DD34FF0F4 +:10355000200AAA68236AD5F80CB0134423622B68AA +:10356000BB425F60A6EB02066361C5F80C8001D127 +:10357000FFF790FF88F311882869D8478AF31188EC +:103580006569AB689E42E4D2DAE76269BA420CD060 +:10359000916823628E1B9660A86802282CBF1818B9 +:1035A000981CBDE8F84FFFF77BBFBDE8F88F00BF60 +:1035B000D4360020000C0040034B59685A68521A58 +:1035C0009042FBD8704700BF001000E04B6843609A +:1035D0008B688360CB68C3600B6943614B6903628E +:1035E0008B6943620B6803607047000008B5224B8B +:1035F00022481A6940F2FF110A431A611A6922F43B +:10360000FF7222F001021A611A691A6B0A431A63E7 +:103610001A6D0A431A651A4A1B6D1146FFF7D6FF49 +:10362000184802F11C01FFF7D1FF174802F13801D9 +:10363000FFF7CCFF154802F15401FFF7C7FF14480C +:1036400002F17001FFF7C2FF124802F18C01FFF78F +:10365000BDFF114802F1A801FFF7B8FF0F4802F1C2 +:10366000C401FFF7B3FF0E4802F1E001FFF7AEFF20 +:10367000BDE8084000F0A4B8003802400000024055 +:10368000B03D00080004024000080240000C024067 +:10369000001002400014024000180240001C0240CA +:1036A0000020024008B500F0FFF9FFF701FFFFF727 +:1036B00039F8BDE80840FEF715BE0000704700006D +:1036C000084B1A69920710B508D500241C61202305 +:1036D00083F31188FFF72AFF84F31188BDE81040B7 +:1036E000FFF742B8000C0040104B1A6C42F0080281 +:1036F0001A641A6E42F008021A660D4A1B6E93682D +:1037000043F0080393600B4B53229A624FF0FF3251 +:10371000DA6200229A615A63DA605A6001225A61C1 +:1037200008211A603220FDF779BD00BF0038024041 +:10373000002004E0000C00401F4B1A696FEAC252DF +:103740006FEAD2521A611A69C2F308021A614FF085 +:10375000FF301A695A69586100215A6959615A69DA +:103760001A6A62F080521A621A6A02F080521A6271 +:103770001A6A5A6A58625A6A59625A6A1A6C42F04C +:1037800080521A641A6E42F080521A661A6E0B4A00 +:10379000106840F480701060186F00F44070B0F54D +:1037A000007F1EBF4FF4803018671967536823F4F9 +:1037B0000073536000F058B90038024000700040B8 +:1037C000324B4FF080521A64314A4FF4404111603D +:1037D0001A6842F001021A601A689207FCD59A68CA +:1037E00022F003029A60294B19469A6812F00C02E3 +:1037F000FBD1186800F0F90018609A601A6842F46A +:1038000080321A600B689B03FCD54B6F43F00103B9 +:103810004B671E4B5A6F9007FCD51E4A5A601A68B8 +:1038200042F080721A601A4A53685904FCD5174B4B +:103830001A689201FCD5184A9A600322C3F88C20BA +:10384000164B1A68164B9A42164B1BD1164A116832 +:10385000164A914216D140F205121A600B4B9A6833 +:1038600042F002029A609A6802F00C02082AFAD129 +:103870005A6C42F480425A645A6E42F480425A664C +:103880005B6E704740F20572E7E700BF0038024008 +:10389000007000401854400700948838002004E06D +:1038A00011640020003C024000ED00E041C20F41E5 +:1038B000084A08B5536911680B4003F001035361CE +:1038C00023B1054A13680BB150689847BDE808401A +:1038D000FEF74ABF003C014010410020084A08B5ED +:1038E000536911680B4003F00203536123B1054A89 +:1038F00093680BB1D0689847BDE80840FEF734BF25 +:10390000003C014010410020084A08B55369116885 +:103910000B4003F00403536123B1054A13690BB153 +:1039200050699847BDE80840FEF71EBF003C0140C3 +:1039300010410020084A08B5536911680B4003F094 +:103940000803536123B1054A93690BB1D0699847C5 +:10395000BDE80840FEF708BF003C014010410020D0 +:10396000084A08B5536911680B4003F0100353610E +:1039700023B1054A136A0BB1506A9847BDE8084065 +:10398000FEF7F2BE003C014010410020174B10B57D +:103990005C691A68144004F478725A61A30604D56D +:1039A000134A936A0BB1D06A9847600604D5104A4F +:1039B000136B0BB1506B9847210604D50C4A936BDF +:1039C0000BB1D06B9847E20504D5094A136C0BB1D3 +:1039D000506C9847A30504D5054A936C0BB1D06C85 +:1039E0009847BDE81040FEF7BFBE00BF003C014055 +:1039F000104100201A4B10B55C691A68144004F499 +:103A00007C425A61620504D5164A136D0BB1506DA4 +:103A10009847230504D5134A936D0BB1D06D984791 +:103A2000E00404D50F4A136E0BB1506E9847A10401 +:103A300004D50C4A936E0BB1D06E9847620404D53E +:103A4000084A136F0BB1506F9847230404D5054AF9 +:103A5000936F0BB1D06F9847BDE81040FEF784BE5E +:103A6000003C014010410020062108B50846FDF742 +:103A7000D5FB06210720FDF7D1FB06210820FDF725 +:103A8000CDFB06210920FDF7C9FB06210A20FDF721 +:103A9000C5FB06211720FDF7C1FB06212820BDE844 +:103AA0000840FDF7BBBB000008B5FFF745FE00F07E +:103AB0000DF8FDF759FDFDF745FFFDF719FEFFF77E +:103AC000FDFDBDE80840FFF7D9BC00000023044914 +:103AD0001A465A50C8180833802B4260F9D17047F3 +:103AE00010410020034611F8012B03F8012B002A96 +:103AF000F9D17047121012131211000053544D33B4 +:103B000032463F3F3F0000000120330000104101DA +:103B100001105A0103105901071031010000000083 +:103B2000FC3A00083F00000013040000543B00086A +:103B30003F000000190400005E3B00083F00000049 +:103B400021040000683B00083F00000014370020FB +:103B50009C29002053544D3332463430780053545E +:103B60004D3332463432780053544D333246343478 +:103B700036585800009600000000000000000000C9 +:103B800000000000000000000000000031150008E7 +:103B90001D150008591500084515000851150008A5 +:103BA0003D150008291500081515000865150008C1 +:103BB000000000006916000855160008911600085C +:103BC0007D160008891600087516000861160008A1 +:103BD0004D160008ED16000800000000010000006E +:103BE000000000000200000000000000951800081E +:103BF000FD18000840004000AC3B0020BC3B00200A +:103C000002000000000000000300000000000000AF +:103C1000411900080000000010000000CC3B00200B +:103C2000000000000100000000000000A03E002095 +:103C3000010102004865782F50726F6669434E4358 +:103C40000043756265426C61636B2D424C002553E5 +:103C5000455249414C25000085220008ED2100080D +:103C6000011800086922000843000000703C0008A9 +:103C700009024300020100C03209040000010202EF +:103C800001000524001001052401000104240202A2 +:103C90000524060001070582030800FF090401004E +:103CA000020A00000007050102400000070581022A +:103CB0004000000012000000BC3C0008120110018E +:103CC00002000040AE2D01100002010203010000BD +:103CD0000403090425424F4152442500437562659F +:103CE000426C61636B0030313233343536373839EA +:103CF00041424344454600000040000000400000AF +:103D00000040000000400000000001000000020030 +:103D1000000002000000020000000200000002009B +:103D2000000002000000020000400000004000000F +:103D30000040000000400000000001000000020000 +:103D4000000002000000020000000200000002006B +:103D5000000002000000020000000000891A0008B4 +:103D6000691D0008151E000840004000003F0020AB +:103D7000003F002001000000103F002080000000F4 +:103D800040010000030000006D61696E000000004A +:103D9000A83D0008183F002010410020010000004D +:103DA000AD3400080000000069646C65000000008C +:103DB0000000812A00020000AAAAAAAA00000000AE +:103DC000FFFF00000000000000A00A00000000004B +:103DD00000000000AAAAAAAA00000000FFFF00003D +:103DE00000000000000000000000000000000000D3 +:103DF000AAAAAAAA00000000FFFF0000000000001D +:103E00000000000080298A0100000000AAAAAAAAD6 +:103E100040154501FFFF000000707007777000003B +:103E20004080020100000000AAAAAAAA00400100E6 +:103E3000F7FF000000000080080000000000000004 +:103E400000000000AAAAAAAA00000000FFFF0000CC +:103E50000000000000000000000000000000000062 +:103E6000AAAAAAAA00000000FFFF000000000000AC +:103E70000000000000000000000000000A00000038 +:103E8000000000000300000000000000000000002F +:103E90000000000000000000000000000000000022 +:103EA0000000000000000000000000000000000012 +:103EB000FF0096000000000804000000D03C00084D +:103EC00000000000000000000000000000000000F2 +:083ED0000000000000000000EA +:00000001FF diff --git a/Tools/bootloaders/MttrCubeOrange_bl.bin b/Tools/bootloaders/MttrCubeOrange_bl.bin new file mode 100755 index 0000000000..022f275dca Binary files /dev/null and b/Tools/bootloaders/MttrCubeOrange_bl.bin differ diff --git a/Tools/scripts/mttr-build-ci.sh b/Tools/scripts/mttr-build-ci.sh new file mode 100755 index 0000000000..bda062f96c --- /dev/null +++ b/Tools/scripts/mttr-build-ci.sh @@ -0,0 +1,24 @@ +#!/bin/bash + +set -ex + +. ~/.profile + +mkdir -p "$HOME/deploy_files" + +python Tools/autotest/param_metadata/param_parse.py --vehicle ArduCopter +mv apm.pdef.xml "$HOME/deploy_files" + +# export PX4_PX4IO_NAME="px4io-v2" + +unset CXX CC + +./waf distclean +./waf configure --board=MttrCubeBlack +./waf build --targets bin/arducopter -j8 +./waf configure --board=sitl +./waf build --targets bin/arducopter -j8 + +mv build/MttrCubeBlack/bin/arducopter.apj "$HOME/deploy_files" +mv build/MttrCubeBlack/bin/arducopter "$HOME/deploy_files/fmu.elf" +mv build/sitl/bin/arducopter "$HOME/deploy_files/sitl.elf" diff --git a/Tools/scripts/runcoptertest.py b/Tools/scripts/runcoptertest.py new file mode 100755 index 0000000000..774b276296 --- /dev/null +++ b/Tools/scripts/runcoptertest.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +import pexpect, time, sys +from pymavlink import mavutil + +def wait_heartbeat(mav, timeout=10): + '''wait for a heartbeat''' + start_time = time.time() + while time.time() < start_time+timeout: + if mav.recv_match(type='HEARTBEAT', blocking=True, timeout=0.5) is not None: + return + failure("Failed to get heartbeat") + +def wait_mode(mav, modes, timeout=10): + '''wait for one of a set of flight modes''' + start_time = time.time() + last_mode = None + while time.time() < start_time+timeout: + wait_heartbeat(mav, timeout=10) + if mav.flightmode != last_mode: + print("Flightmode %s" % mav.flightmode) + last_mode = mav.flightmode + if mav.flightmode in modes: + return + print("Failed to get mode from %s" % modes) + sys.exit(1) + +def wait_time(mav, simtime): + '''wait for simulation time to pass''' + imu = mav.recv_match(type='RAW_IMU', blocking=True) + t1 = imu.time_usec*1.0e-6 + while True: + imu = mav.recv_match(type='RAW_IMU', blocking=True) + t2 = imu.time_usec*1.0e-6 + if t2 - t1 > simtime: + break + +cmd = '../../Tools/autotest/sim_vehicle.py -D -S10' +mavproxy = pexpect.spawn(cmd, logfile=sys.stdout, timeout=30) +mavproxy.expect("Ready to FLY") + +mav = mavutil.mavlink_connection('127.0.0.1:14550') + +wait_time(mav, 2) +mavproxy.send('mode GUIDED\n') +mavproxy.send('param set SIM_SPEEDUP 5\n') +wait_mode(mav, ['GUIDED']) +mavproxy.expect('is using GPS') +mavproxy.expect('is using GPS') +mavproxy.send('arm throttle\n') +mavproxy.expect('ARMED') +mavproxy.send('mode AUTO\n') +wait_mode(mav, ['AUTO']) +wait_time(mav, 15) +mavproxy.send('long MISSION_START\n') +mavproxy.send('module load console\n') +mavproxy.send('module load map\n') +mavproxy.logfile = None +mavproxy.interact() diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index d975bf2f1f..944de7e51c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -153,7 +153,6 @@ void AC_AttitudeControl::relax_attitude_controllers() // Initialize the attitude variables to the current attitude _ahrs.get_quat_body_to_ned(_attitude_target_quat); _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z); - _attitude_ang_error.initialise(); // Initialize the angular rate variables to the current rate _attitude_target_ang_vel = _ahrs.get_gyro(); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index f02393e74c..0d44c78406 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -295,6 +295,9 @@ class AC_AttitudeControl { // Calculates the body frame angular velocities to follow the target attitude void attitude_controller_run_quat(); + // Return the tilt angle limit in radians + float get_tilt_limit_rad() { return radians(_aparm.angle_max*0.01f); } + // sanity check parameters. should be called once before take-off virtual void parameter_sanity_check() {} diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 17a654a76d..0a9feb9fcb 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -261,6 +261,7 @@ void AC_AttitudeControl_Multi::update_althold_lean_angle_max(float throttle_in) } float althold_lean_angle_max = acosf(constrain_float(_throttle_in / (AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX * thr_max), 0.0f, 1.0f)); + althold_lean_angle_max = MIN(althold_lean_angle_max, radians(0.01f*_aparm.angle_max)); _althold_lean_angle_max = _althold_lean_angle_max + (_dt / (_dt + _angle_limit_tc)) * (althold_lean_angle_max - _althold_lean_angle_max); } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 6384a4dfb1..fc489f00b4 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -19,6 +19,7 @@ #define POSCONTROL_ACCEL_XY_MAX 980.0f // max horizontal acceleration in cm/s/s that the position velocity controller will ask from the lower accel controller #define POSCONTROL_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing #define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending +#define POSCONTROL_JERK_LIMIT_CMSSS 3400.0f // default jerk limit on horizontal acceleration (unit: m/s/s/s) #define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s #define POSCONTROL_SPEED_DOWN -150.0f // default descent rate in cm/s @@ -35,8 +36,8 @@ #define POSCONTROL_VEL_ERROR_CUTOFF_FREQ 4.0f // low-pass filter on velocity error (unit: hz) #define POSCONTROL_THROTTLE_CUTOFF_FREQ 2.0f // low-pass filter on accel error (unit: hz) -#define POSCONTROL_ACCEL_FILTER_HZ 2.0f // low-pass filter on acceleration (unit: hz) -#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration +#define POSCONTROL_ACCEL_FILTER_HZ 3.0f // low-pass filter on acceleration (unit: hz) +#define POSCONTROL_JERK_RATIO 2.0f // Defines the time it takes to reach the requested acceleration #define POSCONTROL_OVERSPEED_GAIN_Z 2.0f // gain controlling rate at which z-axis speed is brought back within SPEED_UP and SPEED_DOWN range @@ -79,6 +80,9 @@ class AC_PosControl /// leash length will be recalculated void set_max_accel_z(float accel_cmss); + /// get_vel_error_z - returns current vertical speed error in cm/s + float get_vel_error_z() const { return _vel_error.z; } + /// get_max_accel_z - returns current maximum vertical acceleration in cm/s/s float get_max_accel_z() const { return _accel_z_cms; } diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 701c94aac9..ad51a5a597 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -7,6 +7,8 @@ #include "AC_PrecLand_IRLock.h" #include "AC_PrecLand_SITL_Gazebo.h" #include "AC_PrecLand_SITL.h" +#include +#include #include @@ -208,7 +210,11 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) bool AC_PrecLand::target_acquired() { - _target_acquired = _target_acquired && (AP_HAL::millis()-_last_update_ms) < 2000; + if ((AP_HAL::millis()-_last_update_ms) >= 2000) { + _estimator_initialized = false; + _target_acquired = false; + } + return _target_acquired; } @@ -250,6 +256,16 @@ bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) return true; } +bool AC_PrecLand::get_target_position_relative_measured_cm(Vector2f& ret) +{ + if (!target_acquired()) { + return false; + } + ret.x = _target_pos_rel_meas_NED.x*100.0f; + ret.y = _target_pos_rel_meas_NED.y*100.0f; + return true; +} + // handle_msg - Process a LANDING_TARGET mavlink message void AC_PrecLand::handle_msg(const mavlink_message_t &msg) { @@ -294,6 +310,9 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; _last_update_ms = AP_HAL::millis(); + if (!_target_acquired) { + gcs().send_text(MAV_SEVERITY_INFO, "PL: target acquired %.1fm", rangefinder_alt_m); + } _target_acquired = true; } @@ -315,8 +334,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va // Update if a new Line-Of-Sight measurement is available if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { - float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f); - if (!target_acquired()) { + float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.02f + 0.02f*AP::ahrs().get_gyro().length()) + 0.02f); + if (!_estimator_initialized) { // reset filter state if (inertial_data_delayed->inertialNavVelocityValid) { _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.x, sq(2.0f)); @@ -326,24 +345,36 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f)); } _last_update_ms = AP_HAL::millis(); - _target_acquired = true; + _estimator_initialized = true; + _estimator_init_ms = AP_HAL::millis(); + + _target_pos_rel_out_NE = Vector2f(_ekf_x.getPos(), _ekf_y.getPos()); + _target_vel_rel_out_NE = Vector2f(_ekf_x.getVel(), _ekf_y.getVel()); } else { float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var); float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var); - if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) { - _outlier_reject_count = 0; + if (MAX(NIS_x, NIS_y) < 3.0f) { _ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var); _ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var); _last_update_ms = AP_HAL::millis(); - _target_acquired = true; } else { _outlier_reject_count++; } } } + // Check for estimator timeout + if (!target_acquired() && _estimator_initialized) { + if (AP_HAL::millis()-_last_update_ms > 200) { + _estimator_initialized = false; + } else if (AP_HAL::millis()-_estimator_init_ms > 2000) { + gcs().send_text(MAV_SEVERITY_INFO, "PL: target acquired2"); + _target_acquired = true; + } + } + // Output prediction - if (target_acquired()) { + if (_estimator_initialized) { _target_pos_rel_est_NE.x = _ekf_x.getPos(); _target_pos_rel_est_NE.y = _ekf_y.getPos(); _target_vel_rel_est_NE.x = _ekf_x.getVel(); @@ -372,6 +403,8 @@ bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body) ); target_vec_unit_body = Rz*target_vec_unit_body; + + AP::logger().Write("LOSM", "TimeUS,LOSx,LOSy,LOSz", "Qfff", AP_HAL::micros64(), target_vec_unit_body.x, target_vec_unit_body.y, target_vec_unit_body.z); return true; } else { return false; @@ -389,20 +422,22 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f); if (target_vec_valid && alt_valid) { float dist, alt; + + Vector3f cam_pos_ned = inertial_data_delayed->Tbn * _cam_offset.get(); + if (_backend->distance_to_target() > 0.0f) { dist = _backend->distance_to_target(); alt = dist * target_vec_unit_ned.z; } else { - alt = MAX(rangefinder_alt_m, 0.0f); + alt = MAX(rangefinder_alt_m-cam_pos_ned.z, 0.0f); dist = alt / target_vec_unit_ned.z; } - // Compute camera position relative to IMU - Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); - Vector3f cam_pos_ned = inertial_data_delayed->Tbn * (_cam_offset.get() - accel_body_offset); - // Compute target position relative to IMU - _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt) + cam_pos_ned; + const AP_AHRS &ahrs = AP::ahrs(); + Vector3f imu_pos_ned = inertial_data_delayed->Tbn * AP::ins().get_imu_pos_offset(ahrs.get_primary_accel_index()); + Vector3f cam_pos_ned_rel_imu = cam_pos_ned-imu_pos_ned; + _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt) + cam_pos_ned_rel_imu; return true; } } @@ -411,16 +446,16 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, void AC_PrecLand::run_output_prediction() { - _target_pos_rel_out_NE = _target_pos_rel_est_NE; - _target_vel_rel_out_NE = _target_vel_rel_est_NE; + Vector2f target_pos_rel_est_corrected_NE = _target_pos_rel_est_NE; + Vector2f target_vel_rel_est_corrected_NE = _target_vel_rel_est_NE; // Predict forward from delayed time horizon for (uint8_t i=1; i<_inertial_history->available(); i++) { - const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i]; - _target_vel_rel_out_NE.x -= inertial_data->correctedVehicleDeltaVelocityNED.x; - _target_vel_rel_out_NE.y -= inertial_data->correctedVehicleDeltaVelocityNED.y; - _target_pos_rel_out_NE.x += _target_vel_rel_out_NE.x * inertial_data->dt; - _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data->dt; + const struct inertial_data_frame_s& inertial_data = *(*_inertial_history)[i]; + target_vel_rel_est_corrected_NE.x -= inertial_data.correctedVehicleDeltaVelocityNED.x; + target_vel_rel_est_corrected_NE.y -= inertial_data.correctedVehicleDeltaVelocityNED.y; + target_pos_rel_est_corrected_NE.x += target_vel_rel_est_corrected_NE.x * inertial_data.dt; + target_pos_rel_est_corrected_NE.y += target_vel_rel_est_corrected_NE.y * inertial_data.dt; } const AP_AHRS &_ahrs = AP::ahrs(); @@ -430,21 +465,44 @@ void AC_PrecLand::run_output_prediction() // Apply position correction for CG offset from IMU Vector3f imu_pos_ned = Tbn * accel_body_offset; - _target_pos_rel_out_NE.x += imu_pos_ned.x; - _target_pos_rel_out_NE.y += imu_pos_ned.y; + target_pos_rel_est_corrected_NE.x += imu_pos_ned.x; + target_pos_rel_est_corrected_NE.y += imu_pos_ned.y; // Apply position correction for body-frame horizontal camera offset from CG, so that vehicle lands lens-to-target Vector3f cam_pos_horizontal_ned = Tbn * Vector3f(_cam_offset.get().x, _cam_offset.get().y, 0); - _target_pos_rel_out_NE.x -= cam_pos_horizontal_ned.x; - _target_pos_rel_out_NE.y -= cam_pos_horizontal_ned.y; + target_pos_rel_est_corrected_NE.x -= cam_pos_horizontal_ned.x; + target_pos_rel_est_corrected_NE.y -= cam_pos_horizontal_ned.y; // Apply velocity correction for IMU offset from CG Vector3f vel_ned_rel_imu = Tbn * (_ahrs.get_gyro() % (-accel_body_offset)); - _target_vel_rel_out_NE.x -= vel_ned_rel_imu.x; - _target_vel_rel_out_NE.y -= vel_ned_rel_imu.y; + target_vel_rel_est_corrected_NE.x -= vel_ned_rel_imu.x; + target_vel_rel_est_corrected_NE.y -= vel_ned_rel_imu.y; // Apply land offset Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f; - _target_pos_rel_out_NE.x += land_ofs_ned_m.x; - _target_pos_rel_out_NE.y += land_ofs_ned_m.y; + target_pos_rel_est_corrected_NE.x += land_ofs_ned_m.x; + target_pos_rel_est_corrected_NE.y += land_ofs_ned_m.y; + + const struct inertial_data_frame_s& latest_inertial_data = *(*_inertial_history)[_inertial_history->available()-1]; + + _target_pos_rel_out_NE += _target_vel_rel_est_NE * latest_inertial_data.dt; + _target_vel_rel_out_NE += -Vector2f(latest_inertial_data.correctedVehicleDeltaVelocityNED.x, latest_inertial_data.correctedVehicleDeltaVelocityNED.y); + + const float tc = 0.3f; + float alpha = latest_inertial_data.dt/(latest_inertial_data.dt+tc); + + _target_pos_rel_out_NE += (target_pos_rel_est_corrected_NE - _target_pos_rel_out_NE) * alpha; + _target_vel_rel_out_NE += (target_vel_rel_est_corrected_NE - _target_vel_rel_out_NE) * alpha; +} + +// send landing target mavlink message to ground station +void AC_PrecLand::send_landing_target(mavlink_channel_t chan) +{ + mavlink_msg_landing_target_send(chan, + _last_update_ms, + target_acquired(), + MAV_FRAME_GLOBAL_TERRAIN_ALT, + _ekf_x.getPos()*100.0f, _ekf_y.getPos()*100.0f, + 0.0f, 0.0f, 0.0f, 0, 0, 0, nullptr, 0, 0); + } diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 1253530966..77c895c247 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -79,6 +79,9 @@ class AC_PrecLand // returns target position relative to vehicle bool get_target_position_relative_cm(Vector2f& ret); + // get measured position of target if available + bool get_target_position_relative_measured_cm(Vector2f& ret); + // returns target velocity relative to vehicle bool get_target_velocity_relative_cms(Vector2f& ret); @@ -88,6 +91,9 @@ class AC_PrecLand // process a LANDING_TARGET mavlink message void handle_msg(const mavlink_message_t &msg); + // send GCS_MAVLink message + void send_landing_target(mavlink_channel_t chan); + // parameter var table static const struct AP_Param::GroupInfo var_info[]; @@ -126,6 +132,8 @@ class AC_PrecLand AP_Vector3f _cam_offset; // Position of the camera relative to the CG uint32_t _last_update_ms; // system time in millisecond when update was last called + uint32_t _estimator_init_ms; + bool _estimator_initialized; bool _target_acquired; // true if target has been seen recently uint32_t _last_backend_los_meas_ms; // system time target was last seen diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp index 52b806ea5e..3ff6663d97 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp @@ -17,7 +17,9 @@ void AC_PrecLand_SITL::update() if (_state.healthy && _sitl->precland_sim.last_update_ms() != _los_meas_time_ms) { const Vector3f position = _sitl->precland_sim.get_target_position(); - const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned(); + Matrix3f body_to_ned; + _sitl->state.quaternion.rotation_matrix(body_to_ned); + _target_distance_m = position.length(); _los_meas_body = body_to_ned.mul_transpose(-position); _los_meas_body /= _los_meas_body.length(); _have_los_meas = true; @@ -45,4 +47,13 @@ bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) { return true; } +// returns distance to target in meters (0 means distance is not known) +float AC_PrecLand_SITL::distance_to_target() +{ + if (AP_HAL::millis() - _los_meas_time_ms > 1000) { + return 0; + } + return _target_distance_m; +} + #endif diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.h b/libraries/AC_PrecLand/AC_PrecLand_SITL.h index 6821476a54..94e5257e79 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.h @@ -32,11 +32,15 @@ class AC_PrecLand_SITL : public AC_PrecLand_Backend // return true if there is a valid los measurement available bool have_los_meas() override; + // returns distance to target in meters (0 means distance is not known) + float distance_to_target() override; + private: SITL::SITL *_sitl; // sitl instance pointer Vector3f _los_meas_body; // unit vector in body frame pointing towards target uint32_t _los_meas_time_ms; // system time in milliseconds when los was measured bool _have_los_meas; // true if there is a valid measurement from the camera + float _target_distance_m; }; #endif diff --git a/libraries/AC_PrecLand/PosVelEKF.cpp b/libraries/AC_PrecLand/PosVelEKF.cpp index 935a3d4ba1..410c670912 100644 --- a/libraries/AC_PrecLand/PosVelEKF.cpp +++ b/libraries/AC_PrecLand/PosVelEKF.cpp @@ -7,87 +7,103 @@ __RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R); #define POSVELEKF_POS_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \ __RET_STATE[0] = __P[0]*(-__X[0] + __Z)/(__P[0] + __R) + __X[0]; __RET_STATE[1] = __P[1]*(-__X[0] + \ -__Z)/(__P[0] + __R) + __X[1]; +__Z)/(__P[0] + __R) + __X[1]; __RET_STATE[2] = __P[2]*(-__X[0] + __Z)/(__P[0] + __R) + __X[2]; #define POSVELEKF_POS_CALC_COV(__P, __R, __X, __Z, __RET_COV) \ __RET_COV[0] = ((__P[0])*(__P[0]))*__R/((__P[0] + __R)*(__P[0] + __R)) + __P[0]*((-__P[0]/(__P[0] + \ __R) + 1)*(-__P[0]/(__P[0] + __R) + 1)); __RET_COV[1] = __P[0]*__P[1]*__R/((__P[0] + __R)*(__P[0] + \ __R)) - __P[0]*__P[1]*(-__P[0]/(__P[0] + __R) + 1)/(__P[0] + __R) + __P[1]*(-__P[0]/(__P[0] + __R) + \ -1); __RET_COV[2] = ((__P[1])*(__P[1]))*__R/((__P[0] + __R)*(__P[0] + __R)) - \ -((__P[1])*(__P[1]))/(__P[0] + __R) - __P[1]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + \ -__P[2]; - -#define POSVELEKF_PREDICTION_CALC_STATE(__P, __DT, __DV, __DV_NOISE, __X, __RET_STATE) \ -__RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = __DV + __X[1]; - -#define POSVELEKF_PREDICTION_CALC_COV(__P, __DT, __DV, __DV_NOISE, __X, __RET_COV) \ -__RET_COV[0] = __DT*__P[1] + __DT*(__DT*__P[2] + __P[1]) + __P[0]; __RET_COV[1] = __DT*__P[2] + \ -__P[1]; __RET_COV[2] = ((__DV_NOISE)*(__DV_NOISE)) + __P[2]; +1); __RET_COV[2] = __P[0]*__P[2]*__R/((__P[0] + __R)*(__P[0] + __R)) - __P[0]*__P[2]*(-__P[0]/(__P[0] \ ++ __R) + 1)/(__P[0] + __R) + __P[2]*(-__P[0]/(__P[0] + __R) + 1); __RET_COV[3] = \ +((__P[1])*(__P[1]))*__R/((__P[0] + __R)*(__P[0] + __R)) - ((__P[1])*(__P[1]))/(__P[0] + __R) - \ +__P[1]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + __P[3]; __RET_COV[4] = \ +__P[1]*__P[2]*__R/((__P[0] + __R)*(__P[0] + __R)) - __P[1]*__P[2]/(__P[0] + __R) - \ +__P[2]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + __P[4]; __RET_COV[5] = \ +((__P[2])*(__P[2]))*__R/((__P[0] + __R)*(__P[0] + __R)) - ((__P[2])*(__P[2]))/(__P[0] + __R) - \ +__P[2]*(-__P[0]*__P[2]/(__P[0] + __R) + __P[2])/(__P[0] + __R) + __P[5]; + +#define POSVELEKF_PREDICTION_CALC_STATE(__P, __ABIAS_PNOISE, __DT, __DV, __DV_NOISE, __X, __RET_STATE) \ +__RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = -__DT*__X[2] + __DV + __X[1]; __RET_STATE[2] \ += __X[2]; + +#define POSVELEKF_PREDICTION_CALC_COV(__P, __ABIAS_PNOISE, __DT, __DV, __DV_NOISE, __X, __RET_COV) \ +__RET_COV[0] = __DT*__P[1] + __DT*(__DT*__P[3] + __P[1]) + __P[0]; __RET_COV[1] = __DT*__P[3] - \ +__DT*(__DT*__P[4] + __P[2]) + __P[1]; __RET_COV[2] = __DT*__P[4] + __P[2]; __RET_COV[3] = \ +-__DT*__P[4] - __DT*(-__DT*__P[5] + __P[4]) + ((__DV_NOISE)*(__DV_NOISE)) + __P[3]; __RET_COV[4] = \ +-__DT*__P[5] + __P[4]; __RET_COV[5] = ((__ABIAS_PNOISE)*(__ABIAS_PNOISE))*((__DT)*(__DT)) + __P[5]; #define POSVELEKF_VEL_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \ -__RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[2] + __R); +__RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[3] + __R); #define POSVELEKF_VEL_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \ -__RET_STATE[0] = __P[1]*(-__X[1] + __Z)/(__P[2] + __R) + __X[0]; __RET_STATE[1] = __P[2]*(-__X[1] + \ -__Z)/(__P[2] + __R) + __X[1]; +__RET_STATE[0] = __P[1]*(-__X[1] + __Z)/(__P[3] + __R) + __X[0]; __RET_STATE[1] = __P[3]*(-__X[1] + \ +__Z)/(__P[3] + __R) + __X[1]; __RET_STATE[2] = __P[4]*(-__X[1] + __Z)/(__P[3] + __R) + __X[2]; #define POSVELEKF_VEL_CALC_COV(__P, __R, __X, __Z, __RET_COV) \ -__RET_COV[0] = __P[0] + ((__P[1])*(__P[1]))*__R/((__P[2] + __R)*(__P[2] + __R)) - \ -((__P[1])*(__P[1]))/(__P[2] + __R) - __P[1]*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1])/(__P[2] + __R); \ -__RET_COV[1] = __P[1]*__P[2]*__R/((__P[2] + __R)*(__P[2] + __R)) + (-__P[2]/(__P[2] + __R) + \ -1)*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1]); __RET_COV[2] = ((__P[2])*(__P[2]))*__R/((__P[2] + \ -__R)*(__P[2] + __R)) + __P[2]*((-__P[2]/(__P[2] + __R) + 1)*(-__P[2]/(__P[2] + __R) + 1)); +__RET_COV[0] = __P[0] + ((__P[1])*(__P[1]))*__R/((__P[3] + __R)*(__P[3] + __R)) - \ +((__P[1])*(__P[1]))/(__P[3] + __R) - __P[1]*(-__P[1]*__P[3]/(__P[3] + __R) + __P[1])/(__P[3] + __R); \ +__RET_COV[1] = __P[1]*__P[3]*__R/((__P[3] + __R)*(__P[3] + __R)) + (-__P[3]/(__P[3] + __R) + \ +1)*(-__P[1]*__P[3]/(__P[3] + __R) + __P[1]); __RET_COV[2] = __P[1]*__P[4]*__R/((__P[3] + __R)*(__P[3] \ ++ __R)) - __P[1]*__P[4]/(__P[3] + __R) + __P[2] - __P[4]*(-__P[1]*__P[3]/(__P[3] + __R) + \ +__P[1])/(__P[3] + __R); __RET_COV[3] = ((__P[3])*(__P[3]))*__R/((__P[3] + __R)*(__P[3] + __R)) + \ +__P[3]*((-__P[3]/(__P[3] + __R) + 1)*(-__P[3]/(__P[3] + __R) + 1)); __RET_COV[4] = \ +__P[3]*__P[4]*__R/((__P[3] + __R)*(__P[3] + __R)) - __P[3]*__P[4]*(-__P[3]/(__P[3] + __R) + \ +1)/(__P[3] + __R) + __P[4]*(-__P[3]/(__P[3] + __R) + 1); __RET_COV[5] = \ +((__P[4])*(__P[4]))*__R/((__P[3] + __R)*(__P[3] + __R)) - ((__P[4])*(__P[4]))/(__P[3] + __R) - \ +__P[4]*(-__P[3]*__P[4]/(__P[3] + __R) + __P[4])/(__P[3] + __R) + __P[5]; void PosVelEKF::init(float pos, float posVar, float vel, float velVar) { - _state[0] = pos; - _state[1] = vel; - _cov[0] = posVar; - _cov[1] = 0.0f; - _cov[2] = velVar; + uint8_t next_state_idx = (_state_idx+1)%2; + + _state[next_state_idx].x[0] = pos; + _state[next_state_idx].x[1] = vel; + _state[next_state_idx].x[2] = 0; + _state[next_state_idx].P[0] = posVar; + _state[next_state_idx].P[1] = 0.0f; + _state[next_state_idx].P[2] = 0.0f; + _state[next_state_idx].P[3] = velVar; + _state[next_state_idx].P[4] = 0.0f; + _state[next_state_idx].P[5] = 0.05f*0.05f; + + _state_idx = next_state_idx; } void PosVelEKF::predict(float dt, float dVel, float dVelNoise) { - float newState[2]; - float newCov[3]; - - POSVELEKF_PREDICTION_CALC_STATE(_cov, dt, dVel, dVelNoise, _state, newState) - POSVELEKF_PREDICTION_CALC_COV(_cov, dt, dVel, dVelNoise, _state, newCov) + uint8_t next_state_idx = (_state_idx+1)%2; + const float abias_pnoise = 0.005f; + POSVELEKF_PREDICTION_CALC_STATE(_state[_state_idx].P, abias_pnoise, dt, dVel, dVelNoise, _state[_state_idx].x, _state[next_state_idx].x) + POSVELEKF_PREDICTION_CALC_COV(_state[_state_idx].P, abias_pnoise, dt, dVel, dVelNoise, _state[_state_idx].x, _state[next_state_idx].P) - memcpy(_state,newState,sizeof(_state)); - memcpy(_cov,newCov,sizeof(_cov)); + _state_idx = next_state_idx; } void PosVelEKF::fusePos(float pos, float posVar) { - float newState[2]; - float newCov[3]; + uint8_t next_state_idx = (_state_idx+1)%2; - POSVELEKF_POS_CALC_STATE(_cov, posVar, _state, pos, newState) - POSVELEKF_POS_CALC_COV(_cov, posVar, _state, pos, newCov) + POSVELEKF_POS_CALC_STATE(_state[_state_idx].P, posVar, _state[_state_idx].x, pos, _state[next_state_idx].x) + POSVELEKF_POS_CALC_COV(_state[_state_idx].P, posVar, _state[_state_idx].x, pos, _state[next_state_idx].P) - memcpy(_state,newState,sizeof(_state)); - memcpy(_cov,newCov,sizeof(_cov)); + _state_idx = next_state_idx; } void PosVelEKF::fuseVel(float vel, float velVar) { - float newState[2]; - float newCov[3]; + uint8_t next_state_idx = (_state_idx+1)%2; - POSVELEKF_VEL_CALC_STATE(_cov, velVar, _state, vel, newState) - POSVELEKF_VEL_CALC_COV(_cov, velVar, _state, vel, newCov) + POSVELEKF_VEL_CALC_STATE(_state[_state_idx].P, velVar, _state[_state_idx].x, vel, _state[next_state_idx].x) + POSVELEKF_VEL_CALC_COV(_state[_state_idx].P, velVar, _state[_state_idx].x, vel, _state[next_state_idx].P) - memcpy(_state,newState,sizeof(_state)); - memcpy(_cov,newCov,sizeof(_cov)); + _state_idx = next_state_idx; } float PosVelEKF::getPosNIS(float pos, float posVar) { float ret; - POSVELEKF_POS_CALC_NIS(_cov, posVar, _state, pos, ret) + POSVELEKF_POS_CALC_NIS(_state[_state_idx].P, posVar, _state[_state_idx].x, pos, ret) return ret; } diff --git a/libraries/AC_PrecLand/PosVelEKF.h b/libraries/AC_PrecLand/PosVelEKF.h index a769abd9e7..df7d72ab5c 100644 --- a/libraries/AC_PrecLand/PosVelEKF.h +++ b/libraries/AC_PrecLand/PosVelEKF.h @@ -1,5 +1,7 @@ #pragma once +#include + class PosVelEKF { public: void init(float pos, float posVar, float vel, float velVar); @@ -7,12 +9,16 @@ class PosVelEKF { void fusePos(float pos, float posVar); void fuseVel(float vel, float velVar); - float getPos() const { return _state[0]; } - float getVel() const { return _state[1]; } + float getPos() const { return _state[_state_idx].x[0]; } + float getVel() const { return _state[_state_idx].x[1]; } + float getABias() const { return _state[_state_idx].x[2]; } float getPosNIS(float pos, float posVar); private: - float _state[2]; - float _cov[3]; + struct { + float x[3]; + float P[6]; + } _state[2]; + uint8_t _state_idx; }; diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 8ff6bba6ae..3ce5a4b5dc 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -195,6 +195,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) origin = _pos_control.get_pos_target(); } else { // if waypoint controller is not active, set origin to reasonable stopping point (using curr pos and velocity) + _pos_control.set_desired_velocity_z(0); _pos_control.get_stopping_point_xy(origin); _pos_control.get_stopping_point_z(origin); } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 8eaa95ad2f..e2c9d22a80 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -553,6 +553,8 @@ class AP_AHRS const AP_InertialSensor &_ins = AP::ins(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); + ret = get_rotation_body_to_ned() * get_rotation_autopilot_body_to_vehicle_body() * ret; + ret.z += GRAVITY_MSS*dt; } // create a view diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index af46f008d4..f936a336e5 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -1300,32 +1300,24 @@ bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { const EKF_TYPE type = active_EKF_type(); - if (type == EKF_TYPE2 || type == EKF_TYPE3) { - int8_t imu_idx = 0; - Vector3f accel_bias; - if (type == EKF_TYPE2) { - accel_bias.zero(); - imu_idx = EKF2.getPrimaryCoreIMUIndex(); - EKF2.getAccelZBias(-1,accel_bias.z); - } else if (type == EKF_TYPE3) { - imu_idx = EKF3.getPrimaryCoreIMUIndex(); - EKF3.getAccelBias(-1,accel_bias); - } - if (imu_idx == -1) { - // should never happen, call parent implementation in this scenario - AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt); - return; - } - ret.zero(); - const AP_InertialSensor &_ins = AP::ins(); - _ins.get_delta_velocity((uint8_t)imu_idx, ret); - dt = _ins.get_delta_velocity_dt((uint8_t)imu_idx); - ret -= accel_bias*dt; - ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret; - ret.z += GRAVITY_MSS*dt; - } else { - AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt); - } + + int8_t imu_idx = 0; + Vector3f accel_bias; + if (type == EKF_TYPE2) { + accel_bias.zero(); + imu_idx = EKF2.getPrimaryCoreIMUIndex(); + EKF2.getAccelZBias(-1,accel_bias.z); + } else if (type == EKF_TYPE3) { + imu_idx = EKF3.getPrimaryCoreIMUIndex(); + EKF3.getAccelBias(-1,accel_bias); + } + ret.zero(); + const AP_InertialSensor &_ins = AP::ins(); + _ins.get_delta_velocity((uint8_t)imu_idx, ret); + dt = _ins.get_delta_velocity_dt((uint8_t)imu_idx); + ret -= accel_bias*dt; + ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret; + ret.z += GRAVITY_MSS*dt; } // report any reason for why the backend is refusing to initialise diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index 713e156f31..a80ccc5d5c 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -544,7 +544,7 @@ bool AP_Arming::manual_transmitter_checks(bool report) if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) { - if (AP_Notify::flags.failsafe_radio) { + if (AP_Notify::flags.failsafe_radio && !rc_check_disabled) { check_failed(ARMING_CHECK_RC, report, "Radio failsafe on"); return false; } diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 80396e7239..8ffc7959b9 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -33,6 +33,7 @@ class AP_Arming { ARMING_CHECK_SYSTEM = (1U << 13), ARMING_CHECK_MISSION = (1U << 14), ARMING_CHECK_RANGEFINDER = (1U << 15), + ARMING_CHECK_FTS = (1U << 16), }; enum class Method { @@ -71,6 +72,11 @@ class AP_Arming { // get expected magnetic field strength uint16_t compass_magfield_expected() const; + // allow RC check to be disabled for some modes + void disable_RC_check(bool disable) { + rc_check_disabled = disable; + } + // rudder arming support enum class RudderArming { IS_DISABLED = 0, // DISABLED leaks in from vehicle defines.h @@ -95,6 +101,7 @@ class AP_Arming { bool armed; uint32_t last_accel_pass_ms[INS_MAX_INSTANCES]; uint32_t last_gyro_pass_ms[INS_MAX_INSTANCES]; + bool rc_check_disabled; virtual bool barometer_checks(bool report); diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 08686aea62..891fad8d29 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1363,7 +1363,7 @@ void AP_BLHeli::read_telemetry_packet(void) return; } struct telem_data td; - td.temperature = buf[0]; + td.temperature = int8_t(buf[0]); td.voltage = (buf[1]<<8) | buf[2]; td.current = (buf[3]<<8) | buf[4]; td.consumption = (buf[5]<<8) | buf[6]; diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index f397ebccec..2775a08110 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -46,7 +46,7 @@ class AP_BLHeli { static const struct AP_Param::GroupInfo var_info[]; struct telem_data { - uint8_t temperature; // degrees C + int8_t temperature; // degrees C, negative values allowed uint16_t voltage; // volts * 100 uint16_t current; // amps * 100 uint16_t consumption;// mAh diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index 284c56b72f..3d33b9a1ff 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -1,5 +1,6 @@ #include "AP_BattMonitor.h" #include "AP_BattMonitor_Analog.h" +#include "AP_BattMonitor_Analog_Table.h" #include "AP_BattMonitor_SMBus.h" #include "AP_BattMonitor_Bebop.h" #include "AP_BattMonitor_BLHeliESC.h" @@ -112,6 +113,10 @@ AP_BattMonitor::init() case AP_BattMonitor_Params::BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT: drivers[instance] = new AP_BattMonitor_Analog(*this, state[instance], _params[instance]); break; + case AP_BattMonitor_Params::BattMonitor_TYPE_ANALOG_TABLE: + drivers[instance] = new AP_BattMonitor_Analog_Table(*this, state[instance], _params[instance]); + _num_instances++; + break; case AP_BattMonitor_Params::BattMonitor_TYPE_SOLO: drivers[instance] = new AP_BattMonitor_SMBus_Solo(*this, state[instance], _params[instance], hal.i2c_mgr->get_device(AP_BATTMONITOR_SMBUS_BUS_INTERNAL, AP_BATTMONITOR_SMBUS_I2C_ADDR, diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index af285ac1d2..a88dd61fd4 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -22,6 +22,7 @@ // declare backend class class AP_BattMonitor_Backend; class AP_BattMonitor_Analog; +class AP_BattMonitor_Analog_Table; class AP_BattMonitor_SMBus; class AP_BattMonitor_SMBus_Solo; class AP_BattMonitor_SMBus_Maxell; @@ -31,6 +32,7 @@ class AP_BattMonitor { friend class AP_BattMonitor_Backend; friend class AP_BattMonitor_Analog; + friend class AP_BattMonitor_Analog_Table; friend class AP_BattMonitor_SMBus; friend class AP_BattMonitor_SMBus_Solo; friend class AP_BattMonitor_SMBus_Maxell; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h index a34a736345..8978407edd 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h @@ -83,13 +83,13 @@ class AP_BattMonitor_Analog : public AP_BattMonitor_Backend AP_BattMonitor_Analog(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms); /// Read the battery voltage and current. Should be called at 10hz - void read() override; + virtual void read() override; /// returns true if battery monitor provides consumed energy info bool has_consumed_energy() const override { return has_current(); } /// returns true if battery monitor provides current info - bool has_current() const override; + virtual bool has_current() const override; void init(void) override {} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.cpp new file mode 100644 index 0000000000..a49252fada --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.cpp @@ -0,0 +1,105 @@ +#include +#include "AP_BattMonitor_Analog_Table.h" + +extern const AP_HAL::HAL& hal; + +// assume 12 cells +#define TABLE_NUM_CELLS 12 + +/* + state of charge table for a battery. This should be chosen based on + a battery that is at maximum age of batteries that will be used in + the vehicle + */ +static const struct { + float volt_per_cell; + float soc_pct; +} soc_table[] = { + { 4.173, 100 }, + { 4.112, 96.15 }, + { 4.085, 92.31 }, + { 4.071, 88.46 }, + { 4.039, 84.62 }, + { 3.987, 80.77 }, + { 3.943, 76.92 }, + { 3.908, 73.08 }, + { 3.887, 69.23 }, + { 3.854, 65.38 }, + { 3.833, 61.54 }, + { 3.801, 57.69 }, + { 3.783, 53.85 }, + { 3.742, 50 }, + { 3.715, 46.15 }, + { 3.679, 42.31 }, + { 3.636, 38.46 }, + { 3.588, 34.62 }, + { 3.543, 30.77 }, + { 3.503, 26.92 }, + { 3.462, 23.08 }, + { 3.379, 19.23 }, + { 3.296, 15.38 }, + { 3.218, 11.54 }, + { 3.165, 7.69 }, + { 3.091, 3.85 }, + { 2.977, 0 }}; + +/* + lookup SoC in table, returning SoC as a percentage + */ +float AP_BattMonitor_Analog_Table::lookup_SoC_table(float voltage) +{ + float per_cell = voltage / TABLE_NUM_CELLS; + + if (per_cell >= soc_table[0].volt_per_cell) { + return 100.0; + } + + for (uint8_t i=1; i= soc_table[i].volt_per_cell) { + // linear interpolation between table rows + float dv1 = per_cell - soc_table[i].volt_per_cell; + float dv2 = soc_table[i-1].volt_per_cell - soc_table[i].volt_per_cell; + float soc1 = soc_table[i].soc_pct; + float soc2 = soc_table[i-1].soc_pct; + return soc1 + (dv1 / dv2) * (soc2 - soc1); + } + } + // off the bottom of the table + return 0; +} + +/* + read - read the voltage and current + Use SoC table when disarmed +*/ +void AP_BattMonitor_Analog_Table::read() +{ + // first call base class read() method + AP_BattMonitor_Analog::read(); + + uint32_t now = AP_HAL::millis(); + bool armed = hal.util->get_soft_armed(); + if (armed) { + last_armed_ms = now; + using_table = false; + } + + // if we have been disarmed for more than 2 minutes then we start using the table + if (now - last_armed_ms > 120000) { + using_table = true; + } + + if (!armed && _state.voltage < 2.0*TABLE_NUM_CELLS) { + // below 2.0v/cell we assume battery is removed, force use of + // table when new battery is inserted. The threshold is chosen + // to make it more likely that a floating voltage pin will + // detect a low enough voltage to trigger table use + using_table = true; + } + + if (using_table) { + // reset remaining capacity based on the table + float soc_pct = lookup_SoC_table(_state.voltage); + reset_remaining(soc_pct); + } +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h b/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h new file mode 100644 index 0000000000..9a344b2c25 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog_Table.h @@ -0,0 +1,25 @@ +#pragma once + +#include "AP_BattMonitor_Analog.h" + +class AP_BattMonitor_Analog_Table : public AP_BattMonitor_Analog +{ +public: + /// Constructor + AP_BattMonitor_Analog_Table(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms) : + AP_BattMonitor_Analog(mon, mon_state, params) {} + + + /// Read the battery voltage and current. Should be called at 10hz + void read() override; + + /// returns true if battery monitor provides current info + bool has_current() const override { return true; } + +private: + uint32_t last_armed_ms; + bool using_table = true; + + // lookup SoC in table, returning SoC as a percentage + float lookup_SoC_table(float voltage); +}; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 9f572e1fbf..21d78a54c2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -195,7 +195,9 @@ void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_c } // check critical battery levels - if ((voltage_used > 0) && (_params._critical_voltage > 0) && (voltage_used < _params._critical_voltage)) { + if ((voltage_used > 0) && (_params._critical_voltage > 0) && + (voltage_used < _params._critical_voltage) && + (voltage_used > _params._critical_voltage*0.25)) { critical_voltage = true; } else { critical_voltage = false; @@ -209,7 +211,9 @@ void AP_BattMonitor_Backend::check_failsafe_types(bool &low_voltage, bool &low_c critical_capacity = false; } - if ((voltage_used > 0) && (_params._low_voltage > 0) && (voltage_used < _params._low_voltage)) { + if ((voltage_used > 0) && (_params._low_voltage > 0) && + (voltage_used < _params._low_voltage) && + (voltage_used > _params._low_voltage*0.25)) { low_voltage = true; } else { low_voltage = false; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index a2384f4381..11ce3d7b69 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -128,7 +128,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @DisplayName: Low battery failsafe action // @Description: What action the vehicle should perform if it hits a low battery failsafe // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand - // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate + // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate,6:LandIfManual // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate // @Values{Tracker}: 0:None diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 072ecfca7c..009843e24e 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -25,6 +25,7 @@ class AP_BattMonitor_Params { BattMonitor_TYPE_Sum = 10, BattMonitor_TYPE_FuelFlow = 11, BattMonitor_TYPE_FuelLevel_PWM = 12, + BattMonitor_TYPE_ANALOG_TABLE = 20, }; // low voltage sources (used for BATT_LOW_TYPE parameter) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index bbb58285f0..68552a8822 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -284,6 +284,13 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f), #endif + // @Param: DRV_OPTIONS + // @DisplayName: driver options + // @Description: Additional backend specific options + // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use SERIALn_BAUD instead of auto-bauding + // @User: Advanced + AP_GROUPINFO("DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), + AP_GROUPEND }; @@ -446,6 +453,18 @@ void AP_GPS::send_blob_update(uint8_t instance) } } +/* + get baudrate for a probe step + */ +uint32_t AP_GPS::get_baudrate(uint8_t instance, uint8_t step) const +{ + if (unsigned(_driver_options.get()) & unsigned(DRV_OPTIONS::NO_AUTOBAUD)) { + // use SERIALn_BAUD, ignoring _baudrates array + return AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_GPS, instance); + } + return _baudrates[step]; +} + /* run detection step for one GPS instance. If this finds a GPS then it will fill in drivers[instance] and change state[instance].status @@ -521,7 +540,7 @@ void AP_GPS::detect_instance(uint8_t instance) if (dstate->current_baud == ARRAY_SIZE(_baudrates)) { dstate->current_baud = 0; } - uint32_t baudrate = _baudrates[dstate->current_baud]; + uint32_t baudrate = get_baudrate(instance, dstate->current_baud); _port[instance]->begin(baudrate); _port[instance]->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); dstate->last_baud_change_ms = now; @@ -550,8 +569,8 @@ void AP_GPS::detect_instance(uint8_t instance) for. */ if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && - ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) || - _baudrates[dstate->current_baud] == 115200) && + ((!_auto_config && get_baudrate(instance, dstate->current_baud) >= 38400) || + get_baudrate(instance, dstate->current_baud) == 115200) && AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]); } @@ -723,6 +742,7 @@ void AP_GPS::update_instance(uint8_t instance) if (now != 0) { AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS); } + update_position_change(instance); } #else (void)data_should_be_logged; @@ -793,29 +813,44 @@ void AP_GPS::update(void) if (i == primary_instance) { continue; } - if (state[i].status > state[primary_instance].status) { - // we have a higher status lock, or primary is set to the blended GPS, change GPS - primary_instance = i; - _last_instance_swap_ms = now; - continue; + + GPS_Status status_i = state[i].status; + GPS_Status status_primary = state[primary_instance].status; + + if (_type[i] == GPS_TYPE_UAVCAN && _type[1-i] != GPS_TYPE_UAVCAN && status_i == GPS_OK_FIX_3D) { + // if this is a UAVCAN GPS and the other GPS + // is not UAVCAN then we don't know what type + // it is. It is likely F9, so make status=3 be + // equal to status=4 for comparison purposes + // so we choose it over a 2nd M8 with SBAS + // lock + status_i = GPS_OK_FIX_3D_DGPS; } - bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1); + // Treat U-blox F9 as SBAS if it has more than or equal to 16 satellites, as it is dual band GPS and is more accurate than M8 with SBAS + if (status_i == GPS_OK_FIX_3D && state[i].num_sats >= 16 && strcmp(drivers[i]->name(), "u-blox") == 0 && drivers[i]->hardware_generation() == AP_GPS_UBLOX::UBLOX_F9) { + status_i = GPS_OK_FIX_3D_DGPS; + } - if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) { + // Treat U-blox F9 as SBAS if it has more than or equal to 16 satellites, as it is dual band GPS and is more accurate than M8 with SBAS + if (status_primary == GPS_OK_FIX_3D && state[primary_instance].num_sats >= 16 && strcmp(drivers[primary_instance]->name(), "u-blox") == 0 && drivers[primary_instance]->hardware_generation() == AP_GPS_UBLOX::UBLOX_F9) { + status_primary = GPS_OK_FIX_3D_DGPS; + } + + bool should_switch = false; - bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2); + if (status_i > status_primary) { + should_switch = true; + } + + else if (status_i == status_primary && !hal.util->get_soft_armed() && state[i].num_sats >= state[primary_instance].num_sats+3) { + should_switch = true; + } - if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) || - (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) { - // this GPS has more satellites than the - // current primary, switch primary. Once we switch we will - // then tend to stick to the new GPS as primary. We don't - // want to switch too often as it will look like a - // position shift to the controllers. - primary_instance = i; - _last_instance_swap_ms = now; - } + if (should_switch) { + primary_instance = i; + _last_instance_swap_ms = now; + gcs().send_text(MAV_SEVERITY_CRITICAL, "GPS Switch: Switched to %u", primary_instance+1); } } } @@ -1002,22 +1037,27 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) hacc * 1000, // one-sigma standard deviation in mm vacc * 1000, // one-sigma standard deviation in mm sacc * 1000, // one-sigma standard deviation in mm/s - 0); // TODO one-sigma heading accuracy standard deviation + 0, 0); // TODO one-sigma heading accuracy standard deviation } #if GPS_MAX_RECEIVERS > 1 void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) { static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS]; - if (num_instances < 2 || status(1) <= AP_GPS::NO_GPS) { - return; - } - // when we have a GPS then only send new data - if (last_send_time_ms[chan] == last_message_time_ms(1)) { - return; + if (num_instances >= 2 && status(1) > AP_GPS::NO_GPS) { + // when we have a GPS then only send new data + if (last_send_time_ms[chan] == last_message_time_ms(1)) { + return; + } + last_send_time_ms[chan] = last_message_time_ms(1); + } else { + // when we don't have a GPS then send at 1Hz + uint32_t now = AP_HAL::millis(); + if (now - last_send_time_ms[chan] < 1000) { + return; + } + last_send_time_ms[chan] = now; } - last_send_time_ms[chan] = last_message_time_ms(1); - const Location &loc = location(1); mavlink_msg_gps2_raw_send( chan, @@ -1032,7 +1072,7 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) ground_course(1)*100, // 1/100 degrees, num_sats(1), state[1].rtk_num_sats, - state[1].rtk_age_ms); + state[1].rtk_age_ms, 0); } #endif // GPS_MAX_RECEIVERS @@ -1656,6 +1696,35 @@ bool AP_GPS::prepare_for_arming(void) { return all_passed; } +/* + update data for distance moved since arming. Used for pre-takeoff + check + */ +void AP_GPS::update_position_change(uint8_t instance) +{ + if (!hal.util->get_soft_armed()) { + _arm_loc[instance].lat = 0; + _arm_loc[instance].lng = 0; + } else if (_arm_loc[instance].lat == 0 && + _arm_loc[instance].lng == 0) { + _arm_loc[instance] = state[instance].location; + } +} + +/* + get change in position and altitude since arming + This is used as part of a pre-takeoff check for GPS movement +*/ +bool AP_GPS::get_pre_arm_pos_change(uint8_t instance, float &pos_change, float &alt_change) const +{ + if (!hal.util->get_soft_armed()) { + return false; + } + pos_change = _arm_loc[instance].get_distance(state[instance].location); + alt_change = (_arm_loc[instance].alt - state[instance].location.alt) * 0.01; + return true; +} + namespace AP { AP_GPS &gps() diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 50481ef234..e2155f051e 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -439,6 +439,12 @@ class AP_GPS // handle possibly fragmented RTCM injection data void handle_gps_rtcm_fragment(uint8_t flags, const uint8_t *data, uint8_t len); + // get change in position and altitude since arming + bool get_pre_arm_pos_change(uint8_t instance, float &pos_change, float &alt_change) const; + bool get_pre_arm_pos_change(float &pos_change, float &alt_change) const { + return get_pre_arm_pos_change(primary_instance, pos_change, alt_change); + } + protected: // configuration parameters @@ -460,7 +466,14 @@ class AP_GPS AP_Int16 _delay_ms[GPS_MAX_RECEIVERS]; AP_Int8 _blend_mask; AP_Float _blend_tc; + AP_Int16 _driver_options; + // GPS_DRV_OPTIONS bits + enum class DRV_OPTIONS { + MB_USE_UART2 = 1U<<0, + NO_AUTOBAUD = 1U<<1, + }; + uint32_t _log_gps_bit = -1; private: @@ -521,9 +534,12 @@ class AP_GPS static const char _initialisation_blob[]; static const char _initialisation_raw_blob[]; + uint32_t get_baudrate(uint8_t instance, uint8_t step) const; void detect_instance(uint8_t instance); void update_instance(uint8_t instance); + void update_position_change(uint8_t instance); + /* buffer for re-assembling RTCM data for GPS injection. The 8 bit flags field in GPS_RTCM_DATA is interpreted as: @@ -563,6 +579,12 @@ class AP_GPS bool _output_is_blended; // true when a blended GPS solution being output uint8_t _blend_health_counter; // 0 = perfectly health, 100 = very unhealthy + /* + track change in position and height since arming. Used for + pre-takeoff check + */ + Location _arm_loc[GPS_MAX_RECEIVERS]; + // calculate the blend weight. Returns true if blend could be calculated, false if not bool calc_blend_weights(void); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 15f7ab4eb1..93ed4e7500 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -128,6 +128,17 @@ class AP_GPS_UBLOX : public AP_GPS_Backend const char *name() const override { return "u-blox"; } + enum ubx_hardware_version { + ANTARIS = 0, + UBLOX_5, + UBLOX_6, + UBLOX_7, + UBLOX_M8, + UBLOX_F9 = 0x80, // comes from MON_VER hwVersion string + UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for + // flagging state in the driver + }; + private: // u-blox UBX protocol essentials struct PACKED ubx_header { @@ -564,16 +575,6 @@ class AP_GPS_UBLOX : public AP_GPS_Backend NAV_STATUS_FIX_VALID = 1, NAV_STATUS_DGPS_USED = 2 }; - enum ubx_hardware_version { - ANTARIS = 0, - UBLOX_5, - UBLOX_6, - UBLOX_7, - UBLOX_M8, - UBLOX_F9 = 0x80, // comes from MON_VER hwVersion string - UBLOX_UNKNOWN_HARDWARE_GENERATION = 0xff // not in the ublox spec used for - // flagging state in the driver - }; enum config_step { STEP_PVT = 0, @@ -671,4 +672,9 @@ class AP_GPS_UBLOX : public AP_GPS_Backend uint8_t _ubx_msg_log_index(uint8_t ubx_msg) { return (uint8_t)(ubx_msg + (state.instance * UBX_MSG_TYPES)); } + + // returns hardware generation of the GPS + uint32_t hardware_generation() const override { + return _hardware_generation; + } }; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 1a64e3730f..81ef664f4e 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -147,7 +147,7 @@ void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) cons "GPS %d: detected as %s at %d baud", instance + 1, name(), - gps._baudrates[dstate.current_baud]); + gps.get_baudrate(instance, dstate.current_baud)); } else { hal.util->snprintf(buffer, buflen, "GPS %d: specified as %s", diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 973d7611aa..0dd60f3e30 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -65,6 +65,9 @@ class AP_GPS_Backend virtual bool prepare_for_arming(void) { return true; } + // returns hardware generation variable + virtual uint32_t hardware_generation() const { return 0; } + protected: AP_HAL::UARTDriver *port; ///< UART we are attached to AP_GPS &gps; ///< access to frontend (for parameters) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack+/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack+/defaults.parm new file mode 100644 index 0000000000..e63269317f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack+/defaults.parm @@ -0,0 +1,257 @@ +AHRS_EKF_TYPE 2 +AHRS_ORIENTATION 2 +ANGLE_MAX 2000 +ARMING_CHECK 16318 +ATC_ACCEL_P_MAX 36000 +ATC_ACCEL_R_MAX 36000 +ATC_ACCEL_Y_MAX 10000 +ATC_ANG_LIM_TC 3 +ATC_ANG_PIT_P 6 +ATC_ANG_RLL_P 6 +ATC_ANG_YAW_P 5 +ATC_RAT_PIT_D 0.012 +ATC_RAT_PIT_FLTD 6 +ATC_RAT_PIT_FLTT 6 +ATC_RAT_PIT_IMAX 0.444 +ATC_RAT_PIT_P 0.055 +ATC_RAT_RLL_D 0.012 +ATC_RAT_RLL_FLTD 6 +ATC_RAT_RLL_FLTT 6 +ATC_RAT_RLL_I 0.1 +ATC_RAT_RLL_IMAX 0.444 +ATC_RAT_RLL_P 0.055 +ATC_RAT_YAW_D 0 +ATC_RAT_YAW_FLTE 1.617667 +ATC_RAT_YAW_I 0.088756 +ATC_RAT_YAW_IMAX 0.222 +ATC_RAT_YAW_P 0.4435 +ATC_SLEW_YAW 1500 +ATC_THR_MIX_MAX 0.5 +ATC_THR_MIX_MIN 0.1 +BATT_CAPACITY 13000 +BATT_MONITOR 20 +BATT_VOLT_MULT 22.7 +BRD_SAFETYENABLE 0 +BRD_SAFETY_MASK 0 +BRD_SER1_RTSCTS 2 +BRD_SER2_RTSCTS 2 +CAN_D1_UC_ESC_BM 0 +CAN_D1_UC_SRV_BM 0 +CAN_P1_BITRATE 1000000 +CAN_P1_DRIVER 1 +CAN_P2_DRIVER 0 +CAN_D2_UC_ESC_BM 0 +CAN_D2_UC_SRV_BM 0 +RC7_OPTION 0 +CHUTE_ALT_MIN 0 +CHUTE_DELAY_MS 500 +CHUTE_ENABLED 1 +CHUTE_TYPE 11 +COMPASS_LEARN 0 +COMPASS_MOTCT 2 +COMPASS_MOT_X 2.9 +COMPASS_MOT_Y -1.15 +COMPASS_MOT_Z 2.83 +COMPASS_USE 1 +COMPASS_USE2 1 +COMPASS_USE3 0 +COMPASS_SCALE 1.0 +COMPASS_SCALE2 1.17 +DISARM_DELAY 20 +EK2_ALT_M_NSE 3 +EK2_CHECK_SCALE 100 +EK2_ENABLE 1 +EK2_GBIAS_P_NSE 0.0004 +EK2_GPS_CHECK -1 +EK2_IMU_MASK 3 +EK2_MAGB_P_NSE 0.0002 +EK2_MAGE_P_NSE 0.005 +EK2_MAG_CAL 3 +EK2_RNG_USE_HGT -1 +EK2_MAG_LRN_LIM 30 +FENCE_ENABLE 0 +FLTMODE1 0 +FLTMODE2 0 +FLTMODE3 0 +FLTMODE4 5 +FLTMODE5 2 +FLTMODE6 9 +FRAME_CLASS 1 +FRAME_TYPE 1 +FS_GCS_ENABLE 0 +FS_THR_ENABLE 6 +FS_THR_VALUE 0 +GND_EFFECT_COMP 1 +GPS_HDOP_GOOD 175 +GPS_POS1_X 0.182 +GPS_POS1_Y -0.05 +GPS_POS1_Z -0.04 +GPS_SAVE_CFG 2 +GPS_TYPE 2 +INS_ACCEL_FILTER 20 +INS_FAST_SAMPLE 5 +INS_GYRO_FILTER 20 +INS_GYR_CAL 0 +INS_POS1_X -0.077 +INS_POS1_Y 0 +INS_POS1_Z -0.05 +INS_POS2_X -0.077 +INS_POS2_Y 0 +INS_POS2_Z -0.05 +INS_POS3_X -0.077 +INS_POS3_Y 0 +INS_POS3_Z -0.05 +INS_TRIM_OPTION 0 +INS_USE 1 +INS_USE2 1 +INS_USE3 1 +LAND_REPOSITION 0 +LAND_SPEED 40 +LAND_SPEED_HIGH 200 +LOG_DISARMED 0 +LOG_FILE_BUFSIZE 40 +LOG_FILE_DSRMROT 1 +LOG_REPLAY 0 +MIS_RESTART 0 +MOT_BAT_CURR_MAX 60 +MOT_BAT_CURR_TC 2 +MOT_BAT_POW_MAX 2000 +MOT_BAT_VOLT_MAX 50.8 +MOT_BAT_VOLT_MIN 35 +MOT_HOVER_LEARN 0 +MOT_PWM_MAX 1940 +MOT_PWM_MIN 1060 +MOT_SAFE_DISARM 0 +MOT_SPIN_ARM 0.11 +MOT_SPIN_MAX 1 +MOT_SPIN_MIN 0.15 +MOT_THST_EXPO 0.8 +PILOT_THR_BHV 0 +PILOT_TKOFF_ALT 150 +PLND_ACC_P_NSE 2 +PLND_CAM_POS_X 0.165 +PLND_CAM_POS_Y -0.014 +PLND_CAM_POS_Z 0.151 +PLND_ENABLED 1 +PLND_LAND_OFS_X 0 +PLND_LAND_OFS_Y -2.6 +PLND_TYPE 2 +PLND_YAW_ALIGN 18000 +PSC_ACC_XY_FILT 3.5 +RC1_DZ 30 +RC1_MAX 2000 +RC1_MIN 1002 +RC1_TRIM 1500 +RC2_DZ 30 +RC2_MAX 1999 +RC2_MIN 1000 +RC2_TRIM 1500 +RC3_DZ 30 +RC3_MAX 1998 +RC3_MIN 999 +RC3_TRIM 1500 +RC4_DZ 40 +RC4_MAX 1997 +RC4_MIN 1002 +RC4_TRIM 1500 +RCMAP_PITCH 2 +RCMAP_ROLL 1 +RCMAP_THROTTLE 3 +RCMAP_YAW 4 +RNGFND1_TYPE 7 +RNGFND2_ADDR 112 +RNGFND2_MAX_CM 700 +RNGFND2_MIN_CM 20 +RNGFND2_TYPE 0 +RNGFND1_ADDR 102 +RNGFND1_GNDCLEAR 15 +RNGFND1_MAX_CM 9000 +RNGFND1_MIN_CM 0 +RNGFND1_POS_X -0.124 +RNGFND1_POS_Y -0.097 +RNGFND1_POS_Z 0.037 +RNGFND1_SCALING 1 +RNGFND1_STOP_PIN 55 +SERIAL0_BAUD 115 +SERIAL0_PROTOCOL 2 +SERIAL1_BAUD 921 +SERIAL1_PROTOCOL 1 +SERIAL2_BAUD 57 +SERIAL2_PROTOCOL 5 +SERIAL3_BAUD 38 +SERIAL3_PROTOCOL 5 +SERIAL4_BAUD 115 +SERIAL4_PROTOCOL 100 +SERIAL5_BAUD 57 +SERIAL5_PROTOCOL -1 +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 34 +SERVO3_FUNCTION 36 +SERVO4_FUNCTION 35 +SYSID_MYGCS 255 +TERRAIN_ENABLE 0 +WPNAV_ACCEL 150 +WPNAV_ACCEL_Z 75 +WPNAV_RADIUS 200 +WPNAV_SPEED 1600 +WPNAV_SPEED_DN 400 +WPNAV_SPEED_UP 400 +COMPASS_ORIENT 35 +MOT_THST_HOVER 0.185 +ATC_RAT_PIT_I 0.1 +FS_EKF_ACTION 4 +COMPASS_PRIMARY 1 +FS_EKF_THRESH 0.8 +COMPASS_MOT2_X 1.89 +COMPASS_MOT2_Y 0.37 +COMPASS_MOT2_Z -1.2 +COMPASS_ORIENT2 36 +EK2_ALT_SOURCE 2 +GPS_AUTO_SWITCH 1 +GPS_POS2_X 0.182 +GPS_POS2_Y 0.05 +GPS_POS2_Z -0.04 +GPS_TYPE2 2 +GPS_DELAY_MS2 100 +INS_LOG_BAT_MASK 0 +BATT_AMP_PERVLT 45 +BATT_LOW_VOLT 36 +BATT_FS_LOW_ACT 6 +BATT_LOW_MAH 0 +LOIT_ANG_MAX 20 +PSC_ACCZ_D 0 +PSC_ACCZ_FF 0 +PSC_ACCZ_FLTE 20 +PSC_ACCZ_I 0.5 +PSC_ACCZ_IMAX 800 +PSC_ACCZ_P 0.2 +PSC_ANGLE_MAX 0 +PSC_POSXY_P 1.35 +PSC_POSZ_P 1 +PSC_VELXY_D 0.25 +PSC_VELXY_D_FILT 5 +PSC_VELXY_FILT 5 +PSC_VELXY_I 1.2 +PSC_VELXY_IMAX 1000 +PSC_VELXY_P 1.35 +PSC_VELZ_P 2.0 +BATT_CRT_VOLT 0 +PILOT_SPEED_UP 300 +ATC_INPUT_TC 0.0833 +LOIT_BRK_JERK 3400 +LOIT_ACC_MAX 250 +LOIT_SPEED 750 +BRD_SAFETYOPTION 0 +INS_LOG_BAT_OPT 0 +LOG_BITMASK 65535 +ARM_GPS_HACC 2 +ARM_GPS_VACC 2 +LOG_BACKEND_TYPE 1 +TOFF_ALT_CHANGE 2 +TOFF_BARO_DIP 2 +TOFF_HOV_PCT 80 +TOFF_POS_CHANGE 2 +WP_NAVALT_MAX 1.5 +ESC_CALIBRATION 9 +ADSB_ENABLE 1 +ADSB_LIST_RADIUS 10000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack+/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack+/hwdef.dat new file mode 100644 index 0000000000..3b946260b7 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack+/hwdef.dat @@ -0,0 +1,14 @@ +# hw definition for Matternet CubeBlack+ build + +include ../CubeBlack+/hwdef.dat + +define MODE_SMARTRTL_ENABLED DISABLED +define SPRAYER_ENABLED DISABLED +define GRIPPER_ENABLED DISABLED +define WINCH_ENABLED DISABLED + +# increase arming delay +define ARMING_DELAY_SEC 3.0f + +# log for 60s after disarm +define HAL_LOGGER_ARM_PERSIST 60 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm new file mode 100644 index 0000000000..e63269317f --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/defaults.parm @@ -0,0 +1,257 @@ +AHRS_EKF_TYPE 2 +AHRS_ORIENTATION 2 +ANGLE_MAX 2000 +ARMING_CHECK 16318 +ATC_ACCEL_P_MAX 36000 +ATC_ACCEL_R_MAX 36000 +ATC_ACCEL_Y_MAX 10000 +ATC_ANG_LIM_TC 3 +ATC_ANG_PIT_P 6 +ATC_ANG_RLL_P 6 +ATC_ANG_YAW_P 5 +ATC_RAT_PIT_D 0.012 +ATC_RAT_PIT_FLTD 6 +ATC_RAT_PIT_FLTT 6 +ATC_RAT_PIT_IMAX 0.444 +ATC_RAT_PIT_P 0.055 +ATC_RAT_RLL_D 0.012 +ATC_RAT_RLL_FLTD 6 +ATC_RAT_RLL_FLTT 6 +ATC_RAT_RLL_I 0.1 +ATC_RAT_RLL_IMAX 0.444 +ATC_RAT_RLL_P 0.055 +ATC_RAT_YAW_D 0 +ATC_RAT_YAW_FLTE 1.617667 +ATC_RAT_YAW_I 0.088756 +ATC_RAT_YAW_IMAX 0.222 +ATC_RAT_YAW_P 0.4435 +ATC_SLEW_YAW 1500 +ATC_THR_MIX_MAX 0.5 +ATC_THR_MIX_MIN 0.1 +BATT_CAPACITY 13000 +BATT_MONITOR 20 +BATT_VOLT_MULT 22.7 +BRD_SAFETYENABLE 0 +BRD_SAFETY_MASK 0 +BRD_SER1_RTSCTS 2 +BRD_SER2_RTSCTS 2 +CAN_D1_UC_ESC_BM 0 +CAN_D1_UC_SRV_BM 0 +CAN_P1_BITRATE 1000000 +CAN_P1_DRIVER 1 +CAN_P2_DRIVER 0 +CAN_D2_UC_ESC_BM 0 +CAN_D2_UC_SRV_BM 0 +RC7_OPTION 0 +CHUTE_ALT_MIN 0 +CHUTE_DELAY_MS 500 +CHUTE_ENABLED 1 +CHUTE_TYPE 11 +COMPASS_LEARN 0 +COMPASS_MOTCT 2 +COMPASS_MOT_X 2.9 +COMPASS_MOT_Y -1.15 +COMPASS_MOT_Z 2.83 +COMPASS_USE 1 +COMPASS_USE2 1 +COMPASS_USE3 0 +COMPASS_SCALE 1.0 +COMPASS_SCALE2 1.17 +DISARM_DELAY 20 +EK2_ALT_M_NSE 3 +EK2_CHECK_SCALE 100 +EK2_ENABLE 1 +EK2_GBIAS_P_NSE 0.0004 +EK2_GPS_CHECK -1 +EK2_IMU_MASK 3 +EK2_MAGB_P_NSE 0.0002 +EK2_MAGE_P_NSE 0.005 +EK2_MAG_CAL 3 +EK2_RNG_USE_HGT -1 +EK2_MAG_LRN_LIM 30 +FENCE_ENABLE 0 +FLTMODE1 0 +FLTMODE2 0 +FLTMODE3 0 +FLTMODE4 5 +FLTMODE5 2 +FLTMODE6 9 +FRAME_CLASS 1 +FRAME_TYPE 1 +FS_GCS_ENABLE 0 +FS_THR_ENABLE 6 +FS_THR_VALUE 0 +GND_EFFECT_COMP 1 +GPS_HDOP_GOOD 175 +GPS_POS1_X 0.182 +GPS_POS1_Y -0.05 +GPS_POS1_Z -0.04 +GPS_SAVE_CFG 2 +GPS_TYPE 2 +INS_ACCEL_FILTER 20 +INS_FAST_SAMPLE 5 +INS_GYRO_FILTER 20 +INS_GYR_CAL 0 +INS_POS1_X -0.077 +INS_POS1_Y 0 +INS_POS1_Z -0.05 +INS_POS2_X -0.077 +INS_POS2_Y 0 +INS_POS2_Z -0.05 +INS_POS3_X -0.077 +INS_POS3_Y 0 +INS_POS3_Z -0.05 +INS_TRIM_OPTION 0 +INS_USE 1 +INS_USE2 1 +INS_USE3 1 +LAND_REPOSITION 0 +LAND_SPEED 40 +LAND_SPEED_HIGH 200 +LOG_DISARMED 0 +LOG_FILE_BUFSIZE 40 +LOG_FILE_DSRMROT 1 +LOG_REPLAY 0 +MIS_RESTART 0 +MOT_BAT_CURR_MAX 60 +MOT_BAT_CURR_TC 2 +MOT_BAT_POW_MAX 2000 +MOT_BAT_VOLT_MAX 50.8 +MOT_BAT_VOLT_MIN 35 +MOT_HOVER_LEARN 0 +MOT_PWM_MAX 1940 +MOT_PWM_MIN 1060 +MOT_SAFE_DISARM 0 +MOT_SPIN_ARM 0.11 +MOT_SPIN_MAX 1 +MOT_SPIN_MIN 0.15 +MOT_THST_EXPO 0.8 +PILOT_THR_BHV 0 +PILOT_TKOFF_ALT 150 +PLND_ACC_P_NSE 2 +PLND_CAM_POS_X 0.165 +PLND_CAM_POS_Y -0.014 +PLND_CAM_POS_Z 0.151 +PLND_ENABLED 1 +PLND_LAND_OFS_X 0 +PLND_LAND_OFS_Y -2.6 +PLND_TYPE 2 +PLND_YAW_ALIGN 18000 +PSC_ACC_XY_FILT 3.5 +RC1_DZ 30 +RC1_MAX 2000 +RC1_MIN 1002 +RC1_TRIM 1500 +RC2_DZ 30 +RC2_MAX 1999 +RC2_MIN 1000 +RC2_TRIM 1500 +RC3_DZ 30 +RC3_MAX 1998 +RC3_MIN 999 +RC3_TRIM 1500 +RC4_DZ 40 +RC4_MAX 1997 +RC4_MIN 1002 +RC4_TRIM 1500 +RCMAP_PITCH 2 +RCMAP_ROLL 1 +RCMAP_THROTTLE 3 +RCMAP_YAW 4 +RNGFND1_TYPE 7 +RNGFND2_ADDR 112 +RNGFND2_MAX_CM 700 +RNGFND2_MIN_CM 20 +RNGFND2_TYPE 0 +RNGFND1_ADDR 102 +RNGFND1_GNDCLEAR 15 +RNGFND1_MAX_CM 9000 +RNGFND1_MIN_CM 0 +RNGFND1_POS_X -0.124 +RNGFND1_POS_Y -0.097 +RNGFND1_POS_Z 0.037 +RNGFND1_SCALING 1 +RNGFND1_STOP_PIN 55 +SERIAL0_BAUD 115 +SERIAL0_PROTOCOL 2 +SERIAL1_BAUD 921 +SERIAL1_PROTOCOL 1 +SERIAL2_BAUD 57 +SERIAL2_PROTOCOL 5 +SERIAL3_BAUD 38 +SERIAL3_PROTOCOL 5 +SERIAL4_BAUD 115 +SERIAL4_PROTOCOL 100 +SERIAL5_BAUD 57 +SERIAL5_PROTOCOL -1 +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 34 +SERVO3_FUNCTION 36 +SERVO4_FUNCTION 35 +SYSID_MYGCS 255 +TERRAIN_ENABLE 0 +WPNAV_ACCEL 150 +WPNAV_ACCEL_Z 75 +WPNAV_RADIUS 200 +WPNAV_SPEED 1600 +WPNAV_SPEED_DN 400 +WPNAV_SPEED_UP 400 +COMPASS_ORIENT 35 +MOT_THST_HOVER 0.185 +ATC_RAT_PIT_I 0.1 +FS_EKF_ACTION 4 +COMPASS_PRIMARY 1 +FS_EKF_THRESH 0.8 +COMPASS_MOT2_X 1.89 +COMPASS_MOT2_Y 0.37 +COMPASS_MOT2_Z -1.2 +COMPASS_ORIENT2 36 +EK2_ALT_SOURCE 2 +GPS_AUTO_SWITCH 1 +GPS_POS2_X 0.182 +GPS_POS2_Y 0.05 +GPS_POS2_Z -0.04 +GPS_TYPE2 2 +GPS_DELAY_MS2 100 +INS_LOG_BAT_MASK 0 +BATT_AMP_PERVLT 45 +BATT_LOW_VOLT 36 +BATT_FS_LOW_ACT 6 +BATT_LOW_MAH 0 +LOIT_ANG_MAX 20 +PSC_ACCZ_D 0 +PSC_ACCZ_FF 0 +PSC_ACCZ_FLTE 20 +PSC_ACCZ_I 0.5 +PSC_ACCZ_IMAX 800 +PSC_ACCZ_P 0.2 +PSC_ANGLE_MAX 0 +PSC_POSXY_P 1.35 +PSC_POSZ_P 1 +PSC_VELXY_D 0.25 +PSC_VELXY_D_FILT 5 +PSC_VELXY_FILT 5 +PSC_VELXY_I 1.2 +PSC_VELXY_IMAX 1000 +PSC_VELXY_P 1.35 +PSC_VELZ_P 2.0 +BATT_CRT_VOLT 0 +PILOT_SPEED_UP 300 +ATC_INPUT_TC 0.0833 +LOIT_BRK_JERK 3400 +LOIT_ACC_MAX 250 +LOIT_SPEED 750 +BRD_SAFETYOPTION 0 +INS_LOG_BAT_OPT 0 +LOG_BITMASK 65535 +ARM_GPS_HACC 2 +ARM_GPS_VACC 2 +LOG_BACKEND_TYPE 1 +TOFF_ALT_CHANGE 2 +TOFF_BARO_DIP 2 +TOFF_HOV_PCT 80 +TOFF_POS_CHANGE 2 +WP_NAVALT_MAX 1.5 +ESC_CALIBRATION 9 +ADSB_ENABLE 1 +ADSB_LIST_RADIUS 10000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/hwdef.dat new file mode 100644 index 0000000000..c4148a3445 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeBlack/hwdef.dat @@ -0,0 +1,14 @@ +# hw definition for Matternet CubeBlack build + +include ../CubeBlack/hwdef.dat + +define MODE_SMARTRTL_ENABLED DISABLED +define SPRAYER_ENABLED DISABLED +define GRIPPER_ENABLED DISABLED +define WINCH_ENABLED DISABLED + +# increase arming delay +define ARMING_DELAY_SEC 3.0f + +# log for 60s after disarm +define HAL_LOGGER_ARM_PERSIST 60 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm new file mode 100644 index 0000000000..cdb9b67dc0 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/defaults.parm @@ -0,0 +1,263 @@ +AHRS_EKF_TYPE 2 +AHRS_ORIENTATION 2 +ANGLE_MAX 2000 +ARMING_CHECK 16318 +ATC_ACCEL_P_MAX 30000 +ATC_ACCEL_R_MAX 36000 +ATC_ACCEL_Y_MAX 10000 +ATC_ANG_LIM_TC 3 +ATC_ANG_PIT_P 6 +ATC_ANG_RLL_P 6 +ATC_ANG_YAW_P 5 +ATC_RAT_PIT_D 0.01 +ATC_RAT_PIT_FLTD 10 +ATC_RAT_PIT_FLTT 10 +ATC_RAT_PIT_IMAX 0.444 +ATC_RAT_PIT_P 0.11 +ATC_RAT_RLL_D 0.01 +ATC_RAT_RLL_FLTD 10 +ATC_RAT_RLL_FLTT 10 +ATC_RAT_RLL_I 0.1 +ATC_RAT_RLL_IMAX 0.444 +ATC_RAT_RLL_P 0.1 +ATC_RAT_YAW_D 0 +ATC_RAT_YAW_FLTE 1.617667 +ATC_RAT_YAW_FLTT 10 +ATC_RAT_YAW_I 0.088756 +ATC_RAT_YAW_IMAX 0.222 +ATC_RAT_YAW_P 0.8 +ATC_SLEW_YAW 1500 +ATC_THR_MIX_MAX 0.5 +ATC_THR_MIX_MIN 0.1 +BATT_CAPACITY 13000 +BATT_MONITOR 20 +BATT_VOLT_MULT 22.7 +BRD_SAFETYENABLE 0 +BRD_SAFETY_MASK 0 +BRD_SER1_RTSCTS 2 +BRD_SER2_RTSCTS 2 +CAN_D1_UC_ESC_BM 0 +CAN_D1_UC_SRV_BM 0 +CAN_P1_BITRATE 1000000 +CAN_P1_DRIVER 1 +CAN_P2_DRIVER 0 +RC7_OPTION 0 +CHUTE_ALT_MIN 0 +CHUTE_DELAY_MS 500 +CHUTE_ENABLED 1 +CHUTE_TYPE 11 +COMPASS_LEARN 0 +COMPASS_MOTCT 2 +COMPASS_MOT_X 2.9 +COMPASS_MOT_Y -1.15 +COMPASS_MOT_Z 2.83 +COMPASS_USE 1 +COMPASS_USE2 1 +COMPASS_USE3 0 +COMPASS_SCALE 1.0 +COMPASS_SCALE2 1.17 +DISARM_DELAY 20 +EK2_ALT_M_NSE 3 +EK2_CHECK_SCALE 100 +EK2_ENABLE 1 +EK2_GBIAS_P_NSE 0.0004 +EK2_GPS_CHECK -1 +EK2_IMU_MASK 3 +EK2_MAGB_P_NSE 0.0002 +EK2_MAGE_P_NSE 0.005 +EK2_MAG_CAL 3 +EK2_RNG_USE_HGT -1 +EK2_MAG_LRN_LIM 30 +FENCE_ENABLE 0 +FLTMODE1 0 +FLTMODE2 0 +FLTMODE3 0 +FLTMODE4 5 +FLTMODE5 2 +FLTMODE6 9 +FRAME_CLASS 1 +FRAME_TYPE 1 +FS_GCS_ENABLE 0 +FS_THR_ENABLE 6 +FS_THR_VALUE 0 +GND_EFFECT_COMP 1 +GPS_HDOP_GOOD 175 +GPS_POS1_X 0.182 +GPS_POS1_Y -0.05 +GPS_POS1_Z -0.04 +GPS_SAVE_CFG 2 +GPS_TYPE 2 +INS_ACCEL_FILTER 20 +INS_FAST_SAMPLE 7 +INS_GYRO_FILTER 20 +INS_GYR_CAL 0 +INS_POS1_X -0.077 +INS_POS1_Y 0 +INS_POS1_Z -0.05 +INS_POS2_X -0.077 +INS_POS2_Y 0 +INS_POS2_Z -0.05 +INS_POS3_X -0.077 +INS_POS3_Y 0 +INS_POS3_Z -0.05 +INS_TRIM_OPTION 0 +INS_USE 1 +INS_USE2 1 +INS_USE3 1 +INS_HNTCH_ENABLE 1 +INS_HNTCH_FREQ 56 +INS_HNTCH_BW 37 +INS_HNTCH_ATT 40 +INS_HNTCH_REF 0.193 +INS_HNTCH_MODE 1 +INS_HNTCH_HMNCS 15 +LAND_REPOSITION 0 +LAND_SPEED 40 +LAND_SPEED_HIGH 200 +LOG_DISARMED 0 +LOG_FILE_BUFSIZE 60 +LOG_FILE_DSRMROT 1 +LOG_REPLAY 0 +MIS_RESTART 0 +MOT_BAT_CURR_MAX 60 +MOT_BAT_CURR_TC 2 +MOT_BAT_POW_MAX 2000 +MOT_BAT_VOLT_MAX 50.8 +MOT_BAT_VOLT_MIN 35 +MOT_HOVER_LEARN 0 +MOT_PWM_MAX 1940 +MOT_PWM_MIN 1060 +MOT_SAFE_DISARM 0 +MOT_SPIN_ARM 0.11 +MOT_SPIN_MAX 1 +MOT_SPIN_MIN 0.15 +MOT_THST_EXPO 0.8 +PILOT_THR_BHV 0 +PILOT_TKOFF_ALT 150 +PLND_ACC_P_NSE 2 +PLND_CAM_POS_X 0.165 +PLND_CAM_POS_Y -0.014 +PLND_CAM_POS_Z 0.151 +PLND_ENABLED 1 +PLND_LAND_OFS_X 0 +PLND_LAND_OFS_Y -2.6 +PLND_TYPE 2 +PLND_YAW_ALIGN 18000 +PSC_ACC_XY_FILT 3.5 +RC1_DZ 30 +RC1_MAX 2000 +RC1_MIN 1002 +RC1_TRIM 1500 +RC2_DZ 30 +RC2_MAX 1999 +RC2_MIN 1000 +RC2_TRIM 1500 +RC3_DZ 30 +RC3_MAX 1998 +RC3_MIN 999 +RC3_TRIM 1500 +RC4_DZ 40 +RC4_MAX 1997 +RC4_MIN 1002 +RC4_TRIM 1500 +RCMAP_PITCH 2 +RCMAP_ROLL 1 +RCMAP_THROTTLE 3 +RCMAP_YAW 4 +RNGFND1_TYPE 7 +RNGFND2_ADDR 112 +RNGFND2_MAX_CM 10000 +RNGFND2_MIN_CM 50 +RNGFND2_TYPE 0 +RNGFND1_ADDR 102 +RNGFND1_GNDCLEAR 15 +RNGFND1_MAX_CM 9000 +RNGFND1_MIN_CM 0 +RNGFND1_POS_X -0.124 +RNGFND1_POS_Y -0.097 +RNGFND1_POS_Z 0.037 +RNGFND1_SCALING 1 +RNGFND1_STOP_PIN 55 +SERIAL0_BAUD 115 +SERIAL0_PROTOCOL 2 +SERIAL1_BAUD 921 +SERIAL1_PROTOCOL 1 +SERIAL2_BAUD 57 +SERIAL2_PROTOCOL 5 +SERIAL3_BAUD 38 +SERIAL3_PROTOCOL 5 +SERIAL4_BAUD 115 +SERIAL4_PROTOCOL 100 +SERIAL5_BAUD 57 +SERIAL5_PROTOCOL -1 +SERVO1_FUNCTION 33 +SERVO2_FUNCTION 34 +SERVO3_FUNCTION 36 +SERVO4_FUNCTION 35 +SYSID_MYGCS 255 +TERRAIN_ENABLE 0 +WPNAV_ACCEL 120 +WPNAV_ACCEL_Z 75 +WPNAV_RADIUS 200 +WPNAV_SPEED 1600 +WPNAV_SPEED_DN 400 +WPNAV_SPEED_UP 400 +COMPASS_ORIENT 35 +MOT_THST_HOVER 0.185 +ATC_RAT_PIT_I 0.1 +FS_EKF_ACTION 4 +COMPASS_PRIMARY 1 +FS_EKF_THRESH 0.8 +COMPASS_MOT2_X 1.89 +COMPASS_MOT2_Y 0.37 +COMPASS_MOT2_Z -1.2 +COMPASS_ORIENT2 36 +EK2_ALT_SOURCE 2 +GPS_AUTO_SWITCH 1 +GPS_POS2_X 0.182 +GPS_POS2_Y 0.05 +GPS_POS2_Z -0.04 +GPS_TYPE2 2 +GPS_DELAY_MS2 100 +INS_LOG_BAT_MASK 0 +BATT_AMP_PERVLT 45 +BATT_LOW_VOLT 36 +BATT_FS_LOW_ACT 6 +BATT_LOW_MAH 0 +LOIT_ANG_MAX 20 +PSC_ACCZ_D 0 +PSC_ACCZ_FF 0 +PSC_ACCZ_FLTE 20 +PSC_ACCZ_I 0.5 +PSC_ACCZ_IMAX 800 +PSC_ACCZ_P 0.2 +PSC_ANGLE_MAX 0 +PSC_POSXY_P 1.35 +PSC_POSZ_P 1 +PSC_VELXY_D 0.25 +PSC_VELXY_D_FILT 5 +PSC_VELXY_FILT 5 +PSC_VELXY_I 1.2 +PSC_VELXY_IMAX 1000 +PSC_VELXY_P 1.35 +PSC_VELZ_P 2.0 +BATT_CRT_VOLT 0 +PILOT_SPEED_UP 300 +ATC_INPUT_TC 0.0833 +LOIT_BRK_JERK 3400 +LOIT_ACC_MAX 250 +LOIT_SPEED 750 +BRD_SAFETYOPTION 0 +INS_LOG_BAT_OPT 0 +LOG_BITMASK 65535 +ARM_GPS_HACC 2 +ARM_GPS_VACC 2 +LOG_BACKEND_TYPE 1 +TOFF_ALT_CHANGE 2 +TOFF_BARO_DIP 2 +TOFF_HOV_PCT 80 +TOFF_POS_CHANGE 2 +WP_NAVALT_MAX 1.5 +ESC_CALIBRATION 9 +ADSB_ENABLE 1 +ADSB_LIST_RADIUS 10000 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/hwdef.dat new file mode 100644 index 0000000000..69f106ad5d --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/MttrCubeOrange/hwdef.dat @@ -0,0 +1,20 @@ +# hw definition for Matternet CubeOrange build + +include ../CubeOrange/hwdef.dat + +define MODE_SMARTRTL_ENABLED DISABLED +define SPRAYER_ENABLED DISABLED +define GRIPPER_ENABLED DISABLED +define WINCH_ENABLED DISABLED + +# disable internal compasses +undef COMPASS + +# this is a dummy so we don't fall back to the AP_FEATURE_BOARD_DETECT compass list +COMPASS LSM303D SPI:lsm9ds0_ext_am ROTATION_YAW_270 + +# increase arming delay +define ARMING_DELAY_SEC 3.0f + +# log for 60s after disarm +define HAL_LOGGER_ARM_PERSIST 60 diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp index 2667f4ff48..db4705640f 100644 --- a/libraries/AP_HAL_SITL/Scheduler.cpp +++ b/libraries/AP_HAL_SITL/Scheduler.cpp @@ -344,6 +344,7 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_ */ void Scheduler::check_thread_stacks(void) { +#if 0 WITH_SEMAPHORE(_thread_sem); for (struct thread_attr *p=threads; p; p=p->next) { const uint8_t ncheck = 8; @@ -353,4 +354,5 @@ void Scheduler::check_thread_stacks(void) } } } +#endif } diff --git a/libraries/AP_HAL_SITL/sitl_gps.cpp b/libraries/AP_HAL_SITL/sitl_gps.cpp index 8fdb5eb97e..7d53fa5c1d 100644 --- a/libraries/AP_HAL_SITL/sitl_gps.cpp +++ b/libraries/AP_HAL_SITL/sitl_gps.cpp @@ -1301,41 +1301,62 @@ void SITL_State::_update_gps(double latitude, double longitude, float altitude, } void SITL_State::_update_gps_instance(SITL::SITL::GPSType gps_type, const struct gps_data *data, uint8_t instance) { + struct gps_data d2 = *data; + + static uint32_t start_ms; + uint32_t now = AP_HAL::millis(); + if (hal.util->get_soft_armed()) { + if (start_ms == 0) { + start_ms = now; + } + float dt = (now - start_ms) * 0.001; + // drift to N + //d2.speedN += _sitl->gps_drift_spd; + Location loc; + loc.lat = d2.latitude*1.0e7; + loc.lng = d2.longitude*1.0e7; + loc.offset(dt * _sitl->gps_drift_spd, 0); + d2.latitude = loc.lat * 1.0e-7; + d2.longitude = loc.lng * 1.0e-7; + } else { + start_ms = 0; + } + switch (gps_type) { case SITL::SITL::GPS_TYPE_NONE: // no GPS attached break; case SITL::SITL::GPS_TYPE_UBLOX: - _update_gps_ubx(data, instance); + _update_gps_ubx(&d2, instance); break; case SITL::SITL::GPS_TYPE_MTK: - _update_gps_mtk(data, instance); + _update_gps_mtk(&d2, instance); break; case SITL::SITL::GPS_TYPE_MTK16: - _update_gps_mtk16(data, instance); + _update_gps_mtk16(&d2, instance); break; case SITL::SITL::GPS_TYPE_MTK19: - _update_gps_mtk19(data, instance); + _update_gps_mtk19(&d2, instance); break; case SITL::SITL::GPS_TYPE_NMEA: - _update_gps_nmea(data, instance); + _update_gps_nmea(&d2, instance); break; case SITL::SITL::GPS_TYPE_SBP: - _update_gps_sbp(data, instance); + _update_gps_sbp(&d2, instance); break; case SITL::SITL::GPS_TYPE_SBP2: - _update_gps_sbp2(data, instance); + _update_gps_sbp2(&d2, instance); break; case SITL::SITL::GPS_TYPE_NOVA: - _update_gps_nova(data, instance); + _update_gps_nova(&d2, instance); break; case SITL::SITL::GPS_TYPE_FILE: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 35745f6733..a3d66ad983 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -23,6 +23,7 @@ #include "AP_InertialSensor_BMI055.h" #include "AP_InertialSensor_BMI088.h" #include "AP_InertialSensor_Invensensev2.h" +#include "AP_InertialSensor_UAVCAN.h" /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop. * Output is on the debug console. */ @@ -582,6 +583,11 @@ void AP_InertialSensor::_start_backends() for (uint8_t i = 0; i < _backend_count; i++) { _backends[i]->start(); + if (_gyro_count == INS_MAX_INSTANCES && + _accel_count == INS_MAX_INSTANCES) { + // this can happen with UAVCAN backends and 3 SPI IMUs + break; + } } if (_gyro_count == 0 || _accel_count == 0) { @@ -733,6 +739,11 @@ AP_InertialSensor::detect_backends(void) ADD_BACKEND(AP_InertialSensor_HIL::detect(*this)); return; } + +#if HAL_WITH_UAVCAN + ADD_BACKEND(AP_InertialSensor_UAVCAN::probe(*this)); +#endif + #if defined(HAL_INS_PROBE_LIST) // IMUs defined by IMU lines in hwdef.dat HAL_INS_PROBE_LIST; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UAVCAN.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_UAVCAN.cpp new file mode 100644 index 0000000000..c9f23f4170 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UAVCAN.cpp @@ -0,0 +1,172 @@ +#include + +#if HAL_WITH_UAVCAN + +#include "AP_InertialSensor_UAVCAN.h" + +#include +#include + +#include + +extern const AP_HAL::HAL& hal; + +#define BACKEND_SAMPLE_RATE 2000 + +// UAVCAN Frontend Registry Binder +UC_REGISTRY_BINDER(FastIMUCb, ardupilot::imu::FastIMU); + +AP_InertialSensor_UAVCAN::DetectedModules AP_InertialSensor_UAVCAN::_detected_modules[] = {0}; + +HAL_Semaphore AP_InertialSensor_UAVCAN::_sem_registry; + +/* + constructor + */ +AP_InertialSensor_UAVCAN::AP_InertialSensor_UAVCAN(AP_InertialSensor &ins) : + AP_InertialSensor_Backend(ins) +{} + +/* + subscribe to UAVCAN messages, called in AP_UAVCAN startup + */ +void AP_InertialSensor_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) +{ + if (ap_uavcan == nullptr) { + return; + } + + auto *node = ap_uavcan->get_node(); + + uavcan::Subscriber *FastIMU_listener; + FastIMU_listener = new uavcan::Subscriber(*node); + + // Msg Handler + const int FastIMU_listener_res = FastIMU_listener->start(FastIMUCb(ap_uavcan, &handle_FastIMU)); + if (FastIMU_listener_res < 0) { + AP_HAL::panic("UAVCAN InertialSensor subscriber start fail"); + return; + } +} + +AP_InertialSensor_Backend* AP_InertialSensor_UAVCAN::probe(AP_InertialSensor &ins) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_InertialSensor_UAVCAN* backend = nullptr; + for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) { + if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) { + backend = new AP_InertialSensor_UAVCAN(ins); + if (backend) { + _detected_modules[i].driver = backend; + backend->_ap_uavcan = _detected_modules[i].ap_uavcan; + backend->_node_id = _detected_modules[i].node_id; + } + break; + } + } + return backend; +} + +/* + find a matching backend for a node_id + */ +AP_InertialSensor_UAVCAN* AP_InertialSensor_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 < INS_MAX_INSTANCES; 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 < INS_MAX_INSTANCES; i++) { + if (_detected_modules[i].ap_uavcan == ap_uavcan && _detected_modules[i].node_id == node_id) { + already_detected = true; + break; + } + } + if (!already_detected) { + for (uint8_t i = 0; i < INS_MAX_INSTANCES; 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; +} + +/* + handler for incoming FastIMU messages + */ +void AP_InertialSensor_UAVCAN::handle_FastIMU(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FastIMUCb &cb) +{ + AP_InertialSensor_UAVCAN* driver; + { + WITH_SEMAPHORE(_sem_registry); + driver = get_uavcan_backend(ap_uavcan, node_id, true); + if (driver == nullptr) { + return; + } + } + { + auto *msg = cb.msg; + static uint8_t last_counter; + if ((last_counter+1) % 256 != msg->counter) { + ::printf("FastIMU lost %u %u\n", last_counter, msg->counter); + } + last_counter = msg->counter; + + Vector3f accel(msg->accel[0], msg->accel[1], msg->accel[2]); + accel *= (10.0 * (1+msg->accel_scale)) / INT16_MAX; + + Vector3f gyro(msg->gyro[0], msg->gyro[1], msg->gyro[2]); + gyro *= float(1+msg->gyro_scale) / INT16_MAX; + + WITH_SEMAPHORE(driver->_sem); + driver->sample_time_us = msg->time_us; + + driver->_rotate_and_correct_accel(driver->accel_instance, accel); + driver->_notify_new_accel_raw_sample(driver->accel_instance, accel); + + driver->_rotate_and_correct_gyro(driver->gyro_instance, gyro); + driver->_notify_new_gyro_raw_sample(driver->gyro_instance, gyro); + } +} + +// Read the sensor +bool AP_InertialSensor_UAVCAN::update(void) +{ + WITH_SEMAPHORE(_sem); + update_accel(accel_instance); + update_gyro(gyro_instance); + return true; +} + +void AP_InertialSensor_UAVCAN::start() +{ + if (sample_time_us != 0) { + uint32_t devid = AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_UAVCAN, + _ap_uavcan->get_driver_index(), + _node_id, + 0); + ::printf("registering UAVCAN IMU for %u\n", _node_id); + accel_instance = _imu.register_accel(1e6/sample_time_us, devid); + gyro_instance = _imu.register_gyro(1e6/sample_time_us, devid); + } +} + + +#endif // HAL_WITH_UAVCAN + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_UAVCAN.h b/libraries/AP_InertialSensor/AP_InertialSensor_UAVCAN.h new file mode 100644 index 0000000000..2ce4d27a4e --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_UAVCAN.h @@ -0,0 +1,57 @@ +/* + * 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 . + */ +/* + support for UAVCAN IMUs using the ardupilot::imu::FastIMU message + */ +#pragma once + +#include + +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" +#include + +class FastIMUCb; + +class AP_InertialSensor_UAVCAN : public AP_InertialSensor_Backend { +public: + void start() override; + bool update() override; + + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); + static AP_InertialSensor_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id, bool create_new); + static AP_InertialSensor_Backend* probe(AP_InertialSensor &ins); + + static void handle_FastIMU(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FastIMUCb &cb); + +private: + AP_InertialSensor_UAVCAN(AP_InertialSensor &imu); + + AP_UAVCAN* _ap_uavcan; + uint8_t _node_id; + uint16_t sample_time_us; + + // Module Detection Registry + static struct DetectedModules { + AP_UAVCAN *ap_uavcan; + uint8_t node_id; + AP_InertialSensor_UAVCAN *driver; + } _detected_modules[INS_MAX_INSTANCES]; + + static HAL_Semaphore _sem_registry; + + uint8_t accel_instance; + uint8_t gyro_instance; +}; diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 603097f0ac..3e2741f81c 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -157,9 +157,15 @@ enum class LogErrorCode : uint8_t { FAILED_CIRCLE_INIT = 4, DEST_OUTSIDE_FENCE = 5, -// parachute failed to deploy because of low altitude or landed +// parachute reasons PARACHUTE_TOO_LOW = 2, PARACHUTE_LANDED = 3, + PARACHUTE_ANGLE_ERROR_EXCESSIVE_TIMEOUT = 5, + PARACHUTE_TILT_ANGLE_EXCESSIVE = 6, + PARACHUTE_VEL_Z_ERROR_EXCESSIVE_TIMEOUT = 7, + PARACHUTE_STABILIZE_THROTTLE_CUT = 8, + PARACHUTE_REASON_EKF_FAILSAFE = 9, + // EKF check definitions EKFCHECK_BAD_VARIANCE = 2, EKFCHECK_VARIANCE_CLEARED = 0, diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index b5b2a32948..125799296e 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -1084,6 +1084,9 @@ void AP_Logger_File::vehicle_was_disarmed() // logging restart naturally based on log_disarmed should do // the trick: _rotate_pending = true; + + // write out all of our system ID and vars again at end of log + start_new_log_reset_variables(); } } diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index c9f05dbd5a..d221d32ad8 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -27,10 +27,16 @@ const AP_Param::GroupInfo AP_Mission::var_info[] = { // @Param: OPTIONS // @DisplayName: Mission options bitmask // @Description: Bitmask of what options to use in missions. - // @Bitmask: 0:Clear Mission on reboot + // @Bitmask: 0:Clear Mission on reboot, 2:ContinueAfterLand // @User: Advanced AP_GROUPINFO("OPTIONS", 2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT), + // @Param: FLIGHT_ID + // @DisplayName: Mission flight ID + // @Description: Mission ID for log analysis + // @User: Advanced + AP_GROUPINFO("FLIGHT_ID", 20, AP_Mission, _flight_id, 0), + AP_GROUPEND }; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 9034942fba..e5a3323953 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -39,6 +39,8 @@ #define AP_MISSION_OPTIONS_DEFAULT 0 // Do not clear the mission when rebooting #define AP_MISSION_MASK_MISSION_CLEAR (1<<0) // If set then Clear the mission on boot +#define AP_MISSION_MASK_CONTINUE_AFTER_LAND (1<<2) // Allow mission to continue after land + /// @class AP_Mission /// @brief Object managing Mission class AP_Mission { @@ -480,6 +482,15 @@ class AP_Mission { // returns true if the mission contains the requested items bool contains_item(MAV_CMD command) const; + /* + return true if MIS_OPTIONS is set to allow continue of mission + logic after a land. If this is false then after a landing is + complete the vehicle should disarm and mission logic should stop + */ + bool continue_after_land(void) const { + return (_options.get() & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0; + } + // user settable parameters static const struct AP_Param::GroupInfo var_info[]; @@ -555,6 +566,7 @@ class AP_Mission { AP_Int16 _cmd_total; // total number of commands in the mission AP_Int8 _restart; // controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run) AP_Int16 _options; // bitmask options for missions, currently for mission clearing on reboot but can be expanded as required + AP_Int32 _flight_id; // user settable ID for log analysis // pointer to main program functions mission_cmd_fn_t _cmd_start_fn; // pointer to function which will be called when a new command is started diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 82129b56f4..65806a561f 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -503,6 +503,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1); add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); break; + case MOTOR_FRAME_TYPE_MATTERNET_M2: case MOTOR_FRAME_TYPE_X: add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3); @@ -736,6 +737,17 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4); add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6); break; + case MOTOR_FRAME_TYPE_MATTERNET_M2: + add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1); + add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7); + add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5); + add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3); + add_motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8); + add_motor(AP_MOTORS_MOT_5, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2); + add_motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4); + add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6); + success = true; + break; default: // octaquad frame class does not support this frame type success = false; diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 2c86268e31..e20a4010c6 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -176,7 +176,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { // @User: Advanced AP_GROUPINFO("BOOST_SCALE", 37, AP_MotorsMulticopter, _boost_scale, 0), - // 38 RESERVED for BAT_POW_MAX + // @Param: BAT_POW_MAX + // @DisplayName: Motor Power Max + // @Description: Maximum power over which maximum throttle is limited (0 = Disabled) + // @Range: 0 20000 + // @Units: W + // @User: Advanced + AP_GROUPINFO("BAT_POW_MAX", 38, AP_MotorsMulticopter, _batt_power_max, 0), // @Param: BAT_IDX // @DisplayName: Battery compensation index @@ -296,9 +302,20 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() { AP_BattMonitor &battery = AP::battery(); - float _batt_current; + float batt_voltage = constrain_float(battery.voltage(_batt_idx), _batt_voltage_min, _batt_voltage_max); + + float batt_current_max = 0; + if (!is_zero(_batt_current_max) && !is_zero(_batt_power_max) && !is_zero(batt_voltage)) { + batt_current_max = MIN(_batt_current_max, _batt_power_max/batt_voltage); + } else if(!is_zero(_batt_power_max) && !is_zero(batt_voltage)) { + batt_current_max = _batt_power_max/batt_voltage; + } else if(!is_zero(_batt_current_max)) { + batt_current_max = _batt_current_max; + } - if (_batt_current_max <= 0 || // return maximum if current limiting is disabled + // return maximum if current limiting is disabled + float _batt_current; + if (batt_current_max <= 0 || // return maximum if current limiting is disabled !_flags.armed || // remove throttle limit if disarmed !battery.current_amps(_batt_current, _batt_idx)) { // no current monitoring is available _throttle_limit = 1.0f; @@ -313,9 +330,7 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() } // calculate the maximum current to prevent voltage sag below _batt_voltage_min - float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx) - _batt_voltage_min) / _batt_resistance); - - float batt_current_ratio = _batt_current / batt_current_max; + float batt_current_ratio = _batt_current/batt_current_max; float loop_interval = 1.0f / _loop_rate; _throttle_limit += (loop_interval / (loop_interval + _batt_current_time_constant)) * (1.0f - batt_current_ratio); diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 2b2db7610d..e9fa727e26 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -163,6 +163,7 @@ class AP_MotorsMulticopter : public AP_Motors { AP_Float _throttle_hover; // estimated throttle required to hover throttle in the range 0 ~ 1 AP_Int8 _throttle_hover_learn; // enable/disabled hover thrust learning AP_Int8 _disarm_disable_pwm; // disable PWM output while disarmed + AP_Float _batt_power_max; // power over which maximum throttle is limited // Maximum lean angle of yaw servo in degrees. This is specific to tricopter AP_Float _yaw_servo_angle_max_deg; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 9e18d0a1fa..c518c50013 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -59,6 +59,7 @@ class AP_Motors { MOTOR_FRAME_TYPE_DJI_X = 13, // X frame, DJI ordering MOTOR_FRAME_TYPE_CW_X = 14, // X frame, clockwise ordering MOTOR_FRAME_TYPE_I = 15, // (sideways H) octo only + MOTOR_FRAME_TYPE_MATTERNET_M2 = 100, }; // Constructor @@ -263,4 +264,4 @@ class AP_Motors { namespace AP { AP_Motors *motors(); -}; \ No newline at end of file +}; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 12a0d75f5a..804e65ab22 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -582,6 +582,14 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @RebootRequired: False AP_GROUPINFO("HRT_FILT", 53, NavEKF2, _hrt_filt_freq, 2.0f), + // @Param: MAG_LRN_LIM + // @DisplayName: Magnetometer offset learning limit + // @Description: This limits the size of the learned magnetometer offsets. A value of zero means no limit. Limiting the learned offsets can be helpful when the compass calibration is known to be good and the vehicle is operating in an environment that can lead to significant incorrect learned offsets due to temporary local fields + // @User: Advanced + // @Range: 0 500 + // @Units: mGauss + AP_GROUPINFO("MAG_LRN_LIM", 54, NavEKF2, _mag_learn_limit, 0), + AP_GROUPEND }; @@ -1347,11 +1355,70 @@ void NavEKF2::getFilterGpsStatus(int8_t instance, nav_gps_status &status) const } // send an EKF_STATUS_REPORT message to GCS +// for each variance we send the maximum of all lanes void NavEKF2::send_status_report(mavlink_channel_t chan) { - if (core) { - core[primary].send_status_report(chan); + if (!core) { + return; + } + nav_filter_status filterStatus = core[primary].filterStatus; + + // prepare flags + uint16_t flags = 0; + if (filterStatus.flags.attitude) { + flags |= EKF_ATTITUDE; + } + if (filterStatus.flags.horiz_vel) { + flags |= EKF_VELOCITY_HORIZ; + } + if (filterStatus.flags.vert_vel) { + flags |= EKF_VELOCITY_VERT; + } + if (filterStatus.flags.horiz_pos_rel) { + flags |= EKF_POS_HORIZ_REL; } + if (filterStatus.flags.horiz_pos_abs) { + flags |= EKF_POS_HORIZ_ABS; + } + if (filterStatus.flags.vert_pos) { + flags |= EKF_POS_VERT_ABS; + } + if (filterStatus.flags.terrain_alt) { + flags |= EKF_POS_VERT_AGL; + } + if (filterStatus.flags.const_pos_mode) { + flags |= EKF_CONST_POS_MODE; + } + if (filterStatus.flags.pred_horiz_pos_rel) { + flags |= EKF_PRED_POS_HORIZ_REL; + } + if (filterStatus.flags.pred_horiz_pos_abs) { + flags |= EKF_PRED_POS_HORIZ_ABS; + } + + // get variances + float velVar=0, posVar=0, hgtVar=0, tasVar=0, magVarLen=0, rngVar=0; + Vector2f offset; + + for (uint8_t i=0; i 0) || core[i].PV_AidingMode == NavEKF2_core::AID_RELATIVE || core[i].flowDataValid) { + rngVar = MAX(rngVar, sqrtf(core[primary].auxRngTestRatio)); + } + } + + // send message + mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVarLen, rngVar, tasVar); } // provides the height limit to be observed by the control loops diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index b0d4bc39ef..a993c6ebfd 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -418,12 +418,14 @@ class NavEKF2 { AP_Int8 _flowUse; // Controls if the optical flow data is fused into the main navigation estimator and/or the terrain estimator. AP_Int16 _mag_ef_limit; // limit on difference between WMM tables and learned earth field. AP_Float _hrt_filt_freq; // frequency of output observer height rate complementary filter in Hz + AP_Int16 _mag_learn_limit; // limit on learned magnetometer offsets // Possible values for _flowUse #define FLOW_USE_NONE 0 #define FLOW_USE_NAV 1 #define FLOW_USE_TERRAIN 2 + // Tuning parameters const float gpsNEVelVarAccScale = 0.05f; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration const float gpsDVelVarAccScale = 0.07f; // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 039b1036c3..71cccb3928 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -266,7 +266,7 @@ void NavEKF2_core::setAidingMode() switch (PV_AidingMode) { case AID_NONE: // We have ceased aiding - gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index); + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index); // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors posTimeout = true; velTimeout = true; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index fed0a152cc..37fe114a5d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -149,7 +149,7 @@ void NavEKF2_core::controlMagYawReset() if (finalResetRequest) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u in-flight yaw alignment complete",(unsigned)imu_index); } else if (interimResetRequest) { - gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index); + gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u ground mag anomaly, yaw re-aligned",(unsigned)imu_index); } // update the yaw reset completed status diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index ccb958b05b..5db4e32f9f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -536,62 +536,6 @@ void NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static) } -// send an EKF_STATUS message to GCS -void NavEKF2_core::send_status_report(mavlink_channel_t chan) -{ - // prepare flags - uint16_t flags = 0; - if (filterStatus.flags.attitude) { - flags |= EKF_ATTITUDE; - } - if (filterStatus.flags.horiz_vel) { - flags |= EKF_VELOCITY_HORIZ; - } - if (filterStatus.flags.vert_vel) { - flags |= EKF_VELOCITY_VERT; - } - if (filterStatus.flags.horiz_pos_rel) { - flags |= EKF_POS_HORIZ_REL; - } - if (filterStatus.flags.horiz_pos_abs) { - flags |= EKF_POS_HORIZ_ABS; - } - if (filterStatus.flags.vert_pos) { - flags |= EKF_POS_VERT_ABS; - } - if (filterStatus.flags.terrain_alt) { - flags |= EKF_POS_VERT_AGL; - } - if (filterStatus.flags.const_pos_mode) { - flags |= EKF_CONST_POS_MODE; - } - if (filterStatus.flags.pred_horiz_pos_rel) { - flags |= EKF_PRED_POS_HORIZ_REL; - } - if (filterStatus.flags.pred_horiz_pos_abs) { - flags |= EKF_PRED_POS_HORIZ_ABS; - } - - // get variances - float velVar, posVar, hgtVar, tasVar; - Vector3f magVar; - Vector2f offset; - getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); - - // Only report range finder normalised innovation levels if the EKF needs the data for primary - // height estimation or optical flow operation. This prevents false alarms at the GCS if a - // range finder is fitted for other applications - float temp; - if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { - temp = sqrtf(auxRngTestRatio); - } else { - temp = 0.0f; - } - - // send message - mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp, tasVar); -} - // report the reason for why the backend is refusing to initialise const char *NavEKF2_core::prearm_failure_reason(void) const { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index 61d947f4f2..ae537f89be 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -672,7 +672,7 @@ void NavEKF2_core::FuseVelPosNED() R_OBS[obsIndex] *= sq(gpsNoiseScaler); } else if (obsIndex == 5) { innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex]; - const float gndMaxBaroErr = 4.0f; + const float gndMaxBaroErr = 8.0f; const float gndBaroInnovFloor = -0.5f; if(getTouchdownExpected() && activeHgtSource == HGT_SOURCE_BARO) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 361ff83362..11b8c051ee 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -1523,6 +1523,17 @@ void NavEKF2_core::MagTableConstrain(void) table_earth_field_ga.z+limit_ga); } +// constrain states using limit on mag offsets +void NavEKF2_core::MagOffsetConstrain(void) +{ + if (frontend->_mag_learn_limit > 0) { + float limit_ga = frontend->_mag_learn_limit * 0.001f; + stateStruct.body_magfield.x = constrain_float(stateStruct.body_magfield.x, -limit_ga, limit_ga); + stateStruct.body_magfield.y = constrain_float(stateStruct.body_magfield.y, -limit_ga, limit_ga); + stateStruct.body_magfield.z = constrain_float(stateStruct.body_magfield.z, -limit_ga, limit_ga); + } +} + // constrain states to prevent ill-conditioning void NavEKF2_core::ConstrainStates() { @@ -1550,6 +1561,8 @@ void NavEKF2_core::ConstrainStates() MagTableConstrain(); } + MagOffsetConstrain(); + // body magnetic field limit for (uint8_t i=19; i<=21; i++) statesArray[i] = constrain_float(statesArray[i],-0.5f,0.5f); // wind velocity limit 100 m/s (could be based on some multiple of max airspeed * EAS2TAS) - TODO apply circular limit diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 1ce57d38dc..be95981871 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -62,6 +62,8 @@ class AP_AHRS; class NavEKF2_core : public NavEKF_core_common { public: + friend class NavEKF2; + // Constructor NavEKF2_core(NavEKF2 *_frontend); @@ -270,9 +272,6 @@ class NavEKF2_core : public NavEKF_core_common */ void getFilterStatus(nav_filter_status &status) const; - // send an EKF_STATUS_REPORT message to GCS - void send_status_report(mavlink_channel_t chan); - // provides the height limit to be observed by the control loops // returns false if no height limiting is required // this is needed to ensure the vehicle does not fly too high when using optical flow navigation @@ -512,6 +511,9 @@ class NavEKF2_core : public NavEKF_core_common // constrain earth field using WMM tables void MagTableConstrain(void); + // constrain mag offsets + void MagOffsetConstrain(void); + // fuse selected position, velocity and height measurements void FuseVelPosNED(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 4c1acc7a4f..40c433b1d1 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1441,11 +1441,73 @@ void NavEKF3::getFilterGpsStatus(int8_t instance, nav_gps_status &status) const } // send an EKF_STATUS_REPORT message to GCS +// for each variance we send the maximum of all lanes void NavEKF3::send_status_report(mavlink_channel_t chan) { - if (core) { - core[primary].send_status_report(chan); + if (!core) { + return; + } + nav_filter_status filterStatus = core[primary].filterStatus; + + // prepare flags + uint16_t flags = 0; + if (filterStatus.flags.attitude) { + flags |= EKF_ATTITUDE; + } + if (filterStatus.flags.horiz_vel) { + flags |= EKF_VELOCITY_HORIZ; + } + if (filterStatus.flags.vert_vel) { + flags |= EKF_VELOCITY_VERT; + } + if (filterStatus.flags.horiz_pos_rel) { + flags |= EKF_POS_HORIZ_REL; + } + if (filterStatus.flags.horiz_pos_abs) { + flags |= EKF_POS_HORIZ_ABS; + } + if (filterStatus.flags.vert_pos) { + flags |= EKF_POS_VERT_ABS; + } + if (filterStatus.flags.terrain_alt) { + flags |= EKF_POS_VERT_AGL; } + if (filterStatus.flags.const_pos_mode) { + flags |= EKF_CONST_POS_MODE; + } + if (filterStatus.flags.pred_horiz_pos_rel) { + flags |= EKF_PRED_POS_HORIZ_REL; + } + if (filterStatus.flags.pred_horiz_pos_abs) { + flags |= EKF_PRED_POS_HORIZ_ABS; + } + if (filterStatus.flags.gps_glitching) { + flags |= (1<<15); + } + + // get variances + float velVar=0, posVar=0, hgtVar=0, tasVar=0, magVarLen=0, rngVar=0; + Vector2f offset; + + for (uint8_t i=0; i 0) || core[i].PV_AidingMode == NavEKF3_core::AID_RELATIVE || core[i].flowDataValid) { + rngVar = MAX(rngVar, sqrtf(core[primary].auxRngTestRatio)); + } + } + + // send message + mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVarLen, rngVar, tasVar); } // provides the height limit to be observed by the control loops diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index ed18c8bba4..006a5798da 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -544,66 +544,6 @@ void NavEKF3_core::getFilterGpsStatus(nav_gps_status &faults) const faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static) } -// send an EKF_STATUS message to GCS -void NavEKF3_core::send_status_report(mavlink_channel_t chan) -{ - // prepare flags - uint16_t flags = 0; - if (filterStatus.flags.attitude) { - flags |= EKF_ATTITUDE; - } - if (filterStatus.flags.horiz_vel) { - flags |= EKF_VELOCITY_HORIZ; - } - if (filterStatus.flags.vert_vel) { - flags |= EKF_VELOCITY_VERT; - } - if (filterStatus.flags.horiz_pos_rel) { - flags |= EKF_POS_HORIZ_REL; - } - if (filterStatus.flags.horiz_pos_abs) { - flags |= EKF_POS_HORIZ_ABS; - } - if (filterStatus.flags.vert_pos) { - flags |= EKF_POS_VERT_ABS; - } - if (filterStatus.flags.terrain_alt) { - flags |= EKF_POS_VERT_AGL; - } - if (filterStatus.flags.const_pos_mode) { - flags |= EKF_CONST_POS_MODE; - } - if (filterStatus.flags.pred_horiz_pos_rel) { - flags |= EKF_PRED_POS_HORIZ_REL; - } - if (filterStatus.flags.pred_horiz_pos_abs) { - flags |= EKF_PRED_POS_HORIZ_ABS; - } - if (filterStatus.flags.gps_glitching) { - flags |= (1<<15); - } - - // get variances - float velVar, posVar, hgtVar, tasVar; - Vector3f magVar; - Vector2f offset; - getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); - - // Only report range finder normalised innovation levels if the EKF needs the data for primary - // height estimation or optical flow operation. This prevents false alarms at the GCS if a - // range finder is fitted for other applications - float temp; - if (((frontend->_useRngSwHgt > 0) && activeHgtSource == HGT_SOURCE_RNG) || (PV_AidingMode == AID_RELATIVE && flowDataValid)) { - temp = sqrtf(auxRngTestRatio); - } else { - temp = 0.0f; - } - - // send message - mavlink_msg_ekf_status_report_send(chan, flags, velVar, posVar, hgtVar, magVar.length(), temp, tasVar); - -} - // report the reason for why the backend is refusing to initialise const char *NavEKF3_core::prearm_failure_reason(void) const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 0a09d8f55d..b7681fd77a 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -67,6 +67,8 @@ class AP_AHRS; class NavEKF3_core : public NavEKF_core_common { public: + friend class NavEKF3; + // Constructor NavEKF3_core(NavEKF3 *_frontend); @@ -326,9 +328,6 @@ class NavEKF3_core : public NavEKF_core_common */ void getFilterStatus(nav_filter_status &status) const; - // send an EKF_STATUS_REPORT message to GCS - void send_status_report(mavlink_channel_t chan); - // provides the height limit to be observed by the control loops // returns false if no height limiting is required // this is needed to ensure the vehicle does not fly too high when using optical flow navigation diff --git a/libraries/AP_Notify/ToneAlarm.cpp b/libraries/AP_Notify/ToneAlarm.cpp index ef8b998388..bca2703f08 100644 --- a/libraries/AP_Notify/ToneAlarm.cpp +++ b/libraries/AP_Notify/ToneAlarm.cpp @@ -63,7 +63,7 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] { #define AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS 13 { "MBNT255>A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8", true }, #define AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS 14 - { "MBNT255p4f16e16f8e8d.p4f16e16fp4d16c16d8c8d8cp8e16d16ep4f16e16f8e8d.p4f16e16fp4d16c16d8c8d8cp8c16dp8c16d16e8d8c8fe2p4f16e16d8e2.pp", true }, #define AP_NOTIFY_TONE_WAITING_FOR_THROW 15 { "MBNT90L4O2A#O3DFN0N0N0", true}, #define AP_NOTIFY_TONE_LOUD_1 16 @@ -93,7 +93,7 @@ const AP_ToneAlarm::Tone AP_ToneAlarm::_tones[] { #define AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28 { "MFT200L4c", false }, #define AP_NOTIFY_TONE_NO_SDCARD 30 { "MNBGG", false }, }; @@ -134,6 +134,11 @@ bool AP_ToneAlarm::init() // play_tune - play one of the pre-defined tunes void AP_ToneAlarm::play_tone(const uint8_t tone_index) { + // Just don't play tones while armed + if (hal.util->get_soft_armed() && tone_index != AP_NOTIFY_TONE_QUIET_ARMING_WARNING) { + return; + } + uint32_t tnow_ms = AP_HAL::millis(); const Tone &tone_requested = _tones[tone_index]; @@ -316,6 +321,8 @@ void AP_ToneAlarm::update() } } +#if 0 + // disabled for matternet // notify the user when pre_arm checks are passing if (flags.pre_arm_check != AP_Notify::flags.pre_arm_check) { flags.pre_arm_check = AP_Notify::flags.pre_arm_check; @@ -329,6 +336,7 @@ void AP_ToneAlarm::update() } } } +#endif // check if arming status has changed if (flags.armed != AP_Notify::flags.armed) { diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index 684ee77e5d..b742a1799e 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -8,10 +8,11 @@ #include #include +#include "protocol.h" + extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_Parachute::var_info[] = { - // @Param: ENABLED // @DisplayName: Parachute release enabled or disabled // @Description: Parachute release enabled or disabled @@ -159,6 +160,120 @@ void AP_Parachute::update() // update AP_Notify AP_Notify::flags.parachute_release = 0; } + + + if (_release_type == AP_PARACHUTE_TRIGGER_TYPE_MATTERNET_FTS) { + mttr_fts_update(); + } +} + +void AP_Parachute::mttr_fts_transmit(uint8_t msg_len, uint8_t* msg_buf) +{ + if (!_mttr_uart) { + return; + } + + uint8_t encoded_msg[32]; + uint8_t encoded_msg_len = fts_protocol_encode_message(msg_len, msg_buf, 32, encoded_msg); + if (_mttr_uart->txspace() >= encoded_msg_len) { + _mttr_uart->write(encoded_msg, encoded_msg_len); + } +} + +void AP_Parachute::send_debug_message(uint32_t tnow_ms, uint8_t ind, float value) +{ + mavlink_message_t mav_msg; + mavlink_debug_t mav_dbg_msg; + mav_dbg_msg.time_boot_ms = tnow_ms; + mav_dbg_msg.ind = ind; + mav_dbg_msg.value = value; + mavlink_msg_debug_encode(mavlink_system.sysid, mavlink_system.compid, &mav_msg, &mav_dbg_msg); + GCS_MAVLINK::send_on_all_channels(&mav_msg); +} + +void AP_Parachute::mttr_fts_update() +{ + if (!_mttr_uart) { + return; + } + + uint32_t tnow_ms = AP_HAL::millis(); + + // 20Hz + if (tnow_ms - _mttr_last_loop_ms > 50) { + if (_release_in_progress || _released) { + struct fts_msg_deploy_s deploy_msg; + deploy_msg.msgid = FTS_MSGID_DEPLOY; + deploy_msg.magic = FTS_DEPLOY_MAGIC; + mttr_fts_transmit(sizeof(deploy_msg), (uint8_t*)&deploy_msg); + } + + if (tnow_ms - _mttr_last_status_recv_ms > 1000) { + _mttr_status_pass = false; + } + + struct fts_msg_wdrst_s wdrst_msg; + wdrst_msg.msgid = FTS_MSGID_WDRST; + mttr_fts_transmit(sizeof(wdrst_msg), (uint8_t*)&wdrst_msg); + + if (!_release_initiated) { + if (hal.util->get_soft_armed()) { + struct fts_msg_arm_cmd_s arm_msg; + arm_msg.msgid = FTS_MSGID_ARM_CMD; + mttr_fts_transmit(sizeof(arm_msg), (uint8_t*)&arm_msg); + } else { + struct fts_msg_disarm_cmd_s disarm_msg; + disarm_msg.msgid = FTS_MSGID_DISARM_CMD; + mttr_fts_transmit(sizeof(disarm_msg), (uint8_t*)&disarm_msg); + } + } + + _mttr_last_loop_ms = tnow_ms; + } + + uint8_t msg_buf[32]; + int16_t byte; + + while ((byte = _mttr_uart->read()) != -1) { + uint8_t msg_len = fts_protocol_rx_byte(byte, 32, msg_buf); + if (msg_len > 0) { + enum fts_msg_id_t msg_id = fts_protocol_identify_message(msg_len, msg_buf); + if (msg_id == FTS_MSGID_STATUS) { + struct fts_msg_status_s* msg = (struct fts_msg_status_s*)msg_buf; + AP::logger().Write("FTSS", "TimeUS,State,Rsn,BSoC,BC1mV,BC2mV,BC3mV,BTINT,BTTS1", "QBBBHHHcc", AP_HAL::micros64(), msg->state, msg->state_reason, msg->batt_SoC, msg->batt_cell_mV[0], msg->batt_cell_mV[1], msg->batt_cell_mV[2], msg->batt_temp_INT, msg->batt_temp_TS1); + + // update pre-arm state + _mttr_last_status_recv_ms = tnow_ms; + _mttr_status_pass = (msg->state == FTS_DISARMED_MASTER_PRESENT || msg->state == FTS_ARMED); + + // send via MAVLink + send_debug_message(tnow_ms, 0, msg->state); + send_debug_message(tnow_ms, 1, msg->state_reason); + send_debug_message(tnow_ms, 2, msg->batt_SoC); + send_debug_message(tnow_ms, 3, msg->batt_cell_mV[0]*1e-3f); + send_debug_message(tnow_ms, 4, msg->batt_cell_mV[1]*1e-3f); + send_debug_message(tnow_ms, 5, msg->batt_cell_mV[2]*1e-3f); + send_debug_message(tnow_ms, 6, msg->batt_temp_INT*1e-2f); + send_debug_message(tnow_ms, 7, msg->batt_temp_TS1*1e-2f); + + } else if (msg_id == FTS_MSGID_VERSION) { + struct fts_msg_version_s* msg = (struct fts_msg_version_s*)msg_buf; + memset(_mttr_fts_version, 0, sizeof(_mttr_fts_version)); + memcpy(_mttr_fts_version, msg->git_hash, sizeof(msg->git_hash)); + AP::logger().Write("FTSV", "TimeUS,Hash", "QN", AP_HAL::micros64(), _mttr_fts_version); + } else if (msg_id == FTS_MSGID_STATUS2) { + struct fts_msg_status2_s* msg = (struct fts_msg_status2_s*)msg_buf; + + _mttr_fuse_pass = (msg->fuse_fault_state == FTS_FUSE_STATE_PASS); + + AP::logger().Write("FTS2", "TimeUS,Fuse,WDTm", "QBh", AP_HAL::micros64(), msg->fuse_fault_state, msg->wdt_min_margin_ms); + + // send via MAVLink + send_debug_message(tnow_ms, 8, msg->fuse_fault_state); + send_debug_message(tnow_ms, 9, msg->wdt_min_margin_ms); + } + } + } } // singleton instance diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 4280fb6df8..e80c7729ec 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -5,12 +5,14 @@ #include #include #include +#include #define AP_PARACHUTE_TRIGGER_TYPE_RELAY_0 0 #define AP_PARACHUTE_TRIGGER_TYPE_RELAY_1 1 #define AP_PARACHUTE_TRIGGER_TYPE_RELAY_2 2 #define AP_PARACHUTE_TRIGGER_TYPE_RELAY_3 3 #define AP_PARACHUTE_TRIGGER_TYPE_SERVO 10 +#define AP_PARACHUTE_TRIGGER_TYPE_MATTERNET_FTS 11 #define AP_PARACHUTE_RELEASE_DELAY_MS 500 // delay in milliseconds between call to release() and when servo or relay actually moves. Allows for warning to user #define AP_PARACHUTE_RELEASE_DURATION_MS 2000 // when parachute is released, servo or relay stay at their released position/value for 2000ms (2seconds) @@ -30,6 +32,7 @@ class AP_Parachute { /// Constructor AP_Parachute(AP_Relay &relay) : _relay(relay) + , _mttr_fuse_pass(true) { // setup parameter defaults #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -39,12 +42,18 @@ class AP_Parachute { #endif _singleton = this; AP_Param::setup_object_defaults(this, var_info); + + strcpy(_mttr_fts_version, "UNKNOWN"); } /* Do not allow copies */ AP_Parachute(const AP_Parachute &other) = delete; AP_Parachute &operator=(const AP_Parachute&) = delete; + void init(AP_SerialManager& serial_manager) { + _mttr_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Matternet_FTS, 0); + } + /// enabled - enable or disable parachute release void enabled(bool on_off); @@ -79,6 +88,9 @@ class AP_Parachute { // set_sink_rate - set vehicle sink rate void set_sink_rate(float sink_rate) { _sink_rate = sink_rate; } + const char* mttr_get_fts_version() { return _mttr_fts_version; } + bool get_mttr_prearm_pass() { return _mttr_status_pass && _mttr_fuse_pass; } + static const struct AP_Param::GroupInfo var_info[]; // get singleton instance @@ -104,6 +116,17 @@ class AP_Parachute { bool _is_flying:1; // true if the vehicle is flying float _sink_rate; // vehicle sink rate in m/s uint32_t _sink_time; // time that the vehicle exceeded critical sink rate + + // Matternet FTS + uint32_t _mttr_last_loop_ms; + uint32_t _mttr_last_status_recv_ms; + bool _mttr_status_pass; + bool _mttr_fuse_pass; + AP_HAL::UARTDriver *_mttr_uart = nullptr; + char _mttr_fts_version[16]; + void send_debug_message(uint32_t tnow_ms, uint8_t ind, float value); + void mttr_fts_transmit(uint8_t msg_len, uint8_t* msg_buf); + void mttr_fts_update(); }; namespace AP { diff --git a/libraries/AP_Parachute/messages.h b/libraries/AP_Parachute/messages.h new file mode 100644 index 0000000000..dcf5f3e38a --- /dev/null +++ b/libraries/AP_Parachute/messages.h @@ -0,0 +1,88 @@ +#ifndef __MESSAGES_H +#define __MESSAGES_H + +#include + +#ifndef FTS_PACKED +#define FTS_PACKED __attribute__((packed)) +#endif + +#define FTS_DEPLOY_MAGIC 0x5555 + +enum fts_msg_id_t { + FTS_MSGID_UNKNOWN=0, + FTS_MSGID_STATUS=1, + FTS_MSGID_DEPLOY=2, + FTS_MSGID_WDRST=3, + FTS_MSGID_POWER_OFF=4, + FTS_MSGID_ARM_CMD=5, + FTS_MSGID_DISARM_CMD=6, + FTS_MSGID_VERSION=7, + FTS_MSGID_STATUS2=8 +}; + +enum fts_state_t { + FTS_INIT=0, + FTS_POWER_OFF=1, + FTS_DISARMED_MASTER_NOT_PRESENT=2, + FTS_DISARMED_MASTER_PRESENT=3, + FTS_ARMED=4, + FTS_DEPLOYED=5 +}; + +enum fts_state_reason_t { + FTS_REASON_INIT=0, + FTS_REASON_COMMANDED=1, + FTS_REASON_WATCHDOG_EXPIRE=2, + FTS_REASON_BUTTON_PRESS=3 +}; + +enum fuse_fault_state_t { + FTS_FUSE_STATE_UNKNOWN=0, + FTS_FUSE_STATE_PASS=1, + FTS_FUSE_STATE_FAIL=2 +}; + +struct fts_msg_status_s { + uint8_t msgid; + uint8_t state; + uint8_t state_reason; + uint16_t batt_cell_mV[3]; + uint8_t batt_SoC; + int16_t batt_temp_TS1; + int16_t batt_temp_INT; +} FTS_PACKED; + +struct fts_msg_deploy_s { + uint8_t msgid; + uint16_t magic; +} FTS_PACKED; + +struct fts_msg_wdrst_s { + uint8_t msgid; +} FTS_PACKED; + +struct fts_msg_power_off_s { + uint8_t msgid; +} FTS_PACKED; + +struct fts_msg_arm_cmd_s { + uint8_t msgid; +} FTS_PACKED; + +struct fts_msg_disarm_cmd_s { + uint8_t msgid; +} FTS_PACKED; + +struct fts_msg_version_s { + uint8_t msgid; + uint8_t git_hash[7]; +} FTS_PACKED; + +struct fts_msg_status2_s { + uint8_t msgid; + int16_t wdt_min_margin_ms; + uint8_t fuse_fault_state; +} FTS_PACKED; + +#endif diff --git a/libraries/AP_Parachute/protocol.cpp b/libraries/AP_Parachute/protocol.cpp new file mode 100644 index 0000000000..8a1a2cdd93 --- /dev/null +++ b/libraries/AP_Parachute/protocol.cpp @@ -0,0 +1,176 @@ +#include "protocol.h" +#include "slip.h" + +#include + +#define FTS_PROTOCOL_RXBUF_SIZE 32 + +static uint8_t rxbuf[FTS_PROTOCOL_RXBUF_SIZE]; +static uint8_t rxbuf_len = 0; +static uint8_t wait_for_end_byte = 0; + +static uint16_t crc16_ccitt(uint8_t byte, uint16_t crc); + +enum fts_msg_id_t fts_protocol_identify_message(uint8_t msg_len, uint8_t* msg) { + switch (msg[0]) { + case FTS_MSGID_STATUS: + if (msg_len != sizeof(struct fts_msg_status_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + case FTS_MSGID_DEPLOY: + if (msg_len != sizeof(struct fts_msg_deploy_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + case FTS_MSGID_WDRST: + if (msg_len != sizeof(struct fts_msg_wdrst_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + case FTS_MSGID_POWER_OFF: + if (msg_len != sizeof(struct fts_msg_power_off_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + case FTS_MSGID_ARM_CMD: + if (msg_len != sizeof(struct fts_msg_arm_cmd_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + case FTS_MSGID_DISARM_CMD: + if (msg_len != sizeof(struct fts_msg_disarm_cmd_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + case FTS_MSGID_VERSION: + if (msg_len != sizeof(struct fts_msg_version_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + case FTS_MSGID_STATUS2: + if (msg_len != sizeof(struct fts_msg_status2_s)) { + return FTS_MSGID_UNKNOWN; + } + break; + default: + return FTS_MSGID_UNKNOWN; + } + return (enum fts_msg_id_t)msg[0]; +} + + + +// accepts a byte, appends it to rxbuf +// if a full frame has been received, writes the payload to ret_buf. +// if data was written, returns pointer to outbuf in ret_buf +// returns length of valid data written to outbuf +uint8_t fts_protocol_rx_byte(uint8_t byte, uint8_t ret_buf_size, uint8_t* ret_buf) { + uint8_t i; + uint8_t decoded_buf[FTS_PROTOCOL_RXBUF_SIZE-1]; + uint8_t decoded_len; + uint16_t crc16_provided; + uint16_t crc16_computed; + + if (rxbuf_len >= FTS_PROTOCOL_RXBUF_SIZE) { + // no room in rxbuf, clear it and flag the rxbuf as misaligned unless byte received was an end byte + rxbuf_len = 0; + if (byte != SLIP_END) { + wait_for_end_byte = 1; + + } + return 0; + } + + if (wait_for_end_byte) { + wait_for_end_byte = (byte != SLIP_END); + return 0; + } + + rxbuf[rxbuf_len++] = byte; + + if (byte == SLIP_END) { + decoded_len = slip_decode(rxbuf_len, rxbuf, decoded_buf); + rxbuf_len = 0; + + if (decoded_len-3 > ret_buf_size) { + return 0; + } + + if (decoded_len <= 3) { + return 0; + } + + if (decoded_buf[0] != decoded_len-3) { + return 0; + } + + // little-endian + crc16_provided = 0; + crc16_provided |= decoded_buf[decoded_len-1]; + crc16_provided <<= 8; + crc16_provided |= decoded_buf[decoded_len-2]; + + crc16_computed = 0; + for (i=0; i < decoded_len-2; i++) { + crc16_computed = crc16_ccitt(decoded_buf[i], crc16_computed); + } + + if (crc16_provided != crc16_computed) { + return 0; + } + + for (i=0; i < decoded_len-3; i++) { + ret_buf[i] = decoded_buf[i+1]; + } + + return decoded_len-3; + } + + return 0; +} + +// returns 0 on failure, 1 on success +uint8_t fts_protocol_encode_message(uint8_t in_buf_size, uint8_t* in_buf, uint8_t out_buf_size, uint8_t* out_buf) { + uint8_t out_len; + uint8_t i; + uint16_t crc16_val; + out_len = 0; + crc16_val = 0; + + if (!slip_encode_and_append(in_buf_size, &out_len, out_buf, out_buf_size)) { return 0; } + crc16_val = crc16_ccitt(in_buf_size, crc16_val); + + for (i=0; i> 8) & 0xff, &out_len, out_buf, out_buf_size)) { return 0; } + + if (out_len >= out_buf_size) { return 0; } + out_buf[out_len++] = SLIP_END; + + return out_len; +} + +static uint16_t crc16_ccitt(uint8_t byte, uint16_t crc16) +{ + uint8_t i; + crc16 ^= (byte << (16 - 8)); + for (i = 8; i > 0; i--) + { + if (crc16 & 0x8000) + { + crc16 = (crc16 << 1) ^ 0x1021; + } + else + { + crc16 = (crc16 << 1); + } + } + + return crc16; +} diff --git a/libraries/AP_Parachute/protocol.h b/libraries/AP_Parachute/protocol.h new file mode 100644 index 0000000000..011d92fad1 --- /dev/null +++ b/libraries/AP_Parachute/protocol.h @@ -0,0 +1,11 @@ +#ifndef __PROTOCOL_H +#define __PROTOCOL_H + +#include +#include "messages.h" + +uint8_t fts_protocol_rx_byte(uint8_t byte, uint8_t ret_buf_size, uint8_t* ret_buf); +enum fts_msg_id_t fts_protocol_identify_message(uint8_t msg_len, uint8_t* msg); +uint8_t fts_protocol_encode_message(uint8_t in_buf_size, uint8_t* in_buf, uint8_t out_buf_size, uint8_t* out_buf); + +#endif diff --git a/libraries/AP_Parachute/slip.h b/libraries/AP_Parachute/slip.h new file mode 100644 index 0000000000..67190597ea --- /dev/null +++ b/libraries/AP_Parachute/slip.h @@ -0,0 +1,67 @@ +#ifndef __SLIP_H +#define __SLIP_H + +#define SLIP_END 0xC0 +#define SLIP_ESC 0xDB +#define SLIP_ESC_END 0xDC +#define SLIP_ESC_ESC 0xDD + +// appends a SLIP-encoded byte to out_buf and increments *out_buf_len +// returns 0 on failure, 1 on success +static uint8_t slip_encode_and_append(uint8_t byte, uint8_t* out_buf_len, uint8_t *out_buf, uint8_t out_buf_size) { + if (byte == SLIP_END) { + if ((*out_buf_len) > out_buf_size-2) { + return 0; + } + + out_buf[(*out_buf_len)++] = SLIP_ESC; + out_buf[(*out_buf_len)++] = SLIP_ESC_END; + } else if (byte == SLIP_ESC) { + if ((*out_buf_len) > out_buf_size-2) { + return 0; + } + + out_buf[(*out_buf_len)++] = SLIP_ESC; + out_buf[(*out_buf_len)++] = SLIP_ESC_ESC; + } else { + if ((*out_buf_len) > out_buf_size-1) { + return 0; + } + + out_buf[(*out_buf_len)++] = byte; + } + return 1; +} + +// decodes the first SLIP frame in in_buf, writes it to out_buf +// assumes sizeof(out_buf) >= (in_len - 1) +// returns out_buf length or 0 on failure to find a valid frame +static uint8_t slip_decode(uint8_t in_len, uint8_t *in_buf, uint8_t *out_buf) { + uint8_t i; + uint8_t out_len = 0; + uint8_t esc_flag = 0; + + for (i=0; i_init()) { + bool success = false; + for (uint8_t i=0; i<10; i++) { + if (sensor->_init()) { + success = true; + break; + } + hal.scheduler->delay(10); + } + + if (!success) { delete sensor; return nullptr; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp index e05adf9359..6cca731087 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_UAVCAN.cpp @@ -49,11 +49,12 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u return nullptr; } AP_RangeFinder_UAVCAN* 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 (AP::rangefinder()->params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN && - AP::rangefinder()->params[i].address == address) { - driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i]; + if (frontend.params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN && + frontend.params[i].address == address) { + driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i]; } //Double check if the driver was initialised as UAVCAN Type if (driver != nullptr && (driver->_backend_type == RangeFinder::RangeFinder_TYPE_UAVCAN)) { @@ -70,19 +71,21 @@ AP_RangeFinder_UAVCAN* AP_RangeFinder_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_u if (create_new) { for (uint8_t i = 0; i < RANGEFINDER_MAX_INSTANCES; i++) { - if (AP::rangefinder()->params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN && - AP::rangefinder()->params[i].address == address) { - if (AP::rangefinder()->drivers[i] != nullptr) { + if (frontend.params[i].type == RangeFinder::RangeFinder_TYPE_UAVCAN && + frontend.params[i].address == address) { + WITH_SEMAPHORE(frontend.detect_sem); + if (frontend.drivers[i] != nullptr) { //we probably initialised this driver as something else, reboot is required for setting //it up as UAVCAN type return nullptr; } - AP::rangefinder()->drivers[i] = new AP_RangeFinder_UAVCAN(AP::rangefinder()->state[i], AP::rangefinder()->params[i]); - driver = (AP_RangeFinder_UAVCAN*)AP::rangefinder()->drivers[i]; + frontend.drivers[i] = new AP_RangeFinder_UAVCAN(frontend.state[i], frontend.params[i]); + driver = (AP_RangeFinder_UAVCAN*)frontend.drivers[i]; if (driver == nullptr) { break; } - AP::rangefinder()->num_instances = MAX(i+1, AP::rangefinder()->num_instances); + gcs().send_text(MAV_SEVERITY_INFO, "RangeFinder[%u]: added UAVCAN 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; diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 99689b36d8..72860a8512 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -253,10 +253,11 @@ void RangeFinder::convert_params(void) { */ void RangeFinder::init(enum Rotation orientation_default) { - if (num_instances != 0) { + if (init_done) { // init called a 2nd time? return; } + init_done = true; convert_params(); @@ -268,11 +269,14 @@ void RangeFinder::init(enum Rotation orientation_default) for (uint8_t i=0, serial_instance = 0; isnprintf(failure_msg, failure_msg_len, "Rangefinder %d was not detected", i + 1); - return false; + if (params[i].type == RangeFinder_TYPE_NONE) { + continue; + } + if (drivers[i] == nullptr) { + hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %u was not detected", unsigned(i + 1)); + return false; + } + if (drivers[i]->status() == RangeFinder_NotConnected || + drivers[i]->status() == RangeFinder_NoData) { + hal.util->snprintf(failure_msg, failure_msg_len, "Rangefinder %u unhealthy", unsigned(i + 1)); + return false; } } diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index e117e27bb2..404a7ce164 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -173,6 +173,8 @@ class RangeFinder RangeFinder_State state[RANGEFINDER_MAX_INSTANCES]; AP_RangeFinder_Backend *drivers[RANGEFINDER_MAX_INSTANCES]; uint8_t num_instances; + bool init_done; + HAL_Semaphore detect_sem; float estimated_terrain_height; Vector3f pos_offset_zero; // allows returning position offsets of zero for invalid requests diff --git a/libraries/AP_SerialManager/AP_SerialManager.cpp b/libraries/AP_SerialManager/AP_SerialManager.cpp index 93e2f681a1..f3e067be2c 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.cpp +++ b/libraries/AP_SerialManager/AP_SerialManager.cpp @@ -454,6 +454,13 @@ void AP_SerialManager::init() state[i].uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); break; + case SerialProtocol_Matternet_FTS: + state[i].baud = AP_SERIALMANAGER_MATTERNET_FTS_BAUD / 1000; // update baud param in case user looks at it + state[i].uart->begin(map_baudrate(state[i].baud), + AP_SERIALMANAGER_MATTERNET_FTS_BUFSIZE_RX, + AP_SERIALMANAGER_MATTERNET_FTS_BUFSIZE_TX); + break; + case SerialProtocol_Robotis: state[i].uart->begin(map_baudrate(state[i].baud), AP_SERIALMANAGER_ROBOTIS_BUFSIZE_RX, diff --git a/libraries/AP_SerialManager/AP_SerialManager.h b/libraries/AP_SerialManager/AP_SerialManager.h index f0517bf167..c8ff0555da 100644 --- a/libraries/AP_SerialManager/AP_SerialManager.h +++ b/libraries/AP_SerialManager/AP_SerialManager.h @@ -89,6 +89,10 @@ #define AP_SERIALMANAGER_SLCAN_BUFSIZE_RX 128 #define AP_SERIALMANAGER_SLCAN_BUFSIZE_TX 128 +#define AP_SERIALMANAGER_MATTERNET_FTS_BAUD 115200 +#define AP_SERIALMANAGER_MATTERNET_FTS_BUFSIZE_RX 64 +#define AP_SERIALMANAGER_MATTERNET_FTS_BUFSIZE_TX 64 + class AP_SerialManager { public: AP_SerialManager(); @@ -123,6 +127,7 @@ class AP_SerialManager { SerialProtocol_WindVane = 21, SerialProtocol_SLCAN = 22, SerialProtocol_RCIN = 23, + SerialProtocol_Matternet_FTS = 100, }; // get singleton instance diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index 50c67749fd..26515849ae 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -47,6 +47,13 @@ const AP_Param::GroupInfo AP_Terrain::var_info[] = { // @User: Advanced AP_GROUPINFO("SPACING", 1, AP_Terrain, grid_spacing, 100), + // @Param: OPTIONS + // @DisplayName: Terrain options + // @Description: Options to change behaviour of terrain system + // @Bitmask: 0:Disable Download + // @User: Advanced + AP_GROUPINFO("OPTIONS", 2, AP_Terrain, options, 0), + AP_GROUPEND }; @@ -296,6 +303,7 @@ float AP_Terrain::lookahead(float bearing, float distance, float climb_ratio) */ void AP_Terrain::update(void) { + if (!enable) { return; } // just schedule any needed disk IO schedule_disk_io(); diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index 10ca5aadcd..9144f307ef 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -57,7 +57,13 @@ // format of grid on disk #define TERRAIN_GRID_FORMAT_VERSION 1 +// we allow for a 2cm discrepancy in the grid corners. This is to +// account for different rounding in terrain DAT file generators using +// different programming languages +#define TERRAIN_LATLON_EQUAL(v1, v2) (labs((v1)-(v2)) <= 2) + #if TERRAIN_DEBUG +#include #define ASSERT_RANGE(v,minv,maxv) assert((v)<=(maxv)&&(v)>=(minv)) #else #define ASSERT_RANGE(v,minv,maxv) @@ -325,6 +331,7 @@ class AP_Terrain { void io_timer(void); void open_file(void); void seek_offset(void); + uint32_t east_blocks(struct grid_block &block) const; void write_block(void); void read_block(void); @@ -342,6 +349,11 @@ class AP_Terrain { // parameters AP_Int8 enable; AP_Int16 grid_spacing; // meters between grid points + AP_Int16 options; // option bits + + enum class Options { + DisableDownload = (1U<<0), + }; // reference to AP_Mission, so we can ask preload terrain data for // all waypoints diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp index e57a68cec6..dc719f9223 100644 --- a/libraries/AP_Terrain/TerrainGCS.cpp +++ b/libraries/AP_Terrain/TerrainGCS.cpp @@ -39,6 +39,10 @@ bool AP_Terrain::request_missing(mavlink_channel_t chan, struct grid_cache &gcac { struct grid_block &grid = gcache.grid; + if (options.get() & uint16_t(Options::DisableDownload)) { + return false; + } + if (grid.spacing != grid_spacing) { // an invalid grid return false; @@ -263,8 +267,8 @@ void AP_Terrain::handle_terrain_data(const mavlink_message_t &msg) uint16_t i; for (i=0; iprintf("Seek %lu failed - %s\n", @@ -283,17 +289,23 @@ void AP_Terrain::read_block(void) ssize_t ret = AP::FS().read(fd, &disk_block, sizeof(disk_block)); if (ret != sizeof(disk_block) || - disk_block.block.lat != lat || - disk_block.block.lon != lon || + !TERRAIN_LATLON_EQUAL(disk_block.block.lat,lat) || + !TERRAIN_LATLON_EQUAL(disk_block.block.lon,lon) || disk_block.block.bitmap == 0 || disk_block.block.spacing != grid_spacing || disk_block.block.version != TERRAIN_GRID_FORMAT_VERSION || disk_block.block.crc != get_block_crc(disk_block.block)) { #if TERRAIN_DEBUG - printf("read empty block at %ld %ld ret=%d\n", + printf("read empty block at %ld %ld ret=%d (%ld %ld %u 0x%08lx) 0x%04x:0x%04x\n", (long)lat, (long)lon, - (int)ret); + (int)ret, + (long)disk_block.block.lat, + (long)disk_block.block.lon, + (unsigned)disk_block.block.spacing, + (unsigned long)disk_block.block.bitmap, + (unsigned)disk_block.block.crc, + (unsigned)get_block_crc(disk_block.block)); #endif // a short read or bad data is not an IO failure, just a // missing block on disk diff --git a/libraries/AP_Terrain/TerrainUtil.cpp b/libraries/AP_Terrain/TerrainUtil.cpp index dbb6278de5..34adc8e394 100644 --- a/libraries/AP_Terrain/TerrainUtil.cpp +++ b/libraries/AP_Terrain/TerrainUtil.cpp @@ -113,8 +113,8 @@ AP_Terrain::grid_cache &AP_Terrain::find_grid_cache(const struct grid_info &info // see if we have that grid for (uint16_t i=0; i2 or + abs(lon - block.lon)>2 or + spacing != GRID_SPACING or + bitmap != (1<<56)-1): + return False + buf = buf[:16] + struct.pack(" #include #include -#include +#include #include #include @@ -53,6 +53,7 @@ #include #include "AP_UAVCAN_Server.h" #include +#include #define LED_DELAY_US 50000 @@ -118,8 +119,8 @@ UC_REGISTRY_BINDER(ButtonCb, ardupilot::indication::Button); static uavcan::Subscriber *safety_button_listener[MAX_NUMBER_OF_CAN_DRIVERS]; // handler TrafficReport -UC_REGISTRY_BINDER(TrafficReportCb, ardupilot::equipment::trafficmonitor::TrafficReport); -static uavcan::Subscriber *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS]; +UC_REGISTRY_BINDER(TrafficReportCb, com::matternet::equipment::trafficmonitor::TrafficReport); +static uavcan::Subscriber *traffic_report_listener[MAX_NUMBER_OF_CAN_DRIVERS]; // handler actuator status UC_REGISTRY_BINDER(ActuatorStatusCb, uavcan::equipment::actuator::Status); @@ -248,6 +249,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) AP_Airspeed_UAVCAN::subscribe_msgs(this); AP_OpticalFlow_HereFlow::subscribe_msgs(this); AP_RangeFinder_UAVCAN::subscribe_msgs(this); + AP_InertialSensor_UAVCAN::subscribe_msgs(this); act_out_array[driver_index] = new uavcan::Publisher(*_node); act_out_array[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2)); @@ -278,7 +280,7 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button)); } - traffic_report_listener[driver_index] = new uavcan::Subscriber(*_node); + 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)); } @@ -676,7 +678,7 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con return; } - const ardupilot::equipment::trafficmonitor::TrafficReport &msg = cb.msg[0]; + const com::matternet::equipment::trafficmonitor::TrafficReport &msg = cb.msg[0]; AP_ADSB::adsb_vehicle_t vehicle; mavlink_adsb_vehicle_t &pkt = vehicle.info; @@ -694,10 +696,10 @@ void AP_UAVCAN::handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, con } pkt.emitter_type = msg.traffic_type; - if (msg.alt_type == ardupilot::equipment::trafficmonitor::TrafficReport::ALT_TYPE_PRESSURE_AMSL) { + if (msg.alt_type == com::matternet::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) { + } else if (msg.alt_type == com::matternet::equipment::trafficmonitor::TrafficReport::ALT_TYPE_WGS84) { pkt.flags |= ADSB_FLAGS_VALID_ALTITUDE; pkt.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; } diff --git a/libraries/AP_UAVCAN/dsdl/ardupilot/equipment/gnss/20003.Inject.uavcan b/libraries/AP_UAVCAN/dsdl/ardupilot/equipment/gnss/20003.Inject.uavcan new file mode 100644 index 0000000000..10b5dcc07e --- /dev/null +++ b/libraries/AP_UAVCAN/dsdl/ardupilot/equipment/gnss/20003.Inject.uavcan @@ -0,0 +1,8 @@ +# +# GNSS Inject, used by autopilots to forward GPS_RTCM_DATA or GPS_INJECT mavlink msgs +# to GNSS nodes. Normally sent as broadcast + +# flags follows the same conventions as MAVLink GPS_RTCM_DATA +uint8 flags + +uint8[<=180] data diff --git a/libraries/AP_UAVCAN/dsdl/ardupilot/imu/20002.FastIMU.uavcan b/libraries/AP_UAVCAN/dsdl/ardupilot/imu/20002.FastIMU.uavcan new file mode 100644 index 0000000000..fc9151a8cd --- /dev/null +++ b/libraries/AP_UAVCAN/dsdl/ardupilot/imu/20002.FastIMU.uavcan @@ -0,0 +1,21 @@ +# +# IMU data with scale factors to keep high accuracy with low bus bandwidth cost + +# which sensor from this node +uint8 sensor_id + +# counter to detect packet loss +uint8 counter + +# the amount of time that this sample was collected over, in microseconds +uint16 time_us + +# scale factors. These vary for each packet to keep the data within the range of an int16 on all axes +uint8 accel_scale +uint8 gyro_scale + +# accelerometer data. Value in m/s/s is (10 * accel[i] * (1+accel_scale) / INT16_MAX) +int16[3] accel + +# accelerometer data. Value in rad/sec is (gyro[i] * (1+gyro_scale) / INT16_MAX) +int16[3] gyro diff --git a/libraries/AP_UAVCAN/dsdl/ardupilot/equipment/trafficmonitor/20790.TrafficReport.uavcan b/libraries/AP_UAVCAN/dsdl/com/matternet/equipment/trafficmonitor/20790.TrafficReport.uavcan similarity index 100% rename from libraries/AP_UAVCAN/dsdl/ardupilot/equipment/trafficmonitor/20790.TrafficReport.uavcan rename to libraries/AP_UAVCAN/dsdl/com/matternet/equipment/trafficmonitor/20790.TrafficReport.uavcan diff --git a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp index 160c6d4bbc..c197ef81d9 100644 --- a/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp +++ b/libraries/AP_UAVCAN/examples/UAVCAN_sniffer/UAVCAN_sniffer.cpp @@ -37,6 +37,8 @@ #include +#include + void setup(); void loop(); @@ -153,6 +155,15 @@ MSG_CB(uavcan::equipment::esc::RawCommand, RawCommand) MSG_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); MSG_CB(com::hex::equipment::flow::Measurement, Measurement); +static void cb_FastIMU(const uavcan::ReceivedDataStructure& msg) { + static uint8_t last_counter; + if (msg.counter != (last_counter + 1) % 256) { + ::printf("crt %u %u\n", last_counter, msg.counter); + } + last_counter = msg.counter; + count_msg(msg.getDataTypeFullName()); +} + void UAVCAN_sniffer::init(void) { uint8_t interface = 0; @@ -226,7 +237,7 @@ void UAVCAN_sniffer::init(void) START_CB(uavcan::equipment::esc::RawCommand, RawCommand); START_CB(uavcan::equipment::indication::LightsCommand, LightsCommand); START_CB(com::hex::equipment::flow::Measurement, Measurement); - + START_CB(ardupilot::imu::FastIMU, FastIMU); /* * Informing other nodes that we're ready to work. diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 265768bc91..aa9075c9ad 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -229,6 +229,9 @@ class GCS_MAVLINK // don't get broadcast packets or forwarded packets static void set_channel_private(mavlink_channel_t chan); + // send a message to all active MAVLink connections + static void send_on_all_channels(const mavlink_message_t* msg); + // return true if channel is private static bool is_private(mavlink_channel_t _chan) { return (mavlink_private & (1U<<(unsigned)_chan)) != 0; @@ -365,6 +368,8 @@ class GCS_MAVLINK void handle_send_autopilot_version(const mavlink_message_t &msg); MAV_RESULT handle_command_request_autopilot_capabilities(const mavlink_command_long_t &packet); + void send_matternet_FTS_version(void) const; + virtual void send_banner(); void handle_device_op_read(const mavlink_message_t &msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index d1c6bf875f..7fe2b648f1 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include @@ -1833,7 +1834,7 @@ void GCS::service_statustext(void) mavlink_channel_t chan_index = (mavlink_channel_t)(MAVLINK_COMM_0+i); if (HAVE_PAYLOAD_SPACE(chan_index, STATUSTEXT)) { // we have space so send then clear that channel bit on the mask - mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text); + mavlink_msg_statustext_send(chan_index, statustext->msg.severity, statustext->msg.text, 0, 0); statustext->bitmask &= ~chan_bit; } } @@ -1848,6 +1849,18 @@ void GCS::service_statustext(void) } } +void GCS_MAVLINK::send_on_all_channels(const mavlink_message_t* msg) +{ + for (uint8_t i=0; i= ((uint16_t)msg->len) + GCS_MAVLINK::packet_overhead_chan(chan)) { + _mavlink_resend_uart(chan, msg); + } + } + } +} + void GCS::send_message(enum ap_message id) { for (uint8_t i=0; imttr_get_fts_version()); + } +} + MAV_RESULT GCS_MAVLINK::handle_command_do_send_banner(const mavlink_command_long_t &packet) { diff --git a/libraries/GCS_MAVLink/GCS_Fence.cpp b/libraries/GCS_MAVLink/GCS_Fence.cpp index c54af0d986..5baf189504 100644 --- a/libraries/GCS_MAVLink/GCS_Fence.cpp +++ b/libraries/GCS_MAVLink/GCS_Fence.cpp @@ -67,5 +67,5 @@ void GCS_MAVLINK::send_fence_status() const static_cast(fence->get_breaches() != 0), fence->get_breach_count(), mavlink_breach_type, - fence->get_breach_time()); + fence->get_breach_time(), 0); } diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index a50041162a..a5cd7b9022 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -71,6 +71,7 @@ enum ap_message : uint8_t { MSG_ORIGIN, MSG_HOME, MSG_NAMED_FLOAT, + MSG_LANDING_TARGET, MSG_EXTENDED_SYS_STATE, MSG_AUTOPILOT_VERSION, MSG_LAST // MSG_LAST must be the last entry in this enum diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 5881ffaa1b..aa9f8cf0f1 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -369,6 +369,8 @@ void FlightAxis::update(const struct sitl_input &input) { WITH_SEMAPHORE(mutex); + update_external_payload(input); + last_input = input; double dt_seconds = state.m_currentPhysicsTime_SEC - last_time_s; diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 5e1520fe38..7b8e2b4e50 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -196,6 +196,9 @@ const AP_Param::GroupInfo SITL::var_info2[] = { AP_SUBGROUPINFO(tonealarm_sim, "TA_", 57, SITL, ToneAlarm), AP_GROUPINFO("MAG_SCALING", 60, SITL, mag_scaling, 1), + AP_GROUPINFO("PLAND_OFS", 61, SITL, precland_offset, 0), + + AP_GROUPINFO("GPS_DRIFT_S", 62, SITL, gps_drift_spd, 0), AP_GROUPEND diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index d321818c47..da952a038f 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -225,6 +225,7 @@ class SITL { AP_Float temp_flight; AP_Float temp_tconst; AP_Float temp_baro_factor; + AP_Float gps_drift_spd; // differential pressure sensor tube order AP_Int8 arspd_signflip; @@ -280,6 +281,8 @@ class SITL { AP_Float hdg; // 0 to 360 } opos; + AP_Vector3f precland_offset; // XYZ offset of precland target from SITL origin + uint16_t irlock_port; void simstate_send(mavlink_channel_t chan); diff --git a/modules/mavlink b/modules/mavlink index d2cc7dbff6..2a8ee81a18 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit d2cc7dbff67f8c318a40e1ef57a99488b4737fab +Subproject commit 2a8ee81a186853359a242918279dc3883fd649bb